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

Working TeleOP

Browse Source
This commit is contained in:
Cyberarm
2023-01-26 21:30:28 -06:00
parent 1b50fd015b
commit dd018e1afc
6 changed files with 92 additions and 82 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

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

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

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

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

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

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

@@ -48,7 +48,20 @@ public class DrivetrainDriverControl extends CyberarmState {
}
double y = invertRobotForward ? controller.left_stick_y : -controller.left_stick_y;
double x = (invertRobotForward && !fieldCentricControl ? controller.left_stick_x : -controller.left_stick_x) * 1.1; // Counteract imperfect strafing
double x = (invertRobotForward && !fieldCentricControl ? controller.left_stick_x : -controller.left_stick_x);
// Improve control?
if (y < 0) {
y = -Math.sqrt(-y);
} else {
y = Math.sqrt(y);
}
if (x < 0) {
x = -Math.sqrt(-x) * 1.1; // Counteract imperfect strafing;
} else {
x = Math.sqrt(x) * 1.1; // Counteract imperfect strafing;
}
double rx = -controller.right_stick_x;
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);