2910oi

src/main/deploy/pathplanner/paths/New Path.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 7.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.0,
+        "y": 7.0
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.0,
+        "y": 6.0
+      },
+      "prevControl": {
+        "x": 3.0,
+        "y": 6.0
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 3.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/java/com/team766/robot/common/constants/ControlConstants.java

@@ -3,22 +3,24 @@
 public class ControlConstants {
 
     // Amount to reduce driving power to when holding the fine driving button
-    public static final double FINE_DRIVING_COEFFICIENT = 0.25;
+    public static final double FINE_DRIVING_COEFFICIENT = 0.25; // was 0.25
 
     // Value below which the joystick movement does not register
     public static final double JOYSTICK_DEADZONE = 0.05;
     public static final double GAMEPAD_DEADZONE = 0.20;
 
     // Exponent giving joystick curved power mapping for translational movement
-    public static final double TRANSLATIONAL_CURVE_POWER = 1.0;
+    // 1.0 for flight sticks
+    public static final double TRANSLATIONAL_CURVE_POWER = 2.0; // was 1.0
 
     // Exponent giving joystick curved power mapping for rotational movement
-    public static final double ROTATIONAL_CURVE_POWER = 1.0;
+    // 1.0 for flight sticks
+    public static final double ROTATIONAL_CURVE_POWER = 1.0; // was 1.0
 
     /**
      * Translational velocity of robot that max joystick power controls in m/s
      */
-    public static final double MAX_TRANSLATIONAL_VELOCITY = 5.0;
+    public static final double MAX_TRANSLATIONAL_VELOCITY = 3.0; // 5.0
 
     /**
      * m/s
@@ -29,7 +31,7 @@ public class ControlConstants {
     /**
      * Rotational velocity of robot that max joystick power controls in rad/s
      */
-    public static final double MAX_ROTATIONAL_VELOCITY = 5.0;
+    public static final double MAX_ROTATIONAL_VELOCITY = 7.0; // was 5.0
 
     public static final double DEFAULT_ROTATION_THRESHOLD = 0.40;
 

src/main/java/com/team766/robot/common/constants/InputConstants.java

@@ -12,7 +12,8 @@ public class InputConstants {
     public static final int AXIS_TWIST = 3;
 
     // buttons
-    public static final int BUTTON_FINE_DRIVING = 1;
+    // public static final int BUTTON_FINE_DRIVING = 1; | for flight stick
+    public static final int BUTTON_FINE_DRIVING = 5; // left bumper on controller
     public static final int BUTTON_CROSS_WHEELS = 3;
     public static final int BUTTON_RESET_GYRO = 9;
     public static final int BUTTON_RESET_POS = 15;
@@ -40,4 +41,6 @@ public class InputConstants {
     public static final int GAMEPAD_DPAD_UP = 0;
     public static final int GAMEPAD_BACK_BUTTON = 7;
     public static final int GAMEPAD_START_BUTTON = 8;
+    public static final int GAMEPAD_LEFT_STICK_CLICK = 9;
+    public static final int GAMEPAD_RIGHT_STICK_CLICK = 10;
 }

src/main/java/com/team766/robot/copy_2910/BoxOpOI.java

@@ -0,0 +1,209 @@
+package com.team766.robot.copy_2910;
+
+import static com.team766.framework.RulePersistence.*;
+
+import com.team766.framework.Conditions;
+import com.team766.framework.RuleGroup;
+import com.team766.hal.JoystickReader;
+import com.team766.robot.common.constants.ControlConstants;
+import com.team766.robot.copy_2910.OI.QueuedControl;
+import com.team766.robot.copy_2910.mechanisms.Elevator;
+import com.team766.robot.copy_2910.mechanisms.Elevator.ElevatorPosition;
+import com.team766.robot.copy_2910.mechanisms.Intake;
+import com.team766.robot.copy_2910.mechanisms.Shoulder;
+import com.team766.robot.copy_2910.mechanisms.Shoulder.ShoulderPosition;
+import com.team766.robot.copy_2910.mechanisms.Wrist;
+import com.team766.robot.copy_2910.mechanisms.Wrist.WristPosition;
+import com.team766.robot.copy_2910.procedures.IntakeCoral;
+import com.team766.robot.copy_2910.procedures.MoveWristvator;
+import java.util.Set;
+
+public class BoxOpOI extends RuleGroup {
+
+    public BoxOpOI(
+            JoystickReader boxopGamepad,
+            Shoulder shoulder,
+            Elevator elevator,
+            Wrist wrist,
+            Intake intake,
+            QueuedControl queuedControl) {
+
+        boxopGamepad.setAllAxisDeadzone(ControlConstants.GAMEPAD_DEADZONE);
+
+        // CLIMBER
+
+        addRule(
+                        "Control Climber",
+                        new Conditions.Toggle(boxopGamepad.whenButton(InputConstants.BUTTON_CLIMB)),
+                        ONCE_AND_HOLD,
+                        Set.of(wrist, elevator, shoulder),
+                        () -> {
+                            elevator.setPosition(Elevator.ElevatorPosition.MAXIMUM);
+                            wrist.setPosition(Wrist.WristPosition.ALGAE_LOW);
+
+                            shoulder.setPosition(Shoulder.ShoulderPosition.CLIMBER);
+                        })
+                .withFinishedTriggeringProcedure(
+                        Set.of(shoulder),
+                        context -> {
+                            shoulder.setPosition(ShoulderPosition.CORAL_GROUND);
+                            log("finished triggering");
+                        });
+        // ALGAE INTAKE POSITIONS
+
+        // addRule(
+        //         "Queue to Algae Ground Intake Position",
+        //         () -> boxopGamepad.getPOV() == InputConstants.BUTTON_ALGAE_INTAKE_GROUND,
+        //         ONCE,
+        //         Set.of(elevator, shoulder, intake),
+        //         () -> {
+        //             queuedControl.elevatorPosition = ElevatorPosition.ALGAE_GROUND;
+        //             queuedControl.shoulderPosition = ShoulderPosition.ALGAE_GROUND;
+        //             queuedControl.wristPosition = WristPosition.ALGAE_GROUND;
+        //         });
+
+        // addRule(
+        //         "Queue Algae Intake to L2 L3 Position",
+        //         () -> boxopGamepad.getPOV() == InputConstants.BUTTON_ALGAE_INTAKE_L2_L3,
+        //         ONCE,
+        //         Set.of(elevator, shoulder, intake),
+        //         () -> {
+        //             queuedControl.elevatorPosition = ElevatorPosition.ALGAE_LOW;
+        //             queuedControl.shoulderPosition = ShoulderPosition.ALGAE_LOW;
+        //             queuedControl.wristPosition = WristPosition.ALGAE_LOW;
+        //         });
+
+        // addRule(
+        // "Queue Algae Intake to L3 L4 Position",
+        // () -> boxopGamepad.getPOV() == InputConstants.BUTTON_ALGAE_INTAKE_L3_L4,
+        // ONCE,
+        // Set.of(elevator, shoulder, intake),
+        // () -> {
+        //     queuedControl.elevatorPosition = ElevatorPosition.ALGAE_HIGH;
+        //     queuedControl.shoulderPosition = ShoulderPosition.ALGAE_HIGH;
+        //     queuedControl.wristPosition = WristPosition.ALGAE_HIGH;
+        // });
+
+        addRule(
+                "Queue Elevator and Wrist to L1 Position",
+                boxopGamepad.whenButton(InputConstants.BUTTON_ELEVATOR_WRIST_L1),
+                ONCE,
+                Set.of(elevator, shoulder, intake),
+                () -> {
+                    queuedControl.elevatorPosition = ElevatorPosition.L1;
+                    queuedControl.shoulderPosition = ShoulderPosition.L1;
+                    queuedControl.wristPosition = WristPosition.L1;
+                });
+
+        addRule(
+                "Queue Elevator and Wrist to L2 Position",
+                boxopGamepad.whenButton(InputConstants.BUTTON_ELEVATOR_WRIST_L2),
+                ONCE,
+                Set.of(elevator, shoulder, intake),
+                () -> {
+                    queuedControl.elevatorPosition = ElevatorPosition.L2;
+                    queuedControl.shoulderPosition = ShoulderPosition.L2;
+                    queuedControl.wristPosition = WristPosition.L2;
+                });
+
+        addRule(
+                "Queue Elevator and Wrist to L3 Position",
+                boxopGamepad.whenButton(InputConstants.BUTTON_ELEVATOR_WRIST_L3),
+                ONCE,
+                Set.of(elevator, shoulder, intake),
+                () -> {
+                    queuedControl.elevatorPosition = ElevatorPosition.L3;
+                    queuedControl.shoulderPosition = ShoulderPosition.L3;
+                    queuedControl.wristPosition = WristPosition.L3;
+                });
+
+        addRule(
+                "Queue Elevator and Wrist to L4 Position",
+                boxopGamepad.whenButton(InputConstants.BUTTON_ELEVATOR_WRIST_L4),
+                ONCE,
+                Set.of(elevator, shoulder, intake),
+                () -> {
+                    queuedControl.elevatorPosition = ElevatorPosition.L4;
+                    queuedControl.shoulderPosition = ShoulderPosition.L4;
+                    queuedControl.wristPosition = WristPosition.L4;
+                });
+
+        addRule(
+                        "Ground Intake",
+                        boxopGamepad.whenAxisMoved(InputConstants.BUTTON_ALGAE_MOTOR_INTAKE_POWER),
+                        ONCE_AND_HOLD,
+                        () -> new IntakeCoral(intake, elevator, shoulder, wrist))
+                .withFinishedTriggeringProcedure(
+                        Set.of(intake, elevator, wrist, shoulder),
+                        () -> {
+                            intake.stop();
+                            elevator.setPosition(ElevatorPosition.READY);
+                            wrist.setPosition(WristPosition.STOW);
+                            shoulder.setPosition(ShoulderPosition.STOW);
+                        });
+
+        addRule(
+                        "Move to Target Position",
+                        boxopGamepad.whenButton(InputConstants.GAMEPAD_RIGHT_BUMPER_BUTTON),
+                        ONCE_AND_HOLD,
+                        () ->
+                                new MoveWristvator(
+                                        shoulder,
+                                        elevator,
+                                        wrist,
+                                        queuedControl.shoulderPosition,
+                                        queuedControl.elevatorPosition,
+                                        queuedControl.wristPosition))
+                .whenTriggering(
+                        new RuleGroup() {
+                            {
+                                addRule(
+                                                "Spin Algae Intake Motor In",
+                                                boxopGamepad.whenAxisMoved(
+                                                        InputConstants
+                                                                .BUTTON_ALGAE_MOTOR_INTAKE_POWER),
+                                                ONCE_AND_HOLD,
+                                                Set.of(intake),
+                                                context -> {
+                                                    intake.setAlgaePower(0.5);
+                                                })
+                                        .withFinishedTriggeringProcedure(
+                                                intake, () -> intake.setAlgaePower(0.1));
+
+                                addRule(
+                                        "Nudge Shoulder",
+                                        boxopGamepad.whenAxisMoved(
+                                                InputConstants.AXIS_WRIST_FINETUNE),
+                                        ONCE_AND_HOLD,
+                                        shoulder,
+                                        () -> {
+                                            shoulder.nudge(
+                                                    boxopGamepad.getAxis(
+                                                            InputConstants.AXIS_WRIST_FINETUNE));
+                                        });
+                            }
+                        });
+        addRule(
+                "Nudge Shoulder Up",
+                boxopGamepad.whenButton(9),
+                ONCE,
+                shoulder,
+                () -> shoulder.nudgeUp());
+        addRule(
+                "Nudge Shoulder Down",
+                boxopGamepad.whenButton(10),
+                ONCE,
+                shoulder,
+                () -> shoulder.nudgeDown());
+        addRule(
+                "Stow",
+                () -> boxopGamepad.getPOV() == 0,
+                ONCE,
+                Set.of(elevator, shoulder, intake),
+                () -> {
+                    queuedControl.elevatorPosition = ElevatorPosition.STOW;
+                    queuedControl.wristPosition = WristPosition.STOW;
+                    queuedControl.shoulderPosition = ShoulderPosition.STOW;
+                });
+    }
+}