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: Localizer _seems_ to be working now, more testing needed.

Browse Source
This commit is contained in:
Cyberarm
2024-01-29 21:06:42 -06:00
parent 337652018d
commit 585ded2381
2 changed files with 114 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,6 +1,8 @@
package dev.cyberarm.minibots.red_crab;
import com.arcrobotics.ftclib.geometry.Pose2d;
import com.arcrobotics.ftclib.kinematics.HolonomicOdometry;
import com.arcrobotics.ftclib.kinematics.Odometry;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
@@ -10,79 +12,132 @@ 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 int lastEncoderXLeft = 0, lastEncoderXRight = 0, lastEncoderYCenter = 0;
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 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 HolonomicOdometry odometry;
public Localizer(RedCrabMinibot robot) {
this.robot = robot;
// Preset last encoder to current location to not require resetting encoders, ever. (🤞)
this.lastEncoderXLeft = robot.deadWheelXLeft.getCurrentPosition();
this.lastEncoderXRight = robot.deadWheelXRight.getCurrentPosition();
this.lastEncoderYCenter = robot.deadWheelYCenter.getCurrentPosition();
this.lastEncoderXLeftMM = ticksToMM(robot.deadWheelXLeft.getCurrentPosition());
this.lastEncoderXRightMM = ticksToMM(robot.deadWheelXRight.getCurrentPosition());
this.lastEncoderYCenterMM = ticksToMM(robot.deadWheelYCenter.getCurrentPosition());
this.odometry = new HolonomicOdometry(
this::leftDistance,
this::rightDistance,
this::centerDistance,
trackWidthMM,
forwardOffsetMM
);
}
public void reset() {
robot.resetDeadWheels();
odometry = new HolonomicOdometry(
this::leftDistance,
this::rightDistance,
this::centerDistance,
trackWidthMM,
forwardOffsetMM
);
rawX = 0;
rawY = 0;
rawR = 0;
offsetX = 0;
offsetY = 0;
}
// FIXME
// Meant for setting starting location offset
public void teleport(double xMM, double yMM) {
this.rawX = xMM;
this.rawY = yMM;
this.offsetX = xMM;
this.offsetY = yMM;
}
// FIXME
// FIXME: We need to be able to set rotation to +/- 180 so we can use absolute field coordinates for target location(s)
// and use odometry position as "true" field location.
public void teleport(double xMM, double yMM, double headingDegrees) {
this.rawX = xMM;
this.rawY = yMM;
this.rawR = (AngleUnit.DEGREES).toRadians(AngleUnit.normalizeDegrees(headingDegrees)); // cursed :(
this.offsetX = xMM;
this.offsetY = yMM;
this.rawR = 0; // FIXME HERE // (AngleUnit.DEGREES).toRadians(AngleUnit.normalizeDegrees(headingDegrees)); // cursed :(
}
public void integrate() {
int leftEncoder = robot.deadWheelXLeft.getCurrentPosition();
int rightEncoder = robot.deadWheelXRight.getCurrentPosition();
int centerEncoder = robot.deadWheelYCenter.getCurrentPosition();
odometry.updatePose();
int deltaLeft = leftEncoder - lastEncoderXLeft;
int deltaRight = rightEncoder - lastEncoderXRight;
int deltaCenter = centerEncoder - lastEncoderYCenter;
double phi = (deltaLeft - deltaRight) / trackWidthMM;
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);
rawX += deltaX;
rawY += deltaY;
rawR += phi;
lastEncoderXLeft = leftEncoder;
lastEncoderXRight = rightEncoder;
lastEncoderYCenter = centerEncoder;
// double leftEncoder = ticksToMM(robot.deadWheelXLeft.getCurrentPosition());
// double rightEncoder = ticksToMM(robot.deadWheelXRight.getCurrentPosition());
// double centerEncoder = ticksToMM(robot.deadWheelYCenter.getCurrentPosition());
//
// double deltaLeft = leftEncoder - lastEncoderXLeftMM;
// double deltaRight = rightEncoder - lastEncoderXRightMM;
// double deltaCenter = centerEncoder - lastEncoderYCenterMM;
//
// double phi = (deltaLeft - deltaRight) / trackWidthMM;
// double deltaMiddle = (deltaLeft + deltaRight) / 2.0;
// // double deltaPerp = deltaCenter - forwardOffsetMM * phi;
// double deltaPerp = deltaCenter - (deltaRight - deltaLeft) * forwardOffsetMM / trackWidthMM;
//
// double heading = rawR + (phi / 2.0);
// double deltaX = (deltaMiddle * Math.cos(heading) - deltaPerp * Math.sin(heading)) * xDeltaMultiplier;
// double deltaY = (deltaMiddle * Math.sin(heading) + deltaPerp * Math.cos(heading)) * yDeltaMultiplier;
//
// rawX += deltaX;
// rawY += deltaY;
// rawR += phi;
//
// lastEncoderXLeftMM = leftEncoder;
// lastEncoderXRightMM = rightEncoder;
// lastEncoderYCenterMM = centerEncoder;
}
public double xMM() {
return Utilities.ticksToUnit(8192, 1, wheelDiameterMM, DistanceUnit.MM, (int)rawX);
return odometry.getPose().getX() + offsetX; //rawX;
}
public double yMM() {
return Utilities.ticksToUnit(8192, 1, wheelDiameterMM, DistanceUnit.MM, (int)rawY);
return odometry.getPose().getY() + offsetY; //rawY;
}
// FIXME
public double xIn() {
return xMM() * 0.03937008;
}
public double yIn() {
return yMM() * 0.03937008;
}
// Returns true 360 degrees
public double headingDegrees() {
return rawR;
double degrees = headingRadians() * 180.0 / Math.PI;
return ((degrees + 360.0) % 360.0) % 360.0;
}
// FIXME? (report radians as halves or proper whole?)
// Returns annoying half-split +/- PI radians
public double headingRadians() {
return rawR;
return odometry.getPose().getHeading(); // rawR;
}
private double ticksToMM(int ticks) {
return Utilities.ticksToUnit(encoderTicksPerRevolution, encoderGearRatio, wheelDiameterMM, DistanceUnit.MM, ticks);
}
public double leftDistance() {
return ticksToMM(robot.deadWheelXLeft.getCurrentPosition());
}
public double rightDistance() {
return ticksToMM(robot.deadWheelXRight.getCurrentPosition());
}
public double centerDistance() {
return ticksToMM(robot.deadWheelYCenter.getCurrentPosition());
}
}

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

