start exporting commands

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommandRobot.java

@@ -11,10 +11,20 @@
 import com.qualcomm.robotcore.hardware.Gamepad;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
-import org.firstinspires.ftc.teamcode.commands.claw.ClawSetPivotPosition;
-import org.firstinspires.ftc.teamcode.commands.claw.ClawSetPosition;
-import org.firstinspires.ftc.teamcode.commands.extendo.ExtendoSetPosition;
-import org.firstinspires.ftc.teamcode.commands.lift.LiftSetPosition;
+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.extendo.ExtendoAccepting;
+import org.firstinspires.ftc.teamcode.commands.extendo.ExtendoReady;
+import org.firstinspires.ftc.teamcode.commands.extendo.ExtendoScore;
+import org.firstinspires.ftc.teamcode.commands.lift.LiftAccepting;
+import org.firstinspires.ftc.teamcode.commands.lift.LiftDecrement;
+import org.firstinspires.ftc.teamcode.commands.lift.LiftHighBasket;
+import org.firstinspires.ftc.teamcode.commands.lift.LiftHighRung;
+import org.firstinspires.ftc.teamcode.commands.lift.LiftIncrement;
+import org.firstinspires.ftc.teamcode.commands.lift.LiftLowBasket;
+import org.firstinspires.ftc.teamcode.commands.lift.LiftLowRung;
 import org.firstinspires.ftc.teamcode.roadrunner.MecanumDrive;
 import org.firstinspires.ftc.teamcode.subsystems.Claw;
 import org.firstinspires.ftc.teamcode.subsystems.Drivetrain;
