Autonomous has been wrote to acsend and descend the arms, and camera has been commented out

TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/TestAutonomousEngine.java

@@ -5,9 +5,9 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import org.cyberarm.engine.V2.CyberarmEngine;
 import org.timecrafters.Autonomous.States.CollectorState;
 import org.timecrafters.Autonomous.States.DriverState;
-import org.timecrafters.Autonomous.States.LowerArm;
+import org.timecrafters.Autonomous.States.BottomArm;
 import org.timecrafters.Autonomous.States.RotationState;
-import org.timecrafters.Autonomous.States.UpperArm;
+import org.timecrafters.Autonomous.States.TopArm;
 import org.timecrafters.testing.states.PrototypeBot1;
 
 @Autonomous (name = "Autonomous Test")
@@ -24,25 +24,25 @@ public class TestAutonomousEngine extends CyberarmEngine {
         //turn towards high pole
         addState(new RotationState(robot, "TestAutonomous", "02-0"));
         //lift the upper arm
-        addState(new UpperArm(robot, "TestAutonomous", "03-0"));
+        addState(new TopArm(robot, "TestAutonomous", "03-0"));
         //lift the lower arm
-        addState(new LowerArm(robot, "TestAutonomous", "04-0"));
+        addState(new BottomArm(robot, "TestAutonomous", "04-0"));
         //drive forward
         addState(new DriverState(robot, "TestAutonomous", "05-0"));
         //lower the bottom arm to get closer
-        addState(new LowerArm(robot, "TestAutonomous", "06-0"));
+        addState(new BottomArm(robot, "TestAutonomous", "06-0"));
         //make collector release the cone
-        addState(new CollectorState(robot, "TestAutnomous", "07-0"));
+        addState(new CollectorState(robot, "TestAutonomous", "07-0"));
         //lift the lower arm to clear the pole
-        addState(new LowerArm(robot, "TestAutonomous", "08-0"));
+        addState(new BottomArm(robot, "TestAutonomous", "08-0"));
         //Drive Backwards
         addState(new DriverState(robot, "TestAutonomous", "09-0"));
         // Rotate to either set up for cones to grab or park somewhere
         addState(new RotationState(robot, "TestAutonomous", "10-0"));
         // lower the bottom arm so we dont fall over
-        addState(new LowerArm(robot, "TestAutonomous", "11-0"));
+        addState(new BottomArm(robot, "TestAutonomous", "11-0"));
         // lower the upper arm so we dont fall over
-        addState(new UpperArm(robot, "TestAutonomous", "12-0"));
+        addState(new TopArm(robot, "TestAutonomous", "12-0"));
         //either dont move if your in the right zone otherwise drive forward a specific variable if we aren't already in the right spot
         addState(new DriverState(robot, "TestAutonomous", "13-0"));
 

TeamCode/src/main/java/org/timecrafters/Autonomous/States/BottomArm.java

@@ -0,0 +1,59 @@
+package org.timecrafters.Autonomous.States;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.testing.states.PrototypeBot1;
+
+public class BottomArm extends CyberarmState {
+
+    PrototypeBot1 robot;
+    double LowerRiserRightPos, LowerRiserLeftPos, AddedDistance;
+    long time;
+    long lastStepTime = 0;
+    boolean up;
+
+    public BottomArm(PrototypeBot1 robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.LowerRiserLeftPos = robot.configuration.variable(groupName, actionName, "LowerRiserLeftPos").value();
+        this.LowerRiserRightPos = robot.configuration.variable(groupName, actionName, "LowerRiserRightPos").value();
+        this.time = robot.configuration.variable(groupName, actionName, "time").value();
+        this.AddedDistance = robot.configuration.variable(groupName, actionName, "AddedDistance").value();
+
+    }
+
+    @Override
+    public void start() {
+        up = robot.LowRiserLeft.getPosition() < LowerRiserLeftPos;
+    }
+
+    @Override
+    public void exec() {
+
+        if (System.currentTimeMillis() - lastStepTime >= time) {
+            lastStepTime = System.currentTimeMillis();
+
+            if (robot.LowRiserLeft.getPosition() < LowerRiserLeftPos && up) {
+
+                robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + AddedDistance);
+                robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + AddedDistance);
+
+            } else if (robot.LowRiserLeft.getPosition() > LowerRiserLeftPos && !up) {
+
+                robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - AddedDistance);
+                robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - AddedDistance);
+
+            } else {
+                setHasFinished(true);
+            }
+        }
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("LowerRiserRightPos",LowerRiserRightPos);
+        engine.telemetry.addData("LowerRiserLeftPos",LowerRiserLeftPos);
+        engine.telemetry.addData("AddedDistance",AddedDistance);
+        engine.telemetry.addData("left servo", robot.LowRiserLeft.getPosition());
+        engine.telemetry.addData("right servo", robot.LowRiserRight.getPosition());
+        engine.telemetry.addData("time",time);
+    }
+}

TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverState.java

@@ -8,16 +8,18 @@ public class DriverState extends CyberarmState {
     public DriverState(PrototypeBot1 robot, String groupName, String actionName) {
         this.robot = robot;
         this.drivePower = robot.configuration.variable(groupName, actionName, "drivePower").value();
+        this.traveledDistance = robot.configuration.variable(groupName, actionName, "traveledDistance").value();
+
     }
 
     private float RobotRotation;
     private double drivePower;
-    private int RobotPosition,RobotStartingPosition;
+    private int RobotPosition,RobotStartingPosition,traveledDistance;
 
     @Override
     public void exec() {
 
-        if (RobotPosition - RobotStartingPosition < 2500){
+        if (RobotPosition - RobotStartingPosition < traveledDistance){
             drivePower = 1;
             robot.backLeftDrive.setPower(drivePower);
             robot.backRightDrive.setPower(drivePower);
@@ -28,6 +30,7 @@ public class DriverState extends CyberarmState {
             robot.backRightDrive.setPower(0);
             robot.frontLeftDrive.setPower(0);
             robot.frontRightDrive.setPower(0);
+            setHasFinished(true);
         }
 
     }

TeamCode/src/main/java/org/timecrafters/Autonomous/States/LowerArm.java

@@ -1,32 +0,0 @@
-package org.timecrafters.Autonomous.States;
-
-import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.testing.states.PrototypeBot1;
-
-public class LowerArm extends CyberarmState {
-
-    PrototypeBot1 robot;
-    double LowerRiserRightPos, LowerRiserLeftPos, AddedDistance;
-    long time;
-    long lastStepTime = 0;
-
-    public LowerArm(PrototypeBot1 robot, String groupName, String actionName) {
-        this.robot = robot;
-        this.LowerRiserLeftPos = robot.configuration.variable(groupName, actionName, "LowerRiserLeftPos").value();
-        this.LowerRiserRightPos = robot.configuration.variable(groupName, actionName, "LowerRiserRightPos").value();
-        this.time = robot.configuration.variable(groupName, actionName, "time").value();
-        this.AddedDistance = robot.configuration.variable(groupName, actionName, "AddedDistance").value();
-
-    }
-
-    @Override
-    public void exec() {
-        if (robot.LowRiserLeft.getPosition() > LowerRiserLeftPos) {
-            if (System.currentTimeMillis() - lastStepTime >= time) {
-                lastStepTime = System.currentTimeMillis();
-                robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - AddedDistance);
-                robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - AddedDistance);
-            }
-        }
-    }
-}

TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java

@@ -8,18 +8,20 @@ public class RotationState extends CyberarmState {
     public RotationState(PrototypeBot1 robot, String groupName, String actionName) {
         this.robot = robot;
         this.drivePower = robot.configuration.variable(groupName, actionName, "drivePower").value();
-//        this.RobotRotation = robot.configuration.variable(groupName, actionName, "RobotRotation").value();
+        this.targetRotation = robot.configuration.variable(groupName, actionName, "targetRotation").value();
+
     }
 
-    private float RobotRotation;
     private double drivePower;
+    private float targetRotation;
+    float RobotRotation;
 
     @Override
     public void exec() {
 
 
         RobotRotation = robot.imu.getAngularOrientation().firstAngle;
-        if (RobotRotation <= 45) {
+        if (RobotRotation - 3 <= targetRotation || RobotRotation + 3 <= targetRotation) {
             robot.backLeftDrive.setPower(-drivePower);
             robot.backRightDrive.setPower(drivePower);
             robot.frontLeftDrive.setPower(-drivePower);
@@ -30,7 +32,15 @@ public class RotationState extends CyberarmState {
             robot.backRightDrive.setPower(0);
             robot.frontLeftDrive.setPower(0);
             robot.frontRightDrive.setPower(0);
+            setHasFinished(true);
         }
 
     }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("Robot Rotation", RobotRotation);
+        engine.telemetry.addData("Drive Power", drivePower);
+
+    }
 }

TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArm.java

@@ -0,0 +1,60 @@
+package org.timecrafters.Autonomous.States;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.testing.states.PrototypeBot1;
+
+public class TopArm extends CyberarmState {
+
+    PrototypeBot1 robot;
+    double UpperRiserRightPos, UpperRiserLeftPos, AddedDistance;
+    long time;
+    long lastStepTime = 0;
+    boolean up;
+
+    public TopArm(PrototypeBot1 robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.UpperRiserLeftPos = robot.configuration.variable(groupName, actionName, "UpperRiserLeftPos").value();
+        this.UpperRiserRightPos = robot.configuration.variable(groupName, actionName, "UpperRiserRightPos").value();
+        this.time = robot.configuration.variable(groupName, actionName, "time").value();
+        this.AddedDistance = robot.configuration.variable(groupName, actionName, "AddedDistance").value();
+    }
+
+    @Override
+    public void start() {
+        up = robot.HighRiserLeft.getPosition() < UpperRiserLeftPos;
+    }
+
+    @Override
+    public void exec() {
+        if (System.currentTimeMillis() - lastStepTime >= time) {
+            lastStepTime = System.currentTimeMillis();
+
+            if (robot.HighRiserLeft.getPosition() < UpperRiserLeftPos && up) {
+
+                robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + AddedDistance);
+                robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + AddedDistance);
+
+            } else if (robot.HighRiserLeft.getPosition() > UpperRiserLeftPos && !up) {
+
+                robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - AddedDistance);
+                robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - AddedDistance);
+
+            } else {
+                setHasFinished(true);
+            }
+        }
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("UpperRiserRightPos",UpperRiserRightPos);
+        engine.telemetry.addData("UpperRiserLeftPos",UpperRiserLeftPos);
+        engine.telemetry.addData("AddedDistance",AddedDistance);
+        engine.telemetry.addData("time",time);
+        engine.telemetry.addData("servo position left", robot.HighRiserLeft.getPosition());
+        engine.telemetry.addData("servo position Right", robot.HighRiserRight.getPosition());
+
+
+
+    }
+}

