Added theoretical servo controller that provides a position estimate, added drivetrain control for MentorBot Phoenix implementation, misc. tweaks.

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/PositionalServoController.java

@@ -0,0 +1,79 @@
+package org.timecrafters.minibots.cyberarm.phoenix;
+
+import com.qualcomm.robotcore.hardware.Servo;
+
+public class PositionalServoController {
+    final private Servo servo;
+    final private double targetDegreesPerSecond, servoDegreesPerSecond, servoRangeInDegrees;
+
+    private double lastEstimatedPosition, estimatedPosition, workingPosition, targetPosition;
+    private long lastUpdatedAt;
+    private boolean travelling = false;
+
+    public PositionalServoController(Servo servo, double targetDegreesPerSecond, double servoDegreesPerSecond, double servoRangeInDegrees) {
+        this.servo = servo;
+        this.targetDegreesPerSecond = targetDegreesPerSecond;
+        this.servoDegreesPerSecond = servoDegreesPerSecond;
+        this.servoRangeInDegrees = servoRangeInDegrees;
+
+        this.workingPosition = servo.getPosition();
+        this.estimatedPosition = workingPosition;
+        this.lastEstimatedPosition = estimatedPosition;
+        this.lastUpdatedAt = System.currentTimeMillis();
+    }
+
+    public Servo getServo() {
+        return servo;
+    }
+
+    public void update() {
+        double error = targetPosition - estimatedPosition;
+        double delta = (System.currentTimeMillis() - lastUpdatedAt) / 1000.0;
+        double estimatedTravel = servoDegreesPerSecond * delta;
+        double targetTravel = targetDegreesPerSecond * delta;
+
+        if (targetTravel < estimatedTravel) {
+            estimatedTravel = targetTravel;
+        }
+
+        if (travelling) {
+            this.lastEstimatedPosition = estimatedPosition;
+
+            if (error < 0.0) {
+                this.estimatedPosition -= estimatedTravel;
+            } else {
+                this.estimatedPosition += estimatedTravel;
+            }
+        }
+
+        if (error < 0.0 - estimatedTravel) {
+            workingPosition -= estimatedTravel;
+            travelling = true;
+        } else if (error > 0.0 + estimatedTravel) {
+            workingPosition += estimatedTravel;
+            travelling = true;
+        } else {
+            travelling = false;
+        }
+
+        servo.setPosition(workingPosition);
+        this.lastUpdatedAt = System.currentTimeMillis();
+    }
+
+    public void setTargetPosition(double targetPosition) {
+        this.targetPosition = targetPosition;
+    }
+
+    public double getEstimatedPosition() {
+        return estimatedPosition;
+    }
+
+    public double getEstimatedAngle() {
+        return estimatedPosition * servoRangeInDegrees;
+    }
+
+    private double lerp(double min, double max, double t)
+    {
+        return min + (max - min) * t;
+    }
+}

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/Robot.java

@@ -3,18 +3,15 @@ package org.timecrafters.minibots.cyberarm.phoenix;
 import android.annotation.SuppressLint;
 import android.util.Log;
 
-import com.acmerobotics.roadrunner.geometry.Vector2d;
 import com.qualcomm.hardware.lynx.LynxModule;
 import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
 import com.qualcomm.robotcore.hardware.Blinker;
-import com.qualcomm.robotcore.hardware.CRServoImplEx;
+import com.qualcomm.robotcore.hardware.CRServo;
-import com.qualcomm.robotcore.hardware.ColorSensor;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.DcMotorSimple;
 import com.qualcomm.robotcore.hardware.IMU;
 import com.qualcomm.robotcore.hardware.Servo;
-import com.qualcomm.robotcore.hardware.ServoImplEx;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
 import org.firstinspires.ftc.robotcore.external.ClassFactory;
@@ -27,7 +24,6 @@ import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
 import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
 import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
 import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable;
-import org.timecrafters.minibots.cyberarm.chiron.tasks.FieldLocalizer;
 
 import java.util.ArrayList;
 import java.util.concurrent.ConcurrentHashMap;
