This commit is contained in:
mimigamin
2026-04-07 20:37:26 -06:00
parent 1636a054ed
commit 118dcee1c7
41 changed files with 96 additions and 64 deletions
@@ -58,7 +58,7 @@
"maxAcceleration": 3.0, "maxAcceleration": 3.0,
"maxAngularVelocity": 600.0, "maxAngularVelocity": 600.0,
"maxAngularAcceleration": 750.0, "maxAngularAcceleration": 750.0,
"nominalVoltage": 10.0, "nominalVoltage": 12.0,
"unlimited": false "unlimited": false
}, },
"goalEndState": { "goalEndState": {
@@ -37,7 +37,7 @@
"maxAcceleration": 3.0, "maxAcceleration": 3.0,
"maxAngularVelocity": 600.0, "maxAngularVelocity": 600.0,
"maxAngularAcceleration": 750.0, "maxAngularAcceleration": 750.0,
"nominalVoltage": 10.0, "nominalVoltage": 12.0,
"unlimited": false "unlimited": false
}, },
"goalEndState": { "goalEndState": {
@@ -58,7 +58,7 @@
"maxAcceleration": 3.0, "maxAcceleration": 3.0,
"maxAngularVelocity": 600.0, "maxAngularVelocity": 600.0,
"maxAngularAcceleration": 750.0, "maxAngularAcceleration": 750.0,
"nominalVoltage": 10.0, "nominalVoltage": 12.0,
"unlimited": false "unlimited": false
}, },
"goalEndState": { "goalEndState": {
@@ -37,7 +37,7 @@
"maxAcceleration": 3.0, "maxAcceleration": 3.0,
"maxAngularVelocity": 600.0, "maxAngularVelocity": 600.0,
"maxAngularAcceleration": 750.0, "maxAngularAcceleration": 750.0,
"nominalVoltage": 10.0, "nominalVoltage": 12.0,
"unlimited": false "unlimited": false
}, },
"goalEndState": { "goalEndState": {
@@ -37,7 +37,7 @@
"maxAcceleration": 3.0, "maxAcceleration": 3.0,
"maxAngularVelocity": 600.0, "maxAngularVelocity": 600.0,
"maxAngularAcceleration": 750.0, "maxAngularAcceleration": 750.0,
"nominalVoltage": 10.0, "nominalVoltage": 12.0,
"unlimited": false "unlimited": false
}, },
"goalEndState": { "goalEndState": {
@@ -37,7 +37,7 @@
"maxAcceleration": 3.0, "maxAcceleration": 3.0,
"maxAngularVelocity": 600.0, "maxAngularVelocity": 600.0,
"maxAngularAcceleration": 750.0, "maxAngularAcceleration": 750.0,
"nominalVoltage": 10.0, "nominalVoltage": 12.0,
"unlimited": false "unlimited": false
}, },
"goalEndState": { "goalEndState": {
@@ -48,7 +48,7 @@
"maxAcceleration": 3.0, "maxAcceleration": 3.0,
"maxAngularVelocity": 600.0, "maxAngularVelocity": 600.0,
"maxAngularAcceleration": 750.0, "maxAngularAcceleration": 750.0,
"nominalVoltage": 10.0, "nominalVoltage": 12.0,
"unlimited": false "unlimited": false
}, },
"goalEndState": { "goalEndState": {
@@ -104,7 +104,7 @@
"maxAcceleration": 3.0, "maxAcceleration": 3.0,
"maxAngularVelocity": 600.0, "maxAngularVelocity": 600.0,
"maxAngularAcceleration": 750.0, "maxAngularAcceleration": 750.0,
"nominalVoltage": 10.0, "nominalVoltage": 12.0,
"unlimited": false "unlimited": false
}, },
"goalEndState": { "goalEndState": {
@@ -48,7 +48,7 @@
"maxAcceleration": 3.0, "maxAcceleration": 3.0,
"maxAngularVelocity": 600.0, "maxAngularVelocity": 600.0,
"maxAngularAcceleration": 750.0, "maxAngularAcceleration": 750.0,
"nominalVoltage": 10.0, "nominalVoltage": 12.0,
"unlimited": false "unlimited": false
}, },
"goalEndState": { "goalEndState": {
@@ -88,7 +88,7 @@
"maxAcceleration": 3.0, "maxAcceleration": 3.0,
"maxAngularVelocity": 600.0, "maxAngularVelocity": 600.0,
"maxAngularAcceleration": 750.0, "maxAngularAcceleration": 750.0,
"nominalVoltage": 10.0, "nominalVoltage": 12.0,
"unlimited": false "unlimited": false
}, },
"goalEndState": { "goalEndState": {
@@ -58,7 +58,7 @@
"maxAcceleration": 3.0, "maxAcceleration": 3.0,
"maxAngularVelocity": 600.0, "maxAngularVelocity": 600.0,
"maxAngularAcceleration": 750.0, "maxAngularAcceleration": 750.0,
"nominalVoltage": 10.0, "nominalVoltage": 12.0,
"unlimited": false "unlimited": false
}, },
"goalEndState": { "goalEndState": {
@@ -48,7 +48,7 @@
"maxAcceleration": 3.0, "maxAcceleration": 3.0,
"maxAngularVelocity": 600.0, "maxAngularVelocity": 600.0,
"maxAngularAcceleration": 750.0, "maxAngularAcceleration": 750.0,
"nominalVoltage": 10.0, "nominalVoltage": 12.0,
"unlimited": false "unlimited": false
}, },
"goalEndState": { "goalEndState": {
@@ -88,7 +88,7 @@
"maxAcceleration": 3.0, "maxAcceleration": 3.0,
"maxAngularVelocity": 600.0, "maxAngularVelocity": 600.0,
"maxAngularAcceleration": 750.0, "maxAngularAcceleration": 750.0,
"nominalVoltage": 10.0, "nominalVoltage": 12.0,
"unlimited": false "unlimited": false
}, },
"goalEndState": { "goalEndState": {
@@ -58,7 +58,7 @@
"maxAcceleration": 3.0, "maxAcceleration": 3.0,
"maxAngularVelocity": 600.0, "maxAngularVelocity": 600.0,
"maxAngularAcceleration": 750.0, "maxAngularAcceleration": 750.0,
"nominalVoltage": 10.0, "nominalVoltage": 12.0,
"unlimited": false "unlimited": false
}, },
"goalEndState": { "goalEndState": {
@@ -48,7 +48,7 @@
"maxAcceleration": 3.0, "maxAcceleration": 3.0,
"maxAngularVelocity": 600.0, "maxAngularVelocity": 600.0,
"maxAngularAcceleration": 750.0, "maxAngularAcceleration": 750.0,
"nominalVoltage": 10.0, "nominalVoltage": 12.0,
"unlimited": false "unlimited": false
}, },
"goalEndState": { "goalEndState": {
@@ -64,7 +64,7 @@
"maxAcceleration": 3.0, "maxAcceleration": 3.0,
"maxAngularVelocity": 600.0, "maxAngularVelocity": 600.0,
"maxAngularAcceleration": 750.0, "maxAngularAcceleration": 750.0,
"nominalVoltage": 10.0, "nominalVoltage": 12.0,
"unlimited": false "unlimited": false
}, },
"goalEndState": { "goalEndState": {
@@ -48,7 +48,7 @@
"maxAcceleration": 3.0, "maxAcceleration": 3.0,
"maxAngularVelocity": 600.0, "maxAngularVelocity": 600.0,
"maxAngularAcceleration": 750.0, "maxAngularAcceleration": 750.0,
"nominalVoltage": 10.0, "nominalVoltage": 12.0,
"unlimited": false "unlimited": false
}, },
"goalEndState": { "goalEndState": {
@@ -111,7 +111,7 @@
"maxAcceleration": 3.0, "maxAcceleration": 3.0,
"maxAngularVelocity": 600.0, "maxAngularVelocity": 600.0,
"maxAngularAcceleration": 750.0, "maxAngularAcceleration": 750.0,
"nominalVoltage": 10.0, "nominalVoltage": 12.0,
"unlimited": false "unlimited": false
}, },
"goalEndState": { "goalEndState": {
@@ -42,7 +42,7 @@
"maxAcceleration": 3.0, "maxAcceleration": 3.0,
"maxAngularVelocity": 600.0, "maxAngularVelocity": 600.0,
"maxAngularAcceleration": 750.0, "maxAngularAcceleration": 750.0,
"nominalVoltage": 10.0, "nominalVoltage": 12.0,
"unlimited": false "unlimited": false
}, },
"goalEndState": { "goalEndState": {
@@ -131,7 +131,7 @@
"maxAcceleration": 3.0, "maxAcceleration": 3.0,
"maxAngularVelocity": 600.0, "maxAngularVelocity": 600.0,
"maxAngularAcceleration": 750.0, "maxAngularAcceleration": 750.0,
"nominalVoltage": 10.0, "nominalVoltage": 12.0,
"unlimited": false "unlimited": false
}, },
"goalEndState": { "goalEndState": {
@@ -42,7 +42,7 @@
"maxAcceleration": 3.0, "maxAcceleration": 3.0,
"maxAngularVelocity": 600.0, "maxAngularVelocity": 600.0,
"maxAngularAcceleration": 750.0, "maxAngularAcceleration": 750.0,
"nominalVoltage": 10.0, "nominalVoltage": 12.0,
"unlimited": false "unlimited": false
}, },
"goalEndState": { "goalEndState": {
@@ -48,7 +48,7 @@
"maxAcceleration": 3.0, "maxAcceleration": 3.0,
"maxAngularVelocity": 600.0, "maxAngularVelocity": 600.0,
"maxAngularAcceleration": 750.0, "maxAngularAcceleration": 750.0,
"nominalVoltage": 10.0, "nominalVoltage": 12.0,
"unlimited": false "unlimited": false
}, },
"goalEndState": { "goalEndState": {
@@ -58,7 +58,7 @@
"maxAcceleration": 3.0, "maxAcceleration": 3.0,
"maxAngularVelocity": 600.0, "maxAngularVelocity": 600.0,
"maxAngularAcceleration": 750.0, "maxAngularAcceleration": 750.0,
"nominalVoltage": 10.0, "nominalVoltage": 12.0,
"unlimited": false "unlimited": false
}, },
"goalEndState": { "goalEndState": {
@@ -48,7 +48,7 @@
"maxAcceleration": 3.0, "maxAcceleration": 3.0,
"maxAngularVelocity": 600.0, "maxAngularVelocity": 600.0,
"maxAngularAcceleration": 750.0, "maxAngularAcceleration": 750.0,
"nominalVoltage": 10.0, "nominalVoltage": 12.0,
"unlimited": false "unlimited": false
}, },
"goalEndState": { "goalEndState": {
@@ -111,7 +111,7 @@
"maxAcceleration": 3.0, "maxAcceleration": 3.0,
"maxAngularVelocity": 600.0, "maxAngularVelocity": 600.0,
"maxAngularAcceleration": 750.0, "maxAngularAcceleration": 750.0,
"nominalVoltage": 10.0, "nominalVoltage": 12.0,
"unlimited": false "unlimited": false
}, },
"goalEndState": { "goalEndState": {
@@ -42,7 +42,7 @@
"maxAcceleration": 3.0, "maxAcceleration": 3.0,
"maxAngularVelocity": 600.0, "maxAngularVelocity": 600.0,
"maxAngularAcceleration": 750.0, "maxAngularAcceleration": 750.0,
"nominalVoltage": 10.0, "nominalVoltage": 12.0,
"unlimited": false "unlimited": false
}, },
"goalEndState": { "goalEndState": {
@@ -125,7 +125,7 @@
"maxAcceleration": 3.0, "maxAcceleration": 3.0,
"maxAngularVelocity": 600.0, "maxAngularVelocity": 600.0,
"maxAngularAcceleration": 750.0, "maxAngularAcceleration": 750.0,
"nominalVoltage": 10.0, "nominalVoltage": 12.0,
"unlimited": false "unlimited": false
}, },
"goalEndState": { "goalEndState": {
@@ -42,7 +42,7 @@
"maxAcceleration": 3.0, "maxAcceleration": 3.0,
"maxAngularVelocity": 600.0, "maxAngularVelocity": 600.0,
"maxAngularAcceleration": 750.0, "maxAngularAcceleration": 750.0,
"nominalVoltage": 10.0, "nominalVoltage": 12.0,
"unlimited": false "unlimited": false
}, },
"goalEndState": { "goalEndState": {
@@ -78,7 +78,7 @@
"maxAcceleration": 3.0, "maxAcceleration": 3.0,
"maxAngularVelocity": 600.0, "maxAngularVelocity": 600.0,
"maxAngularAcceleration": 750.0, "maxAngularAcceleration": 750.0,
"nominalVoltage": 10.0, "nominalVoltage": 12.0,
"unlimited": false "unlimited": false
}, },
"goalEndState": { "goalEndState": {
@@ -89,7 +89,7 @@
"folder": null, "folder": null,
"idealStartingState": { "idealStartingState": {
"velocity": 0, "velocity": 0,
"rotation": -106.71386741074436 "rotation": -106.71386741074437
}, },
"useDefaultConstraints": true "useDefaultConstraints": true
} }
@@ -78,7 +78,7 @@
"maxAcceleration": 3.0, "maxAcceleration": 3.0,
"maxAngularVelocity": 600.0, "maxAngularVelocity": 600.0,
"maxAngularAcceleration": 750.0, "maxAngularAcceleration": 750.0,
"nominalVoltage": 10.0, "nominalVoltage": 12.0,
"unlimited": false "unlimited": false
}, },
"goalEndState": { "goalEndState": {
@@ -37,7 +37,7 @@
"maxAcceleration": 3.0, "maxAcceleration": 3.0,
"maxAngularVelocity": 600.0, "maxAngularVelocity": 600.0,
"maxAngularAcceleration": 750.0, "maxAngularAcceleration": 750.0,
"nominalVoltage": 10.0, "nominalVoltage": 12.0,
"unlimited": false "unlimited": false
}, },
"goalEndState": { "goalEndState": {
+1 -1
View File
@@ -11,7 +11,7 @@
"defaultMaxAccel": 3.0, "defaultMaxAccel": 3.0,
"defaultMaxAngVel": 600.0, "defaultMaxAngVel": 600.0,
"defaultMaxAngAccel": 750.0, "defaultMaxAngAccel": 750.0,
"defaultNominalVoltage": 10.0, "defaultNominalVoltage": 12.0,
"robotMass": 74.088, "robotMass": 74.088,
"robotMOI": 6.883, "robotMOI": 6.883,
"robotTrackwidth": 0.546, "robotTrackwidth": 0.546,
+1
View File
@@ -25,5 +25,6 @@ public final class Main {
*/ */
public static void main(String... args) { public static void main(String... args) {
RobotBase.startRobot(Robot::new); RobotBase.startRobot(Robot::new);
} }
} }
+1
View File
@@ -82,6 +82,7 @@ public class Robot extends LoggedRobot {
// and running subsystem periodic() methods. This must be called from the robot's periodic // and running subsystem periodic() methods. This must be called from the robot's periodic
// block in order for anything in the Command-based framework to work. // block in order for anything in the Command-based framework to work.
CommandScheduler.getInstance().run(); CommandScheduler.getInstance().run();
} }
/** /**
* This function is called once each time the robot enters Disabled mode. * This function is called once each time the robot enters Disabled mode.
@@ -139,7 +139,8 @@ public class RobotContainer {
private Command RobotRev = new SequentialCommandGroup( private Command RobotRev = new SequentialCommandGroup(
new InstantCommand(() -> m_robotShooter.spinUpShooting(), m_robotShooter), new InstantCommand(() -> m_robotShooter.spinUpShooting(), m_robotShooter),
IntakeExtended, IntakeExtended,
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Idle), m_robotIntake) new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.ExpelBalls), m_robotIntake)
// new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Idle), m_robotIntake)
); );
private Command WaitIntakeReference = private Command WaitIntakeReference =
@@ -161,10 +162,11 @@ public class RobotContainer {
// TEST NEW AUTO ALIGN // TEST NEW AUTO ALIGN
//new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0)), false), //new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0)), false),
new WaitUntilCommand(m_robotShooter::isShooterUpToSpeed), new WaitUntilCommand(m_robotShooter::isShooterUpToSpeed),
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Idle), m_robotIntake),
new InstantCommand(()-> m_robotShooter.allowShooting(), m_robotShooter), new InstantCommand(()-> m_robotShooter.allowShooting(), m_robotShooter),
new WaitCommand(4), new WaitCommand(3),
IntakeRetracted, IntakeRetracted,
new WaitCommand(7), new WaitCommand(4.5),
new InstantCommand(() -> m_robotShooter.denyShooting(), m_robotShooter), new InstantCommand(() -> m_robotShooter.denyShooting(), m_robotShooter),
new InstantCommand(()->m_robotShooter.spinUpIdle(), m_robotShooter) new InstantCommand(()->m_robotShooter.spinUpIdle(), m_robotShooter)
); );
@@ -183,7 +185,8 @@ public class RobotContainer {
DeferredBlock.addBlock(() -> { DeferredBlock.addBlock(() -> {
TimesNegativeOne.update(); TimesNegativeOne.update();
FieldPositions.update(); FieldPositions.update();
m_robotIntake.setMode(IntakeMode.Idle);
m_robotShooter.spinUpIdle();
m_robotIntake.io.updateGains(); m_robotIntake.io.updateGains();
m_robotShooter.io.updateGains(); m_robotShooter.io.updateGains();
}, true); }, true);
@@ -339,7 +342,6 @@ public class RobotContainer {
new Trigger(() -> getDeadbandedOperatorController().getRightTriggerAxis() >= 0.5) new Trigger(() -> getDeadbandedOperatorController().getRightTriggerAxis() >= 0.5)
.onTrue(new InstantCommand(() -> { .onTrue(new InstantCommand(() -> {
m_robotShooter.allowShooting(); m_robotShooter.allowShooting();
m_robotIntake.rollerStop();
})).onFalse(new InstantCommand(() -> { })).onFalse(new InstantCommand(() -> {
m_robotShooter.denyShooting(); m_robotShooter.denyShooting();
})); }));
@@ -380,6 +382,7 @@ public class RobotContainer {
.onTrue(new InstantCommand(() -> { .onTrue(new InstantCommand(() -> {
m_robotIntake.setMode(IntakeMode.ExtendingRolling); m_robotIntake.setMode(IntakeMode.ExtendingRolling);
})); }));
new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
.onTrue(new InstantCommand(() -> { .onTrue(new InstantCommand(() -> {
m_robotIntake.setMode(IntakeMode.LabubuGrowl); m_robotIntake.setMode(IntakeMode.LabubuGrowl);
@@ -392,7 +395,7 @@ public class RobotContainer {
m_robotIntake.setMode(IntakeMode.Retracting); m_robotIntake.setMode(IntakeMode.Retracting);
})) }))
.onFalse(new InstantCommand(() -> { .onFalse(new InstantCommand(() -> {
m_robotIntake.setMode(IntakeMode.Idle); m_robotIntake.setMode(IntakeMode.ArmIdleRollingNot);
})); }));
@@ -7,12 +7,12 @@ public final class BuildConstants {
public static final String MAVEN_GROUP = ""; public static final String MAVEN_GROUP = "";
public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String MAVEN_NAME = "2026KPopRobotHunters";
public static final String VERSION = "unspecified"; public static final String VERSION = "unspecified";
public static final int GIT_REVISION = 220; public static final int GIT_REVISION = 221;
public static final String GIT_SHA = "9564554c0708fd0b761f959685258d9c1a90d11e"; public static final String GIT_SHA = "1636a054ed1e688234fe329b76f567d6af08081f";
public static final String GIT_DATE = "2026-04-06 19:50:34 MDT"; public static final String GIT_DATE = "2026-04-06 22:44:17 MDT";
public static final String GIT_BRANCH = "New-Intake"; public static final String GIT_BRANCH = "New-Intake";
public static final String BUILD_DATE = "2026-04-06 22:42:14 MDT"; public static final String BUILD_DATE = "2026-04-07 20:27:10 MDT";
public static final long BUILD_UNIX_TIME = 1775536934067L; public static final long BUILD_UNIX_TIME = 1775615230294L;
public static final int DIRTY = 1; public static final int DIRTY = 1;
private BuildConstants(){} private BuildConstants(){}
@@ -97,7 +97,7 @@ public final class Constants {
public static final class LEDConstants { public static final class LEDConstants {
public static final int LED_SPARK_ID = 9; public static final int LED_SPARK_ID = 8;
public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_RAINBOW; public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_RAINBOW;
@@ -35,6 +35,7 @@ public class Intake extends SubsystemBase {
ExtendingRolling, ExtendingRolling,
Retracting, Retracting,
ArmIdleRollingNot,
Idle, Idle,
RectractTorque, RectractTorque,
@@ -79,7 +80,6 @@ public class Intake extends SubsystemBase {
public double getRollerSpeed() { public double getRollerSpeed() {
return state.rollerOutput; return state.rollerOutput;
} }
// public enum FieldZone { // public enum FieldZone {
// // The robot should aim at the hub // // The robot should aim at the hub
// InShootZone, // InShootZone,
@@ -136,7 +136,10 @@ public class Intake extends SubsystemBase {
// io.setRollerOutput(state, 0); // io.setRollerOutput(state, 0);
// } // }
break; break;
case ArmIdleRollingNot:
io.armOutput(0);
io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get());
break;
case Bouncing: case Bouncing:
// io.setRollerOutput(state, 0); // io.setRollerOutput(state, 0);
@@ -23,7 +23,7 @@ public class IntakeConstants {
public static final double ROLLER_MOTOR_GEAR_RATIO = 3; public static final double ROLLER_MOTOR_GEAR_RATIO = 3;
public static final ConfigurableDouble ARM_ENCODER_OFFSET = new ConfigurableDouble("Arm Encoder Offset", 0); public static final ConfigurableDouble ARM_ENCODER_OFFSET = new ConfigurableDouble("Arm Encoder Offset", 0);
public static final int ARM_LIMIT_SWITCH_CHANNEL = 9; public static final int ARM_LIMIT_SWITCH_CHANNEL = 7;
public static final ConfigurableDouble INTAKE_BOUNCE_HALF_PERIOD = new ConfigurableDouble("Bounce Half Period", 5.); 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.1); public static final ConfigurableDouble INTAKE_BOUNCE_OUTPUT = new ConfigurableDouble("Bounce Output", 0.1);
@@ -56,7 +56,7 @@ public class IntakeConstants {
// public static final Angle ARM_LIMIT_UPPER = Degrees.of(-90); // public static final Angle ARM_LIMIT_UPPER = Degrees.of(-90);
public static final ConfigurableDouble ARM_LIMIT_RETRACTED = new ConfigurableDouble("Arm angle retracted", 0.); 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.75); public static final ConfigurableDouble ARM_LIMIT_EXTENDED = new ConfigurableDouble("Arm angle extended", 1.8); //new soft limt
public static final ConfigurableDouble ARM_EXTEND_PERCENT_OUTPUT = new ConfigurableDouble("Arm extend % output", 0.4); public static final ConfigurableDouble ARM_EXTEND_PERCENT_OUTPUT = new ConfigurableDouble("Arm extend % output", 0.4);
public static final ConfigurableDouble ARM_RETRACT_PERCENT_OUTPUT = new ConfigurableDouble("Arm retract % output", -0.2); public static final ConfigurableDouble ARM_RETRACT_PERCENT_OUTPUT = new ConfigurableDouble("Arm retract % output", -0.2);
@@ -108,9 +108,9 @@ public class IntakeConstants {
.forwardLimitSwitchType(Type.kNormallyOpen) .forwardLimitSwitchType(Type.kNormallyOpen)
.forwardLimitSwitchTriggerBehavior(Behavior.kKeepMovingMotor) .forwardLimitSwitchTriggerBehavior(Behavior.kKeepMovingMotor)
.reverseLimitSwitchType(Type.kNormallyClosed) .reverseLimitSwitchType(Type.kNormallyOpen)
.reverseLimitSwitchPosition(0) // .reverseLimitSwitchPosition(0)
.reverseLimitSwitchTriggerBehavior(Behavior.kStopMovingMotorAndSetPosition); .reverseLimitSwitchTriggerBehavior(Behavior.kKeepMovingMotor);
ARM_MOTOR_CONFIG.idleMode(IdleMode.kBrake); ARM_MOTOR_CONFIG.idleMode(IdleMode.kBrake);
@@ -4,6 +4,8 @@ import static edu.wpi.first.units.Units.Amps;
import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.Rotations;
import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.RotationsPerSecond;
import org.littletonrobotics.junction.Logger;
import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.hardware.TalonFX;
import com.revrobotics.PersistMode; import com.revrobotics.PersistMode;
import com.revrobotics.ResetMode; import com.revrobotics.ResetMode;
@@ -13,6 +15,8 @@ import com.revrobotics.spark.SparkMax;
import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Timer;
import frc4388.robot.constants.Constants;
import frc4388.utility.compute.JankCoder; import frc4388.utility.compute.JankCoder;
public class IntakeReal implements IntakeIO { public class IntakeReal implements IntakeIO {
@@ -23,6 +27,9 @@ public class IntakeReal implements IntakeIO {
TalonFX m_rollerMotor; TalonFX m_rollerMotor;
JankCoder m_encoder; JankCoder m_encoder;
DigitalInput m_armLimitSwitch; DigitalInput m_armLimitSwitch;
boolean m_limitTRIGGER = false;
private final Timer m_limitTimer = new Timer();
public IntakeReal( public IntakeReal(
DigitalInput armLimitSwitch, DigitalInput armLimitSwitch,
@@ -78,9 +85,12 @@ public class IntakeReal implements IntakeIO {
// m_rollerMotor.set(0); // m_rollerMotor.set(0);
} }
// private boolean retractedLimit() { private boolean retractedLimit() {
// return m_encoder.get() <= IntakeConstants.ARM_LIMIT_RETRACTED.get(); return m_armLimitSwitch.get();
// } }
private boolean retractedSoftLimit() {
return m_encoder.get() <= IntakeConstants.ARM_LIMIT_RETRACTED.get();
}
private boolean extendedLimit() { private boolean extendedLimit() {
return m_encoder.get() >= IntakeConstants.ARM_LIMIT_EXTENDED.get(); return m_encoder.get() >= IntakeConstants.ARM_LIMIT_EXTENDED.get();
} }
@@ -88,9 +98,9 @@ public class IntakeReal implements IntakeIO {
@Override @Override
public void armOutput(double percentOutput){ public void armOutput(double percentOutput){
// if(retractedLimit()) { if(retractedSoftLimit()) {
// percentOutput = Math.max(percentOutput, 0); percentOutput = Math.max(percentOutput, 0);
// } }
if (extendedLimit()) { if (extendedLimit()) {
percentOutput = Math.min(percentOutput, 0); percentOutput = Math.min(percentOutput, 0);
@@ -120,16 +130,30 @@ public class IntakeReal implements IntakeIO {
state.intakeEncoder = m_encoder.getRotations(); state.intakeEncoder = m_encoder.getRotations();
state.encoderConnected = m_encoder.isConnected(); state.encoderConnected = m_encoder.isConnected();
state.retractedLimitSwitch = m_armLimitSwitch.get(); state.retractedLimitSwitch = retractedLimit();
if(state.retractedLimitSwitch) { // if(state.retractedLimitSwitch && (state.armMotorVelocity.in(RotationsPerSecond) <0)) {
m_encoder.resetRotations(); // if (!m_limitTRIGGER) {
} // m_limitTRIGGER = true;
// m_limitTimer.restart();
// }
// if (m_limitTimer.hasElapsed(1.0)) {
// m_encoder.resetRotations();
// }
// } else {
// m_limitTRIGGER = false;
// m_limitTimer.reset();
// }
} }
@Override @Override
public void updateGains() { public void updateGains() {
m_encoder.loadRotations(); // m_encoder.loadRotations();
if(retractedLimit()) {
m_encoder.resetRotations();
}
// IntakeConstants.ARM_PID.kP = IntakeConstants.arm_kP.get(); // IntakeConstants.ARM_PID.kP = IntakeConstants.arm_kP.get();
// IntakeConstants.ARM_PID.kI = IntakeConstants.arm_kI.get(); // IntakeConstants.ARM_PID.kI = IntakeConstants.arm_kI.get();
@@ -26,7 +26,7 @@ public class ShooterConstants {
// public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 0.0); // public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 0.0);
public static final ConfigurableDouble SHOOTER_IDLE_PERCENT_OUTPUT = new ConfigurableDouble("Shooter idle % output", -0.15); public static final ConfigurableDouble SHOOTER_IDLE_PERCENT_OUTPUT = new ConfigurableDouble("Shooter idle % output", 0.0);//-0.15);
// public static final ConfigurableDouble SHOOTER_IDLE_TARGET_VEL = new ConfigurableDouble("Shooter idle target velocity", 20.); // public static final ConfigurableDouble SHOOTER_IDLE_TARGET_VEL = new ConfigurableDouble("Shooter idle target velocity", 20.);
// public static final ConfigurableDouble SHOOTER_IDLE_MAX_CURRENT = new ConfigurableDouble("Shooter Idle max current", 10); // public static final ConfigurableDouble SHOOTER_IDLE_MAX_CURRENT = new ConfigurableDouble("Shooter Idle max current", 10);