3 pc center source side

src/main/java/org/codeorange/frc2024/auto/actions/Shoot.java → src/main/java/org/codeorange/frc2024/auto/actions/ShootFromGround.java

@@ -6,18 +6,18 @@
 import org.codeorange.frc2024.subsystem.intake.Intake;
 import org.codeorange.frc2024.subsystem.shooter.Shooter;
 
-public class Shoot implements BaseAction {
+public class ShootFromGround implements BaseAction {
     private final Superstructure superstructure = Superstructure.getSuperstructure();
     private final Shooter shooter = Robot.getShooter();
     private final Intake intake = Robot.getIntake();
     private final Timer timer = new Timer();
     private final double angle;
 
-    public Shoot(double angle) {
+    public ShootFromGround(double angle) {
         this.angle = angle;
     }
 
-    public Shoot() {
+    public ShootFromGround() {
         this.angle = 54;
     }
 

src/main/java/org/codeorange/frc2024/auto/actions/ShootFromStow.java

@@ -0,0 +1,55 @@
+package org.codeorange.frc2024.auto.actions;
+
+import edu.wpi.first.wpilibj.Timer;
+import org.codeorange.frc2024.robot.Robot;
+import org.codeorange.frc2024.subsystem.Superstructure;
+import org.codeorange.frc2024.subsystem.intake.Intake;
+import org.codeorange.frc2024.subsystem.shooter.Shooter;
+
+public class ShootFromStow implements BaseAction {
+    private final Superstructure superstructure = Superstructure.getSuperstructure();
+    private final Shooter shooter = Robot.getShooter();
+    private final Intake intake = Robot.getIntake();
+    private final Timer timer = new Timer();
+    private final double angle;
+
+    public ShootFromStow(double angle) {
+        this.angle = angle;
+    }
+
+    public ShootFromStow() {
+        this.angle = 54;
+    }
+
+    @Override
+    public void start() {
+        superstructure.isFlipped = false;
+        if(intake.hasNote()) {
+            superstructure.setGoalState(Superstructure.States.SPEAKER);
+        }
+        intake.stop();
+        superstructure.wantedAngle = angle;
+        timer.stop();
+        timer.reset();
+    }
+
+    @Override
+    public void update() {
+        if(superstructure.getCurrentState().isAtWantedState() && superstructure.isAtGoalState() && shooter.isAtTargetVelocity()) {
+            intake.runIntakeForShooter();
+        }
+        if(!intake.hasNote()) {
+            timer.start();
+        }
+    }
+
+    @Override
+    public boolean isFinished() {
+        return timer.hasElapsed(0.3);
+    }
+
+    public void done() {
+        intake.stop();
+        superstructure.wantedAngle = 54;
+    }
+}

src/main/java/org/codeorange/frc2024/auto/routines/DoNothing.java

@@ -1,11 +1,11 @@
 package org.codeorange.frc2024.auto.routines;
 
 import org.codeorange.frc2024.auto.AutoEndedException;
-import org.codeorange.frc2024.auto.actions.Shoot;
+import org.codeorange.frc2024.auto.actions.ShootFromGround;
 
 public class DoNothing extends BaseRoutine {
     @Override
     protected void routine() throws AutoEndedException {
-        runAction(new Shoot(54));
+        runAction(new ShootFromGround(54));
     }
 }

src/main/java/org/codeorange/frc2024/auto/routines/FourPiece.java

@@ -25,11 +25,11 @@ public FourPiece() {
 
     @Override
     protected void routine() throws AutoEndedException {
-        runAction(new ParallelAction(new ResetOdometry(driveToFirstNote.sample(0)), new Shoot(45)));
-        runAction(new ParallelAction(new SeriesAction(new GroundIntake(), new Shoot(29)), new SeriesAction(new Wait(0.25), new DrivePath(driveToFirstNote))));
-        runAction(new ParallelAction(new SeriesAction(new GroundIntake(), new Shoot(29)), new SeriesAction(new DrivePath(driveToSecondNote))));
+        runAction(new ParallelAction(new ResetOdometry(driveToFirstNote.sample(0)), new ShootFromGround(45)));
+        runAction(new ParallelAction(new SeriesAction(new GroundIntake(), new ShootFromGround(29)), new SeriesAction(new Wait(0.25), new DrivePath(driveToFirstNote))));
+        runAction(new ParallelAction(new SeriesAction(new GroundIntake(), new ShootFromGround(29)), new SeriesAction(new DrivePath(driveToSecondNote))));
         runAction(new ParallelAction(new SeriesAction(new Wait(0.5), new GroundIntake()), new DrivePath(driveToThirdNote)));
-        runAction(new Shoot(29));
+        runAction(new ShootFromGround(29));
         runAction(new Stow());
         runAction(new StopShooter());
     }

src/main/java/org/codeorange/frc2024/auto/routines/SevenPcNearSide.java

@@ -30,45 +30,45 @@ public SevenPcNearSide() {
     @Override
     protected void routine() throws AutoEndedException {
         runAction(new ResetOdometry(driveToFirstNote.sample(0, Robot.isRed())));
-        runAction(new Shoot());
+        runAction(new ShootFromGround());
         runAction(new ParallelAction(
                 new DrivePath(driveToFirstNote),
                 new GroundIntake()
         ));
-        runAction(new Shoot());
+        runAction(new ShootFromGround());
         runAction(new ParallelAction(
                 new DrivePath(driveToSecondNote),
                 new GroundIntake()
         ));
-        runAction(new Shoot());
+        runAction(new ShootFromGround());
         runAction(new ParallelAction(
                 new DrivePath(driveToThirdNote),
                 new GroundIntake()
         ));
-        runAction(new Shoot());
+        runAction(new ShootFromGround());
         runAction(new ParallelAction(
                 new DrivePath(driveToCenter),
                 new SeriesAction(
                         new Wait(0.8),
                         new GroundIntake()
                 )
         ));
-        runAction(new Shoot());
+        runAction(new ShootFromGround());
         runAction(new ParallelAction(
                 new DrivePath(driveToSecondCenter),
                 new SeriesAction(
                         new Wait(0.3),
                         new GroundIntake()
                 )
         ));
-        runAction(new Shoot());
+        runAction(new ShootFromGround());
         runAction(new ParallelAction(
                 new DrivePath(driveToThirdCenter),
                 new SeriesAction(
                         new Wait(0.5),
                         new GroundIntake()
                 )
         ));
-        runAction(new Shoot());
+        runAction(new ShootFromGround());
     }
 }

src/main/java/org/codeorange/frc2024/auto/routines/ThreePieceCenterSourceSide.java

@@ -4,7 +4,6 @@
 import com.choreo.lib.ChoreoTrajectory;
 import org.codeorange.frc2024.auto.AutoEndedException;
 import org.codeorange.frc2024.auto.actions.*;
-import org.codeorange.frc2024.robot.Robot;
 
 public class ThreePieceCenterSourceSide extends BaseRoutine {
 
@@ -19,17 +18,18 @@ public ThreePieceCenterSourceSide() {
     }
     @Override
     protected void routine() throws AutoEndedException {
-        runAction(new ParallelAction(new ResetOdometry(driveToFirstNote.sample(0)), new Shoot(52)));
+        runAction(new ParallelAction(new ResetOdometry(driveToFirstNote.sample(0)), new ShootFromGround(45)));
         runAction(new ParallelAction(
                 new SeriesAction(
                         new ParallelAction(
                                 new Stow(),
-                                new Wait(1.5))
-                        , new GroundIntake(), new Stow()), new SeriesAction(new DrivePath(driveToFirstNote))));
-        runAction(new Shoot(24));
-        runAction(new ParallelAction(new SeriesAction(new Wait(0.1), new GroundIntake(), new Stow()), new SeriesAction(new DrivePath(driveToSecondNote))));
-        runAction(new Shoot(24));
-        runAction(new ParallelAction(new SeriesAction(new Wait(1), new GroundIntake(), new Stow()), new DrivePath(driveToThirdNote)));
-        runAction(new Shoot(24));
+                                new Wait(1.2))
+                        , new GroundIntake(), new Wait(0.4), new ShootFromGround(20)), new DrivePath(driveToFirstNote)));
+        runAction(new ParallelAction(new SeriesAction(new GroundIntake(), new Stow()), new SeriesAction(new DrivePath(driveToSecondNote))));
+        runAction(new ShootFromStow(25));
+        runAction(new Stow());
+        runAction(new ParallelAction(new SeriesAction(new Wait(0.5), new GroundIntake(), new Stow()), new DrivePath(driveToThirdNote)));
+        runAction(new ShootFromStow(25));
+        runAction(new Stow());
     }
 }

src/main/java/org/codeorange/frc2024/robot/Robot.java

@@ -185,6 +185,7 @@ public void robotInit() {
         autoChooser.addDefaultOption("Do Nothing", 0);
         autoChooser.addOption("Test", 1);
         autoChooser.addOption("Four Piece", 2);
+        autoChooser.addOption("Center Source Side 3 Piece", 3);
         sideChooser.addDefaultOption("Blue", "blue");
         sideChooser.addOption("Red", "red");
 

src/main/java/org/codeorange/frc2024/subsystem/Superstructure.java

@@ -176,6 +176,10 @@ public void update() {
                         superstructure.setGoalState(STOW);
                     }
                 }
+                if(DriverStation.isAutonomous()) {
+                    superstructure.setWantedShooterPosition(superstructure.wantedAngle / 360);
+                    shooter.runVelocityAuto(10000.0 / 60);
+                }
                 if(superstructure.goalState != States.SPEAKER) {
                     superstructure.setWantedShooterPosition(0);
                     if(!DriverStation.isAutonomous()) {
@@ -286,7 +290,7 @@ public boolean isAtWantedState() {
             return (MathUtil.epsilonEquals(elevatorPos, elevator.getPositionInInches(), 0.5)
                     && MathUtil.epsilonEquals(armPos, arm.getPivotDegrees(), 0.03)
                     && (MathUtil.epsilonEquals(wristPos, wrist.getWristAbsolutePosition(), 0.015)
-                    || (MathUtil.epsilonEquals(-superstructure.wantedShooterPosition - arm.getPivotDegrees(), wrist.getWristAbsolutePosition(), 0.01) && superstructure.currentState == States.SPEAKER_AUTO)));
+                    || (MathUtil.epsilonEquals(-superstructure.wantedShooterPosition - arm.getPivotDegrees(), wrist.getWristAbsolutePosition(), 0.01) && (superstructure.currentState == States.SPEAKER_AUTO || superstructure.currentState == States.SPEAKER))));
         }
         final double elevatorPos;
         final double armPos;

src/main/java/org/codeorange/frc2024/subsystem/drive/ModuleIOTalonFX.java

@@ -73,7 +73,7 @@ public ModuleIOTalonFX(int id) {
         driveMotor.getConfigurator().apply(
                 new TalonFXConfiguration()
                         .withSlot0(new Slot0Configs()
-                                .withKP(0.00055128)
+                                .withKP(0.0055128)
                                 .withKI(0)
                                 .withKD(0)
                                 .withKS(DRIVE_FEEDFORWARD.ks)

src/main/java/org/codeorange/frc2024/subsystem/wrist/WristIOTalonFX.java

@@ -53,7 +53,7 @@ public WristIOTalonFX() {
         slot0.kI = WRIST_I;
         slot0.kD = WRIST_D;
         slot0.kV = 0.5;
-        slot0.kS = 0.8; // Approximately 0.25V to get the mechanism moving
+        slot0.kS = 1; // Approximately 0.25V to get the mechanism moving
 
         CurrentLimitsConfigs currentLimits = configs.CurrentLimits;
         currentLimits.SupplyCurrentLimit = 50;