diff --git a/.idea/deploymentTargetDropDown.xml b/.idea/deploymentTargetDropDown.xml index 7fb6c55..a4439ce 100644 --- a/.idea/deploymentTargetDropDown.xml +++ b/.idea/deploymentTargetDropDown.xml @@ -12,6 +12,6 @@ - + \ No newline at end of file diff --git a/TeamCode/src/main/java/org/cyberarm/engine/V2/GamepadChecker.java b/TeamCode/src/main/java/org/cyberarm/engine/V2/GamepadChecker.java index 704c077..8868f5b 100644 --- a/TeamCode/src/main/java/org/cyberarm/engine/V2/GamepadChecker.java +++ b/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 buttons = new HashMap<>(); - private final HashMap 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("b", false); buttonsDebounceInterval.put("b", 0L); - buttons.put("x", false); buttonsDebounceInterval.put("x", 0L); - buttons.put("y", false); buttonsDebounceInterval.put("y", 0L); + buttons.put("a", false); + buttons.put("b", false); + buttons.put("x", false); + buttons.put("y", false); - buttons.put("start", false); buttonsDebounceInterval.put("start", 0L); - buttons.put("guide", false); buttonsDebounceInterval.put("guide", 0L); - buttons.put("back", false); buttonsDebounceInterval.put("back", 0L); + buttons.put("start", false); + buttons.put("guide", false); + buttons.put("back", false); - buttons.put("left_bumper", false); buttonsDebounceInterval.put("left_bumper", 0L); - buttons.put("right_bumper", false); buttonsDebounceInterval.put("right_bumper", 0L); + buttons.put("left_bumper", false); + buttons.put("right_bumper", false); - buttons.put("left_stick_button", false); buttonsDebounceInterval.put("left_stick_button", 0L); - buttons.put("right_stick_button", false); buttonsDebounceInterval.put("right_stick_button", 0L); + buttons.put("left_stick_button", false); + buttons.put("right_stick_button", false); - buttons.put("dpad_left", false); buttonsDebounceInterval.put("dpad_left", 0L); - buttons.put("dpad_right", false); buttonsDebounceInterval.put("dpad_right", 0L); - buttons.put("dpad_up", false); buttonsDebounceInterval.put("dpad_up", 0L); - buttons.put("dpad_down", false); buttonsDebounceInterval.put("dpad_down", 0L); + buttons.put("dpad_left", false); + buttons.put("dpad_right", false); + buttons.put("dpad_up", false); + 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(); } diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java index 1556c64..6451d1a 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java +++ b/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( -// angleToTicks(tuningConfig("arm_velocity_in_degrees_per_second").value())); - arm.setPower(0.75); + arm.setPower(tuningConfig("arm_automatic_power").value()); // 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 newTargetVelocity = motorTargetVelocity.getOrDefault("Arm", targetVelocity); - double lastTiming = motorVelocityLastTiming.getOrDefault("Arm", time); - double deltaTime = (time - lastTiming) * 0.001; - - double distanceToTarget = arm.getTargetPosition() - arm.getCurrentPosition(); - double adjustedTargetVelocity = Math.abs(distanceToTarget) < targetVelocity ? Math.abs(distanceToTarget) : targetVelocity; - - double error = adjustedTargetVelocity - arm.getVelocity(); - double kp = 0.9; - - newTargetVelocity += error * kp * deltaTime; - - motorTargetVelocity.put("Arm", newTargetVelocity); - motorVelocityLastTiming.put("Arm", time); +// double time = System.currentTimeMillis(); +// double newTargetVelocity = motorTargetVelocity.getOrDefault("Arm", targetVelocity); +// double lastTiming = motorVelocityLastTiming.getOrDefault("Arm", time); +// double deltaTime = (time - lastTiming) * 0.001; +// +// double distanceToTarget = arm.getTargetPosition() - arm.getCurrentPosition(); +// double adjustedTargetVelocity = Math.abs(distanceToTarget) < targetVelocity ? Math.abs(distanceToTarget) : targetVelocity; +// +// double error = adjustedTargetVelocity - arm.getVelocity(); +// double kp = 0.9; +// +// newTargetVelocity += error * kp * deltaTime; +// +// motorTargetVelocity.put("Arm", newTargetVelocity); +// motorVelocityLastTiming.put("Arm", time); // arm.setVelocity(newTargetVelocity); - arm.setPower(0.75); + arm.setPower(tuningConfig("arm_automatic_power").value()); } public double facing() { diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/SignalProcessor.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/SignalProcessor.java index b2cdde7..4d2d3d0 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/SignalProcessor.java +++ b/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 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 updatedRecognitions = robot.getTfod().getUpdatedRecognitions(); + List recognitions = robot.getTfod().getUpdatedRecognitions(); - if (updatedRecognitions != null) { - for (Recognition recognition : updatedRecognitions) { + if (recognitions != null) { + 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 updatedRecognitions = robot.getTfod().getUpdatedRecognitions(); - if (updatedRecognitions != null) { engine.telemetry.addData("# Objects Detected", updatedRecognitions.size()); diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/ArmDriverControl.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/ArmDriverControl.java index b088fac..9256bee 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/ArmDriverControl.java +++ b/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; } - robot.reportStatus(Robot.Status.WARNING); + 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(); - double stepInterval = robot.tuningConfig("arm_manual_step_interval").value(); - int stepSize = robot.tuningConfig("arm_manual_step_size").value(); + if ((controller.left_trigger > 0 || controller.right_trigger > 0)) { + robot.armManuallyControlled = true; - if ((controller.left_trigger > 0 || controller.right_trigger > 0) && runTime() - lastArmManualControlTime >= stepInterval) { - lastArmManualControlTime = runTime(); + robot.reportStatus(Robot.Status.WARNING); + + robot.arm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); 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")) { - gripperReleaseTriggeredTime = runTime(); - } else if (button.equals("right_bumper")) { + 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(); - } } } diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/DrivetrainDriverControl.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/DrivetrainDriverControl.java index 02bfa42..2444add 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/DrivetrainDriverControl.java +++ b/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);