TimeCrafters/CenterStage
1
0
Fork 0
mirror of https://github.com/TimeCrafters/CenterStage synced 2025-12-13 07:42:36 +00:00
Code Issues Packages Projects Releases Wiki Activity

RedCrab: Updated HookArm servo positions, drone latch resets to initial position after confirmation_time_ms + 1 second, tweaked ClawArm PID to reduce back smacking; changes to Localizer, added AMS (Alert Management System) to show important messages on telemetry for 5 seconds, reset deadwheels on setup, localizer can be reset to 0,0,0 with left and right joystick buttons at the same time for 1 second. probably some other stuff also... 🤷

Browse Source
This commit is contained in:
Cyberarm
2024-01-27 15:02:01 -06:00
parent dca17c786d
commit 337652018d
5 changed files with 158 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

91
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/AlertManagementSystem.java Normal file
View File

@@ -0,0 +1,91 @@
package dev.cyberarm.minibots.red_crab;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import java.util.Collections;
import java.util.Comparator;
import java.util.concurrent.ConcurrentHashMap;
import java.util.concurrent.CopyOnWriteArrayList;
public class AlertManagementSystem {
private CopyOnWriteArrayList<Alert> alerts = new CopyOnWriteArrayList<>();
public enum Priority {
CRITICAL, // Something is on "fire"
WARNING, // Something is soon to be on "fire"
ALERT, // Something isn't quite right
NOTICE // Informational message
}
public AlertManagementSystem() {
}
public void report(Telemetry telemetry) {
if (alerts.size() == 0) {
// Pad a few lines so telemetry doesn't jump about (as much)
telemetry.addLine();
telemetry.addLine();
telemetry.addLine();
return;
}
telemetry.addLine("------ ALERT MANAGEMENT SYSTEM ------");
alerts.stream().sorted(Comparator.comparing(Alert::getPriority)).filter(Alert::valid).forEach(
alert -> telemetry.addData("", " %s: %s", alert.priority.name(), alert.message));
telemetry.addLine("-------------------------------------------------------------"); // Gotta love non-monospace fonts, eh?
alerts.stream().filter(Alert::expired).forEach(alert -> alerts.remove(alert));
}
public void addCritical(String category, String message) {
alerts.add(
new Alert(Priority.CRITICAL, category, message)
);
}
public void addWarning(String category, String message) {
alerts.add(
new Alert(Priority.WARNING, category, message)
);
}
public void addAlert(String category, String message) {
alerts.add(
new Alert(Priority.ALERT, category, message)
);
}
public void addNotice(String category, String message) {
alerts.add(
new Alert(Priority.NOTICE, category, message)
);
}
public class Alert {
private final Priority priority;
private final String category;
private final String message;
private final long timeToLive;
private final long bornAt;
private Alert(Priority priority, String category, String message) {
this.priority = priority;
this.category = category;
this.message = message;
this.bornAt = System.currentTimeMillis();
this.timeToLive = 5_000;
}
private int getPriority() {
return priority.ordinal();
}
private boolean expired() {
return System.currentTimeMillis() - bornAt >= timeToLive;
}
private boolean valid() {
return !expired();
}
}
}

7
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/Localizer.java
View File

