diff --git a/src/main/deploy/pathplanner/paths/Blue Line Start - L1 Score.path b/src/main/deploy/pathplanner/paths/Center Start - L1 Score.path similarity index 100% rename from src/main/deploy/pathplanner/paths/Blue Line Start - L1 Score.path rename to src/main/deploy/pathplanner/paths/Center Start - L1 Score.path diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path new file mode 100644 index 000000000..43cdab567 --- /dev/null +++ b/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 +} \ No newline at end of file diff --git a/src/main/java/com/team766/robot/common/constants/ControlConstants.java b/src/main/java/com/team766/robot/common/constants/ControlConstants.java index 4c89acad9..d6b53816b 100644 --- a/src/main/java/com/team766/robot/common/constants/ControlConstants.java +++ b/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; diff --git a/src/main/java/com/team766/robot/common/constants/InputConstants.java b/src/main/java/com/team766/robot/common/constants/InputConstants.java index bb8a1eddb..4a96c43a9 100644 --- a/src/main/java/com/team766/robot/common/constants/InputConstants.java +++ b/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; } diff --git a/src/main/java/com/team766/robot/copy_2910/BoxOpOI.java b/src/main/java/com/team766/robot/copy_2910/BoxOpOI.java new file mode 100644 index 000000000..e5a823978 --- /dev/null +++ b/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; + }); + } +} diff --git a/src/main/java/com/team766/robot/copy_2910/DriverOI.java b/src/main/java/com/team766/robot/copy_2910/DriverOI.java new file mode 100644 index 000000000..606e8a5a9 --- /dev/null +++ b/src/main/java/com/team766/robot/copy_2910/DriverOI.java @@ -0,0 +1,88 @@ +package com.team766.robot.copy_2910; + +import static com.team766.framework.RulePersistence.*; + +import com.team766.framework.RuleGroup; +import com.team766.hal.JoystickReader; +import com.team766.robot.common.constants.ControlConstants; +import com.team766.robot.common.constants.InputConstants; +import com.team766.robot.common.mechanisms.SwerveDrive; +import com.team766.robot.copy_2910.mechanisms.*; + +public class DriverOI extends RuleGroup { + public DriverOI(JoystickReader gamepad, SwerveDrive drive) { + gamepad.setAllAxisDeadzone(ControlConstants.JOYSTICK_DEADZONE); + // TODO: make sure stick click works well + addRule( + "Reset Gyro", + gamepad.whenButton(InputConstants.GAMEPAD_BACK_BUTTON), + ONCE, + drive, + () -> drive.resetGyro()); + + // addRule( + // "Reset Pos", + // gamepad.whenButton(InputConstants.GAMEPAD_RIGHT_STICK_CLICK), + // ONCE, + // drive, + // () -> drive.resetCurrentPosition()); + + // Sets the wheels to the cross position if the cross button is pressed + // addRule( + // "Cross Wheels", + // new + // Conditions.Toggle(rightJoystick.whenButton(InputConstants.BUTTON_CROSS_WHEELS)), + // drive, + // () -> drive.stopDrive()); + + // Moves the robot if there are joystick inputs + addRule( + "Joysticks moved", + () -> + gamepad.isAxisMoved(InputConstants.GAMEPAD_LEFT_STICK_XAXIS) + || gamepad.isAxisMoved(InputConstants.GAMEPAD_LEFT_STICK_YAXIS) + || gamepad.isAxisMoved(InputConstants.GAMEPAD_RIGHT_STICK_XAXIS), + REPEATEDLY, + drive, + () -> { + // For fwd/rv + // Negative because forward is negative in driver station + final double leftJoystickX = + -gamepad.getAxis(InputConstants.GAMEPAD_LEFT_STICK_YAXIS) + * ControlConstants.MAX_TRANSLATIONAL_VELOCITY; + // For left/right + // Negative because left is negative in driver station + final double leftJoystickY = + -gamepad.getAxis(InputConstants.GAMEPAD_LEFT_STICK_XAXIS) + * ControlConstants.MAX_TRANSLATIONAL_VELOCITY; + // For steer + // Negative because left is negative in driver station + final double rightJoystickY = + -gamepad.getAxis(InputConstants.GAMEPAD_RIGHT_STICK_XAXIS) + * ControlConstants.MAX_ROTATIONAL_VELOCITY; + // If a button is pressed, drive is just fine adjustment + final double drivingCoefficient = + gamepad.getButton(InputConstants.BUTTON_FINE_DRIVING) + ? ControlConstants.FINE_DRIVING_COEFFICIENT + : 1; + drive.controlAllianceOriented( + drivingCoefficient + * curvedJoystickPower( + leftJoystickX, + ControlConstants.TRANSLATIONAL_CURVE_POWER), + drivingCoefficient + * curvedJoystickPower( + leftJoystickY, + ControlConstants.TRANSLATIONAL_CURVE_POWER), + drivingCoefficient + * curvedJoystickPower( + rightJoystickY, + ControlConstants.ROTATIONAL_CURVE_POWER)); + }); + } + + private static double curvedJoystickPower(double value, double power) { + return Math.signum(value) * Math.pow(Math.abs(value), power); + // TODO: tune all of this to work well w/ controller joysticks + } +} diff --git a/src/main/java/com/team766/robot/copy_2910/InputConstants.java b/src/main/java/com/team766/robot/copy_2910/InputConstants.java new file mode 100644 index 000000000..802e2973e --- /dev/null +++ b/src/main/java/com/team766/robot/copy_2910/InputConstants.java @@ -0,0 +1,51 @@ +package com.team766.robot.copy_2910; + +public final class InputConstants extends com.team766.robot.common.constants.InputConstants { + public static final int LEFT_JOYSTICK = 0; + public static final int RIGHT_JOYSTICK = 1; + public static final int BOXOP_GAMEPAD = 2; + + // All controls on gamepad + public static final int BUTTON_ELEVATOR_WRIST_L1 = GAMEPAD_A_BUTTON; + public static final int BUTTON_ELEVATOR_WRIST_L2 = GAMEPAD_X_BUTTON; + public static final int BUTTON_ELEVATOR_WRIST_L3 = GAMEPAD_B_BUTTON; + public static final int BUTTON_ELEVATOR_WRIST_L4 = GAMEPAD_Y_BUTTON; + public static final int BUTTON_ELEVATOR_WRIST_MOVE_TARGETPOSITION = GAMEPAD_RIGHT_BUMPER_BUTTON; + public static final int BUTTON_ALGAE_INTAKE_STOW = GAMEPAD_DPAD_DOWN; + public static final int BUTTON_ALGAE_INTAKE_GROUND = GAMEPAD_DPAD_RIGHT; + public static final int BUTTON_ALGAE_INTAKE_L2_L3 = GAMEPAD_DPAD_LEFT; + public static final int BUTTON_ALGAE_INTAKE_L3_L4 = GAMEPAD_DPAD_UP; + public static final int BUTTON_ALGAE_INTAKE_MOVE_TARGETPOSITION = GAMEPAD_LEFT_BUMPER_BUTTON; + public static final int BUTTON_ALGAE_MOTOR_INTAKE_POWER = GAMEPAD_LEFT_TRIGGER; + public static final int BUTTON_ALGAE_SHOOTER_ON = GAMEPAD_RIGHT_TRIGGER; + public static final int BUTTON_CLIMB = GAMEPAD_BACK_BUTTON; + public static final int AXIS_ELEVATOR_FINETUNE = GAMEPAD_LEFT_STICK_YAXIS; + public static final int AXIS_WRIST_FINETUNE = GAMEPAD_RIGHT_STICK_YAXIS; + public static final int AXIS_ALGAE_FINETUNE = GAMEPAD_LEFT_STICK_YAXIS; + + // Controls on joysticks + public static final int BUTTON_CORAL_PLACE = JOYSTICK_RIGHT_BUTTON; + public static final int BUTTON_ALGAE_SHOOT = JOYSTICK_LEFT_BUTTON; + public static final int BUTTON_AUTO_SHOOT = JOYSTICK_TRIGGER; + public static final int BUTTON_CORAL_AUTO_PLACE_LEFT = JOYSTICK_LEFT_BUTTON; + public static final int BUTTON_CORAL_AUTO_PLACE_RIGHT = JOYSTICK_RIGHT_BUTTON; + public static final int BUTTON_WINCH_CLIMBER = JOYSTICK_BOTTOM_BUTTON; + + // Macropad buttons + public static final int CONTROL_ALGAE = 1; + public static final int CONTROL_ELEVATOR = 2; + public static final int CONTROL_WRIST = 3; + public static final int CONTROL_CLIMBER = 4; + public static final int INTAKE_IN = 5; + public static final int INTAKE_OUT = 6; + public static final int NUDGE_NO_PID = 7; + public static final int NUDGE_UP = 8; + public static final int STOW_POSITION = 9; + public static final int ALGAE_SHOOTER_ON = 10; + public static final int ALGAE_SHOOTER_FEED = 11; + public static final int NUDGE_DOWN = 12; + public static final int GROUND_POSITION = 13; + public static final int SHOOT_POSITION = 14; + public static final int L2L3_POSITION = 15; + public static final int L3L4_POSITION = 16; +} diff --git a/src/main/java/com/team766/robot/copy_2910/OI.java b/src/main/java/com/team766/robot/copy_2910/OI.java new file mode 100644 index 000000000..465e261d6 --- /dev/null +++ b/src/main/java/com/team766/robot/copy_2910/OI.java @@ -0,0 +1,576 @@ +package com.team766.robot.copy_2910; + +import static com.team766.framework.RulePersistence.ONCE; +import static com.team766.framework.RulePersistence.ONCE_AND_HOLD; +import static com.team766.framework.RulePersistence.REPEATEDLY; + +import com.team766.framework.Conditions; +import com.team766.framework.RuleEngine; +import com.team766.framework.RuleGroup; +import com.team766.hal.JoystickReader; +import com.team766.hal.RobotProvider; +import com.team766.robot.common.constants.InputConstants; +import com.team766.robot.common.mechanisms.SwerveDrive; +import com.team766.robot.copy_2910.mechanisms.Climber; +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.Vision; +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.IntakeCoralL1; +import com.team766.robot.copy_2910.procedures.MoveWristvator; +import com.team766.robot.copy_2910.procedures.OuttakeCoral; +import com.team766.robot.copy_2910.procedures.ShootAlgae; +import java.util.Set; + +public class OI extends RuleEngine { + + public static class QueuedControl { + public Wrist.WristPosition wristPosition; + public Shoulder.ShoulderPosition shoulderPosition; + public Elevator.ElevatorPosition elevatorPosition; + } + + public OI( + SwerveDrive swerveDrive, + Intake intake, + Wrist wrist, + Elevator elevator, + Shoulder shoulder, + Climber climber, + Vision vision) { + + // final JoystickReader leftJoystick = RobotProvider.instance.getJoystick(0); + // final JoystickReader rightJoystick = RobotProvider.instance.getJoystick(1); + final JoystickReader boxopGamepad = RobotProvider.instance.getJoystick(0); // previously 2 + // leftJoystick.setAllAxisDeadzone(0.05); + // rightJoystick.setAllAxisDeadzone(0.05); + boxopGamepad.setAllAxisDeadzone(0.05); + + QueuedControl queuedControl = new QueuedControl(); + queuedControl.wristPosition = WristPosition.STOW; + queuedControl.shoulderPosition = ShoulderPosition.STOW; + queuedControl.elevatorPosition = ElevatorPosition.READY; + + addRules(new DriverOI(boxopGamepad, swerveDrive)); + // addRules( + // new BoxOpOI( + // boxopGamepad, shoulder, elevator, wrist, climber, intake, queuedControl)); + + /*addRule( + "Outtake Algae", + boxopGamepad.whenButton(InputConstants.TEMP), //TODO: algaeMode? variable + ONCE_AND_HOLD, + intake, + () -> intake.setAlgaePower(-0.5)) + .withFinishedTriggeringProcedure(intake, () -> intake.stop()); + */ + addRule( + "Wrist Nudge Up", + // TODO: () -> syntax correctness + () -> boxopGamepad.getPOV() == InputConstants.GAMEPAD_DPAD_RIGHT, + ONCE, + wrist, + () -> wrist.nudgeUp()); + addRule( + "Wrist Nudge Down", + () -> boxopGamepad.getPOV() == InputConstants.GAMEPAD_DPAD_LEFT, + ONCE, + wrist, + () -> wrist.nudgeDown()); + /* + addRule("Elevator Nudge Up", + rightJoystick.whenButton(2), + ONCE, + elevator, + () -> elevator.nudgeUp()); + addRule( + "Elevator Nudge Down", + rightJoystick.whenButton(3), + ONCE, + elevator, + () -> elevator.nudgeDown()); + */ + // CLIMBER + + addRule( + "Enable Climber", + boxopGamepad.whenButton(InputConstants.GAMEPAD_START_BUTTON), + ONCE, + Set.of(climber, wrist, elevator, shoulder), + () -> { + elevator.setPosition(Elevator.ElevatorPosition.STOW); // previously MAXIMUM + wrist.setPosition(Wrist.WristPosition.CLIMB); + climber.setClimberSpeed(0.5); + shoulder.setPosition(Shoulder.ShoulderPosition.CLIMBER); + }); // TODO: make sure whenTriggering is the right rule + + addRule( + "Move Climber", + () -> boxopGamepad.getPOV() == InputConstants.GAMEPAD_DPAD_DOWN, + ONCE, + Set.of(climber, wrist, elevator, shoulder), + () -> { + climber.stop(); + elevator.setPosition(ElevatorPosition.CLIMBDOWN); + shoulder.setBrakeMode(); + shoulder.setPosition(ShoulderPosition.CORAL_GROUND); + wrist.setPosition(WristPosition.CORAL_GROUND); + }); + + addRule( + "Toggle Coral or Algae Mode", + new Conditions.Toggle(() -> boxopGamepad.getPOV() == 0)) + .withOnTriggeringProcedure(ONCE, Set.of(intake), () -> intake.stopAlgae()) + .withFinishedTriggeringProcedure(Set.of(intake), () -> intake.stopAlgae()) + .whenTriggering( + new RuleGroup() { + { + addRule( + "Move Algae Intake to Low Position", + boxopGamepad.whenButton(InputConstants.GAMEPAD_A_BUTTON), + ONCE, + Set.of(elevator, shoulder, intake, wrist), + (context) -> { + queuedControl.elevatorPosition = + ElevatorPosition.ALGAE_LOW; + queuedControl.shoulderPosition = + ShoulderPosition.ALGAE_LOW; + queuedControl.wristPosition = WristPosition.ALGAE_LOW; + context.runSync( + new MoveWristvator( + shoulder, + elevator, + wrist, + queuedControl.shoulderPosition, + queuedControl.elevatorPosition, + queuedControl.wristPosition)); + }); + + addRule( + "Move Algae Intake to High Position", + boxopGamepad.whenButton(InputConstants.GAMEPAD_Y_BUTTON), + ONCE, + Set.of(elevator, shoulder, intake, wrist), + (context) -> { + queuedControl.elevatorPosition = + ElevatorPosition.ALGAE_HIGH; + queuedControl.shoulderPosition = + ShoulderPosition.ALGAE_HIGH; + queuedControl.wristPosition = WristPosition.ALGAE_HIGH; + context.runSync( + new MoveWristvator( + shoulder, + elevator, + wrist, + queuedControl.shoulderPosition, + queuedControl.elevatorPosition, + queuedControl.wristPosition)); + }); + + addRule( + "Intake Algae", + boxopGamepad.whenAxisMoved( + InputConstants.GAMEPAD_LEFT_TRIGGER), + ONCE_AND_HOLD, + Set.of(intake), + () -> intake.turnAlgaeNegative()) + .withFinishedTriggeringProcedure( + intake, () -> intake.retainAlgae()); + + addRule( + "Shoot Algae", + boxopGamepad.whenAxisMoved( + InputConstants.GAMEPAD_RIGHT_TRIGGER), + ONCE_AND_HOLD, + Set.of(elevator, shoulder, intake, wrist), + (context) -> { + queuedControl.elevatorPosition = + ElevatorPosition.ALGAE_SHOOT; + queuedControl.shoulderPosition = + ShoulderPosition.ALGAE_HIGH; + queuedControl.wristPosition = + WristPosition.ALGAE_SHOOT; + context.runParallel( + new MoveWristvator( + shoulder, + elevator, + wrist, + queuedControl.shoulderPosition, + queuedControl.elevatorPosition, + queuedControl.wristPosition), + new ShootAlgae(intake)); + }) + .withFinishedTriggeringProcedure( + intake, () -> intake.stopAlgae()); + } + }) + .whenNotTriggering( + new RuleGroup() { + { + addRule( + "Move Elevator and Wrist to L1 Position", + boxopGamepad.whenButton(InputConstants.GAMEPAD_A_BUTTON), + ONCE, + () -> { + queuedControl.elevatorPosition = ElevatorPosition.L1; + queuedControl.shoulderPosition = ShoulderPosition.L1; + queuedControl.wristPosition = WristPosition.L1; + return new MoveWristvator( + shoulder, + elevator, + wrist, + queuedControl.shoulderPosition, + queuedControl.elevatorPosition, + queuedControl.wristPosition); + }); + + addRule( + "Move Elevator and Wrist to L2 Position", + boxopGamepad.whenButton(InputConstants.GAMEPAD_B_BUTTON), + ONCE, + () -> { + queuedControl.elevatorPosition = ElevatorPosition.L2; + queuedControl.shoulderPosition = ShoulderPosition.L2; + queuedControl.wristPosition = WristPosition.L2; + return new MoveWristvator( + shoulder, + elevator, + wrist, + queuedControl.shoulderPosition, + queuedControl.elevatorPosition, + queuedControl.wristPosition); + }); + + addRule( + "Move Elevator and Wrist to L3 Position", + boxopGamepad.whenButton(InputConstants.GAMEPAD_X_BUTTON), + ONCE, + () -> { + queuedControl.elevatorPosition = ElevatorPosition.L3; + queuedControl.shoulderPosition = ShoulderPosition.L3; + queuedControl.wristPosition = WristPosition.L3; + return new MoveWristvator( + shoulder, + elevator, + wrist, + queuedControl.shoulderPosition, + queuedControl.elevatorPosition, + queuedControl.wristPosition); + }); + + addRule( + "Move Elevator and Wrist to L4 Position", + boxopGamepad.whenButton(InputConstants.GAMEPAD_Y_BUTTON), + ONCE, + () -> { + queuedControl.elevatorPosition = ElevatorPosition.L4; + queuedControl.shoulderPosition = ShoulderPosition.L4; + queuedControl.wristPosition = WristPosition.L4; + return new MoveWristvator( + shoulder, + elevator, + wrist, + queuedControl.shoulderPosition, + queuedControl.elevatorPosition, + queuedControl.wristPosition); + }); + + addRule( + "Outtake Coral", + boxopGamepad.whenAxisMoved( + InputConstants.GAMEPAD_RIGHT_TRIGGER), + ONCE_AND_HOLD, + () -> new OuttakeCoral(intake)) + .withFinishedTriggeringProcedure( + intake, () -> intake.stop()); + addRule( + "Ground Intake", + boxopGamepad.whenAxisMoved( + InputConstants.GAMEPAD_LEFT_TRIGGER), + ONCE_AND_HOLD, + () -> { + return 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( + "L1 Ground Intake", + boxopGamepad.whenButton( + InputConstants.GAMEPAD_RIGHT_BUMPER_BUTTON), + ONCE_AND_HOLD, + () -> + new IntakeCoralL1( + 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); + }); + } + }); + + // ALGAE INTAKE POSITIONS + + // Unnecessary for Subjorn? + // 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; + // }); + + // TODO: check if Spin Algae Motor In is necessary + /*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)); + //TODO: check if nudge shoulder below is necessary + + 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()); + */ + + // TODO: Set POV 0 to Algae Mode toggle + + /*addRule("Stow", + () -> boxopGamepad.getPOV() == 0, + ONCE, + Set.of(elevator, shoulder, intake), + () -> { + queuedControl.elevatorPosition = ElevatorPosition.STOW; + queuedControl.shoulderPosition = ShoulderPosition.STOW; + queuedControl.wristPosition = WristPosition.STOW; + }); */ + + // addRule( + // "Apply queued positions", + // leftJoystick.whenButton(4), + // ONCE_AND_HOLD, + // Set.of(wrist, shoulder, elevator), + // () -> { + // wrist.setSetpoint(queuedControl.wristPosition.getPosition()); + // shoulder.setSetpoint(queuedControl.shoulderPosition.getPosition()); + // elevator.setPosition(queuedControl.elevatorPosition.getPosition()); + // }); + + // addRule( + // "Prep L1 Coral", + // leftJoystick.whenButton(5), + // ONCE, + // Set.of(wrist, shoulder, elevator), + // () -> { + // queuedControl.wristPosition = WristPosition.L1; + // queuedControl.shoulderPosition = ShoulderPosition.L1; + // queuedControl.elevatorPosition = ElevatorPosition.L1; + // }); + // addRule( + // "Prep L2 Coral", + // leftJoystick.whenButton(6), + // ONCE, + // Set.of(wrist, shoulder, elevator), + // () -> { + // queuedControl.wristPosition = WristPosition.L2; + // queuedControl.shoulderPosition = ShoulderPosition.L2; + // queuedControl.elevatorPosition = ElevatorPosition.L2; + // }); + // addRule( + // "Prep L3 Coral", + // leftJoystick.whenButton(7), + // ONCE, + // Set.of(wrist, shoulder, elevator), + // () -> { + // queuedControl.wristPosition = WristPosition.L3; + // queuedControl.shoulderPosition = ShoulderPosition.L3; + // queuedControl.elevatorPosition = ElevatorPosition.L3; + // }); + // addRule( + // "Prep L4 Coral", + // leftJoystick.whenButton(8), + // ONCE, + // Set.of(wrist, shoulder, elevator), + // () -> { + // queuedControl.wristPosition = WristPosition.L4; + // queuedControl.shoulderPosition = ShoulderPosition.L4; + // queuedControl.elevatorPosition = ElevatorPosition.L4; + // }); + // addRule( + // "Prep Algae High", + // leftJoystick.whenButton(9), + // ONCE, + // Set.of(wrist, shoulder, elevator), + // () -> { + // queuedControl.wristPosition = WristPosition.ALGAE_HIGH; + // queuedControl.shoulderPosition = ShoulderPosition.ALGAE_HIGH; + // queuedControl.elevatorPosition = ElevatorPosition.ALGAE_HIGH; + // }); + // addRule( + // "Prep Algae Low", + // leftJoystick.whenButton(10), + // ONCE, + // Set.of(wrist, shoulder, elevator), + // () -> { + // queuedControl.wristPosition = WristPosition.ALGAE_LOW; + // queuedControl.shoulderPosition = ShoulderPosition.ALGAE_LOW; + // queuedControl.elevatorPosition = ElevatorPosition.ALGAE_LOW; + // }); + // addRule("Algae In", + // leftJoystick.whenButton(11), + // ONCE_AND_HOLD, + // intake, + // () -> {intake.turnAlgaeNegative();}); + + // addRule("Nudge Shoulder Up", + // leftJoystick.whenButton(12), + // ONCE, + // Set.of(shoulder), + // () -> shoulder.nudgeUp()); + // addRule("Nudge Shoulder Down", + // leftJoystick.whenButton(13), + // ONCE, + // Set.of(shoulder), + // () -> shoulder.nudgeDown()); + // addRule( + // "Prep Coral Ground", + // leftJoystick.whenButton(3), + // ONCE, + // Set.of(wrist, shoulder, elevator), + // () -> { + // queuedControl.wristPosition = WristPosition.CORAL_GROUND; + // queuedControl.shoulderPosition = ShoulderPosition.CORAL_GROUND; + // queuedControl.elevatorPosition = ElevatorPosition.CORAL_GROUND; + // }); + // addRule( + // "Prep Algae Ground", + // boxopGamepad.whenButton(8), + // ONCE, + // Set.of(wrist, shoulder, elevator), + // () -> { + // queuedControl.wristPosition = WristPosition.ALGAE_GROUND; + // queuedControl.shoulderPosition = ShoulderPosition.ALGAE_GROUND; + // queuedControl.elevatorPosition = ElevatorPosition.ALGAE_GROUND; + // }); + + // addRule( + // "Score L2 or L3 LEFT", + // rightJoystick.whenButton(1), + // ONCE_AND_HOLD, + // () -> + // new AutoScore( + // Vision.getTargetPositionLeftL2L3(), + // swerveDrive, + // wrist, + // shoulder, + // elevator, + // queuedControl.elevatorPosition.getPosition(), + // queuedControl.wristPosition.getPosition(), + // queuedControl.shoulderPosition.getPosition())); + // addRule( + // "Score L2 or L3 RIGHT", + // rightJoystick.whenButton(2), + // ONCE_AND_HOLD, + // () -> + // new AutoScore( + // Vision.getTargetPositionRightL2L3(), + // swerveDrive, + // wrist, + // shoulder, + // elevator, + // queuedControl.elevatorPosition.getPosition(), + // queuedControl.wristPosition.getPosition(), + // queuedControl.shoulderPosition.getPosition())); + // addRule( + // "Score L4 LEFT", + // rightJoystick.whenButton(3), + // ONCE_AND_HOLD, + // () -> + // new AutoScore( + // Vision.getTargetPositionLeftL4(), + // swerveDrive, + // wrist, + // shoulder, + // elevator, + // queuedControl.elevatorPosition.getPosition(), + // queuedControl.wristPosition.getPosition(), + // queuedControl.shoulderPosition.getPosition())); + // addRule( + // "Score L4 RIGHT", + // rightJoystick.whenButton(4), + // ONCE_AND_HOLD, + // () -> + // new AutoScore( + // Vision.getTargetPositionRightL4(), + // swerveDrive, + // wrist, + // shoulder, + // elevator, + // queuedControl.elevatorPosition.getPosition(), + // queuedControl.wristPosition.getPosition(), + // queuedControl.shoulderPosition.getPosition())); + } +} diff --git a/src/main/java/com/team766/robot/copy_2910/Robot.java b/src/main/java/com/team766/robot/copy_2910/Robot.java index 4970f9030..04a50bcd4 100644 --- a/src/main/java/com/team766/robot/copy_2910/Robot.java +++ b/src/main/java/com/team766/robot/copy_2910/Robot.java @@ -3,28 +3,31 @@ import com.team766.framework.AutonomousMode; import com.team766.framework.RuleEngine; import com.team766.hal.RobotConfigurator; -import com.team766.robot.burro_elevator.mechanisms.Elevator; import com.team766.robot.common.SwerveConfig; import com.team766.robot.common.mechanisms.SwerveDrive; import com.team766.robot.copy_2910.mechanisms.Climber; +import com.team766.robot.copy_2910.mechanisms.Elevator; import com.team766.robot.copy_2910.mechanisms.Intake; import com.team766.robot.copy_2910.mechanisms.Shoulder; import com.team766.robot.copy_2910.mechanisms.Vision; import com.team766.robot.copy_2910.mechanisms.Wrist; +import com.team766.robot.copy_2910.procedures.CenterL1; +import com.team766.robot.copy_2910.procedures.DriveStraight; +import com.team766.robot.gatorade.Lights; public class Robot implements RobotConfigurator { private SwerveDrive drive; private Intake intake; + private Vision vision; private Climber climber; private Elevator elevator; - private Vision vision; private Shoulder shoulder; private Wrist wrist; @Override public void initializeMechanisms() { - SwerveConfig swerveConfig = new SwerveConfig(); + SwerveConfig swerveConfig = new SwerveConfig().withDistanceBetweenWheels(0.533); drive = new SwerveDrive(swerveConfig); intake = new Intake(); climber = new Climber(); @@ -36,19 +39,24 @@ public void initializeMechanisms() { @Override public RuleEngine createOI() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'createOI'"); + return new OI(drive, intake, wrist, elevator, shoulder, climber, vision); } @Override public RuleEngine createLights() { // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'createLights'"); + // throw new UnsupportedOperationException("Unimplemented method 'createLights'"); + return new Lights(); } @Override public AutonomousMode[] getAutonomousModes() { // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getAutonomousModes'"); + return new AutonomousMode[] { + new AutonomousMode("Move", () -> new DriveStraight(drive)), + new AutonomousMode( + "Center L1", () -> new CenterL1(drive, intake, wrist, elevator, shoulder)) + }; + // throw new UnsupportedOperationException("Unimplemented method 'getAutonomousModes'"); } } diff --git a/src/main/java/com/team766/robot/copy_2910/mechanisms/Elevator.java b/src/main/java/com/team766/robot/copy_2910/mechanisms/Elevator.java index 4e7a2573f..79cc0b9aa 100644 --- a/src/main/java/com/team766/robot/copy_2910/mechanisms/Elevator.java +++ b/src/main/java/com/team766/robot/copy_2910/mechanisms/Elevator.java @@ -1,11 +1,9 @@ package com.team766.robot.copy_2910.mechanisms; -import com.team766.config.ConfigFileReader; import com.team766.framework.MechanismWithStatus; import com.team766.framework.Status; import com.team766.hal.MotorController; import com.team766.hal.RobotProvider; -import com.team766.library.ValueProvider; import com.team766.math.Maths; public class Elevator extends MechanismWithStatus { @@ -14,12 +12,12 @@ public class Elevator extends MechanismWithStatus { private MotorController elevatorMotorLeft; private static double NUDGE_AMOUNT = - 0.1; // Amount to nudge up/down | TODO: Adjust this value based on the elevator's + 0.5; // Amount to nudge up/down | TODO: Adjust this value based on the elevator's // characteristics private static double THRESHOLD = - 0.05; // Threshold for PID controller | TODO: Adjust this value based on the elevator's + 0.5; // Threshold for PID controller | TODO: Adjust this value based on the elevator's // characteristics - private ValueProvider ffGain; + // private ValueProvider ffGain; private double setPoint; public static record ElevatorStatus(double currentPosition, double targetPosition) @@ -40,34 +38,44 @@ public Elevator() { // TODO: FIGURE OUT WHICH MOTOR NEEDS TO BE INVERTED // elevatorMotorRight.setInverted(false); // Set to true if the right motor needs to be // inverted - - elevatorMotorLeft.follow(elevatorMotorRight); setPoint = ElevatorPosition.READY.getPosition(); // Default position - elevatorMotorRight.setCurrentLimit( - 30); // Set current limit for the elevator motor | TODO: Replace with actual value - ffGain = - ConfigFileReader.instance.getDouble( - "ElevatorFFGain"); // Replace with actual config key - - elevatorMotorRight.setSensorPosition( + elevatorMotorLeft.setCurrentLimit(40); + elevatorMotorRight.setCurrentLimit(40); // Set current limit for the elevator motor + // elevatorMotorLeft.setInverted(true); + // elevatorMotorRight.setInverted(false); + // elevatorMotorLeft.setInverted(true); + // elevatorMotorRight.setInverted(false); + elevatorMotorRight.follow(elevatorMotorLeft); + // ffGain = + // ConfigFileReader.instance.getDouble( + // "ElevatorFFGain"); // Replace with actual config key + + elevatorMotorLeft.setSensorPosition( 0.0); // Elevator always has to start at same 0.0 position + // elevatorMotorRight.setInverted(true); } public enum ElevatorPosition { - INTAKE(0.0), - L1(0.881), - L2(-4.452), - L3(-11.5), - L4(-21.357), - ALGAE_HIGH(-9.357), - ALGAE_LOW(-3.262), - CORAL_GROUND(-0.357), - ALGAE_GROUND(-1.643), - READY(-10), // Should be the default position and the ready position for vision so that it + INTAKE(0.4), + L1(3.8), + L2(5.3), + L3(13.27), + L4(24), // -21.357 + ALGAE_HIGH(12), + ALGAE_LOW(7), + ALGAE_SHOOT(17.80), + CORAL_GROUND(-0.25), // -0.25 + ALGAE_GROUND(0.5), + STOW(0.5), + CLIMBDOWN(3), + + READY(1), // Should be the default position and the ready position for vision so that it // can see the tag - MAXIMUM(2), // Maximum height of the elevator, TODO: Adjust based on the actual elevator's + MAXIMUM(25), // Maximum height of the elevator, TODO: Adjust based on the actual + // elevator's // maximum position - MINIMUM(-25); // Minimum height of the elevator, TODO: Adjust based on the actual elevator's + MINIMUM(-1.0); // Minimum height of the elevator, TODO: Adjust based on the actual + // elevator's // minimum position final double position; @@ -89,19 +97,32 @@ public void setPosition(double setPosition) { ElevatorPosition.MAXIMUM.getPosition()); } + public void setPosition(ElevatorPosition setPosition) { + setPosition(setPosition.getPosition()); + } + public void nudgeUp() { - setPosition(setPoint + NUDGE_AMOUNT); + setPoint += NUDGE_AMOUNT; } public void nudgeDown() { - setPosition(setPoint - NUDGE_AMOUNT); + setPoint -= NUDGE_AMOUNT; + } + + public void nudge(double input) { + if (input > 0) { + nudgeUp(); + } else { + nudgeDown(); + } } public void run() { - elevatorMotorRight.set(MotorController.ControlMode.Position, setPoint, ffGain.valueOr(0.0)); + elevatorMotorLeft.set(MotorController.ControlMode.Position, setPoint); + log("SHOULDER Setpoint: " + setPoint + " Pos: " + elevatorMotorLeft.getSensorPosition()); } protected ElevatorStatus updateStatus() { - return new ElevatorStatus(elevatorMotorRight.getSensorPosition(), setPoint); + return new ElevatorStatus(elevatorMotorLeft.getSensorPosition(), setPoint); } } diff --git a/src/main/java/com/team766/robot/copy_2910/mechanisms/Intake.java b/src/main/java/com/team766/robot/copy_2910/mechanisms/Intake.java index 8a2a2a371..4bd139f87 100644 --- a/src/main/java/com/team766/robot/copy_2910/mechanisms/Intake.java +++ b/src/main/java/com/team766/robot/copy_2910/mechanisms/Intake.java @@ -16,12 +16,18 @@ public class Intake extends MechanismWithStatus { private MotorController centerAlgaeMotor; - private static final double CORAL_THRESHOLD = 200; // TODO: Set this to a real value + private static final double CORAL_THRESHOLD = + 0.03; // TODO: Set this to a real value | Previously 0.12 + private static final double CORAL_OUTTAKE_THRESHOLD = 0.12; + // Previous Left & Right CANRange FOV: 27 + // Previous Phoenix Tuner prox threshold: 0.4 + // MAKE SURE both left & right motor are counterclockwise on phoenix tuner - private double leftPower = 0.25; - private double rightPower = 0.25; + private double leftPower = 0.4; + private double rightPower = 0.4; - private double algaePower = 0.35; + private double algaePower = 0.3; // previously 0.5 + private double algaeSlowPower = -0.05; public Intake() { leftCANRange = RobotProvider.instance.getTimeOfFlight("INTAKE.CANRange.left"); @@ -46,6 +52,15 @@ public record IntakeStatus( /* * TODO: During bringup, check the atual distance values to find a range where the coral is validly in the sensor (to ensure no malfunctioning sensors). */ + + public double getLeftDistance() { + return leftDistance; + } + + public double getBackCenterDistance() { + return backCenterDistance; + } + public boolean hasCoralInLeft() { return leftDistance < CORAL_THRESHOLD; } @@ -61,6 +76,10 @@ public boolean hasCoralInFrontCenter() { public boolean hasCoralInBackCenter() { return backCenterDistance < CORAL_THRESHOLD; } + + public boolean hasCoralToOuttake() { + return backCenterDistance < CORAL_OUTTAKE_THRESHOLD; + } } public void setLeft(double power) { @@ -88,25 +107,29 @@ public void setAlgaePower(double power) { } public void turnLeftPositive() { - leftMotor.set(leftPower); + leftMotor.set(-leftPower); } public void turnLeftNegative() { - leftMotor.set(-leftPower); + leftMotor.set(leftPower); } public void turnRightPositive() { - rightMotor.set(rightPower); + rightMotor.set(-rightPower); } public void turnRightNegative() { - rightMotor.set(-rightPower); + rightMotor.set(rightPower); } public void turnAlgaePositive() { centerAlgaeMotor.set(algaePower); } + public void retainAlgae() { + centerAlgaeMotor.set(algaeSlowPower); + } + public void turnAlgaeNegative() { centerAlgaeMotor.set(-algaePower); } @@ -136,6 +159,15 @@ protected IntakeStatus updateStatus() { double frontCenterDistance = frontCenterCANRange.getDistance().orElse(Double.MAX_VALUE); double backCenterDistance = backCenterCANRange.getDistance().orElse(Double.MAX_VALUE); + log( + "Intake Status: leftDistance = " + + leftDistance + + ", rightDistance = " + + rightDistance + + ", frontCenterDistance = " + + frontCenterDistance + + ", backCenterDistance = " + + backCenterDistance); return new IntakeStatus( leftDistance, rightDistance, frontCenterDistance, backCenterDistance); } diff --git a/src/main/java/com/team766/robot/copy_2910/mechanisms/Shoulder.java b/src/main/java/com/team766/robot/copy_2910/mechanisms/Shoulder.java index a8667e1b6..93a7849b3 100644 --- a/src/main/java/com/team766/robot/copy_2910/mechanisms/Shoulder.java +++ b/src/main/java/com/team766/robot/copy_2910/mechanisms/Shoulder.java @@ -1,8 +1,10 @@ package com.team766.robot.copy_2910.mechanisms; +import com.ctre.phoenix.motorcontrol.NeutralMode; import com.team766.config.ConfigFileReader; import com.team766.framework.MechanismWithStatus; import com.team766.framework.Status; +import com.team766.hal.EncoderReader; import com.team766.hal.MotorController; import com.team766.hal.RobotProvider; import com.team766.library.ValueProvider; @@ -12,15 +14,21 @@ public class Shoulder extends MechanismWithStatus { private MotorController leftMotor; private MotorController rightMotor; + private final EncoderReader absoluteEncoder; + private boolean encoderInitialized = false; + private double gearRatio = 120; + private boolean noPIDMode; + private final ValueProvider ffGain; private static final double THRESHOLD = 0.5; // Threshold for determining if the shoulder is near a position | TODO: Adjust this + // was 0.5 // value based on the shoulder's characteristics private double setPoint; - private ValueProvider ffGain; + // private ValueProvider ffGain; private final double NUDGE_AMOUNT = - 5; // Amount to nudge up/down | TODO: Adjust this value based on the shoulder's + 0.5; // Amount to nudge up/down | TODO: Adjust this value based on the shoulder's // characteristics @@ -35,16 +43,18 @@ public boolean isNearTo(double angle) { } public enum ShoulderPosition { - L1(12.119), - L2(9.643), - L3(15.071), - L4(21.5), + L1(14), + L2(50.891), + L3(70.247), + L4(89), ALGAE_HIGH(22.952), ALGAE_LOW(20.405), - CORAL_GROUND(0.071), + CORAL_GROUND(1), ALGAE_GROUND(4.119), - MAXIMUM(40), - MINIMUM(0); + CLIMBER(100), + STOW(0), + MAXIMUM(100), + MINIMUM(-10); private final double angle; @@ -58,22 +68,33 @@ public double getPosition() { } public Shoulder() { + noPIDMode = false; leftMotor = RobotProvider.instance.getMotor( "LeftShoulderMotor"); // Replace with actual motor name rightMotor = RobotProvider.instance.getMotor( "RightShoulderMotor"); // Replace with actual motor name - + absoluteEncoder = + RobotProvider.instance.getEncoder( + "ShoulderEncoder"); // **ShoulderEncoder may not exist** + leftMotor.setNeutralMode(NeutralMode.Coast); + rightMotor.setNeutralMode(NeutralMode.Coast); + // leftMotor.setInverted(true); + // leftMotor.setInverted(true); rightMotor.follow(leftMotor); - ffGain = - ConfigFileReader.instance.getDouble( - "ShoulderFFGain"); // Replace with actual config key - setPoint = ShoulderPosition.L1.getPosition(); // Default position + leftMotor.setCurrentLimit(40); + rightMotor.setCurrentLimit(40); + ffGain = ConfigFileReader.getInstance().getDouble("Shoulder_FFGain"); + // ffGain = + // ConfigFileReader.instance.getDouble( + // "ShoulderFFGain"); // Replace with actual config key + // setPoint = ShoulderPosition.L1.getPosition(); // Default position } public void setSetpoint(double setpoint) { + noPIDMode = false; setPoint = MathUtil.clamp( setpoint, @@ -81,20 +102,62 @@ public void setSetpoint(double setpoint) { ShoulderPosition.MAXIMUM.getPosition()); } + public void setPosition(ShoulderPosition position) { + setSetpoint(position.getPosition()); + } + + public void setBrakeMode() { + leftMotor.setNeutralMode(NeutralMode.Brake); + rightMotor.setNeutralMode(NeutralMode.Brake); + } + public void run() { - leftMotor.set(MotorController.ControlMode.Position, setPoint, ffGain.get()); + // leftMotor.set(.Position, setPoint) + // All of the following PID code is directly copied from the 2910 wrist code -> may not work + // properly + if (!noPIDMode) { + double ff = + ffGain.valueOr(0.0); // * Math.cos(Math.toRadians(getStatus().currentAngle())); + leftMotor.set(MotorController.ControlMode.Position, setPoint, ff); + } else { + leftMotor.set(MotorController.ControlMode.Position, setPoint); + } + log("SHOULDER Setpoint: " + setPoint + " Pos: " + leftMotor.getSensorPosition()); } public void nudgeUp() { - setSetpoint(setPoint + NUDGE_AMOUNT); + setPoint += 1; } public void nudgeDown() { setSetpoint(setPoint - NUDGE_AMOUNT); } + public void nudge(double input) { + noPIDMode = false; + if (input > 0) { + nudgeUp(); + } else { + nudgeDown(); + } + } + + public void nudgeNoPID(double input) { + noPIDMode = true; + if (input > 0) { + nudgeUp(); + } else { + nudgeDown(); + } + } + @Override protected ShoulderStatus updateStatus() { + if (!encoderInitialized && absoluteEncoder.isConnected()) { + double motorRotations = absoluteEncoder.getPosition() * gearRatio; + leftMotor.setSensorPosition(motorRotations); + encoderInitialized = true; + } return new ShoulderStatus(leftMotor.getSensorPosition(), setPoint); } } diff --git a/src/main/java/com/team766/robot/copy_2910/mechanisms/Wrist.java b/src/main/java/com/team766/robot/copy_2910/mechanisms/Wrist.java index d6abcf80d..3c9bbcf0b 100644 --- a/src/main/java/com/team766/robot/copy_2910/mechanisms/Wrist.java +++ b/src/main/java/com/team766/robot/copy_2910/mechanisms/Wrist.java @@ -3,6 +3,7 @@ import com.team766.config.ConfigFileReader; import com.team766.framework.MechanismWithStatus; import com.team766.framework.Status; +import com.team766.hal.EncoderReader; import com.team766.hal.MotorController; import com.team766.hal.RobotProvider; import com.team766.library.ValueProvider; @@ -11,15 +12,20 @@ public class Wrist extends MechanismWithStatus { private MotorController motor; + private double gearRatio = 21.8; + private final ValueProvider ffGain; + private boolean noPIDMode; + private final EncoderReader absoluteEncoder; + private boolean encoderInitialized = false; private static final double THRESHOLD = 0.5; // Threshold for determining if the wrist is near a position | TODO: Adjust this // value based on the wrist's characteristics private double setPoint; - private ValueProvider ffGain; + // private ValueProvider ffGain; private final double NUDGE_AMOUNT = - 5; // Amount to nudge up/down | TODO: Adjust this value based on the wrist's + 0.5; // Amount to nudge up/down | TODO: Adjust this value based on the wrist's // characteristics @@ -34,16 +40,20 @@ public boolean isNearTo(double angle) { } public enum WristPosition { - L1(-16.643), - L2(0.643), - L3(-1.667), - L4(-5.5), + L1(-16.643), // 16.643 + L2(1.095), + L3(-0.808), // -0.808 + L4(-5.561), ALGAE_HIGH(-25.928), ALGAE_LOW(-25.928), - CORAL_GROUND(-15.81), + CORAL_GROUND(-9.75), + ALGAE(-25.2), ALGAE_GROUND(-21.786), - MAXIMUM(10), - MINIMUM(-30); + ALGAE_SHOOT(0.643), + STOW(0), + MAXIMUM(50), + MINIMUM(-30), + CLIMB(-11); private final double angle; @@ -58,14 +68,23 @@ public double getPosition() { public Wrist() { motor = RobotProvider.instance.getMotor("WristMotor"); // Replace with actual motor name - ffGain = - ConfigFileReader.instance.getDouble( - "WristFFGain"); // Replace with actual config key - setPoint = WristPosition.L3.getPosition(); // Default position + ConfigFileReader.getInstance() + .getDouble("Wrist_FFGain"); // ** Wrist_FFGain does not exist yet ** + noPIDMode = true; + absoluteEncoder = + RobotProvider.instance.getEncoder( + "WristEncoder"); // ** WristEncoder may not exist ** + motor.setCurrentLimit(40); + + // ffGain = + // ConfigFileReader.instance.getDouble( + // "WristFFGain"); // Replace with actual config key + // setPoint = WristPosition.L3.getPosition(); // Default position } public void setSetpoint(double setpoint) { + noPIDMode = true; setPoint = MathUtil.clamp( setpoint, @@ -73,20 +92,57 @@ public void setSetpoint(double setpoint) { WristPosition.MAXIMUM.getPosition()); } + public void setPosition(WristPosition wristPosition) { + setSetpoint(wristPosition.getPosition()); + } + public void run() { - motor.set(MotorController.ControlMode.Position, setPoint, ffGain.get()); + if (!noPIDMode) { + double ff = + ffGain.valueOr(0.0); // * Math.cos(Math.toRadians(getStatus().currentAngle())); + motor.set(MotorController.ControlMode.Position, setPoint / gearRatio, ff); + /*wristMotor.set( + MotorController.ControlMode.Position, + EncoderUtils.coralWristDegreesToRotations(setPoint), + ff); */ + } else { + motor.set(MotorController.ControlMode.Position, setPoint); + } } public void nudgeUp() { - setSetpoint(setPoint + NUDGE_AMOUNT); + setPoint += NUDGE_AMOUNT; } public void nudgeDown() { - setSetpoint(setPoint - NUDGE_AMOUNT); + setPoint -= NUDGE_AMOUNT; + } + + public void nudge(double input) { + noPIDMode = false; + if (input > 0) { + nudgeUp(); + } else { + nudgeDown(); + } + } + + public void nudgeNoPID(double input) { + noPIDMode = true; + if (input > 0) { + nudgeUp(); + } else { + nudgeDown(); + } } @Override protected WristStatus updateStatus() { + if (!encoderInitialized && absoluteEncoder.isConnected()) { + double motorRotations = absoluteEncoder.getPosition() * gearRatio; + motor.setSensorPosition(motorRotations); + encoderInitialized = true; + } return new WristStatus(motor.getSensorPosition(), setPoint); } } diff --git a/src/main/java/com/team766/robot/copy_2910/procedures/AutoAlign.java b/src/main/java/com/team766/robot/copy_2910/procedures/AutoScore.java similarity index 74% rename from src/main/java/com/team766/robot/copy_2910/procedures/AutoAlign.java rename to src/main/java/com/team766/robot/copy_2910/procedures/AutoScore.java index 7b33dfd62..0c3681b3f 100644 --- a/src/main/java/com/team766/robot/copy_2910/procedures/AutoAlign.java +++ b/src/main/java/com/team766/robot/copy_2910/procedures/AutoScore.java @@ -4,31 +4,52 @@ import com.team766.framework.Context; import com.team766.framework.Procedure; import com.team766.robot.common.mechanisms.SwerveDrive; +import com.team766.robot.copy_2910.mechanisms.Elevator; +import com.team766.robot.copy_2910.mechanisms.Shoulder; import com.team766.robot.copy_2910.mechanisms.Vision; +import com.team766.robot.copy_2910.mechanisms.Wrist; import edu.wpi.first.math.geometry.Pose2d; -public class AutoAlign extends Procedure { +public class AutoScore extends Procedure { private Pose2d targetPosition; private SwerveDrive drive; private PIDController pidControllerX; private PIDController pidControllerY; private PIDController pidControllerRotation; + private Wrist wrist; + private Shoulder shoulder; + private Elevator elevator; - public AutoAlign(Pose2d targetPosition, SwerveDrive drive) { + private double elevatorHeight; + private double wristAngle; + private double shoulderAngle; + + public AutoScore( + Pose2d targetPosition, + SwerveDrive drive, + Wrist wrist, + Shoulder shoulder, + Elevator elevator, + double elevatorHeight, + double wristAngle, + double shoulderAngle) { this.targetPosition = targetPosition; this.drive = reserve(drive); + this.wrist = reserve(wrist); + this.shoulder = reserve(shoulder); + this.elevator = reserve(elevator); + this.elevatorHeight = elevatorHeight; + this.wristAngle = wristAngle; + this.shoulderAngle = shoulderAngle; pidControllerX = PIDController.loadFromConfig("DRIVE_X_PID"); pidControllerY = PIDController.loadFromConfig("DRIVE_Y_PID"); pidControllerRotation = PIDController.loadFromConfig("DRIVE_ROTATION_PID"); } - public AutoAlign(Pose2d targetPosition, double threshold, SwerveDrive drive) { - this(targetPosition, drive); - pidControllerX.setThreshold(threshold); - pidControllerY.setThreshold(threshold); - } - public void run(Context context) { + shoulder.setSetpoint(shoulderAngle); + wrist.setSetpoint(wristAngle); + elevator.setPosition(elevatorHeight); Pose2d currentPosition; try { currentPosition = getStatusOrThrow(Vision.VisionStatus.class).getApriltagPose2d(); diff --git a/src/main/java/com/team766/robot/copy_2910/procedures/CenterL1.java b/src/main/java/com/team766/robot/copy_2910/procedures/CenterL1.java new file mode 100644 index 000000000..fd7c74ed5 --- /dev/null +++ b/src/main/java/com/team766/robot/copy_2910/procedures/CenterL1.java @@ -0,0 +1,30 @@ +package com.team766.robot.copy_2910.procedures; + +import com.team766.robot.common.mechanisms.SwerveDrive; +import com.team766.robot.common.procedures.PathSequenceAuto; +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 edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; + +public class CenterL1 extends PathSequenceAuto { + public CenterL1( + SwerveDrive drive, Intake intake, Wrist wrist, Elevator elevator, Shoulder shoulder) { + super(drive, new Pose2d(7.160, 3.970, Rotation2d.fromDegrees(-180))); + addProcedure( + new MoveWristvator( + shoulder, + elevator, + wrist, + ShoulderPosition.L1, + ElevatorPosition.L1, + WristPosition.L1)); + addPath("Center Start - L1 Score"); + addProcedure(new OuttakeCoral(intake)); + } +} diff --git a/src/main/java/com/team766/robot/copy_2910/procedures/DriveStraight.java b/src/main/java/com/team766/robot/copy_2910/procedures/DriveStraight.java new file mode 100644 index 000000000..a789e5c4e --- /dev/null +++ b/src/main/java/com/team766/robot/copy_2910/procedures/DriveStraight.java @@ -0,0 +1,21 @@ +package com.team766.robot.copy_2910.procedures; + +import com.team766.framework.Context; +import com.team766.framework.Procedure; +import com.team766.robot.common.mechanisms.SwerveDrive; + +public class DriveStraight extends Procedure { + private SwerveDrive drive; + + public DriveStraight(SwerveDrive drive) { + this.drive = reserve(drive); + } + + @Override + public void run(Context context) { + // context.waitForSeconds(2); + drive.controlRobotOriented(1, 0, 0); + context.waitForSeconds(10); + drive.controlRobotOriented(0, 0, 0); + } +} diff --git a/src/main/java/com/team766/robot/copy_2910/procedures/IntakeCoral.java b/src/main/java/com/team766/robot/copy_2910/procedures/IntakeCoral.java index 9cbf8b3ae..71e95d04c 100644 --- a/src/main/java/com/team766/robot/copy_2910/procedures/IntakeCoral.java +++ b/src/main/java/com/team766/robot/copy_2910/procedures/IntakeCoral.java @@ -2,25 +2,53 @@ import com.team766.framework.Context; import com.team766.framework.Procedure; +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 java.util.Optional; public class IntakeCoral extends Procedure { private Intake intake; + private Elevator elevator; + private Shoulder shoulder; + private Wrist wrist; - public IntakeCoral() { - intake = reserve(intake); + public IntakeCoral(Intake intake, Elevator elevator, Shoulder shoulder, Wrist wrist) { + this.intake = reserve(intake); + this.elevator = reserve(elevator); + this.shoulder = reserve(shoulder); + this.wrist = reserve(wrist); } @Override public void run(Context context) { + elevator.setPosition(ElevatorPosition.CORAL_GROUND); + shoulder.setPosition(ShoulderPosition.CORAL_GROUND); + wrist.setPosition(WristPosition.CORAL_GROUND); + waitForStatusMatching( + context, + Shoulder.ShoulderStatus.class, + s -> s.isNearTo(ShoulderPosition.CORAL_GROUND)); + waitForStatusMatching( + context, Wrist.WristStatus.class, s -> s.isNearTo(WristPosition.CORAL_GROUND)); Optional status = getStatus(Intake.IntakeStatus.class); if (status.isEmpty()) { log("No intake status"); return; } - while (!status.get().hasCoralInBackCenter()) { + log( + "Intake Status: leftDistance = " + + status.get().getLeftDistance() + + "Back center:" + + status.get().getBackCenterDistance()); + while (waitForStatusMatchingOrTimeout( + context, Intake.IntakeStatus.class, s -> !s.hasCoralInBackCenter(), 0.35) + .isPresent()) { context.yield(); intake.turnAlgaePositive(); status = getStatus(Intake.IntakeStatus.class); @@ -40,7 +68,6 @@ public void run(Context context) { if (hasCoralInLeft && hasCoralInRight && hasCoralInFrontCenter) { intake.turnLeftPositive(); intake.turnRightPositive(); - continue; } /* @@ -62,14 +89,14 @@ public void run(Context context) { * Crazy enough, this is the same as situation two! */ /* - * Case 4: Coral is in the front center of the intake - * | coral | - * |-----------------------intake--------------------| - * - * We just need to move the coral inwards, so spin the right motor clockwise, to the right, or in the positive direction, - * and the left motor anticlockwise, to the left, or in the negative direction. - * This is the same as situation two and three! - */ + * Case 4: Coral is in the front center of the intake + * | coral | + + * + * We just need to move the coral inwards, so spin the right motor clockwise, to the right, or in the positive direction, + * and the left motor anticlockwise, to the left, or in the negative direction. + * This is the same as situation two and three! + */ else { intake.turnLeftNegative(); intake.turnRightPositive(); diff --git a/src/main/java/com/team766/robot/copy_2910/procedures/IntakeCoralL1.java b/src/main/java/com/team766/robot/copy_2910/procedures/IntakeCoralL1.java new file mode 100644 index 000000000..281c78581 --- /dev/null +++ b/src/main/java/com/team766/robot/copy_2910/procedures/IntakeCoralL1.java @@ -0,0 +1,106 @@ +package com.team766.robot.copy_2910.procedures; + +import com.team766.framework.Context; +import com.team766.framework.Procedure; +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 java.util.Optional; + +public class IntakeCoralL1 extends Procedure { + + private Intake intake; + private Elevator elevator; + private Shoulder shoulder; + private Wrist wrist; + + public IntakeCoralL1(Intake intake, Elevator elevator, Shoulder shoulder, Wrist wrist) { + this.intake = reserve(intake); + this.elevator = reserve(elevator); + this.shoulder = reserve(shoulder); + this.wrist = reserve(wrist); + } + + @Override + public void run(Context context) { + elevator.setPosition(ElevatorPosition.CORAL_GROUND); + shoulder.setPosition(ShoulderPosition.CORAL_GROUND); + wrist.setPosition(WristPosition.CORAL_GROUND); + waitForStatusMatching( + context, + Shoulder.ShoulderStatus.class, + s -> s.isNearTo(ShoulderPosition.CORAL_GROUND)); + waitForStatusMatching( + context, Wrist.WristStatus.class, s -> s.isNearTo(WristPosition.CORAL_GROUND)); + Optional status = getStatus(Intake.IntakeStatus.class); + if (status.isEmpty()) { + log("No intake status"); + return; + } + log( + "Intake Status: leftDistance = " + + status.get().getLeftDistance() + + "Back center:" + + status.get().getBackCenterDistance()); + while (!status.get().hasCoralInBackCenter()) { + context.yield(); + intake.turnAlgaePositive(); + status = getStatus(Intake.IntakeStatus.class); + boolean hasCoralInLeft = status.get().hasCoralInLeft(); + boolean hasCoralInRight = status.get().hasCoralInRight(); + boolean hasCoralInFrontCenter = status.get().hasCoralInFrontCenter(); + + /* + * Case 1: Coral is near the intake and is alligned horizontally + * + * | coral | + * |-------------intake------------| + * + * We need to move the coral to the right so that it can get intaked from the center. + * Thus, we need to turn both motors clockwise–to the right, or in the positive direction. + */ + if (hasCoralInLeft && hasCoralInRight && hasCoralInFrontCenter) { + intake.turnAlgaePositive(); + continue; + } + + /* + * Case 2: Coral is only in the right side of the intake + * | coral | + * |-----------------------intake--------------------| + * + * We need to move the coral to the right so that it can get intaked from the center. + * Thus, we need to turn the right motor clockwise–to the right, or in the positive direction. + * And, we should turn the left motor anticlockwise–to the left, or in the negative direction, to push it in. + */ + /* + * Case 3: Coral is only in the left side of the intake + * | coral | + * |-----------------------intake--------------------| + * * We need to move the coral to the left so that it can get intaked from the center. + * Thus, we need to turn the left motor anticlockwise, to the left, or in the negative direction. + * And, we should turn the right motor clockwise, to the right, or in the positive direction, to push it in. + * Crazy enough, this is the same as situation two! + */ + /* + * Case 4: Coral is in the front center of the intake + * | coral | + + * + * We just need to move the coral inwards, so spin the right motor clockwise, to the right, or in the positive direction, + * and the left motor anticlockwise, to the left, or in the negative direction. + * This is the same as situation two and three! + */ + else { + intake.turnAlgaePositive(); // No way to move coral slightly to the left or right, + // so needs to be pos/neg all the time + } + } + // Once the coral is in the back center, we stop the intake motors. + intake.stop(); + } +} diff --git a/src/main/java/com/team766/robot/copy_2910/procedures/MoveWristvator.java b/src/main/java/com/team766/robot/copy_2910/procedures/MoveWristvator.java new file mode 100644 index 000000000..ca7807da1 --- /dev/null +++ b/src/main/java/com/team766/robot/copy_2910/procedures/MoveWristvator.java @@ -0,0 +1,66 @@ +package com.team766.robot.copy_2910.procedures; + +import com.team766.framework.Context; +import com.team766.framework.Procedure; +import com.team766.robot.copy_2910.mechanisms.Elevator; +import com.team766.robot.copy_2910.mechanisms.Elevator.ElevatorPosition; +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; + +public class MoveWristvator extends Procedure { + private final Shoulder shoulder; + private final Elevator elevator; + private final Wrist wrist; + private final ShoulderPosition shoulderSetpoint; + private final ElevatorPosition elevatorSetpoint; + private final WristPosition wristSetpoint; + + public MoveWristvator( + Shoulder shoulder, + Elevator elevator, + Wrist wrist, + ShoulderPosition shoulderSetpoint_, + ElevatorPosition elevatorSetpoint_, + WristPosition wristSetpoint_) { + this.shoulder = reserve(shoulder); + this.elevator = reserve(elevator); + this.wrist = reserve(wrist); + this.shoulderSetpoint = shoulderSetpoint_; + this.elevatorSetpoint = elevatorSetpoint_; + this.wristSetpoint = wristSetpoint_; + } + + @Override + public final void run(Context context) { + // Always retract the wrist before moving the elevator. + // It might already be retracted, so it's possible that this step finishes instantaneously. + wrist.setPosition(WristPosition.STOW); + // If raising the shoulder, do that before the elevator (else, lower it after the elevator). + if (shoulderSetpoint.getPosition() + > getStatusOrThrow(Shoulder.ShoulderStatus.class).position()) { + shoulder.setPosition(shoulderSetpoint); + waitForStatusMatching( + context, Shoulder.ShoulderStatus.class, s -> s.isNearTo(shoulderSetpoint)); + } + waitForStatusMatching( + context, Wrist.WristStatus.class, s -> s.isNearTo(WristPosition.STOW)); + + // Move the elevator. Wait until it gets near the target position. + elevator.setPosition(elevatorSetpoint); + waitForStatusMatching(context, Elevator.ElevatorStatus.class, s -> s.isAtHeight()); + + // If lowering the shoulder, do that after the elevator. + if (shoulderSetpoint.getPosition() + < getStatusOrThrow(Shoulder.ShoulderStatus.class).position()) { + shoulder.setPosition(shoulderSetpoint); + } + + // Lastly, move the wrist. + wrist.setPosition(wristSetpoint); + waitForStatusMatching(context, Wrist.WristStatus.class, s -> s.isNearTo(wristSetpoint)); + waitForStatusMatching( + context, Shoulder.ShoulderStatus.class, s -> s.isNearTo(shoulderSetpoint)); + } +} diff --git a/src/main/java/com/team766/robot/copy_2910/procedures/OuttakeCoral.java b/src/main/java/com/team766/robot/copy_2910/procedures/OuttakeCoral.java index 990735b4e..5f2bbde14 100644 --- a/src/main/java/com/team766/robot/copy_2910/procedures/OuttakeCoral.java +++ b/src/main/java/com/team766/robot/copy_2910/procedures/OuttakeCoral.java @@ -8,16 +8,15 @@ public class OuttakeCoral extends Procedure { private Intake intake; - public OuttakeCoral() { - intake = reserve(intake); + public OuttakeCoral(Intake intake2) { + intake = reserve(intake2); } @Override public void run(Context context) { - intake.turnRightPositive(); - intake.turnLeftNegative(); - intake.turnAlgaeNegative(); - waitForStatusMatching(context, Intake.IntakeStatus.class, s -> !s.hasCoralInFrontCenter()); - intake.stop(); + intake.setLeft(1.5); + intake.setRight(-1.5); + // waitForStatusMatching(context, Intake.IntakeStatus.class, s -> !s.hasCoralToOuttake()); + // intake.stop(); } } diff --git a/src/main/java/com/team766/robot/copy_2910/procedures/ShootAlgae.java b/src/main/java/com/team766/robot/copy_2910/procedures/ShootAlgae.java new file mode 100644 index 000000000..f3c43f39b --- /dev/null +++ b/src/main/java/com/team766/robot/copy_2910/procedures/ShootAlgae.java @@ -0,0 +1,20 @@ +package com.team766.robot.copy_2910.procedures; + +import com.team766.framework.Context; +import com.team766.framework.Procedure; +import com.team766.robot.copy_2910.mechanisms.Intake; + +public class ShootAlgae extends Procedure { + + private Intake intake; + + public ShootAlgae(Intake intake2) { + intake = reserve(intake2); + } + + @Override + public void run(Context context) { + context.waitForSeconds(.1); + intake.turnAlgaeNegative(); + } +} diff --git a/src/main/java/com/team766/robot/gatorade/mechanisms/Shoulder.java b/src/main/java/com/team766/robot/gatorade/mechanisms/Shoulder.java index f82b15323..f04c9c805 100644 --- a/src/main/java/com/team766/robot/gatorade/mechanisms/Shoulder.java +++ b/src/main/java/com/team766/robot/gatorade/mechanisms/Shoulder.java @@ -106,7 +106,7 @@ public Shoulder() { rightMotor = (SparkMax) halRightMotor; SparkMaxConfig rightConfig = new SparkMaxConfig(); - rightConfig.follow(leftMotor, true /* invert */); + rightConfig.follow(leftMotor, false /* invert */); rightMotor.configure( rightConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); diff --git a/src/main/java/com/team766/robot/reva_2025/mechanisms/Vision.java b/src/main/java/com/team766/robot/reva_2025/mechanisms/Vision.java index 50c0ba7b9..bef22e751 100644 --- a/src/main/java/com/team766/robot/reva_2025/mechanisms/Vision.java +++ b/src/main/java/com/team766/robot/reva_2025/mechanisms/Vision.java @@ -36,9 +36,9 @@ public Vision() { cameraList = new GetOrinRawValue[] { // disabling cameras for now - new GetOrinRawValue("left_back", 0.0009), + // new GetOrinRawValue("left_back", 0.0009), new GetOrinRawValue("left_front", 0.0049), - new GetOrinRawValue("right_back", 0.0009), + // new GetOrinRawValue("right_back", 0.0009), new GetOrinRawValue("right_front", 0.0049) }; } diff --git a/src/main/java/com/team766/robot/reva_2025/mechanisms/Wrist.java b/src/main/java/com/team766/robot/reva_2025/mechanisms/Wrist.java index f2d17061e..9ec9b9577 100644 --- a/src/main/java/com/team766/robot/reva_2025/mechanisms/Wrist.java +++ b/src/main/java/com/team766/robot/reva_2025/mechanisms/Wrist.java @@ -3,6 +3,7 @@ import static com.team766.robot.reva_2025.constants.ConfigConstants.WRIST_ENCODER; import static com.team766.robot.reva_2025.constants.ConfigConstants.WRIST_GYRO; +import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix6.hardware.Pigeon2; import com.team766.config.ConfigFileReader; import com.team766.framework.MechanismWithStatus; @@ -36,7 +37,7 @@ public enum WristPosition { // TODO: Change these angles to actual values CORAL_BOTTOM(30), CORAL_START(30), - CORAL_INTAKE(40), + CORAL_INTAKE(70), // CORAL_L2_PREP(260), CORAL_L1_PLACE(40), CORAL_L2_PLACE(245), @@ -68,6 +69,7 @@ public Wrist() { absoluteEncoder = RobotProvider.instance.getEncoder(WRIST_ENCODER); gyro = ((PigeonGyro) RobotProvider.instance.getGyro(WRIST_GYRO)).getPigeon(); wristMotor.setCurrentLimit(30); + wristMotor.setNeutralMode(NeutralMode.Brake); } public void setAngle(WristPosition position) {