Boxop no climber (#67)

* added boxop oi rough draft * robot works! made some very temporary code for climber and nudge: FIXME * added boxop oi rough draft * robot works! made some very temporary code for climber and nudge: FIXME * getCurrentSpeed * no climber comp controls * lint * put boxop oi into unified file --------- Co-authored-by: qntmcube <[email protected]> Co-authored-by: BCNOFNeNaMg <[email protected]>
Team766 · Mar 9, 2024 · 4528b72 · 4528b72
1 parent 0ce7acb
commit 4528b72
Show file tree

Hide file tree

Showing 10 changed files with 159 additions and 24 deletions.
diff --git a/src/main/java/com/team766/robot/reva/BoxOpOI.java b/src/main/java/com/team766/robot/reva/BoxOpOI.java
@@ -0,0 +1,106 @@
+package com.team766.robot.reva;
+
+import com.team766.framework.Context;
+import com.team766.hal.JoystickReader;
+import com.team766.robot.reva.constants.InputConstants;
+import com.team766.robot.reva.mechanisms.Climber;
+import com.team766.robot.reva.mechanisms.Intake;
+import com.team766.robot.reva.mechanisms.Shooter;
+import com.team766.robot.reva.mechanisms.Shoulder;
+import com.team766.robot.reva.mechanisms.Shoulder.ShoulderPosition;
+
+public class BoxOpOI {
+    private final JoystickReader gamepad;
+
+    private final Shoulder shoulder;
+    private final Intake intake;
+    private final Shooter shooter;
+    private final Climber climber;
+
+    public BoxOpOI(
+            JoystickReader gamepad,
+            Shoulder shoulder,
+            Intake intake,
+            Shooter shooter,
+            Climber climber) {
+        this.gamepad = gamepad;
+        this.shoulder = shoulder;
+        this.intake = intake;
+        this.shooter = shooter;
+        this.climber = climber;
+    }
+
+    public void handleOI(Context context) {
+
+        // shoulder positions
+        if (gamepad.getButtonPressed(InputConstants.XBOX_A)) {
+            context.takeOwnership(shoulder);
+            shoulder.rotate(ShoulderPosition.SHOOT_MEDIUM);
+            context.releaseOwnership(shoulder);
+        } else if (gamepad.getButtonPressed(InputConstants.XBOX_B)) {
+            context.takeOwnership(shoulder);
+            shoulder.rotate(ShoulderPosition.BOTTOM);
+            context.releaseOwnership(shoulder);
+        } else if (gamepad.getButtonPressed(InputConstants.XBOX_X)) {
+            context.takeOwnership(shoulder);
+            shoulder.rotate(ShoulderPosition.TOP);
+            context.releaseOwnership(shoulder);
+        } else if (gamepad.getButtonPressed(InputConstants.XBOX_Y)) {
+            context.takeOwnership(shoulder);
+            shoulder.rotate(ShoulderPosition.SHOOT_LOW);
+            context.releaseOwnership(shoulder);
+        }
+
+        // dpad
+        // shoulder up 5
+        if (gamepad.getPOV() == 0) {
+            context.takeOwnership(shoulder);
+            shoulder.nudgeUp();
+            context.releaseOwnership(shoulder);
+        }
+
+        if (gamepad.getPOV() == 180) {
+            context.takeOwnership(shoulder);
+            shoulder.nudgeDown();
+            context.releaseOwnership(shoulder);
+        }
+
+        if (gamepad.getPOV() == 90) {
+            context.takeOwnership(climber);
+            // climber down method
+            context.releaseOwnership(climber);
+        }
+
+        if (gamepad.getPOV() == 360) {
+            context.takeOwnership(climber);
+            // climber up method
+            context.releaseOwnership(climber);
+        }
+
+        // shooter
+        if (gamepad.getAxis(InputConstants.XBOX_RT) > 0) {
+            context.takeOwnership(shooter);
+            shooter.shoot();
+            context.releaseOwnership(shooter);
+        } else {
+            context.takeOwnership(shooter);
+            shooter.stop();
+            context.releaseOwnership(shooter);
+        }
+
+        // intake
+        if (gamepad.getButton(InputConstants.XBOX_RB)) {
+            context.takeOwnership(intake);
+            intake.out();
+            context.releaseOwnership(intake);
+        } else if (gamepad.getButton(InputConstants.XBOX_LB)) {
+            context.takeOwnership(intake);
+            intake.in();
+            context.releaseOwnership(intake);
+        } else {
+            context.takeOwnership(intake);
+            intake.stop();
+            context.releaseOwnership(intake);
+        }
+    }
+}
diff --git a/src/main/java/com/team766/robot/reva/DebugOI.java b/src/main/java/com/team766/robot/reva/DebugOI.java
@@ -38,7 +38,7 @@ public void handleOI(Context context) {
                 shoulder.nudgeUp();
             } else if (macropad.getButtonPressed(InputConstants.NUDGE_DOWN)) {
                 shoulder.nudgeDown();
-            } else if (macropad.getButtonPressed(9)) {
+            } else if (macropad.getButtonPressed(InputConstants.MACROPAD_RESET_SHOULDER)) {
                 shoulder.reset();
             }
             context.releaseOwnership(shoulder);
@@ -94,11 +94,11 @@ public void handleOI(Context context) {
             if (macropad.getButtonPressed(InputConstants.MACROPAD_PRESET_3)) {
                 shooter.shoot();
             }
-            context.takeOwnership(shooter);
+            context.releaseOwnership(shooter);
         } else {
             context.takeOwnership(shooter);
             shooter.stop();
-            context.takeOwnership(shooter);
+            context.releaseOwnership(shooter);
         }
     }
 }
