autnomous work added

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

@@ -47,12 +47,27 @@ public class TestAutonomousEngine extends CyberarmEngine {
         addState(new RotationState(robot, "TestAutonomous", "12-0"));
         //adjust arm height to cone.
         addState(new TopArm(robot, "TestAutonomous", "13-0"));
-        //drive towards stack of cones
+        //drive towards stack of cones while collecting
-        addState(new CollectorDistanceState(robot, "", ""));
+        addState(new CollectorDistanceState(robot, "TestAutonomous", "14-0"));
-//        DriverState driverState = new DriverState(robot, "TestAutonomous", "14-0");
+        //drive slightly back
-//        addState(driverState);
+        addState(new DriverState(robot, "TestAutonomous", "15-0"));
-//        //collect next cone
+        //lift arm up
-//        driverState.addParallelState(new CollectorState(robot, "TestAutonomous", "15-0"));
+        addState(new TopArm(robot, "TestAutonomous", "16-0"));
+        // drive backwards too position
+        addState(new DriverState(robot, "TestAutonomous", "17-0"));
+        // rotate
+        addState(new RotationState(robot, "TestAutonomous", "18-0"));
+        //lift the upper arm
+        addState(new TopArm(robot, "TestAutonomous", "19-0"));
+        //lift the lower arm
+        addState(new BottomArm(robot, "TestAutonomous", "20-0"));
+        //drive forward to allign
+        addState(new DriverState(robot, "TestAutonomous", "21-0"));
+        // bring arm down.
+        addState(new TopArm(robot, "TestAutonomous", "22-0"));
+        // get rid of cone
+        addState(new CollectorState(robot, "TestAutonomous", "23-0"));
+
 
 
 

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

@@ -9,6 +9,37 @@ import org.timecrafters.testing.states.PrototypeBot1;
 public class CollectorDistanceState extends CyberarmState {
 
     private final PrototypeBot1 robot;
+    private int traveledDistance;
+    private int RampUpDistance;
+    private int RampDownDistance;
+    private double drivePower;
+    private double targetDrivePower;
+
+    public CollectorDistanceState(PrototypeBot1 robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value();
+        this.traveledDistance = robot.configuration.variable(groupName, actionName, "traveledDistance").value();
+        this.RampUpDistance = robot.configuration.variable(groupName, actionName, "RampUpDistance").value();
+        this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value();
+
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("Distance Sensor", robot.collectorDistance.getDistance(DistanceUnit.MM));
+        engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getCurrentPosition());
+        engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition());
+        engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition());
+        engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition());
+
+        engine.telemetry.addData("drivePower", drivePower);
+        engine.telemetry.addData("targetDrivePower", targetDrivePower);
+
+        engine.telemetry.addData("traveledDistance", traveledDistance);
+        engine.telemetry.addData("RampUpDistance", RampUpDistance);
+        engine.telemetry.addData("RampDownDistance", RampDownDistance);
+
+    }
 
     @Override
     public void start() {
@@ -33,10 +64,35 @@ public class CollectorDistanceState extends CyberarmState {
     @Override
     public void exec() {
     if (robot.collectorDistance.getDistance(DistanceUnit.MM) > 60) {
-        robot.frontRightDrive.setPower(0.15);
+        double delta = traveledDistance - Math.abs(robot.frontRightDrive.getCurrentPosition());
-        robot.frontLeftDrive.setPower(0.15);
+
-        robot.backRightDrive.setPower(0.15);
+        if (Math.abs(robot.frontRightDrive.getCurrentPosition()) <= RampUpDistance){
-        robot.backLeftDrive.setPower(0.15);
+            // ramping up
+            drivePower = (Math.abs((float)robot.frontRightDrive.getCurrentPosition()) / RampUpDistance) + 0.15;
+        }
+        else if (Math.abs(robot.frontRightDrive.getCurrentPosition()) >= delta){
+            // ramping down
+            drivePower = ((delta / RampDownDistance) + 0.15);
+        } else {
+            // middle ground
+            drivePower = targetDrivePower;
+        }
+
+        if (Math.abs(drivePower) > Math.abs(targetDrivePower)){
+            // This is limiting drive power to the targeted drive power
+            drivePower = targetDrivePower;
+        }
+
+        if (targetDrivePower < 0 && drivePower != targetDrivePower) {
+            drivePower = drivePower * -1;
+        }
+
+        if (Math.abs(robot.frontRightDrive.getCurrentPosition()) < traveledDistance){
+            robot.backLeftDrive.setPower(drivePower);
+            robot.backRightDrive.setPower(drivePower);
+            robot.frontLeftDrive.setPower(drivePower);
+            robot.frontRightDrive.setPower(drivePower);
+        }
     } else {
         robot.frontRightDrive.setPower(0);
         robot.frontLeftDrive.setPower(0);
@@ -49,9 +105,7 @@ public class CollectorDistanceState extends CyberarmState {
     }
 
     }
-    public CollectorDistanceState(PrototypeBot1 robot, String groupName, String actionName) {
+
-        this.robot = robot;
-}
 
 
 }

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

@@ -42,24 +42,22 @@ public class DriverState extends CyberarmState {
             return;
         }
 
-        double delta = traveledDistance - robot.frontRightDrive.getCurrentPosition();
+        double delta = traveledDistance - Math.abs(robot.frontRightDrive.getCurrentPosition());
 
         if (Math.abs(robot.frontRightDrive.getCurrentPosition()) <= RampUpDistance){
-           drivePower = (((float)robot.frontRightDrive.getCurrentPosition()) / RampUpDistance) + 0.25;
+            // ramping up
+           drivePower = (Math.abs((float)robot.frontRightDrive.getCurrentPosition()) / RampUpDistance) + 0.25;
         }
         else if (Math.abs(robot.frontRightDrive.getCurrentPosition()) >= delta){
-            drivePower = (((delta - RampDownDistance) / RampDownDistance) + 0.25) - 1;
+            // ramping down
-
+            drivePower = ((delta / RampDownDistance) + 0.25);
-            if (drivePower < 0) {
+            } else {
-
+            // middle ground
-                drivePower = 0.25;
-
-            }
-        } else {
             drivePower = targetDrivePower;
         }
 
         if (Math.abs(drivePower) > Math.abs(targetDrivePower)){
+            // This is limiting drive power to the targeted drive power
             drivePower = targetDrivePower;
         }
 

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

@@ -18,6 +18,24 @@ public class RotationState extends CyberarmState {
     private double drivePower;
     private float targetRotation;
     float RobotRotation;
+    private double RotationTarget, DeltaRotation;
+    private double RotationDirectionMinimum;
+
+
+    public void CalculateDeltaRotation() {
+        if (RotationTarget >= 0 && RobotRotation >= 0) {
+            DeltaRotation = Math.abs(RotationTarget - RobotRotation);
+        }
+        else if (RotationTarget <= 0 && RobotRotation <= 0) {
+            DeltaRotation = Math.abs(RotationTarget - RobotRotation);
+        }
+        else if (RotationTarget >= 0 && RobotRotation <= 0) {
+            DeltaRotation = Math.abs(RotationTarget + RobotRotation);
+        }
+        else if (RotationTarget <=0 && RobotRotation >= 0) {
+            DeltaRotation = Math.abs(RobotRotation + RotationTarget);
+        }
+    }
 
     @Override
     public void exec() {
@@ -27,6 +45,14 @@ public class RotationState extends CyberarmState {
         }
 
         RobotRotation = robot.imu.getAngularOrientation().firstAngle;
+        RotationTarget = targetRotation;
+        CalculateDeltaRotation();
+        if (drivePower < 0){
+            RotationDirectionMinimum = -0.3;
+        } else {
+            RotationDirectionMinimum = 0.3;
+        }
+        drivePower = (drivePower * DeltaRotation/180) + RotationDirectionMinimum;
         if (RobotRotation <= targetRotation -3 || RobotRotation >= targetRotation + 3) {
             robot.backLeftDrive.setPower(-drivePower);
             robot.backRightDrive.setPower(drivePower);

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

@@ -117,8 +117,8 @@ public class PrototypeBot1 {
             LowRiserLeft.setDirection(Servo.Direction.FORWARD);
             LowRiserRight.setDirection(Servo.Direction.REVERSE);
 
-            LowRiserLeft.setPosition(0.45);
+            LowRiserLeft.setPosition(0.35);
-            LowRiserRight.setPosition(0.45);
+            LowRiserRight.setPosition(0.35);
             HighRiserLeft.setPosition(0.45);
             HighRiserRight.setPosition(0.45);