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

Sync changes

Browse Source
This commit is contained in:
Cyberarm
2023-12-16 15:56:44 -06:00
parent b34cfd7439
commit a4ed6101c4
11 changed files with 125 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
TeamCode/src/main/java/dev/cyberarm/engine/V2/CyberarmEngine.java
View File

@@ -39,6 +39,7 @@ public abstract class CyberarmEngine extends OpMode {
public boolean showStateChildrenListInTelemetry = false;
private GamepadChecker gamepadCheckerGamepad1, gamepadCheckerGamepad2;
private boolean useThreads = true;
/**
* Called when INIT button on Driver Station is pushed
@@ -482,4 +483,11 @@ public abstract class CyberarmEngine extends OpMode {
}
}
}
/**
* NO OP
*/
public void threadless() {
useThreads = false;
}
}

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

@@ -32,15 +32,15 @@ public class RedCrabMinibot {
/// TUNING CONSTANTS ///
public static final double DRIVETRAIN_MAX_SPEED = 0.5;
public static final double CLAW_ARM_MAX_SPEED = 0.5;
public static final double CLAW_ARM_kP = 0.025;
public static final double CLAW_ARM_POSITION_TOLERANCE = 3.3;
public static final double CLAW_ARM_kP = 0.1;
public static final double CLAW_ARM_POSITION_TOLERANCE = 1.5;
public static final double WINCH_MAX_SPEED = 0.5;
public static final double CLAW_ARM_STOW_ANGLE = 45.0; // 45.0
public static final double CLAW_ARM_DEPOSIT_ANGLE = 130.0; // 110.0
public static final double CLAW_ARM_COLLECT_FLOAT_ANGLE = 180.0;
public static final double CLAW_ARM_COLLECT_ANGLE = 200.0;
public static final double CLAW_WRIST_STOW_POSITION = 0.5;
public static final double CLAW_WRIST_STOW_POSITION = 0.7;
public static final double CLAW_WRIST_DEPOSIT_POSITION = 0.64;
public static final double CLAW_WRIST_COLLECT_FLOAT_POSITION = 0.64;
public static final double CLAW_WRIST_COLLECT_POSITION = 0.64;
@@ -59,7 +59,7 @@ public class RedCrabMinibot {
/// MOTOR CONSTANTS ///
public static final int CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION = 4;
public static final double CLAW_ARM_MOTOR_GEAR_RATIO = 72;
public static final double CLAW_ARM_MOTOR_GEAR_RATIO = 80; // Technically 72, but there is a lot of slop
/// HARDWARE ///
public final IMU imu;
@@ -104,6 +104,10 @@ public class RedCrabMinibot {
RevHubOrientationOnRobot.UsbFacingDirection.UP));
imu.initialize(parameters);
if (autonomous) {
imu.resetYaw();
}
/// DRIVE TRAIN ///
/// ------------------------------------------------------------------------------------ ///
frontLeft = new MotorEx(engine.hardwareMap, "frontLeft"); // | Ctrl|Ex Hub, Port: ??
@@ -134,6 +138,19 @@ public class RedCrabMinibot {
left = new MotorGroup(frontLeft, backLeft);
right = new MotorGroup(frontRight, backRight);
/// --- MOTOR DISTANCE PER TICK
double gearRatio = config.variable("Robot", "Drivetrain_Tuning", "gear_ratio").value();
double motorTicks = config.variable("Robot", "Drivetrain_Tuning", "motor_ticks").value();
double wheelDiameterMM = config.variable("Robot", "Drivetrain_Tuning", "wheel_diameter_mm").value();
double wheelCircumference = Math.PI * wheelDiameterMM;
double distancePerTick = (motorTicks * gearRatio) / wheelCircumference; // raw motor encoder * gear ratio
frontLeft.setDistancePerPulse(distancePerTick);
frontRight.setDistancePerPulse(distancePerTick);
backLeft.setDistancePerPulse(distancePerTick);
backRight.setDistancePerPulse(distancePerTick);
/// WINCH ///
/// ------------------------------------------------------------------------------------ ///
winch = new MotorEx(engine.hardwareMap, "winch"); // | Ctrl|Ex Hub, Port: ??

8
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueAudienceEngine.java
View File

@@ -6,15 +6,21 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig
import dev.cyberarm.engine.V2.CyberarmEngine;
import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
import dev.cyberarm.minibots.red_crab.states.ClawArmTask;
@Autonomous(name = "Cyberarm Red Crab BLUE AUDIENCE", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp")
public class RedCrabAutonomousBlueAudienceEngine extends CyberarmEngine {
@Override
public void setup() {
RedCrabMinibot robot = new RedCrabMinibot(true);
blackboardSet("clawArmPower", 0.0);
addTask(new ClawArmTask(robot));
setupFromConfig(
new TimeCraftersConfiguration("cyberarm_RedCrab"),
"dev.cyberarm.minibots.red_crab.states",
new RedCrabMinibot(true),
robot,
RedCrabMinibot.class,
"Autonomous_BLUE_Audience"
);

8
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueBackstageEngine.java
View File

@@ -6,15 +6,21 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig
import dev.cyberarm.engine.V2.CyberarmEngine;
import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
import dev.cyberarm.minibots.red_crab.states.ClawArmTask;
@Autonomous(name = "Cyberarm Red Crab BLUE BACKSTAGE", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp")
public class RedCrabAutonomousBlueBackstageEngine extends CyberarmEngine {
@Override
public void setup() {
RedCrabMinibot robot = new RedCrabMinibot(true);
blackboardSet("clawArmPower", 0.0);
addTask(new ClawArmTask(robot));
setupFromConfig(
new TimeCraftersConfiguration("cyberarm_RedCrab"),
"dev.cyberarm.minibots.red_crab.states",
new RedCrabMinibot(true),
robot,
RedCrabMinibot.class,
"Autonomous_BLUE_Backstage"
);

8
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedAudienceEngine.java
View File

@@ -6,15 +6,21 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig
import dev.cyberarm.engine.V2.CyberarmEngine;
import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
import dev.cyberarm.minibots.red_crab.states.ClawArmTask;
@Autonomous(name = "Cyberarm Red Crab RED AUDIENCE", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp")
public class RedCrabAutonomousRedAudienceEngine extends CyberarmEngine {
@Override
public void setup() {
RedCrabMinibot robot = new RedCrabMinibot(true);
blackboardSet("clawArmPower", 0.0);
addTask(new ClawArmTask(robot));
setupFromConfig(
new TimeCraftersConfiguration("cyberarm_RedCrab"),
"dev.cyberarm.minibots.red_crab.states",
new RedCrabMinibot(true),
robot,
RedCrabMinibot.class,
"Autonomous_RED_Audience"
);

8
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedBackstageEngine.java
View File

@@ -6,15 +6,21 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig
import dev.cyberarm.engine.V2.CyberarmEngine;
import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
import dev.cyberarm.minibots.red_crab.states.ClawArmTask;
@Autonomous(name = "Cyberarm Red Crab RED BACKSTAGE", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp")
public class RedCrabAutonomousRedBackstageEngine extends CyberarmEngine {
@Override
public void setup() {
RedCrabMinibot robot = new RedCrabMinibot(true);
blackboardSet("clawArmPower", 0.0);
addTask(new ClawArmTask(robot));
setupFromConfig(
new TimeCraftersConfiguration("cyberarm_RedCrab"),
"dev.cyberarm.minibots.red_crab.states",
new RedCrabMinibot(true),
robot,
RedCrabMinibot.class,
"Autonomous_RED_Backstage"
);

6
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/ClawArmMove.java
View File

@@ -27,7 +27,8 @@ public class ClawArmMove extends CyberarmState {
public void start() {
robot.clawArm.setPositionTolerance(Utilities.motorAngle(motorTicks, gearRatio, toleranceAngle));
robot.clawArm.setTargetPosition(Utilities.motorAngle(motorTicks, gearRatio, targetAngle));
robot.clawArm.set(power);
engine.blackboardSet("clawArmPower", power);
}
@Override
@@ -40,8 +41,11 @@ public class ClawArmMove extends CyberarmState {
@Override
public void telemetry() {
engine.telemetry.addLine();
engine.telemetry.addData("Motor Power", robot.clawArm.get());
engine.telemetry.addData("Motor Position", robot.clawArm.getCurrentPosition());
engine.telemetry.addData("Motor Angle", Utilities.motorAngle(motorTicks, gearRatio, robot.clawArm.getCurrentPosition()));
engine.telemetry.addData("Motor Target Position", Utilities.motorAngle(motorTicks, gearRatio, targetAngle));
engine.telemetry.addData("Motor Target Angle", targetAngle);
engine.telemetry.addData("Timeout MS", timeoutMS);
progressBar(20, runTime() / timeoutMS);
}

18
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/ClawArmTask.java
View File

@@ -1,2 +1,18 @@
package dev.cyberarm.minibots.red_crab.states;public class ClawArmTask {
package dev.cyberarm.minibots.red_crab.states;
import dev.cyberarm.engine.V2.CyberarmState;
import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
public class ClawArmTask extends CyberarmState {
private final RedCrabMinibot robot;
public ClawArmTask(RedCrabMinibot robot) {
this.robot = robot;
}
@Override
public void exec() {
double power = engine.blackboardGetDouble("clawArmPower");
robot.clawArm.set(power);
}
}

25
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Move.java
View File

@@ -93,13 +93,13 @@ public class Move extends CyberarmState {
engine.telemetry.addData("lerp MM UP", lerpMM_UP);
engine.telemetry.addData("lerp MM DOWN", lerpMM_DOWN);
engine.telemetry.addData("Distance MM", distanceMM);
engine.telemetry.addData("Distance Travelled MM", (strafe ? robot.left.getDistance() : robot.frontLeft.getDistance()));
engine.telemetry.addData("Distance Travelled MM", robot.frontLeft.getDistance());
engine.telemetry.addData("Timeout MS", timeoutMS);
progressBar(20, runTime() / timeoutMS);
}
private void tankMove(){
double travelledDistance = Math.abs(robot.left.getDistance());
double travelledDistance = Math.abs(robot.frontLeft.getDistance());
double power = lerpPower(travelledDistance);
double angleDiff = Utilities.angleDiff(initialHeadingDegrees, Utilities.facing(robot.imu));
@@ -107,17 +107,19 @@ public class Move extends CyberarmState {
double leftPower = power;
double rightPower = power;
// use +10% of power at 7 degrees of error to correct angle
double correctivePower = Utilities.lerp(0.0, 1.0, angleDiff / 7.0) * (power + power * 0.1);
if (angleDiff < -0.5) {
double correctivePower = Utilities.lerp(0.0, 1.0, angleDiff / 7.0) * (power * 0.1);
if (angleDiff > -0.5) {
leftPower += correctivePower;
} else if (angleDiff > 0.5) {
} else if (angleDiff < 0.5) {
rightPower += correctivePower;
}
robot.left.set(leftPower);
robot.right.set(rightPower);
if (robot.left.atTargetPosition() && robot.right.atTargetPosition()) {
if (runTime() >= timeoutMS ||
(robot.frontLeft.atTargetPosition() || robot.frontRight.atTargetPosition()) ||
Math.abs(robot.frontLeft.getDistance()) >= Math.abs(distanceMM)) {
robot.left.set(0);
robot.right.set(0);
@@ -133,11 +135,11 @@ public class Move extends CyberarmState {
double frontPower = power;
double backPower = power;
// use +10% of power at 7 degrees of error to correct angle
double correctivePower = Utilities.lerp(0.0, 1.0, angleDiff / 7.0) * (power + power * 0.1);
if (angleDiff < -0.5) {
// use +40% of power at 7 degrees of error to correct angle
double correctivePower = Utilities.lerp(0.0, 1.0, angleDiff / 7.0) * (power * 0.40);
if (angleDiff > -0.5) {
frontPower += correctivePower;
} else if (angleDiff > 0.5) {
} else if (angleDiff < 0.5) {
backPower += correctivePower;
}
@@ -146,7 +148,8 @@ public class Move extends CyberarmState {
robot.backLeft.set(-backPower);
robot.backRight.set(backPower);
if (robot.frontLeft.atTargetPosition() && robot.backRight.atTargetPosition()) {
if (runTime() >= timeoutMS || (robot.frontLeft.atTargetPosition() || robot.backRight.atTargetPosition()) ||
Math.abs(robot.frontLeft.getDistance()) >= Math.abs(distanceMM) || Math.abs(robot.backRight.getDistance()) >= Math.abs(distanceMM)) {
robot.frontLeft.set(0);
robot.frontRight.set(0);
robot.backLeft.set(0);

33
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/PathSelector.java
View File

@@ -11,49 +11,56 @@ import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
public class PathSelector extends CyberarmState {
private final RedCrabMinibot robot;
private final int timeoutMS;
private final int fallbackPath;
private final int path;
private final double minConfidence;
private List<Recognition> recognitions = new ArrayList<>();
public PathSelector(RedCrabMinibot robot, String groupName, String actionName) {
this.robot = robot;
this.timeoutMS = robot.config.variable(groupName, actionName, "timeoutMS").value();
this.fallbackPath = robot.config.variable(groupName, actionName, "fallbackPath").value();
this.path = robot.config.variable(groupName, actionName, "path").value();
this.minConfidence = robot.config.variable(groupName, actionName, "minConfidence").value();
}
@Override
public void init() {
robot.tfod.setClippingMargins(0, 0, 0, 0);
robot.tfod.setMinResultConfidence(0.8f);
robot.tfod.setMinResultConfidence((float) minConfidence);
engine.blackboardSet("autonomousPath", fallbackPath);
engine.blackboardSet("autonomousPath", 0);
}
@Override
public void exec() {
if (engine.blackboardGetInt("autonomousPath") != 0) {
this.finished();
return;
}
recognitions = robot.tfod.getRecognitions();
for (Recognition recognition : recognitions) {
double x = (recognition.getLeft() + recognition.getRight()) / 2;
double y = (recognition.getTop() + recognition.getBottom()) / 2;
if (recognition.getLabel().equals("pixel")) {
if (x < 120) {
engine.blackboardSet("autonomousPath", RedCrabMinibot.Path.LEFT.ordinal());
} else if (x >= 120 && x < 240) {
engine.blackboardSet("autonomousPath", RedCrabMinibot.Path.CENTER.ordinal());
} else {
engine.blackboardSet("autonomousPath", RedCrabMinibot.Path.RIGHT.ordinal());
}
engine.blackboardSet("autonomousPath", path);
}
}
if (runTime() >= timeoutMS) {
robot.visionPortal.close();
robot.tfod.shutdown();
stopVision();
finished();
}
}
private void stopVision() {
robot.visionPortal.close();
robot.tfod.shutdown();
}
@Override
public void telemetry() {
engine.telemetry.addLine();

19
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Rotate.java
View File

@@ -13,6 +13,7 @@ public class Rotate extends CyberarmState {
final private double maxPower, minPower, lerpDegrees, headingDegrees, toleranceDegrees;
final private int timeoutMS;
private boolean commitToRotation = false;
public Rotate(RedCrabMinibot robot, String groupName, String actionName) {
this.robot = robot;
@@ -25,7 +26,7 @@ public class Rotate extends CyberarmState {
this.lerpDegrees = robot.config.variable(groupName, actionName, "lerpDEGREES").value();
this.headingDegrees = robot.config.variable(groupName, actionName, "headingDEGREES").value();
this.toleranceDegrees = robot.config.variable(groupName, actionName, "toleranceDEGREEES").value();
this.toleranceDegrees = robot.config.variable(groupName, actionName, "toleranceDEGREES").value();
this.timeoutMS = robot.config.variable(groupName, actionName, "timeoutMS").value();
}
@@ -45,13 +46,17 @@ public class Rotate extends CyberarmState {
double power = Utilities.lerp(minPower, maxPower, Range.clip(Math.abs(angleDiff) / lerpDegrees, 0.0, 1.0));
if (angleDiff > 0) {
robot.left.set(-power);
robot.right.set(power);
} else {
robot.left.set(power);
robot.right.set(-power);
if (!commitToRotation) {
if (angleDiff < 0) {
robot.left.set(-power);
robot.right.set(power);
} else {
robot.left.set(power);
robot.right.set(-power);
}
}
commitToRotation = Math.abs(angleDiff) > 170;
}
@Override