Merge branch 'master' of https://github.com/FRC1519/2020-Robot

Author: Ihedgehogs

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

@@ -19,6 +19,7 @@
 
 import org.mayheminc.robot2020.autonomousroutines.*;
 import org.mayheminc.robot2020.commands.*;
+import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits;
 import org.mayheminc.robot2020.subsystems.*;
 
 /**
@@ -32,8 +33,11 @@ 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 Shooter shooter = new Shooter();
+    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();
+    public static final Feeder feeder = new Feeder();
     public static final Drive drive = new Drive();
     public static final Intake intake = new Intake();
     public static final Autonomous autonomous = new Autonomous();
@@ -62,40 +66,55 @@ public RobotContainer() {
         pidtuner = new PidTuner(RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_SIX,
                 RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_SEVEN,
                 RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_ELEVEN,
-                RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_TEN, shooter);
+                RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_TEN, drive);
 
         cameraLights.set(true);
     }
 
     public static void init() {
-        shooter.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());
+
+        // 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());
+        // 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(/* 00 */ new TrenchAuto());
-        // autonomousPrograms.push( new ShooterReadyAimFire());
-        // autonomousPrograms.push(new TestTurret());
+
+        autonomousPrograms.push(/* 12 */ new StayStill());
+        autonomousPrograms.push(/* 11 */ new StartBWDriveOnlyToRP());
+        autonomousPrograms.push(/* 10 */ new StartBWDriveOnlyToWall());
+        autonomousPrograms.push(/* 09 */ new StartFWDriveOnlyToRP());
+        autonomousPrograms.push(/* 08 */ new StartFWDriveOnlyToWall());
+        autonomousPrograms.push(/* 07 */ new StartBWShoot3ThenToRP());
+        autonomousPrograms.push(/* 06 */ new StartBWShoot3ThenToWall());
+        autonomousPrograms.push(/* 05 */ new StartFWShoot3ThenToRP());
+        autonomousPrograms.push(/* 04 */ new StartFWShoot3ThenToWall());
+        autonomousPrograms.push(/* 03 */ new StartFWRendezvous());
+        autonomousPrograms.push(/* 02 */ new StartBWOppTrench());
+        autonomousPrograms.push(/* 01 */ new StartBWTrench3());
+        autonomousPrograms.push(/* 00 */ new StartBWTrench5());
 
         autonomous.setAutonomousPrograms(autonomousPrograms);
 
@@ -155,8 +174,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 ShooterSetHoodAbs(Shooter.HOOD_INITIATION_LINE_POSITION));
-        DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whenPressed(new ShooterSetHoodAbs(Shooter.HOOD_TARGET_ZONE_POSITION));
+        DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new HoodSetAbsWhileHeld(Hood.INITIATION_LINE_POSITION));
+        DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whenPressed(new HoodSetAbsWhileHeld(Hood.STARTING_POSITION));
 
         // DRIVER_PAD.DRIVER_PAD_D_PAD_LEFT.whenPressed(new
         // ShooterSetTurretRel(-200.0));
