Cancel outtake when auto is done, set target velocity in SetShooterPI…

…DVelocityFromState
Team3256 · Apr 17, 2022 · 833af5b · 833af5b
1 parent 4c892be
commit 833af5b
Show file tree

Hide file tree

Showing 3 changed files with 5 additions and 2 deletions.
diff --git a/src/main/java/frc/robot/auto/Paths.java b/src/main/java/frc/robot/auto/Paths.java
@@ -142,7 +142,9 @@ public static Command get2BallDefenseMidTarmac1BallSide() {
         return twoBallTarmacSideSegment
                 .andThen(getShootCommand(5))
                 .andThen(twoBallTarmacDefenseSideSegment)
-                .andThen(new InstantCommand(() -> CommandScheduler.getInstance().schedule(outtake)));
+                .andThen(new InstantCommand(() -> CommandScheduler.getInstance().schedule(outtake)))
+                .andThen(new WaitCommand(4))
+                .andThen(new InstantCommand(() -> CommandScheduler.getInstance().cancel(outtake)));
 //                .andThen(outtake);
     }
 

diff --git a/src/main/java/frc/robot/commands/shooter/SetShooterPIDVelocityFromState.java b/src/main/java/frc/robot/commands/shooter/SetShooterPIDVelocityFromState.java
@@ -43,6 +43,7 @@ public void execute() {
 
         double pidOutput;
 
+        flywheelSubsystem.setTargetVelocity(shooterStateSupplier.get().rpmVelocity);
         if (shooterStateSupplier.get().rpmVelocity < 3500){
             pidOutput = flywheelControllerLow.calculate(flywheelSubsystem.getFlywheelRPM(), shooterStateSupplier.get().rpmVelocity);
         } else {

diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
@@ -30,7 +30,7 @@ public class IntakeSubsystem extends SubsystemBase {
     private final DoubleSolenoid rightIntakeSolenoid;
 
     public IntakeSubsystem() {
-        TalonConfiguration config = new TalonConfiguration(NeutralMode.Coast, InvertType.None);
+        TalonConfiguration config = new TalonConfiguration(NeutralMode.Coast, InvertType.InvertMotorOutput);
         intakeMotor = TalonFXFactory.createTalonFX(INTAKE_MOTOR_ID, config, MANI_CAN_BUS);
         leftintakeSolenoid = new DoubleSolenoid(PNEUMATICS_HUB_ID, PneumaticsModuleType.REVPH, INTAKE_SOLENOID_LEFT_FORWARD, INTAKE_SOLENOID_LEFT_BACKWARD);
         rightIntakeSolenoid = new DoubleSolenoid(PNEUMATICS_HUB_ID, PneumaticsModuleType.REVPH, INTAKE_SOLENOID_RIGHT_FORWARD, INTAKE_SOLENOID_RIGHT_BACKWARD);