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 778dd36..0bb5447 100644 --- a/TeamCode/src/main/java/org/cyberarm/engine/V2/GamepadChecker.java +++ b/TeamCode/src/main/java/org/cyberarm/engine/V2/GamepadChecker.java @@ -1,11 +1,14 @@ package org.cyberarm.engine.V2; +import android.util.Log; + import com.qualcomm.robotcore.hardware.Gamepad; import java.lang.reflect.Field; import java.util.HashMap; public class GamepadChecker { + private final String TAG = "GamepadChecker"; private final CyberarmEngine engine; private final Gamepad gamepad; private final HashMap buttons = new HashMap<>(); @@ -49,16 +52,23 @@ public class GamepadChecker { if (button) { if (!buttons.get(btn)) { engine.buttonDown(gamepad, btn); + + Log.d(TAG, "Button '" + btn + "' is DOWN [" + buttons.size() + "]"); } buttons.put(btn, true); } else { if (buttons.get(btn)) { engine.buttonUp(gamepad, btn); + + Log.d(TAG, "Button '" + btn + "' is UP"); } buttons.put(btn, false); } + + buttonsDebounceInterval.put(btn, milliseconds); + } else if (button == buttons.get(btn)) { buttonsDebounceInterval.put(btn, milliseconds); } 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 3e5f1c1..5b10469 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 @@ -222,7 +222,8 @@ public class Robot { // Sensors / IMU engine.telemetry.addLine("IMU"); - engine.telemetry.addData(" Facing", facing()); + engine.telemetry.addData(" Facing (Degrees)", facing()); + engine.telemetry.addData(" Heading (Radians)", heading()); engine.telemetry.addData(" Turn Rate", turnRate()); } diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/ConfigureAndTestHardwareEngine.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/ConfigureAndTestHardwareEngine.java index c6dec44..aea281b 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/ConfigureAndTestHardwareEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/ConfigureAndTestHardwareEngine.java @@ -26,13 +26,13 @@ public class ConfigureAndTestHardwareEngine extends CyberarmEngine { public void setup() { robot = new Robot(this, new TimeCraftersConfiguration("CHIRON")); - addState(new MotorSetupState("Left Drive", robot.backLeftDrive, "left_drive_direction_forward", robot)); - addState(new MotorSetupState("Right Drive", robot.frontRightDrive, "right_drive_direction_forward", robot)); + addState(new MotorSetupState("Back Left Drive", robot.backLeftDrive, "back_left_drive_direction_forward", robot)); + addState(new MotorSetupState("Front Right Drive", robot.frontRightDrive, "front_right_drive_direction_forward", robot)); - addState(new MotorSetupState("Front Drive", robot.frontLeftDrive, "front_drive_direction_forward", robot)); - addState(new MotorSetupState("Back Drive", robot.backRightDrive, "back_drive_direction_forward", robot)); + addState(new MotorSetupState("Front Left Drive", robot.frontLeftDrive, "front_left_drive_direction_forward", robot)); + addState(new MotorSetupState("Back Right Drive", robot.backRightDrive, "back_right_drive_direction_forward", robot)); - addState(new MotorSetupState("Lift Drive", robot.arm, "lift_drive_direction_forward", robot)); + addState(new MotorSetupState("Arm", robot.arm, "arm_direction_forward", robot)); } @Override diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/DriverControlState.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/DriverControlState.java index 859b3d6..c31be76 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/DriverControlState.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/DriverControlState.java @@ -1,5 +1,7 @@ package org.timecrafters.minibots.cyberarm.chiron.states; +import android.util.Log; + import com.qualcomm.robotcore.hardware.Gamepad; import org.cyberarm.engine.V2.CyberarmState; @@ -13,6 +15,8 @@ public class DriverControlState extends CyberarmState { private double lastArmManualControlTime = 0, lastWristManualControlTime = 0, lastLEDStatusAnimationTime = 0; private boolean LEDStatusToggle = false; private boolean fieldCentricControl = true; + private boolean invertRobotForward = false; + private boolean robotSlowMode = false; public DriverControlState(Robot robot) { this.robot = robot; @@ -41,6 +45,7 @@ public class DriverControlState extends CyberarmState { engine.telemetry.addData("Wrist Interval", lastWristManualControlTime); engine.telemetry.addData("LED Status Interval", lastLEDStatusAnimationTime); engine.telemetry.addData("Field Centric Control", fieldCentricControl); + engine.telemetry.addData("Invert Robot Forward", invertRobotForward); } // FIXME: replace .setPower with .setVelocity @@ -50,27 +55,32 @@ public class DriverControlState extends CyberarmState { return; } - double y = -engine.gamepad1.left_stick_y; - double x = engine.gamepad1.left_stick_x * 1.1; - double rx = engine.gamepad1.right_stick_x; + double maxSpeed = robot.tuningConfig("drivetrain_max_power").value(); + double slowSpeed = robot.tuningConfig("drivetrain_slow_power").value(); + double speedLimiter = robotSlowMode ? slowSpeed : maxSpeed; + + double y = (invertRobotForward ? engine.gamepad1.left_stick_y : -engine.gamepad1.left_stick_y) * speedLimiter; + double x = ((invertRobotForward ? engine.gamepad1.left_stick_x : -engine.gamepad1.left_stick_x) * speedLimiter) * 1.1; // Counteract imperfect strafing + double rx = -engine.gamepad1.right_stick_x * speedLimiter; double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); + double frontLeftPower = 0, frontRightPower = 0, backLeftPower = 0 , backRightPower = 0; if (fieldCentricControl) { - double heading = -robot.heading(); + 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; + backLeftPower = (rotY - rotX - rx) / denominator; + frontRightPower = (rotY - rotX + rx) / denominator; backRightPower = (rotY + rotX - rx) / denominator; } else { frontLeftPower = (y + x + rx) / denominator; - backLeftPower = (y - x + rx) / denominator; - frontRightPower = (y - x - rx) / denominator; + backLeftPower = (y - x - rx) / denominator; + frontRightPower = (y - x + rx) / denominator; backRightPower = (y + x - rx) / denominator; } @@ -289,6 +299,16 @@ public class DriverControlState extends CyberarmState { } else if (button.equals("y")) { armPosition(Robot.ArmPosition.MEDIUM); } + + if (button.equals("pause")) { + invertRobotForward = !invertRobotForward; + } + if (button.equals("guide")) { + robot.hardwareFault = !robot.hardwareFault; + } + if (button.equals("start")) { + fieldCentricControl = !fieldCentricControl; + } } @Override @@ -296,11 +316,5 @@ public class DriverControlState extends CyberarmState { if (gamepad != engine.gamepad1) { return; } - - if (button.equals("guide")) { - robot.hardwareFault = !robot.hardwareFault; - } else if (button.equals("start")) { - fieldCentricControl = !fieldCentricControl; - } } }