diff --git a/.idea/deploymentTargetDropDown.xml b/.idea/deploymentTargetDropDown.xml new file mode 100644 index 0000000..a4439ce --- /dev/null +++ b/.idea/deploymentTargetDropDown.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java b/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java index 8fc7885..334e0d8 100644 --- a/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java +++ b/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java @@ -415,7 +415,7 @@ public abstract class CyberarmEngine extends OpMode { * @param objectClass Class to cast object to * @param groupName Group name */ - protected void setupFromConfig(TimeCraftersConfiguration configuration, String packageName, Object object, Class objectClass, String groupName) { + public void setupFromConfig(TimeCraftersConfiguration configuration, String packageName, Object object, Class objectClass, String groupName) { CyberarmState lastState = null; String lastActionName = null; String[] lastActionNameSplit = new String[0]; 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 e9b551c..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 = 20L; // 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 ee9c218..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 @@ -7,6 +7,7 @@ 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.ColorSensor; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; @@ -16,9 +17,13 @@ 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; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer; +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; @@ -32,15 +37,17 @@ import java.util.concurrent.TimeUnit; public class Robot { private static final String TAG = "CHIRON | Robot"; public final DcMotorEx backLeftDrive, frontRightDrive, frontLeftDrive, backRightDrive, arm; - public final ServoImplEx gripper, wrist; + public final ServoImplEx gripper; + public final CRServoImplEx wrist; public final IMU imu; public final ColorSensor indicatorA, indicatorB; 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 = ""; private Status status = Status.OKAY, lastStatus = Status.OKAY; private final CopyOnWriteArrayList reportedStatuses = new CopyOnWriteArrayList<>(); @@ -57,6 +64,11 @@ public class Robot { } + public enum WristPosition { + UP, + DOWN + } + public enum Status { OKAY, MONITORING, @@ -69,8 +81,14 @@ public class Robot { private final FieldLocalizer fieldLocalizer; private final double radius, diameter; - private final double wheelRadius, gearRatio; - private final int ticksPerRevolution; + private final double wheelRadius, wheelGearRatio, armGearRatio; + private final int wheelTicksPerRevolution, armTicksPerRevolution; + + private WristPosition wristTargetPosition, wristCurrentPosition; + private double wristPositionChangeTime, wristPositionChangeRequestTime; + + private final VuforiaLocalizer vuforia; + private final TFObjectDetector tfod; private boolean LEDStatusToggle = false; private double lastLEDStatusAnimationTime = 0; @@ -86,9 +104,16 @@ public class Robot { imuAngleOffset = hardwareConfig("imu_angle_offset").value(); wheelRadius = tuningConfig("wheel_radius").value(); - gearRatio = tuningConfig("wheel_gear_ratio").value(); - ticksPerRevolution = tuningConfig("wheel_ticks_per_revolution").value(); + wheelGearRatio = tuningConfig("wheel_gear_ratio").value(); + wheelTicksPerRevolution = tuningConfig("wheel_ticks_per_revolution").value(); + armGearRatio = tuningConfig("arm_gear_ratio").value(); + armTicksPerRevolution = tuningConfig("arm_ticks_per_revolution").value(); + + wristTargetPosition = WristPosition.UP; + wristCurrentPosition = WristPosition.DOWN; + wristPositionChangeTime = 2500; + wristPositionChangeRequestTime = System.currentTimeMillis(); // FIXME: Rename motors in configuration // Define hardware @@ -102,7 +127,7 @@ public class Robot { arm = engine.hardwareMap.get(DcMotorEx.class, "lift_drive"); // MOTOR PORT: ? gripper = engine.hardwareMap.get(ServoImplEx.class, "gripper"); // SERVO PORT: ? - wrist = engine.hardwareMap.get(ServoImplEx.class, "wrist"); // SERVO PORT: ? + wrist = engine.hardwareMap.get(CRServoImplEx.class, "wrist"); // SERVO PORT: ? indicatorA = engine.hardwareMap.colorSensor.get("indicator_A"); // I2C indicatorB = engine.hardwareMap.colorSensor.get("indicator_B"); // I2C @@ -141,17 +166,17 @@ public class Robot { arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); // MOTOR POWER - arm.setVelocity( - angleToTicks(tuningConfig("arm_velocity_in_degrees_per_second").value())); + arm.setPower(tuningConfig("arm_automatic_power").value()); // SERVOS (POSITIONAL) // Gripper gripper.setDirection(hardwareConfig("gripper_direction_forward").value() ? Servo.Direction.FORWARD : Servo.Direction.REVERSE); gripper.setPosition(tuningConfig("gripper_initial_position").value()); + // SERVOS (CONTINUOUS) // Wrist - wrist.setDirection(hardwareConfig("wrist_direction_forward").value() ? Servo.Direction.FORWARD : Servo.Direction.REVERSE); - wrist.setPosition(tuningConfig("wrist_initial_position").value()); + wrist.setDirection(hardwareConfig("wrist_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE); + wrist.setPower(tuningConfig("wrist_up_power").value()); // SENSORS // COLOR SENSORS @@ -180,11 +205,38 @@ public class Robot { expansionHub.setPattern(ledPatternOkay()); } + // Webcam + vuforia = initVuforia(); + tfod = initTfod(); + // INITIALIZE AFTER EVERYTHING ELSE to prevent use before set crashes this.fieldLocalizer.setRobot(this); this.fieldLocalizer.standardSetup(); } + private VuforiaLocalizer initVuforia() { + VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(); + + parameters.vuforiaLicenseKey = hardwareConfig("vuforia_license_key").value(); + parameters.cameraName = engine.hardwareMap.get(WebcamName.class, "Webcam 1"); + + return ClassFactory.getInstance().createVuforia(parameters); + } + + private TFObjectDetector initTfod() { + int tfodMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier( + "tfodMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName()); + TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId); + tfodParameters.minResultConfidence = 0.75f; + tfodParameters.isModelTensorFlow2 = true; + tfodParameters.inputSize = 300; + TFObjectDetector tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia); + + tfod.loadModelFromAsset("PowerPlay.tflite", "1 Bolt", "2 Bulb", "3 Panel"); + + return tfod; + } + public void standardTelemetry() { engine.telemetry.addLine(); @@ -192,6 +244,7 @@ public class Robot { engine.telemetry.addLine("DATA"); engine.telemetry.addData(" Robot Status", status); engine.telemetry.addData(" Hardware Fault", hardwareFault); + engine.telemetry.addData(" Hardware Fault Message", hardwareFaultMessage); engine.telemetry.addLine(); // Motor Powers @@ -232,7 +285,7 @@ public class Robot { engine.telemetry.addLine(); - engine.telemetry.addData(" Arm", "%d (%8.2f in)", arm.getTargetPosition(), ticksToUnit(DistanceUnit.INCH, arm.getTargetPosition())); + engine.telemetry.addData(" Arm", "%d (%8.2f degrees)", arm.getTargetPosition(), ticksToAngle(arm.getTargetPosition())); // Motor Velocity engine.telemetry.addLine("Motor Velocity"); @@ -283,8 +336,11 @@ public class Robot { engine.telemetry.addData(" Gripper Enabled", gripper.isPwmEnabled()); engine.telemetry.addLine(); engine.telemetry.addData(" Wrist Direction", wrist.getDirection()); - engine.telemetry.addData(" Wrist Position", wrist.getPosition()); + 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(); @@ -293,6 +349,7 @@ public class Robot { engine.telemetry.addData(" Facing (Degrees)", facing()); engine.telemetry.addData(" Heading (Radians)", heading()); engine.telemetry.addData(" Turn Rate", turnRate()); + engine.telemetry.addData(" Angle Offset (Degrees)", imuAngleOffset); engine.telemetry.addLine(); @@ -365,8 +422,19 @@ public class Robot { hub.clearBulkCache(); } - if (getVoltage() < 9.75) { + if (!wristManuallyControlled && wristTargetPosition != wristCurrentPosition && + System.currentTimeMillis() - wristPositionChangeRequestTime >= wristPositionChangeTime) { +// wristPositionChangeRequestTime = System.currentTimeMillis(); + wristCurrentPosition = wristTargetPosition; + + wrist.setPower(0); + } + + double voltage = getVoltage(); + if (voltage < 9.75) { reportStatus(Status.DANGER); + hardwareFaultMessage = "Battery voltage to low! (" + voltage + " volts)"; + hardwareFault = true; } @@ -433,19 +501,90 @@ public class Robot { } } + public void armPosition(ArmPosition position) { + if (hardwareFault) { + return; + } + + reportStatus(Robot.Status.WARNING); + + switch (position) { + case COLLECT: + arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_collect").value())); + break; + + case GROUND: + arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_ground").value())); + break; + + case LOW: + arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_low").value())); + break; + + case MEDIUM: + arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_medium").value())); + break; + + case HIGH: + arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_high").value())); + break; + + default: + throw new RuntimeException("Unexpected arm position!"); + } + } + + public void wristPosition(WristPosition position) { + wristPositionChangeRequestTime = System.currentTimeMillis(); + wristManuallyControlled = false; + wristTargetPosition = position; + + if (position == WristPosition.UP) { + wrist.setPower(tuningConfig("wrist_up_power").value()); + } else { + wrist.setPower(tuningConfig("wrist_down_power").value()); + } + } + + public void gripperOpen() { + gripper.setPosition(tuningConfig("gripper_open_position").value()); + } + + public void gripperClosed() { + gripper.setPosition(tuningConfig("gripper_closed_position").value()); + } + + // Adapted from: https://github.com/gosu/gosu/blob/980d64de2ce52e4b16fdd5cb9c9e11c8bbb80671/src/Math.cpp#L38 + public double angleDiff(double from, double to) { + double value = (to - from + 180) - 180; + + int fmod = (int) Math.floor(value - 0.0 / 360.0 - 0.0); + double result = (value - 0.0) - fmod * (360.0 - 0.0); + + return result < 0 ? result + 360.0 : result + 0.0; + } + public Status getStatus() { return status; } public double getRadius() { return radius; } public double getDiameter() { return diameter; } + public double getVoltage() { + return engine.hardwareMap.voltageSensor.iterator().next().getVoltage(); + } + + public TFObjectDetector getTfod() { return tfod; } + + public VuforiaLocalizer getVuforia() { return vuforia; } + public TimeCraftersConfiguration getConfiguration() { return configuration; } public FieldLocalizer getFieldLocalizer() { return fieldLocalizer; } // For: Drive Wheels public int unitToTicks(DistanceUnit unit, double distance) { - double fI = (gearRatio * ticksPerRevolution) / (wheelRadius * 2 * Math.PI * (gearRatio * ticksPerRevolution) / (gearRatio * ticksPerRevolution)); + double fI = (wheelGearRatio * wheelTicksPerRevolution) / (wheelRadius * 2 * Math.PI * (wheelGearRatio * wheelTicksPerRevolution) / (wheelGearRatio * wheelTicksPerRevolution)); double inches = unit.toInches(unit.fromUnit(unit, distance)); @@ -457,16 +596,14 @@ public class Robot { // For: Drive Wheels public double ticksToUnit(DistanceUnit unit, int ticks) { // Convert to inches, then to unit. - double inches = wheelRadius * 2 * Math.PI * ticks / (gearRatio * ticksPerRevolution); + double inches = wheelRadius * 2 * Math.PI * ticks / (wheelGearRatio * wheelTicksPerRevolution); return unit.fromUnit(DistanceUnit.INCH, inches); } // For: Arm public int angleToTicks(double angle) { - int ticksPerRevolution = tuningConfig("arm_ticks_per_revolution").value(); - - double d = ticksPerRevolution / 360.0; + double d = (armGearRatio * armTicksPerRevolution) / 360.0; // Casting to float so that the int version of Math.round is used. return Math.round((float)d * (float)angle); @@ -474,9 +611,7 @@ public class Robot { // For: Arm public double ticksToAngle(int ticks) { - int ticksPerRevolution = tuningConfig("arm_ticks_per_revolution").value(); - - double oneDegree = 360.0 / ticksPerRevolution; + double oneDegree = 360.0 / (armGearRatio * armTicksPerRevolution); return oneDegree * ticks; } @@ -561,34 +696,36 @@ 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 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 error = targetVelocity - arm.getVelocity(); - double kp = 0.9; +// arm.setVelocity(newTargetVelocity); - newTargetVelocity += error * kp * deltaTime; - - motorTargetVelocity.put("Arm", newTargetVelocity); - motorVelocityLastTiming.put("Arm", time); - - arm.setVelocity(newTargetVelocity); - } - - public double getVoltage() { - return engine.hardwareMap.voltageSensor.iterator().next().getVoltage(); + arm.setPower(tuningConfig("arm_automatic_power").value()); } public double facing() { double imuDegrees = -imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); - return (imuDegrees + imuAngleOffset + 360.0) % 360.0; + return (((imuDegrees + 360.0) % 360.0) + imuAngleOffset) % 360.0; } public double heading() { - return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); + return AngleUnit.normalizeRadians(-facing() * Math.PI / 180.0); +// return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); } public double turnRate() { diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/AutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/AutonomousEngine.java index aa09dbd..230b459 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/AutonomousEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/AutonomousEngine.java @@ -1,11 +1,14 @@ package org.timecrafters.minibots.cyberarm.chiron.engines; +import android.util.Log; + import org.cyberarm.engine.V2.CyberarmEngine; import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; import org.timecrafters.minibots.cyberarm.chiron.Robot; import org.timecrafters.minibots.cyberarm.chiron.tasks.FieldLocalizer; public class AutonomousEngine extends CyberarmEngine { + private static final String TAG = "CHIRON|AutonomousEngine"; protected Robot robot; protected FieldLocalizer fieldLocalizer; protected TimeCraftersConfiguration configuration; @@ -49,6 +52,12 @@ public class AutonomousEngine extends CyberarmEngine { super.loop(); robot.standardTelemetry(); + + if (robot.hardwareFault) { + Log.e(TAG, "Hardware fault detected! Aborting run. Message: " + robot.hardwareFaultMessage); + + stop(); + } } @Override diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousBlueLeftSideEngine.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousBlueLeftSideEngine.java index 9d4e684..1ee4228 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousBlueLeftSideEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousBlueLeftSideEngine.java @@ -8,7 +8,7 @@ import org.timecrafters.minibots.cyberarm.chiron.engines.AutonomousEngine; public class AutonomousBlueLeftSideEngine extends AutonomousEngine { @Override public void setup() { - actionsGroupName = "AutonomousBlueLeftSide"; + actionsGroupName = "AutonomousLeftSide"; tuningGroupName = "Autonomous"; tuningActionName = "Tuning_Blue_LeftSide"; diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousBlueRightSideEngine.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousBlueRightSideEngine.java index 0f55457..08ad0b0 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousBlueRightSideEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousBlueRightSideEngine.java @@ -8,7 +8,7 @@ import org.timecrafters.minibots.cyberarm.chiron.engines.AutonomousEngine; public class AutonomousBlueRightSideEngine extends AutonomousEngine { @Override public void setup() { - actionsGroupName = "AutonomousBlueRightSide"; + actionsGroupName = "AutonomousRightSide"; tuningGroupName = "Autonomous"; tuningActionName = "Tuning_Blue_RightSide"; diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousRedLeftSideEngine.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousRedLeftSideEngine.java index 847a85c..db4e3d0 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousRedLeftSideEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousRedLeftSideEngine.java @@ -8,7 +8,7 @@ import org.timecrafters.minibots.cyberarm.chiron.engines.AutonomousEngine; public class AutonomousRedLeftSideEngine extends AutonomousEngine { @Override public void setup() { - actionsGroupName = "AutonomousRedLeftSide"; + actionsGroupName = "AutonomousLeftSide"; tuningGroupName = "Autonomous"; tuningActionName = "Tuning_Red_LeftSide"; diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousRedRightSideEngine.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousRedRightSideEngine.java index 7da0668..e8d4272 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousRedRightSideEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousRedRightSideEngine.java @@ -8,7 +8,7 @@ import org.timecrafters.minibots.cyberarm.chiron.engines.AutonomousEngine; public class AutonomousRedRightSideEngine extends AutonomousEngine { @Override public void setup() { - actionsGroupName = "AutonomousRedRightSide"; + actionsGroupName = "AutonomousRightSide"; tuningGroupName = "Autonomous"; tuningActionName = "Tuning_Red_RightSide"; diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Arm.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Arm.java index da30682..cb27629 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Arm.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Arm.java @@ -7,15 +7,47 @@ public class Arm extends CyberarmState { private final Robot robot; private final String groupName, actionName; + private final double targetVelocity, timeInMS; + private final int tolerance, targetPosition; + private final boolean stateDisabled; + public Arm(Robot robot, String groupName, String actionName) { this.robot = robot; this.groupName = groupName; this.actionName = actionName; + + targetVelocity = robot.angleToTicks( + robot.getConfiguration().variable(groupName, actionName, "targetVelocityInDegreesPerSecond").value()); + tolerance = robot.angleToTicks( + robot.getConfiguration().variable(groupName, actionName, "toleranceInDegrees").value()); + targetPosition = robot.angleToTicks( + robot.getConfiguration().variable(groupName, actionName, "targetAngle").value()); + timeInMS = robot.getConfiguration().variable(groupName, actionName, "timeInMS").value(); + + stateDisabled = !robot.getConfiguration().action(groupName, actionName).enabled; + } + + @Override + public void start() { + robot.arm.setTargetPosition(targetPosition); + robot.arm.setTargetPositionTolerance(tolerance); + robot.arm.setVelocity(targetVelocity); } @Override public void exec() { - // FIXME: NO OP - setHasFinished(true); + if (stateDisabled) { + setHasFinished(true); + + return; + } + + if (runTime() >= timeInMS) { + stop(); + + setHasFinished(true); + + return; + } } } diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Gripper.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Gripper.java index 83c1e2a..e94199b 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Gripper.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Gripper.java @@ -7,15 +7,49 @@ public class Gripper extends CyberarmState { private final Robot robot; private final String groupName, actionName; + private final boolean positionManually, stateDisabled; + private final String positionLookupLabel; + private final double manualPosition, timeInMS, position; + public Gripper(Robot robot, String groupName, String actionName) { this.robot = robot; this.groupName = groupName; this.actionName = actionName; + + timeInMS = robot.getConfiguration().variable(groupName, actionName, "timeInMS").value(); + + positionLookupLabel = robot.getConfiguration().variable(groupName, actionName, "positionLookupLabel").value(); + positionManually = robot.getConfiguration().variable(groupName, actionName, "positionManually").value(); + manualPosition = robot.getConfiguration().variable(groupName, actionName, "manualPosition").value(); + + stateDisabled = !robot.getConfiguration().action(groupName, actionName).enabled; + + if (positionManually) { + position = manualPosition; + } else { + position = robot.tuningConfig(positionLookupLabel).value(); + } + } + + @Override + public void start() { + robot.gripper.setPosition(position); } @Override public void exec() { - // FIXME: NO OP - setHasFinished(true); + if (stateDisabled) { + setHasFinished(true); + + return; + } + + if (runTime() >= timeInMS) { + stop(); + + setHasFinished(true); + + return; + } } } diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Move.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Move.java index 8dceebd..3b463f2 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Move.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Move.java @@ -8,7 +8,7 @@ public class Move extends CyberarmState { private final Robot robot; private final String groupName, actionName; - private final double positionXInInches, positionYInInches, toleranceInInches, facing, targetVelocity, timeInMS; + private final double targetDistance, tolerance, facing, targetVelocity, timeInMS; private final double maxVelocity; private double speed; @@ -21,11 +21,10 @@ public class Move extends CyberarmState { this.groupName = groupName; this.actionName = actionName; - positionXInInches = robot.getConfiguration().variable(groupName, actionName, "positionXInInches").value(); - positionYInInches = robot.getConfiguration().variable(groupName, actionName, "positionYInInches").value(); - toleranceInInches = robot.getConfiguration().variable(groupName, actionName, "toleranceInInches").value(); + targetDistance = robot.unitToTicks(DistanceUnit.INCH, robot.getConfiguration().variable(groupName, actionName, "targetDistanceInInches").value()); + tolerance = robot.unitToTicks(DistanceUnit.INCH, robot.getConfiguration().variable(groupName, actionName, "toleranceInInches").value()); facing = robot.getConfiguration().variable(groupName, actionName, "facing").value(); - targetVelocity = robot.unitToTicks(DistanceUnit.INCH, robot.getConfiguration().variable(groupName, actionName, "targetVelocity").value()); + targetVelocity = robot.unitToTicks(DistanceUnit.INCH, robot.getConfiguration().variable(groupName, actionName, "targetVelocityInInches").value()); timeInMS = robot.getConfiguration().variable(groupName, actionName, "timeInMS").value(); stateDisabled = !robot.getConfiguration().action(groupName, actionName).enabled; @@ -36,7 +35,7 @@ public class Move extends CyberarmState { @Override public void start() { // TODO: Use a dead wheel for this - distanceAlreadyTravelled = robot.frontLeftDrive.getCurrentPosition(); + distanceAlreadyTravelled = robot.frontRightDrive.getCurrentPosition(); } @Override @@ -55,23 +54,33 @@ public class Move extends CyberarmState { return; } - // TODO: Double check maths -// int travelledDistance = (robot.frontLeftDrive.getCurrentPosition() - distanceAlreadyTravelled); -// int targetDistance = robot.unitToTicks(DistanceUnit.INCH, distanceInInches); -// int tolerance = robot.unitToTicks(DistanceUnit.INCH, toleranceInInches); -// -// if (robot.isBetween(targetDistance, travelledDistance - tolerance, travelledDistance + tolerance)) { -// stop(); -// -// setHasFinished(true); -// -// return; -// } + int travelledDistance = -robot.frontRightDrive.getCurrentPosition() - distanceAlreadyTravelled; - move(); + if (robot.isBetween(travelledDistance, targetDistance - tolerance, targetDistance + tolerance)) { + stop(); + + setHasFinished(true); + + return; + } + + moveDirectional(travelledDistance); } - private void move() { + private void moveDirectional(double travelledDistance) { + // TODO: Ease-in-out velocity + double velocity = targetVelocity; + + if (targetDistance > travelledDistance) { + robot.frontRightDrive.setVelocity(-velocity); + robot.backLeftDrive.setVelocity(-velocity); + } else { + robot.frontRightDrive.setVelocity(velocity); + robot.backLeftDrive.setVelocity(velocity); + } + } + + private void moveOmni() { if (Math.abs(speed) > maxVelocity) { speed = speed < 0 ? -maxVelocity : maxVelocity; } @@ -99,7 +108,7 @@ public class Move extends CyberarmState { double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); - double frontLeftPower = 0, frontRightPower = 0, backLeftPower = 0 , backRightPower = 0; + double frontLeftPower, frontRightPower, backLeftPower, backRightPower; double heading = robot.heading(); double rotX = x * Math.cos(heading) - y * Math.sin(heading); diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Rotate.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Rotate.java index 61cd6e1..b48bb85 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Rotate.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Rotate.java @@ -1,21 +1,101 @@ package org.timecrafters.minibots.cyberarm.chiron.states.autonomous; import org.cyberarm.engine.V2.CyberarmState; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.timecrafters.minibots.cyberarm.chiron.Robot; public class Rotate extends CyberarmState { private final Robot robot; private final String groupName, actionName; + private final double timeInMS, facing, targetFacing, targetVelocity, toleranceInDegrees; + private final boolean stateDisabled, useShortestRotation, rotateRight; + public Rotate(Robot robot, String groupName, String actionName) { this.robot = robot; this.groupName = groupName; this.actionName = actionName; + + timeInMS = robot.getConfiguration().variable(groupName, actionName, "timeInMS").value(); + + targetFacing = robot.getConfiguration().variable(groupName, actionName, "targetFacing").value(); + useShortestRotation = robot.getConfiguration().variable(groupName, actionName, "useShortestRotation").value(); + rotateRight = robot.getConfiguration().variable(groupName, actionName, "rotateRight").value(); + toleranceInDegrees = robot.getConfiguration().variable(groupName, actionName, "toleranceInDegrees").value(); + targetVelocity = robot.unitToTicks(DistanceUnit.INCH, + robot.getConfiguration().variable(groupName, actionName, "targetVelocityInInches").value()); + + stateDisabled = !robot.getConfiguration().action(groupName, actionName).enabled; + + facing = (robot.facing() + targetFacing + 360.0) % 360.0; } @Override public void exec() { - // FIXME: NO OP - setHasFinished(true); + if (stateDisabled) { + stop(); + + setHasFinished(true); + + return; + } + + if (runTime() >= timeInMS) { + stop(); + + setHasFinished(true); + + return; + } + + double currentDegrees = robot.facing(); + double diff = robot.angleDiff(facing, currentDegrees); + + if (Math.abs(diff) <= toleranceInDegrees) { + stop(); + + setHasFinished(true); + + return; + } + + if (useShortestRotation) { + if (diff < 0) { + rotateLeft(); + } else { + rotateRight(); + } + } else { + if (rotateRight) { + rotateRight(); + } else { + rotateLeft(); + } + } + } + + private void rotateLeft() { + rotate(-1); + } + + private void rotateRight() { + rotate(1); + } + + private void rotate(double multiplier) { + robot.frontRightDrive.setVelocity(targetVelocity * multiplier); + robot.backLeftDrive.setVelocity(targetVelocity * multiplier); + + robot.backRightDrive.setVelocity(targetVelocity * multiplier); + robot.frontLeftDrive.setVelocity(targetVelocity * multiplier); + } + + @Override + public void stop() { + robot.frontLeftDrive.setVelocity(0); + robot.frontRightDrive.setVelocity(0); + + robot.backLeftDrive.setVelocity(0); + robot.backRightDrive.setVelocity(0); } } diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/SelectParkingPosition.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/SelectParkingPosition.java index 9edac4a..bb33df3 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/SelectParkingPosition.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/SelectParkingPosition.java @@ -7,15 +7,45 @@ public class SelectParkingPosition extends CyberarmState { private final Robot robot; private final String groupName, actionName; + private final double timeInMS; + private final boolean stateDisabled; + public SelectParkingPosition(Robot robot, String groupName, String actionName) { this.robot = robot; this.groupName = groupName; this.actionName = actionName; + + timeInMS = robot.getConfiguration().variable(groupName, actionName, "timeInMS").value(); + + stateDisabled = !robot.getConfiguration().action(groupName, actionName).enabled; + } + + @Override + public void start() { + int pos = engine.blackboardGetInt("parking_position"); + + engine.setupFromConfig( + robot.getConfiguration(), + "org.timecrafters.minibots.cyberarm.chiron.states.autonomous", + robot, + Robot.class, + "" + groupName + "_Parking_" + pos); } @Override public void exec() { - // FIXME: NO OP - setHasFinished(true); + if (stateDisabled) { + setHasFinished(true); + + return; + } + + if (runTime() >= timeInMS) { + stop(); + + setHasFinished(true); + + return; + } } } 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 fdcce2b..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 @@ -1,21 +1,111 @@ package org.timecrafters.minibots.cyberarm.chiron.states.autonomous; import org.cyberarm.engine.V2.CyberarmState; +import org.firstinspires.ftc.robotcore.external.tfod.Recognition; import org.timecrafters.minibots.cyberarm.chiron.Robot; +import java.util.List; + public class SignalProcessor extends CyberarmState { private final Robot robot; private final String groupName, actionName; + private final double timeInMS, minConfidence; + 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; this.actionName = actionName; + + timeInMS = robot.getConfiguration().variable(groupName, actionName, "timeInMS").value(); + minConfidence = robot.getConfiguration().variable(groupName, actionName, "minConfidence").value(); + fallbackPosition = robot.getConfiguration().variable(groupName, actionName, "fallbackPosition").value(); + + stateDisabled = !robot.getConfiguration().action(groupName, actionName).enabled; + } + + @Override + public void start() { + engine.blackboardSet("parking_position", fallbackPosition); + + robot.getTfod().activate(); } @Override public void exec() { - // FIXME: NO OP + if (stateDisabled) { + stop(); + + return; + } + + if (runTime() >= timeInMS) { + stop(); + + return; + } + + if (robot.getTfod() != null) { + // getUpdatedRecognitions() will return null if no new information is available since + // the last time that call was made. + List recognitions = robot.getTfod().getUpdatedRecognitions(); + + if (recognitions != null) { + updatedRecognitions = recognitions; + } + + if (recognitions != null) { + for (Recognition recognition : recognitions) { + switch (recognition.getLabel()) { + case "1 Bolt": + engine.blackboardSet("parking_position", 1); + + break; + case "2 Bulb": + engine.blackboardSet("parking_position", 2); + + break; + case "3 Panel": + engine.blackboardSet("parking_position", 3); + + break; + } + } + } + } + } + + @Override + public void telemetry() { + if (robot.getTfod() != null) { + if (updatedRecognitions != null) { + engine.telemetry.addData("# Objects Detected", updatedRecognitions.size()); + + // step through the list of recognitions and display image position/size information for each one + // Note: "Image number" refers to the randomized image orientation/number + for (Recognition recognition : updatedRecognitions) { + double col = (recognition.getLeft() + recognition.getRight()) / 2 ; + double row = (recognition.getTop() + recognition.getBottom()) / 2 ; + double width = Math.abs(recognition.getRight() - recognition.getLeft()) ; + double height = Math.abs(recognition.getTop() - recognition.getBottom()) ; + + engine.telemetry.addData(""," "); + engine.telemetry.addData("Image", "%s (%.0f %% Conf.)", recognition.getLabel(), recognition.getConfidence() * 100 ); + engine.telemetry.addData("- Position (Row/Col)","%.0f / %.0f", row, col); + engine.telemetry.addData("- Size (Width/Height)","%.0f / %.0f", width, height); + } + } + } + } + + @Override + public void stop() { setHasFinished(true); + + robot.getTfod().deactivate(); } } diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Wrist.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Wrist.java new file mode 100644 index 0000000..9d8e216 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Wrist.java @@ -0,0 +1,54 @@ +package org.timecrafters.minibots.cyberarm.chiron.states.autonomous; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.minibots.cyberarm.chiron.Robot; + +public class Wrist extends CyberarmState { + private final Robot robot; + private final String groupName, actionName; + + private final double timeInMS; + private final boolean stateDisabled, positionUp; + + public Wrist(Robot robot, String groupName, String actionName) { + this.robot = robot; + this.groupName = groupName; + this.actionName = actionName; + + timeInMS = robot.getConfiguration().variable(groupName, actionName, "timeInMS").value(); + positionUp = robot.getConfiguration().variable(groupName, actionName, "positionUp").value(); + + stateDisabled = !robot.getConfiguration().action(groupName, actionName).enabled; + } + + @Override + public void start() { + if (positionUp) { + robot.wristPosition(Robot.WristPosition.UP); + } else { + robot.wristPosition(Robot.WristPosition.DOWN); + } + } + + @Override + public void exec() { + if (stateDisabled) { + setHasFinished(true); + + return; + } + + if (runTime() >= timeInMS) { + stop(); + + setHasFinished(true); + + return; + } + } + + @Override + public void stop() { + robot.wrist.setPower(0); + } +} 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 9536ce7..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,24 +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() - lastWristManualControlTime >= stepInterval) { - lastWristManualControlTime = runTime(); + robot.reportStatus(Robot.Status.WARNING); + + robot.arm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); if (controller.left_trigger > 0) { // Arm DOWN - // robot.arm.setVelocity(5, AngleUnit.DEGREES); - 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() { @@ -78,18 +84,21 @@ public class ArmDriverControl extends CyberarmState { return; } - double stepInterval = robot.tuningConfig("wrist_manual_step_interval").value(); - double stepSize = robot.tuningConfig("wrist_manual_step_size").value(); + double stepPower = robot.tuningConfig("wrist_manual_step_power").value(); - if ((controller.dpad_left || controller.dpad_right) && runTime() - lastArmManualControlTime >= stepInterval) { - lastArmManualControlTime = runTime(); + if ((controller.dpad_left || controller.dpad_right)) { + robot.wristManuallyControlled = true; if (controller.dpad_left) { // Wrist Left - robot.wrist.setPosition(robot.wrist.getPosition() - stepSize); - - } else if (controller.dpad_right) { // Wrist Right - robot.wrist.setPosition(robot.wrist.getPosition() + stepSize); + robot.wrist.setPower(stepPower); } + if (controller.dpad_right) { // Wrist Right + robot.wrist.setPower(-stepPower); + } + } + + if (robot.wristManuallyControlled && !controller.dpad_left && !controller.dpad_right) { + robot.wrist.setPower(0); } } @@ -103,7 +112,7 @@ public class ArmDriverControl extends CyberarmState { private void automatics() { if (!robot.hardwareFault) { - automaticWrist(); +// automaticWrist(); automaticArmVelocity(); } @@ -118,13 +127,17 @@ public class ArmDriverControl extends CyberarmState { double angle = robot.tuningConfig("wrist_auto_rotate_angle").value(); if (robot.ticksToAngle(robot.arm.getCurrentPosition()) >= angle) { - robot.wrist.setPosition(robot.tuningConfig("wrist_deposit_position").value()); + robot.wrist.setPower(robot.tuningConfig("wrist_up_power").value()); } else { - robot.wrist.setPosition(robot.tuningConfig("wrist_collect_position").value()); + robot.wrist.setPower(robot.tuningConfig("wrist_down_power").value()); } } private void automaticArmVelocity() { + if (robot.armManuallyControlled) { + return; + } + robot.arm.setMode(DcMotor.RunMode.RUN_TO_POSITION); // robot.controlMotorPIDF( @@ -146,47 +159,6 @@ public class ArmDriverControl extends CyberarmState { } } - private void armPosition(Robot.ArmPosition position) { - if (robot.hardwareFault) { - return; - } - - robot.reportStatus(Robot.Status.WARNING); - - switch (position) { - case COLLECT: - robot.arm.setTargetPosition(robot.angleToTicks(robot.tuningConfig("arm_position_angle_collect").value())); - break; - - case GROUND: - robot.arm.setTargetPosition(robot.angleToTicks(robot.tuningConfig("arm_position_angle_ground").value())); - break; - - case LOW: - robot.arm.setTargetPosition(robot.angleToTicks(robot.tuningConfig("arm_position_angle_low").value())); - break; - - case MEDIUM: - robot.arm.setTargetPosition(robot.angleToTicks(robot.tuningConfig("arm_position_angle_medium").value())); - break; - - case HIGH: - robot.arm.setTargetPosition(robot.angleToTicks(robot.tuningConfig("arm_position_angle_high").value())); - break; - - default: - throw new RuntimeException("Unexpected arm position!"); - } - } - - private void gripperOpen() { - robot.gripper.setPosition(robot.tuningConfig("gripper_open_position").value()); - } - - private void gripperClosed() { - robot.gripper.setPosition(robot.tuningConfig("gripper_closed_position").value()); - } - @Override public void buttonDown(Gamepad gamepad, String button) { // Swap controlling gamepad @@ -199,32 +171,35 @@ public class ArmDriverControl extends CyberarmState { } // Gripper Control - if (button.equals("left_bumper")) { - gripperReleaseTriggeredTime = runTime(); - } else if (button.equals("right_bumper")) { - gripperClosed(); + if (button.equals("right_bumper")) { + robot.gripperClosed(); + } else if (button.equals("left_bumper")) { + robot.gripperOpen(); } // Wrist Control + if (button.equals("dpad_up")) { + robot.wristPosition(Robot.WristPosition.UP); + } + if (button.equals("dpad_down")) { - robot.wristManuallyControlled = false; - - robot.wrist.setPosition(robot.tuningConfig("wrist_deposit_position").value()); - } else if (button.equals("dpad_up")) { - robot.wristManuallyControlled = false; - - robot.wrist.setPosition(robot.tuningConfig("wrist_collect_position").value()); + robot.wristPosition(Robot.WristPosition.DOWN); } // Automatic Arm Control - if (button.equals("a")) { - armPosition(Robot.ArmPosition.COLLECT); - } else if (button.equals("x")) { - armPosition(Robot.ArmPosition.GROUND); - } else if (button.equals("b")) { - armPosition(Robot.ArmPosition.LOW); - } else if (button.equals("y")) { - armPosition(Robot.ArmPosition.MEDIUM); + switch (button) { + case "a": + robot.armPosition(Robot.ArmPosition.COLLECT); + break; + case "x": + robot.armPosition(Robot.ArmPosition.GROUND); + break; + case "b": + robot.armPosition(Robot.ArmPosition.LOW); + break; + case "y": + robot.armPosition(Robot.ArmPosition.MEDIUM); + break; } } @@ -233,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) { - 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 cd0ccf6..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 @@ -1,5 +1,6 @@ package org.timecrafters.minibots.cyberarm.chiron.states.teleop; +import com.acmerobotics.roadrunner.geometry.Vector2d; import com.qualcomm.robotcore.hardware.Gamepad; import org.cyberarm.engine.V2.CyberarmState; @@ -47,7 +48,21 @@ 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); @@ -126,6 +141,12 @@ public class DrivetrainDriverControl extends CyberarmState { // 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("back")) {