wednesday updates

src/main/deploy/pathplanner/paths/Center Shoot 3 Get Note.path

@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 2.496814123442796,
-        "y": 4.321298685393376
+        "x": 2.5202357965343594,
+        "y": 4.414985377759629
       },
       "prevControl": {
-        "x": 1.8833079853984915,
-        "y": 4.827684704096611
+        "x": 1.7238989114212167,
+        "y": 4.836575493407763
       },
       "nextControl": null,
       "isLocked": false,
@@ -39,7 +39,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -27.216111557307425,
+    "rotation": -38.23382517744699,
     "rotateFast": false
   },
   "reversed": false,

src/main/java/frc/robot/StateCommandGenerator.java

@@ -29,11 +29,11 @@ public static Command goToAmpCommand() {
         if (RobotContainer.arm.getPosition() < ArmState.kShoot.rotations + 0.02) {
             // arm up
             // move arm and intake together
-            return new ParallelCommandGroup(new SetIntakeState(IntakeState.kUp), new SetArmState(ArmState.kAmp));
+            return new ParallelCommandGroup(new SetArmState(ArmState.kAmp));
         } else {
             // arm down
             // move arm then move intake
-            return new ParallelCommandGroup(new SetArmState(ArmState.kAmp), new SequentialCommandGroup(new WaitForArmStow(), new SetIntakeState(IntakeState.kUp)));
+            return new ParallelCommandGroup(new SetArmState(ArmState.kAmp), new SequentialCommandGroup(new WaitForArmStow()));
         }
     }
 
