From 8acb46f190daeecd482fc5da04a09d5723b07371 Mon Sep 17 00:00:00 2001 From: Max Spier <68516760+Mixmix00@users.noreply.github.com> Date: Mon, 21 Jul 2025 18:40:27 -0700 Subject: [PATCH 01/21] Example OI Code Introduces the OI class to define operator interface rules for the 2910 robot variant, mapping joystick and gamepad inputs to mechanisms such as intake, outtake, and climber operations. --- .../java/com/team766/robot/copy_2910/OI.java | 62 +++++++++++++++++++ 1 file changed, 62 insertions(+) create mode 100644 src/main/java/com/team766/robot/copy_2910/OI.java 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 00000000..cb9c1b7e --- /dev/null +++ b/src/main/java/com/team766/robot/copy_2910/OI.java @@ -0,0 +1,62 @@ +package com.team766.robot.copy_2910; + +import static com.team766.framework.RulePersistence.ONCE; +import static com.team766.framework.RulePersistence.ONCE_AND_HOLD; +import com.team766.framework.RuleEngine; +import com.team766.hal.JoystickReader; +import com.team766.hal.RobotProvider; +import com.team766.robot.common.mechanisms.SwerveDrive; +import com.team766.robot.copy_2910.mechanisms.*; +import com.team766.robot.copy_2910.procedures.IntakeCoral; +import com.team766.robot.copy_2910.procedures.OuttakeCoral; + + +public class OI extends RuleEngine { + + public OI( + SwerveDrive swerveDrive, + Intake intake, + Wrist wrist, + Elevator elevator, + Shoulder shoulder, + Climber climber, + Vision vision) { + + final JoystickReader leftJoystick = + RobotProvider.instance.getJoystick(1); + final JoystickReader rightJoystick = + RobotProvider.instance.getJoystick(2); + final JoystickReader boxopGamepad = + RobotProvider.instance.getJoystick(3); + + addRule("Intake Coral", + leftJoystick.whenButton(1), + ONCE_AND_HOLD, + () -> new IntakeCoral()); + + addRule("Outtake Coral", + leftJoystick.whenButton(2), + ONCE_AND_HOLD, + () -> new OuttakeCoral()); + + addRule("Climb", + boxopGamepad.whenButton(1), + climber, + ONCE_AND_HOLD, + () -> climber.setClimberSpeed(1)); + addRule("Stop Climb", + boxopGamepad.whenButton(2), + climber, + ONCE, + () -> climber.setClimberSpeed(0)); + addRule("Remove Climber", + boxopGamepad.whenButton(3), + climber, + ONCE_AND_HOLD, + () -> climber.setClimberSpeed(-1)); + + + + } + +} From 45f29d946c106765b31eec22c314c58f06249837 Mon Sep 17 00:00:00 2001 From: Max Spier <68516760+Mixmix00@users.noreply.github.com> Date: Mon, 21 Jul 2025 20:11:03 -0700 Subject: [PATCH 02/21] e --- src/main/java/com/team766/robot/copy_2910/OI.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/com/team766/robot/copy_2910/OI.java b/src/main/java/com/team766/robot/copy_2910/OI.java index cb9c1b7e..03b2a7bd 100644 --- a/src/main/java/com/team766/robot/copy_2910/OI.java +++ b/src/main/java/com/team766/robot/copy_2910/OI.java @@ -55,7 +55,6 @@ public OI( ONCE_AND_HOLD, () -> climber.setClimberSpeed(-1)); - } From 3cb5887a97357fc695921d23baa4e98c17face68 Mon Sep 17 00:00:00 2001 From: Max Spier <68516760+Mixmix00@users.noreply.github.com> Date: Mon, 21 Jul 2025 20:30:42 -0700 Subject: [PATCH 03/21] Remove climber controls and update auto procedure Eliminated climber-related rules and references from OI.java. Renamed AutoAlign.java to AutoScore.java and expanded its functionality to include wrist, shoulder, and elevator mechanisms for scoring, updating constructor and run logic accordingly. --- .../java/com/team766/robot/copy_2910/OI.java | 17 ----------- .../{AutoAlign.java => AutoScore.java} | 28 ++++++++++++++----- 2 files changed, 21 insertions(+), 24 deletions(-) rename src/main/java/com/team766/robot/copy_2910/procedures/{AutoAlign.java => AutoScore.java} (75%) diff --git a/src/main/java/com/team766/robot/copy_2910/OI.java b/src/main/java/com/team766/robot/copy_2910/OI.java index 03b2a7bd..f522ecbb 100644 --- a/src/main/java/com/team766/robot/copy_2910/OI.java +++ b/src/main/java/com/team766/robot/copy_2910/OI.java @@ -19,7 +19,6 @@ public OI( Wrist wrist, Elevator elevator, Shoulder shoulder, - Climber climber, Vision vision) { final JoystickReader leftJoystick = @@ -39,22 +38,6 @@ public OI( ONCE_AND_HOLD, () -> new OuttakeCoral()); - addRule("Climb", - boxopGamepad.whenButton(1), - climber, - ONCE_AND_HOLD, - () -> climber.setClimberSpeed(1)); - addRule("Stop Climb", - boxopGamepad.whenButton(2), - climber, - ONCE, - () -> climber.setClimberSpeed(0)); - addRule("Remove Climber", - boxopGamepad.whenButton(3), - climber, - ONCE_AND_HOLD, - () -> climber.setClimberSpeed(-1)); - } 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 75% 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 7b33dfd6..80fe0e62 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,45 @@ 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(); From a977a3bd94374142d67a44487ffbbd50dc711b2b Mon Sep 17 00:00:00 2001 From: Max Spier <68516760+Mixmix00@users.noreply.github.com> Date: Mon, 21 Jul 2025 20:35:59 -0700 Subject: [PATCH 04/21] Add logging for intake sensor distances Introduced a log statement to output left, right, front center, and back center sensor distances in the Intake mechanism for improved debugging and monitoring. --- .../java/com/team766/robot/copy_2910/mechanisms/Intake.java | 4 ++++ 1 file changed, 4 insertions(+) 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 8a2a2a37..2581303f 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 @@ -136,6 +136,10 @@ 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); } From c5b23a41231ba6e38868c763e8dd41d69361a26c Mon Sep 17 00:00:00 2001 From: Max Spier <68516760+Mixmix00@users.noreply.github.com> Date: Tue, 22 Jul 2025 15:47:12 -0700 Subject: [PATCH 05/21] Add queued control and scoring automation to OI Introduces a QueuedControl class in OI to manage preset positions for wrist, shoulder, and elevator. Adds rules for prepping and applying these positions via gamepad buttons, and integrates them into new AutoScore procedures for automated scoring actions. Also improves code formatting in Intake and AutoScore for readability. --- .../java/com/team766/robot/copy_2910/OI.java | 200 ++++++++++++++++-- .../robot/copy_2910/mechanisms/Intake.java | 13 +- .../robot/copy_2910/procedures/AutoScore.java | 11 +- 3 files changed, 196 insertions(+), 28 deletions(-) diff --git a/src/main/java/com/team766/robot/copy_2910/OI.java b/src/main/java/com/team766/robot/copy_2910/OI.java index f522ecbb..6919a005 100644 --- a/src/main/java/com/team766/robot/copy_2910/OI.java +++ b/src/main/java/com/team766/robot/copy_2910/OI.java @@ -2,43 +2,199 @@ import static com.team766.framework.RulePersistence.ONCE; import static com.team766.framework.RulePersistence.ONCE_AND_HOLD; + import com.team766.framework.RuleEngine; import com.team766.hal.JoystickReader; import com.team766.hal.RobotProvider; import com.team766.robot.common.mechanisms.SwerveDrive; import com.team766.robot.copy_2910.mechanisms.*; +import com.team766.robot.copy_2910.mechanisms.Elevator.ElevatorPosition; +import com.team766.robot.copy_2910.mechanisms.Shoulder.ShoulderPosition; +import com.team766.robot.copy_2910.mechanisms.Wrist.WristPosition; +import com.team766.robot.copy_2910.procedures.AutoScore; import com.team766.robot.copy_2910.procedures.IntakeCoral; import com.team766.robot.copy_2910.procedures.OuttakeCoral; - +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, - Vision vision) { - - final JoystickReader leftJoystick = - RobotProvider.instance.getJoystick(1); - final JoystickReader rightJoystick = - RobotProvider.instance.getJoystick(2); - final JoystickReader boxopGamepad = - RobotProvider.instance.getJoystick(3); - - addRule("Intake Coral", - leftJoystick.whenButton(1), - ONCE_AND_HOLD, - () -> new IntakeCoral()); + SwerveDrive swerveDrive, + Intake intake, + Wrist wrist, + Elevator elevator, + Shoulder shoulder, + Vision vision) { + + final JoystickReader leftJoystick = RobotProvider.instance.getJoystick(1); + final JoystickReader rightJoystick = RobotProvider.instance.getJoystick(2); + final JoystickReader boxopGamepad = RobotProvider.instance.getJoystick(3); + + QueuedControl queuedControl = new QueuedControl(); + queuedControl.wristPosition = WristPosition.L3; + queuedControl.shoulderPosition = ShoulderPosition.L3; + queuedControl.elevatorPosition = ElevatorPosition.L3; - addRule("Outtake Coral", + addRule("Intake Coral", leftJoystick.whenButton(1), ONCE_AND_HOLD, () -> new IntakeCoral()); + addRule( + "Outtake Coral", leftJoystick.whenButton(2), ONCE_AND_HOLD, () -> new OuttakeCoral()); - + addRule( + "Apply queued positions", + leftJoystick.whenButton(3), + 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", + boxopGamepad.whenButton(1), + ONCE, + Set.of(wrist, shoulder, elevator), + () -> { + queuedControl.wristPosition = WristPosition.L1; + queuedControl.shoulderPosition = ShoulderPosition.L1; + queuedControl.elevatorPosition = ElevatorPosition.L1; + }); + addRule( + "Prep L2 Coral", + boxopGamepad.whenButton(2), + ONCE, + Set.of(wrist, shoulder, elevator), + () -> { + queuedControl.wristPosition = WristPosition.L2; + queuedControl.shoulderPosition = ShoulderPosition.L2; + queuedControl.elevatorPosition = ElevatorPosition.L2; + }); + addRule( + "Prep L3 Coral", + boxopGamepad.whenButton(3), + ONCE, + Set.of(wrist, shoulder, elevator), + () -> { + queuedControl.wristPosition = WristPosition.L3; + queuedControl.shoulderPosition = ShoulderPosition.L3; + queuedControl.elevatorPosition = ElevatorPosition.L3; + }); + addRule( + "Prep L4 Coral", + boxopGamepad.whenButton(4), + ONCE, + Set.of(wrist, shoulder, elevator), + () -> { + queuedControl.wristPosition = WristPosition.L4; + queuedControl.shoulderPosition = ShoulderPosition.L4; + queuedControl.elevatorPosition = ElevatorPosition.L4; + }); + addRule( + "Prep Algae High", + boxopGamepad.whenButton(5), + 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", + boxopGamepad.whenButton(6), + ONCE, + Set.of(wrist, shoulder, elevator), + () -> { + queuedControl.wristPosition = WristPosition.ALGAE_LOW; + queuedControl.shoulderPosition = ShoulderPosition.ALGAE_LOW; + queuedControl.elevatorPosition = ElevatorPosition.ALGAE_LOW; + }); + addRule( + "Prep Coral Ground", + boxopGamepad.whenButton(7), + 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/mechanisms/Intake.java b/src/main/java/com/team766/robot/copy_2910/mechanisms/Intake.java index 2581303f..e897ca20 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 @@ -136,10 +136,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); + 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/procedures/AutoScore.java b/src/main/java/com/team766/robot/copy_2910/procedures/AutoScore.java index 80fe0e62..0c3681b3 100644 --- a/src/main/java/com/team766/robot/copy_2910/procedures/AutoScore.java +++ b/src/main/java/com/team766/robot/copy_2910/procedures/AutoScore.java @@ -24,7 +24,15 @@ public class AutoScore extends Procedure { private double wristAngle; private double shoulderAngle; - public AutoScore(Pose2d targetPosition, SwerveDrive drive, Wrist wrist, Shoulder shoulder, Elevator elevator, double elevatorHeight, double wristAngle, 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); @@ -38,7 +46,6 @@ public AutoScore(Pose2d targetPosition, SwerveDrive drive, Wrist wrist, Shoulder pidControllerRotation = PIDController.loadFromConfig("DRIVE_ROTATION_PID"); } - public void run(Context context) { shoulder.setSetpoint(shoulderAngle); wrist.setSetpoint(wristAngle); From 242a4f8744cbab41ac4f67e4bbfbf7b1bce2b9c8 Mon Sep 17 00:00:00 2001 From: Max Spier <68516760+maxspier@users.noreply.github.com> Date: Tue, 22 Jul 2025 23:03:52 -0500 Subject: [PATCH 06/21] Refactor mechanism constructors and update control logic Refactored IntakeCoral and OuttakeCoral procedures to require Intake as a constructor argument, updating all call sites accordingly. Fixed joystick port assignments and button mappings in OI. Adjusted Elevator, Shoulder, and Wrist mechanisms to remove unused feedforward gain code and correct motor inversion/follow logic. Added new status accessors to Intake and improved logging. Updated autonomous mode and lights initialization in Robot. --- .../team766/framework/MechanismsAspect.java | 2 +- .../java/com/team766/robot/copy_2910/OI.java | 10 ++++---- .../com/team766/robot/copy_2910/Robot.java | 23 +++++++++++++++---- .../robot/copy_2910/mechanisms/Elevator.java | 19 +++++++-------- .../robot/copy_2910/mechanisms/Intake.java | 18 +++++++++++---- .../robot/copy_2910/mechanisms/Shoulder.java | 11 +++++---- .../robot/copy_2910/mechanisms/Wrist.java | 12 +++++----- .../copy_2910/procedures/IntakeCoral.java | 7 ++++-- .../copy_2910/procedures/OuttakeCoral.java | 4 ++-- 9 files changed, 67 insertions(+), 39 deletions(-) diff --git a/src/main/java/com/team766/framework/MechanismsAspect.java b/src/main/java/com/team766/framework/MechanismsAspect.java index 8b9d653e..ed6a089f 100644 --- a/src/main/java/com/team766/framework/MechanismsAspect.java +++ b/src/main/java/com/team766/framework/MechanismsAspect.java @@ -27,7 +27,7 @@ public class MechanismsAspect { // not also methods of the Mechanism base class, or any other base class contained in the // framework package. Also ignore any methods that are annotated with NoReservationRequired. @Before( - "execution(public * com.team766.framework.Reservable+.*(..))" + "execution(public !static * com.team766.framework.Reservable+.*(..))" + " && !within(com.team766.framework.*)" + " && !@annotation(com.team766.framework.NoReservationRequired)") public void mechanismCheckedPublicMethods(JoinPoint joinPoint) { diff --git a/src/main/java/com/team766/robot/copy_2910/OI.java b/src/main/java/com/team766/robot/copy_2910/OI.java index 6919a005..e5a64142 100644 --- a/src/main/java/com/team766/robot/copy_2910/OI.java +++ b/src/main/java/com/team766/robot/copy_2910/OI.java @@ -32,7 +32,7 @@ public OI( Shoulder shoulder, Vision vision) { - final JoystickReader leftJoystick = RobotProvider.instance.getJoystick(1); + final JoystickReader leftJoystick = RobotProvider.instance.getJoystick(0); final JoystickReader rightJoystick = RobotProvider.instance.getJoystick(2); final JoystickReader boxopGamepad = RobotProvider.instance.getJoystick(3); @@ -41,16 +41,16 @@ public OI( queuedControl.shoulderPosition = ShoulderPosition.L3; queuedControl.elevatorPosition = ElevatorPosition.L3; - addRule("Intake Coral", leftJoystick.whenButton(1), ONCE_AND_HOLD, () -> new IntakeCoral()); + addRule("Intake Coral", leftJoystick.whenButton(1), ONCE_AND_HOLD, () -> new IntakeCoral(intake)); addRule( "Outtake Coral", leftJoystick.whenButton(2), ONCE_AND_HOLD, - () -> new OuttakeCoral()); + () -> new OuttakeCoral(intake)); addRule( "Apply queued positions", - leftJoystick.whenButton(3), + leftJoystick.whenButton(4), ONCE_AND_HOLD, Set.of(wrist, shoulder, elevator), () -> { @@ -121,7 +121,7 @@ public OI( }); addRule( "Prep Coral Ground", - boxopGamepad.whenButton(7), + leftJoystick.whenButton(3), ONCE, Set.of(wrist, shoulder, elevator), () -> { 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 0e7d4e36..08bf7ff1 100644 --- a/src/main/java/com/team766/robot/copy_2910/Robot.java +++ b/src/main/java/com/team766/robot/copy_2910/Robot.java @@ -5,35 +5,50 @@ import com.team766.hal.RobotConfigurator; import com.team766.robot.common.SwerveConfig; import com.team766.robot.common.mechanisms.SwerveDrive; +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.OuttakeCoral; +import com.team766.robot.gatorade.Lights; public class Robot implements RobotConfigurator { private SwerveDrive drive; private Intake intake; + private Elevator elevator; + private Shoulder shoulder; + private Vision vision; + private Wrist wrist; @Override public void initializeMechanisms() { SwerveConfig swerveConfig = new SwerveConfig(); drive = new SwerveDrive(swerveConfig); intake = new Intake(); + elevator = new Elevator(); + shoulder = new Shoulder(); + vision = new Vision(); + wrist = new Wrist(); } @Override public RuleEngine createOI() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'createOI'"); + return new OI(drive, intake, wrist, elevator, shoulder, 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("uh", () -> new OuttakeCoral(intake))}; + //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 4e7a2573..8cadf816 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 @@ -19,7 +19,7 @@ public class Elevator extends MechanismWithStatus { private static double THRESHOLD = 0.05; // 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) @@ -41,16 +41,17 @@ public Elevator() { // elevatorMotorRight.setInverted(false); // Set to true if the right motor needs to be // inverted - elevatorMotorLeft.follow(elevatorMotorRight); + elevatorMotorRight.follow(elevatorMotorLeft); setPoint = ElevatorPosition.READY.getPosition(); // Default position - elevatorMotorRight.setCurrentLimit( + elevatorMotorLeft.setCurrentLimit( 30); // Set current limit for the elevator motor | TODO: Replace with actual value - ffGain = - ConfigFileReader.instance.getDouble( - "ElevatorFFGain"); // Replace with actual config key + //ffGain = + // ConfigFileReader.instance.getDouble( + // "ElevatorFFGain"); // Replace with actual config key - elevatorMotorRight.setSensorPosition( + elevatorMotorLeft.setSensorPosition( 0.0); // Elevator always has to start at same 0.0 position + elevatorMotorLeft.setInverted(true); } public enum ElevatorPosition { @@ -98,10 +99,10 @@ public void nudgeDown() { } public void run() { - elevatorMotorRight.set(MotorController.ControlMode.Position, setPoint, ffGain.valueOr(0.0)); + elevatorMotorLeft.set(MotorController.ControlMode.Position, setPoint); } 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 e897ca20..fc1b09d5 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,7 +16,7 @@ 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.12; // TODO: Set this to a real value private double leftPower = 0.25; private double rightPower = 0.25; @@ -46,6 +46,14 @@ 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; } @@ -88,19 +96,19 @@ 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() { 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 a8667e1b..0a65d28d 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 @@ -17,7 +17,7 @@ public class Shoulder extends MechanismWithStatus { 0.5; // Threshold for determining if the shoulder is near a position | TODO: Adjust this // 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 @@ -65,11 +65,12 @@ public Shoulder() { RobotProvider.instance.getMotor( "RightShoulderMotor"); // Replace with actual motor name + rightMotor.setInverted(true); rightMotor.follow(leftMotor); - ffGain = - ConfigFileReader.instance.getDouble( - "ShoulderFFGain"); // Replace with actual config key + //ffGain = + // ConfigFileReader.instance.getDouble( + // "ShoulderFFGain"); // Replace with actual config key setPoint = ShoulderPosition.L1.getPosition(); // Default position } @@ -82,7 +83,7 @@ public void setSetpoint(double setpoint) { } public void run() { - leftMotor.set(MotorController.ControlMode.Position, setPoint, ffGain.get()); + leftMotor.set(MotorController.ControlMode.Position, setPoint); } public void nudgeUp() { 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 d6abcf80..0968269c 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 @@ -16,7 +16,7 @@ public class Wrist extends MechanismWithStatus { 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 @@ -40,7 +40,7 @@ public enum WristPosition { L4(-5.5), ALGAE_HIGH(-25.928), ALGAE_LOW(-25.928), - CORAL_GROUND(-15.81), + CORAL_GROUND(-12.81), ALGAE_GROUND(-21.786), MAXIMUM(10), MINIMUM(-30); @@ -59,9 +59,9 @@ 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 + //ffGain = + // ConfigFileReader.instance.getDouble( + // "WristFFGain"); // Replace with actual config key setPoint = WristPosition.L3.getPosition(); // Default position } @@ -74,7 +74,7 @@ public void setSetpoint(double setpoint) { } public void run() { - motor.set(MotorController.ControlMode.Position, setPoint, ffGain.get()); + motor.set(MotorController.ControlMode.Position, setPoint); } public void nudgeUp() { 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 9cbf8b3a..f0cc192b 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 @@ -9,8 +9,8 @@ public class IntakeCoral extends Procedure { private Intake intake; - public IntakeCoral() { - intake = reserve(intake); + public IntakeCoral(Intake intake2) { + intake = reserve(intake2); } @Override @@ -20,6 +20,9 @@ public void run(Context context) { 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(); 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 990735b4..a448b111 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,8 +8,8 @@ public class OuttakeCoral extends Procedure { private Intake intake; - public OuttakeCoral() { - intake = reserve(intake); + public OuttakeCoral(Intake intake2) { + intake = reserve(intake2); } @Override From dd840bdcc2db3a406ff8bd7514b0b0b9d41e651b Mon Sep 17 00:00:00 2001 From: Max Spier <68516760+Mixmix00@users.noreply.github.com> Date: Wed, 23 Jul 2025 10:38:32 -0700 Subject: [PATCH 07/21] Refactor formatting and remove unused imports This commit improves code readability by reformatting method calls and comments, and removes unused imports from Elevator, Shoulder, and Wrist mechanism classes. No functional changes were made. --- src/main/java/com/team766/robot/copy_2910/OI.java | 6 +++++- src/main/java/com/team766/robot/copy_2910/Robot.java | 6 +++--- .../team766/robot/copy_2910/mechanisms/Elevator.java | 10 ++++------ .../com/team766/robot/copy_2910/mechanisms/Intake.java | 5 +++-- .../team766/robot/copy_2910/mechanisms/Shoulder.java | 10 ++++------ .../com/team766/robot/copy_2910/mechanisms/Wrist.java | 6 ++---- .../robot/copy_2910/procedures/IntakeCoral.java | 4 +++- 7 files changed, 24 insertions(+), 23 deletions(-) diff --git a/src/main/java/com/team766/robot/copy_2910/OI.java b/src/main/java/com/team766/robot/copy_2910/OI.java index e5a64142..11a7892b 100644 --- a/src/main/java/com/team766/robot/copy_2910/OI.java +++ b/src/main/java/com/team766/robot/copy_2910/OI.java @@ -41,7 +41,11 @@ public OI( queuedControl.shoulderPosition = ShoulderPosition.L3; queuedControl.elevatorPosition = ElevatorPosition.L3; - addRule("Intake Coral", leftJoystick.whenButton(1), ONCE_AND_HOLD, () -> new IntakeCoral(intake)); + addRule( + "Intake Coral", + leftJoystick.whenButton(1), + ONCE_AND_HOLD, + () -> new IntakeCoral(intake)); addRule( "Outtake Coral", leftJoystick.whenButton(2), 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 08bf7ff1..af433726 100644 --- a/src/main/java/com/team766/robot/copy_2910/Robot.java +++ b/src/main/java/com/team766/robot/copy_2910/Robot.java @@ -41,14 +41,14 @@ public RuleEngine createOI() { @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 - return new AutonomousMode[] { new AutonomousMode("uh", () -> new OuttakeCoral(intake))}; - //throw new UnsupportedOperationException("Unimplemented method 'getAutonomousModes'"); + return new AutonomousMode[] {new AutonomousMode("uh", () -> new OuttakeCoral(intake))}; + // 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 8cadf816..742ff029 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 { @@ -19,7 +17,7 @@ public class Elevator extends MechanismWithStatus { private static double THRESHOLD = 0.05; // 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) @@ -45,9 +43,9 @@ public Elevator() { setPoint = ElevatorPosition.READY.getPosition(); // Default position elevatorMotorLeft.setCurrentLimit( 30); // Set current limit for the elevator motor | TODO: Replace with actual value - //ffGain = - // ConfigFileReader.instance.getDouble( - // "ElevatorFFGain"); // Replace with actual config key + // ffGain = + // ConfigFileReader.instance.getDouble( + // "ElevatorFFGain"); // Replace with actual config key elevatorMotorLeft.setSensorPosition( 0.0); // Elevator always has to start at same 0.0 position 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 fc1b09d5..d3c49904 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 @@ -47,13 +47,14 @@ 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(){ + public double getLeftDistance() { return leftDistance; } - public double getBackCenterDistance(){ + public double getBackCenterDistance() { return backCenterDistance; } + public boolean hasCoralInLeft() { return leftDistance < CORAL_THRESHOLD; } 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 0a65d28d..c2f17aab 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,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 edu.wpi.first.math.MathUtil; public class Shoulder extends MechanismWithStatus { @@ -17,7 +15,7 @@ public class Shoulder extends MechanismWithStatus { 0.5; // Threshold for determining if the shoulder is near a position | TODO: Adjust this // 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 @@ -68,9 +66,9 @@ public Shoulder() { rightMotor.setInverted(true); rightMotor.follow(leftMotor); - //ffGain = - // ConfigFileReader.instance.getDouble( - // "ShoulderFFGain"); // Replace with actual config key + // ffGain = + // ConfigFileReader.instance.getDouble( + // "ShoulderFFGain"); // Replace with actual config key setPoint = ShoulderPosition.L1.getPosition(); // Default position } 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 0968269c..3f65cb94 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 @@ -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 edu.wpi.first.math.MathUtil; public class Wrist extends MechanismWithStatus { @@ -16,7 +14,7 @@ public class Wrist extends MechanismWithStatus { 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 @@ -59,7 +57,7 @@ public double getPosition() { public Wrist() { motor = RobotProvider.instance.getMotor("WristMotor"); // Replace with actual motor name - //ffGain = + // ffGain = // ConfigFileReader.instance.getDouble( // "WristFFGain"); // Replace with actual config key setPoint = WristPosition.L3.getPosition(); // Default position 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 f0cc192b..19736700 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 @@ -22,7 +22,9 @@ public void run(Context context) { } log( "Intake Status: leftDistance = " - + status.get().getLeftDistance() + "Back center:" + status.get().getBackCenterDistance()); + + status.get().getLeftDistance() + + "Back center:" + + status.get().getBackCenterDistance()); while (!status.get().hasCoralInBackCenter()) { context.yield(); intake.turnAlgaePositive(); From 721cac664306220c58b7251714f292d55018c24d Mon Sep 17 00:00:00 2001 From: Max Spier <68516760+maxspier@users.noreply.github.com> Date: Thu, 24 Jul 2025 00:28:00 -0500 Subject: [PATCH 08/21] new --- .../java/com/team766/robot/copy_2910/OI.java | 20 +++++++++++------- .../com/team766/robot/copy_2910/Robot.java | 3 +-- .../robot/copy_2910/mechanisms/Elevator.java | 16 +++++++++++--- .../robot/copy_2910/mechanisms/Intake.java | 6 +++--- .../robot/copy_2910/mechanisms/Shoulder.java | 21 ++++++++++++++----- .../robot/copy_2910/mechanisms/Wrist.java | 2 +- .../robot/gatorade/mechanisms/Shoulder.java | 2 +- 7 files changed, 48 insertions(+), 22 deletions(-) diff --git a/src/main/java/com/team766/robot/copy_2910/OI.java b/src/main/java/com/team766/robot/copy_2910/OI.java index 11a7892b..280d7e5d 100644 --- a/src/main/java/com/team766/robot/copy_2910/OI.java +++ b/src/main/java/com/team766/robot/copy_2910/OI.java @@ -30,7 +30,8 @@ public OI( Wrist wrist, Elevator elevator, Shoulder shoulder, - Vision vision) { + Vision vision, + Climber climber) { final JoystickReader leftJoystick = RobotProvider.instance.getJoystick(0); final JoystickReader rightJoystick = RobotProvider.instance.getJoystick(2); @@ -65,7 +66,7 @@ public OI( addRule( "Prep L1 Coral", - boxopGamepad.whenButton(1), + leftJoystick.whenButton(5), ONCE, Set.of(wrist, shoulder, elevator), () -> { @@ -75,7 +76,7 @@ public OI( }); addRule( "Prep L2 Coral", - boxopGamepad.whenButton(2), + leftJoystick.whenButton(6), ONCE, Set.of(wrist, shoulder, elevator), () -> { @@ -85,7 +86,7 @@ public OI( }); addRule( "Prep L3 Coral", - boxopGamepad.whenButton(3), + leftJoystick.whenButton(7), ONCE, Set.of(wrist, shoulder, elevator), () -> { @@ -95,7 +96,7 @@ public OI( }); addRule( "Prep L4 Coral", - boxopGamepad.whenButton(4), + leftJoystick.whenButton(8), ONCE, Set.of(wrist, shoulder, elevator), () -> { @@ -105,7 +106,7 @@ public OI( }); addRule( "Prep Algae High", - boxopGamepad.whenButton(5), + leftJoystick.whenButton(9), ONCE, Set.of(wrist, shoulder, elevator), () -> { @@ -115,7 +116,7 @@ public OI( }); addRule( "Prep Algae Low", - boxopGamepad.whenButton(6), + leftJoystick.whenButton(10), ONCE, Set.of(wrist, shoulder, elevator), () -> { @@ -123,6 +124,11 @@ public OI( queuedControl.shoulderPosition = ShoulderPosition.ALGAE_LOW; queuedControl.elevatorPosition = ElevatorPosition.ALGAE_LOW; }); + addRule("Algae", + leftJoystick.whenButton(11), + ONCE_AND_HOLD, + intake, + () -> {intake.turnAlgaeNegative();}); addRule( "Prep Coral Ground", leftJoystick.whenButton(3), 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 f8d7198d..18335741 100644 --- a/src/main/java/com/team766/robot/copy_2910/Robot.java +++ b/src/main/java/com/team766/robot/copy_2910/Robot.java @@ -3,7 +3,6 @@ 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.Elevator; @@ -39,7 +38,7 @@ public void initializeMechanisms() { @Override public RuleEngine createOI() { - return new OI(drive, intake, wrist, elevator, shoulder, vision); + return new OI(drive, intake, wrist, elevator, shoulder, vision, climber); } @Override 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 742ff029..cc19d35d 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,5 +1,9 @@ package com.team766.robot.copy_2910.mechanisms; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkMaxConfig; import com.team766.framework.MechanismWithStatus; import com.team766.framework.Status; import com.team766.hal.MotorController; @@ -42,14 +46,19 @@ public Elevator() { elevatorMotorRight.follow(elevatorMotorLeft); setPoint = ElevatorPosition.READY.getPosition(); // Default position elevatorMotorLeft.setCurrentLimit( - 30); // Set current limit for the elevator motor | TODO: Replace with actual value + 35); // Set current limit for the elevator motor | TODO: Replace with actual value + elevatorMotorLeft.setInverted(false); + SparkMaxConfig rightConfig = new SparkMaxConfig(); + rightConfig.follow((SparkMax)elevatorMotorLeft, true /* invert */); + ((SparkMax)elevatorMotorRight).configure( + rightConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); // ffGain = // ConfigFileReader.instance.getDouble( // "ElevatorFFGain"); // Replace with actual config key elevatorMotorLeft.setSensorPosition( 0.0); // Elevator always has to start at same 0.0 position - elevatorMotorLeft.setInverted(true); + //elevatorMotorRight.setInverted(true); } public enum ElevatorPosition { @@ -57,7 +66,7 @@ public enum ElevatorPosition { L1(0.881), L2(-4.452), L3(-11.5), - L4(-21.357), + L4(-23), //-21.357 ALGAE_HIGH(-9.357), ALGAE_LOW(-3.262), CORAL_GROUND(-0.357), @@ -98,6 +107,7 @@ public void nudgeDown() { public void run() { elevatorMotorLeft.set(MotorController.ControlMode.Position, setPoint); + log("SHOULDER Setpoint: " + setPoint + " Pos: " + elevatorMotorLeft.getSensorPosition()); } protected ElevatorStatus updateStatus() { 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 d3c49904..523d92b4 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 @@ -18,10 +18,10 @@ public class Intake extends MechanismWithStatus { private static final double CORAL_THRESHOLD = 0.12; // TODO: Set this to a real value - private double leftPower = 0.25; - private double rightPower = 0.25; + private double leftPower = 0.125; + private double rightPower = 1; - private double algaePower = 0.35; + private double algaePower = 1; public Intake() { leftCANRange = RobotProvider.instance.getTimeOfFlight("INTAKE.CANRange.left"); 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 c2f17aab..8dddf7aa 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,5 +1,9 @@ package com.team766.robot.copy_2910.mechanisms; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkMaxConfig; import com.team766.framework.MechanismWithStatus; import com.team766.framework.Status; import com.team766.hal.MotorController; @@ -18,7 +22,7 @@ public class Shoulder extends MechanismWithStatus { // private ValueProvider ffGain; private final double NUDGE_AMOUNT = - 5; // Amount to nudge up/down | TODO: Adjust this value based on the shoulder's + 0.1; // Amount to nudge up/down | TODO: Adjust this value based on the shoulder's // characteristics @@ -34,9 +38,9 @@ public boolean isNearTo(double angle) { public enum ShoulderPosition { L1(12.119), - L2(9.643), - L3(15.071), - L4(21.5), + L2(11.5), + L3(17), + L4(22.5), ALGAE_HIGH(22.952), ALGAE_LOW(20.405), CORAL_GROUND(0.071), @@ -63,9 +67,14 @@ public Shoulder() { RobotProvider.instance.getMotor( "RightShoulderMotor"); // Replace with actual motor name - rightMotor.setInverted(true); + leftMotor.setInverted(true); rightMotor.follow(leftMotor); + SparkMaxConfig rightConfig = new SparkMaxConfig(); + rightConfig.follow((SparkMax)leftMotor, true /* invert */); + ((SparkMax)rightMotor).configure( + rightConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + // ffGain = // ConfigFileReader.instance.getDouble( // "ShoulderFFGain"); // Replace with actual config key @@ -81,7 +90,9 @@ public void setSetpoint(double setpoint) { } public void run() { + //leftMotor.set(.Position, setPoint) leftMotor.set(MotorController.ControlMode.Position, setPoint); + log("SHOULDER Setpoint: " + setPoint + " Pos: " + leftMotor.getSensorPosition()); } public void nudgeUp() { 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 3f65cb94..f7c8a27c 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 @@ -38,7 +38,7 @@ public enum WristPosition { L4(-5.5), ALGAE_HIGH(-25.928), ALGAE_LOW(-25.928), - CORAL_GROUND(-12.81), + CORAL_GROUND(-15.2), ALGAE_GROUND(-21.786), MAXIMUM(10), MINIMUM(-30); 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 f82b1532..f04c9c80 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); From d8cc1f5c73994f007a4139bb76cb59e273ef42ec Mon Sep 17 00:00:00 2001 From: Max Spier <68516760+Mixmix00@users.noreply.github.com> Date: Wed, 23 Jul 2025 22:35:54 -0700 Subject: [PATCH 09/21] Add shoulder nudge controls and adjust nudge amount Added rules to OI for nudging the shoulder up and down using joystick buttons 12 and 13. Increased the shoulder nudge amount from 0.1 to 0.5 in the Shoulder mechanism. --- src/main/java/com/team766/robot/copy_2910/OI.java | 13 ++++++++++++- .../robot/copy_2910/mechanisms/Shoulder.java | 2 +- 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/team766/robot/copy_2910/OI.java b/src/main/java/com/team766/robot/copy_2910/OI.java index 280d7e5d..89917a8d 100644 --- a/src/main/java/com/team766/robot/copy_2910/OI.java +++ b/src/main/java/com/team766/robot/copy_2910/OI.java @@ -124,11 +124,22 @@ public OI( queuedControl.shoulderPosition = ShoulderPosition.ALGAE_LOW; queuedControl.elevatorPosition = ElevatorPosition.ALGAE_LOW; }); - addRule("Algae", + 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), 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 8dddf7aa..2ee42d38 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 @@ -22,7 +22,7 @@ public class Shoulder extends MechanismWithStatus { // private ValueProvider ffGain; private final double NUDGE_AMOUNT = - 0.1; // 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 From 38ca1fda1ff828f46f3bb292545cfd8779d869a7 Mon Sep 17 00:00:00 2001 From: Max Spier <68516760+maxspier@users.noreply.github.com> Date: Thu, 24 Jul 2025 02:05:20 -0500 Subject: [PATCH 10/21] Update mechanism positions and refine IntakeCoral logic Adjusted preset positions for Elevator, Shoulder, and Wrist mechanisms to new values for improved operation. Modified IntakeCoral procedure to explicitly run intake motors for a short duration before stopping, ensuring reliable coral intake. --- .../com/team766/robot/copy_2910/mechanisms/Elevator.java | 4 ++-- .../com/team766/robot/copy_2910/mechanisms/Shoulder.java | 2 +- .../java/com/team766/robot/copy_2910/mechanisms/Wrist.java | 2 +- .../com/team766/robot/copy_2910/procedures/IntakeCoral.java | 5 ++++- 4 files changed, 8 insertions(+), 5 deletions(-) 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 cc19d35d..d097da29 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 @@ -66,7 +66,7 @@ public enum ElevatorPosition { L1(0.881), L2(-4.452), L3(-11.5), - L4(-23), //-21.357 + L4(-29), //-21.357 ALGAE_HIGH(-9.357), ALGAE_LOW(-3.262), CORAL_GROUND(-0.357), @@ -75,7 +75,7 @@ public enum ElevatorPosition { // can see the tag MAXIMUM(2), // 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(-30); // Minimum height of the elevator, TODO: Adjust based on the actual elevator's // minimum position final double position; 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 2ee42d38..e572e320 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 @@ -40,7 +40,7 @@ public enum ShoulderPosition { L1(12.119), L2(11.5), L3(17), - L4(22.5), + L4(23), ALGAE_HIGH(22.952), ALGAE_LOW(20.405), CORAL_GROUND(0.071), 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 f7c8a27c..cd09f3b9 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 @@ -35,7 +35,7 @@ public enum WristPosition { L1(-16.643), L2(0.643), L3(-1.667), - L4(-5.5), + L4(-7), ALGAE_HIGH(-25.928), ALGAE_LOW(-25.928), CORAL_GROUND(-15.2), 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 19736700..d002e42d 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 @@ -69,7 +69,7 @@ public void run(Context context) { /* * 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. @@ -81,6 +81,9 @@ public void run(Context context) { } } // Once the coral is in the back center, we stop the intake motors. + intake.turnLeftPositive(); + intake.turnRightNegative(); + context.waitForSeconds(0.05); intake.stop(); } } From 365508fb594aecdab2eeb3b6b3ca3e5a0c27d432 Mon Sep 17 00:00:00 2001 From: Max Spier <68516760+maxspier@users.noreply.github.com> Date: Fri, 25 Jul 2025 04:13:24 -0500 Subject: [PATCH 11/21] thrusday crunch --- .../com/team766/robot/copy_2910/BoxOpOI.java | 216 +++++++++++ .../robot/copy_2910/InputConstants.java | 51 +++ .../java/com/team766/robot/copy_2910/OI.java | 353 +++++++++--------- .../com/team766/robot/copy_2910/Robot.java | 5 +- .../robot/copy_2910/mechanisms/Elevator.java | 26 +- .../robot/copy_2910/mechanisms/Shoulder.java | 19 +- .../robot/copy_2910/mechanisms/Wrist.java | 15 +- .../robot/copy_2910/procedures/CenterL1.java | 28 ++ .../copy_2910/procedures/IntakeCoral.java | 24 +- .../copy_2910/procedures/MoveWristvator.java | 66 ++++ .../copy_2910/procedures/OuttakeCoral.java | 5 +- 11 files changed, 618 insertions(+), 190 deletions(-) create mode 100644 src/main/java/com/team766/robot/copy_2910/BoxOpOI.java create mode 100644 src/main/java/com/team766/robot/copy_2910/InputConstants.java create mode 100644 src/main/java/com/team766/robot/copy_2910/procedures/CenterL1.java create mode 100644 src/main/java/com/team766/robot/copy_2910/procedures/MoveWristvator.java 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 00000000..8ab5733e --- /dev/null +++ b/src/main/java/com/team766/robot/copy_2910/BoxOpOI.java @@ -0,0 +1,216 @@ +package com.team766.robot.copy_2910; + +import static com.team766.framework.RulePersistence.*; + +import com.team766.framework.Conditions; +import com.team766.framework.Conditions.LogicalAnd; +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.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.Wrist; +import com.team766.robot.copy_2910.mechanisms.Elevator.ElevatorPosition; +import com.team766.robot.copy_2910.mechanisms.Shoulder.ShoulderPosition; +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, + Climber climber, + 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(climber, wrist, elevator, shoulder), + () -> { + elevator.setPosition(Elevator.ElevatorPosition.MAXIMUM); + wrist.setPosition(Wrist.WristPosition.ALGAE_LOW); + climber.setClimberSpeed(0.5); + shoulder.setPosition(Shoulder.ShoulderPosition.CLIMBER); + }) + .withFinishedTriggeringProcedure( + Set.of(climber, shoulder), + context -> { + climber.stop(); + 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_LOW; + queuedControl.shoulderPosition = ShoulderPosition.ALGAE_LOW; + queuedControl.wristPosition = WristPosition.ALGAE_LOW; + }); + + 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( + "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( + "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 Elevator", + boxopGamepad.whenAxisMoved( + InputConstants.AXIS_ELEVATOR_FINETUNE), + ONCE_AND_HOLD, + elevator, + () -> { + elevator.nudge( + boxopGamepad.getAxis( + InputConstants.AXIS_ELEVATOR_FINETUNE)); + }); + + addRule( + "Nudge Wrist", + boxopGamepad.whenAxisMoved( + InputConstants.AXIS_WRIST_FINETUNE), + ONCE_AND_HOLD, + wrist, + () -> { + wrist.nudge( + boxopGamepad.getAxis( + InputConstants.AXIS_WRIST_FINETUNE)); + }); + + addRule( + "Nudge Shoulder", + boxopGamepad.whenAxisMoved( + InputConstants.AXIS_WRIST_FINETUNE), + ONCE_AND_HOLD, + wrist, + () -> { + shoulder.nudge( + boxopGamepad.getAxis( + InputConstants.AXIS_WRIST_FINETUNE)); + }); + } + }) + .withFinishedTriggeringProcedure( + Set.of(elevator, wrist, shoulder), + () -> { + elevator.setPosition(ElevatorPosition.READY); + wrist.setPosition(WristPosition.STOW); + shoulder.setPosition(ShoulderPosition.STOW); + }); + } +} 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 00000000..802e2973 --- /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 index 89917a8d..5ea8748a 100644 --- a/src/main/java/com/team766/robot/copy_2910/OI.java +++ b/src/main/java/com/team766/robot/copy_2910/OI.java @@ -6,6 +6,7 @@ import com.team766.framework.RuleEngine; import com.team766.hal.JoystickReader; import com.team766.hal.RobotProvider; +import com.team766.robot.common.DriverOI; import com.team766.robot.common.mechanisms.SwerveDrive; import com.team766.robot.copy_2910.mechanisms.*; import com.team766.robot.copy_2910.mechanisms.Elevator.ElevatorPosition; @@ -34,188 +35,198 @@ public OI( Climber climber) { final JoystickReader leftJoystick = RobotProvider.instance.getJoystick(0); - final JoystickReader rightJoystick = RobotProvider.instance.getJoystick(2); - final JoystickReader boxopGamepad = RobotProvider.instance.getJoystick(3); + final JoystickReader rightJoystick = RobotProvider.instance.getJoystick(1); + final JoystickReader boxopGamepad = RobotProvider.instance.getJoystick(2); + + leftJoystick.setAllAxisDeadzone(0.05); + rightJoystick.setAllAxisDeadzone(0.05); + boxopGamepad.setAllAxisDeadzone(0.05); QueuedControl queuedControl = new QueuedControl(); - queuedControl.wristPosition = WristPosition.L3; - queuedControl.shoulderPosition = ShoulderPosition.L3; - queuedControl.elevatorPosition = ElevatorPosition.L3; + queuedControl.wristPosition = WristPosition.STOW; + queuedControl.shoulderPosition = ShoulderPosition.STOW; + queuedControl.elevatorPosition = ElevatorPosition.READY; - addRule( - "Intake Coral", - leftJoystick.whenButton(1), - ONCE_AND_HOLD, - () -> new IntakeCoral(intake)); - addRule( - "Outtake Coral", - leftJoystick.whenButton(2), - ONCE_AND_HOLD, - () -> new OuttakeCoral(intake)); + addRules(new DriverOI(leftJoystick, rightJoystick, swerveDrive)); + addRules(new BoxOpOI(boxopGamepad, shoulder, elevator, wrist, climber, intake, queuedControl)); addRule( - "Apply queued positions", - leftJoystick.whenButton(4), + "Outtake Coral", + leftJoystick.whenButton(InputConstants.JOYSTICK_RIGHT_BUTTON), ONCE_AND_HOLD, - Set.of(wrist, shoulder, elevator), - () -> { - wrist.setSetpoint(queuedControl.wristPosition.getPosition()); - shoulder.setSetpoint(queuedControl.shoulderPosition.getPosition()); - elevator.setPosition(queuedControl.elevatorPosition.getPosition()); - }); + () -> new OuttakeCoral(intake)).withFinishedTriggeringProcedure(intake, () -> intake.stop()); 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), + "Outtake Algae", + leftJoystick.whenButton(InputConstants.JOYSTICK_LEFT_BUTTON), ONCE_AND_HOLD, intake, - () -> {intake.turnAlgaeNegative();}); + () -> intake.setAlgaePower(-0.5) + ).withFinishedTriggeringProcedure(intake, () -> intake.stop()); - 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( + // "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( - "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())); + // 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 18335741..a8d8bea3 100644 --- a/src/main/java/com/team766/robot/copy_2910/Robot.java +++ b/src/main/java/com/team766/robot/copy_2910/Robot.java @@ -11,6 +11,7 @@ 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.OuttakeCoral; import com.team766.robot.gatorade.Lights; @@ -26,7 +27,7 @@ public class Robot implements RobotConfigurator { @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(); @@ -51,7 +52,7 @@ public RuleEngine createLights() { @Override public AutonomousMode[] getAutonomousModes() { // TODO Auto-generated method stub - return new AutonomousMode[] {new AutonomousMode("uh", () -> new OuttakeCoral(intake))}; + return new AutonomousMode[] {new AutonomousMode("Center Start 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 d097da29..bda42e34 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 @@ -45,8 +45,8 @@ public Elevator() { elevatorMotorRight.follow(elevatorMotorLeft); setPoint = ElevatorPosition.READY.getPosition(); // Default position - elevatorMotorLeft.setCurrentLimit( - 35); // Set current limit for the elevator motor | TODO: Replace with actual value + elevatorMotorLeft.setCurrentLimit(40); + elevatorMotorRight.setCurrentLimit(40); // Set current limit for the elevator motor elevatorMotorLeft.setInverted(false); SparkMaxConfig rightConfig = new SparkMaxConfig(); rightConfig.follow((SparkMax)elevatorMotorLeft, true /* invert */); @@ -62,16 +62,22 @@ public Elevator() { } public enum ElevatorPosition { - INTAKE(0.0), + INTAKE(2.0), L1(0.881), L2(-4.452), - L3(-11.5), + L3(-11), L4(-29), //-21.357 ALGAE_HIGH(-9.357), ALGAE_LOW(-3.262), - CORAL_GROUND(-0.357), + CORAL_GROUND(2), ALGAE_GROUND(-1.643), - READY(-10), // Should be the default position and the ready position for vision so that it + + + + + + + READY(2), // 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 position @@ -97,6 +103,10 @@ public void setPosition(double setPosition) { ElevatorPosition.MAXIMUM.getPosition()); } + public void setPosition(ElevatorPosition setPosition) { + setPosition(setPosition.getPosition()); + } + public void nudgeUp() { setPosition(setPoint + NUDGE_AMOUNT); } @@ -105,6 +115,10 @@ public void nudgeDown() { setPosition(setPoint - NUDGE_AMOUNT); } + public void nudge(double input) { + if (input > 0) {nudgeUp();} else {nudgeDown();} + } + public void run() { elevatorMotorLeft.set(MotorController.ControlMode.Position, setPoint); log("SHOULDER Setpoint: " + setPoint + " Pos: " + elevatorMotorLeft.getSensorPosition()); 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 e572e320..83f86cd2 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 @@ -37,14 +37,16 @@ public boolean isNearTo(double angle) { } public enum ShoulderPosition { - L1(12.119), + L1(14), L2(11.5), L3(17), - L4(23), + L4(24.5), ALGAE_HIGH(22.952), ALGAE_LOW(20.405), CORAL_GROUND(0.071), ALGAE_GROUND(4.119), + CLIMBER(28), + STOW(5), MAXIMUM(40), MINIMUM(0); @@ -75,10 +77,13 @@ public Shoulder() { ((SparkMax)rightMotor).configure( rightConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + leftMotor.setCurrentLimit(40); + rightMotor.setCurrentLimit(40); + // ffGain = // ConfigFileReader.instance.getDouble( // "ShoulderFFGain"); // Replace with actual config key - setPoint = ShoulderPosition.L1.getPosition(); // Default position + // setPoint = ShoulderPosition.L1.getPosition(); // Default position } public void setSetpoint(double setpoint) { @@ -89,6 +94,10 @@ public void setSetpoint(double setpoint) { ShoulderPosition.MAXIMUM.getPosition()); } + public void setPosition(ShoulderPosition position) { + setSetpoint(position.getPosition()); + } + public void run() { //leftMotor.set(.Position, setPoint) leftMotor.set(MotorController.ControlMode.Position, setPoint); @@ -103,6 +112,10 @@ public void nudgeDown() { setSetpoint(setPoint - NUDGE_AMOUNT); } + public void nudge(double input) { + if (input > 0) {nudgeUp();} else {nudgeDown();} + } + @Override protected ShoulderStatus updateStatus() { 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 cd09f3b9..a73bb617 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 @@ -35,11 +35,12 @@ public enum WristPosition { L1(-16.643), L2(0.643), L3(-1.667), - L4(-7), + L4(-9), ALGAE_HIGH(-25.928), ALGAE_LOW(-25.928), - CORAL_GROUND(-15.2), + CORAL_GROUND(-16.5), ALGAE_GROUND(-21.786), + STOW(-5), MAXIMUM(10), MINIMUM(-30); @@ -60,7 +61,7 @@ public Wrist() { // ffGain = // ConfigFileReader.instance.getDouble( // "WristFFGain"); // Replace with actual config key - setPoint = WristPosition.L3.getPosition(); // Default position + // setPoint = WristPosition.L3.getPosition(); // Default position } public void setSetpoint(double setpoint) { @@ -71,6 +72,10 @@ 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); } @@ -83,6 +88,10 @@ public void nudgeDown() { setSetpoint(setPoint - NUDGE_AMOUNT); } + public void nudge(double input) { + if (input > 0) {nudgeUp();} else {nudgeDown();} + } + @Override protected WristStatus updateStatus() { return new WristStatus(motor.getSensorPosition(), setPoint); 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 00000000..6013eaf7 --- /dev/null +++ b/src/main/java/com/team766/robot/copy_2910/procedures/CenterL1.java @@ -0,0 +1,28 @@ +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.Intake; +import com.team766.robot.copy_2910.mechanisms.Shoulder; +import com.team766.robot.copy_2910.mechanisms.Wrist; +import com.team766.robot.copy_2910.mechanisms.Elevator.ElevatorPosition; +import com.team766.robot.copy_2910.mechanisms.Shoulder.ShoulderPosition; +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/IntakeCoral.java b/src/main/java/com/team766/robot/copy_2910/procedures/IntakeCoral.java index d002e42d..82d98e84 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,19 +2,39 @@ 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 intake2) { - intake = reserve(intake2); + 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"); 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 00000000..fabe2f7e --- /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 a448b111..2d7f0a2a 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 @@ -14,9 +14,8 @@ public OuttakeCoral(Intake intake2) { @Override public void run(Context context) { - intake.turnRightPositive(); - intake.turnLeftNegative(); - intake.turnAlgaeNegative(); + intake.setLeft(1); + intake.setRight(-1); waitForStatusMatching(context, Intake.IntakeStatus.class, s -> !s.hasCoralInFrontCenter()); intake.stop(); } From 6969338144689ef981b6c6866f39fa7d59c4f150 Mon Sep 17 00:00:00 2001 From: Max Spier <68516760+Mixmix00@users.noreply.github.com> Date: Fri, 25 Jul 2025 16:10:33 -0700 Subject: [PATCH 12/21] Refactor formatting and logic in robot mechanisms Improves code formatting and consistency across mechanism classes and procedures, including clearer conditional logic, indentation, and method calls. No functional changes were made; this refactor enhances readability and maintainability. --- .../com/team766/robot/copy_2910/BoxOpOI.java | 102 +++++++++--------- .../java/com/team766/robot/copy_2910/OI.java | 29 +++-- .../com/team766/robot/copy_2910/Robot.java | 8 +- .../robot/copy_2910/mechanisms/Elevator.java | 24 +++-- .../robot/copy_2910/mechanisms/Shoulder.java | 17 ++- .../robot/copy_2910/mechanisms/Wrist.java | 6 +- .../robot/copy_2910/procedures/CenterL1.java | 20 ++-- .../copy_2910/procedures/IntakeCoral.java | 19 ++-- .../copy_2910/procedures/MoveWristvator.java | 12 +-- 9 files changed, 129 insertions(+), 108 deletions(-) diff --git a/src/main/java/com/team766/robot/copy_2910/BoxOpOI.java b/src/main/java/com/team766/robot/copy_2910/BoxOpOI.java index 8ab5733e..835b45a5 100644 --- a/src/main/java/com/team766/robot/copy_2910/BoxOpOI.java +++ b/src/main/java/com/team766/robot/copy_2910/BoxOpOI.java @@ -3,22 +3,20 @@ import static com.team766.framework.RulePersistence.*; import com.team766.framework.Conditions; -import com.team766.framework.Conditions.LogicalAnd; 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.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.Wrist; -import com.team766.robot.copy_2910.mechanisms.Elevator.ElevatorPosition; 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 { @@ -56,7 +54,6 @@ public BoxOpOI( }); // ALGAE INTAKE POSITIONS - addRule( "Queue to Algae Ground Intake Position", () -> boxopGamepad.getPOV() == InputConstants.BUTTON_ALGAE_INTAKE_GROUND, @@ -89,20 +86,20 @@ public BoxOpOI( queuedControl.shoulderPosition = ShoulderPosition.ALGAE_LOW; queuedControl.wristPosition = WristPosition.ALGAE_LOW; }); - + 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); - } - ); + "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( "Queue Elevator and Wrist to L1 Position", @@ -149,25 +146,32 @@ public BoxOpOI( }); 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)) + "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) - ); + "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 Elevator", boxopGamepad.whenAxisMoved( @@ -193,24 +197,24 @@ public BoxOpOI( }); addRule( - "Nudge Shoulder", - boxopGamepad.whenAxisMoved( - InputConstants.AXIS_WRIST_FINETUNE), - ONCE_AND_HOLD, - wrist, - () -> { - shoulder.nudge( - boxopGamepad.getAxis( - InputConstants.AXIS_WRIST_FINETUNE)); - }); + "Nudge Shoulder", + boxopGamepad.whenAxisMoved( + InputConstants.AXIS_WRIST_FINETUNE), + ONCE_AND_HOLD, + wrist, + () -> { + shoulder.nudge( + boxopGamepad.getAxis( + InputConstants.AXIS_WRIST_FINETUNE)); + }); } }) .withFinishedTriggeringProcedure( - Set.of(elevator, wrist, shoulder), - () -> { - elevator.setPosition(ElevatorPosition.READY); - wrist.setPosition(WristPosition.STOW); - shoulder.setPosition(ShoulderPosition.STOW); - }); + Set.of(elevator, wrist, shoulder), + () -> { + elevator.setPosition(ElevatorPosition.READY); + wrist.setPosition(WristPosition.STOW); + shoulder.setPosition(ShoulderPosition.STOW); + }); } } diff --git a/src/main/java/com/team766/robot/copy_2910/OI.java b/src/main/java/com/team766/robot/copy_2910/OI.java index 5ea8748a..7db4a9f8 100644 --- a/src/main/java/com/team766/robot/copy_2910/OI.java +++ b/src/main/java/com/team766/robot/copy_2910/OI.java @@ -1,6 +1,5 @@ package com.team766.robot.copy_2910; -import static com.team766.framework.RulePersistence.ONCE; import static com.team766.framework.RulePersistence.ONCE_AND_HOLD; import com.team766.framework.RuleEngine; @@ -12,10 +11,7 @@ import com.team766.robot.copy_2910.mechanisms.Elevator.ElevatorPosition; import com.team766.robot.copy_2910.mechanisms.Shoulder.ShoulderPosition; import com.team766.robot.copy_2910.mechanisms.Wrist.WristPosition; -import com.team766.robot.copy_2910.procedures.AutoScore; -import com.team766.robot.copy_2910.procedures.IntakeCoral; import com.team766.robot.copy_2910.procedures.OuttakeCoral; -import java.util.Set; public class OI extends RuleEngine { @@ -48,21 +44,24 @@ public OI( queuedControl.elevatorPosition = ElevatorPosition.READY; addRules(new DriverOI(leftJoystick, rightJoystick, swerveDrive)); - addRules(new BoxOpOI(boxopGamepad, shoulder, elevator, wrist, climber, intake, queuedControl)); + addRules( + new BoxOpOI( + boxopGamepad, shoulder, elevator, wrist, climber, intake, queuedControl)); addRule( - "Outtake Coral", - leftJoystick.whenButton(InputConstants.JOYSTICK_RIGHT_BUTTON), - ONCE_AND_HOLD, - () -> new OuttakeCoral(intake)).withFinishedTriggeringProcedure(intake, () -> intake.stop()); + "Outtake Coral", + leftJoystick.whenButton(InputConstants.JOYSTICK_RIGHT_BUTTON), + ONCE_AND_HOLD, + () -> new OuttakeCoral(intake)) + .withFinishedTriggeringProcedure(intake, () -> intake.stop()); addRule( - "Outtake Algae", - leftJoystick.whenButton(InputConstants.JOYSTICK_LEFT_BUTTON), - ONCE_AND_HOLD, - intake, - () -> intake.setAlgaePower(-0.5) - ).withFinishedTriggeringProcedure(intake, () -> intake.stop()); + "Outtake Algae", + leftJoystick.whenButton(InputConstants.JOYSTICK_LEFT_BUTTON), + ONCE_AND_HOLD, + intake, + () -> intake.setAlgaePower(-0.5)) + .withFinishedTriggeringProcedure(intake, () -> intake.stop()); // addRule( // "Apply queued positions", 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 a8d8bea3..80874978 100644 --- a/src/main/java/com/team766/robot/copy_2910/Robot.java +++ b/src/main/java/com/team766/robot/copy_2910/Robot.java @@ -5,14 +5,13 @@ import com.team766.hal.RobotConfigurator; import com.team766.robot.common.SwerveConfig; import com.team766.robot.common.mechanisms.SwerveDrive; -import com.team766.robot.copy_2910.mechanisms.Elevator; 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.OuttakeCoral; import com.team766.robot.gatorade.Lights; public class Robot implements RobotConfigurator { @@ -52,7 +51,10 @@ public RuleEngine createLights() { @Override public AutonomousMode[] getAutonomousModes() { // TODO Auto-generated method stub - return new AutonomousMode[] {new AutonomousMode("Center Start L1", () -> new CenterL1(drive, intake, wrist, elevator, shoulder))}; + return new AutonomousMode[] { + new AutonomousMode( + "Center Start 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 bda42e34..458e66f2 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 @@ -49,16 +49,19 @@ public Elevator() { elevatorMotorRight.setCurrentLimit(40); // Set current limit for the elevator motor elevatorMotorLeft.setInverted(false); SparkMaxConfig rightConfig = new SparkMaxConfig(); - rightConfig.follow((SparkMax)elevatorMotorLeft, true /* invert */); - ((SparkMax)elevatorMotorRight).configure( - rightConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + rightConfig.follow((SparkMax) elevatorMotorLeft, true /* invert */); + ((SparkMax) elevatorMotorRight) + .configure( + rightConfig, + ResetMode.kNoResetSafeParameters, + PersistMode.kPersistParameters); // 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); + // elevatorMotorRight.setInverted(true); } public enum ElevatorPosition { @@ -66,17 +69,12 @@ public enum ElevatorPosition { L1(0.881), L2(-4.452), L3(-11), - L4(-29), //-21.357 + L4(-29), // -21.357 ALGAE_HIGH(-9.357), ALGAE_LOW(-3.262), CORAL_GROUND(2), ALGAE_GROUND(-1.643), - - - - - READY(2), // 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 @@ -116,7 +114,11 @@ public void nudgeDown() { } public void nudge(double input) { - if (input > 0) {nudgeUp();} else {nudgeDown();} + if (input > 0) { + nudgeUp(); + } else { + nudgeDown(); + } } public void run() { 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 83f86cd2..863c7cda 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 @@ -73,9 +73,12 @@ public Shoulder() { rightMotor.follow(leftMotor); SparkMaxConfig rightConfig = new SparkMaxConfig(); - rightConfig.follow((SparkMax)leftMotor, true /* invert */); - ((SparkMax)rightMotor).configure( - rightConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + rightConfig.follow((SparkMax) leftMotor, true /* invert */); + ((SparkMax) rightMotor) + .configure( + rightConfig, + ResetMode.kNoResetSafeParameters, + PersistMode.kPersistParameters); leftMotor.setCurrentLimit(40); rightMotor.setCurrentLimit(40); @@ -99,7 +102,7 @@ public void setPosition(ShoulderPosition position) { } public void run() { - //leftMotor.set(.Position, setPoint) + // leftMotor.set(.Position, setPoint) leftMotor.set(MotorController.ControlMode.Position, setPoint); log("SHOULDER Setpoint: " + setPoint + " Pos: " + leftMotor.getSensorPosition()); } @@ -113,7 +116,11 @@ public void nudgeDown() { } public void nudge(double input) { - if (input > 0) {nudgeUp();} else {nudgeDown();} + if (input > 0) { + nudgeUp(); + } else { + nudgeDown(); + } } @Override 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 a73bb617..a0de9fe7 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 @@ -89,7 +89,11 @@ public void nudgeDown() { } public void nudge(double input) { - if (input > 0) {nudgeUp();} else {nudgeDown();} + if (input > 0) { + nudgeUp(); + } else { + nudgeDown(); + } } @Override 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 index 6013eaf7..fd7c74ed 100644 --- a/src/main/java/com/team766/robot/copy_2910/procedures/CenterL1.java +++ b/src/main/java/com/team766/robot/copy_2910/procedures/CenterL1.java @@ -3,25 +3,27 @@ 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.Wrist; -import com.team766.robot.copy_2910.mechanisms.Elevator.ElevatorPosition; 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) { + 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)); + 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/IntakeCoral.java b/src/main/java/com/team766/robot/copy_2910/procedures/IntakeCoral.java index 82d98e84..e33cee3d 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 @@ -9,7 +9,6 @@ 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 { @@ -32,7 +31,9 @@ public void run(Context context) { shoulder.setPosition(ShoulderPosition.CORAL_GROUND); wrist.setPosition(WristPosition.CORAL_GROUND); waitForStatusMatching( - context, Shoulder.ShoulderStatus.class, s -> s.isNearTo(ShoulderPosition.CORAL_GROUND)); + 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); @@ -87,14 +88,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 | + * 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! - */ + * + * 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/MoveWristvator.java b/src/main/java/com/team766/robot/copy_2910/procedures/MoveWristvator.java index fabe2f7e..ca7807da 100644 --- a/src/main/java/com/team766/robot/copy_2910/procedures/MoveWristvator.java +++ b/src/main/java/com/team766/robot/copy_2910/procedures/MoveWristvator.java @@ -9,7 +9,6 @@ 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; @@ -39,7 +38,8 @@ public final void run(Context context) { // 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()) { + if (shoulderSetpoint.getPosition() + > getStatusOrThrow(Shoulder.ShoulderStatus.class).position()) { shoulder.setPosition(shoulderSetpoint); waitForStatusMatching( context, Shoulder.ShoulderStatus.class, s -> s.isNearTo(shoulderSetpoint)); @@ -49,14 +49,14 @@ public final void run(Context context) { // Move the elevator. Wait until it gets near the target position. elevator.setPosition(elevatorSetpoint); - waitForStatusMatching( - context, Elevator.ElevatorStatus.class, s -> s.isAtHeight()); + 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()) { + 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)); From 362ea6bf1ba89f3d1d99c16211115e8f1053659c Mon Sep 17 00:00:00 2001 From: Max Spier <68516760+Mixmix00@users.noreply.github.com> Date: Fri, 25 Jul 2025 21:41:15 -0700 Subject: [PATCH 13/21] Chatgpt cracken scaled values If this works im gonna cry --- .../robot/copy_2910/mechanisms/Elevator.java | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) 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 458e66f2..141e052f 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 @@ -65,15 +65,15 @@ public Elevator() { } public enum ElevatorPosition { - INTAKE(2.0), - L1(0.881), - L2(-4.452), - L3(-11), - L4(-29), // -21.357 - ALGAE_HIGH(-9.357), - ALGAE_LOW(-3.262), - CORAL_GROUND(2), - ALGAE_GROUND(-1.643), + INTAKE(2.0 * 48.7619), + L1(0.881 * 48.7619), + L2(-4.452 * 48.7619), + L3(-11 * 48.7619), + L4(-29 * 48.7619), // -21.357 + ALGAE_HIGH(-9.357 * 48.7619), + ALGAE_LOW(-3.262 * 48.7619), + CORAL_GROUND(2 * 48.7619), + ALGAE_GROUND(-1.643 * 48.7619), READY(2), // Should be the default position and the ready position for vision so that it // can see the tag From a47598b3b74e2805cacf2fc351d8a48f199875da Mon Sep 17 00:00:00 2001 From: MAXSPIER Date: Wed, 17 Sep 2025 17:22:45 -0700 Subject: [PATCH 14/21] all of the code --- ...core.path => Center Start - L1 Score.path} | 0 .../com/team766/robot/copy_2910/BoxOpOI.java | 144 ++++++++---------- .../java/com/team766/robot/copy_2910/OI.java | 24 +++ .../com/team766/robot/copy_2910/Robot.java | 5 +- .../robot/copy_2910/mechanisms/Elevator.java | 46 +++--- .../robot/copy_2910/mechanisms/Intake.java | 6 +- .../robot/copy_2910/mechanisms/Shoulder.java | 8 +- .../robot/copy_2910/mechanisms/Wrist.java | 11 +- .../copy_2910/procedures/DriveStraight.java | 22 +++ .../copy_2910/procedures/IntakeCoral.java | 3 - .../robot/reva_2025/mechanisms/Wrist.java | 5 +- 11 files changed, 153 insertions(+), 121 deletions(-) rename src/main/deploy/pathplanner/paths/{Blue Line Start - L1 Score.path => Center Start - L1 Score.path} (100%) create mode 100644 src/main/java/com/team766/robot/copy_2910/procedures/DriveStraight.java 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/java/com/team766/robot/copy_2910/BoxOpOI.java b/src/main/java/com/team766/robot/copy_2910/BoxOpOI.java index 835b45a5..02f9b03f 100644 --- a/src/main/java/com/team766/robot/copy_2910/BoxOpOI.java +++ b/src/main/java/com/team766/robot/copy_2910/BoxOpOI.java @@ -54,52 +54,38 @@ public BoxOpOI( }); // 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_LOW; - queuedControl.shoulderPosition = ShoulderPosition.ALGAE_LOW; - queuedControl.wristPosition = WristPosition.ALGAE_LOW; - }); - - 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( + // "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", @@ -145,6 +131,20 @@ public BoxOpOI( 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), @@ -172,49 +172,39 @@ public BoxOpOI( }) .withFinishedTriggeringProcedure( intake, () -> intake.setAlgaePower(0.1)); - addRule( - "Nudge Elevator", - boxopGamepad.whenAxisMoved( - InputConstants.AXIS_ELEVATOR_FINETUNE), - ONCE_AND_HOLD, - elevator, - () -> { - elevator.nudge( - boxopGamepad.getAxis( - InputConstants.AXIS_ELEVATOR_FINETUNE)); - }); - - addRule( - "Nudge Wrist", - boxopGamepad.whenAxisMoved( - InputConstants.AXIS_WRIST_FINETUNE), - ONCE_AND_HOLD, - wrist, - () -> { - wrist.nudge( - boxopGamepad.getAxis( - InputConstants.AXIS_WRIST_FINETUNE)); - }); addRule( "Nudge Shoulder", boxopGamepad.whenAxisMoved( InputConstants.AXIS_WRIST_FINETUNE), ONCE_AND_HOLD, - wrist, + shoulder, () -> { shoulder.nudge( boxopGamepad.getAxis( InputConstants.AXIS_WRIST_FINETUNE)); }); } - }) - .withFinishedTriggeringProcedure( - Set.of(elevator, wrist, shoulder), - () -> { - elevator.setPosition(ElevatorPosition.READY); - wrist.setPosition(WristPosition.STOW); - shoulder.setPosition(ShoulderPosition.STOW); }); + 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.shoulderPosition = ShoulderPosition.STOW; + queuedControl.wristPosition = WristPosition.STOW; + }); + } } diff --git a/src/main/java/com/team766/robot/copy_2910/OI.java b/src/main/java/com/team766/robot/copy_2910/OI.java index 7db4a9f8..f4984538 100644 --- a/src/main/java/com/team766/robot/copy_2910/OI.java +++ b/src/main/java/com/team766/robot/copy_2910/OI.java @@ -1,5 +1,6 @@ package com.team766.robot.copy_2910; +import static com.team766.framework.RulePersistence.ONCE; import static com.team766.framework.RulePersistence.ONCE_AND_HOLD; import com.team766.framework.RuleEngine; @@ -63,6 +64,29 @@ public OI( () -> intake.setAlgaePower(-0.5)) .withFinishedTriggeringProcedure(intake, () -> intake.stop()); + addRule("Wrist Nudge Up", + leftJoystick.whenButton(2), + ONCE, + wrist, + () -> wrist.nudgeUp()); + addRule("Wrist Nudge Down", + leftJoystick.whenButton(3), + 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()); + + // addRule( // "Apply queued positions", // leftJoystick.whenButton(4), 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 80874978..babc4e5d 100644 --- a/src/main/java/com/team766/robot/copy_2910/Robot.java +++ b/src/main/java/com/team766/robot/copy_2910/Robot.java @@ -12,6 +12,7 @@ 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 { @@ -53,7 +54,9 @@ public AutonomousMode[] getAutonomousModes() { // TODO Auto-generated method stub return new AutonomousMode[] { new AutonomousMode( - "Center Start L1", () -> new CenterL1(drive, intake, wrist, elevator, shoulder)) + "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 141e052f..2a353548 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 @@ -16,7 +16,7 @@ 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 @@ -42,19 +42,12 @@ 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 - - elevatorMotorRight.follow(elevatorMotorLeft); setPoint = ElevatorPosition.READY.getPosition(); // Default position elevatorMotorLeft.setCurrentLimit(40); elevatorMotorRight.setCurrentLimit(40); // Set current limit for the elevator motor - elevatorMotorLeft.setInverted(false); - SparkMaxConfig rightConfig = new SparkMaxConfig(); - rightConfig.follow((SparkMax) elevatorMotorLeft, true /* invert */); - ((SparkMax) elevatorMotorRight) - .configure( - rightConfig, - ResetMode.kNoResetSafeParameters, - PersistMode.kPersistParameters); + elevatorMotorLeft.setInverted(true); + elevatorMotorRight.setInverted(false); + elevatorMotorRight.follow(elevatorMotorLeft); // ffGain = // ConfigFileReader.instance.getDouble( // "ElevatorFFGain"); // Replace with actual config key @@ -65,21 +58,22 @@ public Elevator() { } public enum ElevatorPosition { - INTAKE(2.0 * 48.7619), - L1(0.881 * 48.7619), - L2(-4.452 * 48.7619), - L3(-11 * 48.7619), - L4(-29 * 48.7619), // -21.357 - ALGAE_HIGH(-9.357 * 48.7619), - ALGAE_LOW(-3.262 * 48.7619), - CORAL_GROUND(2 * 48.7619), - ALGAE_GROUND(-1.643 * 48.7619), - - READY(2), // Should be the default position and the ready position for vision so that it + INTAKE(0.3), + L1(3.8), + L2(4.44), + L3(13), + L4(17.80), // -21.357 + ALGAE_HIGH(12), + ALGAE_LOW(7), + CORAL_GROUND(0.3), + ALGAE_GROUND(0.5), + STOW(0), + + READY(3), // 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(17.9), // Maximum height of the elevator, TODO: Adjust based on the actual elevator's // maximum position - MINIMUM(-30); // Minimum height of the elevator, TODO: Adjust based on the actual elevator's + MINIMUM(0); // Minimum height of the elevator, TODO: Adjust based on the actual elevator's // minimum position final double position; @@ -106,11 +100,11 @@ public void setPosition(ElevatorPosition setPosition) { } 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) { 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 523d92b4..3cb6132a 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 @@ -18,10 +18,10 @@ public class Intake extends MechanismWithStatus { private static final double CORAL_THRESHOLD = 0.12; // TODO: Set this to a real value - private double leftPower = 0.125; - private double rightPower = 1; + private double leftPower = 0.4; + private double rightPower = 0.4; - private double algaePower = 1; + private double algaePower = 0.5; public Intake() { leftCANRange = RobotProvider.instance.getTimeOfFlight("INTAKE.CANRange.left"); 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 863c7cda..8e7066ef 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 @@ -43,12 +43,12 @@ public enum ShoulderPosition { L4(24.5), ALGAE_HIGH(22.952), ALGAE_LOW(20.405), - CORAL_GROUND(0.071), + CORAL_GROUND(-0.75), ALGAE_GROUND(4.119), CLIMBER(28), - STOW(5), + STOW(0), MAXIMUM(40), - MINIMUM(0); + MINIMUM(-1); private final double angle; @@ -108,7 +108,7 @@ public void run() { } public void nudgeUp() { - setSetpoint(setPoint + NUDGE_AMOUNT); + setPoint += 1; } public void nudgeDown() { 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 a0de9fe7..b41a3c6c 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 @@ -17,7 +17,7 @@ public class Wrist extends MechanismWithStatus { // 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 @@ -38,9 +38,10 @@ public enum WristPosition { L4(-9), ALGAE_HIGH(-25.928), ALGAE_LOW(-25.928), - CORAL_GROUND(-16.5), + CORAL_GROUND(-15), + ALGAE(-25.2), ALGAE_GROUND(-21.786), - STOW(-5), + STOW(0), MAXIMUM(10), MINIMUM(-30); @@ -81,11 +82,11 @@ public void run() { } 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) { 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 00000000..ea26bd6f --- /dev/null +++ b/src/main/java/com/team766/robot/copy_2910/procedures/DriveStraight.java @@ -0,0 +1,22 @@ +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 e33cee3d..43585b16 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 @@ -102,9 +102,6 @@ public void run(Context context) { } } // Once the coral is in the back center, we stop the intake motors. - intake.turnLeftPositive(); - intake.turnRightNegative(); - context.waitForSeconds(0.05); intake.stop(); } } 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 f2d17061..291bc3ec 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 @@ -2,7 +2,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 +36,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 +68,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) { From 918e183b3003dbdb8d71c76fbeb00695260c1290 Mon Sep 17 00:00:00 2001 From: DS Date: Sat, 27 Sep 2025 16:57:16 -0700 Subject: [PATCH 15/21] 9/27 bringup --- .../team766/robot/copy_2910/mechanisms/Elevator.java | 4 ++-- .../team766/robot/copy_2910/mechanisms/Shoulder.java | 10 +--------- 2 files changed, 3 insertions(+), 11 deletions(-) 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 2a353548..c0e688bd 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 @@ -45,8 +45,8 @@ public Elevator() { setPoint = ElevatorPosition.READY.getPosition(); // Default position 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( 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 8e7066ef..96f37def 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 @@ -69,17 +69,9 @@ public Shoulder() { RobotProvider.instance.getMotor( "RightShoulderMotor"); // Replace with actual motor name - leftMotor.setInverted(true); + //leftMotor.setInverted(true); rightMotor.follow(leftMotor); - SparkMaxConfig rightConfig = new SparkMaxConfig(); - rightConfig.follow((SparkMax) leftMotor, true /* invert */); - ((SparkMax) rightMotor) - .configure( - rightConfig, - ResetMode.kNoResetSafeParameters, - PersistMode.kPersistParameters); - leftMotor.setCurrentLimit(40); rightMotor.setCurrentLimit(40); From fb1957048055e969aa6431355938d727b0331c42 Mon Sep 17 00:00:00 2001 From: DS Date: Sat, 18 Oct 2025 15:01:11 -0700 Subject: [PATCH 16/21] changes from calgames & next few weeks --- .../deploy/pathplanner/paths/New Path.path | 54 ++++++++++++++ .../com/team766/robot/copy_2910/BoxOpOI.java | 71 ++++++++++--------- .../java/com/team766/robot/copy_2910/OI.java | 21 +++--- .../com/team766/robot/copy_2910/Robot.java | 11 ++- .../robot/copy_2910/mechanisms/Elevator.java | 39 +++++----- .../robot/copy_2910/mechanisms/Shoulder.java | 32 +++++---- .../robot/copy_2910/mechanisms/Wrist.java | 28 +++++--- .../copy_2910/procedures/DriveStraight.java | 3 +- .../robot/reva_2025/mechanisms/Wrist.java | 1 + 9 files changed, 164 insertions(+), 96 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/New Path.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 00000000..43cdab56 --- /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/copy_2910/BoxOpOI.java b/src/main/java/com/team766/robot/copy_2910/BoxOpOI.java index 02f9b03f..903b9366 100644 --- a/src/main/java/com/team766/robot/copy_2910/BoxOpOI.java +++ b/src/main/java/com/team766/robot/copy_2910/BoxOpOI.java @@ -26,7 +26,6 @@ public BoxOpOI( Shoulder shoulder, Elevator elevator, Wrist wrist, - Climber climber, Intake intake, QueuedControl queuedControl) { @@ -38,17 +37,17 @@ public BoxOpOI( "Control Climber", new Conditions.Toggle(boxopGamepad.whenButton(InputConstants.BUTTON_CLIMB)), ONCE_AND_HOLD, - Set.of(climber, wrist, elevator, shoulder), + Set.of(wrist, elevator, shoulder), () -> { elevator.setPosition(Elevator.ElevatorPosition.MAXIMUM); wrist.setPosition(Wrist.WristPosition.ALGAE_LOW); - climber.setClimberSpeed(0.5); + shoulder.setPosition(Shoulder.ShoulderPosition.CLIMBER); }) .withFinishedTriggeringProcedure( - Set.of(climber, shoulder), + Set.of(shoulder), context -> { - climber.stop(); + shoulder.setPosition(ShoulderPosition.CORAL_GROUND); log("finished triggering"); }); @@ -76,16 +75,16 @@ public BoxOpOI( // 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 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", @@ -132,18 +131,18 @@ public BoxOpOI( }); 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); - }); + "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", @@ -186,25 +185,27 @@ public BoxOpOI( }); } }); - addRule("Nudge Shoulder Up", + addRule( + "Nudge Shoulder Up", boxopGamepad.whenButton(9), ONCE, shoulder, () -> shoulder.nudgeUp()); - addRule("Nudge Shoulder Down", + addRule( + "Nudge Shoulder Down", boxopGamepad.whenButton(10), ONCE, shoulder, () -> shoulder.nudgeDown()); - addRule("Stow", - () -> boxopGamepad.getPOV() == 0, - ONCE, - Set.of(elevator, shoulder, intake), + addRule( + "Stow", + () -> boxopGamepad.getPOV() == 0, + ONCE, + Set.of(elevator, shoulder, intake), () -> { queuedControl.elevatorPosition = ElevatorPosition.STOW; - queuedControl.shoulderPosition = ShoulderPosition.STOW; queuedControl.wristPosition = WristPosition.STOW; + queuedControl.shoulderPosition = ShoulderPosition.STOW; }); - } } diff --git a/src/main/java/com/team766/robot/copy_2910/OI.java b/src/main/java/com/team766/robot/copy_2910/OI.java index f4984538..27a4c180 100644 --- a/src/main/java/com/team766/robot/copy_2910/OI.java +++ b/src/main/java/com/team766/robot/copy_2910/OI.java @@ -28,8 +28,7 @@ public OI( Wrist wrist, Elevator elevator, Shoulder shoulder, - Vision vision, - Climber climber) { + Vision vision) { final JoystickReader leftJoystick = RobotProvider.instance.getJoystick(0); final JoystickReader rightJoystick = RobotProvider.instance.getJoystick(1); @@ -47,7 +46,7 @@ public OI( addRules(new DriverOI(leftJoystick, rightJoystick, swerveDrive)); addRules( new BoxOpOI( - boxopGamepad, shoulder, elevator, wrist, climber, intake, queuedControl)); + boxopGamepad, shoulder, elevator, wrist, intake, queuedControl)); addRule( "Outtake Coral", @@ -64,28 +63,26 @@ public OI( () -> intake.setAlgaePower(-0.5)) .withFinishedTriggeringProcedure(intake, () -> intake.stop()); - addRule("Wrist Nudge Up", - leftJoystick.whenButton(2), - ONCE, - wrist, - () -> wrist.nudgeUp()); - addRule("Wrist Nudge Down", + addRule("Wrist Nudge Up", leftJoystick.whenButton(2), ONCE, wrist, () -> wrist.nudgeUp()); + addRule( + "Wrist Nudge Down", leftJoystick.whenButton(3), ONCE, wrist, () -> wrist.nudgeDown()); - addRule("Elevator Nudge Up", + addRule( + "Elevator Nudge Up", rightJoystick.whenButton(2), ONCE, elevator, () -> elevator.nudgeUp()); - addRule("Elevator Nudge Down", + addRule( + "Elevator Nudge Down", rightJoystick.whenButton(3), ONCE, elevator, () -> elevator.nudgeDown()); - // addRule( // "Apply queued positions", 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 babc4e5d..853d7597 100644 --- a/src/main/java/com/team766/robot/copy_2910/Robot.java +++ b/src/main/java/com/team766/robot/copy_2910/Robot.java @@ -20,7 +20,7 @@ public class Robot implements RobotConfigurator { private SwerveDrive drive; private Intake intake; private Vision vision; - private Climber climber; + //private Climber climber; private Elevator elevator; private Shoulder shoulder; private Wrist wrist; @@ -30,7 +30,7 @@ public void initializeMechanisms() { SwerveConfig swerveConfig = new SwerveConfig().withDistanceBetweenWheels(0.533); drive = new SwerveDrive(swerveConfig); intake = new Intake(); - climber = new Climber(); + //climber = new Climber(); elevator = new Elevator(); vision = new Vision(); shoulder = new Shoulder(); @@ -39,7 +39,7 @@ public void initializeMechanisms() { @Override public RuleEngine createOI() { - return new OI(drive, intake, wrist, elevator, shoulder, vision, climber); + return new OI(drive, intake, wrist, elevator, shoulder, vision); } @Override @@ -53,10 +53,9 @@ public RuleEngine createLights() { public AutonomousMode[] getAutonomousModes() { // TODO Auto-generated method stub return new AutonomousMode[] { + new AutonomousMode("Move", () -> new DriveStraight(drive)), new AutonomousMode( - "Move", () -> new DriveStraight(drive)), - new AutonomousMode( - "Center L1", () -> new CenterL1(drive, intake, wrist, elevator, shoulder)) + "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 c0e688bd..0a9d238c 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,9 +1,5 @@ package com.team766.robot.copy_2910.mechanisms; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.config.SparkMaxConfig; import com.team766.framework.MechanismWithStatus; import com.team766.framework.Status; import com.team766.hal.MotorController; @@ -19,7 +15,7 @@ public class Elevator extends MechanismWithStatus { 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 double setPoint; @@ -45,35 +41,36 @@ public Elevator() { setPoint = ElevatorPosition.READY.getPosition(); // Default position elevatorMotorLeft.setCurrentLimit(40); elevatorMotorRight.setCurrentLimit(40); // Set current limit for the elevator motor - //elevatorMotorLeft.setInverted(true); - //elevatorMotorRight.setInverted(false); - elevatorMotorRight.follow(elevatorMotorLeft); + // elevatorMotorLeft.setInverted(true); + // elevatorMotorRight.setInverted(false); + elevatorMotorLeft.follow(elevatorMotorRight); // ffGain = // ConfigFileReader.instance.getDouble( // "ElevatorFFGain"); // Replace with actual config key - elevatorMotorLeft.setSensorPosition( + elevatorMotorRight.setSensorPosition( 0.0); // Elevator always has to start at same 0.0 position // elevatorMotorRight.setInverted(true); } public enum ElevatorPosition { - INTAKE(0.3), + INTAKE(0), L1(3.8), - L2(4.44), - L3(13), - L4(17.80), // -21.357 + L2(3.631), + L3(9.536), + L4(21.33), // -21.357 ALGAE_HIGH(12), ALGAE_LOW(7), - CORAL_GROUND(0.3), + CORAL_GROUND(-0.25), ALGAE_GROUND(0.5), - STOW(0), + STOW(0.5), - READY(3), // Should be the default position and the ready position for vision so that it + READY(1), // Should be the default position and the ready position for vision so that it // can see the tag - MAXIMUM(17.9), // 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(0); // 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; @@ -116,11 +113,11 @@ public void nudge(double input) { } public void run() { - elevatorMotorLeft.set(MotorController.ControlMode.Position, setPoint); - log("SHOULDER Setpoint: " + setPoint + " Pos: " + elevatorMotorLeft.getSensorPosition()); + elevatorMotorRight.set(MotorController.ControlMode.Position, setPoint); + log("SHOULDER Setpoint: " + setPoint + " Pos: " + elevatorMotorRight.getSensorPosition()); } protected ElevatorStatus updateStatus() { - return new ElevatorStatus(elevatorMotorLeft.getSensorPosition(), setPoint); + return new ElevatorStatus(elevatorMotorRight.getSensorPosition(), setPoint); } } 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 96f37def..ffdb2feb 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,11 +1,8 @@ package com.team766.robot.copy_2910.mechanisms; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.config.SparkMaxConfig; 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 edu.wpi.first.math.MathUtil; @@ -14,9 +11,12 @@ public class Shoulder extends MechanismWithStatus { private MotorController leftMotor; private MotorController rightMotor; + private final EncoderReader absoluteEncoder; + private boolean encoderInitialized = false; + private double gearRatio = 120; private static final double THRESHOLD = - 0.5; // Threshold for determining if the shoulder is near a position | TODO: Adjust this + 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; @@ -38,17 +38,17 @@ public boolean isNearTo(double angle) { public enum ShoulderPosition { L1(14), - L2(11.5), - L3(17), - L4(24.5), + L2(50.891), + L3(70.247), + L4(87.606), ALGAE_HIGH(22.952), ALGAE_LOW(20.405), - CORAL_GROUND(-0.75), + CORAL_GROUND(1), ALGAE_GROUND(4.119), CLIMBER(28), STOW(0), - MAXIMUM(40), - MINIMUM(-1); + MAXIMUM(100), + MINIMUM(-10); private final double angle; @@ -68,8 +68,11 @@ public Shoulder() { rightMotor = RobotProvider.instance.getMotor( "RightShoulderMotor"); // Replace with actual motor name + absoluteEncoder = + RobotProvider.instance.getEncoder( + "ShoulderEncoder"); // **ShoulderEncoder may not exist** - //leftMotor.setInverted(true); + // leftMotor.setInverted(true); rightMotor.follow(leftMotor); leftMotor.setCurrentLimit(40); @@ -117,6 +120,11 @@ public void nudge(double input) { @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 b41a3c6c..e88c1cdc 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 @@ -2,6 +2,7 @@ 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 edu.wpi.first.math.MathUtil; @@ -9,6 +10,10 @@ public class Wrist extends MechanismWithStatus { private MotorController motor; + private final EncoderReader absoluteEncoder; + private boolean encoderInitialized = false; + + private double gearRatio = 21.8; private static final double THRESHOLD = 0.5; // Threshold for determining if the wrist is near a position | TODO: Adjust this @@ -32,17 +37,17 @@ public boolean isNearTo(double angle) { } public enum WristPosition { - L1(-16.643), - L2(0.643), - L3(-1.667), - L4(-9), + 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), + CORAL_GROUND(-9.75), ALGAE(-25.2), ALGAE_GROUND(-21.786), - STOW(0), - MAXIMUM(10), + STOW(-1.0), + MAXIMUM(50), MINIMUM(-30); private final double angle; @@ -58,7 +63,9 @@ public double getPosition() { public Wrist() { motor = RobotProvider.instance.getMotor("WristMotor"); // Replace with actual motor name - + absoluteEncoder = + RobotProvider.instance.getEncoder("WristEncoder"); // **WristEncoder may not exist** + motor.setCurrentLimit(40); // ffGain = // ConfigFileReader.instance.getDouble( // "WristFFGain"); // Replace with actual config key @@ -99,6 +106,11 @@ public void nudge(double input) { @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/DriveStraight.java b/src/main/java/com/team766/robot/copy_2910/procedures/DriveStraight.java index ea26bd6f..d125558d 100644 --- a/src/main/java/com/team766/robot/copy_2910/procedures/DriveStraight.java +++ b/src/main/java/com/team766/robot/copy_2910/procedures/DriveStraight.java @@ -7,7 +7,7 @@ public class DriveStraight extends Procedure { private SwerveDrive drive; - public DriveStraight(SwerveDrive drive){ + public DriveStraight(SwerveDrive drive) { this.drive = reserve(drive); } @@ -18,5 +18,4 @@ public void run(Context context) { context.waitForSeconds(10); drive.controlRobotOriented(0, 0, 0); } - } 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 291bc3ec..9ec9b957 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 @@ -2,6 +2,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; From 1721591293e15e7aff4ac00a79278190a07c27ac Mon Sep 17 00:00:00 2001 From: DS Date: Tue, 21 Oct 2025 19:11:24 -0700 Subject: [PATCH 17/21] 10/21 intake & position fixes --- .../robot/copy_2910/mechanisms/Elevator.java | 18 +++++++++--------- .../robot/copy_2910/mechanisms/Intake.java | 7 +++++-- .../copy_2910/procedures/IntakeCoral.java | 2 +- 3 files changed, 15 insertions(+), 12 deletions(-) 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 0a9d238c..39cadb3a 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 @@ -43,22 +43,22 @@ public Elevator() { elevatorMotorRight.setCurrentLimit(40); // Set current limit for the elevator motor // elevatorMotorLeft.setInverted(true); // elevatorMotorRight.setInverted(false); - elevatorMotorLeft.follow(elevatorMotorRight); + elevatorMotorRight.follow(elevatorMotorLeft); // ffGain = // ConfigFileReader.instance.getDouble( // "ElevatorFFGain"); // Replace with actual config key - elevatorMotorRight.setSensorPosition( + elevatorMotorLeft.setSensorPosition( 0.0); // Elevator always has to start at same 0.0 position // elevatorMotorRight.setInverted(true); } public enum ElevatorPosition { - INTAKE(0), + INTAKE(0.4), L1(3.8), - L2(3.631), - L3(9.536), - L4(21.33), // -21.357 + L2(5.3), + L3(13.27), + L4(23.5), // -21.357 ALGAE_HIGH(12), ALGAE_LOW(7), CORAL_GROUND(-0.25), @@ -113,11 +113,11 @@ public void nudge(double input) { } public void run() { - elevatorMotorRight.set(MotorController.ControlMode.Position, setPoint); - log("SHOULDER Setpoint: " + setPoint + " Pos: " + elevatorMotorRight.getSensorPosition()); + 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 3cb6132a..2ee2f6e4 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,15 @@ public class Intake extends MechanismWithStatus { private MotorController centerAlgaeMotor; - private static final double CORAL_THRESHOLD = 0.12; // 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 + //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.4; private double rightPower = 0.4; - private double algaePower = 0.5; + private double algaePower = 0.3; //previously 0.5 public Intake() { leftCANRange = RobotProvider.instance.getTimeOfFlight("INTAKE.CANRange.left"); 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 43585b16..b5eff10a 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 @@ -46,7 +46,7 @@ public void run(Context context) { + status.get().getLeftDistance() + "Back center:" + status.get().getBackCenterDistance()); - while (!status.get().hasCoralInBackCenter()) { + while (waitForStatusMatchingOrTimeout(context, Intake.IntakeStatus.class, s -> !s.hasCoralInBackCenter(), 0.35).isPresent()) { context.yield(); intake.turnAlgaePositive(); status = getStatus(Intake.IntakeStatus.class); From d660f30e0e731b1d16e0b6d98bdaebc5ad0b2725 Mon Sep 17 00:00:00 2001 From: DS Date: Tue, 21 Oct 2025 19:50:22 -0700 Subject: [PATCH 18/21] more 10/21 changes --- .../com/team766/robot/copy_2910/mechanisms/Elevator.java | 2 +- .../com/team766/robot/copy_2910/mechanisms/Intake.java | 5 +++++ .../com/team766/robot/copy_2910/mechanisms/Shoulder.java | 2 +- .../team766/robot/copy_2910/procedures/OuttakeCoral.java | 7 ++++--- 4 files changed, 11 insertions(+), 5 deletions(-) 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 39cadb3a..292ffe37 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 @@ -58,7 +58,7 @@ public enum ElevatorPosition { L1(3.8), L2(5.3), L3(13.27), - L4(23.5), // -21.357 + L4(24), // -21.357 ALGAE_HIGH(12), ALGAE_LOW(7), CORAL_GROUND(-0.25), 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 2ee2f6e4..78913802 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 @@ -17,6 +17,7 @@ public class Intake extends MechanismWithStatus { private MotorController centerAlgaeMotor; 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 @@ -70,6 +71,10 @@ public boolean hasCoralInFrontCenter() { return frontCenterDistance < CORAL_THRESHOLD; } + public boolean hasCoralToOuttake() { + return frontCenterDistance < CORAL_OUTTAKE_THRESHOLD; + } + public boolean hasCoralInBackCenter() { return backCenterDistance < CORAL_THRESHOLD; } 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 ffdb2feb..edee28e2 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 @@ -40,7 +40,7 @@ public enum ShoulderPosition { L1(14), L2(50.891), L3(70.247), - L4(87.606), + L4(89), ALGAE_HIGH(22.952), ALGAE_LOW(20.405), CORAL_GROUND(1), 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 2d7f0a2a..290fd48a 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 @@ -14,9 +14,10 @@ public OuttakeCoral(Intake intake2) { @Override public void run(Context context) { - intake.setLeft(1); - intake.setRight(-1); - waitForStatusMatching(context, Intake.IntakeStatus.class, s -> !s.hasCoralInFrontCenter()); + intake.setLeft(1.5); + intake.setRight(-1.5 + ); + waitForStatusMatching(context, Intake.IntakeStatus.class, s -> !s.hasCoralToOuttake()); intake.stop(); } } From e1a5e269bedfcc083639e69c9e348c1c2091e9cd Mon Sep 17 00:00:00 2001 From: wh1te-lotus <111452810+wh1te-lotus@users.noreply.github.com> Date: Tue, 21 Oct 2025 20:06:25 -0700 Subject: [PATCH 19/21] Single driver OI, algae intake & shooting, L1 intake procedure, other minor mechanism changes --- .../common/constants/InputConstants.java | 5 +- .../com/team766/robot/copy_2910/BoxOpOI.java | 4 +- .../com/team766/robot/copy_2910/DriverOI.java | 87 ++++ .../java/com/team766/robot/copy_2910/OI.java | 385 ++++++++++++++++-- .../com/team766/robot/copy_2910/Robot.java | 5 +- .../robot/copy_2910/mechanisms/Elevator.java | 6 +- .../robot/copy_2910/mechanisms/Intake.java | 24 +- .../robot/copy_2910/mechanisms/Shoulder.java | 32 +- .../robot/copy_2910/mechanisms/Wrist.java | 43 +- .../copy_2910/procedures/IntakeCoral.java | 5 +- .../copy_2910/procedures/IntakeCoralL1.java | 106 +++++ .../copy_2910/procedures/OuttakeCoral.java | 3 +- .../copy_2910/procedures/ShootAlgae.java | 20 + 13 files changed, 668 insertions(+), 57 deletions(-) create mode 100644 src/main/java/com/team766/robot/copy_2910/DriverOI.java create mode 100644 src/main/java/com/team766/robot/copy_2910/procedures/IntakeCoralL1.java create mode 100644 src/main/java/com/team766/robot/copy_2910/procedures/ShootAlgae.java 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 bb8a1edd..4a96c43a 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 index 903b9366..e5a82397 100644 --- a/src/main/java/com/team766/robot/copy_2910/BoxOpOI.java +++ b/src/main/java/com/team766/robot/copy_2910/BoxOpOI.java @@ -7,7 +7,6 @@ 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.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; @@ -41,13 +40,12 @@ public BoxOpOI( () -> { 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"); }); 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 00000000..db907f05 --- /dev/null +++ b/src/main/java/com/team766/robot/copy_2910/DriverOI.java @@ -0,0 +1,87 @@ +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_LEFT_STICK_CLICK), + 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/OI.java b/src/main/java/com/team766/robot/copy_2910/OI.java index 27a4c180..322a09ae 100644 --- a/src/main/java/com/team766/robot/copy_2910/OI.java +++ b/src/main/java/com/team766/robot/copy_2910/OI.java @@ -3,16 +3,27 @@ import static com.team766.framework.RulePersistence.ONCE; import static com.team766.framework.RulePersistence.ONCE_AND_HOLD; +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.DriverOI; +import com.team766.robot.common.constants.InputConstants; import com.team766.robot.common.mechanisms.SwerveDrive; -import com.team766.robot.copy_2910.mechanisms.*; +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 { @@ -30,12 +41,11 @@ public OI( Shoulder shoulder, Vision vision) { - final JoystickReader leftJoystick = RobotProvider.instance.getJoystick(0); - final JoystickReader rightJoystick = RobotProvider.instance.getJoystick(1); - final JoystickReader boxopGamepad = RobotProvider.instance.getJoystick(2); - - leftJoystick.setAllAxisDeadzone(0.05); - rightJoystick.setAllAxisDeadzone(0.05); + // 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(); @@ -43,36 +53,34 @@ public OI( queuedControl.shoulderPosition = ShoulderPosition.STOW; queuedControl.elevatorPosition = ElevatorPosition.READY; - addRules(new DriverOI(leftJoystick, rightJoystick, swerveDrive)); - addRules( - new BoxOpOI( - boxopGamepad, shoulder, elevator, wrist, intake, queuedControl)); - - addRule( - "Outtake Coral", - leftJoystick.whenButton(InputConstants.JOYSTICK_RIGHT_BUTTON), - ONCE_AND_HOLD, - () -> new OuttakeCoral(intake)) - .withFinishedTriggeringProcedure(intake, () -> intake.stop()); + addRules(new DriverOI(boxopGamepad, swerveDrive)); + // addRules( + // new BoxOpOI( + // boxopGamepad, shoulder, elevator, wrist, climber, intake, queuedControl)); - addRule( + /*addRule( "Outtake Algae", - leftJoystick.whenButton(InputConstants.JOYSTICK_LEFT_BUTTON), + boxopGamepad.whenButton(InputConstants.TEMP), //TODO: algaeMode? variable ONCE_AND_HOLD, intake, () -> intake.setAlgaePower(-0.5)) .withFinishedTriggeringProcedure(intake, () -> intake.stop()); - - addRule("Wrist Nudge Up", leftJoystick.whenButton(2), ONCE, wrist, () -> wrist.nudgeUp()); + */ + addRule( + "Wrist Nudge Up", + // TODO: () -> syntax correctness + () -> boxopGamepad.getPOV() == InputConstants.GAMEPAD_DPAD_RIGHT, + ONCE, + wrist, + () -> wrist.nudgeUp()); addRule( "Wrist Nudge Down", - leftJoystick.whenButton(3), + () -> boxopGamepad.getPOV() == InputConstants.GAMEPAD_DPAD_LEFT, ONCE, wrist, () -> wrist.nudgeDown()); - - addRule( - "Elevator Nudge Up", + /* + addRule("Elevator Nudge Up", rightJoystick.whenButton(2), ONCE, elevator, @@ -83,6 +91,329 @@ public OI( ONCE, elevator, () -> elevator.nudgeDown()); + */ + // CLIMBER + + // addRule( + // "Enable Climber", + // boxopGamepad.whenButton(InputConstants.GAMEPAD_BACK_BUTTON), + // ONCE, + // Set.of(climber, wrist, elevator, shoulder), + // () -> { + // elevator.setPosition( + // Elevator.ElevatorPosition.STOW); // previously MAXIMUM + // wrist.setPosition(Wrist.WristPosition.ALGAE_LOW); + // climber.setClimberSpeed(0.5); + // shoulder.setPosition(Shoulder.ShoulderPosition.CLIMBER); + // }) // TODO: make sure whenTriggering is the right rule & not going to + // break + // // anything + // .whenTriggering( + // new RuleGroup() { + // { + // addRule( + // "Move Climber", + // + // boxopGamepad.whenButton(InputConstants.GAMEPAD_DPAD_DOWN), + // ONCE, + // Set.of(climber, wrist, elevator, shoulder), + // () -> { + // climber.stop(); + // shoulder.setPosition(ShoulderPosition.CORAL_GROUND); + // }); + // } + // }); + + addRule("Toggle Coral/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.getPOV() + == 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.getPOV() + == 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.getPOV() + == InputConstants + .GAMEPAD_LEFT_TRIGGER, + ONCE_AND_HOLD, + Set.of(intake), + () -> intake.turnAlgaePositive()) + .withFinishedTriggeringProcedure( + intake, () -> intake.turnAlgaePositiveSlow()); + + addRule( + "Shoot Algae", + () -> + boxopGamepad.getPOV() + == 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.whenButton( + 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, + () -> + 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.whenAxisMoved( + 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", 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 853d7597..8744171f 100644 --- a/src/main/java/com/team766/robot/copy_2910/Robot.java +++ b/src/main/java/com/team766/robot/copy_2910/Robot.java @@ -5,7 +5,6 @@ import com.team766.hal.RobotConfigurator; 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; @@ -20,7 +19,7 @@ public class Robot implements RobotConfigurator { private SwerveDrive drive; private Intake intake; private Vision vision; - //private Climber climber; + // private Climber climber; private Elevator elevator; private Shoulder shoulder; private Wrist wrist; @@ -30,7 +29,7 @@ public void initializeMechanisms() { SwerveConfig swerveConfig = new SwerveConfig().withDistanceBetweenWheels(0.533); drive = new SwerveDrive(swerveConfig); intake = new Intake(); - //climber = new Climber(); + // climber = new Climber(); elevator = new Elevator(); vision = new Vision(); shoulder = new Shoulder(); 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 292ffe37..c54bfae8 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 @@ -43,6 +43,8 @@ public Elevator() { 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( @@ -61,6 +63,7 @@ public enum ElevatorPosition { L4(24), // -21.357 ALGAE_HIGH(12), ALGAE_LOW(7), + ALGAE_SHOOT(17.80), CORAL_GROUND(-0.25), ALGAE_GROUND(0.5), STOW(0.5), @@ -70,7 +73,8 @@ public enum ElevatorPosition { MAXIMUM(25), // Maximum height of the elevator, TODO: Adjust based on the actual // elevator's // maximum position - MINIMUM(-1.0); // 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; 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 78913802..3815d201 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,16 +16,18 @@ public class Intake extends MechanismWithStatus { private MotorController centerAlgaeMotor; - private static final double CORAL_THRESHOLD = 0.03; // TODO: Set this to a real value | Previously 0.12 + 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 + // 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.4; private double rightPower = 0.4; - private double algaePower = 0.3; //previously 0.5 + private double algaePower = 0.3; // previously 0.5 + private double algaeSlowPower = 0.1; public Intake() { leftCANRange = RobotProvider.instance.getTimeOfFlight("INTAKE.CANRange.left"); @@ -71,13 +73,13 @@ public boolean hasCoralInFrontCenter() { return frontCenterDistance < CORAL_THRESHOLD; } - public boolean hasCoralToOuttake() { - return frontCenterDistance < CORAL_OUTTAKE_THRESHOLD; - } - public boolean hasCoralInBackCenter() { return backCenterDistance < CORAL_THRESHOLD; } + + public boolean hasCoralToOuttake() { + return backCenterDistance < CORAL_OUTTAKE_THRESHOLD; + } } public void setLeft(double power) { @@ -124,6 +126,10 @@ public void turnAlgaePositive() { centerAlgaeMotor.set(algaePower); } + public void turnAlgaePositiveSlow() { + centerAlgaeMotor.set(algaeSlowPower); + } + public void turnAlgaeNegative() { centerAlgaeMotor.set(-algaePower); } 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 edee28e2..25fccdec 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,10 +1,12 @@ 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.EncoderReader; import com.team766.hal.MotorController; import com.team766.hal.RobotProvider; +import com.team766.library.ValueProvider; import edu.wpi.first.math.MathUtil; public class Shoulder extends MechanismWithStatus { @@ -14,9 +16,12 @@ public class Shoulder extends MechanismWithStatus { 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 + 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; @@ -62,6 +67,7 @@ public double getPosition() { } public Shoulder() { + noPIDMode = false; leftMotor = RobotProvider.instance.getMotor( "LeftShoulderMotor"); // Replace with actual motor name @@ -72,12 +78,13 @@ public Shoulder() { RobotProvider.instance.getEncoder( "ShoulderEncoder"); // **ShoulderEncoder may not exist** + // leftMotor.setInverted(true); // leftMotor.setInverted(true); rightMotor.follow(leftMotor); leftMotor.setCurrentLimit(40); rightMotor.setCurrentLimit(40); - + ffGain = ConfigFileReader.getInstance().getDouble("Shoulder_FFGain"); // ffGain = // ConfigFileReader.instance.getDouble( // "ShoulderFFGain"); // Replace with actual config key @@ -85,6 +92,7 @@ public Shoulder() { } public void setSetpoint(double setpoint) { + noPIDMode = false; setPoint = MathUtil.clamp( setpoint, @@ -98,7 +106,15 @@ public void setPosition(ShoulderPosition position) { public void run() { // leftMotor.set(.Position, setPoint) - leftMotor.set(MotorController.ControlMode.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()); } @@ -111,6 +127,16 @@ public void nudgeDown() { } 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 { 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 e88c1cdc..1b0babe2 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 @@ -1,20 +1,23 @@ 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.EncoderReader; import com.team766.hal.MotorController; import com.team766.hal.RobotProvider; +import com.team766.library.ValueProvider; import edu.wpi.first.math.MathUtil; 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 double gearRatio = 21.8; - 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 @@ -37,15 +40,16 @@ public boolean isNearTo(double angle) { } public enum WristPosition { - L1(-16.643),//16.643 + L1(-16.643), // 16.643 L2(1.095), - L3(-0.808), //-0.808 + L3(-0.808), // -0.808 L4(-5.561), ALGAE_HIGH(-25.928), ALGAE_LOW(-25.928), CORAL_GROUND(-9.75), ALGAE(-25.2), ALGAE_GROUND(-21.786), + ALGAE_SHOOT(0.643), STOW(-1.0), MAXIMUM(50), MINIMUM(-30); @@ -63,9 +67,15 @@ public double getPosition() { public Wrist() { motor = RobotProvider.instance.getMotor("WristMotor"); // Replace with actual motor name + ffGain = + ConfigFileReader.getInstance() + .getDouble("Wrist_FFGain"); // ** Wrist_FFGain does not exist yet ** + noPIDMode = false; absoluteEncoder = - RobotProvider.instance.getEncoder("WristEncoder"); // **WristEncoder may not exist** + RobotProvider.instance.getEncoder( + "WristEncoder"); // ** WristEncoder may not exist ** motor.setCurrentLimit(40); + // ffGain = // ConfigFileReader.instance.getDouble( // "WristFFGain"); // Replace with actual config key @@ -73,6 +83,7 @@ public Wrist() { } public void setSetpoint(double setpoint) { + noPIDMode = false; setPoint = MathUtil.clamp( setpoint, @@ -85,7 +96,17 @@ public void setPosition(WristPosition wristPosition) { } public void run() { - motor.set(MotorController.ControlMode.Position, setPoint); + 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() { @@ -97,6 +118,16 @@ public void nudgeDown() { } 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 { 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 b5eff10a..71e95d04 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 @@ -46,7 +46,9 @@ public void run(Context context) { + status.get().getLeftDistance() + "Back center:" + status.get().getBackCenterDistance()); - while (waitForStatusMatchingOrTimeout(context, Intake.IntakeStatus.class, s -> !s.hasCoralInBackCenter(), 0.35).isPresent()) { + while (waitForStatusMatchingOrTimeout( + context, Intake.IntakeStatus.class, s -> !s.hasCoralInBackCenter(), 0.35) + .isPresent()) { context.yield(); intake.turnAlgaePositive(); status = getStatus(Intake.IntakeStatus.class); @@ -66,7 +68,6 @@ public void run(Context context) { if (hasCoralInLeft && hasCoralInRight && hasCoralInFrontCenter) { intake.turnLeftPositive(); intake.turnRightPositive(); - continue; } /* 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 00000000..281c7858 --- /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/OuttakeCoral.java b/src/main/java/com/team766/robot/copy_2910/procedures/OuttakeCoral.java index 290fd48a..546d0254 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 @@ -15,8 +15,7 @@ public OuttakeCoral(Intake intake2) { @Override public void run(Context context) { intake.setLeft(1.5); - intake.setRight(-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 00000000..f3c43f39 --- /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(); + } +} From 2e4a8b91805c0364d66d08b24c2a1a24b86bec61 Mon Sep 17 00:00:00 2001 From: DS Date: Thu, 23 Oct 2025 16:35:26 -0700 Subject: [PATCH 20/21] 10/22 practice field --- .../common/constants/ControlConstants.java | 10 +++-- .../com/team766/robot/copy_2910/DriverOI.java | 15 +++---- .../java/com/team766/robot/copy_2910/OI.java | 39 ++++++++----------- .../robot/copy_2910/mechanisms/Intake.java | 4 +- .../robot/copy_2910/mechanisms/Wrist.java | 10 ++--- 5 files changed, 38 insertions(+), 40 deletions(-) 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 4c89acad..a73c263a 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 = 0.25; // 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 diff --git a/src/main/java/com/team766/robot/copy_2910/DriverOI.java b/src/main/java/com/team766/robot/copy_2910/DriverOI.java index db907f05..606e8a5a 100644 --- a/src/main/java/com/team766/robot/copy_2910/DriverOI.java +++ b/src/main/java/com/team766/robot/copy_2910/DriverOI.java @@ -15,16 +15,17 @@ public DriverOI(JoystickReader gamepad, SwerveDrive drive) { // TODO: make sure stick click works well addRule( "Reset Gyro", - gamepad.whenButton(InputConstants.GAMEPAD_LEFT_STICK_CLICK), + gamepad.whenButton(InputConstants.GAMEPAD_BACK_BUTTON), ONCE, drive, () -> drive.resetGyro()); - addRule( - "Reset Pos", - gamepad.whenButton(InputConstants.GAMEPAD_RIGHT_STICK_CLICK), - ONCE, - drive, - () -> drive.resetCurrentPosition()); + + // 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( diff --git a/src/main/java/com/team766/robot/copy_2910/OI.java b/src/main/java/com/team766/robot/copy_2910/OI.java index 322a09ae..73778af5 100644 --- a/src/main/java/com/team766/robot/copy_2910/OI.java +++ b/src/main/java/com/team766/robot/copy_2910/OI.java @@ -124,7 +124,9 @@ public OI( // } // }); - addRule("Toggle Coral/Algae Mode", new Conditions.Toggle(() -> boxopGamepad.getPOV() == 0)) + 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( @@ -132,9 +134,7 @@ public OI( { addRule( "Move Algae Intake to Low Position", - () -> - boxopGamepad.getPOV() - == InputConstants.GAMEPAD_A_BUTTON, + boxopGamepad.whenButton(InputConstants.GAMEPAD_A_BUTTON), ONCE, Set.of(elevator, shoulder, intake, wrist), (context) -> { @@ -155,9 +155,7 @@ public OI( addRule( "Move Algae Intake to High Position", - () -> - boxopGamepad.getPOV() - == InputConstants.GAMEPAD_Y_BUTTON, + boxopGamepad.whenButton(InputConstants.GAMEPAD_Y_BUTTON), ONCE, Set.of(elevator, shoulder, intake, wrist), (context) -> { @@ -178,22 +176,18 @@ public OI( addRule( "Intake Algae", - () -> - boxopGamepad.getPOV() - == InputConstants - .GAMEPAD_LEFT_TRIGGER, + boxopGamepad.whenAxisMoved( + InputConstants.GAMEPAD_LEFT_TRIGGER), ONCE_AND_HOLD, Set.of(intake), - () -> intake.turnAlgaePositive()) + () -> intake.turnAlgaeNegative()) .withFinishedTriggeringProcedure( - intake, () -> intake.turnAlgaePositiveSlow()); + intake, () -> intake.retainAlgae()); addRule( "Shoot Algae", - () -> - boxopGamepad.getPOV() - == InputConstants - .GAMEPAD_RIGHT_TRIGGER, + boxopGamepad.whenAxisMoved( + InputConstants.GAMEPAD_RIGHT_TRIGGER), ONCE_AND_HOLD, Set.of(elevator, shoulder, intake, wrist), (context) -> { @@ -290,7 +284,7 @@ public OI( addRule( "Outtake Coral", - boxopGamepad.whenButton( + boxopGamepad.whenAxisMoved( InputConstants.GAMEPAD_RIGHT_TRIGGER), ONCE_AND_HOLD, () -> new OuttakeCoral(intake)) @@ -301,9 +295,10 @@ public OI( boxopGamepad.whenAxisMoved( InputConstants.GAMEPAD_LEFT_TRIGGER), ONCE_AND_HOLD, - () -> - new IntakeCoral( - intake, elevator, shoulder, wrist)) + () -> { + return new IntakeCoral( + intake, elevator, shoulder, wrist); + }) .withFinishedTriggeringProcedure( Set.of(intake, elevator, wrist, shoulder), () -> { @@ -314,7 +309,7 @@ public OI( }); addRule( "L1 Ground Intake", - boxopGamepad.whenAxisMoved( + boxopGamepad.whenButton( InputConstants.GAMEPAD_RIGHT_BUMPER_BUTTON), ONCE_AND_HOLD, () -> 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 3815d201..4bd139f8 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 @@ -27,7 +27,7 @@ public class Intake extends MechanismWithStatus { private double rightPower = 0.4; private double algaePower = 0.3; // previously 0.5 - private double algaeSlowPower = 0.1; + private double algaeSlowPower = -0.05; public Intake() { leftCANRange = RobotProvider.instance.getTimeOfFlight("INTAKE.CANRange.left"); @@ -126,7 +126,7 @@ public void turnAlgaePositive() { centerAlgaeMotor.set(algaePower); } - public void turnAlgaePositiveSlow() { + public void retainAlgae() { centerAlgaeMotor.set(algaeSlowPower); } 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 1b0babe2..25b68b0f 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 @@ -50,7 +50,7 @@ public enum WristPosition { ALGAE(-25.2), ALGAE_GROUND(-21.786), ALGAE_SHOOT(0.643), - STOW(-1.0), + STOW(0), MAXIMUM(50), MINIMUM(-30); @@ -70,7 +70,7 @@ public Wrist() { ffGain = ConfigFileReader.getInstance() .getDouble("Wrist_FFGain"); // ** Wrist_FFGain does not exist yet ** - noPIDMode = false; + noPIDMode = true; absoluteEncoder = RobotProvider.instance.getEncoder( "WristEncoder"); // ** WristEncoder may not exist ** @@ -83,7 +83,7 @@ public Wrist() { } public void setSetpoint(double setpoint) { - noPIDMode = false; + noPIDMode = true; setPoint = MathUtil.clamp( setpoint, @@ -137,11 +137,11 @@ public void nudgeNoPID(double input) { @Override protected WristStatus updateStatus() { - /*if (!encoderInitialized && absoluteEncoder.isConnected()) { + if (!encoderInitialized && absoluteEncoder.isConnected()) { double motorRotations = absoluteEncoder.getPosition() * gearRatio; motor.setSensorPosition(motorRotations); encoderInitialized = true; - } */ + } return new WristStatus(motor.getSensorPosition(), setPoint); } } From 13cfb2898ac3af3387cd96a6fe3893af86798916 Mon Sep 17 00:00:00 2001 From: Max Spier <68516760+maxspier@users.noreply.github.com> Date: Sat, 1 Nov 2025 14:08:20 -0700 Subject: [PATCH 21/21] push code from comp --- .../common/constants/ControlConstants.java | 4 +- .../java/com/team766/robot/copy_2910/OI.java | 56 +++++++++---------- .../com/team766/robot/copy_2910/Robot.java | 7 ++- .../robot/copy_2910/mechanisms/Elevator.java | 3 +- .../robot/copy_2910/mechanisms/Shoulder.java | 11 +++- .../robot/copy_2910/mechanisms/Wrist.java | 3 +- .../copy_2910/procedures/DriveStraight.java | 2 +- .../copy_2910/procedures/OuttakeCoral.java | 4 +- .../robot/reva_2025/mechanisms/Vision.java | 4 +- 9 files changed, 51 insertions(+), 43 deletions(-) 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 a73c263a..d6b53816 100644 --- a/src/main/java/com/team766/robot/common/constants/ControlConstants.java +++ b/src/main/java/com/team766/robot/common/constants/ControlConstants.java @@ -15,7 +15,7 @@ public class ControlConstants { // Exponent giving joystick curved power mapping for rotational movement // 1.0 for flight sticks - public static final double ROTATIONAL_CURVE_POWER = 0.25; // was 1.0 + public static final double ROTATIONAL_CURVE_POWER = 1.0; // was 1.0 /** * Translational velocity of robot that max joystick power controls in m/s @@ -31,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/copy_2910/OI.java b/src/main/java/com/team766/robot/copy_2910/OI.java index 73778af5..465e261d 100644 --- a/src/main/java/com/team766/robot/copy_2910/OI.java +++ b/src/main/java/com/team766/robot/copy_2910/OI.java @@ -2,6 +2,7 @@ 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; @@ -10,6 +11,7 @@ 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; @@ -39,6 +41,7 @@ public OI( Wrist wrist, Elevator elevator, Shoulder shoulder, + Climber climber, Vision vision) { // final JoystickReader leftJoystick = RobotProvider.instance.getJoystick(0); @@ -94,35 +97,30 @@ public OI( */ // CLIMBER - // addRule( - // "Enable Climber", - // boxopGamepad.whenButton(InputConstants.GAMEPAD_BACK_BUTTON), - // ONCE, - // Set.of(climber, wrist, elevator, shoulder), - // () -> { - // elevator.setPosition( - // Elevator.ElevatorPosition.STOW); // previously MAXIMUM - // wrist.setPosition(Wrist.WristPosition.ALGAE_LOW); - // climber.setClimberSpeed(0.5); - // shoulder.setPosition(Shoulder.ShoulderPosition.CLIMBER); - // }) // TODO: make sure whenTriggering is the right rule & not going to - // break - // // anything - // .whenTriggering( - // new RuleGroup() { - // { - // addRule( - // "Move Climber", - // - // boxopGamepad.whenButton(InputConstants.GAMEPAD_DPAD_DOWN), - // ONCE, - // Set.of(climber, wrist, elevator, shoulder), - // () -> { - // climber.stop(); - // shoulder.setPosition(ShoulderPosition.CORAL_GROUND); - // }); - // } - // }); + 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", 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 8744171f..04a50bcd 100644 --- a/src/main/java/com/team766/robot/copy_2910/Robot.java +++ b/src/main/java/com/team766/robot/copy_2910/Robot.java @@ -5,6 +5,7 @@ import com.team766.hal.RobotConfigurator; 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; @@ -19,7 +20,7 @@ public class Robot implements RobotConfigurator { private SwerveDrive drive; private Intake intake; private Vision vision; - // private Climber climber; + private Climber climber; private Elevator elevator; private Shoulder shoulder; private Wrist wrist; @@ -29,7 +30,7 @@ public void initializeMechanisms() { SwerveConfig swerveConfig = new SwerveConfig().withDistanceBetweenWheels(0.533); drive = new SwerveDrive(swerveConfig); intake = new Intake(); - // climber = new Climber(); + climber = new Climber(); elevator = new Elevator(); vision = new Vision(); shoulder = new Shoulder(); @@ -38,7 +39,7 @@ public void initializeMechanisms() { @Override public RuleEngine createOI() { - return new OI(drive, intake, wrist, elevator, shoulder, vision); + return new OI(drive, intake, wrist, elevator, shoulder, climber, vision); } @Override 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 c54bfae8..79cc0b9a 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 @@ -64,9 +64,10 @@ public enum ElevatorPosition { ALGAE_HIGH(12), ALGAE_LOW(7), ALGAE_SHOOT(17.80), - CORAL_GROUND(-0.25), + 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 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 25fccdec..93a7849b 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,5 +1,6 @@ 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; @@ -50,7 +51,7 @@ public enum ShoulderPosition { ALGAE_LOW(20.405), CORAL_GROUND(1), ALGAE_GROUND(4.119), - CLIMBER(28), + CLIMBER(100), STOW(0), MAXIMUM(100), MINIMUM(-10); @@ -77,7 +78,8 @@ public Shoulder() { 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); @@ -104,6 +106,11 @@ public void setPosition(ShoulderPosition position) { setSetpoint(position.getPosition()); } + public void setBrakeMode() { + leftMotor.setNeutralMode(NeutralMode.Brake); + rightMotor.setNeutralMode(NeutralMode.Brake); + } + public void run() { // leftMotor.set(.Position, setPoint) // All of the following PID code is directly copied from the 2910 wrist code -> may not work 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 25b68b0f..3c9bbcf0 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 @@ -52,7 +52,8 @@ public enum WristPosition { ALGAE_SHOOT(0.643), STOW(0), MAXIMUM(50), - MINIMUM(-30); + MINIMUM(-30), + CLIMB(-11); private final double angle; 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 index d125558d..a789e5c4 100644 --- a/src/main/java/com/team766/robot/copy_2910/procedures/DriveStraight.java +++ b/src/main/java/com/team766/robot/copy_2910/procedures/DriveStraight.java @@ -13,7 +13,7 @@ public DriveStraight(SwerveDrive drive) { @Override public void run(Context context) { - context.waitForSeconds(2); + // 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/OuttakeCoral.java b/src/main/java/com/team766/robot/copy_2910/procedures/OuttakeCoral.java index 546d0254..5f2bbde1 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 @@ -16,7 +16,7 @@ public OuttakeCoral(Intake intake2) { public void run(Context context) { intake.setLeft(1.5); intake.setRight(-1.5); - waitForStatusMatching(context, Intake.IntakeStatus.class, s -> !s.hasCoralToOuttake()); - intake.stop(); + // waitForStatusMatching(context, Intake.IntakeStatus.class, s -> !s.hasCoralToOuttake()); + // intake.stop(); } } 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 50c0ba7b..bef22e75 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) }; }