mirror of
https://github.com/TimeCrafters/CenterStage
synced 2025-12-12 23:32:36 +00:00
RedCrab: Initial implementation of autonomous MoveToCoordinate, added debug automonous, added a Vector2D class
This commit is contained in:
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;
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
|
||||
@@ -245,6 +248,15 @@ public class RedCrabMinibot {
|
||||
if (autonomous)
|
||||
resetDeadWheels();
|
||||
|
||||
/// LED(s) ///
|
||||
greenLED = engine.hardwareMap.get(DigitalChannel.class, "led_green"); // GREEN LED NOT WORKING
|
||||
redLED = engine.hardwareMap.get(DigitalChannel.class, "led_red");
|
||||
|
||||
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);
|
||||
|
||||
|
||||
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