@@ -242,16 +242,8 @@ public class RedCrabMinibot {
deadWheelXRight = (DcMotorEx) engine.hardwareMap.dcMotor.get("deadwheel_x_right");
deadWheelYCenter = engine.hardwareMap.get(EncoderCustomKB2040.class, "deadwheel_y_center");
deadWheelXLeft.setDirection(DcMotorSimple.Direction.REVERSE);
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);
if (autonomous)
resetDeadWheels();
// Bulk read from hubs
Utilities.hubsBulkReadMode(engine.hardwareMap, LynxModule.BulkCachingMode.MANUAL);
@@ -320,6 +312,19 @@ public class RedCrabMinibot {
RedCrabMinibot.DRONE_LAUNCH_CONFIRMATION_TIME_MS = config.variable("Robot", "DroneLauncher_Tuning", "launch_confirmation_time_ms").value();
}
public void resetDeadWheels() {
deadWheelXLeft.setDirection(DcMotorSimple.Direction.REVERSE);
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);
}
public void standardTelemetry() {
engine.telemetry.addLine();
@@ -327,8 +332,8 @@ public class RedCrabMinibot {
if (RedCrabMinibot.localizer != null) {
engine.telemetry.addLine("Localizer");
engine.telemetry.addData("X (MM)", "%.2fmm", RedCrabMinibot.localizer.xMM());
engine.telemetry.addData("Y (MM)", "%.2fmm", RedCrabMinibot.localizer.yMM());
engine.telemetry.addData("X (MM)", "%.2fmm (%.2f IN)", RedCrabMinibot.localizer.xMM(), RedCrabMinibot.localizer.xIn());
engine.telemetry.addData("Y (MM)", "%.2fmm (%.2f IN)", RedCrabMinibot.localizer.yMM(), RedCrabMinibot.localizer.yIn());
engine.telemetry.addData("R (De)", "%.2fdeg", RedCrabMinibot.localizer.headingDegrees());
engine.telemetry.addLine();
}