TimeCrafters/FTC_2022
1
0
Fork 0
mirror of https://github.com/TimeCrafters/FTC_2022 synced 2025-12-13 06:22:33 +00:00
Code Issues Packages Projects Releases Wiki Activity

autonomous work

Browse Source
This commit is contained in:
SpencerPiha
2023-02-01 20:36:29 -06:00
parent a826059286
commit c86ecfc6c9
42 changed files with 122 additions and 4209 deletions
Download Patch File Download Diff File Expand all files Collapse all files

83
TeamCode/src/main/java/org/RoadRunner/drive/DriveConstants.java
View File

@@ -1,83 +0,0 @@
package org.RoadRunner.drive;
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
/*
* Constants shared between multiple drive types.
*
* TODO: Tune or adjust the following constants to fit your robot. Note that the non-final
* fields may also be edited through the dashboard (connect to the robot's WiFi network and
* navigate to https://192.168.49.1:8080/dash). Make sure to save the values here after you
* adjust them in the dashboard; **config variable changes don't persist between app restarts**.
*
* These are not the only parameters; some are located in the localizer classes, drive base classes,
* and op modes themselves.
*/
public class DriveConstants {
/*
* These are motor constants that should be listed online for your motors.
*/
public static final double TICKS_PER_REV = 560;
public static final double MAX_RPM = 300;
/*
* Set RUN_USING_ENCODER to true to enable built-in hub velocity control using drive encoders.
* Set this flag to false if drive encoders are not present and an alternative localization
* method is in use (e.g., tracking wheels).
*
* If using the built-in motor velocity PID, update MOTOR_VELO_PID with the tuned coefficients
* from DriveVelocityPIDTuner.
*/
public static final boolean RUN_USING_ENCODER = true;
public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(0, 0, 0,
getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REV));
/*
* These are physical constants that can be determined from your robot (including the track
* width; it will be tune empirically later although a rough estimate is important). Users are
* free to chose whichever linear distance unit they would like so long as it is consistently
* used. The default values were selected with inches in mind. Road runner uses radians for
* angular distances although most angular parameters are wrapped in Math.toRadians() for
* convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO.
*/
public static double WHEEL_RADIUS = 1.47637795; // in
public static double GEAR_RATIO = 1; // output (wheel) speed / input (motor) speed
public static double TRACK_WIDTH = 10; // in
/*
* These are the feedforward parameters used to model the drive motor behavior. If you are using
* the built-in velocity PID, *these values are fine as is*. However, if you do not have drive
* motor encoders or have elected not to use them for velocity control, these values should be
* empirically tuned.
*/
public static double kV = 1.0 / rpmToVelocity(MAX_RPM);
public static double kA = 0;
public static double kStatic = 0;
/*
* These values are used to generate the trajectories for you robot. To ensure proper operation,
* the constraints should never exceed ~80% of the robot's actual capabilities. While Road
* Runner is designed to enable faster autonomous motion, it is a good idea for testing to start
* small and gradually increase them later after everything is working. All distance units are
* inches.
*/
public static double MAX_VEL = 37.10542498;
public static double MAX_ACCEL = 37.10542498;
public static double MAX_ANG_VEL = Math.toRadians(60);
public static double MAX_ANG_ACCEL = Math.toRadians(60);
public static double encoderTicksToInches(double ticks) {
return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
}
public static double rpmToVelocity(double rpm) {
return rpm * GEAR_RATIO * 2 * Math.PI * WHEEL_RADIUS / 60.0;
}
public static double getMotorVelocityF(double ticksPerSecond) {
// see https://docs.google.com/document/d/1tyWrXDfMidwYyP_5H4mZyVgaEswhOC35gvdmP-V-5hA/edit#heading=h.61g9ixenznbx
return 32767 / ticksPerSecond;
}
}

335
TeamCode/src/main/java/org/RoadRunner/drive/SampleMecanumDrive.java
View File

@@ -1,335 +0,0 @@
package org.RoadRunner.drive;
import androidx.annotation.NonNull;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.control.PIDCoefficients;
import com.acmerobotics.roadrunner.drive.DriveSignal;
import com.acmerobotics.roadrunner.drive.MecanumDrive;
import com.acmerobotics.roadrunner.followers.HolonomicPIDVAFollower;
import com.acmerobotics.roadrunner.followers.TrajectoryFollower;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
import com.acmerobotics.roadrunner.trajectory.constraints.AngularVelocityConstraint;
import com.acmerobotics.roadrunner.trajectory.constraints.MecanumVelocityConstraint;
import com.acmerobotics.roadrunner.trajectory.constraints.MinVelocityConstraint;
import com.acmerobotics.roadrunner.trajectory.constraints.ProfileAccelerationConstraint;
import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint;
import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint;
import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
import com.qualcomm.robotcore.hardware.VoltageSensor;
import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import static org.RoadRunner.drive.DriveConstants.MAX_ACCEL;
import static org.RoadRunner.drive.DriveConstants.MAX_ANG_ACCEL;
import static org.RoadRunner.drive.DriveConstants.MAX_ANG_VEL;
import static org.RoadRunner.drive.DriveConstants.MAX_VEL;
import static org.RoadRunner.drive.DriveConstants.MOTOR_VELO_PID;
import static org.RoadRunner.drive.DriveConstants.RUN_USING_ENCODER;
import static org.RoadRunner.drive.DriveConstants.TRACK_WIDTH;
import static org.RoadRunner.drive.DriveConstants.encoderTicksToInches;
import static org.RoadRunner.drive.DriveConstants.kA;
import static org.RoadRunner.drive.DriveConstants.kStatic;
import static org.RoadRunner.drive.DriveConstants.kV;
import org.RoadRunner.trajectorysequence.TrajectorySequence;
import org.RoadRunner.trajectorysequence.TrajectorySequenceBuilder;
import org.RoadRunner.trajectorysequence.TrajectorySequenceRunner;
import org.RoadRunner.util.LynxModuleUtil;
import org.firstinspires.ftc.robotcore.external.Telemetry;
/*
* Simple mecanum drive hardware implementation for REV hardware.
*/
@Config
public class SampleMecanumDrive extends MecanumDrive {
public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(0, 0, 0);
public static PIDCoefficients HEADING_PID = new PIDCoefficients(0, 0, 0);
public static double LATERAL_MULTIPLIER = 1;
public static double VX_WEIGHT = 1;
public static double VY_WEIGHT = 1;
public static double OMEGA_WEIGHT = 1;
public final StandardTrackingWheelLocalizer localizer;
private TrajectorySequenceRunner trajectorySequenceRunner;
private static final TrajectoryVelocityConstraint VEL_CONSTRAINT = getVelocityConstraint(MAX_VEL, MAX_ANG_VEL, TRACK_WIDTH);
private static final TrajectoryAccelerationConstraint ACCEL_CONSTRAINT = getAccelerationConstraint(MAX_ACCEL);
private TrajectoryFollower follower;
private DcMotorEx leftFront, leftRear, rightRear, rightFront;
private List<DcMotorEx> motors;
private BNO055IMU imu;
private VoltageSensor batteryVoltageSensor;
public SampleMecanumDrive(HardwareMap hardwareMap) {
super(kV, kA, kStatic, TRACK_WIDTH, TRACK_WIDTH, LATERAL_MULTIPLIER);
follower = new HolonomicPIDVAFollower(TRANSLATIONAL_PID, TRANSLATIONAL_PID, HEADING_PID,
new Pose2d(0.5, 0.5, Math.toRadians(5.0)), 0.5);
LynxModuleUtil.ensureMinimumFirmwareVersion(hardwareMap);
batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next();
for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
}
// TODO: adjust the names of the following hardware devices to match your configuration
imu = hardwareMap.get(BNO055IMU.class, "imu");
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
parameters.angleUnit = BNO055IMU.AngleUnit.RADIANS;
imu.initialize(parameters);
// TODO: If the hub containing the IMU you are using is mounted so that the "REV" logo does
// not face up, remap the IMU axes so that the z-axis points upward (normal to the floor.)
//
// | +Z axis
// |
// |
// |
// _______|_____________ +Y axis
// / |_____________/|__________
// / REV / EXPANSION //
// / / HUB //
// /_______/_____________//
// |_______/_____________|/
// /
// / +X axis
//
// This diagram is derived from the axes in section 3.4 https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bno055-ds000.pdf
// and the placement of the dot/orientation from https://docs.revrobotics.com/rev-control-system/control-system-overview/dimensions#imu-location
//
// For example, if +Y in this diagram faces downwards, you would use AxisDirection.NEG_Y.
// BNO055IMUUtil.remapZAxis(imu, AxisDirection.NEG_Y);
leftFront = hardwareMap.get(DcMotorEx.class, "Front Left");
leftRear = hardwareMap.get(DcMotorEx.class, "Back Left");
rightRear = hardwareMap.get(DcMotorEx.class, "Back Right");
rightFront = hardwareMap.get(DcMotorEx.class, "Front Right");
leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
leftRear.setDirection(DcMotorSimple.Direction.REVERSE);
rightRear.setDirection(DcMotorSimple.Direction.FORWARD);
rightFront.setDirection(DcMotorSimple.Direction.FORWARD);
motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront);
for (DcMotorEx motor : motors) {
MotorConfigurationType motorConfigurationType = motor.getMotorType().clone();
motorConfigurationType.setAchieveableMaxRPMFraction(1.0);
motor.setMotorType(motorConfigurationType);
}
if (RUN_USING_ENCODER) {
setMode(DcMotor.RunMode.RUN_USING_ENCODER);
}
setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
if (RUN_USING_ENCODER && MOTOR_VELO_PID != null) {
setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID);
}
// TODO: reverse any motors using DcMotor.setDirection()
leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
leftRear.setDirection(DcMotorSimple.Direction.REVERSE);
rightRear.setDirection(DcMotorSimple.Direction.FORWARD);
rightFront.setDirection(DcMotorSimple.Direction.FORWARD);
// TODO: if desired, use setLocalizer() to change the localization method
// for instance, setLocalizer(new ThreeTrackingWheelLocalizer(...));
localizer = new StandardTrackingWheelLocalizer(hardwareMap);
setLocalizer(localizer);
trajectorySequenceRunner = new TrajectorySequenceRunner(follower, HEADING_PID);
}
public TrajectoryBuilder trajectoryBuilder(Pose2d startPose) {
return new TrajectoryBuilder(startPose, VEL_CONSTRAINT, ACCEL_CONSTRAINT);
}
public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, boolean reversed) {
return new TrajectoryBuilder(startPose, reversed, VEL_CONSTRAINT, ACCEL_CONSTRAINT);
}
public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, double startHeading) {
return new TrajectoryBuilder(startPose, startHeading, VEL_CONSTRAINT, ACCEL_CONSTRAINT);
}
public TrajectorySequenceBuilder trajectorySequenceBuilder(Pose2d startPose) {
return new TrajectorySequenceBuilder(
startPose,
VEL_CONSTRAINT, ACCEL_CONSTRAINT,
MAX_ANG_VEL, MAX_ANG_ACCEL
);
}
public void turnAsync(double angle) {
trajectorySequenceRunner.followTrajectorySequenceAsync(
trajectorySequenceBuilder(getPoseEstimate())
.turn(angle)
.build()
);
}
public void turn(double angle) {
turnAsync(angle);
waitForIdle();
}
public void followTrajectoryAsync(Trajectory trajectory) {
trajectorySequenceRunner.followTrajectorySequenceAsync(
trajectorySequenceBuilder(trajectory.start())
.addTrajectory(trajectory)
.build()
);
}
public void followTrajectory(Trajectory trajectory) {
followTrajectoryAsync(trajectory);
waitForIdle();
}
public void followTrajectorySequenceAsync(TrajectorySequence trajectorySequence) {
trajectorySequenceRunner.followTrajectorySequenceAsync(trajectorySequence);
}
public void followTrajectorySequence(TrajectorySequence trajectorySequence) {
followTrajectorySequenceAsync(trajectorySequence);
waitForIdle();
}
public Pose2d getLastError() {
return trajectorySequenceRunner.getLastPoseError();
}
public void update() {
updatePoseEstimate();
DriveSignal signal = trajectorySequenceRunner.update(getPoseEstimate(), getPoseVelocity());
if (signal != null) setDriveSignal(signal);
}
public void waitForIdle() {
while (!Thread.currentThread().isInterrupted() && isBusy())
update();
}
public boolean isBusy() {
return trajectorySequenceRunner.isBusy();
}
public void setMode(DcMotor.RunMode runMode) {
for (DcMotorEx motor : motors) {
motor.setMode(runMode);
}
}
public void setZeroPowerBehavior(DcMotor.ZeroPowerBehavior zeroPowerBehavior) {
for (DcMotorEx motor : motors) {
motor.setZeroPowerBehavior(zeroPowerBehavior);
}
}
public void setPIDFCoefficients(DcMotor.RunMode runMode, PIDFCoefficients coefficients) {
PIDFCoefficients compensatedCoefficients = new PIDFCoefficients(
coefficients.p, coefficients.i, coefficients.d,
coefficients.f * 12 / batteryVoltageSensor.getVoltage()
);
for (DcMotorEx motor : motors) {
motor.setPIDFCoefficients(runMode, compensatedCoefficients);
}
}
public void setWeightedDrivePower(Pose2d drivePower) {
Pose2d vel = drivePower;
if (Math.abs(drivePower.getX()) + Math.abs(drivePower.getY())
+ Math.abs(drivePower.getHeading()) > 1) {
// re-normalize the powers according to the weights
double denom = VX_WEIGHT * Math.abs(drivePower.getX())
+ VY_WEIGHT * Math.abs(drivePower.getY())
+ OMEGA_WEIGHT * Math.abs(drivePower.getHeading());
vel = new Pose2d(
VX_WEIGHT * drivePower.getX(),
VY_WEIGHT * drivePower.getY(),
OMEGA_WEIGHT * drivePower.getHeading()
).div(denom);
}
setDrivePower(vel);
}
@NonNull
@Override
public List<Double> getWheelPositions() {
List<Double> wheelPositions = new ArrayList<>();
for (DcMotorEx motor : motors) {
wheelPositions.add(encoderTicksToInches(motor.getCurrentPosition()));
}
return wheelPositions;
}
@Override
public List<Double> getWheelVelocities() {
List<Double> wheelVelocities = new ArrayList<>();
for (DcMotorEx motor : motors) {
wheelVelocities.add(encoderTicksToInches(motor.getVelocity()));
}
return wheelVelocities;
}
@Override
public void setMotorPowers(double v, double v1, double v2, double v3) {
leftFront.setPower(v);
leftRear.setPower(v1);
rightRear.setPower(v2);
rightFront.setPower(v3);
}
@Override
public double getRawExternalHeading() {
return imu.getAngularOrientation().firstAngle;
}
public void telemetry(Telemetry telemetry) {
telemetry.addData("front left", leftFront.getPower());
telemetry.addData("back left", leftRear.getPower());
telemetry.addData("front right", rightFront.getPower());
telemetry.addData("back right", rightRear.getPower());
}
@Override
public Double getExternalHeadingVelocity() {
return (double) imu.getAngularVelocity().zRotationRate;
}
public static TrajectoryVelocityConstraint getVelocityConstraint(double maxVel, double maxAngularVel, double trackWidth) {
return new MinVelocityConstraint(Arrays.asList(
new AngularVelocityConstraint(maxAngularVel),
new MecanumVelocityConstraint(maxVel, trackWidth)
));
}
public static TrajectoryAccelerationConstraint getAccelerationConstraint(double maxAccel) {
return new ProfileAccelerationConstraint(maxAccel);
}
}

319
TeamCode/src/main/java/org/RoadRunner/drive/SampleTankDrive.java
View File

