mirror of
https://github.com/TimeCrafters/CenterStage
synced 2025-12-12 23:32:36 +00:00
Merge remote-tracking branch 'origin/master'
This commit is contained in:
NerdyBirdy460
108
TeamCode/src/main/java/dev/cyberarm/engine/V2/Vector2D.java
Normal file
108
TeamCode/src/main/java/dev/cyberarm/engine/V2/Vector2D.java
Normal file
@@ -0,0 +1,108 @@
|
||||
package dev.cyberarm.engine.V2;
|
||||
|
||||
public class Vector2D {
|
||||
double x, y;
|
||||
public Vector2D() {
|
||||
this.x = 0;
|
||||
this.y = 0;
|
||||
}
|
||||
|
||||
public Vector2D(double x, double y) {
|
||||
this.x = x;
|
||||
this.y = y;
|
||||
}
|
||||
|
||||
public double x() {
|
||||
return x;
|
||||
}
|
||||
|
||||
public double y() {
|
||||
return y;
|
||||
}
|
||||
|
||||
public Vector2D plus(Vector2D other) {
|
||||
return new Vector2D(
|
||||
this.x + other.x(),
|
||||
this.y + other.y()
|
||||
);
|
||||
}
|
||||
|
||||
public Vector2D plus(double other) {
|
||||
return new Vector2D(
|
||||
this.x + other,
|
||||
this.y + other
|
||||
);
|
||||
}
|
||||
|
||||
public Vector2D minus(Vector2D other) {
|
||||
return new Vector2D(
|
||||
this.x - other.x(),
|
||||
this.y - other.y()
|
||||
);
|
||||
}
|
||||
|
||||
public Vector2D minus(double other) {
|
||||
return new Vector2D(
|
||||
this.x - other,
|
||||
this.y - other
|
||||
);
|
||||
}
|
||||
|
||||
public Vector2D multiply(Vector2D other) {
|
||||
return new Vector2D(
|
||||
this.x * other.x(),
|
||||
this.y * other.y()
|
||||
);
|
||||
}
|
||||
|
||||
public Vector2D multiply(double other) {
|
||||
return new Vector2D(
|
||||
this.x * other,
|
||||
this.y * other
|
||||
);
|
||||
}
|
||||
|
||||
public Vector2D divide(Vector2D other) {
|
||||
return new Vector2D(
|
||||
(this.x == 0 ? 0.0 : this.x / other.x()),
|
||||
(this.y == 0 ? 0.0 : this.y / other.y())
|
||||
);
|
||||
}
|
||||
|
||||
public Vector2D divide(double other) {
|
||||
return new Vector2D(
|
||||
(this.x == 0 ? 0.0 : this.x / other),
|
||||
(this.y == 0 ? 0.0 : this.y / other)
|
||||
);
|
||||
}
|
||||
|
||||
public double magnitude() {
|
||||
return Math.sqrt((this.x * this.x) + (this.y * this.y));
|
||||
}
|
||||
|
||||
public Vector2D normalize() {
|
||||
double mag = magnitude();
|
||||
|
||||
return divide(mag);
|
||||
}
|
||||
|
||||
public double distance(Vector2D other) {
|
||||
return Math.sqrt((this.x - other.x) * (this.x - other.x) + (this.y - other.y) * (this.y - other.y));
|
||||
}
|
||||
|
||||
public Vector2D inverse() {
|
||||
return new Vector2D(1.0 / this.x, 1.0 / this.y);
|
||||
}
|
||||
|
||||
public double dot(Vector2D other) {
|
||||
double product = 0.0;
|
||||
product += this.x * other.x;
|
||||
product += this.y * other.y;
|
||||
|
||||
return product;
|
||||
}
|
||||
|
||||
public double sum() {
|
||||
return this.x + this.y;
|
||||
}
|
||||
}
|
||||
@@ -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());
|
||||
}
|
||||
}
|
||||
|
||||
@@ -6,7 +6,9 @@ import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorSimple;
|
||||
import com.qualcomm.robotcore.hardware.DigitalChannel;
|
||||
import com.qualcomm.robotcore.hardware.IMU;
|
||||
import com.qualcomm.robotcore.hardware.LED;
|
||||
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
|
||||
@@ -85,6 +87,7 @@ public class RedCrabMinibot {
|
||||
public final Servo leftClaw, rightClaw, clawWrist, droneLatch, hookArm;
|
||||
public final DcMotorEx deadWheelXLeft, deadWheelXRight;
|
||||
public final EncoderCustomKB2040 deadWheelYCenter;
|
||||
public final DigitalChannel greenLED, redLED;
|
||||
|
||||
final CyberarmEngine engine;
|
||||
|
||||
@@ -242,16 +245,17 @@ 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);
|
||||
if (autonomous)
|
||||
resetDeadWheels();
|
||||
|
||||
deadWheelXLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
deadWheelXRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
deadWheelYCenter.reset();
|
||||
/// LED(s) ///
|
||||
greenLED = engine.hardwareMap.get(DigitalChannel.class, "led_green"); // GREEN LED NOT WORKING
|
||||
redLED = engine.hardwareMap.get(DigitalChannel.class, "led_red");
|
||||
|
||||
deadWheelXLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||
deadWheelXRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||
greenLED.setMode(DigitalChannel.Mode.OUTPUT);
|
||||
redLED.setMode(DigitalChannel.Mode.OUTPUT);
|
||||
greenLED.setState(true);
|
||||
redLED.setState(true);
|
||||
|
||||
// Bulk read from hubs
|
||||
Utilities.hubsBulkReadMode(engine.hardwareMap, LynxModule.BulkCachingMode.MANUAL);
|
||||
@@ -320,6 +324,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 +344,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();
|
||||
}
|
||||
|
||||
File diff suppressed because one or more lines are too long
@@ -0,0 +1,25 @@
|
||||
package dev.cyberarm.minibots.red_crab.engines;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
|
||||
|
||||
import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
|
||||
import dev.cyberarm.minibots.red_crab.states.ClawArmTask;
|
||||
|
||||
@Autonomous(name = "Cyberarm Red Crab Auto DEBUGGING", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp")
|
||||
public class RedCrabAutonomousDebuggingOnlyEngine extends RedCrabAutonomousEngine {
|
||||
@Override
|
||||
public void setup() {
|
||||
robot = new RedCrabMinibot(true);
|
||||
addTask(new ClawArmTask(robot));
|
||||
|
||||
setupFromConfig(
|
||||
new TimeCraftersConfiguration("cyberarm_RedCrab"),
|
||||
"dev.cyberarm.minibots.red_crab.states",
|
||||
robot,
|
||||
RedCrabMinibot.class,
|
||||
"Autonomous_Z_DEBUG"
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,114 @@
|
||||
package dev.cyberarm.minibots.red_crab.states;
|
||||
|
||||
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;
|
||||
import dev.cyberarm.engine.V2.Vector2D;
|
||||
import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
|
||||
|
||||
public class MoveToCoordinate extends CyberarmState {
|
||||
private final RedCrabMinibot robot;
|
||||
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 Vector2D targetPosMM;
|
||||
|
||||
public MoveToCoordinate(RedCrabMinibot robot, String groupName, String actionName) {
|
||||
this.robot = robot;
|
||||
this.groupName = groupName;
|
||||
this.actionName = actionName;
|
||||
|
||||
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.targetAngleDegrees = robot.config.variable(groupName, actionName, "targetAngleDEGREES").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(this.targetX_MM, this.targetY_MM);
|
||||
}
|
||||
|
||||
@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);
|
||||
|
||||
if (distanceMM <= minDistanceMM || runTime() >= timeoutMS) {
|
||||
if (stopDrivetrain) {
|
||||
robot.frontLeft.setVelocity(0);
|
||||
robot.frontRight.setVelocity(0);
|
||||
robot.backLeft.setVelocity(0);
|
||||
robot.backRight.setVelocity(0);
|
||||
}
|
||||
|
||||
finished();
|
||||
return;
|
||||
}
|
||||
|
||||
drivetrain(direction);
|
||||
}
|
||||
|
||||
private void drivetrain(Vector2D direction) {
|
||||
// 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 botHeading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
|
||||
|
||||
// Rotate the movement direction counter to the bot's rotation
|
||||
double rotX = x * Math.cos(-botHeading) - y * Math.sin(-botHeading);
|
||||
double rotY = x * Math.sin(-botHeading) + y * Math.cos(-botHeading);
|
||||
|
||||
rotX = rotX * 1.1; // Counteract imperfect strafing
|
||||
|
||||
// Denominator is the largest motor power (absolute value) or 1
|
||||
// This ensures all the powers maintain the same ratio,
|
||||
// but only if at least one is out of the range [-1, 1]
|
||||
double denominator = Math.max(Math.abs(rotY) + Math.abs(rotX) + Math.abs(rx), 1);
|
||||
double frontLeftPower = (rotY + rotX + rx) / denominator;
|
||||
double backLeftPower = (rotY - rotX + rx) / denominator;
|
||||
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;
|
||||
|
||||
robot.frontLeft.setVelocity(frontLeftPower * velocity);
|
||||
robot.backLeft.setVelocity(backLeftPower * velocity);
|
||||
robot.frontRight.setVelocity(frontRightPower * velocity);
|
||||
robot.backRight.setVelocity(backRightPower * velocity);
|
||||
}
|
||||
|
||||
@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));
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user