An Autonomous implemented for Yellow/Vexy on RED LEFT and BLUE RIGHT

TeamCode/src/main/java/dev/cyberarm/minibots/yellow/YellowMinibot.java

@@ -3,22 +3,37 @@ package dev.cyberarm.minibots.yellow;
 import com.arcrobotics.ftclib.hardware.motors.MotorEx;
 import com.arcrobotics.ftclib.hardware.motors.MotorGroup;
 import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.hardware.CRServo;
 import com.qualcomm.robotcore.hardware.IMU;
 
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
+import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
+
 import dev.cyberarm.engine.V2.CyberarmEngine;
 
 public class YellowMinibot {
+    private final CyberarmEngine engine;
+    public final TimeCraftersConfiguration config;
     public MotorEx leftFront, rightFront, leftBack, rightBack;
     public MotorGroup left, right;
     public IMU imu;
+    public CRServo droneLauncher;
+    public final double wheelCircumference, distancePerTick, imuAngleOffset, initialFacing;
     public YellowMinibot(CyberarmEngine engine) {
+        this.engine = engine;
+
+        this.config = new TimeCraftersConfiguration("Vexy_2023");
+
         leftFront = new MotorEx(engine.hardwareMap, "leftFront");
         rightFront = new MotorEx(engine.hardwareMap, "rightFront");
 
         leftBack = new MotorEx(engine.hardwareMap, "leftBack");
         rightBack = new MotorEx(engine.hardwareMap, "rightBack");
 
-        double distancePerTick = 1962.5; // 3.14 * 25mm * 25mm
+        wheelCircumference = Math.PI * (50.0 * 2); //wheelRadius * wheelRadius;
+        distancePerTick = 288 / wheelCircumference; // raw motor encoder * gear ratio
+
         leftFront.setDistancePerPulse(distancePerTick);
         rightFront.setDistancePerPulse(distancePerTick);
         leftBack.setDistancePerPulse(distancePerTick);
@@ -34,5 +49,94 @@ public class YellowMinibot {
                         RevHubOrientationOnRobot.UsbFacingDirection.RIGHT));
 
         imu.initialize(parameters);
+
+        imuAngleOffset = 0;
+        initialFacing = facing();
+
+        droneLauncher = engine.hardwareMap.crservo.get("droneLauncher"); /// Port 5
+    }
+
+    public void standardTelemetry() {
+        engine.telemetry.addLine();
+
+        engine.telemetry.addLine("Motors");
+        engine.telemetry.addData(
+                "Left Front",
+                "Power: %.2f, Current: %.2f mAmp, Position: %d",
+                leftFront.motorEx.getPower(),
+                leftFront.motorEx.getCurrent(CurrentUnit.MILLIAMPS),
+                leftFront.motorEx.getCurrentPosition());
+        engine.telemetry.addData(
+                "Right Front",
+                "Power: %.2f, Current: %.2f mAmp, Position: %d",
+                rightFront.motorEx.getPower(),
+                rightFront.motorEx.getCurrent(CurrentUnit.MILLIAMPS),
+                rightFront.motorEx.getCurrentPosition());
+        engine.telemetry.addData(
+                "Left Back",
+                "Power: %.2f, Current: %.2f mAmp, Position: %d",
+                leftBack.motorEx.getPower(),
+                leftBack.motorEx.getCurrent(CurrentUnit.MILLIAMPS),
+                leftBack.motorEx.getCurrentPosition());
+        engine.telemetry.addData(
+                "Right Back",
+                "Power: %.2f, Current: %.2f mAmp, Position: %d",
+                rightBack.motorEx.getPower(),
+                rightBack.motorEx.getCurrent(CurrentUnit.MILLIAMPS),
+                rightBack.motorEx.getCurrentPosition());
+
+        engine.telemetry.addLine();
+        engine.telemetry.addLine("Servos");
+        engine.telemetry.addData("droneLauncher", droneLauncher.getPower());
+
+        engine.telemetry.addLine();
+    }
+
+    public void teleopTelemetry() {
+        engine.telemetry.addLine();
+
+        engine.telemetry.addLine();
+    }
+
+    public double distanceMM(int ticks) {
+        return distancePerTick * ticks;
+    }
+
+
+    public double initialFacing() {
+        return initialFacing;
+    }
+
+    public double facing() {
+        double imuDegrees = -imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
+
+        return (((imuDegrees + 360.0) % 360.0) + imuAngleOffset) % 360.0;
+    }
+
+    public double heading() {
+        return AngleUnit.normalizeRadians(-facing() * Math.PI / 180.0);
+//        return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
+    }
+
+    public double turnRate() {
+        return imu.getRobotAngularVelocity(AngleUnit.DEGREES).yRotationRate; // NOTE: UNTESTED
+    }
+
+    public boolean isBetween(double value, double min, double max) {
+        return value >= min && value <= max;
+    }
+
+    // Adapted from: https://github.com/gosu/gosu/blob/980d64de2ce52e4b16fdd5cb9c9e11c8bbb80671/src/Math.cpp#L38
+    public double angleDiff(double from, double to) {
+        double value = (to - from + 180);
+
+        double fmod = (value - 0.0) % (360.0 - 0.0);
+
+        return (fmod < 0 ? fmod + 360.0 : fmod +  0.0) - 180;
+    }
+
+    public double lerp(double min, double max, double t)
+    {
+        return min + (max - min) * t;
     }
 }