@@ -1,319 +0,0 @@
package org.RoadRunner.drive;
import androidx.annotation.NonNull;
import com.acmerobotics.roadrunner.control.PIDCoefficients;
import com.acmerobotics.roadrunner.drive.DriveSignal;
import com.acmerobotics.roadrunner.drive.TankDrive;
import com.acmerobotics.roadrunner.followers.TankPIDVAFollower;
import com.acmerobotics.roadrunner.followers.TrajectoryFollower;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
import com.acmerobotics.roadrunner.trajectory.constraints.AngularVelocityConstraint;
import com.acmerobotics.roadrunner.trajectory.constraints.MinVelocityConstraint;
import com.acmerobotics.roadrunner.trajectory.constraints.ProfileAccelerationConstraint;
import com.acmerobotics.roadrunner.trajectory.constraints.TankVelocityConstraint;
import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint;
import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint;
import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
import com.qualcomm.robotcore.hardware.VoltageSensor;
import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
import org.RoadRunner.trajectorysequence.TrajectorySequence;
import org.RoadRunner.trajectorysequence.TrajectorySequenceBuilder;
import org.RoadRunner.trajectorysequence.TrajectorySequenceRunner;
import org.RoadRunner.util.LynxModuleUtil;
import java.util.Arrays;
import java.util.List;
import static org.RoadRunner.drive.DriveConstants.MAX_ACCEL;
import static org.RoadRunner.drive.DriveConstants.MAX_ANG_ACCEL;
import static org.RoadRunner.drive.DriveConstants.MAX_ANG_VEL;
import static org.RoadRunner.drive.DriveConstants.MAX_VEL;
import static org.RoadRunner.drive.DriveConstants.MOTOR_VELO_PID;
import static org.RoadRunner.drive.DriveConstants.RUN_USING_ENCODER;
import static org.RoadRunner.drive.DriveConstants.TRACK_WIDTH;
import static org.RoadRunner.drive.DriveConstants.encoderTicksToInches;
import static org.RoadRunner.drive.DriveConstants.kA;
import static org.RoadRunner.drive.DriveConstants.kStatic;
import static org.RoadRunner.drive.DriveConstants.kV;
/*
* Simple tank drive hardware implementation for REV hardware.
*/
public class SampleTankDrive extends TankDrive {
public static PIDCoefficients AXIAL_PID = new PIDCoefficients(0, 0, 0);
public static PIDCoefficients CROSS_TRACK_PID = new PIDCoefficients(0, 0, 0);
public static PIDCoefficients HEADING_PID = new PIDCoefficients(0, 0, 0);
public static double VX_WEIGHT = 1;
public static double OMEGA_WEIGHT = 1;
private TrajectorySequenceRunner trajectorySequenceRunner;
private static final TrajectoryVelocityConstraint VEL_CONSTRAINT = getVelocityConstraint(MAX_VEL, MAX_ANG_VEL, TRACK_WIDTH);
private static final TrajectoryAccelerationConstraint accelConstraint = getAccelerationConstraint(MAX_ACCEL);
private TrajectoryFollower follower;
private List<DcMotorEx> motors, leftMotors, rightMotors;
private BNO055IMU imu;
private VoltageSensor batteryVoltageSensor;
public SampleTankDrive(HardwareMap hardwareMap) {
super(kV, kA, kStatic, TRACK_WIDTH);
follower = new TankPIDVAFollower(AXIAL_PID, CROSS_TRACK_PID,
new Pose2d(0.5, 0.5, Math.toRadians(5.0)), 0.5);
LynxModuleUtil.ensureMinimumFirmwareVersion(hardwareMap);
batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next();
for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
}
// TODO: adjust the names of the following hardware devices to match your configuration
imu = hardwareMap.get(BNO055IMU.class, "imu");
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
parameters.angleUnit = BNO055IMU.AngleUnit.RADIANS;
imu.initialize(parameters);
// TODO: If the hub containing the IMU you are using is mounted so that the "REV" logo does
// not face up, remap the IMU axes so that the z-axis points upward (normal to the floor.)
//
// | +Z axis
// |
// |
// |
// _______|_____________ +Y axis
// / |_____________/|__________
// / REV / EXPANSION //
// / / HUB //
// /_______/_____________//
// |_______/_____________|/
// /
// / +X axis
//
// This diagram is derived from the axes in section 3.4 https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bno055-ds000.pdf
// and the placement of the dot/orientation from https://docs.revrobotics.com/rev-control-system/control-system-overview/dimensions#imu-location
//
// For example, if +Y in this diagram faces downwards, you would use AxisDirection.NEG_Y.
// BNO055IMUUtil.remapZAxis(imu, AxisDirection.NEG_Y);
// add/remove motors depending on your robot (e.g., 6WD)
DcMotorEx leftFront = hardwareMap.get(DcMotorEx.class, "leftFront");
DcMotorEx leftRear = hardwareMap.get(DcMotorEx.class, "leftRear");
DcMotorEx rightRear = hardwareMap.get(DcMotorEx.class, "rightRear");
DcMotorEx rightFront = hardwareMap.get(DcMotorEx.class, "rightFront");
motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront);
leftMotors = Arrays.asList(leftFront, leftRear);
rightMotors = Arrays.asList(rightFront, rightRear);
for (DcMotorEx motor : motors) {
MotorConfigurationType motorConfigurationType = motor.getMotorType().clone();
motorConfigurationType.setAchieveableMaxRPMFraction(1.0);
motor.setMotorType(motorConfigurationType);
}
if (RUN_USING_ENCODER) {
setMode(DcMotor.RunMode.RUN_USING_ENCODER);
}
setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
if (RUN_USING_ENCODER && MOTOR_VELO_PID != null) {
setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID);
}
// TODO: reverse any motors using DcMotor.setDirection()
// TODO: if desired, use setLocalizer() to change the localization method
// for instance, setLocalizer(new ThreeTrackingWheelLocalizer(...));
trajectorySequenceRunner = new TrajectorySequenceRunner(follower, HEADING_PID);
}
public TrajectoryBuilder trajectoryBuilder(Pose2d startPose) {
return new TrajectoryBuilder(startPose, VEL_CONSTRAINT, accelConstraint);
}
public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, boolean reversed) {
return new TrajectoryBuilder(startPose, reversed, VEL_CONSTRAINT, accelConstraint);
}
public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, double startHeading) {
return new TrajectoryBuilder(startPose, startHeading, VEL_CONSTRAINT, accelConstraint);
}
public TrajectorySequenceBuilder trajectorySequenceBuilder(Pose2d startPose) {
return new TrajectorySequenceBuilder(
startPose,
VEL_CONSTRAINT, accelConstraint,
MAX_ANG_VEL, MAX_ANG_ACCEL
);
}
public void turnAsync(double angle) {
trajectorySequenceRunner.followTrajectorySequenceAsync(
trajectorySequenceBuilder(getPoseEstimate())
.turn(angle)
.build()
);
}
public void turn(double angle) {
turnAsync(angle);
waitForIdle();
}
public void followTrajectoryAsync(Trajectory trajectory) {
trajectorySequenceRunner.followTrajectorySequenceAsync(
trajectorySequenceBuilder(trajectory.start())
.addTrajectory(trajectory)
.build()
);
}
public void followTrajectory(Trajectory trajectory) {
followTrajectoryAsync(trajectory);
waitForIdle();
}
public void followTrajectorySequenceAsync(TrajectorySequence trajectorySequence) {
trajectorySequenceRunner.followTrajectorySequenceAsync(trajectorySequence);
}
public void followTrajectorySequence(TrajectorySequence trajectorySequence) {
followTrajectorySequenceAsync(trajectorySequence);
waitForIdle();
}
public Pose2d getLastError() {
return trajectorySequenceRunner.getLastPoseError();
}
public void update() {
updatePoseEstimate();
DriveSignal signal = trajectorySequenceRunner.update(getPoseEstimate(), getPoseVelocity());
if (signal != null) setDriveSignal(signal);
}
public void waitForIdle() {
while (!Thread.currentThread().isInterrupted() && isBusy())
update();
}
public boolean isBusy() {
return trajectorySequenceRunner.isBusy();
}
public void setMode(DcMotor.RunMode runMode) {
for (DcMotorEx motor : motors) {
motor.setMode(runMode);
}
}
public void setZeroPowerBehavior(DcMotor.ZeroPowerBehavior zeroPowerBehavior) {
for (DcMotorEx motor : motors) {
motor.setZeroPowerBehavior(zeroPowerBehavior);
}
}
public void setPIDFCoefficients(DcMotor.RunMode runMode, PIDFCoefficients coefficients) {
PIDFCoefficients compensatedCoefficients = new PIDFCoefficients(
coefficients.p, coefficients.i, coefficients.d,
coefficients.f * 12 / batteryVoltageSensor.getVoltage()
);
for (DcMotorEx motor : motors) {
motor.setPIDFCoefficients(runMode, compensatedCoefficients);
}
}
public void setWeightedDrivePower(Pose2d drivePower) {
Pose2d vel = drivePower;
if (Math.abs(drivePower.getX()) + Math.abs(drivePower.getHeading()) > 1) {
// re-normalize the powers according to the weights
double denom = VX_WEIGHT * Math.abs(drivePower.getX())
+ OMEGA_WEIGHT * Math.abs(drivePower.getHeading());
vel = new Pose2d(
VX_WEIGHT * drivePower.getX(),
0,
OMEGA_WEIGHT * drivePower.getHeading()
).div(denom);
} else {
// Ensure the y axis is zeroed out.
vel = new Pose2d(drivePower.getX(), 0, drivePower.getHeading());
}
setDrivePower(vel);
}
@NonNull
@Override
public List<Double> getWheelPositions() {
double leftSum = 0, rightSum = 0;
for (DcMotorEx leftMotor : leftMotors) {
leftSum += encoderTicksToInches(leftMotor.getCurrentPosition());
}
for (DcMotorEx rightMotor : rightMotors) {
rightSum += encoderTicksToInches(rightMotor.getCurrentPosition());
}
return Arrays.asList(leftSum / leftMotors.size(), rightSum / rightMotors.size());
}
public List<Double> getWheelVelocities() {
double leftSum = 0, rightSum = 0;
for (DcMotorEx leftMotor : leftMotors) {
leftSum += encoderTicksToInches(leftMotor.getVelocity());
}
for (DcMotorEx rightMotor : rightMotors) {
rightSum += encoderTicksToInches(rightMotor.getVelocity());
}
return Arrays.asList(leftSum / leftMotors.size(), rightSum / rightMotors.size());
}
@Override
public void setMotorPowers(double v, double v1) {
for (DcMotorEx leftMotor : leftMotors) {
leftMotor.setPower(v);
}
for (DcMotorEx rightMotor : rightMotors) {
rightMotor.setPower(v1);
}
}
@Override
public double getRawExternalHeading() {
return imu.getAngularOrientation().firstAngle;
}
@Override
public Double getExternalHeadingVelocity() {
return (double) imu.getAngularVelocity().zRotationRate;
}
public static TrajectoryVelocityConstraint getVelocityConstraint(double maxVel, double maxAngularVel, double trackWidth) {
return new MinVelocityConstraint(Arrays.asList(
new AngularVelocityConstraint(maxAngularVel),
new TankVelocityConstraint(maxVel, trackWidth)
));
}
public static TrajectoryAccelerationConstraint getAccelerationConstraint(double maxAccel) {
return new ProfileAccelerationConstraint(maxAccel);
}
}

101
TeamCode/src/main/java/org/RoadRunner/drive/StandardTrackingWheelLocalizer.java
View File

@@ -1,101 +0,0 @@
package org.RoadRunner.drive;
import androidx.annotation.NonNull;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.RoadRunner.util.Encoder;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import java.util.Arrays;
import java.util.List;
/*
* Sample tracking wheel localizer implementation assuming the standard configuration:
*
* /--------------\
* | ____ |
* | ---- |
* | || || |
* | || || |
* | |
* | |
* \--------------/
*
*/
@Config
public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer {
public static double TICKS_PER_REV = 8192;
public static double WHEEL_RADIUS = 1.47637795; // in
public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed
public static double LATERAL_DISTANCE = 9.2913393; // in; distance between the left and right wheels
public static double FORWARD_OFFSET = 3.54833; // in; offset of the lateral wheel
public static double X_MULTIPLIER = 0.8039; // Multiplier in the X direction
// 9.14228723
// 8.65411042
public static double Y_MULTIPLIER = 0.8039; // Multiplier in the Y direction
private Encoder leftEncoder, rightEncoder, frontEncoder;
public StandardTrackingWheelLocalizer(HardwareMap hardwareMap) {
super(Arrays.asList(
new Pose2d(0, LATERAL_DISTANCE / 2, 0), // left
new Pose2d(0, -LATERAL_DISTANCE / 2, 0), // right
new Pose2d(FORWARD_OFFSET, 0, Math.toRadians(90)) // front
));
leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "odometerEncoderL"));
rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "odometerEncoderR"));
frontEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "odometerEncoderH"));
// TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE)
rightEncoder.setDirection(Encoder.Direction.REVERSE);
leftEncoder.setDirection(Encoder.Direction.REVERSE);
frontEncoder.setDirection(Encoder.Direction.REVERSE);
}
public static double encoderTicksToInches(double ticks) {
return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
}
@NonNull
@Override
public List<Double> getWheelPositions() {
return Arrays.asList(
encoderTicksToInches(leftEncoder.getCurrentPosition()) * X_MULTIPLIER,
encoderTicksToInches(rightEncoder.getCurrentPosition()) * X_MULTIPLIER,
encoderTicksToInches(frontEncoder.getCurrentPosition()) * Y_MULTIPLIER
);
}
@NonNull
@Override
public List<Double> getWheelVelocities() {
// TODO: If your encoder velocity can exceed 32767 counts / second (such as the REV Through Bore and other
// competing magnetic encoders), change Encoder.getRawVelocity() to Encoder.getCorrectedVelocity() to enable a
// compensation method
return Arrays.asList(
encoderTicksToInches(leftEncoder.getCorrectedVelocity()) * X_MULTIPLIER,
encoderTicksToInches(rightEncoder.getCorrectedVelocity()) * X_MULTIPLIER,
encoderTicksToInches(frontEncoder.getCorrectedVelocity()) * Y_MULTIPLIER
);
}
public void telemetry(Telemetry telemetry){
telemetry.addData("right", rightEncoder.getCurrentPosition());
telemetry.addData("left", leftEncoder.getCurrentPosition());
telemetry.addData("front", frontEncoder.getCurrentPosition());
}
}

221
TeamCode/src/main/java/org/RoadRunner/drive/opmode/AutomaticFeedforwardTuner.java
View File

@@ -1,221 +0,0 @@
package org.RoadRunner.drive.opmode;
import static org.RoadRunner.drive.DriveConstants.MAX_RPM;
import static org.RoadRunner.drive.DriveConstants.RUN_USING_ENCODER;
import static org.RoadRunner.drive.DriveConstants.rpmToVelocity;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.util.NanoClock;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.util.RobotLog;
import org.RoadRunner.drive.SampleMecanumDrive;
import org.RoadRunner.util.LoggingUtil;
import org.RoadRunner.util.RegressionUtil;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.robotcore.internal.system.Misc;
import java.util.ArrayList;
import java.util.List;
/*
* Op mode for computing kV, kStatic, and kA from various drive routines. For the curious, here's an
* outline of the procedure:
* 1. Slowly ramp the motor power and record encoder values along the way.
* 2. Run a linear regression on the encoder velocity vs. motor power plot to obtain a slope (kV)
* and an optional intercept (kStatic).
* 3. Accelerate the robot (apply constant power) and record the encoder counts.
* 4. Adjust the encoder data based on the velocity tuning data and find kA with another linear
* regression.
*/
@Config
@Autonomous(group = "drive")
public class AutomaticFeedforwardTuner extends LinearOpMode {
public static double MAX_POWER = 0.7;
public static double DISTANCE = 100; // in
@Override
public void runOpMode() throws InterruptedException {
if (RUN_USING_ENCODER) {
RobotLog.setGlobalErrorMsg("Feedforward constants usually don't need to be tuned " +
"when using the built-in drive motor velocity PID.");
}
Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
NanoClock clock = NanoClock.system();
telemetry.addLine("Press play to begin the feedforward tuning routine");
telemetry.update();
waitForStart();
if (isStopRequested()) return;
telemetry.clearAll();
telemetry.addLine("Would you like to fit kStatic?");
telemetry.addLine("Press (Y/Δ) for yes, (B/O) for no");
telemetry.update();
boolean fitIntercept = false;
while (!isStopRequested()) {
if (gamepad1.y) {
fitIntercept = true;
while (!isStopRequested() && gamepad1.y) {
idle();
}
break;
} else if (gamepad1.b) {
while (!isStopRequested() && gamepad1.b) {
idle();
}
break;
}
idle();
}
telemetry.clearAll();
telemetry.addLine(Misc.formatInvariant(
"Place your robot on the field with at least %.2f in of room in front", DISTANCE));
telemetry.addLine("Press (Y/Δ) to begin");
telemetry.update();
while (!isStopRequested() && !gamepad1.y) {
idle();
}
while (!isStopRequested() && gamepad1.y) {
idle();
}
telemetry.clearAll();
telemetry.addLine("Running...");
telemetry.update();
double maxVel = rpmToVelocity(MAX_RPM);
double finalVel = MAX_POWER * maxVel;
double accel = (finalVel * finalVel) / (2.0 * DISTANCE);
double rampTime = Math.sqrt(2.0 * DISTANCE / accel);
List<Double> timeSamples = new ArrayList<>();
List<Double> positionSamples = new ArrayList<>();
List<Double> powerSamples = new ArrayList<>();
drive.setPoseEstimate(new Pose2d());
double startTime = clock.seconds();
while (!isStopRequested()) {
double elapsedTime = clock.seconds() - startTime;
if (elapsedTime > rampTime) {
break;
}
double vel = accel * elapsedTime;
double power = vel / maxVel;
timeSamples.add(elapsedTime);
positionSamples.add(drive.getPoseEstimate().getX());
powerSamples.add(power);
drive.setDrivePower(new Pose2d(power, 0.0, 0.0));
drive.updatePoseEstimate();
}
drive.setDrivePower(new Pose2d(0.0, 0.0, 0.0));
RegressionUtil.RampResult rampResult = RegressionUtil.fitRampData(
timeSamples, positionSamples, powerSamples, fitIntercept,
LoggingUtil.getLogFile(Misc.formatInvariant(
"DriveRampRegression-%d.csv", System.currentTimeMillis())));
telemetry.clearAll();
telemetry.addLine("Quasi-static ramp up test complete");
if (fitIntercept) {
telemetry.addLine(Misc.formatInvariant("kV = %.5f, kStatic = %.5f (R^2 = %.2f)",
rampResult.kV, rampResult.kStatic, rampResult.rSquare));
} else {
telemetry.addLine(Misc.formatInvariant("kV = %.5f (R^2 = %.2f)",
rampResult.kStatic, rampResult.rSquare));
}
telemetry.addLine("Would you like to fit kA?");
telemetry.addLine("Press (Y/Δ) for yes, (B/O) for no");
telemetry.update();
boolean fitAccelFF = false;
while (!isStopRequested()) {
if (gamepad1.y) {
fitAccelFF = true;
while (!isStopRequested() && gamepad1.y) {
idle();
}
break;
} else if (gamepad1.b) {
while (!isStopRequested() && gamepad1.b) {
idle();
}
break;
}
idle();
}
if (fitAccelFF) {
telemetry.clearAll();
telemetry.addLine("Place the robot back in its starting position");
telemetry.addLine("Press (Y/Δ) to continue");
telemetry.update();
while (!isStopRequested() && !gamepad1.y) {
idle();
}
while (!isStopRequested() && gamepad1.y) {
idle();
}
telemetry.clearAll();
telemetry.addLine("Running...");
telemetry.update();
double maxPowerTime = DISTANCE / maxVel;
timeSamples.clear();
positionSamples.clear();
powerSamples.clear();
drive.setPoseEstimate(new Pose2d());
drive.setDrivePower(new Pose2d(MAX_POWER, 0.0, 0.0));
startTime = clock.seconds();
while (!isStopRequested()) {
double elapsedTime = clock.seconds() - startTime;
if (elapsedTime > maxPowerTime) {
break;
}
timeSamples.add(elapsedTime);
positionSamples.add(drive.getPoseEstimate().getX());
powerSamples.add(MAX_POWER);
drive.updatePoseEstimate();
}
drive.setDrivePower(new Pose2d(0.0, 0.0, 0.0));
RegressionUtil.AccelResult accelResult = RegressionUtil.fitAccelData(
timeSamples, positionSamples, powerSamples, rampResult,
LoggingUtil.getLogFile(Misc.formatInvariant(
"DriveAccelRegression-%d.csv", System.currentTimeMillis())));
telemetry.clearAll();
telemetry.addLine("Constant power test complete");
telemetry.addLine(Misc.formatInvariant("kA = %.5f (R^2 = %.2f)",
accelResult.kA, accelResult.rSquare));
telemetry.update();
}
while (!isStopRequested()) {
idle();
}
}
}