diff --git a/src/main/java/com/team766/robot/reva/OI.java b/src/main/java/com/team766/robot/reva/OI.java
@@ -19,18 +19,22 @@ public class OI extends Procedure {
     private final JoystickReader leftJoystick;
     private final JoystickReader rightJoystick;
     private final JoystickReader macropad;
+    private final JoystickReader gamepad;
     private final DriverOI driverOI;
     private final DebugOI debugOI;
+    private final BoxOpOI boxOpOI;
 
     public OI() {
         loggerCategory = Category.OPERATOR_INTERFACE;
 
         leftJoystick = RobotProvider.instance.getJoystick(InputConstants.LEFT_JOYSTICK);
         rightJoystick = RobotProvider.instance.getJoystick(InputConstants.RIGHT_JOYSTICK);
         macropad = RobotProvider.instance.getJoystick(InputConstants.MACROPAD);
+        gamepad = RobotProvider.instance.getJoystick(InputConstants.BOXOP_GAMEPAD_X);
 
         driverOI = new DriverOI(Robot.drive, leftJoystick, rightJoystick);
         debugOI = new DebugOI(macropad, Robot.shoulder, Robot.climber, Robot.intake, Robot.shooter);
+        boxOpOI = new BoxOpOI(gamepad, Robot.shoulder, Robot.intake, Robot.shooter, Robot.climber);
     }
 
     public void run(Context context) {
@@ -47,6 +51,8 @@ public void run(Context context) {
             driverOI.handleOI(context);
             // Debug OI: allow for finer-grain testing of each mechanism.
             debugOI.handleOI(context);
+
+            boxOpOI.handleOI(context);
         }
     }
 }
