TimeCrafters/FTC_2022
1
0
Fork 0
mirror of https://github.com/TimeCrafters/FTC_2022 synced 2025-12-15 14:22:34 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'origin/master'

Browse Source
This commit is contained in:
Sodi
2023-01-27 09:46:10 -06:00
parent 5baefbccf7 dd018e1afc
commit ebbb373a23
18 changed files with 663 additions and 191 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
.idea/deploymentTargetDropDown.xml generated Normal file
View File

@@ -0,0 +1,17 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="deploymentTargetDropDown">
<runningDeviceTargetSelectedWithDropDown>
<Target>
<type value="RUNNING_DEVICE_TARGET" />
<deviceKey>
<Key>
<type value="SERIAL_NUMBER" />
<value value="192.168.43.1:5555" />
</Key>
</deviceKey>
</Target>
</runningDeviceTargetSelectedWithDropDown>
<timeTargetWasSelectedWithDropDown value="2023-01-26T22:23:44.435674Z" />
</component>
</project>

2
TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java
View File

@@ -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];

41
TeamCode/src/main/java/org/cyberarm/engine/V2/GamepadChecker.java
View File

@@ -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 = 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();
}

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

@@ -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<Status> 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() {

9
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/AutonomousEngine.java
View File

@@ -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

2
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousBlueLeftSideEngine.java
View File

@@ -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";

2
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousBlueRightSideEngine.java
View File

@@ -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";

2
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousRedLeftSideEngine.java
View File

@@ -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";

2
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/autonomous_engines/AutonomousRedRightSideEngine.java
View File

@@ -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";

36
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Arm.java
View File

@@ -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;
}
}
}

38
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Gripper.java
View File

@@ -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;
}
}
}

51
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Move.java
View File

@@ -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);

84
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Rotate.java
View File

@@ -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);
}
}

34
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/SelectParkingPosition.java
View File

@@ -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;
}
}
}

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

@@ -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<Recognition> 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<Recognition> 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();
}
}

54
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Wrist.java Normal file
View File

@@ -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);
}
}

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

@@ -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();
}
}
}

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

@@ -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")) {