Merge pull request #3 from rh-robotics/JackInitialTeleop

Added Initial Teleop and State Machine Enum/Switch

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/InitialTeleOp.java

@@ -0,0 +1,123 @@
+package org.firstinspires.ftc.teamcode.teleop;
+
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.util.ElapsedTime;
+import com.qualcomm.robotcore.util.Range;
+
+import org.firstinspires.ftc.teamcode.subsystems.HWC;
+
+/**
+ * TeleOp OpMode for simply driving with strafing wheels
+ * Look at JAVA DOC!
+ */
+@TeleOp(name = "Basic Strafe Drive", group = "Iterative OpMode")
+public class InitialTeleOp extends OpMode {
+    private final ElapsedTime time = new ElapsedTime();
+    HWC robot; // Declare the object for HWC, will allow us to access all the motors declared there!
+    TeleOpStates state; // Creates object of states enum
+    // init() Runs ONCE after the driver hits initialize
+    @Override
+    public void init() {
+        // Tell the driver the Op is initializing
+        telemetry.addData("Status", "Initializing");
+
+        // Do all init stuff
+        // TODO: ADD INITS THAT YOU NEED
+        robot = new HWC(hardwareMap, telemetry);
+
+        // Tell the driver the robot is ready
+        telemetry.addData("Status", "Initialized");
+
+        // Creates States
+       state = TeleOpStates.START;
+    }
+
+    // init_loop() - Runs continuously until the driver hits play
+    @Override
+    public void init_loop() {
+    }
+
+    // Start() - Runs ONCE when the driver presses play
+    @Override
+    public void start() {
+        time.reset();
+    }
+
+    // loop() - Runs continuously while the OpMode is active
+    @Override
+    public void loop() {
+        double leftFPower;
+        double rightFPower;
+        double leftBPower;
+        double rightBPower;
+        double drive = -gamepad1.left_stick_x * 0.8;
+        double turn = gamepad1.left_stick_y * 0.6;
+        double strafe = -gamepad1.right_stick_x * 0.8;
+
+        // Calculate drive power
+        if (drive != 0 || turn != 0) {
+            leftFPower = Range.clip(drive + turn, -1.0, 1.0);
+            rightFPower = Range.clip(drive - turn, -1.0, 1.0);
+            leftBPower = Range.clip(drive + turn, -1.0, 1.0);
+            rightBPower = Range.clip(drive - turn, -1.0, 1.0);
+        } else if (strafe != 0) {
+            // Strafing
+            leftFPower = -strafe;
+            rightFPower = strafe;
+            leftBPower = strafe;
+            rightBPower = -strafe;
+        } else {
+            leftFPower = 0;
+            rightFPower = 0;
+            leftBPower = 0;
+            rightBPower = 0;
+        }
+
+        // Set power to values calculated above
+        robot.leftFront.setPower(leftFPower);
+        robot.leftRear.setPower(leftBPower);
+        robot.rightFront.setPower(rightFPower);
+        robot.rightRear.setPower(rightBPower);
+
+
+        switch(state){
+
+            case START:
+                break;
+
+            case DRIVE:
+                break;
+
+            case INTAKE_SAMPLE:
+                break;
+
+            case INTAKE_SPECIMEN:
+                break;
+
+            case DELIVER_SAMPLE:
+                break;
+
+            case DELIVER_SPECIMEN:
+                break;
+
+            case CLIMB_ONE:
+                break;
+
+            case CLIMB_TWO:
+                break;
+
+            case END:
+                break;
+
+            case UNKNOWN:
+                break;
+
+            default:
+                state = TeleOpStates.UNKNOWN;
+        }
+
+        telemetry.addData("State", state);
+        telemetry.update();
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleOpStates.java

@@ -0,0 +1,14 @@
+package org.firstinspires.ftc.teamcode.teleop;
+
+public enum TeleOpStates {
+    INTAKE_SAMPLE,
+    INTAKE_SPECIMEN,
+    DELIVER_SAMPLE,
+    DELIVER_SPECIMEN,
+    DRIVE,
+    CLIMB_ONE,
+    CLIMB_TWO,
+    START,
+    END,
+    UNKNOWN
+}