Merge pull request #134 from 2491-NoMythic/pracField5-1

src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto

@@ -5,7 +5,7 @@
       "x": 0.6842044526319189,
       "y": 6.690138362483131
     },
-    "rotation": -125.5
+    "rotation": -117.0
   },
   "command": {
     "type": "sequential",

src/main/deploy/pathplanner/autos/(AP)StartSourceScore3FarSource.auto

@@ -5,7 +5,7 @@
       "x": 0.7523718013035084,
       "y": 4.450354048988052
     },
-    "rotation": 119.3
+    "rotation": 117.3
   },
   "command": {
     "type": "sequential",

src/main/java/frc/robot/RobotContainer.java

@@ -134,7 +134,7 @@ public class RobotContainer {
   BooleanSupplier ForceVisionSup;
   BooleanSupplier GroundIntakeSup;
   BooleanSupplier FarStageAngleSup;
-  BooleanSupplier OperatorRevForPass;
+  BooleanSupplier OperatorRevToZero;
   BooleanSupplier OverStagePassSup;
   BooleanSupplier OppositeStageShotSup;
   BooleanSupplier falseSup;
@@ -187,7 +187,7 @@ public RobotContainer() {
     ManualShootSup = driverController::getR2Button;
     ClimberDownSup = operatorController::getPSButton;
     GroundIntakeSup = driverController::getL1Button;
-    OperatorRevForPass = ()->operatorController.getPOV() != -1;
+    OperatorRevToZero = ()->operatorController.getPOV() != -1;
     SubwooferAngleSup =()-> driverController.getCrossButton()||operatorController.getCrossButton();
     StageAngleSup = ()->operatorController.getTriangleButton()||driverController.getTriangleButton();;
     FarStageAngleSup = ()->operatorController.getSquareButton()||driverController.getSquareButton();
@@ -250,7 +250,7 @@ private void shooterInst() {
   }
   private void angleShooterInst(){
     angleShooterSubsystem = new AngleShooterSubsystem();
-    defaultShooterAngleCommand = new AimShooter(angleShooterSubsystem, AmpAngleSup, HumanPlaySup, SubwooferAngleSup, StageAngleSup, GroundIntakeSup, FarStageAngleSup, OverStagePassSup, OppositeStageShotSup);
+    defaultShooterAngleCommand = new AimShooter(angleShooterSubsystem, AmpAngleSup, HumanPlaySup, SubwooferAngleSup, StageAngleSup, GroundIntakeSup, FarStageAngleSup, OverStagePassSup, OppositeStageShotSup, CenterAmpPassSup);
     angleShooterSubsystem.setDefaultCommand(defaultShooterAngleCommand);
   }
   private void intakeInst() {
@@ -268,7 +268,7 @@ private void indexInit() {
     indexer = new IndexerSubsystem(intakeExists ? intake::isNoteSeen : () -> false);
   }
   private void indexCommandInst() {
-    defaulNoteHandlingCommand = new IndexCommand(indexer, ShootIfReadySup, AimWhileMovingSup, shooter, intake, driveTrain, angleShooterSubsystem, HumanPlaySup, StageAngleSup, SubwooferAngleSup, GroundIntakeSup, FarStageAngleSup, OperatorRevForPass, intakeReverse, OverStagePassSup, OppositeStageShotSup);
+    defaulNoteHandlingCommand = new IndexCommand(indexer, ShootIfReadySup, AimWhileMovingSup, shooter, intake, driveTrain, angleShooterSubsystem, HumanPlaySup, StageAngleSup, SubwooferAngleSup, GroundIntakeSup, FarStageAngleSup, OperatorRevToZero, intakeReverse, OverStagePassSup, OppositeStageShotSup);
     indexer.setDefaultCommand(defaulNoteHandlingCommand);
   }
 
@@ -398,7 +398,7 @@ private void configureBindings() {
         );
         SmartDashboard.putNumber("Indexer Amp Speed", indexerAmpSpeed);
       SequentialCommandGroup orbitAmpShot = new SequentialCommandGroup(
-        new InstantCommand(()->shooter.shootWithSupplier(()->shooterAmpSpeed, true), shooter),
+        new InstantCommand(()->shooter.setTargetVelocity(shooterAmpSpeed, shooterAmpSpeed, 50, 50), shooter),
         new MoveMeters(driveTrain, 0.015, 0.5, 0, 0),
         new InstantCommand(driveTrain::pointWheelsInward, driveTrain),
         new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(50), angleShooterSubsystem),
@@ -607,8 +607,8 @@ public void execute() {
     }
     if(angleShooterExists) {
       //the same command that we use during teleop, but all the buttons that would aim the shooter anywhere other than the speaker are set to false.
-      NamedCommands.registerCommand("autoAimAtSpeaker", new AimShooter(angleShooterSubsystem, ()->false, ()->false, ()->false, ()->false, ()->false, ()->false, ()->false, ()->false));
-      SmartDashboard.putData("autoAimAtSpeaker", new AimShooter(angleShooterSubsystem, ()->false, ()->false, ()->false, ()->false, ()->false, ()->false, ()->false, ()->false));
+      NamedCommands.registerCommand("autoAimAtSpeaker", new AimShooter(angleShooterSubsystem, ()->false, ()->false, ()->false, ()->false, ()->false, ()->false, ()->false, ()->false, ()->false));
+      SmartDashboard.putData("autoAimAtSpeaker", new AimShooter(angleShooterSubsystem, ()->false, ()->false, ()->false, ()->false, ()->false, ()->false, ()->false, ()->false, ()->false));
     }
     if (indexerExists&&intakeExists) {
       NamedCommands.registerCommand("groundIntake", new AutoGroundIntake(indexer, intake, driveTrain));

src/main/java/frc/robot/commands/AimShooter.java

@@ -20,6 +20,7 @@ public class AimShooter extends Command {
 	BooleanSupplier farStageAngleSup;
 	BooleanSupplier OverStageAngleSup;
 	BooleanSupplier OppositeStageShotSup;
+	BooleanSupplier LowPassAngleSup;
 	/**
 	 * A Command that will control the angle of the shooter. If none of the supplied buttons are pressed, than it will automatically aim at the speaker
 	 * @param angleShooterSubsystem the subsytem to control the angle of the shooter
@@ -33,7 +34,7 @@ public class AimShooter extends Command {
    	 * @param OppositeStageShotSup button to press to aim at the speaker from the opposite side of the stage (from the speaker)
 	 */
 	public AimShooter(AngleShooterSubsystem angleShooterSubsystem, BooleanSupplier AmpSup, BooleanSupplier humanPlayerSupplier,
-					  BooleanSupplier SubwooferSupplier1, BooleanSupplier StageAngleSupplier, BooleanSupplier groundIntakeSup, BooleanSupplier farStageAngleSup, BooleanSupplier OverStageAngleSup, BooleanSupplier OppositeStageShotSup) {
+					  BooleanSupplier SubwooferSupplier1, BooleanSupplier StageAngleSupplier, BooleanSupplier groundIntakeSup, BooleanSupplier farStageAngleSup, BooleanSupplier OverStageAngleSup, BooleanSupplier OppositeStageShotSup, BooleanSupplier LowPassAngleSup) {
 		this.angleShooterSubsystem = angleShooterSubsystem;
 		this.AmpSup = AmpSup;
 		this.humanPlayerSupplier = humanPlayerSupplier;
@@ -43,6 +44,7 @@ public AimShooter(AngleShooterSubsystem angleShooterSubsystem, BooleanSupplier A
 		this.farStageAngleSup = farStageAngleSup;
 		this.OverStageAngleSup = OverStageAngleSup;
 		this.OppositeStageShotSup = OppositeStageShotSup;
+		this.LowPassAngleSup = LowPassAngleSup;
 		addRequirements(angleShooterSubsystem);
 	}
 
@@ -70,6 +72,8 @@ public void execute() {
 			angleShooterSubsystem.setDesiredShooterAngle(ShooterConstants.OVER_STAGE_PASS_ANGLE);
 		} else if(OppositeStageShotSup.getAsBoolean()) {
 			angleShooterSubsystem.setDesiredShooterAngle(Field.OPPOSITE_STAGE_SHOOTER_ANGLE);
+		} else if (LowPassAngleSup.getAsBoolean()) {
+			angleShooterSubsystem.setDesiredShooterAngle(12);
 		} else {
 			angleShooterSubsystem.setDesiredShooterAngle(angleShooterSubsystem.calculateSpeakerAngle());
 		}

src/main/java/frc/robot/commands/IndexCommand.java

@@ -6,6 +6,7 @@
 
 import static frc.robot.settings.Constants.DriveConstants.MAX_VELOCITY_METERS_PER_SECOND;
 import static frc.robot.settings.Constants.ShooterConstants.PRAC_AMP_RPS;
+import static frc.robot.settings.Constants.ShooterConstants.HUMAN_PLAYER_RPS;
 import static frc.robot.settings.Constants.ShooterConstants.LONG_SHOOTING_RPS;
 import static frc.robot.settings.Constants.ShooterConstants.PASS_RPS;
 
@@ -45,7 +46,7 @@ public class IndexCommand extends Command {
   BooleanSupplier stageAngleSup;
   BooleanSupplier subwooferAngleSup;
   BooleanSupplier farStageAngleSup;
-  BooleanSupplier operatorOverStageRev;
+  BooleanSupplier revToZeroSup;
   BooleanSupplier intakeReverse; 
   BooleanSupplier OverStagePassSup; 
   BooleanSupplier OppositeStageShotSup; 
@@ -89,7 +90,7 @@ public IndexCommand(IndexerSubsystem m_IndexerSubsystem, BooleanSupplier shootIf
     this.stageAngleSup = stageAngleSup;
     this.farStageAngleSup = farStageAngleSup;
     this.groundIntakeSup = groundIntakeSup;
-    this.operatorOverStageRev = operatorRevSup;
+    this.revToZeroSup = operatorRevSup;
     this.intakeReverse = intakeReverse;
     this.OverStagePassSup = OverStagePassSup;
     this.OppositeStageShotSup = OppositeStageShotSup;
@@ -123,7 +124,7 @@ public void execute() {
         intake.setNoteHeld(false);
         if(humanPlayerSupplier.getAsBoolean()) {
           m_Indexer.set(IndexerConstants.HUMAN_PLAYER_INDEXER_SPEED);
-          shooter.shootSameRPS(ShooterConstants.HUMAN_PLAYER_RPS);
+          shooter.setTargetVelocity(HUMAN_PLAYER_RPS, HUMAN_PLAYER_RPS, 40, 40);
           intake.intakeOff();
         } else {
           if(!groundIntakeSup.getAsBoolean()) {
@@ -144,8 +145,8 @@ public void execute() {
           idleReving = false;
         }
       } else {
-        if (operatorOverStageRev.getAsBoolean()){ 
-          shooter.shootRPSWithCurrent(PASS_RPS, 20, 30);
+        if (revToZeroSup.getAsBoolean()){ 
+          shooter.shootRPSWithCurrent(0, 20, 30);
           idleReving = false;
         } else {
           shooter.shootRPSWithCurrent(LONG_SHOOTING_RPS, 20, 30);

src/main/java/frc/robot/settings/Constants.java

@@ -466,8 +466,8 @@ public final class Field{
 
     public static final double CALCULATED_SHOOTER_RED_SPEAKER_X = AMP_SIDE_OUTER_TAPE_CORNER_RED_X+3.213;
     public static final double CALCULATED_RED_SPEAKER_Y = AMP_SIDE_OUTER_TAPE_CORNER_RED_Y+1.263;
-    public static final double RED_SPEAKER_Y = 5.613;//home field: 5.613 HCPA: 5.68
-    public static final double SHOOTER_RED_SPEAKER_X = 16.582;//home field: 16.582 HCPA: 16.55
+    public static final double RED_SPEAKER_Y = 5.68;//home field: 5.613 HCPA: 5.68
+    public static final double SHOOTER_RED_SPEAKER_X = 16.55;//home field: 16.582 HCPA: 16.55
     public static final double ROBOT_RED_SPEAKER_X = SHOOTER_RED_SPEAKER_X-0.165;//changed so that shots from the side wil aim to the opposite side, and bank in
 
     public static final double CALCULATED_SHOOTER_BLUE_SPEAKER_X = AMP_SIDE_OUTER_TAPE_CORNER_BLUE_X-3.213; //changed so that shots from the side wil aim to the opposite side, and bank in

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

@@ -198,17 +198,17 @@ public void turnOff(){
    * @return Updated isReving
    */
   private static int updateIsReving(int revState, double targetSpeed, double speed){
-    double revUpEn =  -10;
-    double revUpDis = -2;
-    double revDownEn = 10;
+    double revUpEn =  10;
+    double revUpDis = 2;
+    double revDownEn = -10;
     double revDownDis = 2;
-    targetSpeed = Math.abs(targetSpeed);
-    speed = Math.abs(speed);
-    if (speed < targetSpeed + revUpEn){
+    double difference = targetSpeed - speed;
+    SmartDashboard.putNumber("SHOOTER/desired speed difference", difference);
+    if (difference>revUpEn){
       revState = 1;
-    } else if (speed > targetSpeed + revDownEn && targetSpeed<10){
+    } else if (difference<revDownEn && targetSpeed<10){
       revState = -1;
-    } else if((revState == 1 && speed > targetSpeed + revUpDis) || (revState == -1 && speed < targetSpeed + revDownDis)) {
+    } else if(revState != 0 && Math.abs(difference)<revDownDis) {
       revState = 0;
     }
     return revState;
@@ -221,10 +221,10 @@ private static void updateMotor(TalonFX shooterMotor, int revState, double targe
     shooterMotor.getConfigurator().apply(currentLimits);
     if(targetSpeed == 0) {
       shooterMotor.set(0);
-    } else if (revState == -1) {
-      shooterMotor.set(-Math.signum(targetSpeed));
-    } else if(revState == 1) {
-      shooterMotor.set(Math.signum(targetSpeed));
+    } else if(targetSpeed<0) {
+      shooterMotor.setControl(new VelocityDutyCycle(targetSpeed).withSlot(0));
+    } else if (revState != 0) {
+      shooterMotor.set(revState);
     } else {
       shooterMotor.setControl(new VelocityDutyCycle(targetSpeed).withSlot(0));
     }