TeamCode/src/main/java/org/timecrafters/Autonomous/States/UpperArm.java

@@ -1,34 +0,0 @@
-package org.timecrafters.Autonomous.States;
-
-import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.testing.states.PrototypeBot1;
-
-public class UpperArm extends CyberarmState {
-
-    PrototypeBot1 robot;
-    double UpperRiserRightPos, UpperRiserLeftPos, AddedDistance;
-    long time;
-    long lastStepTime = 0;
-
-    public UpperArm(PrototypeBot1 robot, String groupName, String actionName) {
-        this.robot = robot;
-        this.UpperRiserLeftPos = robot.configuration.variable(groupName, actionName, "LowerRiserLeftPos").value();
-        this.UpperRiserRightPos = robot.configuration.variable(groupName, actionName, "LowerRiserRightPos").value();
-        this.time = robot.configuration.variable(groupName, actionName, "time").value();
-        this.AddedDistance = robot.configuration.variable(groupName, actionName, "AddedDistance").value();
-    }
-
-    @Override
-    public void exec() {
-        if (robot.HighRiserLeft.getPosition() > UpperRiserLeftPos) {
-            if (System.currentTimeMillis() - lastStepTime >= time) {
-                lastStepTime = System.currentTimeMillis();
-                robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - AddedDistance);
-                robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - AddedDistance);
-            } else {
-                setHasFinished(true);
-
-            }
-        }
-    }
-}

TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java

@@ -48,8 +48,8 @@ public class PrototypeBot1 {
         public PrototypeBot1(CyberarmEngine engine) {
             this.engine = engine;
 
-            initVuforia();
+//            initVuforia();
-            initTfod();
+//            initTfod();
             setupRobot();
         }
 
@@ -66,7 +66,7 @@ public class PrototypeBot1 {
 
             imu.startAccelerationIntegration(new Position(), new Velocity(), 10);
 
-            ///configuration = new TimeCraftersConfiguration("Phoenix");
+            configuration = new TimeCraftersConfiguration("Phoenix");
 
             //motors configuration
             frontLeftDrive = engine.hardwareMap.dcMotor.get("Front Left");
@@ -99,6 +99,17 @@ public class PrototypeBot1 {
 
             backRightDrive.setDirection(DcMotorSimple.Direction.FORWARD);
             backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+            HighRiserLeft.setDirection(Servo.Direction.REVERSE);
+            HighRiserRight.setDirection(Servo.Direction.FORWARD);
+            LowRiserLeft.setDirection(Servo.Direction.FORWARD);
+            LowRiserRight.setDirection(Servo.Direction.REVERSE);
+
+            LowRiserLeft.setPosition(0.45);
+            LowRiserRight.setPosition(0.45);
+            HighRiserLeft.setPosition(0.45);
+            HighRiserRight.setPosition(0.45);
+
         }
 
             private void initVuforia(){