mirror of
https://github.com/Team4388/2026KPopRobotHunters.git
synced 2026-06-09 00:38:03 -06:00
Oh god I hate REV
This commit is contained in:
@@ -15,6 +15,8 @@ import org.littletonrobotics.junction.networktables.NT4Publisher;
|
||||
import org.littletonrobotics.junction.wpilog.WPILOGReader;
|
||||
import org.littletonrobotics.junction.wpilog.WPILOGWriter;
|
||||
|
||||
import com.ctre.phoenix6.SignalLogger;
|
||||
|
||||
import edu.wpi.first.wpilibj.GenericHID.RumbleType;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
||||
@@ -50,6 +52,9 @@ public class Robot extends LoggedRobot {
|
||||
// Start logging with AdvantageKit
|
||||
startLogging();
|
||||
|
||||
com.revrobotics.util.StatusLogger.disableAutoLogging();
|
||||
SignalLogger.enableAutoLogging(false);
|
||||
|
||||
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
|
||||
// autonomous chooser on the dashboard.
|
||||
m_robotContainer = new RobotContainer();
|
||||
|
||||
@@ -7,12 +7,12 @@ public final class BuildConstants {
|
||||
public static final String MAVEN_GROUP = "";
|
||||
public static final String MAVEN_NAME = "2026KPopRobotHunters";
|
||||
public static final String VERSION = "unspecified";
|
||||
public static final int GIT_REVISION = 190;
|
||||
public static final String GIT_SHA = "3fe5d0e5a1cfe24710b755b489fa61255e307095";
|
||||
public static final String GIT_DATE = "2026-03-30 16:15:43 MDT";
|
||||
public static final int GIT_REVISION = 191;
|
||||
public static final String GIT_SHA = "9021f480beaf35912eb54a845ec87522fef5337a";
|
||||
public static final String GIT_DATE = "2026-03-30 18:57:14 MDT";
|
||||
public static final String GIT_BRANCH = "New-Intake";
|
||||
public static final String BUILD_DATE = "2026-03-30 18:15:12 MDT";
|
||||
public static final long BUILD_UNIX_TIME = 1774916112363L;
|
||||
public static final String BUILD_DATE = "2026-03-30 19:50:05 MDT";
|
||||
public static final long BUILD_UNIX_TIME = 1774921805228L;
|
||||
public static final int DIRTY = 1;
|
||||
|
||||
private BuildConstants(){}
|
||||
|
||||
@@ -129,12 +129,13 @@ public class Intake extends SubsystemBase {
|
||||
break;
|
||||
|
||||
case Bouncing:
|
||||
io.setRollerOutput(state, 0);
|
||||
// io.setRollerOutput(state, 0);
|
||||
|
||||
if(
|
||||
state.armMotorCurrent.in(Amps) < IntakeConstants.INTAKE_BOUNCE_CURRENT_LIMIT.get() &&
|
||||
state.armMotorVelocity.in(RotationsPerSecond) < IntakeConstants.INTAKE_BOUNCE_VELOCITY_LIMIT.get()
|
||||
state.armMotorCurrent.in(Amps) > IntakeConstants.INTAKE_BOUNCE_CURRENT_LIMIT.get()
|
||||
// Math.abs(state.armMotorVelocity.in(RotationsPerSecond)) < IntakeConstants.INTAKE_BOUNCE_VELOCITY_LIMIT.get()
|
||||
) {
|
||||
System.out.println("RESET BOUNCE");
|
||||
this.state.currentBounceTime = Utils.getSystemTimeSeconds() + IntakeConstants.INTAKE_BOUNCE_HALF_PERIOD.get();
|
||||
}
|
||||
|
||||
@@ -143,11 +144,11 @@ public class Intake extends SubsystemBase {
|
||||
// Get the percentage through the bounce period (0 output means one half period has passed)
|
||||
double percentOutput = (currentTime / IntakeConstants.INTAKE_BOUNCE_HALF_PERIOD.get()) * IntakeConstants.INTAKE_BOUNCE_OUTPUT.get();
|
||||
// Clamp the output of the motor to some value
|
||||
percentOutput = Math.max(Math.min(percentOutput, IntakeConstants.INTAKE_BOUNCE_MAX_OUTPUT.get()), -IntakeConstants.INTAKE_BOUNCE_MAX_OUTPUT.get());
|
||||
percentOutput = -Math.max(Math.min(percentOutput, IntakeConstants.INTAKE_BOUNCE_MAX_OUTPUT.get()), -IntakeConstants.INTAKE_BOUNCE_MAX_OUTPUT.get());
|
||||
|
||||
io.armOutput(percentOutput);
|
||||
|
||||
if(state.intakeEncoder.in(Rotations) > IntakeConstants.ARM_REVERSE_ROLLER_RANGE.get()) {
|
||||
if(percentOutput < 0) {
|
||||
io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get());
|
||||
} else {
|
||||
io.setRollerOutput(state, 0);
|
||||
|
||||
@@ -19,10 +19,10 @@ public class IntakeConstants {
|
||||
|
||||
|
||||
public static final ConfigurableDouble INTAKE_BOUNCE_HALF_PERIOD = new ConfigurableDouble("Bounce Half Period", 5.);
|
||||
public static final ConfigurableDouble INTAKE_BOUNCE_OUTPUT = new ConfigurableDouble("Bounce Output", 0.2);
|
||||
public static final ConfigurableDouble INTAKE_BOUNCE_MAX_OUTPUT = new ConfigurableDouble("Bounce Max Output", 0.5);
|
||||
public static final ConfigurableDouble INTAKE_BOUNCE_CURRENT_LIMIT = new ConfigurableDouble("Intake Bounce Current Limit", 20);
|
||||
public static final ConfigurableDouble INTAKE_BOUNCE_VELOCITY_LIMIT = new ConfigurableDouble("Intake Bounce Velocity Limit", 20);
|
||||
public static final ConfigurableDouble INTAKE_BOUNCE_OUTPUT = new ConfigurableDouble("Bounce Output", 0.1);
|
||||
public static final ConfigurableDouble INTAKE_BOUNCE_MAX_OUTPUT = new ConfigurableDouble("Bounce Max Output", 0.2);
|
||||
public static final ConfigurableDouble INTAKE_BOUNCE_CURRENT_LIMIT = new ConfigurableDouble("Intake Bounce Current Limit", 16);
|
||||
public static final ConfigurableDouble INTAKE_BOUNCE_VELOCITY_LIMIT = new ConfigurableDouble("Intake Bounce Velocity Limit", 4);
|
||||
|
||||
|
||||
|
||||
@@ -44,8 +44,8 @@ public class IntakeConstants {
|
||||
|
||||
public static final ConfigurableDouble ARM_LIMIT_RETRACTED = new ConfigurableDouble("Arm angle retracted", 0.);
|
||||
public static final ConfigurableDouble ARM_LIMIT_EXTENDED = new ConfigurableDouble("Arm angle extended", 1.5);
|
||||
public static final ConfigurableDouble ARM_EXTEND_PERCENT_OUTPUT = new ConfigurableDouble("Arm extend % output", 0.5);
|
||||
public static final ConfigurableDouble ARM_RETRACT_PERCENT_OUTPUT = new ConfigurableDouble("Arm retract % output", -0.5);
|
||||
public static final ConfigurableDouble ARM_EXTEND_PERCENT_OUTPUT = new ConfigurableDouble("Arm extend % output", 0.2);
|
||||
public static final ConfigurableDouble ARM_RETRACT_PERCENT_OUTPUT = new ConfigurableDouble("Arm retract % output", -0.2);
|
||||
|
||||
public static final ConfigurableDouble ARM_REVERSE_ROLLER_RANGE = new ConfigurableDouble("Arm reverse roller range", 1.17);
|
||||
|
||||
|
||||
Reference in New Issue
Block a user