Working TeleOP

.idea/deploymentTargetDropDown.xml

@@ -12,6 +12,6 @@
         </deviceKey>
       </Target>
     </runningDeviceTargetSelectedWithDropDown>
-    <timeTargetWasSelectedWithDropDown value="2023-01-24T21:07:00.815029Z" />
+    <timeTargetWasSelectedWithDropDown value="2023-01-26T22:23:44.435674Z" />
   </component>
 </project>

TeamCode/src/main/java/org/cyberarm/engine/V2/GamepadChecker.java

@@ -12,43 +12,38 @@ public class GamepadChecker {
   private final CyberarmEngine engine;
   private final Gamepad gamepad;
   private final HashMap<String, Boolean> buttons = new HashMap<>();
-  private final HashMap<String, Long> buttonsDebounceInterval = new HashMap<>();
-  private final long debounceTime = 0L; // ms
 
   public GamepadChecker(CyberarmEngine engine, Gamepad gamepad) {
     this.engine = engine;
     this.gamepad = gamepad;
 
-    buttons.put("a", false); buttonsDebounceInterval.put("a", 0L);
+    buttons.put("a", false);
-    buttons.put("b", false); buttonsDebounceInterval.put("b", 0L);
+    buttons.put("b", false);
-    buttons.put("x", false); buttonsDebounceInterval.put("x", 0L);
+    buttons.put("x", false);
-    buttons.put("y", false); buttonsDebounceInterval.put("y", 0L);
+    buttons.put("y", false);
 
-    buttons.put("start", false); buttonsDebounceInterval.put("start", 0L);
+    buttons.put("start", false);
-    buttons.put("guide", false); buttonsDebounceInterval.put("guide", 0L);
+    buttons.put("guide", false);
-    buttons.put("back", false);  buttonsDebounceInterval.put("back", 0L);
+    buttons.put("back", false);
 
-    buttons.put("left_bumper", false); buttonsDebounceInterval.put("left_bumper", 0L);
+    buttons.put("left_bumper", false);
-    buttons.put("right_bumper", false);  buttonsDebounceInterval.put("right_bumper", 0L);
+    buttons.put("right_bumper", false);
 
-    buttons.put("left_stick_button", false); buttonsDebounceInterval.put("left_stick_button", 0L);
+    buttons.put("left_stick_button", false);
-    buttons.put("right_stick_button", false);  buttonsDebounceInterval.put("right_stick_button", 0L);
+    buttons.put("right_stick_button", false);
 
-    buttons.put("dpad_left", false); buttonsDebounceInterval.put("dpad_left", 0L);
+    buttons.put("dpad_left", false);
-    buttons.put("dpad_right", false);  buttonsDebounceInterval.put("dpad_right", 0L);
+    buttons.put("dpad_right", false);
-    buttons.put("dpad_up", false); buttonsDebounceInterval.put("dpad_up", 0L);
+    buttons.put("dpad_up", false);
-    buttons.put("dpad_down", false); buttonsDebounceInterval.put("dpad_down", 0L);
+    buttons.put("dpad_down", false);
   }
 
   public void update() {
-    long milliseconds = System.currentTimeMillis();
-
     for (String btn : buttons.keySet()) {
       try {
         Field field = gamepad.getClass().getDeclaredField(btn);
         boolean button = field.getBoolean(gamepad);
 
-        if (button != buttons.get(btn) && milliseconds - buttonsDebounceInterval.get(btn) >= debounceTime) {
           if (button) {
             if (!buttons.get(btn)) {
               engine.buttonDown(gamepad, btn);
@@ -62,12 +57,6 @@ public class GamepadChecker {
 
             buttons.put(btn, false);
           }
-
-          buttonsDebounceInterval.put(btn, milliseconds);
-
-        } else if (button == buttons.get(btn)) {
-          buttonsDebounceInterval.put(btn, milliseconds);
-        }
       } catch (NoSuchFieldException|IllegalAccessException e) {
         e.printStackTrace();
       }

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

@@ -44,7 +44,7 @@ public class Robot {
     public LynxModule expansionHub;
 
     public final double imuAngleOffset;
-    public boolean wristManuallyControlled = false;
+    public boolean wristManuallyControlled = false, armManuallyControlled = false;
     public boolean automaticAntiTipActive = false;
     public boolean hardwareFault = false;
     public String hardwareFaultMessage = "";
@@ -87,8 +87,6 @@ public class Robot {
     private WristPosition wristTargetPosition, wristCurrentPosition;
     private double wristPositionChangeTime, wristPositionChangeRequestTime;
 
-    private static final String VUFORIA_KEY =
-            "Abmu1jv/////AAABmYzrcgDEi014nv+wD6PkEPVnOlV2pI3S9sGUMMR/X7hF72x20rP1JcVtsU0nI6VK0yUlYbCSA2k+yMo4hQmPDBvrqeqAgXKa57ilPhW5e1cB3BEevP+9VoJ9QYFhKA3JJTiuFS50WQeuFy3dp0gOPoqHL3XClRFZWbhzihyNnLXgXlKiq+i5GbfONECucQU2DgiuuxYlCaeNdUHl1X5C2pO80zZ6y7PYAp3p0ciXJxqfBoVAklhd69avaAE5Z84ctKscvcbxCS16lq81X7XgIFjshLoD/vpWa300llDG83+Y777q7b5v7gsUCZ6FiuK152Rd272HLuBRhoTXAt0ug9Baq5cz3sn0sAIEzSHX1nah";
     private final VuforiaLocalizer vuforia;
     private final TFObjectDetector tfod;
 
@@ -113,8 +111,9 @@ public class Robot {
         armTicksPerRevolution = tuningConfig("arm_ticks_per_revolution").value();
 
         wristTargetPosition = WristPosition.UP;
-        wristCurrentPosition = WristPosition.UP;
+        wristCurrentPosition = WristPosition.DOWN;
         wristPositionChangeTime = 2500;
+        wristPositionChangeRequestTime = System.currentTimeMillis();
 
         // FIXME: Rename motors in configuration
         // Define hardware
@@ -167,9 +166,7 @@ public class Robot {
         arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
 
         //      MOTOR POWER
-//        arm.setVelocity(
+        arm.setPower(tuningConfig("arm_automatic_power").value());
-//                angleToTicks(tuningConfig("arm_velocity_in_degrees_per_second").value()));
-        arm.setPower(0.75);
 
         //   SERVOS (POSITIONAL)
         //      Gripper
@@ -220,7 +217,7 @@ public class Robot {
     private VuforiaLocalizer initVuforia() {
         VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
 
-        parameters.vuforiaLicenseKey = VUFORIA_KEY;
+        parameters.vuforiaLicenseKey = hardwareConfig("vuforia_license_key").value();
         parameters.cameraName = engine.hardwareMap.get(WebcamName.class, "Webcam 1");
 
          return ClassFactory.getInstance().createVuforia(parameters);
@@ -341,6 +338,9 @@ public class Robot {
         engine.telemetry.addData("      Wrist Direction", wrist.getDirection());
         engine.telemetry.addData("      Wrist Power", wrist.getPower());
         engine.telemetry.addData("      Wrist Enabled", wrist.isPwmEnabled());
+        engine.telemetry.addData("      Wrist Current Position", wristCurrentPosition);
+        engine.telemetry.addData("      Wrist Target Position", wristTargetPosition);
+        engine.telemetry.addData("      Wrist Position Change Request Time", System.currentTimeMillis() - wristPositionChangeRequestTime);
 
         engine.telemetry.addLine();
 
@@ -424,7 +424,7 @@ public class Robot {
 
         if (!wristManuallyControlled && wristTargetPosition != wristCurrentPosition &&
                 System.currentTimeMillis() - wristPositionChangeRequestTime >= wristPositionChangeTime) {
-            wristPositionChangeRequestTime = System.currentTimeMillis();
+//            wristPositionChangeRequestTime = System.currentTimeMillis();
             wristCurrentPosition = wristTargetPosition;
 
             wrist.setPower(0);
@@ -537,6 +537,7 @@ public class Robot {
     public void wristPosition(WristPosition position) {
         wristPositionChangeRequestTime = System.currentTimeMillis();
         wristManuallyControlled = false;
+        wristTargetPosition = position;
 
         if (position == WristPosition.UP) {
             wrist.setPower(tuningConfig("wrist_up_power").value());
@@ -695,25 +696,25 @@ public class Robot {
 
     @SuppressLint("NewApi")
     public void controlArmMotor(double targetVelocity) {
-        double time = System.currentTimeMillis();
+//        double time = System.currentTimeMillis();
-        double newTargetVelocity = motorTargetVelocity.getOrDefault("Arm", targetVelocity);
+//        double newTargetVelocity = motorTargetVelocity.getOrDefault("Arm", targetVelocity);
-        double lastTiming = motorVelocityLastTiming.getOrDefault("Arm", time);
+//        double lastTiming = motorVelocityLastTiming.getOrDefault("Arm", time);
-        double deltaTime = (time - lastTiming) * 0.001;
+//        double deltaTime = (time - lastTiming) * 0.001;
-
+//
-        double distanceToTarget = arm.getTargetPosition() - arm.getCurrentPosition();
+//        double distanceToTarget = arm.getTargetPosition() - arm.getCurrentPosition();
-        double adjustedTargetVelocity = Math.abs(distanceToTarget) < targetVelocity ? Math.abs(distanceToTarget) : targetVelocity;
+//        double adjustedTargetVelocity = Math.abs(distanceToTarget) < targetVelocity ? Math.abs(distanceToTarget) : targetVelocity;
-
+//
-        double error = adjustedTargetVelocity - arm.getVelocity();
+//        double error = adjustedTargetVelocity - arm.getVelocity();
-        double kp = 0.9;
+//        double kp = 0.9;
-
+//
-        newTargetVelocity += error * kp * deltaTime;
+//        newTargetVelocity += error * kp * deltaTime;
-
+//
-        motorTargetVelocity.put("Arm", newTargetVelocity);
+//        motorTargetVelocity.put("Arm", newTargetVelocity);
-        motorVelocityLastTiming.put("Arm", time);
+//        motorVelocityLastTiming.put("Arm", time);
 
 //        arm.setVelocity(newTargetVelocity);
 
-        arm.setPower(0.75);
+        arm.setPower(tuningConfig("arm_automatic_power").value());
     }
 
     public double facing() {

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/SignalProcessor.java

@@ -14,6 +14,8 @@ public class SignalProcessor extends CyberarmState {
     private final int fallbackPosition;
     private final boolean stateDisabled;
 
+    private List<Recognition> updatedRecognitions;
+
     public SignalProcessor(Robot robot, String groupName, String actionName) {
         this.robot = robot;
         this.groupName = groupName;
@@ -50,10 +52,14 @@ public class SignalProcessor extends CyberarmState {
         if (robot.getTfod() != null) {
             // getUpdatedRecognitions() will return null if no new information is available since
             // the last time that call was made.
-            List<Recognition> updatedRecognitions = robot.getTfod().getUpdatedRecognitions();
+            List<Recognition> recognitions = robot.getTfod().getUpdatedRecognitions();
 
-            if (updatedRecognitions != null) {
+            if (recognitions != null) {
-                for (Recognition recognition : updatedRecognitions) {
+                updatedRecognitions = recognitions;
+            }
+
+            if (recognitions != null) {
+                for (Recognition recognition : recognitions) {
                     switch (recognition.getLabel()) {
                         case "1 Bolt":
                             engine.blackboardSet("parking_position", 1);
@@ -76,10 +82,6 @@ public class SignalProcessor extends CyberarmState {
     @Override
     public void telemetry() {
         if (robot.getTfod() != null) {
-            // getUpdatedRecognitions() will return null if no new information is available since
-            // the last time that call was made.
-            List<Recognition> updatedRecognitions = robot.getTfod().getUpdatedRecognitions();
-
             if (updatedRecognitions != null) {
                 engine.telemetry.addData("# Objects Detected", updatedRecognitions.size());
 

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/ArmDriverControl.java

@@ -19,8 +19,6 @@ public class ArmDriverControl extends CyberarmState {
 
     private Gamepad controller;
 
-    private double lastArmManualControlTime = 0, lastWristManualControlTime = 0;
-
     private final double gripperOpenConfirmationDelay;
     private double gripperReleaseTriggeredTime = 0;
 
@@ -44,8 +42,6 @@ public class ArmDriverControl extends CyberarmState {
 
     @Override
     public void telemetry() {
-        engine.telemetry.addData("Arm Interval", lastArmManualControlTime);
-        engine.telemetry.addData("Wrist Interval", lastWristManualControlTime);
     }
 
     private void armManualControl() {
@@ -53,23 +49,34 @@ public class ArmDriverControl extends CyberarmState {
             return;
         }
 
+        double armVelocity = robot.tuningConfig("arm_velocity_in_degrees_per_second").value();
+        double armManualPower = robot.tuningConfig("arm_manual_power").value();
+        double armAutomaticPower = robot.tuningConfig("arm_automatic_power").value();
+
+        if ((controller.left_trigger > 0 || controller.right_trigger > 0)) {
+            robot.armManuallyControlled = true;
+
             robot.reportStatus(Robot.Status.WARNING);
 
-        double stepInterval = robot.tuningConfig("arm_manual_step_interval").value();
+            robot.arm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        int stepSize = robot.tuningConfig("arm_manual_step_size").value();
-
-        if ((controller.left_trigger > 0 || controller.right_trigger > 0) && runTime() - lastArmManualControlTime >= stepInterval) {
-            lastArmManualControlTime = runTime();
 
             if (controller.left_trigger > 0) { // Arm DOWN
-                robot.arm.setTargetPosition(robot.arm.getCurrentPosition() - stepSize);
+                robot.arm.setPower(-armManualPower * Math.sqrt(controller.left_trigger));
 
             } else if (controller.right_trigger > 0) { // Arm UP
-                robot.arm.setTargetPosition(robot.arm.getCurrentPosition() + stepSize);
+                robot.arm.setPower(armManualPower * Math.sqrt(controller.right_trigger));
             }
         }
 
-        // FIXME: Detect when the triggers have been released and park arm at the current position
+        if (robot.armManuallyControlled && controller.left_trigger == 0 && controller.right_trigger == 0) {
+            robot.armManuallyControlled = false;
+
+            robot.arm.setPower(armAutomaticPower);
+
+            robot.arm.setTargetPosition(robot.arm.getCurrentPosition());
+
+            robot.arm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        }
     }
 
     private void wristManualControl() {
@@ -84,14 +91,13 @@ public class ArmDriverControl extends CyberarmState {
 
             if (controller.dpad_left) { // Wrist Left
                 robot.wrist.setPower(stepPower);
-
             }
             if (controller.dpad_right) { // Wrist Right
                 robot.wrist.setPower(-stepPower);
             }
         }
 
-        if (!controller.dpad_left && !controller.dpad_right) {
+        if (robot.wristManuallyControlled && !controller.dpad_left && !controller.dpad_right) {
             robot.wrist.setPower(0);
         }
     }
@@ -128,6 +134,10 @@ public class ArmDriverControl extends CyberarmState {
     }
 
     private void automaticArmVelocity() {
+        if (robot.armManuallyControlled) {
+            return;
+        }
+
         robot.arm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
 
         // robot.controlMotorPIDF(
@@ -161,10 +171,10 @@ public class ArmDriverControl extends CyberarmState {
         }
 
         // Gripper Control
-        if (button.equals("left_bumper")) {
+        if (button.equals("right_bumper")) {
-            gripperReleaseTriggeredTime = runTime();
-        } else if (button.equals("right_bumper")) {
             robot.gripperClosed();
+        } else if (button.equals("left_bumper")) {
+            robot.gripperOpen();
         }
 
         // Wrist Control
@@ -198,10 +208,5 @@ public class ArmDriverControl extends CyberarmState {
         if (gamepad != controller) {
             return;
         }
-
-        // Gripper Control - Require confirmation before opening gripper
-        if (button.equals("left_bumper") && runTime() - gripperReleaseTriggeredTime >= gripperOpenConfirmationDelay) {
-            robot.gripperOpen();
-        }
     }
 }

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/DrivetrainDriverControl.java

@@ -48,7 +48,20 @@ public class DrivetrainDriverControl extends CyberarmState {
         }
 
         double y = invertRobotForward ? controller.left_stick_y : -controller.left_stick_y;
-        double x = (invertRobotForward && !fieldCentricControl ? controller.left_stick_x : -controller.left_stick_x) * 1.1; // Counteract imperfect strafing
+        double x = (invertRobotForward && !fieldCentricControl ? controller.left_stick_x : -controller.left_stick_x);
+
+        // 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);