Gil - fixed conflicts in the middle east

GreenBlitz · Jun 19, 2024 · f1e61c8 · f1e61c8
2 parents af9a943 + b7d29ed
commit f1e61c8
Show file tree

Hide file tree

Showing 38 changed files with 634 additions and 62 deletions.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Alliance.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Alliance.java
@@ -5,14 +5,4 @@ public enum Alliance {
     BLUE,
     RED;
 
-    @Override
-    public String toString() {
-        if(this == Alliance.BLUE) {
-            return "BLUE";
-        }
-        else {
-            return "RED";
-        }
-    }
-
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Bindings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Bindings.java
@@ -3,11 +3,15 @@
 import com.arcrobotics.ftclib.gamepad.GamepadKeys;
 import com.qualcomm.robotcore.hardware.Gamepad;
 
+import org.firstinspires.ftc.teamcode.gamepads.GamepadFunctions;
 import org.firstinspires.ftc.teamcode.gamepads.GamepadWrapper;
-import org.firstinspires.ftc.teamcode.subsystems.launcher.LauncherCommands;
 import org.firstinspires.ftc.teamcode.subsystems.arm.ArmCommands;
 import org.firstinspires.ftc.teamcode.subsystems.arm.ArmState;
-import org.firstinspires.ftc.teamcode.gamepads.GamepadFunctions;
+import org.firstinspires.ftc.teamcode.subsystems.claw.ClawCommands;
+import org.firstinspires.ftc.teamcode.subsystems.wrist.WristCommands;
+import org.firstinspires.ftc.teamcode.subsystems.wrist.WristState;
+import org.firstinspires.ftc.teamcode.subsystems.chassis.ChassisCommands;
+import org.firstinspires.ftc.teamcode.subsystems.launcher.LauncherCommands;
 import org.firstinspires.ftc.teamcode.subsystems.elevator.ElevatorCommands;
 import org.firstinspires.ftc.teamcode.subsystems.elevator.ElevatorState;
 
@@ -16,27 +20,47 @@ public class Bindings {
     private static GamepadWrapper mainGamepad;
     private static GamepadWrapper secondGamepad;
 
-    public static void razLauncherTest(Gamepad gamepad1, Gamepad gamepad2) {
-        mainGamepad = new GamepadWrapper(gamepad1);
+    public static void razRobotStateTest(Gamepad gamepad){
+        mainGamepad = new GamepadWrapper(gamepad);
+
+        mainGamepad.getGamepadButton(GamepadKeys.Button.Y).whenPressed(Robot.getInstance().setState(RobotState.SCORE));
+        mainGamepad.getGamepadButton(GamepadKeys.Button.A).whenPressed(Robot.getInstance().setState(RobotState.INTAKE));
+        mainGamepad.getGamepadButton(GamepadKeys.Button.X).whenPressed(Robot.getInstance().setState(RobotState.CLIMB));
+        mainGamepad.getGamepadButton(GamepadKeys.Button.B).whenPressed(Robot.getInstance().setState(RobotState.DRIVE));
+    }
+
+    public static void razClawTest(Gamepad gamepad) {
+        mainGamepad = new GamepadWrapper(gamepad);
+
+        mainGamepad.getGamepadButton(GamepadKeys.Button.B).whenPressed(ClawCommands.toggleRightFinger());
+        mainGamepad.getGamepadButton(GamepadKeys.Button.X).whenPressed(ClawCommands.toggleLeftFinger());
+        mainGamepad.getGamepadButton(GamepadKeys.Button.A).whenPressed(ClawCommands.openBothFingers());
+        mainGamepad.getGamepadButton(GamepadKeys.Button.Y).whenPressed(ClawCommands.closeBothFingers());
+    }
+
+    public static void razLauncherTest(Gamepad gamepad) {
+        mainGamepad = new GamepadWrapper(gamepad);
 
         mainGamepad.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER).whenPressed(LauncherCommands.launchPlane());
     }
 
-    public static void razArmTest(Gamepad gamepad1, Gamepad gamepad2) {
-        mainGamepad = new GamepadWrapper(gamepad1);
+    public static void razArmTest(Gamepad gamepad) {
+        mainGamepad = new GamepadWrapper(gamepad);
 
         mainGamepad.getGamepadButton(GamepadKeys.Button.DPAD_UP).whenPressed(ArmCommands.goToState(ArmState.SCORE));
         mainGamepad.getGamepadButton(GamepadKeys.Button.DPAD_DOWN).whenPressed(ArmCommands.goToState(ArmState.INTAKE));
         mainGamepad.getGamepadButton(GamepadKeys.Button.DPAD_RIGHT).whenPressed(ArmCommands.goToState(ArmState.STARTING));
+        mainGamepad.getGamepadButton(GamepadKeys.Button.Y).whenPressed(ArmCommands.goToState(ArmState.IDLE));
         mainGamepad.getGamepadButton(GamepadKeys.Button.DPAD_LEFT).whenPressed(ArmCommands.goToState(ArmState.STAND_IN_PLACE));
     }
 
-    public static void razElevatorTest(Gamepad gamepad1, Gamepad gamepad2) {
-        mainGamepad = new GamepadWrapper(gamepad1);
+    public static void razElevatorTest(Gamepad gamepad) {
+        mainGamepad = new GamepadWrapper(gamepad);
 
         mainGamepad.getGamepadButton(GamepadKeys.Button.X).whenPressed(ElevatorCommands.goToState(ElevatorState.SCORE));
         mainGamepad.getGamepadButton(GamepadKeys.Button.Y).whenPressed(ElevatorCommands.goToState(ElevatorState.CLIMB));
         mainGamepad.getGamepadButton(GamepadKeys.Button.A).whenPressed(ElevatorCommands.goToState(ElevatorState.INTAKE));
+        mainGamepad.getGamepadButton(GamepadKeys.Button.B).whenPressed(ElevatorCommands.goToState(ElevatorState.IDLE));
         mainGamepad.getTriggerAsButton(GamepadKeys.Trigger.LEFT_TRIGGER).whenActive(
                 ElevatorCommands.humanControl(() -> {
                             double triggerValue = mainGamepad.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER);
@@ -53,4 +77,26 @@ public static void razElevatorTest(Gamepad gamepad1, Gamepad gamepad2) {
         );
     }
 
+    public static void razWristTest(Gamepad gamepad) {
+        mainGamepad = new GamepadWrapper(gamepad);
+
+        mainGamepad.getGamepadButton(GamepadKeys.Button.DPAD_DOWN).whenPressed(WristCommands.moveToState(WristState.SCORE));
+        mainGamepad.getGamepadButton(GamepadKeys.Button.DPAD_UP).whenPressed(WristCommands.moveToState(WristState.INTAKE));
+        mainGamepad.getGamepadButton(GamepadKeys.Button.DPAD_RIGHT).whenPressed(WristCommands.moveToState(WristState.IDLE));
+    }
+
+    public static void razChassisTest(Gamepad gamepad) {
+        mainGamepad = new GamepadWrapper(gamepad);
+
+        Robot.getInstance().getChassis().setDefaultCommand(
+                ChassisCommands.fieldCentricDrive(
+                        () -> -mainGamepad.getLeftX(),
+                        () -> -mainGamepad.getLeftY(),
+                        () -> -mainGamepad.getRightX()
+                )
+        );
+        mainGamepad.getGamepadButton(GamepadKeys.Button.Y).whenPressed(ChassisCommands.resetHeading());
+        mainGamepad.getGamepadButton(GamepadKeys.Button.DPAD_DOWN).whenPressed(ChassisCommands.stop());
+    }
+
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Location.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Location.java
@@ -5,23 +5,4 @@ public enum Location {
     LEFT, CENTER, RIGHT,
     FAR, CLOSE;
 
-    @Override
-    public String toString() {
-        if(this == Location.LEFT) {
-            return "LEFT";
-        }
-        else if(this == Location.CENTER) {
-            return "CENTER";
-        }
-        else if(this == Location.RIGHT) {
-            return "RIGHT";
-        }
-        else if(this == Location.FAR) {
-            return "FAR";
-        }
-        else {
-            return "CLOSE";
-        }
-    }
-
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java
@@ -1,11 +1,14 @@
 package org.firstinspires.ftc.teamcode;
 
+import com.arcrobotics.ftclib.command.SequentialCommandGroup;
 import com.qualcomm.robotcore.hardware.HardwareMap;
-
-import org.firstinspires.ftc.teamcode.subsystems.launcher.Launcher;
-import org.firstinspires.ftc.teamcode.subsystems.elevator.Elevator;
 import org.firstinspires.ftc.teamcode.subsystems.arm.Arm;
 import org.firstinspires.ftc.teamcode.subsystems.vision.Vision;
+import org.firstinspires.ftc.teamcode.subsystems.claw.Claw;
+import org.firstinspires.ftc.teamcode.subsystems.elevator.Elevator;
+import org.firstinspires.ftc.teamcode.subsystems.wrist.Wrist;
+import org.firstinspires.ftc.teamcode.subsystems.launcher.Launcher;
+import org.firstinspires.ftc.teamcode.subsystems.chassis.MecanumChassis;
 
 public class Robot {
 
@@ -21,24 +24,59 @@ public static Robot getInstance() {
         }
         return instance;
     }
-
+
+
+    private RobotState currentState;
     private Arm arm;
+    private MecanumChassis chassis;
+    private Claw claw;
     private Elevator elevator;
     private Launcher launcher;
     private Vision vision;
+    private Wrist wrist;
 
     public void initSubsystems(HardwareMap hardwareMap) {
+        this.currentState = RobotState.DRIVE;
         this.arm = new Arm(hardwareMap);
+        this.chassis = new MecanumChassis(hardwareMap);
+        this.claw = new Claw(hardwareMap);
         this.elevator = new Elevator(hardwareMap);
         this.launcher = new Launcher(hardwareMap);
         this.vision = new Vision(hardwareMap);
+        this.wrist = new Wrist(hardwareMap);
+    }
+
+    public SequentialCommandGroup setState(RobotState robotState) {
+        currentState = robotState;
+        switch (robotState) {
+            case SCORE:
+                return StateCommands.scoreState();
+            case INTAKE:
+                return StateCommands.intakeState();
+            case CLIMB:
+                return StateCommands.climbState();
+            case DRIVE:
+            default:
+                return StateCommands.driveState();
+        }
     }
 
+    public RobotState getCurrentState() {
+        return currentState;
+    }
 
     public Arm getArm() {
         return arm;
     }
 
+    public MecanumChassis getChassis() {
+        return chassis;
+    }
+
+    public Claw getClaw() {
+        return claw;
+    }
+
     public Elevator getElevator() {
         return elevator;
     }
@@ -51,4 +89,8 @@ public Vision getVision() {
         return vision;
     }
 
+    public Wrist getWrist() {
+        return wrist;
+    }
+
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotState.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotState.java
@@ -0,0 +1,10 @@
+package org.firstinspires.ftc.teamcode;
+
+public enum RobotState {
+
+    DRIVE,
+    SCORE,
+    CLIMB,
+    INTAKE,
+
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StateCommands.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StateCommands.java
@@ -0,0 +1,58 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.arcrobotics.ftclib.command.ParallelCommandGroup;
+import com.arcrobotics.ftclib.command.SequentialCommandGroup;
+
+import org.firstinspires.ftc.teamcode.subsystems.arm.ArmCommands;
+import org.firstinspires.ftc.teamcode.subsystems.arm.ArmState;
+import org.firstinspires.ftc.teamcode.subsystems.claw.ClawCommands;
+import org.firstinspires.ftc.teamcode.subsystems.elevator.ElevatorCommands;
+import org.firstinspires.ftc.teamcode.subsystems.elevator.ElevatorState;
+import org.firstinspires.ftc.teamcode.subsystems.wrist.WristCommands;
+import org.firstinspires.ftc.teamcode.subsystems.wrist.WristState;
+
+public class StateCommands {
+
+    protected static SequentialCommandGroup scoreState() {
+        return new SequentialCommandGroup(
+                new ParallelCommandGroup(
+                        WristCommands.moveToState(WristState.SCORE),
+                        ArmCommands.goToState(ArmState.SCORE),
+                        ElevatorCommands.goToState(ElevatorState.SCORE)
+                )
+        );
+    }
+
+    protected static SequentialCommandGroup intakeState() {
+        return new SequentialCommandGroup(
+                new ParallelCommandGroup(
+                        WristCommands.moveToState(WristState.INTAKE),
+                        ArmCommands.goToState(ArmState.INTAKE),
+                        ElevatorCommands.goToState(ElevatorState.INTAKE)
+                ),
+                ClawCommands.openBothFingers()
+        );
+    }
+
+    protected static SequentialCommandGroup climbState() {
+        return new SequentialCommandGroup(
+                ClawCommands.closeBothFingers(),
+                new ParallelCommandGroup(
+                        ArmCommands.goToState(ArmState.CLIMB),
+                        ElevatorCommands.goToState(ElevatorState.CLIMB)
+                )
+        );
+    }
+
+    protected static SequentialCommandGroup driveState() {
+        return new SequentialCommandGroup(
+                ClawCommands.closeBothFingers(),
+                new ParallelCommandGroup(
+                        WristCommands.moveToState(WristState.IDLE),
+                        ArmCommands.goToState(ArmState.IDLE),
+                        ElevatorCommands.goToState(ElevatorState.IDLE)
+                )
+        );
+    }
+
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DefaultRaz.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DefaultRaz.java
@@ -20,9 +20,9 @@ public final void loop() {
         execute();
     }
 
-
     public abstract void initialize();
     public abstract void execute();
     public abstract void configureBindings();
 
 }
+
diff --git a/...main/java/org/firstinspires/ftc/teamcode/opmodes/robotstatetesters/RazRobotStateTest.java b/...main/java/org/firstinspires/ftc/teamcode/opmodes/robotstatetesters/RazRobotStateTest.java
@@ -0,0 +1,28 @@
+package org.firstinspires.ftc.teamcode.opmodes.robotstatetesters;
+
+
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.Bindings;
+import org.firstinspires.ftc.teamcode.Robot;
+import org.firstinspires.ftc.teamcode.opmodes.DefaultRaz;
+
+@TeleOp(name = "Raz Robot State Test")
+public class RazRobotStateTest extends DefaultRaz {
+
+    @Override
+    public void initialize() {
+
+    }
+
+    @Override
+    public void execute() {
+        telemetry.addData("Current Robot State: ", Robot.getInstance().getCurrentState());
+    }
+
+    @Override
+    public void configureBindings() {
+        Bindings.razRobotStateTest(gamepad1);
+    }
+
+}
diff --git a/.../teamcode/opmodes/testers/RazArmTest.java → ...opmodes/subsystemstesters/RazArmTest.java b/.../teamcode/opmodes/testers/RazArmTest.java → ...opmodes/subsystemstesters/RazArmTest.java
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.opmodes.testers;
+package org.firstinspires.ftc.teamcode.opmodes.subsystemstesters;
 
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
@@ -21,7 +21,7 @@ public void execute() {
 
     @Override
     public void configureBindings() {
-        Bindings.razArmTest(gamepad1, gamepad2);
+        Bindings.razArmTest(gamepad1);
     }
 
 }
diff --git a/...rc/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazChassisTest.java b/...rc/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazChassisTest.java
@@ -0,0 +1,26 @@
+package org.firstinspires.ftc.teamcode.opmodes.subsystemstesters;
+
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.Bindings;
+import org.firstinspires.ftc.teamcode.Robot;
+import org.firstinspires.ftc.teamcode.opmodes.DefaultRaz;
+
+@TeleOp(name = "Raz Drive Test")
+public class RazChassisTest extends DefaultRaz {
+
+    @Override
+    public void initialize() {
+    }
+
+    @Override
+    public void execute() {
+        Robot.getInstance().getChassis().telemetry(telemetry);
+    }
+
+    @Override
+    public void configureBindings() {
+        Bindings.razChassisTest(gamepad1);
+    }
+
+}