@@ -59,10 +59,10 @@ public static Command goToHandoffCommand() {
             // intake up
             // move intake then move 
             return new SequentialCommandGroup(new SetIntakeState(IntakeState.kDown), new ParallelCommandGroup(new SetArmState(ArmState.kIntake), new SetShooterState(ShooterState.kIntake)));
-        } else if (RobotContainer.arm.isAtState(ArmState.kShootFromPodium)) {
+        } else if (!(RobotContainer.arm.isArmClearingIntake() || RobotContainer.intake.isIntakeDown())) {
             // arm flat
             // move arm up and intake down together, then arm down and shooter down together
-            return new SequentialCommandGroup(new ParallelCommandGroup(new SetArmState(ArmState.kStow), new SequentialCommandGroup(new WaitCommand(0.1), new SetIntakeState(IntakeState.kDown))), new ParallelCommandGroup(new SetArmState(ArmState.kIntake), new SetShooterState(ShooterState.kIntake)));
+            return new SequentialCommandGroup(new SequentialCommandGroup(new SetArmState(ArmState.kStow), new SetIntakeState(IntakeState.kDown)), new ParallelCommandGroup(new SetArmState(ArmState.kIntake), new SetShooterState(ShooterState.kIntake)));
         } else {
             // arm down
             // move arm and shooter together
@@ -78,7 +78,7 @@ public static Command goToSubwooferCommand() {
         } else if (RobotContainer.arm.isAtState(ArmState.kShootFromPodium)) {
             // arm flat
             // move arm up and intake down together, then arm down and shooter down together
-            return new SequentialCommandGroup(new ParallelCommandGroup(new SetArmState(ArmState.kStow), new SequentialCommandGroup(new WaitCommand(0.1), new SetIntakeState(IntakeState.kDown))), new ParallelCommandGroup(new SetArmState(ArmState.kShootFromSubwoofer), new SetShooterState(ShooterState.kShootFromSubwoofer)));
+            return new SequentialCommandGroup(new SequentialCommandGroup(new SetArmState(ArmState.kStow), new SetIntakeState(IntakeState.kDown)), new ParallelCommandGroup(new SetArmState(ArmState.kShootFromSubwoofer), new SetShooterState(ShooterState.kShootFromSubwoofer)));
         } else {
             // arm down
             // move arm and shooter together

src/main/java/frc/robot/hid/OperatorStick.java

@@ -63,4 +63,8 @@ public boolean levelArm() {
   public boolean fullyTrustVision() {
     return this.getRawButton(11);
   }
+
+  public boolean getShooterRamping() {
+    return this.getRawAxis(3) < -0.99;
+  }
 }

src/main/java/frc/robot/subsystems/Arm.java

@@ -75,7 +75,7 @@ public boolean isAtState(ArmState state) {
   }
 
   public boolean isArmClearingIntake() {
-    return rotationSetpoint < 0.75;
+    return getPosition() < 0.69;
   }
 
   public enum ArmState {
@@ -96,9 +96,10 @@ public enum ArmState {
 
   @Override
   public void periodic() {
-    // SmartDashboard.putNumber("armAngle", getPosition());
+    SmartDashboard.putNumber("armAngle", getPosition());
     // SmartDashboard.putNumber("armSetpoint", rotationSetpoint);
-    // SmartDashboard.putNumber("armError", Math.abs(rotationSetpoint - getPosition()));
+    SmartDashboard.putNumber("armError", Math.abs(rotationSetpoint - getPosition()));
+    // SmartDashboard.putBoolean("armAtSetpoint", Math.abs(rotationSetpoint - getPosition()) <= 0.02);
 
     // kG = SmartDashboard.getNumber("kG", kG);
     // kP = SmartDashboard.getNumber("kP", kP);

src/main/java/frc/robot/subsystems/Intake.java

@@ -59,7 +59,7 @@ public Intake() {
 
   public void goToSetpoint(double rotations) {
     double pidOutput = pivotPositionController.calculate(getPosition(), rotations);
-    double ffOutput = kG * Math.cos(Math.PI * 2 * getPosition());
+    double ffOutput = kG * Math.cos(Math.PI * 2 * (getPosition() - 0.081));
     double output = pidOutput + ffOutput;
 
     if (getPosition() <= 0.04 && rotations <= IntakeState.kDown.rotations) {
@@ -104,7 +104,7 @@ public double getPosition() {
   }
 
   public boolean isIntakeDown() {
-    return getPosition() < 0.07;
+    return rotationSetpoint < (IntakeState.kDown.rotations + 0.02);
   }
 
   public boolean isIntakeUp() {
@@ -129,9 +129,9 @@ public enum IntakeState {
 
   @Override
   public void periodic() {
-    // SmartDashboard.putNumber("IntakeAngle", getPosition());
+    SmartDashboard.putNumber("IntakeAngle", getPosition());
     // SmartDashboard.putNumber("IntakeSetpoint", rotationSetpoint);
-    // SmartDashboard.putNumber("IntakeError", Math.abs(getPosition() - rotationSetpoint));
+    SmartDashboard.putNumber("IntakeError", Math.abs(getPosition() - rotationSetpoint));
     // SmartDashboard.putBoolean("IntakeHasNote", hasNoteInside());
 
     // kP = SmartDashboard.getNumber("kp", kP);

src/main/java/frc/robot/subsystems/Shooter.java

@@ -72,7 +72,7 @@ public class Shooter extends SubsystemBase {
   private final double pivotkG = 0.30328;
   private final double pivotkV = 0.9972;
   private final double pivotkA = 0.025145;
-  private final double pivotkP = 18;
+  private final double pivotkP = 25;
   private final double pivotkI = 2;
   private final double pivotkD = 1;
 
@@ -385,7 +385,8 @@ public enum ShooterState {
     kShoot(0.2),
     kShootFromSubwoofer(0.085),
     kShootFromPodium(0.09),
-    kAmp(0.15);
+    kAmp(0.15),
+    kShootFromSubwooferAuto(0.094);
 
     public final double rotations;
 
@@ -400,9 +401,9 @@ public void periodic() {
     // SmartDashboard.putNumber("shooterAngleSetpoint", rotationSetpoint);
     // SmartDashboard.putNumber("shooterAngleError", Math.abs(rotationSetpoint - getPosition()));
     // SmartDashboard.putNumber("shooterAngleAbsolute", getAbsoluteAngle());
-    // SmartDashboard.putNumber("Shooter-Velocity", getVelocity());
+    SmartDashboard.putNumber("Shooter-Velocity", getVelocity());
     // SmartDashboard.putBoolean("Shooter-HasNote", hasNote());
-    // SmartDashboard.putNumber("shooterVelocityError", (Math.abs(getVelocity() - (-veloctiySetpoint))));
+    SmartDashboard.putNumber("shooterVelocityError", (Math.abs(getVelocity() - (-veloctiySetpoint))));
     // SmartDashboard.putBoolean("shooterAtSetpoint", isShooterRampedUp(1));
     // SmartDashboard.putNumbershooterSetpoint", veloctiySetpoint);
     // SmartDashboard.putBoolean((""shooterGoodTrajectory", goodTrajectory);