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

Merge remote-tracking branch 'origin/master'

Browse Source
This commit is contained in:
NerdyBirdy460
2024-02-01 20:44:32 -06:00
parent ac998525a1 6d53ab38eb
commit 66b3fb8669
16 changed files with 204 additions and 100 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -41,6 +41,8 @@ public abstract class CyberarmEngine extends OpMode {
private GamepadChecker gamepadCheckerGamepad1, gamepadCheckerGamepad2;
private boolean useThreads = true;
private long startTime;
/**
* Called when INIT button on Driver Station is pushed
* ENSURE to call super.init() if you override this method
@@ -84,6 +86,8 @@ public abstract class CyberarmEngine extends OpMode {
* ENSURE to call super.start() if you override this method
*/
public void start() {
startTime = System.currentTimeMillis();
if (cyberarmStates.size() > 0) {
runState(cyberarmStates.get(0));
}
@@ -534,4 +538,12 @@ public abstract class CyberarmEngine extends OpMode {
public void threadless() {
useThreads = false;
}
/**
* Time in milliseconds since START button was pushed
* @return runTime
*/
public double runTime() {
return (System.currentTimeMillis() - startTime);
}
}

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

@@ -13,12 +13,12 @@ import dev.cyberarm.engine.V2.Utilities;
public class Localizer {
private final RedCrabMinibot robot;
private double rawX = 0, rawY = 0, rawR = 0, offsetX = 0, offsetY = 0;
private final double trackWidthMM = 365.0, forwardOffsetMM = 140.0, wheelDiameterMM = 90.0;
private final double trackWidthMM = 387.35, forwardOffsetMM = 133.35, wheelDiameterMM = 90.0;
private final int encoderTicksPerRevolution = 8192;
private final double encoderGearRatio = 1;
private double lastEncoderXLeftMM, lastEncoderXRightMM, lastEncoderYCenterMM;
// private double xDeltaMultiplier = 0.87012987, yDeltaMultiplier = 0.25;
private double xDeltaMultiplier = 1, yDeltaMultiplier = 1;
private double xPosMultiplier = 0.675956739, yPosMultiplier = 0.941867531;
private HolonomicOdometry odometry;
public Localizer(RedCrabMinibot robot) {
this.robot = robot;
@@ -100,11 +100,11 @@ public class Localizer {
}
public double xMM() {
return odometry.getPose().getX() + offsetX; //rawX;
return odometry.getPose().getX() * xPosMultiplier + offsetX; //rawX;
}
public double yMM() {
return odometry.getPose().getY() + offsetY; //rawY;
return odometry.getPose().getY() * xPosMultiplier + offsetY; //rawY;
}
public double xIn() {

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

@@ -88,8 +88,10 @@ public class RedCrabMinibot {
public final DcMotorEx deadWheelXLeft, deadWheelXRight;
public final EncoderCustomKB2040 deadWheelYCenter;
public final DigitalChannel greenLED, redLED;
public final boolean LED_OFF = true, LED_ON = false;
final CyberarmEngine engine;
public final boolean autonomous;
public TimeCraftersConfiguration config;
private final PIDFController clawArmPIDFController;
@@ -124,6 +126,7 @@ public class RedCrabMinibot {
public RedCrabMinibot(boolean autonomous) {
engine = CyberarmEngine.instance;
this.autonomous = autonomous;
config = new TimeCraftersConfiguration("cyberarm_RedCrab");
loadConstants();
@@ -254,8 +257,8 @@ public class RedCrabMinibot {
greenLED.setMode(DigitalChannel.Mode.OUTPUT);
redLED.setMode(DigitalChannel.Mode.OUTPUT);
greenLED.setState(true);
redLED.setState(true);
greenLED.setState(LED_OFF);
redLED.setState(LED_ON);
// Bulk read from hubs
Utilities.hubsBulkReadMode(engine.hardwareMap, LynxModule.BulkCachingMode.MANUAL);
@@ -269,6 +272,12 @@ public class RedCrabMinibot {
RedCrabMinibot.localizer.reset();
}
public void shutdown() {
// Prevent LED(s) from being on while idle
greenLED.setMode(DigitalChannel.Mode.INPUT);
redLED.setMode(DigitalChannel.Mode.INPUT);
}
public void reloadConfig() {
config = new TimeCraftersConfiguration("cyberarm_RedCrab");

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

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

@@ -4,16 +4,17 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
import dev.cyberarm.engine.V2.CyberarmEngine;
import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
import dev.cyberarm.minibots.red_crab.states.ClawArmTask;
import dev.cyberarm.minibots.red_crab.states.LocalizerTask;
@Autonomous(name = "Cyberarm Red Crab BLUE AUDIENCE", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp")
public class RedCrabAutonomousBlueAudienceEngine extends RedCrabAutonomousEngine {
public class RedCrabAutonomousBlueAudienceEngine extends RedCrabEngine {
@Override
public void setup() {
robot = new RedCrabMinibot(true);
addTask(new ClawArmTask(robot));
addTask(new LocalizerTask(robot));
setupFromConfig(
new TimeCraftersConfiguration("cyberarm_RedCrab"),

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

@@ -4,16 +4,17 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
import dev.cyberarm.engine.V2.CyberarmEngine;
import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
import dev.cyberarm.minibots.red_crab.states.ClawArmTask;
import dev.cyberarm.minibots.red_crab.states.LocalizerTask;
@Autonomous(name = "Cyberarm Red Crab BLUE BACKSTAGE", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp")
public class RedCrabAutonomousBlueBackstageEngine extends RedCrabAutonomousEngine {
public class RedCrabAutonomousBlueBackstageEngine extends RedCrabEngine {
@Override
public void setup() {
robot = new RedCrabMinibot(true);
addTask(new ClawArmTask(robot));
addTask(new LocalizerTask(robot));
setupFromConfig(
new TimeCraftersConfiguration("cyberarm_RedCrab"),

4
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousDebuggingOnlyEngine.java
View File

@@ -6,13 +6,15 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig
import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
import dev.cyberarm.minibots.red_crab.states.ClawArmTask;
import dev.cyberarm.minibots.red_crab.states.LocalizerTask;
@Autonomous(name = "Cyberarm Red Crab Auto DEBUGGING", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp")
public class RedCrabAutonomousDebuggingOnlyEngine extends RedCrabAutonomousEngine {
public class RedCrabAutonomousDebuggingOnlyEngine extends RedCrabEngine {
@Override
public void setup() {
robot = new RedCrabMinibot(true);
addTask(new ClawArmTask(robot));
addTask(new LocalizerTask(robot));
setupFromConfig(
new TimeCraftersConfiguration("cyberarm_RedCrab"),

22
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousEngine.java
View File

@@ -1,22 +0,0 @@
package dev.cyberarm.minibots.red_crab.engines;
import dev.cyberarm.engine.V2.CyberarmEngine;
import dev.cyberarm.engine.V2.Utilities;
import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
public abstract class RedCrabAutonomousEngine extends CyberarmEngine {
protected RedCrabMinibot robot;
@Override
public void loop() {
Utilities.hubsClearBulkReadCache(hardwareMap);
if (RedCrabMinibot.localizer != null) {
RedCrabMinibot.localizer.integrate();
}
super.loop();
if (robot != null)
robot.standardTelemetry();
}
}

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

@@ -4,16 +4,17 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
import dev.cyberarm.engine.V2.CyberarmEngine;
import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
import dev.cyberarm.minibots.red_crab.states.ClawArmTask;
import dev.cyberarm.minibots.red_crab.states.LocalizerTask;
@Autonomous(name = "Cyberarm Red Crab RED AUDIENCE", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp")
public class RedCrabAutonomousRedAudienceEngine extends RedCrabAutonomousEngine {
public class RedCrabAutonomousRedAudienceEngine extends RedCrabEngine {
@Override
public void setup() {
robot = new RedCrabMinibot(true);
addTask(new ClawArmTask(robot));
addTask(new LocalizerTask(robot));
setupFromConfig(
new TimeCraftersConfiguration("cyberarm_RedCrab"),

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

@@ -4,16 +4,17 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
import dev.cyberarm.engine.V2.CyberarmEngine;
import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
import dev.cyberarm.minibots.red_crab.states.ClawArmTask;
import dev.cyberarm.minibots.red_crab.states.LocalizerTask;
@Autonomous(name = "Cyberarm Red Crab RED BACKSTAGE", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp")
public class RedCrabAutonomousRedBackstageEngine extends RedCrabAutonomousEngine {
public class RedCrabAutonomousRedBackstageEngine extends RedCrabEngine {
@Override
public void setup() {
robot = new RedCrabMinibot(true);
addTask(new ClawArmTask(robot));
addTask(new LocalizerTask(robot));
setupFromConfig(
new TimeCraftersConfiguration("cyberarm_RedCrab"),

49
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabEngine.java Normal file
View File

@@ -0,0 +1,49 @@
package dev.cyberarm.minibots.red_crab.engines;
import dev.cyberarm.engine.V2.CyberarmEngine;
import dev.cyberarm.engine.V2.Utilities;
import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
public abstract class RedCrabEngine extends CyberarmEngine {
protected RedCrabMinibot robot;
@Override
public void loop() {
Utilities.hubsClearBulkReadCache(hardwareMap);
super.loop();
robot.standardTelemetry();
if (robot.autonomous) {
if (runTime() < 20_000.0) { // KEEP CALM and CARRY ON
robot.redLED.setState(robot.LED_OFF);
robot.greenLED.setState(robot.LED_ON);
} else if (runTime() < 25_000.0) { // RUNNING LOW ON TIME
robot.redLED.setState(robot.LED_ON);
robot.greenLED.setState(robot.LED_ON);
} else { // 5 SECONDS LEFT!
robot.redLED.setState(robot.LED_ON);
robot.greenLED.setState(robot.LED_OFF);
}
} else {
if (runTime() >= 90_000.0) { // LAUNCH DRONE and DO CHIN UP
robot.redLED.setState(robot.LED_OFF);
robot.greenLED.setState(robot.LED_ON);
} else if (runTime() >= 80_000.0) { // GET READY
robot.redLED.setState(robot.LED_ON);
robot.greenLED.setState(robot.LED_ON);
} else { // KEEP CALM and CARRY ON
robot.redLED.setState(robot.LED_ON);
robot.greenLED.setState(robot.LED_OFF);
}
}
}
@Override
public void stop() {
robot.shutdown();
super.stop();
}
}

20
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpDebuggingOnlyEngine.java
View File

@@ -8,15 +8,19 @@ import dev.cyberarm.engine.V2.CyberarmEngine;
import dev.cyberarm.engine.V2.CyberarmState;
import dev.cyberarm.engine.V2.Utilities;
import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
import dev.cyberarm.minibots.red_crab.states.LocalizerTask;
@TeleOp(name = "Cyberarm Red Crab TeleOp DEBUGGING", group = "MINIBOT")
public class RedCrabTeleOpDebuggingOnlyEngine extends CyberarmEngine {
public class RedCrabTeleOpDebuggingOnlyEngine extends RedCrabEngine {
@Override
public void setup() {
threadless();
robot = new RedCrabMinibot(false);
addTask(new LocalizerTask(robot));
addState(new CyberarmState() {
final RedCrabMinibot robot = new RedCrabMinibot(false);
@Override
public void exec() {
@@ -30,18 +34,6 @@ public class RedCrabTeleOpDebuggingOnlyEngine extends CyberarmEngine {
robot.clawArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.clawArm.setPower(-engine.gamepad1.left_stick_y * 0.5);
}
@Override
public void telemetry() {
robot.standardTelemetry();
}
});
}
@Override
public void loop() {
Utilities.hubsClearBulkReadCache(hardwareMap);
super.loop();
}
}

16
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpEngine.java
View File

@@ -6,26 +6,18 @@ import dev.cyberarm.engine.V2.CyberarmEngine;
import dev.cyberarm.engine.V2.Utilities;
import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
import dev.cyberarm.minibots.red_crab.states.ClawArmTask;
import dev.cyberarm.minibots.red_crab.states.LocalizerTask;
import dev.cyberarm.minibots.red_crab.states.Pilot;
@TeleOp(name = "Cyberarm Red Crab TeleOp", group = "MINIBOT")
public class RedCrabTeleOpEngine extends CyberarmEngine {
public class RedCrabTeleOpEngine extends RedCrabEngine {
@Override
public void setup() {
RedCrabMinibot robot = new RedCrabMinibot(false);
robot = new RedCrabMinibot(false);
addTask(new ClawArmTask(robot));
addTask(new LocalizerTask(robot));
addState(new Pilot(robot));
}
@Override
public void loop() {
Utilities.hubsClearBulkReadCache(hardwareMap);
if (RedCrabMinibot.localizer != null) {
RedCrabMinibot.localizer.integrate();
}
super.loop();
}
}

21
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/LocalizerTask.java Normal file
View File

@@ -0,0 +1,21 @@
package dev.cyberarm.minibots.red_crab.states;
import dev.cyberarm.engine.V2.CyberarmState;
import dev.cyberarm.engine.V2.Utilities;
import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
public class LocalizerTask extends CyberarmState {
private final RedCrabMinibot robot;
public LocalizerTask(RedCrabMinibot robot) {
this.robot = robot;
}
@Override
public void exec() {
Utilities.hubsClearBulkReadCache(engine.hardwareMap);
if (RedCrabMinibot.localizer != null) {
RedCrabMinibot.localizer.integrate();
}
}
}

112
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/MoveToCoordinate.java
View File

@@ -1,7 +1,8 @@
package dev.cyberarm.minibots.red_crab.states;
import com.qualcomm.robotcore.util.Range;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import dev.cyberarm.engine.V2.CyberarmState;
import dev.cyberarm.engine.V2.Utilities;
@@ -13,8 +14,15 @@ public class MoveToCoordinate extends CyberarmState {
private final String groupName, actionName;
private final boolean stopDrivetrain;
private final int timeoutMS;
private final double targetAngleDegrees, angleToleranceDegrees, minDistanceMM, targetX_MM, targetY_MM;
private final double targetAngleDegrees, targetAngleToleranceDegrees, minDistanceMM;
private final double maxVelocityMM, minVelocityMM, lerpMM_UP, lerpMM_DOWN, lerpDegrees;
private final Vector2D targetPosMM;
private Vector2D robotInitialPosMM = new Vector2D(), robotPosMM = new Vector2D();
private Vector2D direction = new Vector2D();
private double distanceFromTargetMM = 0;
private double velocity = 0;
private double angleDiffDegrees = 0;
private double rotationStrength = 0;
public MoveToCoordinate(RedCrabMinibot robot, String groupName, String actionName) {
this.robot = robot;
@@ -23,24 +31,46 @@ public class MoveToCoordinate extends CyberarmState {
this.timeoutMS = robot.config.variable(groupName, actionName, "timeoutMS").value();
this.stopDrivetrain = robot.config.variable(groupName, actionName, "stopDrivetrain").value();
this.angleToleranceDegrees = robot.config.variable(groupName, actionName, "angleToleranceDEGREES").value();
this.targetAngleToleranceDegrees = robot.config.variable(groupName, actionName, "targetAngleToleranceDEGREES").value();
this.targetAngleDegrees = robot.config.variable(groupName, actionName, "targetAngleDEGREES").value();
this.lerpDegrees = robot.config.variable(groupName, actionName, "lerpDEGREES").value();
this.lerpMM_UP = robot.config.variable(groupName, actionName, "lerpMM_UP").value();
this.lerpMM_DOWN = robot.config.variable(groupName, actionName, "lerpMM_DOWN").value();
this.maxVelocityMM = robot.config.variable(groupName, actionName, "maxVelocityMM").value();
this.minVelocityMM = robot.config.variable(groupName, actionName, "minVelocityMM").value();
this.minDistanceMM = robot.config.variable(groupName, actionName, "minDistanceMM").value();
String targetPosMM = robot.config.variable(groupName, actionName, "targetPosMM").value();
String[] targetPos = targetPosMM.split("x");
this.targetX_MM = Double.parseDouble(targetPos[0]);
this.targetY_MM = Double.parseDouble(targetPos[1]);
this.targetPosMM = new Vector2D(Double.parseDouble(targetPos[0]), Double.parseDouble(targetPos[1]));
}
this.targetPosMM = new Vector2D(this.targetX_MM, this.targetY_MM);
@Override
public void start() {
this.robotInitialPosMM = new Vector2D(RedCrabMinibot.localizer.xMM(), RedCrabMinibot.localizer.yMM());
}
@Override
public void exec() {
Vector2D robotPosMM = new Vector2D(RedCrabMinibot.localizer.xMM(), RedCrabMinibot.localizer.yMM());
Vector2D direction = targetPosMM.minus(robotPosMM).normalize();
double distanceMM = robotPosMM.distance(this.targetPosMM);
this.robotPosMM = new Vector2D(RedCrabMinibot.localizer.xMM(), RedCrabMinibot.localizer.yMM());
this.direction = targetPosMM.minus(robotPosMM).normalize();
this.distanceFromTargetMM = robotPosMM.distance(this.targetPosMM);
double newAngleDiffDegrees = Utilities.angleDiff(Utilities.facing(robot.imu), targetAngleDegrees);
// FIXME: Test this!
// Ignore new angle diff since it appears to be a numeric sign flip and not a useful value (prevent toggling shortest rotation decision.)
if (!(Math.abs(newAngleDiffDegrees) - Math.abs(this.angleDiffDegrees) <= 2.0 && Math.abs(newAngleDiffDegrees) >= 178.0))
this.angleDiffDegrees = newAngleDiffDegrees;
if (distanceMM <= minDistanceMM || runTime() >= timeoutMS) {
boolean rotationGood = Utilities.isBetween(
angleDiffDegrees,
targetAngleDegrees - targetAngleToleranceDegrees,
targetAngleDegrees + targetAngleToleranceDegrees);
if ((distanceFromTargetMM <= minDistanceMM && rotationGood) || runTime() >= timeoutMS) {
if (stopDrivetrain) {
robot.frontLeft.setVelocity(0);
robot.frontRight.setVelocity(0);
@@ -56,12 +86,19 @@ public class MoveToCoordinate extends CyberarmState {
}
private void drivetrain(Vector2D direction) {
// rotationStrength = Utilities.lerp(
// minVelocityMM,
// maxVelocityMM,
// Range.clip(Math.abs(angleDiffDegrees) / lerpDegrees, 0.0, 1.0));
rotationStrength = Range.clip(Math.abs(angleDiffDegrees) / lerpDegrees, 0.0, 1.0);
if (angleDiffDegrees < 0)
rotationStrength *= -1;
// https://gm0.org/en/latest/docs/software/tutorials/mecanum-drive.html#field-centric
double y = direction.x(); // robot forward is in the X axis
double x = direction.y(); // robot side to side is on the Y axis
// FIXME: Dynamically set rotation as needed to achieve target heading using shortest rotation
double rx = 0;//engine.gamepad1.right_stick_x;
double rx = 0; // -rotationStrength; //engine.gamepad1.right_stick_x;
double botHeading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
@@ -80,20 +117,7 @@ public class MoveToCoordinate extends CyberarmState {
double frontRightPower = (rotY - rotX - rx) / denominator;
double backRightPower = (rotY + rotX - rx) / denominator;
double maxVelocity = Utilities.unitToTicks(
RedCrabMinibot.DRIVETRAIN_MOTOR_TICKS_PER_REVOLUTION,
RedCrabMinibot.DRIVETRAIN_GEAR_RATIO,
RedCrabMinibot.DRIVETRAIN_WHEEL_DIAMETER_MM,
DistanceUnit.MM,
RedCrabMinibot.DRIVETRAIN_VELOCITY_MAX_MM);
double slowVelocity = Utilities.unitToTicks(
RedCrabMinibot.DRIVETRAIN_MOTOR_TICKS_PER_REVOLUTION,
RedCrabMinibot.DRIVETRAIN_GEAR_RATIO,
RedCrabMinibot.DRIVETRAIN_WHEEL_DIAMETER_MM,
DistanceUnit.MM,
RedCrabMinibot.DRIVETRAIN_VELOCITY_SLOW_MM);
// FIXME: Lerp up/down when starting move and getting close to target
double velocity = slowVelocity; //robotSlowMode ? slowVelocity : maxVelocity;
velocity = lerpVelocity();
robot.frontLeft.setVelocity(frontLeftPower * velocity);
robot.backLeft.setVelocity(backLeftPower * velocity);
@@ -101,14 +125,40 @@ public class MoveToCoordinate extends CyberarmState {
robot.backRight.setVelocity(backRightPower * velocity);
}
private double lerpVelocity() {
double distanceFromInitialPosMM = robotInitialPosMM.distance(robotPosMM);
double lerpVelocity;
// Ease power up
if (distanceFromInitialPosMM < lerpMM_UP) {
lerpVelocity = Utilities.lerp(
minVelocityMM,
maxVelocityMM,
Range.clip(
distanceFromInitialPosMM / lerpMM_UP, 0.0, 1.0));
// Cruising power
} else if (distanceFromTargetMM > lerpMM_DOWN) {
lerpVelocity = maxVelocityMM;
// Ease power down
} else {
lerpVelocity = Utilities.lerp(
minVelocityMM,
maxVelocityMM,
Range.clip(
distanceFromTargetMM / lerpMM_DOWN, 0.0, 1.0));
}
return lerpVelocity;
}
@Override
public void telemetry() {
Vector2D robotPosMM = new Vector2D(RedCrabMinibot.localizer.xMM(), RedCrabMinibot.localizer.yMM());
Vector2D direction = targetPosMM.minus(robotPosMM).normalize();
engine.telemetry.addData("Current Position MM", "X: %.2f Y: %.2f", robotPosMM.x(), robotPosMM.y());
engine.telemetry.addData("Target Position MM", "X: %.2f Y: %.2f", targetPosMM.x(), targetPosMM.y());
engine.telemetry.addData("Move NORMAL", "X: %.2f Y: %.2f", direction.x(), direction.y());
engine.telemetry.addData("Distance MM", "%.2fmm", robotPosMM.distance(targetPosMM));
engine.telemetry.addData("Move NORMAL", "X: %.2f Y: %.2f R: %.2f", direction.x(), direction.y(), rotationStrength);
engine.telemetry.addData("Distance MM", "%.2fmm", distanceFromTargetMM);
engine.telemetry.addData("Angle Diff DEGREES", "%.2f degrees", angleDiffDegrees);
// engine.telemetry.addData("Velocity", "%.2f T/s", velocity);
// engine.telemetry.addData("LERP DOWN", "%.2f (%.2fmm)", distanceFromTargetMM / lerpMM_DOWN, distanceFromTargetMM);
}
}

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

@@ -130,11 +130,6 @@ public class Pilot extends CyberarmState {
}
}
@Override
public void telemetry() {
robot.standardTelemetry();
}
private void drivetrain() {
// https://gm0.org/en/latest/docs/software/tutorials/mecanum-drive.html#field-centric