diff --git a/src/main/java/com/team766/robot/reva/constants/InputConstants.java b/src/main/java/com/team766/robot/reva/constants/InputConstants.java
@@ -9,17 +9,33 @@ public final class InputConstants {
     public static final int LEFT_JOYSTICK = 0;
     public static final int RIGHT_JOYSTICK = 1;
     public static final int MACROPAD = 2;
-    public static final int BOXOP_GAMEPAD = 3; // should be in Logitech Mode
+    public static final int BOXOP_GAMEPAD_L = 3; // should be in Logitech Mode
+    public static final int BOXOP_GAMEPAD_X = 4; // xbox
 
     // Macropad buttons
     public static final int CONTROL_SHOULDER = 1;
     public static final int CONTROL_CLIMBER = 2;
     public static final int CONTROL_INTAKE = 3;
     public static final int CONTROL_SHOOTER = 4;
     public static final int NUDGE_UP = 8;
+    public static final int MACROPAD_RESET_SHOULDER = 9;
     public static final int NUDGE_DOWN = 12;
     public static final int MACROPAD_PRESET_1 = 13;
     public static final int MACROPAD_PRESET_2 = 14;
     public static final int MACROPAD_PRESET_3 = 15;
     public static final int MACROPAD_PRESET_4 = 16;
+
+    // Xbox buttons
+    // TODO: change
+    public static final int XBOX_A = 1;
+    public static final int XBOX_B = 2;
+    public static final int XBOX_X = 3;
+    public static final int XBOX_Y = 4;
+    public static final int XBOX_LB = 5;
+    public static final int XBOX_RB = 6;
+    public static final int XBOX_LT = 2;
+    public static final int XBOX_RT = 3;
+
+    public static final int POV_UP = 0;
+    public static final int POV_DOWN = 180;
 }