52
TeamCode/src/main/java/org/RoadRunner/drive/opmode/BackAndForth.java
View File

@@ -1,52 +0,0 @@
package org.RoadRunner.drive.opmode;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.RoadRunner.drive.SampleMecanumDrive;
/*
* Op mode for preliminary tuning of the follower PID coefficients (located in the drive base
* classes). The robot drives back and forth in a straight line indefinitely. Utilization of the
* dashboard is recommended for this tuning routine. To access the dashboard, connect your computer
* to the RC's WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're
* using the RC phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once
* you've successfully connected, start the program, and your robot will begin moving forward and
* backward. You should observe the target position (green) and your pose estimate (blue) and adjust
* your follower PID coefficients such that you follow the target position as accurately as possible.
* If you are using SampleMecanumDrive, you should be tuning TRANSLATIONAL_PID and HEADING_PID.
* If you are using SampleTankDrive, you should be tuning AXIAL_PID, CROSS_TRACK_PID, and HEADING_PID.
* These coefficients can be tuned live in dashboard.
*
* This opmode is designed as a convenient, coarse tuning for the follower PID coefficients. It
* is recommended that you use the FollowerPIDTuner opmode for further fine tuning.
*/
@Config
@Autonomous(group = "drive")
public class BackAndForth extends LinearOpMode {
public static double DISTANCE = 50;
@Override
public void runOpMode() throws InterruptedException {
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
Trajectory trajectoryForward = drive.trajectoryBuilder(new Pose2d())
.forward(DISTANCE)
.build();
Trajectory trajectoryBackward = drive.trajectoryBuilder(trajectoryForward.end())
.back(DISTANCE)
.build();
waitForStart();
while (opModeIsActive() && !isStopRequested()) {
drive.followTrajectory(trajectoryForward);
drive.followTrajectory(trajectoryBackward);
}
}
}

171
TeamCode/src/main/java/org/RoadRunner/drive/opmode/DriveVelocityPIDTuner.java
View File

@@ -1,171 +0,0 @@
package org.RoadRunner.drive.opmode;
import static org.RoadRunner.drive.DriveConstants.MAX_ACCEL;
import static org.RoadRunner.drive.DriveConstants.MAX_VEL;
import static org.RoadRunner.drive.DriveConstants.MOTOR_VELO_PID;
import static org.RoadRunner.drive.DriveConstants.RUN_USING_ENCODER;
import static org.RoadRunner.drive.DriveConstants.kV;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.profile.MotionProfile;
import com.acmerobotics.roadrunner.profile.MotionProfileGenerator;
import com.acmerobotics.roadrunner.profile.MotionState;
import com.acmerobotics.roadrunner.util.NanoClock;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.util.RobotLog;
import org.RoadRunner.drive.SampleMecanumDrive;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import java.util.List;
/*
* This routine is designed to tune the PID coefficients used by the REV Expansion Hubs for closed-
* loop velocity control. Although it may seem unnecessary, tuning these coefficients is just as
* important as the positional parameters. Like the other manual tuning routines, this op mode
* relies heavily upon the dashboard. To access the dashboard, connect your computer to the RC's
* WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're using the RC
* phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once you've successfully
* connected, start the program, and your robot will begin moving forward and backward according to
* a motion profile. Your job is to graph the velocity errors over time and adjust the PID
* coefficients (note: the tuning variable will not appear until the op mode finishes initializing).
* Once you've found a satisfactory set of gains, add them to the DriveConstants.java file under the
* MOTOR_VELO_PID field.
*
* Recommended tuning process:
*
* 1. Increase kP until any phase lag is eliminated. Concurrently increase kD as necessary to
* mitigate oscillations.
* 2. Add kI (or adjust kF) until the steady state/constant velocity plateaus are reached.
* 3. Back off kP and kD a little until the response is less oscillatory (but without lag).
*
* Pressing Y/Δ (Xbox/PS4) will pause the tuning process and enter driver override, allowing the
* user to reset the position of the bot in the event that it drifts off the path.
* Pressing B/O (Xbox/PS4) will cede control back to the tuning process.
*/
@Config
@Autonomous(group = "drive")
public class DriveVelocityPIDTuner extends LinearOpMode {
public static double DISTANCE = 72; // in
enum Mode {
DRIVER_MODE,
TUNING_MODE
}
private static MotionProfile generateProfile(boolean movingForward) {
MotionState start = new MotionState(movingForward ? 0 : DISTANCE, 0, 0, 0);
MotionState goal = new MotionState(movingForward ? DISTANCE : 0, 0, 0, 0);
return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, MAX_VEL, MAX_ACCEL);
}
@Override
public void runOpMode() {
if (!RUN_USING_ENCODER) {
RobotLog.setGlobalErrorMsg("%s does not need to be run if the built-in motor velocity" +
"PID is not in use", getClass().getSimpleName());
}
Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
Mode mode = Mode.TUNING_MODE;
double lastKp = MOTOR_VELO_PID.p;
double lastKi = MOTOR_VELO_PID.i;
double lastKd = MOTOR_VELO_PID.d;
double lastKf = MOTOR_VELO_PID.f;
drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID);
NanoClock clock = NanoClock.system();
telemetry.addLine("Ready!");
telemetry.update();
telemetry.clearAll();
waitForStart();
if (isStopRequested()) return;
boolean movingForwards = true;
MotionProfile activeProfile = generateProfile(true);
double profileStart = clock.seconds();
while (!isStopRequested()) {
telemetry.addData("mode", mode);
switch (mode) {
case TUNING_MODE:
if (gamepad1.y) {
mode = Mode.DRIVER_MODE;
drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
}
// calculate and set the motor power
double profileTime = clock.seconds() - profileStart;
if (profileTime > activeProfile.duration()) {
// generate a new profile
movingForwards = !movingForwards;
activeProfile = generateProfile(movingForwards);
profileStart = clock.seconds();
}
MotionState motionState = activeProfile.get(profileTime);
double targetPower = kV * motionState.getV();
drive.setDrivePower(new Pose2d(targetPower, 0, 0));
List<Double> velocities = drive.getWheelVelocities();
// update telemetry
telemetry.addData("targetVelocity", motionState.getV());
for (int i = 0; i < velocities.size(); i++) {
telemetry.addData("measuredVelocity" + i, velocities.get(i));
telemetry.addData(
"error" + i,
motionState.getV() - velocities.get(i)
);
}
break;
case DRIVER_MODE:
if (gamepad1.b) {
drive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
mode = Mode.TUNING_MODE;
movingForwards = true;
activeProfile = generateProfile(movingForwards);
profileStart = clock.seconds();
}
drive.setWeightedDrivePower(
new Pose2d(
-gamepad1.left_stick_y,
-gamepad1.left_stick_x,
-gamepad1.right_stick_x
)
);
break;
}
if (lastKp != MOTOR_VELO_PID.p || lastKd != MOTOR_VELO_PID.d
|| lastKi != MOTOR_VELO_PID.i || lastKf != MOTOR_VELO_PID.f) {
drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID);
lastKp = MOTOR_VELO_PID.p;
lastKi = MOTOR_VELO_PID.i;
lastKd = MOTOR_VELO_PID.d;
lastKf = MOTOR_VELO_PID.f;
}
telemetry.update();
}
}
}

55
TeamCode/src/main/java/org/RoadRunner/drive/opmode/FollowerPIDTuner.java
View File

@@ -1,55 +0,0 @@
package org.RoadRunner.drive.opmode;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.RoadRunner.drive.SampleMecanumDrive;
import org.RoadRunner.trajectorysequence.TrajectorySequence;
/*
* Op mode for preliminary tuning of the follower PID coefficients (located in the drive base
* classes). The robot drives in a DISTANCE-by-DISTANCE square indefinitely. Utilization of the
* dashboard is recommended for this tuning routine. To access the dashboard, connect your computer
* to the RC's WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're
* using the RC phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once
* you've successfully connected, start the program, and your robot will begin driving in a square.
* You should observe the target position (green) and your pose estimate (blue) and adjust your
* follower PID coefficients such that you follow the target position as accurately as possible.
* If you are using SampleMecanumDrive, you should be tuning TRANSLATIONAL_PID and HEADING_PID.
* If you are using SampleTankDrive, you should be tuning AXIAL_PID, CROSS_TRACK_PID, and HEADING_PID.
* These coefficients can be tuned live in dashboard.
*/
@Config
@Autonomous(group = "drive")
public class FollowerPIDTuner extends LinearOpMode {
public static double DISTANCE = 48; // in
@Override
public void runOpMode() throws InterruptedException {
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
Pose2d startPose = new Pose2d(-DISTANCE / 2, -DISTANCE / 2, 0);
drive.setPoseEstimate(startPose);
waitForStart();
if (isStopRequested()) return;
while (!isStopRequested()) {
TrajectorySequence trajSeq = drive.trajectorySequenceBuilder(startPose)
.forward(DISTANCE)
.turn(Math.toRadians(90))
.forward(DISTANCE)
.turn(Math.toRadians(90))
.forward(DISTANCE)
.turn(Math.toRadians(90))
.forward(DISTANCE)
.turn(Math.toRadians(90))
.build();
drive.followTrajectorySequence(trajSeq);
}
}
}

48
TeamCode/src/main/java/org/RoadRunner/drive/opmode/LocalizationTest.java
View File

@@ -1,48 +0,0 @@
package org.RoadRunner.drive.opmode;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.RoadRunner.drive.SampleMecanumDrive;
/**
* This is a simple teleop routine for testing localization. Drive the robot around like a normal
* teleop routine and make sure the robot's estimated pose matches the robot's actual pose (slight
* errors are not out of the ordinary, especially with sudden drive motions). The goal of this
* exercise is to ascertain whether the localizer has been configured properly (note: the pure
* encoder localizer heading may be significantly off if the track width has not been tuned).
*/
@TeleOp(name = "TC LocalizationTest", group = "drive")
public class LocalizationTest extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
waitForStart();
while (!isStopRequested()) {
drive.setWeightedDrivePower(
new Pose2d(
-gamepad1.left_stick_y,
-gamepad1.left_stick_x,
-gamepad1.right_stick_x
)
);
drive.update();
Pose2d poseEstimate = drive.getPoseEstimate();
telemetry.addData("x", poseEstimate.getX());
telemetry.addData("y", poseEstimate.getY());
telemetry.addData("heading", poseEstimate.getHeading());
drive.localizer.telemetry(telemetry);
telemetry.update();
}
}
}

147
TeamCode/src/main/java/org/RoadRunner/drive/opmode/ManualFeedforwardTuner.java
View File

@@ -1,147 +0,0 @@
package org.RoadRunner.drive.opmode;
import static org.RoadRunner.drive.DriveConstants.MAX_ACCEL;
import static org.RoadRunner.drive.DriveConstants.MAX_VEL;
import static org.RoadRunner.drive.DriveConstants.RUN_USING_ENCODER;
import static org.RoadRunner.drive.DriveConstants.kA;
import static org.RoadRunner.drive.DriveConstants.kStatic;
import static org.RoadRunner.drive.DriveConstants.kV;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.kinematics.Kinematics;
import com.acmerobotics.roadrunner.profile.MotionProfile;
import com.acmerobotics.roadrunner.profile.MotionProfileGenerator;
import com.acmerobotics.roadrunner.profile.MotionState;
import com.acmerobotics.roadrunner.util.NanoClock;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.util.RobotLog;
import org.RoadRunner.drive.SampleMecanumDrive;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import java.util.Objects;
/*
* This routine is designed to tune the open-loop feedforward coefficients. Although it may seem unnecessary,
* tuning these coefficients is just as important as the positional parameters. Like the other
* manual tuning routines, this op mode relies heavily upon the dashboard. To access the dashboard,
* connect your computer to the RC's WiFi network. In your browser, navigate to
* https://192.168.49.1:8080/dash if you're using the RC phone or https://192.168.43.1:8080/dash if
* you are using the Control Hub. Once you've successfully connected, start the program, and your
* robot will begin moving forward and backward according to a motion profile. Your job is to graph
* the velocity errors over time and adjust the feedforward coefficients. Once you've found a
* satisfactory set of gains, add them to the appropriate fields in the DriveConstants.java file.
*
* Pressing Y/Δ (Xbox/PS4) will pause the tuning process and enter driver override, allowing the
* user to reset the position of the bot in the event that it drifts off the path.
* Pressing B/O (Xbox/PS4) will cede control back to the tuning process.
*/
@Config
@Autonomous(group = "drive")
public class ManualFeedforwardTuner extends LinearOpMode {
public static double DISTANCE = 72; // in
private FtcDashboard dashboard = FtcDashboard.getInstance();
private SampleMecanumDrive drive;
enum Mode {
DRIVER_MODE,
TUNING_MODE
}
private Mode mode;
private static MotionProfile generateProfile(boolean movingForward) {
MotionState start = new MotionState(movingForward ? 0 : DISTANCE, 0, 0, 0);
MotionState goal = new MotionState(movingForward ? DISTANCE : 0, 0, 0, 0);
return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, MAX_VEL, MAX_ACCEL);
}
@Override
public void runOpMode() {
if (RUN_USING_ENCODER) {
RobotLog.setGlobalErrorMsg("Feedforward constants usually don't need to be tuned " +
"when using the built-in drive motor velocity PID.");
}
Telemetry telemetry = new MultipleTelemetry(this.telemetry, dashboard.getTelemetry());
drive = new SampleMecanumDrive(hardwareMap);
mode = Mode.TUNING_MODE;
NanoClock clock = NanoClock.system();
telemetry.addLine("Ready!");
telemetry.update();
telemetry.clearAll();
waitForStart();
if (isStopRequested()) return;
boolean movingForwards = true;
MotionProfile activeProfile = generateProfile(true);
double profileStart = clock.seconds();
while (!isStopRequested()) {
telemetry.addData("mode", mode);
switch (mode) {
case TUNING_MODE:
if (gamepad1.y) {
mode = Mode.DRIVER_MODE;
}
// calculate and set the motor power
double profileTime = clock.seconds() - profileStart;
if (profileTime > activeProfile.duration()) {
// generate a new profile
movingForwards = !movingForwards;
activeProfile = generateProfile(movingForwards);
profileStart = clock.seconds();
}
MotionState motionState = activeProfile.get(profileTime);
double targetPower = Kinematics.calculateMotorFeedforward(motionState.getV(), motionState.getA(), kV, kA, kStatic);
drive.setDrivePower(new Pose2d(targetPower, 0, 0));
drive.updatePoseEstimate();
Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer.");
double currentVelo = poseVelo.getX();
// update telemetry
telemetry.addData("targetVelocity", motionState.getV());
telemetry.addData("measuredVelocity", currentVelo);
telemetry.addData("error", motionState.getV() - currentVelo);
break;
case DRIVER_MODE:
if (gamepad1.b) {
mode = Mode.TUNING_MODE;
movingForwards = true;
activeProfile = generateProfile(movingForwards);
profileStart = clock.seconds();
}
drive.setWeightedDrivePower(
new Pose2d(
-gamepad1.left_stick_y,
-gamepad1.left_stick_x,
-gamepad1.right_stick_x
)
);
break;
}
telemetry.update();
}
}
}

73
TeamCode/src/main/java/org/RoadRunner/drive/opmode/MaxAngularVeloTuner.java
View File

@@ -1,73 +0,0 @@
package org.RoadRunner.drive.opmode;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.RoadRunner.drive.SampleMecanumDrive;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import java.util.Objects;
/**
* This routine is designed to calculate the maximum angular velocity your bot can achieve under load.
* <p>
* Upon pressing start, your bot will turn at max power for RUNTIME seconds.
* <p>
* Further fine tuning of MAX_ANG_VEL may be desired.
*/
@Config
@Autonomous(group = "drive")
public class MaxAngularVeloTuner extends LinearOpMode {
public static double RUNTIME = 4.0;
private ElapsedTime timer;
private double maxAngVelocity = 0.0;
@Override
public void runOpMode() throws InterruptedException {
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetry.addLine("Your bot will turn at full speed for " + RUNTIME + " seconds.");
telemetry.addLine("Please ensure you have enough space cleared.");
telemetry.addLine("");
telemetry.addLine("Press start when ready.");
telemetry.update();
waitForStart();
telemetry.clearAll();
telemetry.update();
drive.setDrivePower(new Pose2d(0, 0, 1));
timer = new ElapsedTime();
while (!isStopRequested() && timer.seconds() < RUNTIME) {
drive.updatePoseEstimate();
Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer.");
maxAngVelocity = Math.max(poseVelo.getHeading(), maxAngVelocity);
}
drive.setDrivePower(new Pose2d());
telemetry.addData("Max Angular Velocity (rad)", maxAngVelocity);
telemetry.addData("Max Angular Velocity (deg)", Math.toDegrees(maxAngVelocity));
telemetry.addData("Max Recommended Angular Velocity (rad)", maxAngVelocity * 0.8);
telemetry.addData("Max Recommended Angular Velocity (deg)", Math.toDegrees(maxAngVelocity * 0.8));
telemetry.update();
while (!isStopRequested()) idle();
}
}

84
TeamCode/src/main/java/org/RoadRunner/drive/opmode/MaxVelocityTuner.java
View File