@@ -1,5 +1,7 @@
package dev.cyberarm.minibots.red_crab;
import com.arcrobotics.ftclib.kinematics.HolonomicOdometry;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.slf4j.helpers.Util;
@@ -9,7 +11,7 @@ import dev.cyberarm.engine.V2.Utilities;
public class Localizer {
private final RedCrabMinibot robot;
private double rawX = 0, rawY = 0, rawR = 0;
private double trackWidthMM = 365.0, forwardOffsetMM = -140.0, wheelDiameterMM = 90.0;
private double trackWidthMM = 365.0, forwardOffsetMM = 140.0, wheelDiameterMM = 90.0;
private int lastEncoderXLeft = 0, lastEncoderXRight = 0, lastEncoderYCenter = 0;
public Localizer(RedCrabMinibot robot) {
@@ -45,8 +47,6 @@ public class Localizer {
int rightEncoder = robot.deadWheelXRight.getCurrentPosition();
int centerEncoder = robot.deadWheelYCenter.getCurrentPosition();
double heading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
int deltaLeft = leftEncoder - lastEncoderXLeft;
int deltaRight = rightEncoder - lastEncoderXRight;
int deltaCenter = centerEncoder - lastEncoderYCenter;
@@ -55,6 +55,7 @@ public class Localizer {
double deltaMiddle = (deltaLeft + deltaRight) / 2.0;
double deltaPerp = deltaCenter - forwardOffsetMM * phi;
double heading = rawR + phi;
double deltaX = deltaMiddle * Math.cos(heading) - deltaPerp * Math.sin(heading);
double deltaY = deltaMiddle * Math.sin(heading) + deltaPerp * Math.cos(heading);

14
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java
View File

@@ -113,8 +113,12 @@ public class RedCrabMinibot {
/// Doohickey
public VisionPortal visionPortal = null;
/* --- Localizer / Odometry --- */
public static Localizer localizer;
/// Alert Management System ///
public AlertManagementSystem ams = new AlertManagementSystem();
public RedCrabMinibot(boolean autonomous) {
engine = CyberarmEngine.instance;
@@ -242,6 +246,13 @@ public class RedCrabMinibot {
deadWheelXRight.setDirection(DcMotorSimple.Direction.FORWARD);
// deadWheelYCenter.setDirection(DcMotorSimple.Direction.FORWARD);
deadWheelXLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
deadWheelXRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
deadWheelYCenter.reset();
deadWheelXLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
deadWheelXRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
// Bulk read from hubs
Utilities.hubsBulkReadMode(engine.hardwareMap, LynxModule.BulkCachingMode.MANUAL);
@@ -312,6 +323,8 @@ public class RedCrabMinibot {
public void standardTelemetry() {
engine.telemetry.addLine();
this.ams.report(engine.telemetry);
if (RedCrabMinibot.localizer != null) {
engine.telemetry.addLine("Localizer");
engine.telemetry.addData("X (MM)", "%.2fmm", RedCrabMinibot.localizer.xMM());
@@ -457,6 +470,7 @@ public class RedCrabMinibot {
if (milliseconds - lastClawArmOverCurrentAnnounced >= CLAW_ARM_WARN_OVERCURRENT_AFTER_MS) {
lastClawArmOverCurrentAnnounced = System.currentTimeMillis();
this.ams.addWarning("CLAW ARM", String.format("Claw Arm Over Current (%.0f mAmp)", clawArm.getCurrent(CurrentUnit.MILLIAMPS)));
engine.telemetry.speak("WARNING. ARM. OVER. CURRENT.");
}
} else {

2
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/cyberarm_RedCrab.json
View File

File diff suppressed because one or more lines are too long

50
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java
View File

@@ -20,6 +20,10 @@ public class Pilot extends CyberarmState {
private boolean droneLaunchAuthorized = false;
private boolean droneLaunchRequested = false;
private double droneLastLaunchRequestStartMS = 0;
private double odometryResetRequestStartMS = 0;
private boolean odometryResetRequested = false;
private boolean odometryResetRequestLeftStick = false;
private boolean odometryResetRequestRightStick = false;
private boolean robotSlowMode = false;
private boolean triggersControlClawArm = false;
private double deltaTime = 0;
@@ -40,6 +44,7 @@ public class Pilot extends CyberarmState {
droneLatchController();
hookArmController(); // disabled for swrist debug
winchController();
odometryDebugController();
lastLoopTimeMS = runTime();
}
@@ -60,6 +65,8 @@ public class Pilot extends CyberarmState {
break;
case "guide":
robot.imu.resetYaw();
robot.ams.addNotice("IMU", "IMU RESET");
engine.telemetry.speak("IMU. RESET.");
break;
case "left_bumper":
leftClawOpen = !leftClawOpen;
@@ -85,14 +92,22 @@ public class Pilot extends CyberarmState {
if (triggersControlClawArm)
{
engine.telemetry.speak("Claw Arm Manual");
robot.ams.addNotice("CLAW ARM", "Claw Arm Manual");
engine.telemetry.speak("Claw. Arm. Manual.");
} else {
engine.telemetry.speak("Winch Manual");
robot.ams.addNotice("WINCH", "Winch Manual");
engine.telemetry.speak("Winch. Manual.");
}
break;
case "start":
robot.reloadConfig();
break;
case "left_stick_button":
odometryResetRequestLeftStick = true;
break;
case "right_stick_button":
odometryResetRequestRightStick = true;
break;
}
}
}
@@ -105,6 +120,12 @@ public class Pilot extends CyberarmState {
droneLaunchRequested = false;
droneLastLaunchRequestStartMS = runTime();
break;
case "left_stick_button":
odometryResetRequestLeftStick = false;
break;
case "right_stick_button":
odometryResetRequestRightStick = false;
break;
}
}
}
@@ -224,6 +245,13 @@ public class Pilot extends CyberarmState {
if (droneLaunchAuthorized) {
robot.droneLatch.setPosition(RedCrabMinibot.DRONE_LATCH_LAUNCH_POSITION);
}
// Auto reset drone latch after DRONE_LAUNCH_CONFIRMATION_TIME_MS + 1 second
if (!droneLaunchRequested && runTime() - droneLastLaunchRequestStartMS >= RedCrabMinibot.DRONE_LAUNCH_CONFIRMATION_TIME_MS + 1_000) {
droneLaunchAuthorized = false;
robot.droneLatch.setPosition(RedCrabMinibot.DRONE_LATCH_INITIAL_POSITION);
}
}
private void hookArmController() {
@@ -249,4 +277,22 @@ public class Pilot extends CyberarmState {
robot.winch.setPower(RedCrabMinibot.WINCH_MAX_SPEED);
}
}
private void odometryDebugController() {
if (odometryResetRequestLeftStick && odometryResetRequestRightStick) {
if (!odometryResetRequested) {
odometryResetRequested = true;
odometryResetRequestStartMS = runTime();
} else if (runTime() - odometryResetRequestStartMS >= 1_000) {
RedCrabMinibot.localizer.reset();
odometryResetRequestStartMS = runTime();
odometryResetRequested = false;
robot.ams.addNotice("ODOMETRY", "Odometry Reset");
engine.telemetry.speak("ODOMETRY. RESET.");
}
} else {
odometryResetRequested = false;
odometryResetRequestStartMS = runTime();
}
}
}