diff --git a/src/main/java/com/team766/robot/reva/mechanisms/Climber.java b/src/main/java/com/team766/robot/reva/mechanisms/Climber.java
@@ -10,15 +10,15 @@
 
 public class Climber extends Mechanism {
 
-    public enum Position {
+    public enum ClimberPosition {
         // A very rough measurement, and was being very safe.
         // TODO: Needs to be measured more accurately.
         TOP(43.18),
         BOTTOM(0);
 
         private final double height;
 
-        private Position(double height) {
+        private ClimberPosition(double height) {
             this.height = height;
         }
 
@@ -35,7 +35,7 @@ public double getHeight() {
     private static final double GEAR_RATIO_AND_CIRCUMFERENCE =
             (14. / 50.) * (30. / 42.) * (1.25 * Math.PI);
     private static final double NUDGE_INCREMENT = 10; // in cm
-    private static final double PIDLESS_NUDGE_INCREMENT = 0.1;
+    private static final double PIDLESS_NUDGE_INCREMENT = 0.2;
     private double pidlessPower = 0.0;
 
     public Climber() {
@@ -52,7 +52,7 @@ public boolean isRunningNoPID() {
     }
 
     public void goNoPID() {
-        leftMotor.set(pidlessPower);
+        leftMotor.set(PIDLESS_NUDGE_INCREMENT);
     }
 
     public void stop() {
@@ -68,14 +68,16 @@ private double rotationsToHeight(double rotations) {
         return rotations / GEAR_RATIO_AND_CIRCUMFERENCE;
     }
 
-    public void setHeight(Position position) {
+    public void setHeight(ClimberPosition position) {
         setHeight(position.getHeight());
     }
 
     public void setHeight(double height) {
         double targetHeight =
                 com.team766.math.Math.clamp(
-                        height, Position.BOTTOM.getHeight(), Position.TOP.getHeight());
+                        height,
+                        ClimberPosition.BOTTOM.getHeight(),
+                        ClimberPosition.TOP.getHeight());
         targetRotations = heightToRotations(targetHeight);
         leftMotor.set(MotorController.ControlMode.Position, targetRotations);
     }

diff --git a/src/main/java/com/team766/robot/reva/mechanisms/Intake.java b/src/main/java/com/team766/robot/reva/mechanisms/Intake.java
@@ -15,7 +15,7 @@ public enum State {
         STOPPED
     }
 
-    private static final double DEFAULT_POWER = 0.75;
+    private static final double DEFAULT_POWER = 1.0;
     private static final double NUDGE_INCREMENT = 0.05;
     private static final double MAX_POWER = 1.0;
     private static final double MIN_POWER = -1 * MAX_POWER;

diff --git a/src/main/java/com/team766/robot/reva/mechanisms/Shooter.java b/src/main/java/com/team766/robot/reva/mechanisms/Shooter.java
@@ -8,9 +8,9 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 public class Shooter extends Mechanism {
-    private static final double DEFAULT_POWER = 0.75;
+    private static final double DEFAULT_POWER = 1.00;
     private static final double NUDGE_INCREMENT = 0.05;
-    private static final double MAX_POWER = 0.8;
+    private static final double MAX_POWER = 1.00;
     private static final double MIN_POWER = 0.0;
 
     private MotorController shooterMotorTop;
@@ -22,6 +22,10 @@ public Shooter() {
         shooterMotorBottom = RobotProvider.instance.getMotor(SHOOTER_MOTOR_BOTTOM);
     }
 
+    public double getCurrentSpeed() {
+        return shooterMotorTop.getSensorVelocity();
+    }
+
     public void runShooter() {
         checkContextOwnership();
         shooterMotorTop.set(shooterPower);
@@ -58,5 +62,6 @@ public void nudgeDown() {
 
     public void run() {
         SmartDashboard.putNumber("[SHOOTER POWER]", shooterPower);
+        SmartDashboard.putNumber("[SHOOTER] speed", getCurrentSpeed());
     }
 }
diff --git a/src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java b/src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java
@@ -14,18 +14,18 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 public class Shoulder extends Mechanism {
-    enum Position {
+    public enum ShoulderPosition {
         // TODO: Find actual values.
         BOTTOM(0),
         INTAKE_FLOOR(0),
-        SHOOT_LOW(35),
-        SHOOT_MEDIUM(45),
+        SHOOT_LOW(10),
+        SHOOT_MEDIUM(30),
         SHOOT_HIGH(80),
         TOP(90);
 
         private final double angle;
 
-        Position(double angle) {
+        ShoulderPosition(double angle) {
             this.angle = angle;
         }
 
@@ -34,7 +34,7 @@ public double getAngle() {
         }
     }
 
-    private static final double NUDGE_AMOUNT = 30; // degrees
+    private static final double NUDGE_AMOUNT = 5; // degrees
 
     private MotorController leftMotor;
     private MotorController rightMotor;
@@ -87,15 +87,15 @@ private double rotationsToDegrees(double rotations) {
         return rotations * (15. / 54.) * (1. / 4.) * (1. / 3.) * (1. / 3.) * (360. / 1.);
     }
 
-    public void rotate(Position position) {
+    public void rotate(ShoulderPosition position) {
         rotate(position.getAngle());
     }
 
     public void rotate(double angle) {
         checkContextOwnership();
         double targetAngle =
                 com.team766.math.Math.clamp(
-                        angle, Position.BOTTOM.getAngle(), Position.TOP.getAngle());
+                        angle, ShoulderPosition.BOTTOM.getAngle(), ShoulderPosition.TOP.getAngle());
         targetRotations = degreesToRotations(targetAngle);
         SmartDashboard.putNumber("[SHOULDER Target Angle]", targetAngle);
         // actual rotation will happen in run()

diff --git a/src/main/java/com/team766/robot/reva/procedures/ExtendClimber.java b/src/main/java/com/team766/robot/reva/procedures/ExtendClimber.java
@@ -3,13 +3,13 @@
 import com.team766.framework.Context;
 import com.team766.framework.Procedure;
 import com.team766.robot.reva.Robot;
-import com.team766.robot.reva.mechanisms.Climber.Position;
+import com.team766.robot.reva.mechanisms.Climber.ClimberPosition;
 
 public class ExtendClimber extends Procedure {
 
     @Override
     public void run(Context context) {
         context.takeOwnership(Robot.climber);
-        Robot.climber.setHeight(Position.TOP);
+        Robot.climber.setHeight(ClimberPosition.TOP);
     }
 }
diff --git a/src/main/java/com/team766/robot/reva/procedures/RetractClimber.java b/src/main/java/com/team766/robot/reva/procedures/RetractClimber.java
@@ -3,13 +3,13 @@
 import com.team766.framework.Context;
 import com.team766.framework.Procedure;
 import com.team766.robot.reva.Robot;
-import com.team766.robot.reva.mechanisms.Climber.Position;
+import com.team766.robot.reva.mechanisms.Climber.ClimberPosition;
 
 public class RetractClimber extends Procedure {
 
     @Override
     public void run(Context context) {
         context.takeOwnership(Robot.climber);
-        Robot.climber.setHeight(Position.BOTTOM);
+        Robot.climber.setHeight(ClimberPosition.BOTTOM);
     }
 }