@@ -1,84 +0,0 @@
package org.RoadRunner.drive.opmode;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.VoltageSensor;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.RoadRunner.drive.SampleMecanumDrive;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.RoadRunner.drive.DriveConstants;
import java.util.Objects;
/**
* This routine is designed to calculate the maximum velocity your bot can achieve under load. It
* will also calculate the effective kF value for your velocity PID.
* <p>
* Upon pressing start, your bot will run at max power for RUNTIME seconds.
* <p>
* Further fine tuning of kF may be desired.
*/
@Config
@Autonomous(group = "drive")
public class MaxVelocityTuner extends LinearOpMode {
public static double RUNTIME = 2.0;
private ElapsedTime timer;
private double maxVelocity = 0.0;
private VoltageSensor batteryVoltageSensor;
@Override
public void runOpMode() throws InterruptedException {
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next();
Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetry.addLine("Your bot will go at full speed for " + RUNTIME + " seconds.");
telemetry.addLine("Please ensure you have enough space cleared.");
telemetry.addLine("");
telemetry.addLine("Press start when ready.");
telemetry.update();
waitForStart();
telemetry.clearAll();
telemetry.update();
drive.setDrivePower(new Pose2d(1, 0, 0));
timer = new ElapsedTime();
while (!isStopRequested() && timer.seconds() < RUNTIME) {
drive.updatePoseEstimate();
Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer.");
maxVelocity = Math.max(poseVelo.vec().norm(), maxVelocity);
}
drive.setDrivePower(new Pose2d());
double effectiveKf = DriveConstants.getMotorVelocityF(veloInchesToTicks(maxVelocity));
telemetry.addData("Max Velocity", maxVelocity);
telemetry.addData("Max Recommended Velocity", maxVelocity * 0.8);
telemetry.addData("Voltage Compensated kF", effectiveKf * batteryVoltageSensor.getVoltage() / 12);
telemetry.update();
while (!isStopRequested() && opModeIsActive()) idle();
}
private double veloInchesToTicks(double inchesPerSec) {
return inchesPerSec / (2 * Math.PI * DriveConstants.WHEEL_RADIUS) / DriveConstants.GEAR_RATIO * DriveConstants.TICKS_PER_REV;
}
}

93
TeamCode/src/main/java/org/RoadRunner/drive/opmode/MotorDirectionDebugger.java
View File

@@ -1,93 +0,0 @@
package org.RoadRunner.drive.opmode;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.RoadRunner.drive.SampleMecanumDrive;
import org.firstinspires.ftc.robotcore.external.Telemetry;
/**
* This is a simple teleop routine for debugging your motor configuration.
* Pressing each of the buttons will power its respective motor.
*
* Button Mappings:
*
* Xbox/PS4 Button - Motor
* X / ▢ - Front Left
* Y / Δ - Front Right
* B / O - Rear Right
* A / X - Rear Left
* The buttons are mapped to match the wheels spatially if you
* were to rotate the gamepad 45deg°. x/square is the front left
* ________ and each button corresponds to the wheel as you go clockwise
* / ______ \
* ------------.-' _ '-..+ Front of Bot
* / _ ( Y ) _ \ ^
* | ( X ) _ ( B ) | Front Left \ Front Right
* ___ '. ( A ) /| Wheel \ Wheel
* .' '. '-._____.-' .' (x/▢) \ (Y/Δ)
* | | | \
* '.___.' '. | Rear Left \ Rear Right
* '. / Wheel \ Wheel
* \. .' (A/X) \ (B/O)
* \________/
*
* Uncomment the @Disabled tag below to use this opmode.
*/
@Disabled
@Config
@TeleOp(group = "drive")
public class MotorDirectionDebugger extends LinearOpMode {
public static double MOTOR_POWER = 0.7;
@Override
public void runOpMode() throws InterruptedException {
Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
telemetry.addLine("Press play to begin the debugging opmode");
telemetry.update();
waitForStart();
if (isStopRequested()) return;
telemetry.clearAll();
telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML);
while (!isStopRequested()) {
telemetry.addLine("Press each button to turn on its respective motor");
telemetry.addLine();
telemetry.addLine("<font face=\"monospace\">Xbox/PS4 Button - Motor</font>");
telemetry.addLine("<font face=\"monospace\">&nbsp;&nbsp;X / ▢&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;- Front Left</font>");
telemetry.addLine("<font face=\"monospace\">&nbsp;&nbsp;Y / Δ&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;- Front Right</font>");
telemetry.addLine("<font face=\"monospace\">&nbsp;&nbsp;B / O&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;- Rear&nbsp;&nbsp;Right</font>");
telemetry.addLine("<font face=\"monospace\">&nbsp;&nbsp;A / X&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;- Rear&nbsp;&nbsp;Left</font>");
telemetry.addLine();
if(gamepad1.x) {
drive.setMotorPowers(MOTOR_POWER, 0, 0, 0);
telemetry.addLine("Running Motor: Front Left");
} else if(gamepad1.y) {
drive.setMotorPowers(0, 0, 0, MOTOR_POWER);
telemetry.addLine("Running Motor: Front Right");
} else if(gamepad1.b) {
drive.setMotorPowers(0, 0, MOTOR_POWER, 0);
telemetry.addLine("Running Motor: Rear Right");
} else if(gamepad1.a) {
drive.setMotorPowers(0, MOTOR_POWER, 0, 0);
telemetry.addLine("Running Motor: Rear Left");
} else {
drive.setMotorPowers(0, 0, 0, 0);
telemetry.addLine("Running Motor: None");
}
telemetry.update();
}
}
}

38
TeamCode/src/main/java/org/RoadRunner/drive/opmode/SplineTest.java
View File

@@ -1,38 +0,0 @@
package org.RoadRunner.drive.opmode;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.RoadRunner.drive.SampleMecanumDrive;
/*
* This is an example of a more complex path to really test the tuning.
*/
@Autonomous(group = "drive")
public class SplineTest extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
waitForStart();
if (isStopRequested()) return;
Trajectory traj = drive.trajectoryBuilder(new Pose2d())
.splineTo(new Vector2d(30, 30), 0)
.build();
drive.followTrajectory(traj);
sleep(2000);
drive.followTrajectory(
drive.trajectoryBuilder(traj.end(), true)
.splineTo(new Vector2d(0, 0), Math.toRadians(180))
.build()
);
}
}

46
TeamCode/src/main/java/org/RoadRunner/drive/opmode/StrafeTest.java
View File

@@ -1,46 +0,0 @@
package org.RoadRunner.drive.opmode;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.RoadRunner.drive.SampleMecanumDrive;
import org.firstinspires.ftc.robotcore.external.Telemetry;
/*
* This is a simple routine to test translational drive capabilities.
*/
@Config
@Autonomous(group = "drive")
public class StrafeTest extends LinearOpMode {
public static double DISTANCE = 60; // in
@Override
public void runOpMode() throws InterruptedException {
Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
Trajectory trajectory = drive.trajectoryBuilder(new Pose2d())
.strafeRight(DISTANCE)
.build();
waitForStart();
if (isStopRequested()) return;
drive.followTrajectory(trajectory);
Pose2d poseEstimate = drive.getPoseEstimate();
telemetry.addData("finalX", poseEstimate.getX());
telemetry.addData("finalY", poseEstimate.getY());
telemetry.addData("finalHeading", poseEstimate.getHeading());
telemetry.update();
while (!isStopRequested() && opModeIsActive()) ;
}
}

48
TeamCode/src/main/java/org/RoadRunner/drive/opmode/StraightTest.java
View File

@@ -1,48 +0,0 @@
package org.RoadRunner.drive.opmode;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.RoadRunner.drive.SampleMecanumDrive;
import org.firstinspires.ftc.robotcore.external.Telemetry;
/*
* This is a simple routine to test translational drive capabilities.
*/
@Config
@Autonomous(group = "drive")
public class StraightTest extends LinearOpMode {
public static double DISTANCE = 60; // in
@Override
public void runOpMode() throws InterruptedException {
Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
Trajectory trajectory = drive.trajectoryBuilder(new Pose2d())
.forward(DISTANCE)
.build();
waitForStart();
if (isStopRequested()) return;
drive.followTrajectory(trajectory);
Pose2d poseEstimate = drive.getPoseEstimate();
telemetry.addData("finalX", poseEstimate.getX());
telemetry.addData("finalY", poseEstimate.getY());
telemetry.addData("finalHeading", poseEstimate.getHeading());
drive.telemetry(telemetry);
telemetry.update();
while (!isStopRequested() && opModeIsActive()) ;
}
}

88
TeamCode/src/main/java/org/RoadRunner/drive/opmode/TrackWidthTuner.java
View File

@@ -1,88 +0,0 @@
package org.RoadRunner.drive.opmode;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.util.Angle;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.util.MovingStatistics;
import org.RoadRunner.drive.SampleMecanumDrive;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.robotcore.internal.system.Misc;
import org.RoadRunner.drive.DriveConstants;
/*
* This routine determines the effective track width. The procedure works by executing a point turn
* with a given angle and measuring the difference between that angle and the actual angle (as
* indicated by an external IMU/gyro, track wheels, or some other localizer). The quotient
* given angle / actual angle gives a multiplicative adjustment to the estimated track width
* (effective track width = estimated track width * given angle / actual angle). The routine repeats
* this procedure a few times and averages the values for additional accuracy. Note: a relatively
* accurate track width estimate is important or else the angular constraints will be thrown off.
*/
@Config
@Autonomous(group = "drive")
public class TrackWidthTuner extends LinearOpMode {
public static double ANGLE = 180; // deg
public static int NUM_TRIALS = 5;
public static int DELAY = 1000; // ms
@Override
public void runOpMode() throws InterruptedException {
Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
// TODO: if you haven't already, set the localizer to something that doesn't depend on
// drive encoders for computing the heading
telemetry.addLine("Press play to begin the track width tuner routine");
telemetry.addLine("Make sure your robot has enough clearance to turn smoothly");
telemetry.update();
waitForStart();
if (isStopRequested()) return;
telemetry.clearAll();
telemetry.addLine("Running...");
telemetry.update();
MovingStatistics trackWidthStats = new MovingStatistics(NUM_TRIALS);
for (int i = 0; i < NUM_TRIALS; i++) {
drive.setPoseEstimate(new Pose2d());
// it is important to handle heading wraparounds
double headingAccumulator = 0;
double lastHeading = 0;
drive.turnAsync(Math.toRadians(ANGLE));
while (!isStopRequested() && drive.isBusy()) {
double heading = drive.getPoseEstimate().getHeading();
headingAccumulator += Angle.normDelta(heading - lastHeading);
lastHeading = heading;
drive.update();
}
double trackWidth = DriveConstants.TRACK_WIDTH * Math.toRadians(ANGLE) / headingAccumulator;
trackWidthStats.add(trackWidth);
sleep(DELAY);
}
telemetry.clearAll();
telemetry.addLine("Tuning complete");
telemetry.addLine(Misc.formatInvariant("Effective track width = %.2f (SE = %.3f)",
trackWidthStats.getMean(),
trackWidthStats.getStandardDeviation() / Math.sqrt(NUM_TRIALS)));
telemetry.update();
while (!isStopRequested()) {
idle();
}
}
}

104
TeamCode/src/main/java/org/RoadRunner/drive/opmode/TrackingWheelForwardOffsetTuner.java
View File

@@ -1,104 +0,0 @@
package org.RoadRunner.drive.opmode;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.util.Angle;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.util.MovingStatistics;
import com.qualcomm.robotcore.util.RobotLog;
import org.RoadRunner.drive.SampleMecanumDrive;
import org.RoadRunner.drive.StandardTrackingWheelLocalizer;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.robotcore.internal.system.Misc;
/**
* This routine determines the effective forward offset for the lateral tracking wheel.
* The procedure executes a point turn at a given angle for a certain number of trials,
* along with a specified delay in milliseconds. The purpose of this is to track the
* change in the y position during the turn. The offset, or distance, of the lateral tracking
* wheel from the center or rotation allows the wheel to spin during a point turn, leading
* to an incorrect measurement for the y position. This creates an arc around around
* the center of rotation with an arc length of change in y and a radius equal to the forward
* offset. We can compute this offset by calculating (change in y position) / (change in heading)
* which returns the radius if the angle (change in heading) is in radians. This is based
* on the arc length formula of length = theta * radius.
*
* To run this routine, simply adjust the desired angle and specify the number of trials
* and the desired delay. Then, run the procedure. Once it finishes, it will print the
* average of all the calculated forward offsets derived from the calculation. This calculated
* forward offset is then added onto the current forward offset to produce an overall estimate
* for the forward offset. You can run this procedure as many times as necessary until a
* satisfactory result is produced.
*/
@Config
@Autonomous(group="drive")
public class TrackingWheelForwardOffsetTuner extends LinearOpMode {
public static double ANGLE = 180; // deg
public static int NUM_TRIALS = 5;
public static int DELAY = 1000; // ms
@Override
public void runOpMode() throws InterruptedException {
Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
if (!(drive.getLocalizer() instanceof StandardTrackingWheelLocalizer)) {
RobotLog.setGlobalErrorMsg("StandardTrackingWheelLocalizer is not being set in the "
+ "drive class. Ensure that \"setLocalizer(new StandardTrackingWheelLocalizer"
+ "(hardwareMap));\" is called in SampleMecanumDrive.java");
}
telemetry.addLine("Press play to begin the forward offset tuner");
telemetry.addLine("Make sure your robot has enough clearance to turn smoothly");
telemetry.update();
waitForStart();
if (isStopRequested()) return;
telemetry.clearAll();
telemetry.addLine("Running...");
telemetry.update();
MovingStatistics forwardOffsetStats = new MovingStatistics(NUM_TRIALS);
for (int i = 0; i < NUM_TRIALS; i++) {
drive.setPoseEstimate(new Pose2d());
// it is important to handle heading wraparounds
double headingAccumulator = 0;
double lastHeading = 0;
drive.turnAsync(Math.toRadians(ANGLE));
while (!isStopRequested() && drive.isBusy()) {
double heading = drive.getPoseEstimate().getHeading();
headingAccumulator += Angle.norm(heading - lastHeading);
lastHeading = heading;
drive.update();
}
double forwardOffset = StandardTrackingWheelLocalizer.FORWARD_OFFSET +
drive.getPoseEstimate().getY() / headingAccumulator;
forwardOffsetStats.add(forwardOffset);
sleep(DELAY);
}
telemetry.clearAll();
telemetry.addLine("Tuning complete");
telemetry.addLine(Misc.formatInvariant("Effective forward offset = %.2f (SE = %.3f)",
forwardOffsetStats.getMean(),
forwardOffsetStats.getStandardDeviation() / Math.sqrt(NUM_TRIALS)));
telemetry.update();
while (!isStopRequested()) {
idle();
}
}
}

130
TeamCode/src/main/java/org/RoadRunner/drive/opmode/TrackingWheelLateralDistanceTuner.java
View File

@@ -1,130 +0,0 @@
package org.RoadRunner.drive.opmode;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.util.Angle;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.util.RobotLog;
import org.RoadRunner.drive.SampleMecanumDrive;
import org.RoadRunner.drive.StandardTrackingWheelLocalizer;
/**
* Opmode designed to assist the user in tuning the `StandardTrackingWheelLocalizer`'s
* LATERAL_DISTANCE value. The LATERAL_DISTANCE is the center-to-center distance of the parallel
* wheels.
*
* Tuning Routine:
*
* 1. Set the LATERAL_DISTANCE value in StandardTrackingWheelLocalizer.java to the physical
* measured value. This need only be an estimated value as you will be tuning it anyways.
*
* 2. Make a mark on the bot (with a piece of tape or sharpie or however you wish) and make an
* similar mark right below the indicator on your bot. This will be your reference point to
* ensure you've turned exactly 360°.
*
* 3. Although not entirely necessary, having the bot's pose being drawn in dashbooard does help
* identify discrepancies in the LATERAL_DISTANCE value. To access the dashboard,
* connect your computer to the RC's WiFi network. In your browser, navigate to
* https://192.168.49.1:8080/dash if you're using the RC phone or https://192.168.43.1:8080/dash
* if you are using the Control Hub.
* Ensure the field is showing (select the field view in top right of the page).
*
* 4. Press play to begin the tuning routine.
*
* 5. Use the right joystick on gamepad 1 to turn the bot counterclockwise.
*
* 6. Spin the bot 10 times, counterclockwise. Make sure to keep track of these turns.
*
* 7. Once the bot has finished spinning 10 times, press A to finishing the routine. The indicators
* on the bot and on the ground you created earlier should be lined up.
*
* 8. Your effective LATERAL_DISTANCE will be given. Stick this value into your
* StandardTrackingWheelLocalizer.java class.
*
* 9. If this value is incorrect, run the routine again while adjusting the LATERAL_DISTANCE value
* yourself. Read the heading output and follow the advice stated in the note below to manually
* nudge the values yourself.
*
* Note:
* It helps to pay attention to how the pose on the field is drawn in dashboard. A blue circle with
* a line from the circumference to the center should be present, representing the bot. The line
* indicates forward. If your LATERAL_DISTANCE value is tuned currently, the pose drawn in
* dashboard should keep track with the pose of your actual bot. If the drawn bot turns slower than
* the actual bot, the LATERAL_DISTANCE should be decreased. If the drawn bot turns faster than the
* actual bot, the LATERAL_DISTANCE should be increased.
*
* If your drawn bot oscillates around a point in dashboard, don't worry. This is because the
* position of the perpendicular wheel isn't perfectly set and causes a discrepancy in the
* effective center of rotation. You can ignore this effect. The center of rotation will be offset
* slightly but your heading will still be fine. This does not affect your overall tracking
* precision. The heading should still line up.
*/
@Config
@TeleOp(group = "drive")
public class TrackingWheelLateralDistanceTuner extends LinearOpMode {
public static int NUM_TURNS = 10;
@Override
public void runOpMode() throws InterruptedException {
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
if (!(drive.getLocalizer() instanceof StandardTrackingWheelLocalizer)) {
RobotLog.setGlobalErrorMsg("StandardTrackingWheelLocalizer is not being set in the "
+ "drive class. Ensure that \"setLocalizer(new StandardTrackingWheelLocalizer"
+ "(hardwareMap));\" is called in SampleMecanumDrive.java");
}
telemetry.addLine("Prior to beginning the routine, please read the directions "
+ "located in the comments of the opmode file.");
telemetry.addLine("Press play to begin the tuning routine.");
telemetry.addLine("");
telemetry.addLine("Press Y/△ to stop the routine.");
telemetry.update();
waitForStart();
if (isStopRequested()) return;
telemetry.clearAll();
telemetry.update();
double headingAccumulator = 0;
double lastHeading = 0;
boolean tuningFinished = false;
while (!isStopRequested() && !tuningFinished) {
Pose2d vel = new Pose2d(0, 0, -gamepad1.right_stick_x);
drive.setDrivePower(vel);
drive.update();
double heading = drive.getPoseEstimate().getHeading();
double deltaHeading = heading - lastHeading;
headingAccumulator += Angle.normDelta(deltaHeading);
lastHeading = heading;
telemetry.clearAll();
telemetry.addLine("Total Heading (deg): " + Math.toDegrees(headingAccumulator));
telemetry.addLine("Raw Heading (deg): " + Math.toDegrees(heading));
telemetry.addLine();
telemetry.addLine("Press Y/△ to conclude routine");
telemetry.update();
if (gamepad1.y)
tuningFinished = true;
}
telemetry.clearAll();
telemetry.addLine("Localizer's total heading: " + Math.toDegrees(headingAccumulator) + "°");
telemetry.addLine("Effective LATERAL_DISTANCE: " +
(headingAccumulator / (NUM_TURNS * Math.PI * 2)) * StandardTrackingWheelLocalizer.LATERAL_DISTANCE);
telemetry.update();
while (!isStopRequested()) idle();
}
}

