created auto. states and made toggle for teleop (not tested yet)

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

@@ -9,6 +9,6 @@ public class RedWarehouseEngine extends CyberarmEngine {
     public void setup() {
         Robot robot = new Robot(this);
 
-        addState(new DriveState(robot, 2000, 2000,.25,.25));
+        addState(new DriveState(robot, 2000, 2000,1,1));
     }
 }

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

@@ -0,0 +1,42 @@
+package org.timecrafters.FreightFrenzy.Competition.Autonomous.States;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.FreightFrenzy.Competition.Common.Robot;
+
+public class FaceState extends CyberarmState {
+    private double faceDirection, tolerance, power;
+    private Robot robot;
+    public FaceState(Robot robot, double faceDirection, double tolerance, double power) {
+        this.robot = robot;
+        this.faceDirection = faceDirection;
+        this.tolerance = tolerance;
+        this.power = power;
+    }
+
+    @Override
+    public void exec() {
+        robot.updateRobotOrientation();
+
+        if (robot.heading() - tolerance > faceDirection) {
+            robot.driveGoalLeft.setPower(power);
+            robot.driveWarehouseLeft.setPower(power);
+            robot.driveGoalRight.setPower(0);
+            robot.driveWarehouseRight.setPower(0);
+        }
+
+        else if (robot.heading() + tolerance < faceDirection){
+            robot.driveGoalLeft.setPower(0);
+            robot.driveWarehouseLeft.setPower(0);
+            robot.driveGoalRight.setPower(power);
+            robot.driveWarehouseRight.setPower(power);
+        }
+
+        else {
+            robot.driveGoalLeft.setPower(0);
+            robot.driveWarehouseLeft.setPower(0);
+            robot.driveGoalRight.setPower(0);
+            robot.driveWarehouseRight.setPower(0);
+            setHasFinished(true);
+        }
+    }
+}

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

@@ -0,0 +1,26 @@
+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 TurretArmExtension extends CyberarmState {
+
+    private Robot robot;
+    private DcMotor motor;
+    private int position;
+    private double power;
+
+    public TurretArmExtension(Robot robot, DcMotor motor, int position, double power) {
+        this.robot = robot;
+        this.motor = motor;
+        this.position = position;
+        this.power = power;
+    }
+
+    @Override
+    public void exec() {
+
+    }
+}

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

@@ -0,0 +1,31 @@
+package org.timecrafters.FreightFrenzy.Competition.Autonomous.States;
+
+import com.qualcomm.robotcore.hardware.CRServo;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.FreightFrenzy.Competition.Common.Robot;
+
+public class TurretOrbit extends CyberarmState {
+    private Robot robot;
+    private CRServo servo;
+    private long time;
+    private double power;
+
+    public TurretOrbit(Robot robot, CRServo servo, long time, double power) {
+        this.robot = robot;
+        this.servo = servo;
+        this.time = time;
+        this. power = power;
+    }
+
+    @Override
+    public void exec() {
+        if (runTime() < time) {
+            servo.setPower(power);
+        }
+        else {
+            servo.setPower(0);
+            setHasFinished(true);
+        }
+    }
+}

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

@@ -65,6 +65,7 @@ public class Robot {
             .translation(VUFORIA_CAMERA_FORWARD_DISPLACEMENT, VUFORIA_CAMERA_LEFT_DISPLACEMENT, VUFORIA_CAMERA_VERTICAL_DISPLACEMENT)
             .multiplied(Orientation.getRotationMatrix(AxesReference.EXTRINSIC, AxesOrder.XZY, AngleUnit.DEGREES, 90, 90, 0));
     private RobotLocation vuforiaLastLocation;
+    private Orientation orientation;
 
     // Sensors
     public BNO055IMU imu;
@@ -102,15 +103,19 @@ public class Robot {
     }
 
     public double heading() {
-        return imu.getAngularOrientation().firstAngle;
+        return orientation.firstAngle;
     }
 
     public double roll() {
-        return imu.getAngularOrientation().secondAngle;
+        return orientation.secondAngle;
     }
 
     public double pitch() {
-        return imu.getAngularOrientation().thirdAngle;
+        return orientation.thirdAngle;
+    }
+
+    public void updateRobotOrientation(){
+        orientation = imu.getAngularOrientation();
     }
 
     public void activateTensorflow() {
@@ -215,6 +220,8 @@ public class Robot {
         orangeArmRiser.setDirection(DcMotorSimple.Direction.FORWARD);
         orangeArmRiser.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
         collectorOrange = engine.hardwareMap.crservo.get("collectorOrange");
+
+        orangeDispenser.setPosition(0.5);
     }
 
     private void initDepositor(){

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

@@ -12,7 +12,7 @@ public class TeleOpState extends CyberarmState {
     double maxDriveSpeed, maxCollectorArmSpeed, maxDepositorArmSpeed;
     int maxArmTravelDistance, minArmTravelDistance;
     boolean armLimitToggle = false, orangeCollectorToggle = false, whiteCollectorToggle = false;
-    boolean allianceRed = true;
+    boolean allianceRedDriver1 = true, allianceRedDriver2 = true;
 
     public TeleOpState(Robot robot) {
         this.robot = robot;
@@ -48,13 +48,13 @@ public class TeleOpState extends CyberarmState {
 
         // dispenser powered
         if (engine.gamepad1.b){
-            robot.orangeDispenser.setPosition(.5);
-        }
-
-        // if not pressed dispenser off
-        else {
             robot.orangeDispenser.setPosition(0);
         }
+        // if not pressed dispenser off
+        else {
+            robot.orangeDispenser.setPosition(0.5);
+        }
+
         //  if one of triggers pressed arm extends or unextends, there is also a limit on how far arm can extend
         if (robot.orangeArmBobbin.getCurrentPosition() <= maxArmTravelDistance && engine.gamepad1.left_trigger > 0) {
 
@@ -84,7 +84,7 @@ public class TeleOpState extends CyberarmState {
         else {
             robot.turretServoOrange.setPower(0);
         }*/
-        turretOrbitControl(engine.gamepad1, robot.turretServoOrange, allianceRed);
+        turretOrbitControl(engine.gamepad1, robot.turretServoOrange, allianceRedDriver1);
 
         // if dpad verticles pressed arm rises or lowers...
         if (engine.gamepad1.dpad_up || engine.gamepad1.dpad_down) {
@@ -154,7 +154,7 @@ public class TeleOpState extends CyberarmState {
 //        else {
 //            robot.turretServoWhite.setPower(0);
 //        }
-        turretOrbitControl(engine.gamepad2, robot.turretServoWhite, allianceRed);
+        turretOrbitControl(engine.gamepad2, robot.turretServoWhite, allianceRedDriver2);
         // if dpad verticles pressed arm rises or lowers
         if (engine.gamepad2.dpad_down || engine.gamepad2.dpad_up) {
 
@@ -228,6 +228,12 @@ public class TeleOpState extends CyberarmState {
                         robot.collectorOrange.setPower(-1);
                     }
                     break;
+
+                case "guide":
+                    allianceRedDriver1 = !allianceRedDriver1;
+                    break;
+
+
             }
         }
 
@@ -252,6 +258,9 @@ public class TeleOpState extends CyberarmState {
                         robot.collectorWhite.setPower(-1);
                     }
                     break;
+                case "guide":
+                    allianceRedDriver2 = !allianceRedDriver2;
+                    break;
             }
         }
     }