Adding the autonomous, day 1

2026-03-23 05:46:12 +00:00 · 2022-10-20 20:02:08 -05:00
parent 7442efa1c9
commit cae56e4e20
5 changed files with 133 additions and 0 deletions
--- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorState.java
+++ b/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);
+        }
+
+
+
+    }
+
+    }
+
+
--- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java
+++ b/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);
+        }
+
+    }
+}