28
TeamCode/src/main/java/org/RoadRunner/drive/opmode/TurnTest.java
View File

@@ -1,28 +0,0 @@
package org.RoadRunner.drive.opmode;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.RoadRunner.drive.SampleMecanumDrive;
/*
* This is a simple routine to test turning capabilities.
*/
@Config
@Autonomous(group = "drive")
public class TurnTest extends LinearOpMode {
public static double ANGLE = 90; // deg
@Override
public void runOpMode() throws InterruptedException {
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
waitForStart();
if (isStopRequested()) return;
drive.turn(Math.toRadians(ANGLE));
}
}

4
TeamCode/src/main/java/org/RoadRunner/trajectorysequence/EmptySequenceException.java
View File

@@ -1,4 +0,0 @@
package org.RoadRunner.trajectorysequence;
public class EmptySequenceException extends RuntimeException { }

44
TeamCode/src/main/java/org/RoadRunner/trajectorysequence/TrajectorySequence.java
View File

@@ -1,44 +0,0 @@
package org.RoadRunner.trajectorysequence;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import org.RoadRunner.trajectorysequence.sequencesegment.SequenceSegment;
import java.util.Collections;
import java.util.List;
public class TrajectorySequence {
private final List<SequenceSegment> sequenceList;
public TrajectorySequence(List<SequenceSegment> sequenceList) {
if (sequenceList.size() == 0) throw new EmptySequenceException();
this.sequenceList = Collections.unmodifiableList(sequenceList);
}
public Pose2d start() {
return sequenceList.get(0).getStartPose();
}
public Pose2d end() {
return sequenceList.get(sequenceList.size() - 1).getEndPose();
}
public double duration() {
double total = 0.0;
for (SequenceSegment segment : sequenceList) {
total += segment.getDuration();
}
return total;
}
public SequenceSegment get(int i) {
return sequenceList.get(i);
}
public int size() {
return sequenceList.size();
}
}

711
TeamCode/src/main/java/org/RoadRunner/trajectorysequence/TrajectorySequenceBuilder.java
View File

@@ -1,711 +0,0 @@
package org.RoadRunner.trajectorysequence;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.acmerobotics.roadrunner.path.PathContinuityViolationException;
import com.acmerobotics.roadrunner.profile.MotionProfile;
import com.acmerobotics.roadrunner.profile.MotionProfileGenerator;
import com.acmerobotics.roadrunner.profile.MotionState;
import com.acmerobotics.roadrunner.trajectory.DisplacementMarker;
import com.acmerobotics.roadrunner.trajectory.DisplacementProducer;
import com.acmerobotics.roadrunner.trajectory.MarkerCallback;
import com.acmerobotics.roadrunner.trajectory.SpatialMarker;
import com.acmerobotics.roadrunner.trajectory.TemporalMarker;
import com.acmerobotics.roadrunner.trajectory.TimeProducer;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint;
import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint;
import com.acmerobotics.roadrunner.util.Angle;
import org.RoadRunner.trajectorysequence.sequencesegment.SequenceSegment;
import org.RoadRunner.trajectorysequence.sequencesegment.TrajectorySegment;
import org.RoadRunner.trajectorysequence.sequencesegment.TurnSegment;
import org.RoadRunner.trajectorysequence.sequencesegment.WaitSegment;
import java.util.ArrayList;
import java.util.Collections;
import java.util.List;
public class TrajectorySequenceBuilder {
private final double resolution = 0.25;
private final TrajectoryVelocityConstraint baseVelConstraint;
private final TrajectoryAccelerationConstraint baseAccelConstraint;
private TrajectoryVelocityConstraint currentVelConstraint;
private TrajectoryAccelerationConstraint currentAccelConstraint;
private final double baseTurnConstraintMaxAngVel;
private final double baseTurnConstraintMaxAngAccel;
private double currentTurnConstraintMaxAngVel;
private double currentTurnConstraintMaxAngAccel;
private final List<SequenceSegment> sequenceSegments;
private final List<TemporalMarker> temporalMarkers;
private final List<DisplacementMarker> displacementMarkers;
private final List<SpatialMarker> spatialMarkers;
private Pose2d lastPose;
private double tangentOffset;
private boolean setAbsoluteTangent;
private double absoluteTangent;
private TrajectoryBuilder currentTrajectoryBuilder;
private double currentDuration;
private double currentDisplacement;
private double lastDurationTraj;
private double lastDisplacementTraj;
public TrajectorySequenceBuilder(
Pose2d startPose,
Double startTangent,
TrajectoryVelocityConstraint baseVelConstraint,
TrajectoryAccelerationConstraint baseAccelConstraint,
double baseTurnConstraintMaxAngVel,
double baseTurnConstraintMaxAngAccel
) {
this.baseVelConstraint = baseVelConstraint;
this.baseAccelConstraint = baseAccelConstraint;
this.currentVelConstraint = baseVelConstraint;
this.currentAccelConstraint = baseAccelConstraint;
this.baseTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel;
this.baseTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel;
this.currentTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel;
this.currentTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel;
sequenceSegments = new ArrayList<>();
temporalMarkers = new ArrayList<>();
displacementMarkers = new ArrayList<>();
spatialMarkers = new ArrayList<>();
lastPose = startPose;
tangentOffset = 0.0;
setAbsoluteTangent = (startTangent != null);
absoluteTangent = startTangent != null ? startTangent : 0.0;
currentTrajectoryBuilder = null;
currentDuration = 0.0;
currentDisplacement = 0.0;
lastDurationTraj = 0.0;
lastDisplacementTraj = 0.0;
}
public TrajectorySequenceBuilder(
Pose2d startPose,
TrajectoryVelocityConstraint baseVelConstraint,
TrajectoryAccelerationConstraint baseAccelConstraint,
double baseTurnConstraintMaxAngVel,
double baseTurnConstraintMaxAngAccel
) {
this(
startPose, null,
baseVelConstraint, baseAccelConstraint,
baseTurnConstraintMaxAngVel, baseTurnConstraintMaxAngAccel
);
}
public TrajectorySequenceBuilder lineTo(Vector2d endPosition) {
return addPath(() -> currentTrajectoryBuilder.lineTo(endPosition, currentVelConstraint, currentAccelConstraint));
}
public TrajectorySequenceBuilder lineTo(
Vector2d endPosition,
TrajectoryVelocityConstraint velConstraint,
TrajectoryAccelerationConstraint accelConstraint
) {
return addPath(() -> currentTrajectoryBuilder.lineTo(endPosition, velConstraint, accelConstraint));
}
public TrajectorySequenceBuilder lineToConstantHeading(Vector2d endPosition) {
return addPath(() -> currentTrajectoryBuilder.lineToConstantHeading(endPosition, currentVelConstraint, currentAccelConstraint));
}
public TrajectorySequenceBuilder lineToConstantHeading(
Vector2d endPosition,
TrajectoryVelocityConstraint velConstraint,
TrajectoryAccelerationConstraint accelConstraint
) {
return addPath(() -> currentTrajectoryBuilder.lineToConstantHeading(endPosition, velConstraint, accelConstraint));
}
public TrajectorySequenceBuilder lineToLinearHeading(Pose2d endPose) {
return addPath(() -> currentTrajectoryBuilder.lineToLinearHeading(endPose, currentVelConstraint, currentAccelConstraint));
}
public TrajectorySequenceBuilder lineToLinearHeading(
Pose2d endPose,
TrajectoryVelocityConstraint velConstraint,
TrajectoryAccelerationConstraint accelConstraint
) {
return addPath(() -> currentTrajectoryBuilder.lineToLinearHeading(endPose, velConstraint, accelConstraint));
}
public TrajectorySequenceBuilder lineToSplineHeading(Pose2d endPose) {
return addPath(() -> currentTrajectoryBuilder.lineToSplineHeading(endPose, currentVelConstraint, currentAccelConstraint));
}
public TrajectorySequenceBuilder lineToSplineHeading(
Pose2d endPose,
TrajectoryVelocityConstraint velConstraint,
TrajectoryAccelerationConstraint accelConstraint
) {
return addPath(() -> currentTrajectoryBuilder.lineToSplineHeading(endPose, velConstraint, accelConstraint));
}
public TrajectorySequenceBuilder strafeTo(Vector2d endPosition) {
return addPath(() -> currentTrajectoryBuilder.strafeTo(endPosition, currentVelConstraint, currentAccelConstraint));
}
public TrajectorySequenceBuilder strafeTo(
Vector2d endPosition,
TrajectoryVelocityConstraint velConstraint,
TrajectoryAccelerationConstraint accelConstraint
) {
return addPath(() -> currentTrajectoryBuilder.strafeTo(endPosition, velConstraint, accelConstraint));
}
public TrajectorySequenceBuilder forward(double distance) {
return addPath(() -> currentTrajectoryBuilder.forward(distance, currentVelConstraint, currentAccelConstraint));
}
public TrajectorySequenceBuilder forward(
double distance,
TrajectoryVelocityConstraint velConstraint,
TrajectoryAccelerationConstraint accelConstraint
) {
return addPath(() -> currentTrajectoryBuilder.forward(distance, velConstraint, accelConstraint));
}
public TrajectorySequenceBuilder back(double distance) {
return addPath(() -> currentTrajectoryBuilder.back(distance, currentVelConstraint, currentAccelConstraint));
}
public TrajectorySequenceBuilder back(
double distance,
TrajectoryVelocityConstraint velConstraint,
TrajectoryAccelerationConstraint accelConstraint
) {
return addPath(() -> currentTrajectoryBuilder.back(distance, velConstraint, accelConstraint));
}
public TrajectorySequenceBuilder strafeLeft(double distance) {
return addPath(() -> currentTrajectoryBuilder.strafeLeft(distance, currentVelConstraint, currentAccelConstraint));
}
public TrajectorySequenceBuilder strafeLeft(
double distance,
TrajectoryVelocityConstraint velConstraint,
TrajectoryAccelerationConstraint accelConstraint
) {
return addPath(() -> currentTrajectoryBuilder.strafeLeft(distance, velConstraint, accelConstraint));
}
public TrajectorySequenceBuilder strafeRight(double distance) {
return addPath(() -> currentTrajectoryBuilder.strafeRight(distance, currentVelConstraint, currentAccelConstraint));
}
public TrajectorySequenceBuilder strafeRight(
double distance,
TrajectoryVelocityConstraint velConstraint,
TrajectoryAccelerationConstraint accelConstraint
) {
return addPath(() -> currentTrajectoryBuilder.strafeRight(distance, velConstraint, accelConstraint));
}
public TrajectorySequenceBuilder splineTo(Vector2d endPosition, double endHeading) {
return addPath(() -> currentTrajectoryBuilder.splineTo(endPosition, endHeading, currentVelConstraint, currentAccelConstraint));
}
public TrajectorySequenceBuilder splineTo(
Vector2d endPosition,
double endHeading,
TrajectoryVelocityConstraint velConstraint,
TrajectoryAccelerationConstraint accelConstraint
) {
return addPath(() -> currentTrajectoryBuilder.splineTo(endPosition, endHeading, velConstraint, accelConstraint));
}
public TrajectorySequenceBuilder splineToConstantHeading(Vector2d endPosition, double endHeading) {
return addPath(() -> currentTrajectoryBuilder.splineToConstantHeading(endPosition, endHeading, currentVelConstraint, currentAccelConstraint));
}
public TrajectorySequenceBuilder splineToConstantHeading(
Vector2d endPosition,
double endHeading,
TrajectoryVelocityConstraint velConstraint,
TrajectoryAccelerationConstraint accelConstraint
) {
return addPath(() -> currentTrajectoryBuilder.splineToConstantHeading(endPosition, endHeading, velConstraint, accelConstraint));
}
public TrajectorySequenceBuilder splineToLinearHeading(Pose2d endPose, double endHeading) {
return addPath(() -> currentTrajectoryBuilder.splineToLinearHeading(endPose, endHeading, currentVelConstraint, currentAccelConstraint));
}
public TrajectorySequenceBuilder splineToLinearHeading(
Pose2d endPose,
double endHeading,
TrajectoryVelocityConstraint velConstraint,
TrajectoryAccelerationConstraint accelConstraint
) {
return addPath(() -> currentTrajectoryBuilder.splineToLinearHeading(endPose, endHeading, velConstraint, accelConstraint));
}
public TrajectorySequenceBuilder splineToSplineHeading(Pose2d endPose, double endHeading) {
return addPath(() -> currentTrajectoryBuilder.splineToSplineHeading(endPose, endHeading, currentVelConstraint, currentAccelConstraint));
}
public TrajectorySequenceBuilder splineToSplineHeading(
Pose2d endPose,
double endHeading,
TrajectoryVelocityConstraint velConstraint,
TrajectoryAccelerationConstraint accelConstraint
) {
return addPath(() -> currentTrajectoryBuilder.splineToSplineHeading(endPose, endHeading, velConstraint, accelConstraint));
}
private TrajectorySequenceBuilder addPath(AddPathCallback callback) {
if (currentTrajectoryBuilder == null) newPath();
try {
callback.run();
} catch (PathContinuityViolationException e) {
newPath();
callback.run();
}
Trajectory builtTraj = currentTrajectoryBuilder.build();
double durationDifference = builtTraj.duration() - lastDurationTraj;
double displacementDifference = builtTraj.getPath().length() - lastDisplacementTraj;
lastPose = builtTraj.end();
currentDuration += durationDifference;
currentDisplacement += displacementDifference;
lastDurationTraj = builtTraj.duration();
lastDisplacementTraj = builtTraj.getPath().length();
return this;
}
public TrajectorySequenceBuilder setTangent(double tangent) {
setAbsoluteTangent = true;
absoluteTangent = tangent;
pushPath();
return this;
}
private TrajectorySequenceBuilder setTangentOffset(double offset) {
setAbsoluteTangent = false;
this.tangentOffset = offset;
this.pushPath();
return this;
}
public TrajectorySequenceBuilder setReversed(boolean reversed) {
return reversed ? this.setTangentOffset(Math.toRadians(180.0)) : this.setTangentOffset(0.0);
}
public TrajectorySequenceBuilder setConstraints(
TrajectoryVelocityConstraint velConstraint,
TrajectoryAccelerationConstraint accelConstraint
) {
this.currentVelConstraint = velConstraint;
this.currentAccelConstraint = accelConstraint;
return this;
}
public TrajectorySequenceBuilder resetConstraints() {
this.currentVelConstraint = this.baseVelConstraint;
this.currentAccelConstraint = this.baseAccelConstraint;
return this;
}
public TrajectorySequenceBuilder setVelConstraint(TrajectoryVelocityConstraint velConstraint) {
this.currentVelConstraint = velConstraint;
return this;
}
public TrajectorySequenceBuilder resetVelConstraint() {
this.currentVelConstraint = this.baseVelConstraint;
return this;
}
public TrajectorySequenceBuilder setAccelConstraint(TrajectoryAccelerationConstraint accelConstraint) {
this.currentAccelConstraint = accelConstraint;
return this;
}
public TrajectorySequenceBuilder resetAccelConstraint() {
this.currentAccelConstraint = this.baseAccelConstraint;
return this;
}
public TrajectorySequenceBuilder setTurnConstraint(double maxAngVel, double maxAngAccel) {
this.currentTurnConstraintMaxAngVel = maxAngVel;
this.currentTurnConstraintMaxAngAccel = maxAngAccel;
return this;
}
public TrajectorySequenceBuilder resetTurnConstraint() {
this.currentTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel;
this.currentTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel;
return this;
}
public TrajectorySequenceBuilder addTemporalMarker(MarkerCallback callback) {
return this.addTemporalMarker(currentDuration, callback);
}
public TrajectorySequenceBuilder UNSTABLE_addTemporalMarkerOffset(double offset, MarkerCallback callback) {
return this.addTemporalMarker(currentDuration + offset, callback);
}
public TrajectorySequenceBuilder addTemporalMarker(double time, MarkerCallback callback) {
return this.addTemporalMarker(0.0, time, callback);
}
public TrajectorySequenceBuilder addTemporalMarker(double scale, double offset, MarkerCallback callback) {
return this.addTemporalMarker(time -> scale * time + offset, callback);
}
public TrajectorySequenceBuilder addTemporalMarker(TimeProducer time, MarkerCallback callback) {
this.temporalMarkers.add(new TemporalMarker(time, callback));
return this;
}
public TrajectorySequenceBuilder addSpatialMarker(Vector2d point, MarkerCallback callback) {
this.spatialMarkers.add(new SpatialMarker(point, callback));
return this;
}
public TrajectorySequenceBuilder addDisplacementMarker(MarkerCallback callback) {
return this.addDisplacementMarker(currentDisplacement, callback);
}
public TrajectorySequenceBuilder UNSTABLE_addDisplacementMarkerOffset(double offset, MarkerCallback callback) {
return this.addDisplacementMarker(currentDisplacement + offset, callback);
}
public TrajectorySequenceBuilder addDisplacementMarker(double displacement, MarkerCallback callback) {
return this.addDisplacementMarker(0.0, displacement, callback);
}
public TrajectorySequenceBuilder addDisplacementMarker(double scale, double offset, MarkerCallback callback) {
return addDisplacementMarker((displacement -> scale * displacement + offset), callback);
}
public TrajectorySequenceBuilder addDisplacementMarker(DisplacementProducer displacement, MarkerCallback callback) {
displacementMarkers.add(new DisplacementMarker(displacement, callback));
return this;
}
public TrajectorySequenceBuilder turn(double angle) {
return turn(angle, currentTurnConstraintMaxAngVel, currentTurnConstraintMaxAngAccel);
}
public TrajectorySequenceBuilder turn(double angle, double maxAngVel, double maxAngAccel) {
pushPath();
MotionProfile turnProfile = MotionProfileGenerator.generateSimpleMotionProfile(
new MotionState(lastPose.getHeading(), 0.0, 0.0, 0.0),
new MotionState(lastPose.getHeading() + angle, 0.0, 0.0, 0.0),
maxAngVel,
maxAngAccel
);
sequenceSegments.add(new TurnSegment(lastPose, angle, turnProfile, Collections.emptyList()));
lastPose = new Pose2d(
lastPose.getX(), lastPose.getY(),
Angle.norm(lastPose.getHeading() + angle)
);
currentDuration += turnProfile.duration();
return this;
}
public TrajectorySequenceBuilder waitSeconds(double seconds) {
pushPath();
sequenceSegments.add(new WaitSegment(lastPose, seconds, Collections.emptyList()));
currentDuration += seconds;
return this;
}
public TrajectorySequenceBuilder addTrajectory(Trajectory trajectory) {
pushPath();
sequenceSegments.add(new TrajectorySegment(trajectory));
return this;
}
private void pushPath() {
if (currentTrajectoryBuilder != null) {
Trajectory builtTraj = currentTrajectoryBuilder.build();
sequenceSegments.add(new TrajectorySegment(builtTraj));
}
currentTrajectoryBuilder = null;
}
private void newPath() {
if (currentTrajectoryBuilder != null)
pushPath();
lastDurationTraj = 0.0;
lastDisplacementTraj = 0.0;
double tangent = setAbsoluteTangent ? absoluteTangent : Angle.norm(lastPose.getHeading() + tangentOffset);
currentTrajectoryBuilder = new TrajectoryBuilder(lastPose, tangent, currentVelConstraint, currentAccelConstraint, resolution);
}
public TrajectorySequence build() {
pushPath();
List<TrajectoryMarker> globalMarkers = convertMarkersToGlobal(
sequenceSegments,
temporalMarkers, displacementMarkers, spatialMarkers
);
return new TrajectorySequence(projectGlobalMarkersToLocalSegments(globalMarkers, sequenceSegments));
}
private List<TrajectoryMarker> convertMarkersToGlobal(
List<SequenceSegment> sequenceSegments,
List<TemporalMarker> temporalMarkers,
List<DisplacementMarker> displacementMarkers,
List<SpatialMarker> spatialMarkers
) {
ArrayList<TrajectoryMarker> trajectoryMarkers = new ArrayList<>();
// Convert temporal markers
for (TemporalMarker marker : temporalMarkers) {
trajectoryMarkers.add(
new TrajectoryMarker(marker.getProducer().produce(currentDuration), marker.getCallback())
);
}
// Convert displacement markers
for (DisplacementMarker marker : displacementMarkers) {
double time = displacementToTime(
sequenceSegments,
marker.getProducer().produce(currentDisplacement)
);
trajectoryMarkers.add(
new TrajectoryMarker(
time,
marker.getCallback()
)
);
}
// Convert spatial markers
for (SpatialMarker marker : spatialMarkers) {
trajectoryMarkers.add(
new TrajectoryMarker(
pointToTime(sequenceSegments, marker.getPoint()),
marker.getCallback()
)
);
}
return trajectoryMarkers;
}
private List<SequenceSegment> projectGlobalMarkersToLocalSegments(List<TrajectoryMarker> markers, List<SequenceSegment> sequenceSegments) {
if (sequenceSegments.isEmpty()) return Collections.emptyList();
double totalSequenceDuration = 0;
for (SequenceSegment segment : sequenceSegments) {
totalSequenceDuration += segment.getDuration();
}
for (TrajectoryMarker marker : markers) {
SequenceSegment segment = null;
int segmentIndex = 0;
double segmentOffsetTime = 0;
double currentTime = 0;
for (int i = 0; i < sequenceSegments.size(); i++) {
SequenceSegment seg = sequenceSegments.get(i);
double markerTime = Math.min(marker.getTime(), totalSequenceDuration);
if (currentTime + seg.getDuration() >= markerTime) {
segment = seg;
segmentIndex = i;
segmentOffsetTime = markerTime - currentTime;
break;
} else {
currentTime += seg.getDuration();
}
}
SequenceSegment newSegment = null;
if (segment instanceof WaitSegment) {
List<TrajectoryMarker> newMarkers = new ArrayList<>(segment.getMarkers());
newMarkers.addAll(sequenceSegments.get(segmentIndex).getMarkers());
newMarkers.add(new TrajectoryMarker(segmentOffsetTime, marker.getCallback()));
WaitSegment thisSegment = (WaitSegment) segment;
newSegment = new WaitSegment(thisSegment.getStartPose(), thisSegment.getDuration(), newMarkers);
} else if (segment instanceof TurnSegment) {
List<TrajectoryMarker> newMarkers = new ArrayList<>(segment.getMarkers());
newMarkers.addAll(sequenceSegments.get(segmentIndex).getMarkers());
newMarkers.add(new TrajectoryMarker(segmentOffsetTime, marker.getCallback()));
TurnSegment thisSegment = (TurnSegment) segment;
newSegment = new TurnSegment(thisSegment.getStartPose(), thisSegment.getTotalRotation(), thisSegment.getMotionProfile(), newMarkers);
} else if (segment instanceof TrajectorySegment) {
TrajectorySegment thisSegment = (TrajectorySegment) segment;
List<TrajectoryMarker> newMarkers = new ArrayList<>(thisSegment.getTrajectory().getMarkers());
newMarkers.add(new TrajectoryMarker(segmentOffsetTime, marker.getCallback()));
newSegment = new TrajectorySegment(new Trajectory(thisSegment.getTrajectory().getPath(), thisSegment.getTrajectory().getProfile(), newMarkers));
}
sequenceSegments.set(segmentIndex, newSegment);
}
return sequenceSegments;
}
// Taken from Road Runner's TrajectoryGenerator.displacementToTime() since it's private
// note: this assumes that the profile position is monotonic increasing
private Double motionProfileDisplacementToTime(MotionProfile profile, double s) {
double tLo = 0.0;
double tHi = profile.duration();
while (!(Math.abs(tLo - tHi) < 1e-6)) {
double tMid = 0.5 * (tLo + tHi);
if (profile.get(tMid).getX() > s) {
tHi = tMid;
} else {
tLo = tMid;
}
}
return 0.5 * (tLo + tHi);
}
private Double displacementToTime(List<SequenceSegment> sequenceSegments, double s) {
double currentTime = 0.0;
double currentDisplacement = 0.0;
for (SequenceSegment segment : sequenceSegments) {
if (segment instanceof TrajectorySegment) {
TrajectorySegment thisSegment = (TrajectorySegment) segment;
double segmentLength = thisSegment.getTrajectory().getPath().length();
if (currentDisplacement + segmentLength > s) {
double target = s - currentDisplacement;
double timeInSegment = motionProfileDisplacementToTime(
thisSegment.getTrajectory().getProfile(),
target
);
return currentTime + timeInSegment;
} else {
currentDisplacement += segmentLength;
currentTime += thisSegment.getTrajectory().duration();
}
} else {
currentTime += segment.getDuration();
}
}
return 0.0;
}
private Double pointToTime(List<SequenceSegment> sequenceSegments, Vector2d point) {
class ComparingPoints {
private final double distanceToPoint;
private final double totalDisplacement;
private final double thisPathDisplacement;
public ComparingPoints(double distanceToPoint, double totalDisplacement, double thisPathDisplacement) {
this.distanceToPoint = distanceToPoint;
this.totalDisplacement = totalDisplacement;
this.thisPathDisplacement = thisPathDisplacement;
}
}
List<ComparingPoints> projectedPoints = new ArrayList<>();
for (SequenceSegment segment : sequenceSegments) {
if (segment instanceof TrajectorySegment) {
TrajectorySegment thisSegment = (TrajectorySegment) segment;
double displacement = thisSegment.getTrajectory().getPath().project(point, 0.25);
Vector2d projectedPoint = thisSegment.getTrajectory().getPath().get(displacement).vec();
double distanceToPoint = point.minus(projectedPoint).norm();
double totalDisplacement = 0.0;
for (ComparingPoints comparingPoint : projectedPoints) {
totalDisplacement += comparingPoint.totalDisplacement;
}
totalDisplacement += displacement;
projectedPoints.add(new ComparingPoints(distanceToPoint, displacement, totalDisplacement));
}
}
ComparingPoints closestPoint = null;
for (ComparingPoints comparingPoint : projectedPoints) {
if (closestPoint == null) {
closestPoint = comparingPoint;
continue;
}
if (comparingPoint.distanceToPoint < closestPoint.distanceToPoint)
closestPoint = comparingPoint;
}
return displacementToTime(sequenceSegments, closestPoint.thisPathDisplacement);
}
private interface AddPathCallback {
void run();
}
}

