added safety init

More robot tuning.
Chimney has IR sensors at 1 and 2.
Tweaked drive D value.
Drive the pivot down slightly.

Author: coach-streeter

src/main/java/frc/robot/Robot.java

@@ -35,7 +35,7 @@ public class Robot extends TimedRobot {
   @Override
   @SuppressWarnings("deprecation")
   public void robotInit() {
-    // Instantiate our RobotContainer.  This will perform all our button bindings,
+    // Instantiate our RobotContainer. This will perform all our button bindings,
     // and put our autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
 
@@ -81,6 +81,9 @@ public void disabledPeriodic() {
    */
   @Override
   public void autonomousInit() {
+    // ensure robot "safety" before anything else when entering new mode
+    RobotContainer.safetyInit();
+
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
 
     // schedule the autonomous command (example)
@@ -98,6 +101,9 @@ public void autonomousPeriodic() {
 
   @Override
   public void teleopInit() {
+    // ensure robot "safety" before anything else when entering new mode
+    RobotContainer.safetyInit();
+
     // This makes sure that the autonomous stops running when
     // teleop starts running. If you want the autonomous to
     // continue until interrupted by another command, remove
@@ -106,7 +112,7 @@ public void teleopInit() {
       m_autonomousCommand.cancel();
     }
 
-    // RobotContainer.compressor.start();
+    // for safety reasons, call
   }
 
   /**

src/main/java/org/mayheminc/robot2020/Constants.java

@@ -34,8 +34,8 @@ public final class Talon {
         public static final int INTAKE_ROLLERS = 9;
         public static final int INTAKE_PIVOT = 10;
 
-        public static final int MAGAZINE_TURNTABLE = 11;
-        public static final int MAGAZINE_CHIMNEY = 12;
+        public static final int REVOLVER_TURNTABLE = 11;
+        public static final int CHIMNEY_ROLLER = 12;
 
         public static final int CLIMBER_WINCH_LEFT = 14; // high current
         public static final int CLIMBER_WINCH_RIGHT = 13; // high current

src/main/java/org/mayheminc/robot2020/RobotContainer.java

@@ -32,7 +32,7 @@ public class RobotContainer {
     // The robot's subsystems and commands are defined here...
 
     public static final Climber climber = new Climber();
-    public static final Magazine magazine = new Magazine();
+    public static final Revolver revolver = new Revolver();
     public static final ShooterWheel shooterWheel = new ShooterWheel();
     public static final Hood hood = new Hood();
     public static final Turret turret = new Turret();
@@ -74,33 +74,50 @@ public static void init() {
         shooterWheel.init();
     }
 
+    public static void safetyInit() {
+        hood.setVBus(0.0);
+        turret.setVBus(0.0);
+        climber.setPistons(false);
+    }
+
     private void configureDefaultCommands() {
         drive.setDefaultCommand(new DriveDefault());
         // intake.setDefaultCommand(new IntakeExtenderVBus());
-        magazine.setDefaultCommand(new MagazineDefault());
-
-        // TODO:  Figure out if the current approach of "AirCompressorDefault()" is the way to go for compressor control.
-        // KBS doesn't think the below is the right way to have the compressor be on "by default" because
-        // it would require there to always be a command running to keep the compressor off.  However, that
-        // is a good way to ensure it doesn't get left off by accident.  Not quite sure how to handle this;
-        // would really rather that other commands which need the compressor off (such as a high-power command
-        // which wants all the battery power available) would turn the compressor off when the command starts
-        // and off when the command ends.)  Then again, maybe the "defaultCommand" is a good way to do this
+        revolver.setDefaultCommand(new RevolverDefault());
+
+        // TODO: Figure out if the current approach of "AirCompressorDefault()" is the
+        // way to go for compressor control.
+        // KBS doesn't think the below is the right way to have the compressor be on "by
+        // default" because
+        // it would require there to always be a command running to keep the compressor
+        // off. However, that
+        // is a good way to ensure it doesn't get left off by accident. Not quite sure
+        // how to handle this;
+        // would really rather that other commands which need the compressor off (such
+        // as a high-power command
+        // which wants all the battery power available) would turn the compressor off
+        // when the command starts
+        // and off when the command ends.) Then again, maybe the "defaultCommand" is a
+        // good way to do this
         // and I just don't understand the style yet.
         // compressor.setDefaultCommand(new AirCompressorDefault());
     }
 
     private void configureAutonomousPrograms() {
         LinkedList<Command> autonomousPrograms = new LinkedList<Command>();
-        // TODO:  fix "wierdness" with auto program selection - sometimes doesn't seem to work
-        // TODO:  fix so that auto program is shown not just when changed (as shows old setting sometimes)
-        
-        // autonomousPrograms.push(/* 01 */ new StayStill());
-        autonomousPrograms.push(/* 01 */ new DriveStraightOnly());
-        autonomousPrograms.push(/* 00 */ new TrenchAuto());
 
-        // autonomousPrograms.push( new ShooterReadyAimFire());
-        // autonomousPrograms.push(new TestTurret());
+        autonomousPrograms.push(/* 11 */ new StayStill());
+        autonomousPrograms.push(/* 10 */ new StartBWDriveOnlyToRP());
+        autonomousPrograms.push(/* 09 */ new StartBWDriveOnlyToWall());
+        autonomousPrograms.push(/* 08 */ new StartFWDriveOnlyToRP());
+        autonomousPrograms.push(/* 07 */ new StartFWDriveOnlyToWall());
+        autonomousPrograms.push(/* 06 */ new StartBWShoot3ThenToRP());
+        autonomousPrograms.push(/* 05 */ new StartBWShoot3ThenToWall());
+        autonomousPrograms.push(/* 04 */ new StartFWShoot3ThenToRP());
+        autonomousPrograms.push(/* 03 */ new StartFWShoot3ThenToWall());
+        autonomousPrograms.push(/* 02 */ new StartFWRendezvous());
+        autonomousPrograms.push(/* 01 */ new StartBWOppTrench());
+        autonomousPrograms.push(/* 00 */ new StartBWTrench());
 
         autonomous.setAutonomousPrograms(autonomousPrograms);
 
@@ -160,8 +177,8 @@ private void configureDriverPadButtons() {
         // about -30 degrees
         // DRIVER_PAD.DRIVER_PAD_D_PAD_RIGHT.whileHeld(new
         // ShooterSetTurretVBus(+0.2));// about +30 degrees
-        DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new HoodSetAbs(Hood.HOOD_INITIATION_LINE_POSITION));
-        DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whenPressed(new HoodSetAbs(Hood.HOOD_TARGET_ZONE_POSITION));
+        DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new HoodSetAbsWhileHeld(Hood.HOOD_INITIATION_LINE_POSITION));
+        DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whenPressed(new HoodSetAbsWhileHeld(Hood.HOOD_TARGET_ZONE_POSITION));
 
         // DRIVER_PAD.DRIVER_PAD_D_PAD_LEFT.whenPressed(new
         // ShooterSetTurretRel(-200.0));
@@ -184,7 +201,8 @@ private void configureDriverPadButtons() {
         DRIVER_PAD.DRIVER_PAD_YELLOW_BUTTON.whenPressed(new ShooterWheelSet(3000));
 
         DRIVER_PAD.DRIVER_PAD_BACK_BUTTON.whileHeld(new DriveStraight(0.1));
-        // TODO:  above hard-coded constant (3000) should be a named constant from Shooter.java
+        // TODO: above hard-coded constant (3000) should be a named constant from
+        // Shooter.java
 
         DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whileHeld(new FeederSet(1.0));
 
@@ -194,37 +212,36 @@ private void configureOperatorStickButtons() {
     }
 
     private void configureOperatorPadButtons() {
-        OPERATOR_PAD.OPERATOR_PAD_BUTTON_ONE.whileHeld(new MagazineSetTurntable(0.2));
+        OPERATOR_PAD.OPERATOR_PAD_BUTTON_ONE.whileHeld(new RevolverSetTurntable(0.2));
         OPERATOR_PAD.OPERATOR_PAD_BUTTON_TWO.whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN));
-        OPERATOR_PAD.OPERATOR_PAD_BUTTON_THREE.whileHeld(new MagazineSetTurntable(0.5));
+        OPERATOR_PAD.OPERATOR_PAD_BUTTON_THREE.whileHeld(new RevolverSetTurntable(0.5));
         OPERATOR_PAD.OPERATOR_PAD_BUTTON_FOUR.whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_UP));
 
         // new ShooterSetWheel(1000));
-        OPERATOR_PAD.OPERATOR_PAD_BUTTON_FIVE.whileHeld(new ChimneySetChimney(1.0));
-        OPERATOR_PAD.OPERATOR_PAD_BUTTON_SIX.whileHeld(new IntakeSetRollers(-1.0));
+        OPERATOR_PAD.OPERATOR_PAD_BUTTON_FIVE.whileHeld(new ChimneySet(1.0));
+        OPERATOR_PAD.OPERATOR_PAD_BUTTON_SIX.whileHeld(new IntakeSetRollersWhileHeld(-1.0));
 
         OPERATOR_PAD.OPERATOR_PAD_BUTTON_SEVEN.whileHeld(new TurretAimToTarget());
-        OPERATOR_PAD.OPERATOR_PAD_BUTTON_EIGHT.whileHeld(new IntakeSetRollers(1.0));
-        
+        OPERATOR_PAD.OPERATOR_PAD_BUTTON_EIGHT.whileHeld(new IntakeSetRollersWhileHeld(1.0));
+
         OPERATOR_PAD.OPERATOR_PAD_BUTTON_NINE.whenPressed(new ClimberSetPistons(true));
         OPERATOR_PAD.OPERATOR_PAD_BUTTON_TEN.whenPressed(new ClimberSetPistons(false));
 
-
         // OPERATOR_PAD.OPERATOR_PAD_D_PAD_UP.whenPressed(new
         // IntakeSetPosition(RobotContainer.intake.PIVOT_UP));
         // OPERATOR_PAD.OPERATOR_PAD_D_PAD_DOWN.whenPressed(new
         // IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN));
-        OPERATOR_PAD.OPERATOR_PAD_D_PAD_LEFT.whileHeld(new TurretSetVBus(-0.2));
-        OPERATOR_PAD.OPERATOR_PAD_D_PAD_RIGHT.whileHeld(new TurretSetVBus(+0.2));
+        OPERATOR_PAD.OPERATOR_PAD_D_PAD_LEFT.whileHeld(new TurretSetVBus(-0.4));
+        OPERATOR_PAD.OPERATOR_PAD_D_PAD_RIGHT.whileHeld(new TurretSetVBus(+0.4));
         OPERATOR_PAD.OPERATOR_PAD_D_PAD_UP.whileHeld(new HoodAdjust(+1000.0));
         OPERATOR_PAD.OPERATOR_PAD_D_PAD_DOWN.whileHeld(new HoodAdjust(-1000.0));
 
         OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_UP.whileHeld(new ClimberSetWinchesPower(1.0));
         OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_DOWN.whileHeld(new ClimberSetWinchesPower(-1.0));
 
         // OPERATOR_PAD.OPERATOR_PAD_LEFT_Y_AXIS_UP.whenPressed(new
-        // MagazineSetTurntable());
-        OPERATOR_PAD.OPERATOR_PAD_LEFT_Y_AXIS_DOWN.whileHeld(new ChimneySetChimney(-1.0));
+        // RevolverSetTurntable());
+        OPERATOR_PAD.OPERATOR_PAD_LEFT_Y_AXIS_DOWN.whileHeld(new ChimneySet(-1.0));
     }
 
     /**

src/main/java/org/mayheminc/robot2020/autonomousroutines/TrenchAuto.java

@@ -9,9 +9,9 @@
 
 import org.mayheminc.robot2020.RobotContainer;
 
-import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.CommandBase;
 
-public class HoodSetAbs extends InstantCommand {
+public class HoodSetAbs extends CommandBase {
   double m_set;
 
   /**
@@ -30,4 +30,8 @@ public void initialize() {
     RobotContainer.hood.setPosition(m_set);
   }
 
+  @Override
+  public boolean isFinished() {
+    return (RobotContainer.hood.isAtPosition());
+  }
 }

src/main/java/org/mayheminc/robot2020/commands/ChimneySetChimney.java

@@ -13,17 +13,27 @@
 
 public class IntakeSetPosition extends CommandBase {
   double m_position;
+  boolean m_waitForDone;
 
   /**
-   * Creates a new IntakeSetPosition.
+   * Creates a new IntakeSetPosition, with "wait" set to false
    */
-  public IntakeSetPosition(Double position) {
+  public IntakeSetPosition(double position) {
+    this (position, false);
+  }
+
+  /**
+   * Creates a new IntakeSetPosition
+   */
+  public IntakeSetPosition(double position, boolean wait) {
     // Use addRequirements() here to declare subsystem dependencies.
     addRequirements(RobotContainer.intake);
-    m_position = position;
 
+    m_position = position;
+    m_waitForDone = wait;
   }
 
+
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
@@ -33,6 +43,10 @@ public void initialize() {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return RobotContainer.intake.isPivotAtPosition();
+    if (m_waitForDone) {
+      return RobotContainer.intake.isPivotAtPosition();
+    } else {
+      return true;
+    }
   }
 }