made auto draft, and started on vuforia/tensorflow state

(:

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

@@ -1,7 +1,10 @@
 package org.timecrafters.FreightFrenzy.Competition.Autonomous.Engines;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.DepositorDoor;
 import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.DriveState;
+import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.TurretArmExtension;
+import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.TurretOrbit;
 import org.timecrafters.FreightFrenzy.Competition.Common.Robot;
 
 public class RedWarehouseEngine extends CyberarmEngine {
@@ -9,6 +12,14 @@ public class RedWarehouseEngine extends CyberarmEngine {
     public void setup() {
         Robot robot = new Robot(this);
 
-        addState(new DriveState(robot, 2000, 2000,1,1));
+        addState(new TurretOrbit(robot, robot.turretServoWhite, 3, 1));
+        addState(new TurretArmExtension(robot, robot.whiteArmBobbin, 3000, .75, 150));
+        addState(new DepositorDoor(robot.whiteDispenser, .5, 1));
+        addState(new DepositorDoor(robot.whiteDispenser, 0, 0));
+        addState(new TurretArmExtension(robot, robot.whiteArmBobbin, 0, 1, 150));
+        addState(new TurretOrbit(robot, robot.turretServoWhite, 3, -1));
+        addState(new DriveState(robot, 1500, 500, 1, .75));
+        addState(new DriveState(robot, 1500, 1500, 1, 1));
+
     }
 }

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

@@ -0,0 +1,35 @@
+package org.timecrafters.FreightFrenzy.Competition.Autonomous.States;
+
+import com.qualcomm.robotcore.hardware.CRServo;
+import com.qualcomm.robotcore.hardware.Servo;
+
+import org.cyberarm.engine.V2.CyberarmState;
+
+public class CollectorToggle extends CyberarmState {
+    final static public int MODE_REVERSE = -1;
+    final static public int MODE_COLLECT = 1;
+    final static public int MODE_STOPPED = 0;
+
+    CRServo servo;
+    int mode;
+
+    public CollectorToggle(CRServo servo, int mode) {
+        this.servo = servo;
+        this.mode = mode;
+    }
+
+    @Override
+    public void exec() {
+        if (mode == MODE_REVERSE){
+            servo.setPower(-1);
+        }
+
+        else if (mode == MODE_COLLECT){
+            servo.setPower(1);
+        }
+
+        else {
+            servo.setPower(0);
+        }
+    }
+}

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

@@ -0,0 +1,29 @@
+package org.timecrafters.FreightFrenzy.Competition.Autonomous.States;
+
+import com.qualcomm.robotcore.hardware.Servo;
+
+import org.cyberarm.engine.V2.CyberarmState;
+
+public class DepositorDoor extends CyberarmState {
+
+    Servo servo;
+    double targetPosition;
+    long time;
+
+    public DepositorDoor(Servo servo, double targetPosition, long time) {
+        this.servo = servo;
+        this.targetPosition = targetPosition;
+        this.time = time;
+    }
+
+    @Override
+    public void exec() {
+        if (runTime() < time) {
+            servo.setPosition(targetPosition);
+        }
+        else {
+            setHasFinished(true);
+        }
+
+    }
+}

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

@@ -9,18 +9,29 @@ public class TurretArmExtension extends CyberarmState {
 
     private Robot robot;
     private DcMotor motor;
-    private int position;
+    private int targetPosition, tolerance;
     private double power;
 
-    public TurretArmExtension(Robot robot, DcMotor motor, int position, double power) {
+    public TurretArmExtension(Robot robot, DcMotor motor, int targetPosition, double power, int tolerance) {
         this.robot = robot;
         this.motor = motor;
-        this.position = position;
+        this.targetPosition = targetPosition;
         this.power = power;
+        this.tolerance = tolerance;
     }
 
     @Override
     public void exec() {
+        if (motor.getCurrentPosition() < targetPosition + tolerance){
+            motor.setPower(power);
+        }
 
+        else if (motor.getCurrentPosition() > targetPosition - tolerance){
+            motor.setPower(-power);
+        }
+        else {
+            setHasFinished(true);
+            motor.setPower(0);
+        }
     }
 }

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

@@ -0,0 +1,37 @@
+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 TurretArmRiser extends CyberarmState {
+
+    private Robot robot;
+    private DcMotor motor;
+    private int targetPosition, tolerance;
+    private double power;
+
+    public TurretArmRiser(Robot robot, DcMotor motor, int targetPosition, double power, int tolerance) {
+        this.robot = robot;
+        this.motor = motor;
+        this.targetPosition = targetPosition;
+        this.power = power;
+        this.tolerance = tolerance;
+    }
+
+    @Override
+    public void exec() {
+        if (motor.getCurrentPosition() < targetPosition + tolerance){
+            motor.setPower(power);
+        }
+
+        else if (motor.getCurrentPosition() > targetPosition - tolerance){
+            motor.setPower(-power);
+        }
+        else {
+            setHasFinished(true);
+            motor.setPower(0);
+        }
+    }
+}

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

@@ -0,0 +1,36 @@
+package org.timecrafters.FreightFrenzy.Competition.Autonomous.States;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
+import org.timecrafters.FreightFrenzy.Competition.Common.Robot;
+
+public class VuforiaState extends CyberarmState {
+
+    Robot robot;
+
+    public VuforiaState(Robot robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void start() {
+        robot.activateVuforia();
+        robot.activateTensorflow();
+    }
+
+    @Override
+    public void exec() {
+        for (Recognition recognition : robot.tensorflowDetections()) {
+        }
+    }
+
+    @Override
+    public void telemetry() {
+        for (Recognition recognition : robot.tensorflowDetections()) {
+            engine.telemetry.addData("Label", recognition.getLabel());
+            engine.telemetry.addData("Left", recognition.getLeft());
+            engine.telemetry.addData("Top", recognition.getTop());
+            engine.telemetry.addData("Confidence", recognition.getConfidence());
+        }
+    }
+}