Adding the autonomous, day 1

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

@@ -0,0 +1,58 @@
+package org.timecrafters.Autonomous.States;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.testing.states.PrototypeBot1;
+
+public class CollectorState extends CyberarmState {
+
+    private final PrototypeBot1 robot;
+    private boolean collecting;
+    private long duration;
+    private long BeginningofActionTime;
+
+
+    public CollectorState(PrototypeBot1 robot, String groupName, String actionName) {
+        this.robot = robot;
+
+        this.duration = robot.configuration.variable(groupName, actionName, "Duration").value();
+        this.collecting = robot.configuration.variable(groupName, actionName, "Collecting").value();
+    }
+
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("High Riser Right Position", robot.HighRiserRight.getPosition());
+        engine.telemetry.addData("High Riser Left Position", robot.HighRiserLeft.getPosition());
+    }
+
+    @Override
+    public void start() {
+        BeginningofActionTime = System.currentTimeMillis();
+    }
+
+    @Override
+    public void exec() {
+
+        if (System.currentTimeMillis() - BeginningofActionTime < duration) {
+            if (collecting) {
+                robot.collectorRight.setPower(1);
+                robot.collectorLeft.setPower(1);
+            } else {
+                robot.collectorRight.setPower(-1);
+                robot.collectorLeft.setPower(-1);
+
+            }
+
+        } else {
+            robot.collectorLeft.setPower(0);
+            robot.collectorRight.setPower(0);
+            setHasFinished(true);
+        }
+
+
+
+    }
+
+    }
+
+

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

@@ -0,0 +1,36 @@
+package org.timecrafters.Autonomous.States;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.testing.states.PrototypeBot1;
+
+public class RotationState extends CyberarmState {
+    PrototypeBot1 robot;
+    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();
+    }
+
+    private float RobotRotation;
+    private double drivePower;
+
+    @Override
+    public void exec() {
+
+
+        RobotRotation = robot.imu.getAngularOrientation().firstAngle;
+        if (RobotRotation <= 45) {
+            robot.backLeftDrive.setPower(-drivePower);
+            robot.backRightDrive.setPower(drivePower);
+            robot.frontLeftDrive.setPower(-drivePower);
+            robot.frontRightDrive.setPower(drivePower);
+        } else
+        {
+            robot.backLeftDrive.setPower(0);
+            robot.backRightDrive.setPower(0);
+            robot.frontLeftDrive.setPower(0);
+            robot.frontRightDrive.setPower(0);
+        }
+
+    }
+}

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

@@ -0,0 +1,27 @@
+package org.timecrafters.testing.states;
+
+import com.qualcomm.hardware.bosch.BNO055IMU;
+import com.qualcomm.robotcore.hardware.CRServo;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.Servo;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+
+public class PhoenixWingEngine {
+
+    public Servo LowRiserLeft, LowRiserRight, HighRiserLeft, HighRiserRight;
+    private final CyberarmEngine engine;
+
+    public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive;
+
+    public CRServo collectorLeft, collectorRight;
+
+    public BNO055IMU imu;
+
+//        public Servo collectorWrist;
+
+    public PhoenixWingEngine(CyberarmEngine engine) {
+        this.engine = engine;
+
+        setupRobot();
+    }

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

@@ -0,0 +1,10 @@
+package org.timecrafters.testing.states;
+
+import org.cyberarm.engine.V2.CyberarmState;
+
+public class PhoenixWingState extends CyberarmState {
+    @Override
+    public void exec() {
+
+    }
+}

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

@@ -58,6 +58,8 @@ public class PrototypeBot1 {
             // Collector
             collectorLeft = engine.hardwareMap.crservo.get("Collector Left");
             collectorRight = engine.hardwareMap.crservo.get("Collector Right");
+            collectorLeft.setDirection(CRServo.Direction.REVERSE);
+            collectorRight.setDirection(CRServo.Direction.FORWARD);
 
             // Arm
             LowRiserLeft = engine.hardwareMap.servo.get("LowRiserLeft");