Merge pull request #22 from Spikes-2212-Programming-Guild/adi-controller

Adi controller

src/main/java/frc/robot/OI.java

@@ -1,8 +1,91 @@
 package frc.robot;
 
-public class OI /*GEVALD*/{
+import com.spikes2212.command.genericsubsystem.commands.MoveGenericSubsystem;
+import com.spikes2212.util.PlaystationControllerWrapper;
+import edu.wpi.first.wpilibj2.command.ConditionalCommand;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.commands.*;
+import frc.robot.subsystems.*;
+
+public class OI /*GEVALD*/ {
+
+    private final PlaystationControllerWrapper driverJoystick = new PlaystationControllerWrapper(0);
+    private final PlaystationControllerWrapper navigatorJoystick = new PlaystationControllerWrapper(1);
+
+    private final Elevator elevator = Elevator.getInstance();
+    private final Storage storage = Storage.getInstance();
+    private final Gripper gripper = Gripper.getInstance();
+    private final CoralJoint coralJoint = CoralJoint.getInstance();
+    private final AlgaeJoint algaeJoint = AlgaeJoint.getInstance();
+    private final Drivetrain drivetrain = Drivetrain.getInstance();
+
+    private boolean inAlgaeMode;
 
     public OI() {
+        navigatorJoystick.getL1Button().onTrue(new ConditionalCommand(
+                new PlaceCoralAndTakeAlgae(elevator, algaeJoint, gripper,
+                        drivetrain, coralJoint, storage,
+                        Elevator.ElevatorLevel.L1),
+                new PlaceOnReef(elevator,
+                        coralJoint, storage, Elevator.ElevatorLevel.L1),
+                () -> inAlgaeMode));
+
+        navigatorJoystick.getR1Button().onTrue(new ConditionalCommand(
+                new PlaceCoralAndTakeAlgae(elevator, algaeJoint, gripper,
+                        drivetrain, coralJoint, storage,
+                        Elevator.ElevatorLevel.L2),
+                new PlaceOnReef(elevator,
+                        coralJoint, storage, Elevator.ElevatorLevel.L2),
+                () -> inAlgaeMode));
+
+        navigatorJoystick.getL2Button().onTrue(new ConditionalCommand(
+                new PlaceCoralAndTakeAlgae(elevator, algaeJoint, gripper,
+                        drivetrain, coralJoint, storage,
+                        Elevator.ElevatorLevel.L3),
+                new PlaceOnReef(elevator,
+                        coralJoint, storage, Elevator.ElevatorLevel.L3),
+                () -> inAlgaeMode));
+
+        navigatorJoystick.getR2Button().onTrue(new PlaceOnReef(elevator, coralJoint,
+                storage, Elevator.ElevatorLevel.L4));
+
+        navigatorJoystick.getTriangleButton().onTrue(new InstantCommand(() -> inAlgaeMode = !inAlgaeMode));
+        navigatorJoystick.getSquareButton().onTrue(new IntakeCoral(storage));
+        navigatorJoystick.getCircleButton().onTrue(new ReleaseAlgae(gripper));
+
+        navigatorJoystick.getRightStickButton().onTrue(new Reset(elevator, coralJoint,
+                algaeJoint));
+
+        navigatorJoystick.getUpButton().whileTrue(new MoveGenericSubsystem(elevator,
+                Elevator.ELEVATOR_FORWARD_SPEED));
+        navigatorJoystick.getDownButton().whileTrue(new MoveGenericSubsystem(elevator,
+                Elevator.ELEVATOR_BACKWARD_SPEED));
+        navigatorJoystick.getRightButton().whileTrue(new MoveGenericSubsystem(coralJoint,
+                CoralJoint.CORAL_JOINT_FORWARD_SPEED));
+        navigatorJoystick.getLeftButton().whileTrue(new MoveGenericSubsystem(coralJoint,
+                CoralJoint.CORAL_JOINT_BACKWARD_SPEED));
+
+        navigatorJoystick.getLeftStickButton().onTrue(new ReleaseCoral(storage));
+
+        navigatorJoystick.getShareButton().onTrue(new RotateAlgaeJointToBottom(algaeJoint));
+        navigatorJoystick.getOptionsButton().onTrue(new IntakeAlgae(gripper));
+
+        driverJoystick.getR1Button().onTrue(new InstantCommand(drivetrain::resetGyro));
+    }
+
+    public double getLeftX() {
+        return driverJoystick.getLeftX();
+    }
+
+    public double getLeftY() {
+        return driverJoystick.getLeftY();
+    }
+
+    public double getRightX() {
+        return driverJoystick.getRightX();
+    }
 
+    public double getRightY() {
+        return driverJoystick.getRightY();
     }
 }

src/main/java/frc/robot/commands/Reset.java

@@ -0,0 +1,17 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.subsystems.AlgaeJoint;
+import frc.robot.subsystems.CoralJoint;
+import frc.robot.subsystems.Elevator;
+import frc.robot.subsystems.Storage;
+
+public class Reset extends ParallelCommandGroup {
+
+    public Reset(Elevator elevator, CoralJoint coralJoint, AlgaeJoint algaeJoint) {
+        addCommands(new MoveToHeight(elevator, Elevator.ElevatorLevel.BOTTOM),
+                new RotateStorage(coralJoint, CoralJoint.StoragePose.RESTING),
+                new RotateAlgaeJointToTop(algaeJoint));
+    }
+}

src/main/java/frc/robot/subsystems/CoralJoint.java

@@ -19,6 +19,9 @@ public enum StoragePose {
         }
     }
 
+    public static final double CORAL_JOINT_FORWARD_SPEED = 0.175;
+    public static final double CORAL_JOINT_BACKWARD_SPEED = -0.175;
+
     private static final String NAMESPACE_NAME = "coral joint";
     private static final double GEAR_RATIO = 1;
     private static final double DEGREES_IN_ROTATIONS = 360;

src/main/java/frc/robot/subsystems/Elevator.java

@@ -6,6 +6,8 @@
 import edu.wpi.first.wpilibj.DigitalInput;
 import frc.robot.RobotMap;
 
+import java.util.function.Supplier;
+
 public class Elevator extends SmartMotorControllerGenericSubsystem {
 
     public enum ElevatorLevel {
@@ -19,6 +21,9 @@ public enum ElevatorLevel {
         }
     }
 
+    public static final double ELEVATOR_FORWARD_SPEED = 0.175;
+    public static final double ELEVATOR_BACKWARD_SPEED = -0.175;
+
     private static final String NAMESPACE_NAME = "elevator";
 
     private static final double GEAR_RATIO = (14 / 50.0) * (16 / 50.0);