276
TeamCode/src/main/java/org/RoadRunner/trajectorysequence/TrajectorySequenceRunner.java
View File

@@ -1,276 +0,0 @@
package org.RoadRunner.trajectorysequence;
import androidx.annotation.Nullable;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.canvas.Canvas;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.control.PIDCoefficients;
import com.acmerobotics.roadrunner.control.PIDFController;
import com.acmerobotics.roadrunner.drive.DriveSignal;
import com.acmerobotics.roadrunner.followers.TrajectoryFollower;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.profile.MotionState;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
import com.acmerobotics.roadrunner.util.NanoClock;
import org.RoadRunner.trajectorysequence.sequencesegment.SequenceSegment;
import org.RoadRunner.trajectorysequence.sequencesegment.TrajectorySegment;
import org.RoadRunner.trajectorysequence.sequencesegment.TurnSegment;
import org.RoadRunner.trajectorysequence.sequencesegment.WaitSegment;
import org.RoadRunner.util.DashboardUtil;
import java.util.ArrayList;
import java.util.Collections;
import java.util.LinkedList;
import java.util.List;
@Config
public class TrajectorySequenceRunner {
public static String COLOR_INACTIVE_TRAJECTORY = "#4caf507a";
public static String COLOR_INACTIVE_TURN = "#7c4dff7a";
public static String COLOR_INACTIVE_WAIT = "#dd2c007a";
public static String COLOR_ACTIVE_TRAJECTORY = "#4CAF50";
public static String COLOR_ACTIVE_TURN = "#7c4dff";
public static String COLOR_ACTIVE_WAIT = "#dd2c00";
public static int POSE_HISTORY_LIMIT = 100;
private final TrajectoryFollower follower;
private final PIDFController turnController;
private final NanoClock clock;
private TrajectorySequence currentTrajectorySequence;
private double currentSegmentStartTime;
private int currentSegmentIndex;
private int lastSegmentIndex;
private Pose2d lastPoseError = new Pose2d();
List<TrajectoryMarker> remainingMarkers = new ArrayList<>();
private final FtcDashboard dashboard;
private final LinkedList<Pose2d> poseHistory = new LinkedList<>();
public TrajectorySequenceRunner(TrajectoryFollower follower, PIDCoefficients headingPIDCoefficients) {
this.follower = follower;
turnController = new PIDFController(headingPIDCoefficients);
turnController.setInputBounds(0, 2 * Math.PI);
clock = NanoClock.system();
dashboard = FtcDashboard.getInstance();
dashboard.setTelemetryTransmissionInterval(25);
}
// public void followTrajectorySequenceAsync(TrajectorySequence trajectorySequence) {
// currentTrajectorySequence = trajectorySequence;
// currentSegmentStartTime = clock.seconds();
// currentSegmentIndex = 0;
// lastSegmentIndex = -1;
// }
public @Nullable
DriveSignal update(Pose2d poseEstimate, Pose2d poseVelocity) {
Pose2d targetPose = null;
DriveSignal driveSignal = null;
TelemetryPacket packet = new TelemetryPacket();
Canvas fieldOverlay = packet.fieldOverlay();
SequenceSegment currentSegment = null;
if (currentTrajectorySequence != null) {
if (currentSegmentIndex >= currentTrajectorySequence.size()) {
for (TrajectoryMarker marker : remainingMarkers) {
marker.getCallback().onMarkerReached();
}
remainingMarkers.clear();
currentTrajectorySequence = null;
}
if (currentTrajectorySequence == null)
return new DriveSignal();
double now = clock.seconds();
boolean isNewTransition = currentSegmentIndex != lastSegmentIndex;
currentSegment = currentTrajectorySequence.get(currentSegmentIndex);
if (isNewTransition) {
currentSegmentStartTime = now;
lastSegmentIndex = currentSegmentIndex;
for (TrajectoryMarker marker : remainingMarkers) {
marker.getCallback().onMarkerReached();
}
remainingMarkers.clear();
remainingMarkers.addAll(currentSegment.getMarkers());
Collections.sort(remainingMarkers, (t1, t2) -> Double.compare(t1.getTime(), t2.getTime()));
}
double deltaTime = now - currentSegmentStartTime;
if (currentSegment instanceof TrajectorySegment) {
Trajectory currentTrajectory = ((TrajectorySegment) currentSegment).getTrajectory();
if (isNewTransition)
follower.followTrajectory(currentTrajectory);
if (!follower.isFollowing()) {
currentSegmentIndex++;
driveSignal = new DriveSignal();
} else {
driveSignal = follower.update(poseEstimate, poseVelocity);
lastPoseError = follower.getLastError();
}
targetPose = currentTrajectory.get(deltaTime);
} else if (currentSegment instanceof TurnSegment) {
MotionState targetState = ((TurnSegment) currentSegment).getMotionProfile().get(deltaTime);
turnController.setTargetPosition(targetState.getX());
double correction = turnController.update(poseEstimate.getHeading());
double targetOmega = targetState.getV();
double targetAlpha = targetState.getA();
lastPoseError = new Pose2d(0, 0, turnController.getLastError());
Pose2d startPose = currentSegment.getStartPose();
targetPose = startPose.copy(startPose.getX(), startPose.getY(), targetState.getX());
driveSignal = new DriveSignal(
new Pose2d(0, 0, targetOmega + correction),
new Pose2d(0, 0, targetAlpha)
);
if (deltaTime >= currentSegment.getDuration()) {
currentSegmentIndex++;
driveSignal = new DriveSignal();
}
} else if (currentSegment instanceof WaitSegment) {
lastPoseError = new Pose2d();
targetPose = currentSegment.getStartPose();
driveSignal = new DriveSignal();
if (deltaTime >= currentSegment.getDuration()) {
currentSegmentIndex++;
}
}
while (remainingMarkers.size() > 0 && deltaTime > remainingMarkers.get(0).getTime()) {
remainingMarkers.get(0).getCallback().onMarkerReached();
remainingMarkers.remove(0);
}
}
poseHistory.add(poseEstimate);
if (POSE_HISTORY_LIMIT > -1 && poseHistory.size() > POSE_HISTORY_LIMIT) {
poseHistory.removeFirst();
}
packet.put("x", poseEstimate.getX());
packet.put("y", poseEstimate.getY());
packet.put("heading (deg)", Math.toDegrees(poseEstimate.getHeading()));
packet.put("xError", getLastPoseError().getX());
packet.put("yError", getLastPoseError().getY());
packet.put("headingError (deg)", Math.toDegrees(getLastPoseError().getHeading()));
draw(fieldOverlay, currentTrajectorySequence, currentSegment, targetPose, poseEstimate);
dashboard.sendTelemetryPacket(packet);
return driveSignal;
}
private void draw(
Canvas fieldOverlay,
TrajectorySequence sequence, SequenceSegment currentSegment,
Pose2d targetPose, Pose2d poseEstimate
) {
if (sequence != null) {
for (int i = 0; i < sequence.size(); i++) {
SequenceSegment segment = sequence.get(i);
if (segment instanceof TrajectorySegment) {
fieldOverlay.setStrokeWidth(1);
fieldOverlay.setStroke(COLOR_INACTIVE_TRAJECTORY);
DashboardUtil.drawSampledPath(fieldOverlay, ((TrajectorySegment) segment).getTrajectory().getPath());
} else if (segment instanceof TurnSegment) {
Pose2d pose = segment.getStartPose();
fieldOverlay.setFill(COLOR_INACTIVE_TURN);
fieldOverlay.fillCircle(pose.getX(), pose.getY(), 2);
} else if (segment instanceof WaitSegment) {
Pose2d pose = segment.getStartPose();
fieldOverlay.setStrokeWidth(1);
fieldOverlay.setStroke(COLOR_INACTIVE_WAIT);
fieldOverlay.strokeCircle(pose.getX(), pose.getY(), 3);
}
}
}
if (currentSegment != null) {
if (currentSegment instanceof TrajectorySegment) {
Trajectory currentTrajectory = ((TrajectorySegment) currentSegment).getTrajectory();
fieldOverlay.setStrokeWidth(1);
fieldOverlay.setStroke(COLOR_ACTIVE_TRAJECTORY);
DashboardUtil.drawSampledPath(fieldOverlay, currentTrajectory.getPath());
} else if (currentSegment instanceof TurnSegment) {
Pose2d pose = currentSegment.getStartPose();
fieldOverlay.setFill(COLOR_ACTIVE_TURN);
fieldOverlay.fillCircle(pose.getX(), pose.getY(), 3);
} else if (currentSegment instanceof WaitSegment) {
Pose2d pose = currentSegment.getStartPose();
fieldOverlay.setStrokeWidth(1);
fieldOverlay.setStroke(COLOR_ACTIVE_WAIT);
fieldOverlay.strokeCircle(pose.getX(), pose.getY(), 3);
}
}
if (targetPose != null) {
fieldOverlay.setStrokeWidth(1);
fieldOverlay.setStroke("#4CAF50");
DashboardUtil.drawRobot(fieldOverlay, targetPose);
}
fieldOverlay.setStroke("#3F51B5");
DashboardUtil.drawPoseHistory(fieldOverlay, poseHistory);
fieldOverlay.setStroke("#3F51B5");
DashboardUtil.drawRobot(fieldOverlay, poseEstimate);
}
public Pose2d getLastPoseError() {
return lastPoseError;
}
public boolean isBusy() {
return currentTrajectorySequence != null;
}
public void followTrajectorySequenceAsync(TrajectorySequence trajectorySequence) {
}
}