@@ -24,13 +34,12 @@
 
 public class CommandRobot {
     private final MultipleTelemetry telemetry;
-
-    private final Drivetrain drivetrain;
     private final Lift lift;
     private final Extendo extendo;
     private final Claw claw;
     private final OpModeCore opMode;
-    public Command ready, accepting, highBasket, highRung, lowBasket, lowRung, score, liftIncrement, liftDecrement, extendoIncrement, extendoDecrement, open, close;
+    public Command ready, accepting, highBasket, highRung, lowBasket, lowRung, score, liftIncrement, liftDecrement, open, close;
+    private Drivetrain drivetrain;
     private GamepadEx gamepad1;
     private GamepadEx gamepad2;
 
@@ -56,7 +65,6 @@ public CommandRobot(HardwareMap hwMap, MultipleTelemetry telemetry, Gamepad game
     public CommandRobot(HardwareMap hwMap, Pose2d startPose, MultipleTelemetry telemetry, OpModeCore opMode) {
         this.telemetry = telemetry;
 
-        this.drivetrain = new Drivetrain(hwMap, telemetry, startPose);
         this.lift = new Lift(hwMap, telemetry);
         this.extendo = new Extendo(hwMap, telemetry);
         this.claw = new Claw(hwMap, telemetry);
@@ -78,61 +86,57 @@ public void startThreads() {
 
     public void configureCommands() {
         this.ready = new SequentialCommandGroup(
-                new ClawSetPivotPosition(this.telemetry, this.claw, Claw.READY),
+                new ClawPivotScore(this.telemetry, this.claw),
                 new WaitCommand(100),
-                new ExtendoSetPosition(this.telemetry, this.extendo, Extendo.READY),
-                new LiftSetPosition(this.telemetry, this.lift, Lift.ACCEPTING)
+                new ExtendoReady(this.telemetry, this.extendo),
+                new LiftAccepting(this.telemetry, this.lift)
         );
 
         this.accepting = new SequentialCommandGroup(
-                new LiftSetPosition(this.telemetry, this.lift, Lift.ACCEPTING),
-                new ExtendoSetPosition(this.telemetry, this.extendo, Extendo.ACCEPTING),
+                new LiftAccepting(this.telemetry, this.lift),
+                new ExtendoAccepting(this.telemetry, this.extendo),
                 new WaitCommand(100),
-                new ClawSetPivotPosition(this.telemetry, this.claw, Claw.ACCEPTING)
+                new ClawPivotAccepting(this.telemetry, this.claw)
         );
 
         this.highBasket = new ParallelCommandGroup(
-                new LiftSetPosition(this.telemetry, this.lift, Lift.HIGH_BASKET),
-                new ExtendoSetPosition(this.telemetry, this.extendo, Extendo.SCORE),
-                new ClawSetPivotPosition(this.telemetry, this.claw, Claw.SCORE)
+                new LiftHighBasket(this.telemetry, this.lift),
+                new ExtendoScore(this.telemetry, this.extendo),
+                new ClawPivotScore(this.telemetry, this.claw)
         );
 
         this.lowBasket = new ParallelCommandGroup(
-                new LiftSetPosition(this.telemetry, this.lift, Lift.LOW_BASKET),
-                new ExtendoSetPosition(this.telemetry, this.extendo, Extendo.SCORE),
-                new ClawSetPivotPosition(this.telemetry, this.claw, Claw.SCORE)
+                new LiftLowBasket(this.telemetry, this.lift),
+                new ExtendoScore(this.telemetry, this.extendo),
+                new ClawPivotScore(this.telemetry, this.claw)
         );
 
         this.highRung = new SequentialCommandGroup(
-                new LiftSetPosition(this.telemetry, this.lift, Lift.HIGH_RUNG),
-                new ExtendoSetPosition(this.telemetry, this.extendo, Extendo.SCORE),
-                new ClawSetPivotPosition(this.telemetry, this.claw, Claw.SCORE)
+                new LiftHighRung(this.telemetry, this.lift),
+                new ExtendoScore(this.telemetry, this.extendo),
+                new ClawPivotScore(this.telemetry, this.claw)
         );
 
         this.lowRung = new SequentialCommandGroup(
-                new LiftSetPosition(this.telemetry, this.lift, Lift.LOW_RUNG),
-                new ExtendoSetPosition(this.telemetry, this.extendo, Extendo.SCORE),
-                new ClawSetPivotPosition(this.telemetry, this.claw, Claw.SCORE)
+                new LiftLowRung(this.telemetry, this.lift),
+                new ExtendoScore(this.telemetry, this.extendo),
+                new ClawPivotScore(this.telemetry, this.claw)
         );
 
         this.score = new SequentialCommandGroup(
                 new WaitCommand(800),
-                new ClawSetPivotPosition(this.telemetry, this.claw, Claw.READY),
-                new ExtendoSetPosition(this.telemetry, this.extendo, Extendo.READY),
-                new LiftSetPosition(this.telemetry, this.lift, Lift.ACCEPTING)
+                new ClawPivotScore(this.telemetry, this.claw),
+                new ExtendoReady(this.telemetry, this.extendo),
+                new LiftAccepting(this.telemetry, this.lift)
         );
 
-        this.liftIncrement = new LiftSetPosition(this.telemetry, this.lift, this.lift.getTarget() + Lift.INCREMENT);
-
-        this.liftDecrement = new LiftSetPosition(this.telemetry, this.lift, this.lift.getTarget() - Lift.INCREMENT);
-
-        this.extendoIncrement = new ExtendoSetPosition(this.telemetry, this.extendo, this.extendo.getPosition() + Extendo.INCREMENT);
+        this.liftIncrement = new LiftIncrement(this.telemetry, this.lift);
 
-        this.extendoDecrement = new ExtendoSetPosition(this.telemetry, this.extendo, this.extendo.getPosition() - Extendo.INCREMENT);
+        this.liftDecrement = new LiftDecrement(this.telemetry, this.lift);
 
-        this.open = new ClawSetPosition(this.telemetry, this.claw, Claw.OPEN);
+        this.open = new ClawOpen(this.telemetry, this.claw);
 
-        this.close = new ClawSetPosition(this.telemetry, this.claw, Claw.CLOSE);
+        this.close = new ClawClose(this.telemetry, this.claw);
     }
 
     // TODO: Configure controls for gamepad (talk with drive team)
@@ -155,10 +159,6 @@ public void configureControls() {
                 .whenPressed(this.lowRung);
         this.gamepad2.getGamepadButton(GamepadKeys.Button.B)
                 .whenPressed(this.highRung);
-        this.gamepad1.getGamepadButton(GamepadKeys.Button.DPAD_DOWN)
-                .whenPressed(this.extendoDecrement);
-        this.gamepad1.getGamepadButton(GamepadKeys.Button.DPAD_UP)
-                .whenPressed(this.extendoIncrement);
         this.gamepad1.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER)
                 .whenPressed(this.open);
         this.gamepad1.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER)

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/claw/ClawSetPosition.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/claw/ClawClose.java

@@ -5,21 +5,19 @@
 
 import org.firstinspires.ftc.teamcode.subsystems.Claw;
 
-public class ClawSetPosition extends CommandBase {
-    private final MultipleTelemetry telemetry;
+public class ClawClose extends CommandBase {
     private final Claw claw;
-    private final double position;
+    private final MultipleTelemetry telemetry;
 
-    public ClawSetPosition(final MultipleTelemetry telemetry, final Claw claw, final double position) {
+    public ClawClose(final MultipleTelemetry telemetry, final Claw claw) {
         this.telemetry = telemetry;
-        this.position = position;
-        this.claw = claw;
 
+        this.claw = claw;
         super.addRequirements(claw);
     }
 
     @Override
     public void initialize() {
-        this.claw.setClawPosition(this.position);
+        this.claw.setClawPosition(Claw.CLOSE);
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/claw/ClawSetPivotPosition.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/claw/ClawOpen.java

@@ -5,26 +5,19 @@
 
 import org.firstinspires.ftc.teamcode.subsystems.Claw;
 
-public class ClawSetPivotPosition extends CommandBase {
+public class ClawOpen extends CommandBase {
     private final MultipleTelemetry telemetry;
     private final Claw claw;
-    private final double position;
 
-    public ClawSetPivotPosition(final MultipleTelemetry telemetry, final Claw claw, final double position) {
+    public ClawOpen(final MultipleTelemetry telemetry, final Claw claw) {
         this.telemetry = telemetry;
-        this.position = position;
 
         this.claw = claw;
         super.addRequirements(claw);
     }
 
     @Override
     public void initialize() {
-        this.claw.setPivotPosition(position);
-    }
-
-    @Override
-    public boolean isFinished() {
-        return true;
+        this.claw.setClawPosition(Claw.OPEN);
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/claw/ClawPivotAccepting.java

@@ -0,0 +1,23 @@
+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 ClawPivotAccepting extends CommandBase {
+    private final MultipleTelemetry telemetry;
+    private final Claw claw;
+
+    public ClawPivotAccepting(final MultipleTelemetry telemetry, final Claw claw) {
+        this.telemetry = telemetry;
+
+        this.claw = claw;
+        super.addRequirements(claw);
+    }
+
+    @Override
+    public void initialize() {
+        this.claw.setPivotPosition(Claw.ACCEPTING);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/claw/ClawPivotScore.java

@@ -0,0 +1,23 @@
+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 ClawPivotScore extends CommandBase {
+    private final MultipleTelemetry telemetry;
+    private final Claw claw;
+
+    public ClawPivotScore(final MultipleTelemetry telemetry, final Claw claw) {
+        this.telemetry = telemetry;
+
+        this.claw = claw;
+        super.addRequirements(claw);
+    }
+
+    @Override
+    public void initialize() {
+        this.claw.setPivotPosition(Claw.SCORE);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/extendo/ExtendoAccepting.java

@@ -0,0 +1,28 @@
+package org.firstinspires.ftc.teamcode.commands.extendo;
+
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.arcrobotics.ftclib.command.CommandBase;
+
+import org.firstinspires.ftc.teamcode.subsystems.Extendo;
+
+public class ExtendoAccepting extends CommandBase {
+    private final MultipleTelemetry telemetry;
+    private final Extendo extendo;
+
+    public ExtendoAccepting(final MultipleTelemetry telemetry, final Extendo extendo) {
+        this.telemetry = telemetry;
+
+        this.extendo = extendo;
+        super.addRequirements(extendo);
+    }
+
+    @Override
+    public void initialize() {
+        this.extendo.setPosition(Extendo.ACCEPTING);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return true;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/extendo/ExtendoReady.java

@@ -0,0 +1,28 @@
+package org.firstinspires.ftc.teamcode.commands.extendo;
+
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.arcrobotics.ftclib.command.CommandBase;
+
+import org.firstinspires.ftc.teamcode.subsystems.Extendo;
+
+public class ExtendoReady extends CommandBase {
+    private final MultipleTelemetry telemetry;
+    private final Extendo extendo;
+
+    public ExtendoReady(final MultipleTelemetry telemetry, final Extendo extendo) {
+        this.telemetry = telemetry;
+
+        this.extendo = extendo;
+        super.addRequirements(extendo);
+    }
+
+    @Override
+    public void initialize() {
+        this.extendo.setPosition(Extendo.READY);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return true;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/extendo/ExtendoScore.java

@@ -0,0 +1,28 @@
+package org.firstinspires.ftc.teamcode.commands.extendo;
+
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.arcrobotics.ftclib.command.CommandBase;
+
+import org.firstinspires.ftc.teamcode.subsystems.Extendo;
+
+public class ExtendoScore extends CommandBase {
+    private final MultipleTelemetry telemetry;
+    private final Extendo extendo;
+
+    public ExtendoScore(final MultipleTelemetry telemetry, final Extendo extendo) {
+        this.telemetry = telemetry;
+
+        this.extendo = extendo;
+        super.addRequirements(extendo);
+    }
+
+    @Override
+    public void initialize() {
+        this.extendo.setPosition(Extendo.SCORE);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return true;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftSetPosition.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftAccepting.java

@@ -6,22 +6,20 @@
 import org.firstinspires.ftc.teamcode.subsystems.Lift;
 
 
-public class LiftSetPosition extends CommandBase {
+public class LiftAccepting extends CommandBase {
     private final MultipleTelemetry telemetry;
     private final Lift lift;
-    private final double position;
 
-    public LiftSetPosition(final MultipleTelemetry telemetry, final Lift lift, final double position) {
+    public LiftAccepting(final MultipleTelemetry telemetry, final Lift lift) {
         this.telemetry = telemetry;
-        this.position = position;
 
         this.lift = lift;
         super.addRequirements(lift);
     }
 
     @Override
     public void initialize() {
-        this.lift.setTarget(position);
+        this.lift.setTarget(Lift.ACCEPTING);
     }
 
     @Override

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/lift/LiftDecrement.java

@@ -0,0 +1,29 @@
+package org.firstinspires.ftc.teamcode.commands.lift;
+
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.arcrobotics.ftclib.command.CommandBase;
+
+import org.firstinspires.ftc.teamcode.subsystems.Lift;
+
+
+public class LiftDecrement extends CommandBase {
+    private final MultipleTelemetry telemetry;
+    private final Lift lift;
+
+    public LiftDecrement(final MultipleTelemetry telemetry, final Lift lift) {
+        this.telemetry = telemetry;
+
+        this.lift = lift;
+        super.addRequirements(lift);
+    }
+
+    @Override
+    public void initialize() {
+        this.lift.setTarget(Lift.HIGH_BASKET);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return this.lift.isFinished();
+    }
+}