(:

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/RedWarehouseEngine.java

@@ -0,0 +1,14 @@
+package org.timecrafters.FreightFrenzy.Competition.Autonomous.Engines;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.DriveState;
+import org.timecrafters.FreightFrenzy.Competition.Common.Robot;
+
+public class RedWarehouseEngine extends CyberarmEngine {
+    @Override
+    public void setup() {
+        Robot robot = new Robot(this);
+
+        addState(new DriveState(robot, 2000, 2000,.25,.25));
+    }
+}

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/DriveState.java

@@ -0,0 +1,50 @@
+package org.timecrafters.FreightFrenzy.Competition.Autonomous.States;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.FreightFrenzy.Competition.Common.Robot;
+
+public class DriveState extends CyberarmState {
+    Robot robot;
+    double distanceLeft, distanceRight, powerLeft, powerRight;
+
+    public DriveState(Robot robot, double distanceLeft, double distanceRight, double powerLeft, double powerRight) {
+        this.robot = robot;
+        this.distanceLeft = distanceLeft;
+        this.distanceRight = distanceRight;
+        this.powerLeft = powerLeft;
+        this.powerRight = powerRight;
+    }
+
+    @Override
+    public void start() {
+        robot.driveGoalLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.driveGoalRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.driveWarehouseLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.driveWarehouseRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+
+        robot.driveGoalLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.driveGoalRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.driveWarehouseLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.driveWarehouseRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+    }
+
+    @Override
+    public void exec() {
+        if (Math.abs(robot.driveGoalLeft.getCurrentPosition()) >= distanceLeft && Math.abs(robot.driveGoalRight.getCurrentPosition()) >= distanceRight) {
+            robot.driveGoalLeft.setPower(0);
+            robot.driveGoalRight.setPower(0);
+            robot.driveWarehouseRight.setPower(0);
+            robot.driveWarehouseLeft.setPower(0);
+            setHasFinished(true);
+        } else {
+            robot.driveGoalLeft.setPower(powerLeft);
+            robot.driveGoalRight.setPower(powerRight);
+            robot.driveWarehouseRight.setPower(powerRight);
+            robot.driveWarehouseLeft.setPower(powerLeft);
+
+        }
+    }
+
+}

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/Robot.java

@@ -72,15 +72,16 @@ public class Robot {
     public static final double WHEEL_CIRCUMFERENCE = Math.PI * 8;
     public static final int COUNTS_PER_REVOLUTION = 1000;
 
-    public DcMotor driveFrontLeft, driveFrontRight, driveBackLeft, driveBackRight;
+    public DcMotor driveWarehouseLeft, driveWarehouseRight, driveGoalLeft, driveGoalRight,
+            collectorArmRiser, collectorArmBobbin, depositorArmRiser, depositorArmBobbin;
 
     // Collector
     public DcMotor collectorBobbin;
-    public Servo collectorDoor;
+    public Servo collectorDispenser;
 
     // Depositor
     public DcMotor depositorBobbin;
-    public Servo depositorDoor;
+    public Servo depositorDispenser;
 
     public Robot(CyberarmEngine engine) {
         this.engine = engine;
@@ -189,20 +190,22 @@ public class Robot {
     }
 
     private void initDrivetrain() {
-        driveFrontRight = engine.hardwareMap.dcMotor.get("driveFrontRight");
+        driveWarehouseRight = engine.hardwareMap.dcMotor.get("driveWarehouseRight");
-        driveFrontLeft = engine.hardwareMap.dcMotor.get("driveFrontLeft");
+        driveWarehouseLeft  = engine.hardwareMap.dcMotor.get("driveWarehouseLeft");
-        driveBackRight = engine.hardwareMap.dcMotor.get("driveBackRight");
+        driveGoalRight      = engine.hardwareMap.dcMotor.get("driveGoalRight");
-        driveBackLeft = engine.hardwareMap.dcMotor.get("driveBackLeft");
+        driveGoalLeft       = engine.hardwareMap.dcMotor.get("driveGoalLeft");
 
-        driveFrontLeft.setDirection(DcMotorSimple.Direction.REVERSE);
+        driveWarehouseLeft.setDirection(DcMotorSimple.Direction.REVERSE);
-        driveBackLeft.setDirection(DcMotorSimple.Direction.REVERSE);
+        driveGoalLeft.setDirection(DcMotorSimple.Direction.REVERSE);
     }
 
     private void initCollector() {
+        collectorDispenser = engine.hardwareMap.servo.get("collectorDispenser");
 
     }
 
     private void initDepositor(){
+        depositorDispenser = engine.hardwareMap.servo.get("depositorDispenser");
 
     }
 

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/TeleOp/Engines/TeleOpEngine.java

@@ -0,0 +1,12 @@
+package org.timecrafters.FreightFrenzy.Competition.TeleOp.Engines;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.FreightFrenzy.Competition.Common.Robot;
+import org.timecrafters.FreightFrenzy.Competition.TeleOp.States.TeleOpState;
+
+public class TeleOpEngine extends CyberarmEngine {
+    @Override
+    public void setup() {
+        addState(new TeleOpState(new Robot(this)));
+    }
+}

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/TeleOp/States/TeleOpState.java

@@ -0,0 +1,41 @@
+package org.timecrafters.FreightFrenzy.Competition.TeleOp.States;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.FreightFrenzy.Competition.Common.Robot;
+
+public class TeleOpState extends CyberarmState {
+    Robot robot;
+    double maxDriveSpeed;
+
+    public TeleOpState(Robot robot) {
+        this.robot = robot;
+     maxDriveSpeed = 0.7;
+    }
+
+    @Override
+    public void start() {
+        super.start();
+    }
+
+    @Override
+    public void exec() {
+        robot.driveWarehouseLeft.setPower(-engine.gamepad1.left_stick_y * maxDriveSpeed);
+        robot.driveWarehouseRight.setPower(-engine.gamepad1.right_stick_y * maxDriveSpeed);
+        robot.driveGoalLeft.setPower(-engine.gamepad1.left_stick_y * maxDriveSpeed);
+        robot.driveGoalRight.setPower(-engine.gamepad1.right_stick_y * maxDriveSpeed);
+
+        if (engine.gamepad1.left_bumper){
+            robot.depositorDispenser.setPosition(.5);
+        } else {
+            robot.depositorDispenser.setPosition(0);
+        }
+
+        if (engine.gamepad1.right_bumper){
+            robot.collectorDispenser.setPosition(.5);
+        } else {
+            robot.collectorDispenser.setPosition(0);
+        }
+    }
+
+
+}