TeamCode/src/main/java/dev/cyberarm/minibots/yellow/engines/YellowMinibotAutonomousBlueRightEngine.java

@@ -0,0 +1,78 @@
+package dev.cyberarm.minibots.yellow.engines;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import dev.cyberarm.engine.V2.CyberarmEngine;
+import dev.cyberarm.minibots.yellow.YellowMinibot;
+import dev.cyberarm.minibots.yellow.states.Rotate;
+import dev.cyberarm.minibots.yellow.states.TankMove;
+
+@Autonomous(name = "Cyberarm Yellow Autonomous BLUE RIGHT", group = "MINIBOT", preselectTeleOp = "Cyberarm Yellow Teleop")
+public class YellowMinibotAutonomousBlueRightEngine extends CyberarmEngine {
+    @Override
+    public void setup() {
+        YellowMinibot robot = new YellowMinibot(this);
+        robot.imu.resetYaw();
+
+        /// Move Purple pixel into position
+        addState(new TankMove(
+                robot,
+                1450,
+                1450,
+                -1.0,
+                -1.0,
+                10,
+                5000
+        ));
+        /// Move away from pixel
+        addState(new TankMove(
+                robot,
+                250,
+                250,
+                1.0,
+                1.0,
+                10,
+                5000
+        ));
+        /// Turn around
+        addState(new Rotate(
+                robot,
+                182,
+                1.0,
+                0.25,
+                1,
+                5000
+        ));
+        /// Move back towards yellow pixel
+        addState(new TankMove(
+                robot,
+                850,
+                850,
+                -1.0,
+                -1.0,
+                10,
+                5000
+        ));
+        /// Turn around
+        addState(new Rotate(
+                robot,
+                270,
+                1.0,
+                0.25,
+                1,
+                5000
+        ));
+        /// Move towards backstage
+        addState(new TankMove(
+                robot,
+                4000,
+                4000,
+                -1.0,
+                -1.0,
+                10,
+                8000
+        ));
+    }
+}
+
+

TeamCode/src/main/java/dev/cyberarm/minibots/yellow/engines/YellowMinibotAutonomousRedLeftEngine.java

@@ -0,0 +1,76 @@
+package dev.cyberarm.minibots.yellow.engines;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import dev.cyberarm.engine.V2.CyberarmEngine;
+import dev.cyberarm.minibots.yellow.YellowMinibot;
+import dev.cyberarm.minibots.yellow.states.Rotate;
+import dev.cyberarm.minibots.yellow.states.TankMove;
+
+@Autonomous(name = "Cyberarm Yellow Autonomous RED LEFT", group = "MINIBOT", preselectTeleOp = "Cyberarm Yellow Teleop")
+public class YellowMinibotAutonomousRedLeftEngine extends CyberarmEngine {
+    @Override
+    public void setup() {
+        YellowMinibot robot = new YellowMinibot(this);
+        robot.imu.resetYaw();
+
+        /// Move Purple pixel into position
+        addState(new TankMove(
+                robot,
+                1450,
+                1450,
+                -1.0,
+                -1.0,
+                10,
+                5000
+            ));
+        /// Move away from pixel
+        addState(new TankMove(
+                robot,
+                250,
+                250,
+                1.0,
+                1.0,
+                10,
+                5000
+            ));
+        /// Turn around
+        addState(new Rotate(
+                robot,
+                178,
+                1.0,
+                0.25,
+                1,
+                5000
+            ));
+        /// Move back towards yellow pixel
+        addState(new TankMove(
+                robot,
+                850,
+                850,
+                -1.0,
+                -1.0,
+                10,
+                5000
+        ));
+        /// Turn around
+        addState(new Rotate(
+                robot,
+                90,
+                1.0,
+                0.25,
+                1,
+                5000
+        ));
+        /// Move towards backstage
+        addState(new TankMove(
+                robot,
+                4000,
+                4000,
+                -1.0,
+                -1.0,
+                10,
+                8000
+        ));
+    }
+}

TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Move.java

@@ -1,34 +0,0 @@
-package dev.cyberarm.minibots.yellow.states;
-
-import dev.cyberarm.engine.V2.CyberarmState;
-import dev.cyberarm.minibots.yellow.YellowMinibot;
-
-public class Move extends CyberarmState {
-    final YellowMinibot robot;
-    final double leftDistance, rightDistance, leftPower, rightPower;
-    public Move(YellowMinibot robot, double leftDistance, double rightDistance, double leftPower, double rightPower) {
-        this.robot = robot;
-
-        this.leftDistance = leftDistance;
-        this.rightDistance = rightDistance;
-        this.leftPower = leftPower;
-        this.rightPower = rightPower;
-    }
-
-    @Override
-    public void start() {
-        robot.leftFront.setTargetDistance(leftDistance);
-        robot.leftBack.setTargetDistance(leftDistance);
-
-        robot.leftFront.setTargetDistance(rightDistance);
-        robot.leftBack.setTargetDistance(rightDistance);
-    }
-
-    @Override
-    public void exec() {
-        if (robot.leftFront.atTargetPosition()) {
-            robot.leftFront.setVelocity(0);
-            robot.leftBack.setVelocity(0);
-        }
-    }
-}

TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Pilot.java

@@ -18,6 +18,13 @@ public class Pilot extends CyberarmState {
             robot.imu.resetYaw();
         }
 
+        /// --- DRONE --- ///
+        if (engine.gamepad1.y) {
+            robot.droneLauncher.setPower(1.0);
+        } else if (engine.gamepad1.a) {
+            robot.droneLauncher.setPower(0.0);
+        }
+
         /// --- DRIVE --- ///
 //        robot.left.set(engine.gamepad1.left_stick_y);
 //        robot.right.set(engine.gamepad1.right_stick_y);

TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Rotate.java

@@ -0,0 +1,86 @@
+package dev.cyberarm.minibots.yellow.states;
+
+import com.qualcomm.robotcore.util.Range;
+
+import dev.cyberarm.engine.V2.CyberarmState;
+import dev.cyberarm.minibots.pizza.PizzaMinibot;
+import dev.cyberarm.minibots.yellow.YellowMinibot;
+
+public class Rotate extends CyberarmState {
+    final private YellowMinibot robot;
+    final private String groupName, actionName;
+
+    final private double heading, maxVelocity, minVelocity, tolerance;
+    final private int timeoutMS;
+
+    public Rotate(YellowMinibot robot, String groupName, String actionName) {
+        this.robot = robot;
+
+        this.groupName = groupName;
+        this.actionName = actionName;
+
+        this.heading = robot.config.variable(groupName, actionName, "heading").value();
+
+        this.minVelocity = robot.config.variable(groupName, actionName, "minVelocity").value();
+        this.maxVelocity = robot.config.variable(groupName, actionName, "maxVelocity").value();
+
+        this.tolerance = robot.config.variable(groupName, actionName, "tolerance").value();
+
+        this.timeoutMS  = robot.config.variable(groupName, actionName, "timeoutMS").value();
+    }
+
+    public Rotate(YellowMinibot robot, double heading, double maxVelocity, double minVelocity, double tolerance, int timeoutMS) {
+        this.groupName = "";
+        this.actionName = "";
+
+        this.robot = robot;
+        this.heading = heading;
+        this.maxVelocity = maxVelocity;
+        this.minVelocity = minVelocity;
+        this.tolerance = tolerance;
+        this.timeoutMS = timeoutMS;
+    }
+
+    @Override
+    public void start() {
+    }
+
+    @Override
+    public void exec() {
+        if (Math.abs(robot.angleDiff(robot.facing(), heading)) <= tolerance) {
+            robot.left.set(0);
+            robot.right.set(0);
+
+            setHasFinished(true);
+
+            return;
+        }
+
+        if (runTime() >= timeoutMS) {
+            robot.left.set(0);
+            robot.right.set(0);
+
+            setHasFinished(true);
+        }
+
+        double angleDiff = robot.angleDiff(robot.facing() + robot.turnRate(), heading);
+        double velocity = robot.lerp(minVelocity, maxVelocity, Range.clip(Math.abs(angleDiff) / 90.0, 0.0, 1.0));
+
+        if (angleDiff > 0) {
+            robot.left.set(-velocity);
+            robot.right.set(velocity);
+        } else {
+            robot.left.set(velocity);
+            robot.right.set(-velocity);
+        }
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addLine();
+        engine.telemetry.addData("Robot Heading", robot.facing());
+        engine.telemetry.addData("Robot Target Heading", heading);
+        engine.telemetry.addData("Robot Angle Diff", robot.angleDiff(robot.facing() + robot.turnRate(), heading));
+        engine.telemetry.addData("Robot Turn Rate", robot.turnRate());
+    }
+}

TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/ServoMove.java

@@ -0,0 +1,56 @@
+package dev.cyberarm.minibots.yellow.states;
+
+import com.qualcomm.robotcore.hardware.Servo;
+
+import dev.cyberarm.engine.V2.CyberarmState;
+import dev.cyberarm.minibots.pizza.PizzaMinibot;
+import dev.cyberarm.minibots.yellow.YellowMinibot;
+
+public class ServoMove extends CyberarmState {
+    final private YellowMinibot robot;
+    final private String groupName, actionName;
+
+    final private String servoName;
+    final private double position;
+    final private int timeoutMS;
+
+    final private Servo servo;
+    public ServoMove(YellowMinibot robot, String groupName, String actionName) {
+        this.robot = robot;
+
+        this.groupName = groupName;
+        this.actionName = actionName;
+
+        this.servoName = robot.config.variable(groupName, actionName, "servoName").value();
+
+        this.position = robot.config.variable(groupName, actionName, "position").value();
+
+        this.timeoutMS  = robot.config.variable(groupName, actionName, "timeoutMS").value();
+
+        this.servo = engine.hardwareMap.servo.get(servoName);
+    }
+
+    public ServoMove(YellowMinibot robot, String servoName, double position, int timeoutMS) {
+        this.groupName = "";
+        this.actionName = "";
+
+        this.robot = robot;
+        this.servoName = servoName;
+        this.position = position;
+        this.timeoutMS = timeoutMS;
+
+        this.servo = engine.hardwareMap.servo.get(servoName);
+    }
+
+    @Override
+    public void start() {
+        this.servo.setPosition(this.position);
+    }
+
+    @Override
+    public void exec() {
+        if (runTime() >= timeoutMS) {
+            setHasFinished(true);
+        }
+    }
+}

TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/StrafeMove.java

@@ -0,0 +1,63 @@
+package dev.cyberarm.minibots.yellow.states;
+
+import dev.cyberarm.engine.V2.CyberarmState;
+import dev.cyberarm.minibots.pizza.PizzaMinibot;
+import dev.cyberarm.minibots.yellow.YellowMinibot;
+
+public class StrafeMove extends CyberarmState {
+    final private YellowMinibot robot;
+    final private String groupName, actionName;
+
+    final private double distanceMM, velocity;
+    final private int tolerance, timeoutMS;
+
+    public StrafeMove(YellowMinibot robot, String groupName, String actionName) {
+        this.robot = robot;
+
+        this.groupName = groupName;
+        this.actionName = actionName;
+
+        this.distanceMM = robot.config.variable(groupName, actionName, "distanceMM").value();
+
+        this.velocity = robot.config.variable(groupName, actionName, "velocity").value();
+
+        this.tolerance = robot.config.variable(groupName, actionName, "tolerance").value();
+        this.timeoutMS  = robot.config.variable(groupName, actionName, "timeoutMS").value();
+    }
+
+    public StrafeMove(YellowMinibot robot, double distanceMM, double velocity, int tolerance, int timeoutMS) {
+        this.groupName = "";
+        this.actionName = "";
+
+        this.robot = robot;
+        this.distanceMM = distanceMM;
+        this.velocity = velocity;
+        this.tolerance = tolerance;
+        this.timeoutMS = timeoutMS;
+    }
+
+    @Override
+    public void start() {
+        robot.leftFront.setTargetDistance(distanceMM);
+
+        robot.left.setPositionTolerance(tolerance);
+        robot.right.setPositionTolerance(tolerance);
+
+        double motorVelocity = (distanceMM < 0 ? velocity * -1 : velocity);
+
+        robot.leftFront.set(motorVelocity);
+        robot.rightFront.set(-motorVelocity);
+
+        robot.leftBack.set(-motorVelocity);
+        robot.rightBack.set(motorVelocity);
+    }
+
+    @Override
+    public void exec() {
+        if (robot.left.atTargetPosition() || Math.abs(robot.leftFront.getDistance()) >= Math.abs(distanceMM) ||  runTime() >= timeoutMS) {
+            robot.left.set(0);
+            robot.right.set(0);
+            setHasFinished(true);
+        }
+    }
+}

TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/TankMove.java

@@ -0,0 +1,93 @@
+package dev.cyberarm.minibots.yellow.states;
+
+import dev.cyberarm.engine.V2.CyberarmState;
+import dev.cyberarm.minibots.pizza.PizzaMinibot;
+import dev.cyberarm.minibots.yellow.YellowMinibot;
+
+public class TankMove extends CyberarmState {
+    final private YellowMinibot robot;
+    final private String groupName, actionName;
+
+    final private double leftDistanceMM, rightDistanceMM;
+    final private double leftVelocity, rightVelocity;
+    final private int tolerance, timeoutMS;
+
+    public TankMove(YellowMinibot robot, String groupName, String actionName) {
+        this.robot = robot;
+
+        this.groupName = groupName;
+        this.actionName = actionName;
+
+        this.leftDistanceMM = robot.config.variable(groupName, actionName, "leftDistanceMM").value();
+        this.rightDistanceMM = robot.config.variable(groupName, actionName, "rightDistanceMM").value();
+
+        this.leftVelocity = robot.config.variable(groupName, actionName, "leftVelocity").value();
+        this.rightVelocity = robot.config.variable(groupName, actionName, "rightVelocity").value();
+
+        this.tolerance = robot.config.variable(groupName, actionName, "tolerance").value();
+        this.timeoutMS  = robot.config.variable(groupName, actionName, "timeoutMS").value();
+    }
+
+    public TankMove(YellowMinibot robot, double leftDistanceMM, double rightDistanceMM, double leftVelocity, double rightVelocity, int tolerance, int timeoutMS) {
+        this.groupName = "";
+        this.actionName = "";
+
+        this.robot = robot;
+        this.leftDistanceMM = leftDistanceMM;
+        this.rightDistanceMM = rightDistanceMM;
+        this.leftVelocity = leftVelocity;
+        this.rightVelocity = rightVelocity;
+        this.tolerance = tolerance;
+        this.timeoutMS = timeoutMS;
+    }
+
+    @Override
+    public void start() {
+        robot.left.resetEncoder();
+        robot.right.resetEncoder();
+
+        robot.left.setTargetDistance(leftDistanceMM);
+        robot.right.setTargetDistance(rightDistanceMM);
+
+        robot.left.setPositionTolerance(tolerance);
+        robot.right.setPositionTolerance(tolerance);
+
+        robot.left.set(leftVelocity);
+        robot.right.set(rightVelocity);
+    }
+
+    @Override
+    public void exec() {
+        if (robot.left.atTargetPosition() || Math.abs(robot.leftFront.getDistance()) >= Math.abs(leftDistanceMM)) {
+            robot.left.set(0);
+        }
+        if (robot.right.atTargetPosition() || Math.abs(robot.rightFront.getDistance()) >= Math.abs(leftDistanceMM)) {
+            robot.right.set(0);
+        }
+
+        if (
+                (robot.left.atTargetPosition() && robot.right.atTargetPosition()) ||
+                (Math.abs(robot.leftFront.getDistance()) >= Math.abs(leftDistanceMM) && robot.right.atTargetPosition() || Math.abs(robot.rightFront.getDistance()) >= Math.abs(leftDistanceMM))  ||
+                runTime() >= timeoutMS) {
+            robot.left.set(0);
+            robot.right.set(0);
+            setHasFinished(true);
+        }
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addLine();
+
+        engine.telemetry.addData("Left distance", robot.leftFront.getDistance());
+        engine.telemetry.addData("Left position", robot.left.getPositions().get(0));
+        engine.telemetry.addData("Left speed", robot.left.getSpeeds().get(0));
+        engine.telemetry.addData("Left velocity", robot.left.getVelocity());
+        engine.telemetry.addLine();
+
+        engine.telemetry.addData("Right distance", robot.rightFront.getDistance());
+        engine.telemetry.addData("Right position", robot.right.getPositions().get(0));
+        engine.telemetry.addData("Right speed", robot.right.getSpeeds().get(0));
+        engine.telemetry.addData("Right velocity", robot.right.getVelocity());
+    }
+}