Merge pull request #156 from frc5687/Shoot/#149-Toggle-Auto-Shoot

Toggle auto shoot and buttonpad integration

src/main/java/org/frc5687/rapidreact/OI.java

@@ -8,8 +8,11 @@
 import static org.frc5687.rapidreact.subsystems.Catapult.CatapultState.ZEROING;
 import static org.frc5687.rapidreact.util.Helpers.*;
 
+import javax.swing.text.DefaultStyledDocument.ElementSpec;
+
 import edu.wpi.first.math.trajectory.Trajectory;
 import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 
 import org.frc5687.rapidreact.commands.AutoIntake;
@@ -33,7 +36,7 @@ public class OI extends OutliersProxy {
     // Joysticks and gamepads
     private Joystick _translation;
     private Joystick _rotation;
-    private Gamepad _debug;
+    private Gamepad _buttonpad;
 
     // Buttons
     private JoystickButton _autoAim;
@@ -59,28 +62,31 @@ public class OI extends OutliersProxy {
     private JoystickButton _shootSetpointTwo;
     private JoystickButton _shootSetpointThree;
 
+    private JoystickButton _autoShootToggle;
+
     // "Raw" joystick values
     private double yIn = 0;
     private double xIn = 0;
 
     public OI() {
         _translation = new Joystick(0);
         _rotation = new Joystick(1);
-        _debug = new Gamepad(2);
+        _buttonpad = new Gamepad(2);
 
         // debug gamepad
-        _catapultDebugButton = new JoystickButton(_debug, Gamepad.Buttons.A.getNumber());
-        _preloadButton = new JoystickButton(_debug, Gamepad.Buttons.B.getNumber());
-//        _release = new JoystickButton(_debug, Gamepad.Buttons.X.getNumber());
-        _readyToClimb = new JoystickButton(_debug, Gamepad.Buttons.RIGHT_BUMPER.getNumber());
-        _stowClimber = new JoystickButton(_debug, Gamepad.Buttons.Y.getNumber());
-        _rockerFlip = new JoystickButton(_debug, Gamepad.Buttons.X.getNumber());
-        _manualIndexer = new JoystickButton(_debug, Gamepad.Buttons.LEFT_BUMPER.getNumber());
+        _catapultDebugButton = new JoystickButton(_buttonpad, Gamepad.Buttons.LEFT_BUMPER.getNumber());
+        _preloadButton = new JoystickButton(_buttonpad, Gamepad.Buttons.LEFT_STICK.getNumber());
+//        _release = new JoystickButton(_buttonpad, Gamepad.Buttons..getNumber());
+        _readyToClimb = new JoystickButton(_buttonpad, Gamepad.Buttons.B.getNumber());
+        _stowClimber = new JoystickButton(_buttonpad, Gamepad.Buttons.RIGHT_BUMPER.getNumber());
+        _rockerFlip = new JoystickButton(_buttonpad, Gamepad.Buttons.X.getNumber());
+        _manualIndexer = new JoystickButton(_buttonpad, Gamepad.Buttons.A.getNumber());
+        _autoShootToggle = new JoystickButton(_buttonpad, Gamepad.Buttons.Y.getNumber());
 
 
         // adding buttons while driving: Ben pls look
 
-//        _shootButton = new JoystickButton(_debug, Gamepad.Buttons.Y.getNumber());
+//        _shootButton = new JoystickButton(_buttonpad, Gamepad.Buttons.Y.getNumber());
 
         // rotation joystick
         _intakeButton = new JoystickButton(_rotation, 1);
@@ -119,8 +125,11 @@ public void initializeButtons(DriveTrain driveTrain, Catapult catapult, Intake i
         _stowClimber.whenPressed(new Stow(climber));
         _rockerFlip.whenPressed(new RockerFlip(climber));
         _manualIndexer.whenPressed(indexer::up);
+        _autoShootToggle.whenPressed(catapult::toggleAutomatShoot);
     }
 
+    public boolean hold = false;
+
     public boolean readyToClimb() { return _readyToClimb.get(); }
     public boolean isShootButtonPressed() { return _shootButton.get(); }
     public boolean exitDebugCatapult() { return _catapultDebugButton.get(); }
@@ -139,7 +148,7 @@ public boolean preloadCatapult() {
     public double getDriveY() {
         //Comment for gamepad control
         yIn = -getSpeedFromAxis(_translation, _translation.getYChannel());
-//         yIn = -getSpeedFromAxis(_debug, Gamepad.Axes.LEFT_Y.getNumber());
+//         yIn = -getSpeedFromAxis(_buttonpad, Gamepad.Axes.LEFT_Y.getNumber());
         yIn = applyDeadband(yIn, Constants.DriveTrain.DEADBAND);
 
         double yOut = yIn / (Math.sqrt(yIn * yIn + (xIn * xIn)) + Constants.EPSILON);
@@ -150,7 +159,7 @@ public double getDriveY() {
     public double getDriveX() {
 //        Comment for gamepad control
         xIn = -getSpeedFromAxis(_translation, _translation.getXChannel());
-//        xIn = -getSpeedFromAxis(_debug, Gamepad.Axes.LEFT_X.getNumber());
+//        xIn = -getSpeedFromAxis(_buttonpad, Gamepad.Axes.LEFT_X.getNumber());
         xIn = applyDeadband(xIn, Constants.DriveTrain.DEADBAND);
         double xOut = xIn / (Math.sqrt(yIn * yIn + (xIn * xIn)) + Constants.EPSILON);
         xOut = (xOut + (xIn * 2)) / 3.0; // numbers from empirical testing.
@@ -159,13 +168,13 @@ public double getDriveX() {
 
     public double getRotationX() {
         double speed = -getSpeedFromAxis(_rotation, _rotation.getXChannel());
-//        double speed = getSpeedFromAxis(_debug, Gamepad.Axes.RIGHT_X.getNumber());
+//        double speed = getSpeedFromAxis(_buttonpad, Gamepad.Axes.RIGHT_X.getNumber());
         speed = applyDeadband(speed, Constants.DEADBAND);
         return speed;
     }
 
     public double getSpringMotorSpeed() {
-        double speed = -getSpeedFromAxis(_debug, Gamepad.Axes.LEFT_Y.getNumber());
+        double speed = -getSpeedFromAxis(_buttonpad, Gamepad.Axes.LEFT_Y.getNumber());
         speed = applyDeadband(speed, Constants.DEADBAND);
         return speed;
     }

src/main/java/org/frc5687/rapidreact/RobotContainer.java

@@ -21,7 +21,6 @@
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 
 import org.frc5687.rapidreact.commands.*;
-import org.frc5687.rapidreact.commands.catapult.AutoShoot;
 import org.frc5687.rapidreact.commands.catapult.DriveCatapult;
 import org.frc5687.rapidreact.commands.auto.*;
 import org.frc5687.rapidreact.commands.Climber.IdleClimber;

src/main/java/org/frc5687/rapidreact/commands/catapult/DriveCatapult.java

@@ -52,6 +52,8 @@ public void initialize() {
 
     @Override
     public void execute() {
+        metric("Automatic Shooting (Teleop)", _catapult.getAutomatShoot());
+        metric("Autonomous Shooting (Autonomous)", _catapult.getAutonomShoot());
         metric("String from dist", _catapult.calculateIdealString(_driveTrain.getDistanceToTarget()));
         metric("Spring from dist", _catapult.calculateIdealSpring(_driveTrain.getDistanceToTarget()));
         metric("Setpoint value", _catapult.getSetpoint().toString());
@@ -134,7 +136,7 @@ public void execute() {
                 }
                 if (isShootTriggered() && ((_catapult.isWinchAtGoal() && _catapult.isSpringAtPosition()))) {
 //                    if (_indexer.isBallDetected() && (System.currentTimeMillis() > _indexerWait)) {
-                        _catapult.setAutoshoot(false);
+                        _catapult.setAutonomShoot(false);
                         _catapult.setState(SHOOTING);
 //                    }
                 }
@@ -233,10 +235,13 @@ protected boolean zeroSpring() {
     }
 
     boolean isShootTriggered() {
-        if(!_driveTrain.isMoving() && _driveTrain.onTarget()){
+        metric("On target", _driveTrain.onTarget());
+        metric("Is drivetrain moving", _driveTrain.isMoving());
+        if(!_driveTrain.isMoving() && _driveTrain.onTarget() && _catapult.getAutomatShoot()){
+            metric("Can shoot", true);
             return true;
         }
-        if (_catapult.isAutoShoot() && !_driveTrain.isMoving()) {
+        if (_catapult.getAutonomShoot() && !_driveTrain.isMoving()) {
             return true;
         }
         return _oi.isShootButtonPressed();

src/main/java/org/frc5687/rapidreact/commands/catapult/Shoot.java

@@ -27,7 +27,7 @@ public void execute() {
         if (!_done && _catapult.isInitialized() && _indexer.isBallDetected()) {
             info("Shooting");
             _catapult.setState(CatapultState.AIMING);
-            _catapult.setAutoshoot(true);
+            _catapult.setAutonomShoot(true);
             _done = true;
         }
     }

src/main/java/org/frc5687/rapidreact/subsystems/Catapult.java

@@ -38,7 +38,8 @@ public class Catapult extends OutliersSubsystem {
     private CatapultState _state;
     private double _springGoal;
     private double _winchGoal;
-    private boolean _autoShoot;
+    private boolean _autonomShoot;
+    private boolean _automatShoot;
     private boolean _initialized = false;
     private CatapultSetpoint _setpoint = CatapultSetpoint.NONE;
 
@@ -114,6 +115,8 @@ public Catapult(OutliersContainer container) {
         _state = CatapultState.DEBUG;
         _springGoal = 0;
         _winchGoal = 0;
+        _automatShoot = true;
+        _autonomShoot = false;
     }
 
 
@@ -361,16 +364,33 @@ public void updateDashboard() {
 //        metric("Arm Hall Effect", isArmLowered());
     }
 
+    public boolean getAutomatShoot() {
+        return _automatShoot;
+    }
+
+    public boolean getAutonomShoot() {
+        return _autonomShoot;
+    }
+
+    public void setAutomatShoot(boolean value) {
+        _automatShoot = value;
+    }
     /**
      * Pass true here to trigger a shot from autonomous.
      * @param value
      */
-    public void setAutoshoot(boolean value) {
-        _autoShoot = value;
+    public void setAutonomShoot(boolean value) {
+        _autonomShoot = value;
     }
 
-    public boolean isAutoShoot() {
-        return _autoShoot;
+    public void toggleAutomatShoot() {
+        if (getAutomatShoot() == true) {
+            setAutomatShoot(false);
+        } else if (getAutomatShoot() == false) {
+            setAutomatShoot(true);
+        } else {
+            setAutomatShoot(false);
+        }
     }
 
     public void setSetpoint(CatapultSetpoint setpoint) {

src/main/java/org/frc5687/rapidreact/util/Version.java

@@ -6,7 +6,7 @@
  */
 
 public class Version {
-    public static final String BRANCH = "Intake/#150-New-Intake";
-    public static final String REVISION = "ce0f164";
+    public static final String BRANCH = "Shoot/#149-Toggle-Auto-Shoot";
+    public static final String REVISION = "99e5d84";
     public static final String MODIFIED = " *";
 }