40
TeamCode/src/main/java/org/RoadRunner/trajectorysequence/sequencesegment/SequenceSegment.java
View File

@@ -1,40 +0,0 @@
package org.RoadRunner.trajectorysequence.sequencesegment;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
import java.util.List;
public abstract class SequenceSegment {
private final double duration;
private final Pose2d startPose;
private final Pose2d endPose;
private final List<TrajectoryMarker> markers;
protected SequenceSegment(
double duration,
Pose2d startPose, Pose2d endPose,
List<TrajectoryMarker> markers
) {
this.duration = duration;
this.startPose = startPose;
this.endPose = endPose;
this.markers = markers;
}
public double getDuration() {
return this.duration;
}
public Pose2d getStartPose() {
return startPose;
}
public Pose2d getEndPose() {
return endPose;
}
public List<TrajectoryMarker> getMarkers() {
return markers;
}
}

20
TeamCode/src/main/java/org/RoadRunner/trajectorysequence/sequencesegment/TrajectorySegment.java
View File

@@ -1,20 +0,0 @@
package org.RoadRunner.trajectorysequence.sequencesegment;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import java.util.Collections;
public final class TrajectorySegment extends SequenceSegment {
private final Trajectory trajectory;
public TrajectorySegment(Trajectory trajectory) {
// Note: Markers are already stored in the `Trajectory` itself.
// This class should not hold any markers
super(trajectory.duration(), trajectory.start(), trajectory.end(), Collections.emptyList());
this.trajectory = trajectory;
}
public Trajectory getTrajectory() {
return this.trajectory;
}
}

36
TeamCode/src/main/java/org/RoadRunner/trajectorysequence/sequencesegment/TurnSegment.java
View File

@@ -1,36 +0,0 @@
package org.RoadRunner.trajectorysequence.sequencesegment;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.profile.MotionProfile;
import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
import com.acmerobotics.roadrunner.util.Angle;
import java.util.List;
public final class TurnSegment extends SequenceSegment {
private final double totalRotation;
private final MotionProfile motionProfile;
public TurnSegment(Pose2d startPose, double totalRotation, MotionProfile motionProfile, List<TrajectoryMarker> markers) {
super(
motionProfile.duration(),
startPose,
new Pose2d(
startPose.getX(), startPose.getY(),
Angle.norm(startPose.getHeading() + totalRotation)
),
markers
);
this.totalRotation = totalRotation;
this.motionProfile = motionProfile;
}
public final double getTotalRotation() {
return this.totalRotation;
}
public final MotionProfile getMotionProfile() {
return this.motionProfile;
}
}

12
TeamCode/src/main/java/org/RoadRunner/trajectorysequence/sequencesegment/WaitSegment.java
View File

@@ -1,12 +0,0 @@
package org.RoadRunner.trajectorysequence.sequencesegment;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
import java.util.List;
public final class WaitSegment extends SequenceSegment {
public WaitSegment(Pose2d pose, double seconds, List<TrajectoryMarker> markers) {
super(seconds, pose, pose, markers);
}
}

70
TeamCode/src/main/java/org/RoadRunner/util/AssetsTrajectoryManager.java
View File

@@ -1,70 +0,0 @@
package org.RoadRunner.util;
import androidx.annotation.Nullable;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
import com.acmerobotics.roadrunner.trajectory.config.TrajectoryConfig;
import com.acmerobotics.roadrunner.trajectory.config.TrajectoryConfigManager;
import com.acmerobotics.roadrunner.trajectory.config.TrajectoryGroupConfig;
import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
import java.io.IOException;
import java.io.InputStream;
/**
* Set of utilities for loading trajectories from assets (the plugin save location).
*/
public class AssetsTrajectoryManager {
/**
* Loads the group config.
*/
public static @Nullable
TrajectoryGroupConfig loadGroupConfig() {
try {
InputStream inputStream = AppUtil.getDefContext().getAssets().open(
"trajectory/" + TrajectoryConfigManager.GROUP_FILENAME);
return TrajectoryConfigManager.loadGroupConfig(inputStream);
} catch (IOException e) {
return null;
}
}
/**
* Loads a trajectory config with the given name.
*/
public static @Nullable TrajectoryConfig loadConfig(String name) {
try {
InputStream inputStream = AppUtil.getDefContext().getAssets().open(
"trajectory/" + name + ".yaml");
return TrajectoryConfigManager.loadConfig(inputStream);
} catch (IOException e) {
return null;
}
}
/**
* Loads a trajectory builder with the given name.
*/
public static @Nullable TrajectoryBuilder loadBuilder(String name) {
TrajectoryGroupConfig groupConfig = loadGroupConfig();
TrajectoryConfig config = loadConfig(name);
if (groupConfig == null || config == null) {
return null;
}
return config.toTrajectoryBuilder(groupConfig);
}
/**
* Loads a trajectory with the given name.
*/
public static @Nullable Trajectory load(String name) {
TrajectoryBuilder builder = loadBuilder(name);
if (builder == null) {
return null;
}
return builder.build();
}
}

45
TeamCode/src/main/java/org/RoadRunner/util/AxesSigns.java
View File

@@ -1,45 +0,0 @@
package org.RoadRunner.util;
/**
* IMU axes signs in the order XYZ (after remapping).
*/
public enum AxesSigns {
PPP(0b000),
PPN(0b001),
PNP(0b010),
PNN(0b011),
NPP(0b100),
NPN(0b101),
NNP(0b110),
NNN(0b111);
public final int bVal;
AxesSigns(int bVal) {
this.bVal = bVal;
}
public static AxesSigns fromBinaryValue(int bVal) {
int maskedVal = bVal & 0x07;
switch (maskedVal) {
case 0b000:
return AxesSigns.PPP;
case 0b001:
return AxesSigns.PPN;
case 0b010:
return AxesSigns.PNP;
case 0b011:
return AxesSigns.PNN;
case 0b100:
return AxesSigns.NPP;
case 0b101:
return AxesSigns.NPN;
case 0b110:
return AxesSigns.NNP;
case 0b111:
return AxesSigns.NNN;
default:
throw new IllegalStateException("Unexpected value for maskedVal: " + maskedVal);
}
}
}

8
TeamCode/src/main/java/org/RoadRunner/util/AxisDirection.java
View File

@@ -1,8 +0,0 @@
package org.RoadRunner.util;
/**
* A direction for an axis to be remapped to
*/
public enum AxisDirection {
POS_X, NEG_X, POS_Y, NEG_Y, POS_Z, NEG_Z
}

128
TeamCode/src/main/java/org/RoadRunner/util/BNO055IMUUtil.java
View File

@@ -1,128 +0,0 @@
package org.RoadRunner.util;
import com.qualcomm.hardware.bosch.BNO055IMU;
import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
/**
* Various utility functions for the BNO055 IMU.
*/
public class BNO055IMUUtil {
/**
* Error for attempting an illegal remapping (lhs or multiple same axes)
*/
public static class InvalidAxisRemapException extends RuntimeException {
public InvalidAxisRemapException(String detailMessage) {
super(detailMessage);
}
}
/**
* Remap BNO055 IMU axes and signs. For reference, the default order is {@link AxesOrder#XYZ}.
* Call after {@link BNO055IMU#initialize(BNO055IMU.Parameters)}. Although this isn't
* mentioned in the datasheet, the axes order appears to affect the onboard sensor fusion.
*
* Adapted from <a href="https://ftcforum.firstinspires.org/forum/ftc-technology/53812-mounting-the-revhub-vertically?p=56587#post56587">this post</a>.
* Reference the <a href="https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bno055-ds000.pdf">BNO055 Datasheet</a> for details.
*
* NOTE: Remapping axes can be somewhat confusing. Instead, use {@link #remapZAxis}, if appropriate.
*
* @param imu IMU
* @param order axes order
* @param signs axes signs
*/
public static void swapThenFlipAxes(BNO055IMU imu, AxesOrder order, AxesSigns signs) {
try {
// the indices correspond with the 2-bit axis encodings specified in the datasheet
int[] indices = order.indices();
// AxesSign's values align with the datasheet
int axisMapSigns = signs.bVal;
if (indices[0] == indices[1] || indices[0] == indices[2] || indices[1] == indices[2]) {
throw new InvalidAxisRemapException("Same axis cannot be included in AxesOrder twice");
}
// ensure right-handed coordinate system
boolean isXSwapped = indices[0] != 0;
boolean isYSwapped = indices[1] != 1;
boolean isZSwapped = indices[2] != 2;
boolean areTwoAxesSwapped = (isXSwapped || isYSwapped || isZSwapped)
&& (!isXSwapped || !isYSwapped || !isZSwapped);
boolean oddNumOfFlips = (((axisMapSigns >> 2) ^ (axisMapSigns >> 1) ^ axisMapSigns) & 1) == 1;
// != functions as xor
if (areTwoAxesSwapped != oddNumOfFlips) {
throw new InvalidAxisRemapException("Coordinate system is left-handed");
}
// Bit: 7 6 | 5 4 | 3 2 | 1 0 |
// reserved | z axis | y axis | x axis |
int axisMapConfig = indices[2] << 4 | indices[1] << 2 | indices[0];
// Enter CONFIG mode
imu.write8(BNO055IMU.Register.OPR_MODE, BNO055IMU.SensorMode.CONFIG.bVal & 0x0F);
Thread.sleep(100);
// Write the AXIS_MAP_CONFIG register
imu.write8(BNO055IMU.Register.AXIS_MAP_CONFIG, axisMapConfig & 0x3F);
// Write the AXIS_MAP_SIGN register
imu.write8(BNO055IMU.Register.AXIS_MAP_SIGN, axisMapSigns & 0x07);
// Switch back to the previous mode
imu.write8(BNO055IMU.Register.OPR_MODE, imu.getParameters().mode.bVal & 0x0F);
Thread.sleep(100);
} catch (InterruptedException e) {
Thread.currentThread().interrupt();
}
}
/**
* Remaps the IMU coordinate system so that the remapped +Z faces the provided
* {@link AxisDirection}. See {@link #swapThenFlipAxes} for details about the remapping.
*
* @param imu IMU
* @param direction axis direction
*/
public static void remapZAxis(BNO055IMU imu, AxisDirection direction) {
switch (direction) {
case POS_X:
swapThenFlipAxes(imu, AxesOrder.ZYX, AxesSigns.NPP);
break;
case NEG_X:
swapThenFlipAxes(imu, AxesOrder.ZYX, AxesSigns.PPN);
break;
case POS_Y:
swapThenFlipAxes(imu, AxesOrder.XZY, AxesSigns.PNP);
break;
case NEG_Y:
swapThenFlipAxes(imu, AxesOrder.XZY, AxesSigns.PPN);
break;
case POS_Z:
swapThenFlipAxes(imu, AxesOrder.XYZ, AxesSigns.PPP);
break;
case NEG_Z:
swapThenFlipAxes(imu, AxesOrder.XYZ, AxesSigns.PNN);
break;
}
}
/**
* Now deprecated due to unintuitive parameter order.
* Use {@link #swapThenFlipAxes} or {@link #remapZAxis} instead.
*
* @param imu IMU
* @param order axes order
* @param signs axes signs
*/
@Deprecated
public static void remapAxes(BNO055IMU imu, AxesOrder order, AxesSigns signs) {
AxesOrder adjustedAxesOrder = order.reverse();
int[] indices = order.indices();
int axisSignValue = signs.bVal ^ (0b100 >> indices[0]);
AxesSigns adjustedAxesSigns = AxesSigns.fromBinaryValue(axisSignValue);
swapThenFlipAxes(imu, adjustedAxesOrder, adjustedAxesSigns);
}
}

54
TeamCode/src/main/java/org/RoadRunner/util/DashboardUtil.java
View File

@@ -1,54 +0,0 @@
package org.RoadRunner.util;
import com.acmerobotics.dashboard.canvas.Canvas;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.acmerobotics.roadrunner.path.Path;
import java.util.List;
/**
* Set of helper functions for drawing Road Runner paths and trajectories on dashboard canvases.
*/
public class DashboardUtil {
private static final double DEFAULT_RESOLUTION = 2.0; // distance units; presumed inches
private static final double ROBOT_RADIUS = 9; // in
public static void drawPoseHistory(Canvas canvas, List<Pose2d> poseHistory) {
double[] xPoints = new double[poseHistory.size()];
double[] yPoints = new double[poseHistory.size()];
for (int i = 0; i < poseHistory.size(); i++) {
Pose2d pose = poseHistory.get(i);
xPoints[i] = pose.getX();
yPoints[i] = pose.getY();
}
canvas.strokePolyline(xPoints, yPoints);
}
public static void drawSampledPath(Canvas canvas, Path path, double resolution) {
int samples = (int) Math.ceil(path.length() / resolution);
double[] xPoints = new double[samples];
double[] yPoints = new double[samples];
double dx = path.length() / (samples - 1);
for (int i = 0; i < samples; i++) {
double displacement = i * dx;
Pose2d pose = path.get(displacement);
xPoints[i] = pose.getX();
yPoints[i] = pose.getY();
}
canvas.strokePolyline(xPoints, yPoints);
}
public static void drawSampledPath(Canvas canvas, Path path) {
drawSampledPath(canvas, path, DEFAULT_RESOLUTION);
}
public static void drawRobot(Canvas canvas, Pose2d pose) {
canvas.strokeCircle(pose.getX(), pose.getY(), ROBOT_RADIUS);
Vector2d v = pose.headingVec().times(ROBOT_RADIUS);
double x1 = pose.getX() + v.getX() / 2, y1 = pose.getY() + v.getY() / 2;
double x2 = pose.getX() + v.getX(), y2 = pose.getY() + v.getY();
canvas.strokeLine(x1, y1, x2, y2);
}
}

125
TeamCode/src/main/java/org/RoadRunner/util/Encoder.java
View File

@@ -1,125 +0,0 @@
package org.RoadRunner.util;
import com.acmerobotics.roadrunner.util.NanoClock;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
/**
* Wraps a motor instance to provide corrected velocity counts and allow reversing independently of the corresponding
* slot's motor direction
*/
public class Encoder {
private final static int CPS_STEP = 0x10000;
private static double inverseOverflow(double input, double estimate) {
// convert to uint16
int real = (int) input & 0xffff;
// initial, modulo-based correction: it can recover the remainder of 5 of the upper 16 bits
// because the velocity is always a multiple of 20 cps due to Expansion Hub's 50ms measurement window
real += ((real % 20) / 4) * CPS_STEP;
// estimate-based correction: it finds the nearest multiple of 5 to correct the upper bits by
real += Math.round((estimate - real) / (5 * CPS_STEP)) * 5 * CPS_STEP;
return real;
}
public enum Direction {
FORWARD(1),
REVERSE(-1);
private int multiplier;
Direction(int multiplier) {
this.multiplier = multiplier;
}
public int getMultiplier() {
return multiplier;
}
}
private DcMotorEx motor;
private NanoClock clock;
private Direction direction;
private int lastPosition;
private int velocityEstimateIdx;
private double[] velocityEstimates;
private double lastUpdateTime;
public Encoder(DcMotorEx motor, NanoClock clock) {
this.motor = motor;
this.clock = clock;
this.direction = Direction.FORWARD;
this.lastPosition = 0;
this.velocityEstimates = new double[3];
this.lastUpdateTime = clock.seconds();
}
public Encoder(DcMotorEx motor) {
this(motor, NanoClock.system());
}
public Direction getDirection() {
return direction;
}
private int getMultiplier() {
return getDirection().getMultiplier() * (motor.getDirection() == DcMotorSimple.Direction.FORWARD ? 1 : -1);
}
/**
* Allows you to set the direction of the counts and velocity without modifying the motor's direction state
* @param direction either reverse or forward depending on if encoder counts should be negated
*/
public void setDirection(Direction direction) {
this.direction = direction;
}
/**
* Gets the position from the underlying motor and adjusts for the set direction.
* Additionally, this method updates the velocity estimates used for compensated velocity
*
* @return encoder position
*/
public int getCurrentPosition() {
int multiplier = getMultiplier();
int currentPosition = motor.getCurrentPosition() * multiplier;
if (currentPosition != lastPosition) {
double currentTime = clock.seconds();
double dt = currentTime - lastUpdateTime;
velocityEstimates[velocityEstimateIdx] = (currentPosition - lastPosition) / dt;
velocityEstimateIdx = (velocityEstimateIdx + 1) % 3;
lastPosition = currentPosition;
lastUpdateTime = currentTime;
}
return currentPosition;
}
/**
* Gets the velocity directly from the underlying motor and compensates for the direction
* See {@link #getCorrectedVelocity} for high (>2^15) counts per second velocities (such as on REV Through Bore)
*
* @return raw velocity
*/
public double getRawVelocity() {
int multiplier = getMultiplier();
return motor.getVelocity() * multiplier;
}
/**
* Uses velocity estimates gathered in {@link #getCurrentPosition} to estimate the upper bits of velocity
* that are lost in overflow due to velocity being transmitted as 16 bits.
* CAVEAT: must regularly call {@link #getCurrentPosition} for the compensation to work correctly.
*
* @return corrected velocity
*/
public double getCorrectedVelocity() {
double median = velocityEstimates[0] > velocityEstimates[1]
? Math.max(velocityEstimates[1], Math.min(velocityEstimates[0], velocityEstimates[2]))
: Math.max(velocityEstimates[0], Math.min(velocityEstimates[1], velocityEstimates[2]));
return inverseOverflow(getRawVelocity(), median);
}
}

60
TeamCode/src/main/java/org/RoadRunner/util/LoggingUtil.java
View File

