Merge remote-tracking branch 'origin/master'

TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java

@@ -1,9 +1,6 @@
 package dev.cyberarm.minibots.red_crab;
 
 import com.arcrobotics.ftclib.controller.PIDFController;
-import com.arcrobotics.ftclib.hardware.motors.Motor;
-import com.arcrobotics.ftclib.hardware.motors.MotorEx;
-import com.arcrobotics.ftclib.hardware.motors.MotorGroup;
 import com.qualcomm.hardware.lynx.LynxModule;
 import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
 import com.qualcomm.robotcore.hardware.DcMotor;
@@ -12,7 +9,6 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple;
 import com.qualcomm.robotcore.hardware.IMU;
 import com.qualcomm.robotcore.hardware.PIDFCoefficients;
 import com.qualcomm.robotcore.hardware.Servo;
-import com.qualcomm.robotcore.util.Range;
 
 import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
 import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
@@ -46,6 +42,7 @@ public class RedCrabMinibot {
     public static double CLAW_ARM_MAX_SPEED = 0.5;
     public static double CLAW_ARM_MAX_VELOCITY_DEGREES = 10;
     private static double CLAW_ARM_MOTOR_MAX_CURRENT_MILLIAMPS = 1588.0;
+    private static long CLAW_ARM_WARN_OVERCURRENT_AFTER_MS = 5000;
     public static double CLAW_ARM_kP = 0.0;
     public static double CLAW_ARM_kI = 0.0;
     public static double CLAW_ARM_kD = 0.0;
@@ -92,6 +89,8 @@ public class RedCrabMinibot {
     private final PIDFController clawArmPIDFController;
     public final String webcamName = "Webcam 1";
 
+    private long lastClawArmOverCurrentAnnounced = 0;
+    private boolean clawArmOverCurrent = false;
     public enum Path {
         LEFT,
         CENTER,
@@ -258,6 +257,7 @@ public class RedCrabMinibot {
         RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO = config.variable("Robot", "ClawArm_Tuning", "gear_ratio").value();
         RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION = config.variable("Robot", "ClawArm_Tuning", "motor_ticks").value();
         RedCrabMinibot.CLAW_ARM_MOTOR_MAX_CURRENT_MILLIAMPS = config.variable("Robot", "ClawArm_Tuning", "max_current_milliamps").value();
+        RedCrabMinibot.CLAW_ARM_WARN_OVERCURRENT_AFTER_MS = config.variable("Robot", "ClawArm_Tuning", "warn_overcurrent_after_ms").value();
 
         /// WINCH
 
@@ -412,6 +412,18 @@ public class RedCrabMinibot {
     public void controlClawArm() {
         Action action = config.action("Robot", "ClawArm_Tuning");
 
+        long milliseconds = System.currentTimeMillis();
+        if (clawArm.isOverCurrent())
+        {
+            if (milliseconds - lastClawArmOverCurrentAnnounced >= CLAW_ARM_WARN_OVERCURRENT_AFTER_MS) {
+                lastClawArmOverCurrentAnnounced = System.currentTimeMillis();
+
+                engine.telemetry.speak("WARNING. ARM. OVER. CURRENT.");
+            }
+        } else {
+            lastClawArmOverCurrentAnnounced = milliseconds;
+        }
+
         double ticksInDegree = Utilities.motorAngleToTicks(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, 1);
 
         for (Variable v : action.getVariables()) {
@@ -450,6 +462,18 @@ public class RedCrabMinibot {
 
         double velocity = Utilities.motorAngleToTicks(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, CLAW_ARM_MAX_VELOCITY_DEGREES);
 
+        double currentAngle = Utilities.motorTicksToAngle(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, clawArm.getCurrentPosition());
+        double targetAngle = Utilities.motorTicksToAngle(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, clawArm.getTargetPosition());
+        double angleDiff = Math.abs(Utilities.angleDiff(currentAngle, targetAngle));
+
+        // Turn off motor if it is stowed or all the way down
+        if (targetAngle <= CLAW_ARM_STOW_ANGLE + 5.0 && angleDiff <= 5.0) {
+            velocity = 0.0;
+        } else if (targetAngle >= CLAW_ARM_COLLECT_ANGLE - 5.0 && angleDiff <= 5.0)
+        {
+            velocity = 0.0;
+        }
+
         clawArm.setVelocity(velocity);
     }
 

TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/TeamPropVisionProcessor.java

@@ -158,6 +158,15 @@ public class TeamPropVisionProcessor implements VisionProcessor {
         return saturationRight;
     }
 
+    public double getHighestSaturation() {
+        if (getSaturationLeft() > getSaturationCenter() && getSaturationLeft() > getSaturationRight())
+            return getSaturationLeft();
+        if (getSaturationCenter() > getSaturationLeft() && getSaturationCenter() > getSaturationRight())
+            return getSaturationCenter();
+
+        return getSaturationRight();
+    }
+
     private android.graphics.Rect makeDrawableRect(Rect rect, float scaleBmpPxToCanvasPx) {
         int left = Math.round(rect.x * scaleBmpPxToCanvasPx);
         int top = Math.round(rect.y * scaleBmpPxToCanvasPx);

TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueAudienceEngine.java

@@ -12,9 +12,7 @@ import dev.cyberarm.minibots.red_crab.states.ClawArmTask;
 public class RedCrabAutonomousBlueAudienceEngine extends RedCrabAutonomousEngine {
     @Override
     public void setup() {
-        RedCrabMinibot robot = new RedCrabMinibot(true);
-        blackboardSet("clawArmPower", 0.0);
-
+        robot = new RedCrabMinibot(true);
         addTask(new ClawArmTask(robot));
 
         setupFromConfig(

TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueBackstageEngine.java

@@ -12,9 +12,7 @@ import dev.cyberarm.minibots.red_crab.states.ClawArmTask;
 public class RedCrabAutonomousBlueBackstageEngine extends RedCrabAutonomousEngine {
     @Override
     public void setup() {
-        RedCrabMinibot robot = new RedCrabMinibot(true);
-        blackboardSet("clawArmPower", 0.0);
-
+        robot = new RedCrabMinibot(true);
         addTask(new ClawArmTask(robot));
 
         setupFromConfig(

TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousEngine.java

@@ -2,12 +2,18 @@ package dev.cyberarm.minibots.red_crab.engines;
 
 import dev.cyberarm.engine.V2.CyberarmEngine;
 import dev.cyberarm.engine.V2.Utilities;
+import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
 
 public abstract class RedCrabAutonomousEngine extends CyberarmEngine {
+    protected RedCrabMinibot robot;
+
     @Override
     public void loop() {
         Utilities.hubsClearBulkReadCache(hardwareMap);
 
+        if (robot != null)
+            robot.standardTelemetry();
+
         super.loop();
     }
 }

TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedAudienceEngine.java

@@ -12,9 +12,7 @@ import dev.cyberarm.minibots.red_crab.states.ClawArmTask;
 public class RedCrabAutonomousRedAudienceEngine extends RedCrabAutonomousEngine {
     @Override
     public void setup() {
-        RedCrabMinibot robot = new RedCrabMinibot(true);
-        blackboardSet("clawArmPower", 0.0);
-
+        robot = new RedCrabMinibot(true);
         addTask(new ClawArmTask(robot));
 
         setupFromConfig(

TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedBackstageEngine.java

@@ -12,9 +12,7 @@ import dev.cyberarm.minibots.red_crab.states.ClawArmTask;
 public class RedCrabAutonomousRedBackstageEngine extends RedCrabAutonomousEngine {
     @Override
     public void setup() {
-        RedCrabMinibot robot = new RedCrabMinibot(true);
-        blackboardSet("clawArmPower", 0.0);
-
+        robot = new RedCrabMinibot(true);
         addTask(new ClawArmTask(robot));
 
         setupFromConfig(

TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/PathEnactor.java

@@ -25,7 +25,7 @@ public class PathEnactor extends CyberarmState {
                 break;
         }
 
-        this.pathGroupName = String.format("AutonomousPixelPath_%s", path);
+        this.pathGroupName = String.format("Autonomous_SpikePath_%s", path);
     }
 
     @Override

TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/PathSelector.java

@@ -18,7 +18,6 @@ public class PathSelector extends CyberarmState {
     private final int timeoutMS;
     private final int path;
     private final double minConfidence;
-    private List<Recognition> recognitions = new ArrayList<>();
 
     public PathSelector(RedCrabMinibot robot, String groupName, String actionName) {
         this.robot = robot;
@@ -34,40 +33,27 @@ public class PathSelector extends CyberarmState {
         robot.visionPortal = VisionPortal.easyCreateWithDefaults(
                 engine.hardwareMap.get(WebcamName.class, robot.webcamName), robot.teamProp);
 
-//        robot.tfPixel.setClippingMargins(0, 0, 0, 0);
-//        robot.tfPixel.setMinResultConfidence((float) minConfidence);
-
         engine.blackboardSet("autonomousPath", path);
     }
 
     @Override
     public void exec() {
-        if (engine.blackboardGetInt("autonomousPath") != 0) {
-            this.finished();
-
-            return;
-        }
-
-//        recognitions = robot.tfPixel.getRecognitions();
-//        for (Recognition recognition : recognitions) {
-//            double x = (recognition.getLeft() + recognition.getRight()) / 2;
-//            double y = (recognition.getTop()  + recognition.getBottom()) / 2;
-//
-//            if (recognition.getLabel().equals("pixel")) {
-//                engine.blackboardSet("autonomousPath", path);
-//            }
-//        }
-
-        switch (robot.teamProp.getLocation()) {
-            case LEFT:
-                engine.blackboardSet("autonomousPath", 0);
-                break;
-            case CENTER:
-                engine.blackboardSet("autonomousPath", 1);
-                break;
-            case RIGHT:
-                engine.blackboardSet("autonomousPath", 2);
-                break;
+        // If we've got enough Saturation for our min confidence then do the needful.
+        if (robot.teamProp.getHighestSaturation() >= minConfidence) {
+            switch (robot.teamProp.getLocation()) {
+                case LEFT:
+                    engine.blackboardSet("autonomousPath", 0);
+                    break;
+                case CENTER:
+                    engine.blackboardSet("autonomousPath", 1);
+                    break;
+                case RIGHT:
+                    engine.blackboardSet("autonomousPath", 2);
+                    break;
+            }
+        } else
+        {
+            engine.blackboardSet("autonomousPath", path); // min confidence not meant, default to center.
         }
 
         if (runTime() >= timeoutMS) {
@@ -85,19 +71,9 @@ public class PathSelector extends CyberarmState {
     public void telemetry() {
         engine.telemetry.addLine();
 
-//        engine.telemetry.addData("Saturation", robot.spikeMark.getSaturation());
-
-//        engine.telemetry.addData("# Objects Detected", recognitions.size());
-//
-//        for(Recognition recognition : recognitions) {
-//            double x = (recognition.getLeft() + recognition.getRight()) / 2;
-//            double y = (recognition.getTop()  + recognition.getBottom()) / 2;
-//
-//            engine.telemetry.addLine();
-//
-//            engine.telemetry.addData("Image", "%s (%.0f %% Conf.)", recognition.getLabel(), recognition.getConfidence() * 100);
-//            engine.telemetry.addData("- Position", "%.0f / %.0f", x, y);
-//            engine.telemetry.addData("- Size", "%.0f x %.0f", recognition.getWidth(), recognition.getHeight());
-//        }
+        engine.telemetry.addData("Location", robot.teamProp.getLocation());
+        engine.telemetry.addData("Saturation Left", robot.teamProp.getSaturationLeft());
+        engine.telemetry.addData("Saturation Center", robot.teamProp.getSaturationCenter());
+        engine.telemetry.addData("Saturation Right", robot.teamProp.getSaturationRight());
     }
 }

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

@@ -209,17 +209,6 @@ public class Pilot extends CyberarmState {
 
                 robot.clawWrist.setPosition(RedCrabMinibot.CLAW_WRIST_COLLECT_POSITION);
                 break;
-
-        }
-
-        if (clawArmPosition == RedCrabMinibot.ClawArm_COLLECT &&
-                robot.clawArm.getCurrentPosition() >= Utilities.motorAngleToTicks(
-                RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION,
-                RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO,
-                RedCrabMinibot.CLAW_ARM_COLLECT_ANGLE) - 5.0) {
-            robot.clawArm.setPower(0);
-        } else {
-            robot.clawArm.setPower(RedCrabMinibot.CLAW_ARM_MAX_SPEED);
         }
     }
 

TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MiniYTeleOPBot.java

@@ -30,7 +30,8 @@ public class MiniYTeleOPBot extends Robot {
         this.hardwareMap = CyberarmEngine.instance.hardwareMap;
         CyberarmEngine engine = CyberarmEngine.instance;
 
-        configuration = new TimeCraftersConfiguration("Minibot Yellow");
+//        configuration = new TimeCraftersConfiguration("Minibot Yellow");
+        configuration = new TimeCraftersConfiguration();
 
 
         imu = engine.hardwareMap.get(IMU.class, "imu");

TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/SodiPizzaTeleOPState.java

@@ -78,52 +78,30 @@ public class SodiPizzaTeleOPState extends CyberarmState {
 
     @Override
     public void exec() {
-        
-        
-        
 
-        if (Math.abs(engine.gamepad1.left_stick_y) < minInput &&
-                Math.abs(engine.gamepad1.left_stick_x) < minInput &&
-                Math.abs(engine.gamepad1.right_stick_x) < minInput){
+        double y = -engine.gamepad1.left_stick_y; // Remember, this is reversed!
+        double x = engine.gamepad1.left_stick_x;
+        double rx = engine.gamepad1.right_stick_x;
+
+        double lbPower = (y - x + rx);
+        double rbPower = (y + x - rx);
+        double lfPower = (y + x + rx);
+        double rfPower = (y - x - rx);
+
+        robot.leftFront.setPower(lfPower * drivePower);
+        robot.leftBack.setPower(lbPower * drivePower);
+        robot.rightFront.setPower(rfPower * drivePower);
+        robot.rightBack.setPower(rbPower * drivePower);
+
+        if (engine.gamepad1.left_stick_x > 0.1) {
+            robot.leftBack.setPower(lbPower);
+            robot.rightBack.setPower(rbPower);
+            robot.leftFront.setPower(lfPower);
+            robot.rightFront.setPower(rfPower);
+
 
-            drivePower = 0;
-            robot.leftFront.setPower(drivePower);
-            robot.rightFront.setPower(drivePower);
-            robot.leftBack.setPower(drivePower);
-            robot.rightBack.setPower(drivePower);
         }
 
-        if (Math.abs(yTransitPercent) > 0.01) {
-
-            percentDenom = 100;
-        } else {
-            percentDenom = 0;
-        }
-
-        if (Math.abs(xTransitPercent) > 0.01) {
-
-            percentDenom = percentDenom + 100;
-        }
-
-        if (Math.abs(rotPercent) > 0.01) {
-
-            percentDenom = percentDenom + 100;
-        }
-        yTransitPercent = engine.gamepad1.left_stick_y * 100;
-        xTransitPercent = engine.gamepad1.left_stick_x * 100;
-        rotPercent = engine.gamepad1.right_stick_x * -100;
-
-        lfPower = ((yTransitPercent + -xTransitPercent + rotPercent) / percentDenom);
-        robot.leftFront.setPower(lfPower);
-
-        rfPower = ((yTransitPercent + xTransitPercent + -rotPercent) / percentDenom);
-        robot.rightFront.setPower(rfPower);
-
-        lbPower = ((yTransitPercent + xTransitPercent + rotPercent) / percentDenom);
-        robot.leftBack.setPower(lbPower);
-
-        rbPower = ((yTransitPercent + -xTransitPercent + -rotPercent) / percentDenom);
-        robot.rightBack.setPower(rbPower);
 
         if (engine.gamepad2.left_stick_button) {
             if (System.currentTimeMillis() - lastMoveTime >= 200) {