last meetings code that I forgot to push

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

@@ -53,6 +53,9 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine {
         // 7 Drop bottom arm down on the junction to place cone
         addState(new BottomArm(robot, "RightFourCone", "07-0"));
 
+        // 7-1 drive back slightly
+        addState(new DriverStateWithOdometer(robot, "RightFourCone", "07-1"));
+
         // 8 Drop cone as soon as arm is in position
         addState(new CollectorState(robot, "RightFourCone", "08-0"));
 
@@ -61,12 +64,12 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine {
 //
 //        // 10 Back up and bring lower arm down (parallel state)
         addState(new DriverStateWithOdometerLowerArmParallelState2nd(robot, "RightFourCone", "10-0"));
+
+        // 11 Rotate towards stack
+        addState(new RotationState(robot, "RightFourCone", "11-0"));
 //
-//        // 11 Bring upper arm to the correct position for the top cone on stack (check with distance sensor)
+//        // 12 Bring upper arm to the correct position for the top cone on stack (check with distance sensor)
-//        addState(new TopArm(robot, "RightFourCone", "11-0"));
+//        addState(new TopArm(robot, "RightFourCone", "12-0"));
-//
-//        // 12 Rotate towards stack
-//        //filled in as parallel state. in parallel with previous state
 //
 //        // 13 Drive at stack while collecting and check to see when we grab it
 //        addState(new CollectorDistanceState(robot, "RightFourCone", "13-0"));

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

@@ -1,5 +1,7 @@
 package org.timecrafters.Autonomous.States;
 
+import android.util.Log;
+
 import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.cyberarm.engine.V2.CyberarmState;
@@ -91,6 +93,22 @@ public class DriverStateWithOdometer extends CyberarmState {
             setHasFinished(true);
         }
 
+        if (!getHasFinished()){
+            float angle = robot.imu.getAngularOrientation().firstAngle;
+
+            if (angle < 0){
+                robot.backLeftDrive.setPower(-drivePower);
+                robot.backRightDrive.setPower(drivePower);
+                robot.frontLeftDrive.setPower(-drivePower);
+                robot.frontRightDrive.setPower(drivePower);
+            }
+            if (angle > 0){
+                robot.backLeftDrive.setPower(drivePower);
+                robot.backRightDrive.setPower(-drivePower);
+                robot.frontLeftDrive.setPower(drivePower);
+                robot.frontRightDrive.setPower(-drivePower);
+            }
+        }
 
     }
 
@@ -100,7 +118,15 @@ public class DriverStateWithOdometer extends CyberarmState {
         engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition());
         engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition());
         engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition());
+        engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getPower());
+        engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getPower());
+        engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getPower());
+        engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getPower());
         engine.telemetry.addData("Odometer", robot.OdometerEncoder.getCurrentPosition());
+        engine.telemetry.addData("imu 1 angle", robot.imu.getAngularOrientation().firstAngle);
+        engine.telemetry.addData("imu 2 angle", robot.imu.getAngularOrientation().secondAngle);
+        engine.telemetry.addData("imu 3 angle", robot.imu.getAngularOrientation().thirdAngle);
+
 
         engine.telemetry.addData("drivePower", drivePower);
         engine.telemetry.addData("targetDrivePower", targetDrivePower);
@@ -109,5 +135,9 @@ public class DriverStateWithOdometer extends CyberarmState {
         engine.telemetry.addData("RampUpDistance", RampUpDistance);
         engine.telemetry.addData("RampDownDistance", RampDownDistance);
 
+        Log.i("TELEMETRY", "imu 1 angle:: " + robot.imu.getAngularOrientation().firstAngle);
+        Log.i("TELEMETRY", "imu 2 angle:: " + robot.imu.getAngularOrientation().secondAngle);
+        Log.i("TELEMETRY", "imu 3 angle:: " + robot.imu.getAngularOrientation().thirdAngle);
+
     }
 }

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

@@ -1,5 +1,7 @@
 package org.timecrafters.Autonomous.States;
 
+import android.util.Log;
+
 import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.cyberarm.engine.V2.CyberarmState;
@@ -35,12 +37,14 @@ public class DriverStateWithOdometerUpperArmParallelState extends CyberarmState
         robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
         robot.backRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
         robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+        addParallelState(new TopArm(robot, "RightFourCone", "04-1"));
+
     }
 
     @Override
     public void exec() {
 
-        addParallelState(new TopArm(robot, "RightFourCone", "04-1"));
         if (stateDisabled) {
             setHasFinished(true);
             return;
@@ -89,6 +93,22 @@ public class DriverStateWithOdometerUpperArmParallelState extends CyberarmState
             robot.frontRightDrive.setPower(0);
             setHasFinished(true);
         }
+        if (!getHasFinished()){
+            float angle = robot.imu.getAngularOrientation().firstAngle;
+
+            if (angle < -0.25){
+                robot.backLeftDrive.setPower(drivePower * 0.25);
+                robot.backRightDrive.setPower(drivePower * 1.25);
+                robot.frontLeftDrive.setPower(drivePower * 0.25);
+                robot.frontRightDrive.setPower(drivePower * 1.25);
+            }
+            if (angle > 0.25){
+                robot.backLeftDrive.setPower(drivePower * 1.25);
+                robot.backRightDrive.setPower(drivePower * 0.25);
+                robot.frontLeftDrive.setPower(drivePower * 1.25);
+                robot.frontRightDrive.setPower(drivePower * 0.25);
+            }
+        }
 
 
     }
@@ -99,7 +119,15 @@ public class DriverStateWithOdometerUpperArmParallelState extends CyberarmState
         engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition());
         engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition());
         engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition());
+        engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getPower());
+        engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getPower());
+        engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getPower());
+        engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getPower());
         engine.telemetry.addData("Odometer", robot.OdometerEncoder.getCurrentPosition());
+        engine.telemetry.addData("imu 1 angle", robot.imu.getAngularOrientation().firstAngle);
+        engine.telemetry.addData("imu 2 angle", robot.imu.getAngularOrientation().secondAngle);
+        engine.telemetry.addData("imu 3 angle", robot.imu.getAngularOrientation().thirdAngle);
+
 
         engine.telemetry.addData("drivePower", drivePower);
         engine.telemetry.addData("targetDrivePower", targetDrivePower);
@@ -108,5 +136,9 @@ public class DriverStateWithOdometerUpperArmParallelState extends CyberarmState
         engine.telemetry.addData("RampUpDistance", RampUpDistance);
         engine.telemetry.addData("RampDownDistance", RampDownDistance);
 
+        Log.i("TELEMETRY", "imu 1 angle:: " + robot.imu.getAngularOrientation().firstAngle);
+        Log.i("TELEMETRY", "imu 2 angle:: " + robot.imu.getAngularOrientation().secondAngle);
+        Log.i("TELEMETRY", "imu 3 angle:: " + robot.imu.getAngularOrientation().thirdAngle);
+
     }
 }

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

@@ -53,7 +53,8 @@ public class TopArm extends CyberarmState {
                     robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + AddedDistance);
                     robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + AddedDistance);
 
-                } if (robot.HighRiserLeft.getPosition() > UpperRiserLeftPos && !up) {
+                }
+                 else if (robot.HighRiserLeft.getPosition() > UpperRiserLeftPos && !up) {
 
                     robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - AddedDistance);
                     robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - AddedDistance);