@@ -1,60 +0,0 @@
package org.RoadRunner.util;
import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
import java.io.File;
import java.util.ArrayList;
import java.util.Collections;
import java.util.List;
/**
* Utility functions for log files.
*/
public class LoggingUtil {
public static final File ROAD_RUNNER_FOLDER =
new File(AppUtil.ROOT_FOLDER + "/RoadRunner/");
private static final long LOG_QUOTA = 25 * 1024 * 1024; // 25MB log quota for now
private static void buildLogList(List<File> logFiles, File dir) {
for (File file : dir.listFiles()) {
if (file.isDirectory()) {
buildLogList(logFiles, file);
} else {
logFiles.add(file);
}
}
}
private static void pruneLogsIfNecessary() {
List<File> logFiles = new ArrayList<>();
buildLogList(logFiles, ROAD_RUNNER_FOLDER);
Collections.sort(logFiles, (lhs, rhs) ->
Long.compare(lhs.lastModified(), rhs.lastModified()));
long dirSize = 0;
for (File file: logFiles) {
dirSize += file.length();
}
while (dirSize > LOG_QUOTA) {
if (logFiles.size() == 0) break;
File fileToRemove = logFiles.remove(0);
dirSize -= fileToRemove.length();
//noinspection ResultOfMethodCallIgnored
fileToRemove.delete();
}
}
/**
* Obtain a log file with the provided name
*/
public static File getLogFile(String name) {
//noinspection ResultOfMethodCallIgnored
ROAD_RUNNER_FOLDER.mkdirs();
pruneLogsIfNecessary();
return new File(ROAD_RUNNER_FOLDER, name);
}
}

124
TeamCode/src/main/java/org/RoadRunner/util/LynxModuleUtil.java
View File

@@ -1,124 +0,0 @@
package org.RoadRunner.util;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.robotcore.internal.system.Misc;
import java.util.HashMap;
import java.util.Map;
/**
* Collection of utilites for interacting with Lynx modules.
*/
public class LynxModuleUtil {
private static final LynxFirmwareVersion MIN_VERSION = new LynxFirmwareVersion(1, 8, 2);
/**
* Parsed representation of a Lynx module firmware version.
*/
public static class LynxFirmwareVersion implements Comparable<LynxFirmwareVersion> {
public final int major;
public final int minor;
public final int eng;
public LynxFirmwareVersion(int major, int minor, int eng) {
this.major = major;
this.minor = minor;
this.eng = eng;
}
@Override
public boolean equals(Object other) {
if (other instanceof LynxFirmwareVersion) {
LynxFirmwareVersion otherVersion = (LynxFirmwareVersion) other;
return major == otherVersion.major && minor == otherVersion.minor &&
eng == otherVersion.eng;
} else {
return false;
}
}
@Override
public int compareTo(LynxFirmwareVersion other) {
int majorComp = Integer.compare(major, other.major);
if (majorComp == 0) {
int minorComp = Integer.compare(minor, other.minor);
if (minorComp == 0) {
return Integer.compare(eng, other.eng);
} else {
return minorComp;
}
} else {
return majorComp;
}
}
@Override
public String toString() {
return Misc.formatInvariant("%d.%d.%d", major, minor, eng);
}
}
/**
* Retrieve and parse Lynx module firmware version.
* @param module Lynx module
* @return parsed firmware version
*/
public static LynxFirmwareVersion getFirmwareVersion(LynxModule module) {
String versionString = module.getNullableFirmwareVersionString();
if (versionString == null) {
return null;
}
String[] parts = versionString.split("[ :,]+");
try {
// note: for now, we ignore the hardware entry
return new LynxFirmwareVersion(
Integer.parseInt(parts[3]),
Integer.parseInt(parts[5]),
Integer.parseInt(parts[7])
);
} catch (NumberFormatException e) {
return null;
}
}
/**
* Exception indicating an outdated Lynx firmware version.
*/
public static class LynxFirmwareVersionException extends RuntimeException {
public LynxFirmwareVersionException(String detailMessage) {
super(detailMessage);
}
}
/**
* Ensure all of the Lynx modules attached to the robot satisfy the minimum requirement.
* @param hardwareMap hardware map containing Lynx modules
*/
public static void ensureMinimumFirmwareVersion(HardwareMap hardwareMap) {
Map<String, LynxFirmwareVersion> outdatedModules = new HashMap<>();
for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
LynxFirmwareVersion version = getFirmwareVersion(module);
if (version == null || version.compareTo(MIN_VERSION) < 0) {
for (String name : hardwareMap.getNamesOf(module)) {
outdatedModules.put(name, version);
}
}
}
if (outdatedModules.size() > 0) {
StringBuilder msgBuilder = new StringBuilder();
msgBuilder.append("One or more of the attached Lynx modules has outdated firmware\n");
msgBuilder.append(Misc.formatInvariant("Mandatory minimum firmware version for Road Runner: %s\n",
MIN_VERSION.toString()));
for (Map.Entry<String, LynxFirmwareVersion> entry : outdatedModules.entrySet()) {
msgBuilder.append(Misc.formatInvariant(
"\t%s: %s\n", entry.getKey(),
entry.getValue() == null ? "Unknown" : entry.getValue().toString()));
}
throw new LynxFirmwareVersionException(msgBuilder.toString());
}
}
}

156
TeamCode/src/main/java/org/RoadRunner/util/RegressionUtil.java
View File

@@ -1,156 +0,0 @@
package org.RoadRunner.util;
import androidx.annotation.Nullable;
import com.acmerobotics.roadrunner.kinematics.Kinematics;
import org.apache.commons.math3.stat.regression.SimpleRegression;
import java.io.File;
import java.io.FileNotFoundException;
import java.io.PrintWriter;
import java.util.ArrayList;
import java.util.List;
/**
* Various regression utilities.
*/
public class RegressionUtil {
/**
* Feedforward parameter estimates from the ramp regression and additional summary statistics
*/
public static class RampResult {
public final double kV, kStatic, rSquare;
public RampResult(double kV, double kStatic, double rSquare) {
this.kV = kV;
this.kStatic = kStatic;
this.rSquare = rSquare;
}
}
/**
* Feedforward parameter estimates from the ramp regression and additional summary statistics
*/
public static class AccelResult {
public final double kA, rSquare;
public AccelResult(double kA, double rSquare) {
this.kA = kA;
this.rSquare = rSquare;
}
}
/**
* Numerically compute dy/dx from the given x and y values. The returned list is padded to match
* the length of the original sequences.
*
* @param x x-values
* @param y y-values
* @return derivative values
*/
private static List<Double> numericalDerivative(List<Double> x, List<Double> y) {
List<Double> deriv = new ArrayList<>(x.size());
for (int i = 1; i < x.size() - 1; i++) {
deriv.add(
(y.get(i + 1) - y.get(i - 1)) /
(x.get(i + 1) - x.get(i - 1))
);
}
// copy endpoints to pad output
deriv.add(0, deriv.get(0));
deriv.add(deriv.get(deriv.size() - 1));
return deriv;
}
/**
* Run regression to compute velocity and static feedforward from ramp test data.
*
* Here's the general procedure for gathering the requisite data:
* 1. Slowly ramp the motor power/voltage and record encoder values along the way.
* 2. Run a linear regression on the encoder velocity vs. motor power plot to obtain a slope
* (kV) and an optional intercept (kStatic).
*
* @param timeSamples time samples
* @param positionSamples position samples
* @param powerSamples power samples
* @param fitStatic fit kStatic
* @param file log file
*/
public static RampResult fitRampData(List<Double> timeSamples, List<Double> positionSamples,
List<Double> powerSamples, boolean fitStatic,
@Nullable File file) {
if (file != null) {
try (PrintWriter pw = new PrintWriter(file)) {
pw.println("time,position,power");
for (int i = 0; i < timeSamples.size(); i++) {
double time = timeSamples.get(i);
double pos = positionSamples.get(i);
double power = powerSamples.get(i);
pw.println(time + "," + pos + "," + power);
}
} catch (FileNotFoundException e) {
// ignore
}
}
List<Double> velSamples = numericalDerivative(timeSamples, positionSamples);
SimpleRegression rampReg = new SimpleRegression(fitStatic);
for (int i = 0; i < timeSamples.size(); i++) {
double vel = velSamples.get(i);
double power = powerSamples.get(i);
rampReg.addData(vel, power);
}
return new RampResult(Math.abs(rampReg.getSlope()), Math.abs(rampReg.getIntercept()),
rampReg.getRSquare());
}
/**
* Run regression to compute acceleration feedforward.
*
* @param timeSamples time samples
* @param positionSamples position samples
* @param powerSamples power samples
* @param rampResult ramp result
* @param file log file
*/
public static AccelResult fitAccelData(List<Double> timeSamples, List<Double> positionSamples,
List<Double> powerSamples, RampResult rampResult,
@Nullable File file) {
if (file != null) {
try (PrintWriter pw = new PrintWriter(file)) {
pw.println("time,position,power");
for (int i = 0; i < timeSamples.size(); i++) {
double time = timeSamples.get(i);
double pos = positionSamples.get(i);
double power = powerSamples.get(i);
pw.println(time + "," + pos + "," + power);
}
} catch (FileNotFoundException e) {
// ignore
}
}
List<Double> velSamples = numericalDerivative(timeSamples, positionSamples);
List<Double> accelSamples = numericalDerivative(timeSamples, velSamples);
SimpleRegression accelReg = new SimpleRegression(false);
for (int i = 0; i < timeSamples.size(); i++) {
double vel = velSamples.get(i);
double accel = accelSamples.get(i);
double power = powerSamples.get(i);
double powerFromVel = Kinematics.calculateMotorFeedforward(
vel, 0.0, rampResult.kV, 0.0, rampResult.kStatic);
double powerFromAccel = power - powerFromVel;
accelReg.addData(accel, powerFromAccel);
}
return new AccelResult(Math.abs(accelReg.getSlope()), accelReg.getRSquare());
}
}

2
TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java
View File

@@ -15,7 +15,7 @@ import org.timecrafters.Autonomous.States.ServoCameraRotate;
import org.timecrafters.Autonomous.States.TopArm;
import org.timecrafters.TeleOp.states.PhoenixBot1;
@Autonomous (name = "Right Side")
@Autonomous (name = "Right ")
@Disabled
public class RightSideAutonomousEngine extends CyberarmEngine {

51
TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightStateAutoEngine.java Normal file
View File

@@ -0,0 +1,51 @@
package org.timecrafters.Autonomous.Engines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.Autonomous.States.BottomArm;
import org.timecrafters.Autonomous.States.CollectorDistanceState;
import org.timecrafters.Autonomous.States.CollectorState;
import org.timecrafters.Autonomous.States.ConeIdentification;
import org.timecrafters.Autonomous.States.DriverParkPlaceState;
import org.timecrafters.Autonomous.States.DriverStateWithOdometer;
import org.timecrafters.Autonomous.States.PathDecision;
import org.timecrafters.Autonomous.States.RotationState;
import org.timecrafters.Autonomous.States.ServoCameraRotate;
import org.timecrafters.Autonomous.States.TopArm;
import org.timecrafters.TeleOp.states.PhoenixBot1;
@Autonomous(name = "Right Side")
public class RightStateAutoEngine extends CyberarmEngine {
PhoenixBot1 robot;
@Override
public void setup() {
robot = new PhoenixBot1(this);
// driving towards Low
addState(new DriverStateWithOdometer(robot, "Right State Auto", "02-0"));
// rotate towards low
addState(new RotationState(robot, "Right State Auto", "02-1"));
// drive forwards or backwards to adjust to pole
addState(new DriverStateWithOdometer(robot, "Right State Auto", "03-0"));
// counteract distance driven
addState(new DriverStateWithOdometer(robot, "Right State Auto", "04-0"));
// rotate towards opposing alliance
addState(new RotationState(robot, "Right State Auto", "04-1"));
// drive to stack column
addState(new DriverStateWithOdometer(robot, "Right State Auto", "05-0"));
// rotate at stack
addState(new RotationState(robot, "Right State Auto", "05-1"));
// drive at stack
// addState(new DriverStateWithOdometer(robot, "Right State Auto", "06-0"));
}
}

79
TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java
View File

@@ -12,10 +12,13 @@ public class DriverStateWithOdometer extends CyberarmState {
PhoenixBot1 robot;
private double RampUpDistance;
private double RampDownDistance;
private int maximumTolerance;
private double maximumTolerance;
private float direction;
private boolean targetAchieved = false;
private double CurrentPosition;
public final double WHEEL_CIRCUMFERENCE = 7.42108499;
public final int COUNTS_PER_REVOLUTION = 8192;
public final double distanceMultiplier;
public DriverStateWithOdometer(PhoenixBot1 robot, String groupName, String actionName) {
this.robot = robot;
this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value();
@@ -24,6 +27,7 @@ public class DriverStateWithOdometer extends CyberarmState {
this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value();
this.maximumTolerance = robot.configuration.variable(groupName, actionName, "maximumTolerance").value();
this.direction = robot.configuration.variable(groupName, actionName, "direction").value();
this.distanceMultiplier = robot.configuration.variable(groupName, actionName, "distanceMultiplier").value();
this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
}
@@ -32,12 +36,6 @@ public class DriverStateWithOdometer extends CyberarmState {
@Override
public void start() {
robot.frontRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.frontLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.OdometerEncoderRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.OdometerEncoderLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.frontRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
@@ -46,6 +44,11 @@ public class DriverStateWithOdometer extends CyberarmState {
robot.OdometerEncoderRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.OdometerEncoderLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
traveledDistance = (int) ((traveledDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * distanceMultiplier);
RampUpDistance = (int) ((RampUpDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * distanceMultiplier);
RampDownDistance = (int) ((RampDownDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * distanceMultiplier);
maximumTolerance = (int) ((maximumTolerance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * distanceMultiplier);
}
@@ -96,7 +99,7 @@ public class DriverStateWithOdometer extends CyberarmState {
drivePower = drivePower * -1;
}
if (Math.abs(CurrentPosition) < traveledDistance - maximumTolerance){
if (Math.abs(LeftCurrentPosition) < traveledDistance - maximumTolerance || Math.abs(RightCurrentPosition) < traveledDistance - maximumTolerance){
if (targetAchieved) {
drivePower = drivePower * 0.25;
@@ -108,13 +111,13 @@ public class DriverStateWithOdometer extends CyberarmState {
}
}
}
robot.backLeftDrive.setPower(drivePower);
robot.backLeftDrive.setPower(drivePower * robot.VEER_COMPENSATION_DBL);
robot.backRightDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(drivePower * robot.VEER_COMPENSATION_DBL);
robot.frontRightDrive.setPower(drivePower);
}
else if (Math.abs(CurrentPosition) > traveledDistance + maximumTolerance) {
else if (Math.abs(LeftCurrentPosition) > traveledDistance + maximumTolerance || Math.abs(RightCurrentPosition) > traveledDistance + maximumTolerance) {
targetAchieved = true;
drivePower = targetDrivePower * -0.25;
@@ -133,33 +136,47 @@ public class DriverStateWithOdometer extends CyberarmState {
robot.frontRightDrive.setPower(drivePower);
} else {
robot.backLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.frontRightDrive.setPower(0);
setHasFinished(true);
}
if (!getHasFinished() && !targetAchieved){
float angle = robot.imu.getAngularOrientation().firstAngle - direction;
else {
if (targetDrivePower < 0) { angle = angle * -1;}
if (Math.abs(LeftCurrentPosition) > Math.abs(RightCurrentPosition)){
if (angle < -0.25){
robot.backLeftDrive.setPower(drivePower * 0);
robot.backRightDrive.setPower(drivePower * 1.25);
robot.frontLeftDrive.setPower(drivePower * 0);
robot.frontRightDrive.setPower(drivePower * 1.25);
if (Math.abs(Math.abs(LeftCurrentPosition) - Math.abs(RightCurrentPosition)) < 20) {
drivePower = 0;
} else {
drivePower = 0.25;
robot.backLeftDrive.setPower(-drivePower);
robot.backRightDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(-drivePower);
robot.frontRightDrive.setPower(drivePower);
}
}
if (angle > 0.25) {
robot.backLeftDrive.setPower(drivePower * 1.25);
robot.backRightDrive.setPower(drivePower * 0);
robot.frontLeftDrive.setPower(drivePower * 1.25);
robot.frontRightDrive.setPower(drivePower * 0);
if (Math.abs(LeftCurrentPosition) < Math.abs(RightCurrentPosition)){
if (Math.abs(LeftCurrentPosition) == Math.abs(RightCurrentPosition)){
drivePower = 0;
} else {
drivePower = 0.25;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
}
else {
robot.backLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.frontRightDrive.setPower(0);
setHasFinished(true);
}
}
}
//
}
@Override
public void telemetry() {

10
TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java
View File

@@ -23,7 +23,15 @@ public class RotationState extends CyberarmState {
private double RotationDirectionMinimum;
private String debugStatus = "?";
private double drivePowerVariable;
private double leftCompensator;
private double RightCompensator;
@Override
public void start() {
leftCompensator = robot.OdometerEncoderLeft.getCurrentPosition();
RightCompensator = robot.OdometerEncoderRight.getCurrentPosition();
}
@Override
public void exec() {
@@ -67,6 +75,8 @@ public class RotationState extends CyberarmState {
robot.backRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.frontRightDrive.setPower(0);
PhoenixBot1.leftCompensatorGlobal = (leftCompensator + robot.OdometerEncoderLeft.getCurrentPosition()) - leftCompensator;
PhoenixBot1.RightCompensatorGlobal = (RightCompensator + robot.OdometerEncoderRight.getCurrentPosition()) - RightCompensator;
setHasFinished(true);
}

12
TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java
View File

@@ -16,6 +16,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
import org.timecrafters.minibots.cyberarm.chiron.Robot;
import org.timecrafters.minibots.cyberarm.chiron.states.autonomous.Arm;
public class PhoenixBot1 {
@@ -23,6 +24,9 @@ public class PhoenixBot1 {
private static final float mmPerInch = 25.4f;
public static final double WHEEL_CIRCUMFERENCE = 7.42108499;
public static final int COUNTS_PER_REVOLUTION = 8192;
public static double leftCompensatorGlobal;
public static double RightCompensatorGlobal;
public double VEER_COMPENSATION_DBL; // some place around 1, .99 is 1% power reduction
// private static final String TFOD_MODEL_ASSET = "22-23_PowerPlay_Colors.tflite";
private static final String TFOD_MODEL_ASSET = "AprilTagsV1.tflite";
@@ -69,11 +73,19 @@ public class PhoenixBot1 {
public PhoenixBot1(CyberarmEngine engine) {
this.engine = engine;
configuration = new TimeCraftersConfiguration();
initConstants();
initVuforia();
initTfod();
setupRobot();
}
public void initConstants(){
VEER_COMPENSATION_DBL = configuration.variable("Robot", "Tuning", "VEER_COMPENSATION_DBL").value();
}
private void initVuforia(){
/*
* Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine.