new claw code for the 3rd time

fusion479 · Dec 17, 2024 · 600cbd1 · 600cbd1
1 parent 477e89a
commit 600cbd1
Show file tree

Hide file tree

Showing 6 changed files with 87 additions and 48 deletions.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommandRobot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommandRobot.java
@@ -11,10 +11,11 @@
 import com.qualcomm.robotcore.hardware.Gamepad;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
-import org.firstinspires.ftc.teamcode.commands.claw.ClawClose;
-import org.firstinspires.ftc.teamcode.commands.claw.ClawOpen;
 import org.firstinspires.ftc.teamcode.commands.claw.ClawPivotAccepting;
 import org.firstinspires.ftc.teamcode.commands.claw.ClawPivotScore;
+import org.firstinspires.ftc.teamcode.commands.claw.ClawRotateDown;
+import org.firstinspires.ftc.teamcode.commands.claw.ClawRotateReady;
+import org.firstinspires.ftc.teamcode.commands.claw.ClawRotateUp;
 import org.firstinspires.ftc.teamcode.commands.extendo.ExtendoAccepting;
 import org.firstinspires.ftc.teamcode.commands.extendo.ExtendoReady;
 import org.firstinspires.ftc.teamcode.commands.extendo.ExtendoScore;
@@ -30,11 +31,14 @@
 import org.firstinspires.ftc.teamcode.subsystems.Drivetrain;
 import org.firstinspires.ftc.teamcode.subsystems.Extendo;
 import org.firstinspires.ftc.teamcode.subsystems.Lift;
+import org.firstinspires.ftc.teamcode.utils.commands.GamepadTrigger;
 import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore;
 
 @Config
 public class CommandRobot {
-    public Command ready, accepting, highBasket, highRung, lowBasket, lowRung, liftIncrement, liftDecrement, open, close, slamdown, specimen;
+    public Command ready, accepting, highBasket, highRung, lowBasket, lowRung, liftIncrement, liftDecrement, slamdown, specimen;
+    public GamepadTrigger intakeAccept, intakeReject;
+
     private TeleOpMode mode;
 
     private final MultipleTelemetry telemetry;
@@ -98,8 +102,8 @@ public void startThreads() {
     public void configureCommands() {
         this.ready = new SequentialCommandGroup(
                 new LiftAccepting(this.telemetry, this.lift),
-                new ClawClose(this.telemetry, this.claw),
-                new ClawPivotScore(this.telemetry, this.claw),
+                new ClawRotateReady(this.telemetry, this.claw),
+                new ClawPivotAccepting(this.telemetry, this.claw),
                 new WaitCommand(CommandRobot.CLAW_RETRACT_DELAY),
                 new ExtendoReady(this.telemetry, this.extendo)
         );
@@ -108,48 +112,53 @@ public void configureCommands() {
                 new LiftAccepting(this.telemetry, this.lift),
                 new ExtendoAccepting(this.telemetry, this.extendo),
                 new WaitCommand(CommandRobot.CLAW_ACCEPT_DELAY),
-                new ClawOpen(this.telemetry, this.claw),
+                new ClawRotateDown(this.telemetry, this.claw),
                 new ClawPivotAccepting(this.telemetry, this.claw)
         );
 
         this.highBasket = new SequentialCommandGroup(
                 new LiftHighBasket(this.telemetry, this.lift),
                 new ExtendoScore(this.telemetry, this.extendo),
-                new ClawPivotScore(this.telemetry, this.claw)
+                new ClawPivotScore(this.telemetry, this.claw),
+                new ClawRotateUp(this.telemetry, this.claw)
         );
 
         this.lowBasket = new SequentialCommandGroup(
                 new LiftLowBasket(this.telemetry, this.lift),
                 new ExtendoScore(this.telemetry, this.extendo),
-                new ClawPivotScore(this.telemetry, this.claw)
+                new ClawPivotScore(this.telemetry, this.claw),
+                new ClawRotateUp(this.telemetry, this.claw)
         );
 
         this.highRung = new SequentialCommandGroup(
                 new LiftHighRung(this.telemetry, this.lift),
                 new ExtendoScore(this.telemetry, this.extendo),
-                new ClawPivotScore(this.telemetry, this.claw)
+                new ClawPivotScore(this.telemetry, this.claw),
+                new ClawRotateUp(this.telemetry, this.claw)
         );
 
         this.lowRung = new SequentialCommandGroup(
                 new LiftLowRung(this.telemetry, this.lift),
                 new ExtendoScore(this.telemetry, this.extendo),
-                new ClawPivotScore(this.telemetry, this.claw)
+                new ClawPivotScore(this.telemetry, this.claw),
+                new ClawRotateUp(this.telemetry, this.claw)
+
         );
 
         this.specimen = new SequentialCommandGroup(
                 new LiftLowRung(this.telemetry, this.lift),
                 new ExtendoReady(this.telemetry, this.extendo),
                 new ClawPivotScore(this.telemetry, this.claw),
-                new ClawOpen(this.telemetry, this.claw)
+                new ClawRotateReady(this.telemetry, this.claw)
         );
 
+        this.intakeAccept = new GamepadTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER, d -> this.claw.setClawPower(-d), this.gamepad2);
+        this.intakeReject = new GamepadTrigger(GamepadKeys.Trigger.LEFT_TRIGGER, this.claw::setClawPower, this.gamepad2);
+
         this.liftIncrement = new LiftIncrement(this.telemetry, this.lift);
 
         this.liftDecrement = new LiftDecrement(this.telemetry, this.lift);
 
-        this.open = new ClawOpen(this.telemetry, this.claw);
-
-        this.close = new ClawClose(this.telemetry, this.claw);
     }
 
     public void configureControls() {
@@ -175,10 +184,6 @@ public void configureControls() {
                         .whenPressed(this.highBasket);
                 this.gamepad1.getGamepadButton(GamepadKeys.Button.DPAD_LEFT)
                         .whenPressed(this.lowBasket);
-                this.gamepad1.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER)
-                        .whenPressed(this.open);
-                this.gamepad1.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER)
-                        .whenPressed(this.close);
                 break;
 
             /* ------------------------------------- */
@@ -202,10 +207,6 @@ public void configureControls() {
                         .whenPressed(this.lowRung);
                 this.gamepad1.getGamepadButton(GamepadKeys.Button.B)
                         .whenPressed(this.highRung);
-                this.gamepad1.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER)
-                        .whenPressed(this.open);
-                this.gamepad1.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER)
-                        .whenPressed(this.close);
                 break;
 
             /* ------------------------------------- */