@@ -37,6 +33,9 @@ import java.util.concurrent.TimeUnit;
 public class Robot {
     private static final String TAG = "Phoenix | Robot";
     public final DcMotorEx backLeftDrive, frontRightDrive, frontLeftDrive, backRightDrive, arm;
+    public final PositionalServoController leftRiserServoController, rightRiserServoController;
+    public final Servo cameraServo;
+    public final CRServo collectorLeftServo, collectorRightServo;
 //    public final ServoImplEx gripper;
     public final IMU imu;
     public LynxModule expansionHub;
@@ -52,6 +51,7 @@ public class Robot {
     private final ConcurrentHashMap<String, Double> motorVelocityError = new ConcurrentHashMap<>();
     private final ConcurrentHashMap<String, Double> motorVelocityLastTiming = new ConcurrentHashMap<>();
     private final ConcurrentHashMap<String, Double> motorTargetVelocity = new ConcurrentHashMap<>();
+    private ArmPosition armTargetPosition;
 
     public enum ArmPosition {
         COLLECT,
@@ -147,6 +147,33 @@ public class Robot {
         //      MOTOR POWER
         arm.setPower(tuningConfig("arm_automatic_power").value());
 
+        //   SERVOS
+        //      NOTES: 428.57142858° per second = no load speed of 60° per 0.14s at 6V, see: https://docs.revrobotics.com/duo-build/actuators/servos/smart-robot-servo#mechanical-specifications
+        leftRiserServoController = new PositionalServoController(
+                engine.hardwareMap.servo.get("LowRiserLeft"),
+                5.0,
+                428.57142858,
+                270.0
+        ); // SERVER PORT: ? - ? HUB
+
+        rightRiserServoController = new PositionalServoController(
+                engine.hardwareMap.servo.get("LowRiserRight"),
+                5.0,
+                428.57142858,
+                270.0
+        ); // SERVER PORT: ? - ? HUB
+
+        cameraServo = engine.hardwareMap.servo.get("Camera Servo"); // SERVER PORT: ? - ? HUB
+
+        collectorLeftServo = engine.hardwareMap.crservo.get("Collector Left");   // SERVER PORT: ? - ? HUB
+        collectorRightServo = engine.hardwareMap.crservo.get("Collector Right"); // SERVER PORT: ? - ? HUB
+
+        //      SERVO DIRECTIONS
+        cameraServo.setDirection(Servo.Direction.REVERSE);
+        collectorLeftServo.setDirection(DcMotorSimple.Direction.REVERSE);
+        collectorRightServo.setDirection(DcMotorSimple.Direction.FORWARD);
+
+        //  SENSORS
         //      IMU
         IMU.Parameters parameters = new IMU.Parameters(
                 new RevHubOrientationOnRobot(
@@ -394,6 +421,8 @@ public class Robot {
                 if (lastStatus == Status.DANGER) { expansionHub.setPattern(ledPatternDanger()); }
             }
         }
+
+        manageArmAndRiserServos();
     }
 
     public void stop() {
@@ -407,32 +436,67 @@ public class Robot {
             return;
         }
 
+        armTargetPosition = position;
+
         reportStatus(Status.WARNING);
+    }
 
-        switch (position) {
+    private void manageArmAndRiserServos() {
+        boolean lowerServos = true;
+
+        switch (armTargetPosition) {
             case COLLECT:
-                arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_collect").value()));
+                if (areRiserServosInLoweredPosition()) {
+                    arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_collect").value()));
+                }
                 break;
 
-            case GROUND:
+//            case GROUND:
-                arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_ground").value()));
+//                if (areRiserServosInLoweredPosition()) {
-                break;
+//                    arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_ground").value()));
+//                }
+//                break;
 
+            case GROUND: // DISABLE GROUND JUNCTION SUPPORT FOR NOW
             case LOW:
-                arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_low").value()));
+                if (areRiserServosInLoweredPosition()) {
+                    arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_low").value()));
+                }
                 break;
 
             case MEDIUM:
-                arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_medium").value()));
+                if (areRiserServosInLoweredPosition()) {
+                    arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_medium").value()));
+                }
                 break;
 
             case HIGH:
-                arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_high").value()));
+                double angleHigh = tuningConfig("arm_position_angle_high").value();
+                arm.setTargetPosition(angleToTicks(angleHigh));
+
+                if (arm.getCurrentPosition() >= angleToTicks(angleHigh - 10.0)) {
+                    lowerServos = false;
+                }
                 break;
 
             default:
                 throw new RuntimeException("Unexpected arm position!");
         }
+
+        if (lowerServos) {
+            leftRiserServoController.setTargetPosition(0.45);
+            rightRiserServoController.setTargetPosition(0.45);
+        } else {
+            leftRiserServoController.setTargetPosition(0.8);
+            rightRiserServoController.setTargetPosition(0.8);
+        }
+
+        leftRiserServoController.update();
+        rightRiserServoController.update();
+    }
+
+    private boolean areRiserServosInLoweredPosition() {
+        return leftRiserServoController.getEstimatedPosition() < 0.5 && rightRiserServoController.getEstimatedPosition() < 0.5;
     }
 
     // Adapted from: https://github.com/gosu/gosu/blob/980d64de2ce52e4b16fdd5cb9c9e11c8bbb80671/src/Math.cpp#L38

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/states/teleop/ArmController.java

@@ -11,6 +11,7 @@ import org.timecrafters.minibots.cyberarm.phoenix.Robot;
 public class ArmController extends CyberarmState {
     private final Robot robot;
     private final String groupName, actionName;
+    private final Gamepad controller;
     private PIDController pidController;
     private double p = 0, i = 0, d = 0, f = 0;
     private final double ticksInDegree = 700 / 180;
@@ -20,6 +21,8 @@ public class ArmController extends CyberarmState {
         this.groupName = groupName;
         this.actionName = actionName;
 
+        this.controller = engine.gamepad1;
+
         pidController = new PIDController(p, i, d);
     }
 
@@ -38,7 +41,7 @@ public class ArmController extends CyberarmState {
                 case "derivative":
                     d = v.value();
                     break;
-                case "feed":
+                case "feed": // feedback
                     f = v.value();
                     break;
             }
@@ -56,7 +59,7 @@ public class ArmController extends CyberarmState {
 
     @Override
     public void buttonDown(Gamepad gamepad, String button) {
-        if (gamepad != engine.gamepad2) {
+        if (gamepad != controller) {
             return;
         }
 
@@ -64,17 +67,55 @@ public class ArmController extends CyberarmState {
             case "guide":
                 robot.reloadConfig();
                 break;
+
+            // Arm control
             case "a":
                 robot.armPosition(Robot.ArmPosition.COLLECT);
                 break;
             case "x":
-                robot.armPosition(Robot.ArmPosition.GROUND);
+                robot.armPosition(Robot.ArmPosition.LOW); // DISABLED GROUND JUNCTION
                 break;
             case "b":
-                robot.armPosition(Robot.ArmPosition.LOW);
+                robot.armPosition(Robot.ArmPosition.MEDIUM);
                 break;
             case "y":
-                robot.armPosition(Robot.ArmPosition.MEDIUM);
+                robot.armPosition(Robot.ArmPosition.HIGH);
+                break;
+            // Manual stepping arm
+            case "left_bumper":
+                robot.arm.setTargetPosition(
+                        robot.arm.getCurrentPosition() - robot.angleToTicks(5.0)
+                );
+                break;
+            case "right_bumper":
+                robot.arm.setTargetPosition(
+                        robot.arm.getCurrentPosition() + robot.angleToTicks(5.0)
+                );
+                break;
+
+            // Collector control
+            case "dpad_up":
+                robot.collectorLeftServo.setPower(1);
+                robot.collectorRightServo.setPower(1);
+                break;
+            case "dpad_down":
+                robot.collectorLeftServo.setPower(-1);
+                robot.collectorRightServo.setPower(-1);
+        }
+    }
+
+    @Override
+    public void buttonUp(Gamepad gamepad, String button) {
+        if (gamepad != controller) {
+            return;
+        }
+
+        switch (button) {
+            // Collector control
+            case "dpad_up":
+            case "dpad_down":
+                robot.collectorLeftServo.setPower(0);
+                robot.collectorRightServo.setPower(0);
                 break;
         }
     }

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/states/teleop/DriveController.java

@@ -0,0 +1,118 @@
+package org.timecrafters.minibots.cyberarm.phoenix.states.teleop;
+
+import com.qualcomm.robotcore.hardware.Gamepad;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.timecrafters.minibots.cyberarm.phoenix.Robot;
+
+public class DriveController extends CyberarmState {
+    private final Robot robot;
+    private final String groupName, actionName;
+
+    private final Gamepad controller;
+    private boolean robotSlowMode;
+
+    public DriveController(Robot robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.groupName = groupName;
+        this.actionName = actionName;
+
+        this.controller = engine.gamepad1;
+
+        this.robotSlowMode = true;
+    }
+
+    @Override
+    public void exec() {
+        move();
+    }
+
+    @Override
+    public void stop() {
+        stopDrive();
+    }
+
+    @Override
+    public void buttonDown(Gamepad gamepad, String button) {
+        if (gamepad != controller) {
+            return;
+        }
+
+        // DEBUG: Toggle hardware fault
+        if (button.equals("guide")) {
+            robot.hardwareFault = !robot.hardwareFault;
+
+            if (robot.hardwareFault) {
+                robot.hardwareFaultMessage = "Manually triggered.";
+            } else {
+                robot.hardwareFaultMessage = "";
+            }
+        }
+
+        if (button.equals("right_stick_button")) {
+            robotSlowMode = !robotSlowMode;
+        }
+
+        if (button.equals("left_stick_button") && robot.hardwareFault) {
+            robot.imu.resetYaw();
+        }
+    }
+
+    // REF: https://gm0.org/en/latest/docs/software/tutorials/mecanum-drive.html
+    private void move() {
+        if (robot.automaticAntiTipActive || robot.hardwareFault) {
+            return;
+        }
+
+        double x = -controller.left_stick_x;
+        double y = -controller.left_stick_y;
+
+        // Improve control?
+        if (y < 0) {
+            y = -Math.sqrt(-y);
+        } else {
+            y = Math.sqrt(y);
+        }
+
+        if (x < 0) {
+            x = -Math.sqrt(-x) * 1.1; // Counteract imperfect strafing;
+        } else {
+            x = Math.sqrt(x) * 1.1; // Counteract imperfect strafing;
+        }
+
+        double rx = -controller.right_stick_x;
+        double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
+
+
+        double frontLeftPower = 0, frontRightPower = 0, backLeftPower = 0 , backRightPower = 0;
+
+        double heading = robot.heading();
+        double rotX = x * Math.cos(heading) - y * Math.sin(heading);
+        double rotY = x * Math.sin(heading) + y * Math.cos(heading);
+
+        frontLeftPower = (rotY + rotX + rx) / denominator;
+        backLeftPower = (rotY - rotX - rx) / denominator;
+        frontRightPower = (rotY - rotX + rx) / denominator;
+        backRightPower = (rotY + rotX - rx) / denominator;
+
+        double maxVelocity = robot.unitToTicks(DistanceUnit.INCH, robot.tuningConfig("drivetrain_max_velocity_in_inches").value());
+        double slowVelocity = robot.unitToTicks(DistanceUnit.INCH, robot.tuningConfig("drivetrain_slow_velocity_in_inches").value());
+        double velocity = robotSlowMode ? slowVelocity : maxVelocity;
+
+        // Power is treated as a ratio here
+        robot.frontLeftDrive.setVelocity(frontLeftPower * velocity);
+        robot.frontRightDrive.setVelocity(frontRightPower * velocity);
+
+        robot.backLeftDrive.setVelocity(backLeftPower * velocity);
+        robot.backRightDrive.setVelocity(backRightPower * velocity);
+    }
+
+    private void stopDrive() {
+        robot.backLeftDrive.setVelocity(0); robot.backLeftDrive.setPower(0);
+        robot.frontRightDrive.setVelocity(0); robot.frontRightDrive.setPower(0);
+
+        robot.frontLeftDrive.setVelocity(0); robot.frontLeftDrive.setPower(0);
+        robot.backRightDrive.setVelocity(0); robot.backRightDrive.setPower(0);
+    }
+}