@@ -173,52 +192,59 @@ private void configureDriverPadButtons() {
         // DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whileHeld(new ShooterSetHoodVBus(-1.0));
 
         // Debug shooter pid velocity
-        DRIVER_PAD.DRIVER_PAD_BLUE_BUTTON.whenPressed(new ShooterAdjustWheel(100.0));
-        DRIVER_PAD.DRIVER_PAD_GREEN_BUTTON.whenPressed(new ShooterAdjustWheel(-100.0));
-        DRIVER_PAD.DRIVER_PAD_RED_BUTTON.whenPressed(new ShooterSetWheelVBus(0.0));
-        DRIVER_PAD.DRIVER_PAD_YELLOW_BUTTON.whenPressed(new ShooterSetWheel(3000));
-        // TODO:  above hard-coded constant (3000) should be a named constant from Shooter.java
+        DRIVER_PAD.DRIVER_PAD_BLUE_BUTTON.whenPressed(new ShooterWheelAdjust(50.0));
+        DRIVER_PAD.DRIVER_PAD_GREEN_BUTTON.whenPressed(new ShooterWheelAdjust(-50.0));
+        DRIVER_PAD.DRIVER_PAD_RED_BUTTON.whenPressed(new ShooterWheelSetVBus(0.0));
+        DRIVER_PAD.DRIVER_PAD_YELLOW_BUTTON.whenPressed(new ShooterWheelSet(ShooterWheel.INITIATION_LINE_SPEED));
+
+        DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whenHeld(new ShooterFiringSequence(60.0));
+        DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whenReleased(new ShooterCeaseFire());
 
-        DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whileHeld(new ShooterSetFeeder(1.0));
+        DRIVER_PAD.DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON.whileHeld(new ShooterCloseFiringSequence(60.0));
+        DRIVER_PAD.DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON.whenReleased(new ShooterCeaseFire());
+
+        DRIVER_PAD.DRIVER_PAD_BACK_BUTTON.whenPressed(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 240, 0));
+        DRIVER_PAD.DRIVER_PAD_START_BUTTON.whenPressed(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 240, 0));
+
+        // DRIVER_PAD.DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON.whileHeld(new
+        // FeederSet(1.0));
 
     }
 
     private void configureOperatorStickButtons() {
     }
 
     private void configureOperatorPadButtons() {
-        OPERATOR_PAD.OPERATOR_PAD_BUTTON_ONE.whileHeld(new MagazineSetTurntable(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_FOUR.whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_UP));
+        OPERATOR_PAD.OPERATOR_PAD_BUTTON_ONE.whileHeld(new RevolverSetTurntable(0.2));
+        OPERATOR_PAD.OPERATOR_PAD_BUTTON_TWO.whenPressed(new IntakeSetPosition(Intake.PIVOT_DOWN));
+        OPERATOR_PAD.OPERATOR_PAD_BUTTON_THREE.whileHeld(new RevolverSetTurntable(1.0));
+        OPERATOR_PAD.OPERATOR_PAD_BUTTON_FOUR.whenPressed(new IntakeSetPosition(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.whenPressed(new ShooterAimToTarget());
+        OPERATOR_PAD.OPERATOR_PAD_BUTTON_EIGHT.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_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 ShooterSetTurretVBus(-0.2));
-        OPERATOR_PAD.OPERATOR_PAD_D_PAD_RIGHT.whileHeld(new ShooterSetTurretVBus(+0.2));
-        OPERATOR_PAD.OPERATOR_PAD_D_PAD_UP.whileHeld(new ShooterAdjustHood(+1000.0));
-        OPERATOR_PAD.OPERATOR_PAD_D_PAD_DOWN.whileHeld(new ShooterAdjustHood(-1000.0));
+        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));
 
-        // TODO:  Consider if below should use "variable power" for climber or just always full speed?
         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/DriveStraight.java renamed to src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraightOnly.java

@@ -9,20 +9,22 @@
 
 import org.mayheminc.robot2020.commands.*;
 import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits;
-import org.mayheminc.robot2020.subsystems.Shooter;
 
 import edu.wpi.first.wpilibj2.command.*;
 
-public class DriveStraight extends SequentialCommandGroup {
+public class DriveStraightOnly extends SequentialCommandGroup {
   /**
    * Add your docs here.
    */
-  public DriveStraight() {
+  public DriveStraightOnly() {
 
-    addCommands(new DriveZeroGyro());
-    // addCommands(new DriveStraightOnHeading(0.2, DistanceUnits.INCHES, 100, 0));
-    addCommands(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 50, 0));
-    addCommands(new ShooterSetHoodAbs(Shooter.HOOD_INITIATION_LINE_POSITION));
+    addCommands(new DriveZeroGyro(0.0));
+
+    addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 50, 0));
+
+    addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 100, 270));
+
+    // addCommands(new ShooterSetHoodAbs(Shooter.HOOD_INITIATION_LINE_POSITION));
     // addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 100, 0));
 
   }

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

@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package org.mayheminc.robot2020.autonomousroutines;
+
+import org.mayheminc.robot2020.commands.*;
+import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits;
+import org.mayheminc.robot2020.subsystems.Intake;
+
+import edu.wpi.first.wpilibj2.command.*;
+
+public class StartBWDriveOnlyToRP extends SequentialCommandGroup {
+        /**
+         * Add your docs here.
+         */
+        public StartBWDriveOnlyToRP() {
+
+                // start backwards
+                addCommands(new DriveZeroGyro(180.0));
+
+                // lower the intake
+                addCommands(new IntakeSetPosition(Intake.PIVOT_DOWN));
+
+                // then, drive to the rendezvous point
+                addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 12, 180));
+        }
+}

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

@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package org.mayheminc.robot2020.autonomousroutines;
+
+import org.mayheminc.robot2020.commands.*;
+import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits;
+import org.mayheminc.robot2020.subsystems.Intake;
+
+import edu.wpi.first.wpilibj2.command.*;
+
+public class StartBWDriveOnlyToWall extends SequentialCommandGroup {
+        /**
+         * Add your docs here.
+         */
+        public StartBWDriveOnlyToWall() {
+
+                // start backwards
+                addCommands(new DriveZeroGyro(180.0));
+
+                // lower the intake
+                addCommands(new IntakeSetPosition(Intake.PIVOT_DOWN));
+
+                // then, drive towards the wall
+                addCommands(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 40, 180));
+        }
+}