@@ -229,11 +230,6 @@ public void configureControls() {
                         .whenPressed(this.lowRung);
                 this.gamepad2.getGamepadButton(GamepadKeys.Button.B)
                         .whenPressed(this.highRung);
-                this.gamepad1.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER)
-                        .whenPressed(this.open);
-                this.gamepad1.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER)
-                        .whenPressed(this.close);
-
                 break;
         }
     }

diff --git a/.../ftc/teamcode/commands/claw/ClawOpen.java → ...eamcode/commands/claw/ClawRotateDown.java b/.../ftc/teamcode/commands/claw/ClawOpen.java → ...eamcode/commands/claw/ClawRotateDown.java
@@ -5,11 +5,11 @@
 
 import org.firstinspires.ftc.teamcode.subsystems.Claw;
 
-public class ClawOpen extends CommandBase {
+public class ClawRotateDown extends CommandBase {
     private final MultipleTelemetry telemetry;
     private final Claw claw;
 
-    public ClawOpen(final MultipleTelemetry telemetry, final Claw claw) {
+    public ClawRotateDown(final MultipleTelemetry telemetry, final Claw claw) {
         this.telemetry = telemetry;
 
         this.claw = claw;
@@ -18,11 +18,12 @@ public ClawOpen(final MultipleTelemetry telemetry, final Claw claw) {
 
     @Override
     public void initialize() {
-        this.claw.setClawPosition(Claw.OPEN);
+        this.claw.setPivotPosition(Claw.DOWN);
     }
 
     @Override
     public boolean isFinished() {
         return true;
     }
 }
+
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/claw/ClawRotateReady.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/claw/ClawRotateReady.java
@@ -0,0 +1,28 @@
+package org.firstinspires.ftc.teamcode.commands.claw;
+
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.arcrobotics.ftclib.command.CommandBase;
+
+import org.firstinspires.ftc.teamcode.subsystems.Claw;
+
+public class ClawRotateReady extends CommandBase {
+    private final MultipleTelemetry telemetry;
+    private final Claw claw;
+
+    public ClawRotateReady(final MultipleTelemetry telemetry, final Claw claw) {
+        this.telemetry = telemetry;
+
+        this.claw = claw;
+        super.addRequirements(claw);
+    }
+
+    @Override
+    public void initialize() {
+        this.claw.setPivotPosition(Claw.READY);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return true;
+    }
+}
diff --git a/...ftc/teamcode/commands/claw/ClawClose.java → .../teamcode/commands/claw/ClawRotateUp.java b/...ftc/teamcode/commands/claw/ClawClose.java → .../teamcode/commands/claw/ClawRotateUp.java
@@ -5,11 +5,11 @@
 
 import org.firstinspires.ftc.teamcode.subsystems.Claw;
 
-public class ClawClose extends CommandBase {
-    private final Claw claw;
+public class ClawRotateUp extends CommandBase {
     private final MultipleTelemetry telemetry;
+    private final Claw claw;
 
-    public ClawClose(final MultipleTelemetry telemetry, final Claw claw) {
+    public ClawRotateUp(final MultipleTelemetry telemetry, final Claw claw) {
         this.telemetry = telemetry;
 
         this.claw = claw;
@@ -18,7 +18,7 @@ public ClawClose(final MultipleTelemetry telemetry, final Claw claw) {
 
     @Override
     public void initialize() {
-        this.claw.setClawPosition(Claw.CLOSE);
+        this.claw.setPivotPosition(Claw.UP);
     }
 
     @Override

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/ClawTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/test/ClawTest.java
@@ -5,10 +5,10 @@
 import com.arcrobotics.ftclib.gamepad.GamepadKeys;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
-import org.firstinspires.ftc.teamcode.commands.claw.ClawClose;
-import org.firstinspires.ftc.teamcode.commands.claw.ClawOpen;
 import org.firstinspires.ftc.teamcode.commands.claw.ClawPivotAccepting;
 import org.firstinspires.ftc.teamcode.commands.claw.ClawPivotScore;
+import org.firstinspires.ftc.teamcode.commands.claw.ClawRotateDown;
+import org.firstinspires.ftc.teamcode.commands.claw.ClawRotateUp;
 import org.firstinspires.ftc.teamcode.commands.extendo.ExtendoAccepting;
 import org.firstinspires.ftc.teamcode.commands.extendo.ExtendoScore;
 import org.firstinspires.ftc.teamcode.subsystems.Claw;
@@ -21,20 +21,22 @@ public class ClawTest extends OpModeCore {
     private Claw claw;
     private Extendo extendo;
     private GamepadEx gamepad;
-    private GamepadTrigger intake, outtake;
+    private GamepadTrigger intakeAccept, intakeReject;
 
     @Override
     public void initialize() {
         this.gamepad = new GamepadEx(super.gamepad1);
         this.claw = new Claw(super.hardwareMap, super.multipleTelemetry);
         this.extendo = new Extendo(super.hardwareMap, super.multipleTelemetry);
 
-        this.gamepad.getGamepadButton(GamepadKeys.Button.A).whenPressed(new ClawOpen(super.multipleTelemetry, this.claw));
-        this.gamepad.getGamepadButton(GamepadKeys.Button.Y).whenPressed(new ClawClose(super.multipleTelemetry, this.claw));
+        this.gamepad.getGamepadButton(GamepadKeys.Button.A).whenPressed(new ClawRotateDown(super.multipleTelemetry, this.claw));
+        this.gamepad.getGamepadButton(GamepadKeys.Button.Y).whenPressed(new ClawRotateDown(super.multipleTelemetry, this.claw));
+        this.gamepad.getGamepadButton(GamepadKeys.Button.DPAD_LEFT).whenPressed(new ClawRotateUp(super.multipleTelemetry, this.claw));
         this.gamepad.getGamepadButton(GamepadKeys.Button.B).whenPressed(new ClawPivotScore(super.multipleTelemetry, this.claw));
         this.gamepad.getGamepadButton(GamepadKeys.Button.X).whenPressed(new ClawPivotAccepting(super.multipleTelemetry, this.claw));
-        this.gamepad.getGamepadButton(GamepadKeys.Button.DPAD_UP).whenPressed(new ExtendoScore(super.multipleTelemetry, this.extendo));
-        this.gamepad.getGamepadButton(GamepadKeys.Button.DPAD_DOWN).whenPressed(new ExtendoAccepting(super.multipleTelemetry, this.extendo));
+
+        this.intakeAccept = new GamepadTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER, d -> this.claw.setClawPower(-d), this.gamepad);
+        this.intakeReject = new GamepadTrigger(GamepadKeys.Trigger.LEFT_TRIGGER, this.claw::setClawPower, this.gamepad);
     }
 
     @Override

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Claw.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Claw.java
@@ -4,37 +4,49 @@
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.arcrobotics.ftclib.command.SubsystemBase;
 import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.CRServo;
 import com.qualcomm.robotcore.hardware.Servo;
 
 @Config
 public class Claw extends SubsystemBase {
     public static double SCORE = 1;
     public static double ACCEPTING = 0.55;
 
-    public static double OPEN = 1;
-    public static double CLOSE = 0.49;
+    //TODO: Get positions for claw rotate
+    public static double UP = 1;
+    public static double DOWN = 0.49;
+    public static double READY = 0;
 
     private final MultipleTelemetry telemetry;
 
-    private final Servo leftPivot, rightPivot, claw;
+    private final Servo leftPivot, rightPivot, clawRotate;
+    private final CRServo leftClaw, rightClaw;
+
 
     public Claw(final HardwareMap hwMap, final MultipleTelemetry telemetry) {
         this.telemetry = telemetry;
 
         this.leftPivot = hwMap.get(Servo.class, "leftPivot");
         this.rightPivot = hwMap.get(Servo.class, "rightPivot");
-        this.claw = hwMap.get(Servo.class, "claw");
+        this.leftClaw = hwMap.get(CRServo.class, "leftClaw");
+        this.rightClaw = hwMap.get(CRServo.class, "rightClaw");
+        this.clawRotate = hwMap.get(Servo.class, "clawRotate");
 
         this.setPivotPosition(Claw.SCORE);
-        this.setClawPosition(Claw.CLOSE);
+        this.setRotatePosition(Claw.READY);
     }
 
     public void setPivotPosition(double position) {
         this.leftPivot.setPosition(position);
         this.rightPivot.setPosition(1 - position);
     }
 
-    public void setClawPosition(double position) {
-        this.claw.setPosition(position);
+    public void setRotatePosition(double position) {
+        this.clawRotate.setPosition(position);
+    }
+
+    public void setClawPower(double power) {
+        this.leftClaw.setPower(power);
+        this.rightClaw.setPower(-power);
     }
 }