fixed auto

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

@@ -29,14 +29,14 @@ public static final class DipSwitches{
     public static final class DrivetrainConstants {
         public static final class Autonomous {
             public static final class Turning {
-                public static final double TURN_SPEED = 0.15;
-                public static final double TURN_ADJUSMENT =220;
+                public static final double TURN_SPEED = 0.25;
+                public static final double TURN_ADJUSMENT =237;
 
-                public static final int HHH_TURN = 86;
+                public static final int HHH_TURN = 72;
                 // public static final int SECOND_HHH_TURN = -70;
                 //public static final int SECOND_HHH_OFFSET = 46;
                 public static final double TURNING_PP = 0.0006;
-                public static final int SECOND_HHH_TURN = -30;
+                public static final int SECOND_HHH_TURN = 40;
             }
             public static final class Distance {
                 public static final int HIGH_GOAL_SHOT = -40000;
@@ -45,7 +45,7 @@ public static final class Distance {
             }
             public static final class Offsets {
                 public static final int AUTO_H_H_OFFSET = 0;//-56000;
-                public static final int AUTO_L_H_OFFSET = -56000;
+                public static final int AUTO_L_H_OFFSET = -41000;
                 public static final int AUTO_LF_H_OFFSET = -40000;
                 public static final double STEP_TWO_RETURN_OFFSET = -10000;
             }
@@ -216,11 +216,11 @@ public static final class ShooterMotionParameters {
             public static final double SHOOTER_VELOCITY_HIGH = -4100; //0.17 //0.175
             public static final double NOMINAL_HIGH_VELOCITY = 4080;
 
-            public static final double SHOOTER_VELOCITY_HIGH_FAR = -4900; //0.17 //0.175
-            public static final double NOMINAL_HIGH_VELOCITY_FAR = 4880;
+            public static final double SHOOTER_VELOCITY_HIGH_FAR = -5520; //0.17 //0.175
+            public static final double NOMINAL_HIGH_VELOCITY_FAR = 5500;
 
 
-            public static final double KP = 0.00011;
+            public static final double KP = 0.00012;
             public static final double KI = 0;
             public static final double KD = 0;
             public static final double KF = 0.00008947; // 0.17/1900

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

@@ -78,7 +78,7 @@ public class RobotContainer {
 
     private final JoystickButton stopAll = new JoystickButton(buttonBoard, 4);
 
-    private final JoystickButton hookZeroButton = new JoystickButton(rightJoystick, 7);
+    //private final JoystickButton hookZeroButton = new JoystickButton(rightJoystick, 7);
     private final JoystickButton HHHButton = new JoystickButton(leftJoystick, 10);
     private final JoystickButton TurnButton = new JoystickButton(leftJoystick, 11);
 
@@ -87,9 +87,11 @@ public class RobotContainer {
     private final JoystickButton indexTwoButton = new JoystickButton(buttonBoard, gitforcepushorginmaster);
 
     private final JoystickButton climberControlButton = new JoystickButton(leftJoystick, 3);
-    private final JoystickButton climberTiltZeroButton = new JoystickButton(rightJoystick, 8);
+    //private final JoystickButton climberTiltZeroButton = new JoystickButton(rightJoystick, 8);
 
     private final JoystickButton climberTiltCenterButton = new JoystickButton(rightJoystick, 6);
+    private final JoystickButton shootWayDowntownButton = new JoystickButton(leftJoystick, 6);
+
 
     //private final JoystickButton lockStationariesButton = new JoystickButton(gamepad, 1);
     //private final JoystickButton nextBarButton = new JoystickButton(gamepad, 2);
@@ -104,6 +106,8 @@ public class RobotContainer {
     private final IntakeIntake intakeIntakeCommand = new IntakeIntake(intake);
     private final IntakeEject intakeEjectCommand = new IntakeEject(intake);
     private final BetterIntakeStop intakeStopCommand = new BetterIntakeStop(intake);
+    private final ShootWayDowntown shootWayDowntownCommand = new ShootWayDowntown(shooter);
+    private final IndexerFeedWayDowntown indexWayDowntownCommand = new IndexerFeedWayDowntown(indexer, shooter);
     private final ShootHigh shootHighCommand = new ShootHigh(shooter);
     private final ShootLow shootLowCommand = new ShootLow(shooter);
     private final ShootEject shootEjectCommand = new ShootEject(shooter);
@@ -121,7 +125,7 @@ public class RobotContainer {
     private final HookUpLow hookUpLowCommand = new HookUpLow(climber);
     private final HookDown hookDownCommand = new HookDown(climber); 
 
-    private final HomeAlgorithm homeAlgorithmCommand = new HomeAlgorithm(climber); 
+    // private final HomeAlgorithm homeAlgorithmCommand = new HomeAlgorithm(climber); 
 
     private final NextBar nextBarCommand = new NextBar(climber);
     private final LockStationary lockStationariesCommand = new LockStationary(climber);
@@ -158,7 +162,8 @@ private void configureButtonBindings() {
         shootLowButton.whileHeld(new ParallelCommandGroup(shootLowCommand, feedLowCommand));
         ejectButton.whileHeld(new ParallelCommandGroup(intakeEjectCommand, indexerEjectCommand, shootEjectCommand));
 
-        hookZeroButton.whileHeld(hookZeroCommand);
+        shootWayDowntownButton.whileHeld(new ParallelCommandGroup(shootWayDowntownCommand, indexWayDowntownCommand));
+        //hookZeroButton.whileHeld(hookZeroCommand);
 
         // hookDownButton.whenPressed(hookDownCommand);
         // hookUpButton.whenPressed(hookUpCommand);
@@ -167,9 +172,9 @@ private void configureButtonBindings() {
         hookUpLowButton.whenPressed(hookUpLowCommand);
         hookDownButton.whenPressed(hookDownCommand);
 
-        climberTiltZeroButton.whenPressed(homeAlgorithmCommand);
+        //climberTiltZeroButton.whenPressed(homeAlgorithmCommand);
 
-        climberTiltCenterButton.whenPressed(centerClimberCommand);
+        climberTiltCenterButton.whileHeld(centerClimberCommand);
 
         nextBarButton.whenPressed(nextBarCommand);
         lockStationariesButton.whenPressed(lockStationariesCommand);

src/main/java/frc/robot/commands/autonomous/AutonProcedureHH.java

@@ -34,7 +34,7 @@ public AutonProcedureHH(Drivetrain drivetrain, Intake intake, Indexer indexer, S
         addCommands(new ParallelDeadlineGroup(new WaitCommand(0.6), new IntakeIntake(intake)));//drop the intake
         addCommands(new StepOne(intake, indexer, drivetrain)); //drive back and grab another ball
         addCommands(new ParallelDeadlineGroup(new StepTwo(drivetrain, Constants.DrivetrainConstants.Autonomous.Offsets.AUTO_H_H_OFFSET), new BetterIntakeStop(intake), new IndexerStop(indexer), new ShootHigh(shooter))); //drive back to correct point to sink two high shots
-        addCommands(new ParallelDeadlineGroup(new WaitCommand(6), new ShootHigh(shooter),  new IndexerFeedHigh(indexer, shooter))); //hold down the shoot high button for the same number of seconds as the wait command
+        addCommands(new ParallelDeadlineGroup(new WaitCommand(4), new ShootHigh(shooter),  new IndexerFeedHigh(indexer, shooter))); //hold down the shoot high button for the same number of seconds as the wait command
         addCommands(new ParallelCommandGroup(new IndexerStop(indexer), new ShootStop(shooter))); //stop everything
     }
 }

src/main/java/frc/robot/commands/autonomous/AutonProcedureLH.java

@@ -38,7 +38,7 @@ public AutonProcedureLH(Drivetrain drivetrain, Intake intake, Indexer indexer, S
         addCommands(new InstantCommand(() -> { shooter.set(0); }, shooter)); //stops the shoot
         addCommands(new StepOne(intake, indexer, drivetrain)); //drive back and grab another ball
         addCommands(new ParallelDeadlineGroup(new StepTwo(drivetrain, Constants.DrivetrainConstants.Autonomous.Offsets.AUTO_L_H_OFFSET), new BetterIntakeStop(intake), new IndexerStop(indexer), new ShootHigh(shooter))); //drive back to start point(maybe just put drivetrain falcons in brake?)
-        addCommands(new ParallelDeadlineGroup(new WaitCommand(3), new ShootHigh(shooter),  new IndexerFeedHigh(indexer, shooter))); //shoot your next loaded ball into high goal(needs to be tested)
+        addCommands(new ParallelDeadlineGroup(new WaitCommand(2), new ShootHigh(shooter),  new IndexerFeedHigh(indexer, shooter))); //shoot your next loaded ball into high goal(needs to be tested)
 
         addCommands(new ParallelCommandGroup(new IndexerStop(indexer), new ShootStop(shooter))); //stop everything
     }

src/main/java/frc/robot/commands/climber/ClimberCenter.java

@@ -21,7 +21,6 @@ public class ClimberCenter extends CommandBase {
 
     public ClimberCenter(Climber climber) {
         this.climber = climber;
-        done = false;
 
         // Use addRequirements() here to declare subsystem dependencies.
         addRequirements(climber);
@@ -30,6 +29,13 @@ public ClimberCenter(Climber climber) {
     // Called when the command is initially scheduled.
     @Override
     public void initialize() {
+        done = false;
+
+    }
+
+    // Called every time the scheduler runs while the command is scheduled.
+    @Override
+    public void execute() {
         if(climber.tiltAngle()>0){
             climber.setTilt(-ClimberConstants.ClimberMotionParameters.CLIMBER_TILT_ZERO_SPEED);
         }else if(climber.tiltAngle()<0){
@@ -40,12 +46,6 @@ public void initialize() {
         }
     }
 
-    // Called every time the scheduler runs while the command is scheduled.
-    @Override
-    public void execute() {
-
-    }
-
     // Called once the command ends or is interrupted.
     @Override
     public void end(boolean interrupted) {

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

@@ -180,7 +180,7 @@ public boolean getAutoEnd(int distance) {
     }
 
     public boolean getAutoEnd() {
-        return getCurrentEncoderPosition() <= -150000;
+        return getCurrentEncoderPosition() <= -100000;
     }
 
     public boolean getAutoEndHighReverse() {