diff --git a/TeamCode/src/main/java/org/RoadRunner/drive/DriveConstants.java b/TeamCode/src/main/java/org/RoadRunner/drive/DriveConstants.java deleted file mode 100644 index c41e660..0000000 --- a/TeamCode/src/main/java/org/RoadRunner/drive/DriveConstants.java +++ /dev/null @@ -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; - } -} diff --git a/TeamCode/src/main/java/org/RoadRunner/drive/SampleMecanumDrive.java b/TeamCode/src/main/java/org/RoadRunner/drive/SampleMecanumDrive.java deleted file mode 100644 index d0fd65c..0000000 --- a/TeamCode/src/main/java/org/RoadRunner/drive/SampleMecanumDrive.java +++ /dev/null @@ -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 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 getWheelPositions() { - List wheelPositions = new ArrayList<>(); - for (DcMotorEx motor : motors) { - wheelPositions.add(encoderTicksToInches(motor.getCurrentPosition())); - } - return wheelPositions; - } - - @Override - public List getWheelVelocities() { - List 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); - } -} diff --git a/TeamCode/src/main/java/org/RoadRunner/drive/SampleTankDrive.java b/TeamCode/src/main/java/org/RoadRunner/drive/SampleTankDrive.java deleted file mode 100644 index 6fe31de..0000000 --- a/TeamCode/src/main/java/org/RoadRunner/drive/SampleTankDrive.java +++ /dev/null @@ -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 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 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 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); - } -} diff --git a/TeamCode/src/main/java/org/RoadRunner/drive/StandardTrackingWheelLocalizer.java b/TeamCode/src/main/java/org/RoadRunner/drive/StandardTrackingWheelLocalizer.java deleted file mode 100644 index 7c8bb3c..0000000 --- a/TeamCode/src/main/java/org/RoadRunner/drive/StandardTrackingWheelLocalizer.java +++ /dev/null @@ -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 getWheelPositions() { - return Arrays.asList( - encoderTicksToInches(leftEncoder.getCurrentPosition()) * X_MULTIPLIER, - encoderTicksToInches(rightEncoder.getCurrentPosition()) * X_MULTIPLIER, - encoderTicksToInches(frontEncoder.getCurrentPosition()) * Y_MULTIPLIER - ); - } - - @NonNull - @Override - public List 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()); - } - -} diff --git a/TeamCode/src/main/java/org/RoadRunner/drive/opmode/AutomaticFeedforwardTuner.java b/TeamCode/src/main/java/org/RoadRunner/drive/opmode/AutomaticFeedforwardTuner.java deleted file mode 100644 index a14dcf7..0000000 --- a/TeamCode/src/main/java/org/RoadRunner/drive/opmode/AutomaticFeedforwardTuner.java +++ /dev/null @@ -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 timeSamples = new ArrayList<>(); - List positionSamples = new ArrayList<>(); - List 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(); - } - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/RoadRunner/drive/opmode/BackAndForth.java b/TeamCode/src/main/java/org/RoadRunner/drive/opmode/BackAndForth.java deleted file mode 100644 index e4f32bb..0000000 --- a/TeamCode/src/main/java/org/RoadRunner/drive/opmode/BackAndForth.java +++ /dev/null @@ -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); - } - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/RoadRunner/drive/opmode/DriveVelocityPIDTuner.java b/TeamCode/src/main/java/org/RoadRunner/drive/opmode/DriveVelocityPIDTuner.java deleted file mode 100644 index 4237424..0000000 --- a/TeamCode/src/main/java/org/RoadRunner/drive/opmode/DriveVelocityPIDTuner.java +++ /dev/null @@ -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 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(); - } - } -} diff --git a/TeamCode/src/main/java/org/RoadRunner/drive/opmode/FollowerPIDTuner.java b/TeamCode/src/main/java/org/RoadRunner/drive/opmode/FollowerPIDTuner.java deleted file mode 100644 index 08d535f..0000000 --- a/TeamCode/src/main/java/org/RoadRunner/drive/opmode/FollowerPIDTuner.java +++ /dev/null @@ -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); - } - } -} diff --git a/TeamCode/src/main/java/org/RoadRunner/drive/opmode/LocalizationTest.java b/TeamCode/src/main/java/org/RoadRunner/drive/opmode/LocalizationTest.java deleted file mode 100644 index 2af1af5..0000000 --- a/TeamCode/src/main/java/org/RoadRunner/drive/opmode/LocalizationTest.java +++ /dev/null @@ -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(); - } - } -} diff --git a/TeamCode/src/main/java/org/RoadRunner/drive/opmode/ManualFeedforwardTuner.java b/TeamCode/src/main/java/org/RoadRunner/drive/opmode/ManualFeedforwardTuner.java deleted file mode 100644 index 8bef31b..0000000 --- a/TeamCode/src/main/java/org/RoadRunner/drive/opmode/ManualFeedforwardTuner.java +++ /dev/null @@ -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(); - } - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/RoadRunner/drive/opmode/MaxAngularVeloTuner.java b/TeamCode/src/main/java/org/RoadRunner/drive/opmode/MaxAngularVeloTuner.java deleted file mode 100644 index 50a8e14..0000000 --- a/TeamCode/src/main/java/org/RoadRunner/drive/opmode/MaxAngularVeloTuner.java +++ /dev/null @@ -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. - *

- * Upon pressing start, your bot will turn at max power for RUNTIME seconds. - *

- * 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(); - } -} diff --git a/TeamCode/src/main/java/org/RoadRunner/drive/opmode/MaxVelocityTuner.java b/TeamCode/src/main/java/org/RoadRunner/drive/opmode/MaxVelocityTuner.java deleted file mode 100644 index 46509ac..0000000 --- a/TeamCode/src/main/java/org/RoadRunner/drive/opmode/MaxVelocityTuner.java +++ /dev/null @@ -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. - *

- * Upon pressing start, your bot will run at max power for RUNTIME seconds. - *

- * 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; - } -} diff --git a/TeamCode/src/main/java/org/RoadRunner/drive/opmode/MotorDirectionDebugger.java b/TeamCode/src/main/java/org/RoadRunner/drive/opmode/MotorDirectionDebugger.java deleted file mode 100644 index 2d53b0e..0000000 --- a/TeamCode/src/main/java/org/RoadRunner/drive/opmode/MotorDirectionDebugger.java +++ /dev/null @@ -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("Xbox/PS4 Button - Motor"); - telemetry.addLine("  X / ▢         - Front Left"); - telemetry.addLine("  Y / Δ         - Front Right"); - telemetry.addLine("  B / O         - Rear  Right"); - telemetry.addLine("  A / X         - Rear  Left"); - 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(); - } - } -} diff --git a/TeamCode/src/main/java/org/RoadRunner/drive/opmode/SplineTest.java b/TeamCode/src/main/java/org/RoadRunner/drive/opmode/SplineTest.java deleted file mode 100644 index 7494fa6..0000000 --- a/TeamCode/src/main/java/org/RoadRunner/drive/opmode/SplineTest.java +++ /dev/null @@ -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() - ); - } -} diff --git a/TeamCode/src/main/java/org/RoadRunner/drive/opmode/StrafeTest.java b/TeamCode/src/main/java/org/RoadRunner/drive/opmode/StrafeTest.java deleted file mode 100644 index dde530b..0000000 --- a/TeamCode/src/main/java/org/RoadRunner/drive/opmode/StrafeTest.java +++ /dev/null @@ -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()) ; - } -} diff --git a/TeamCode/src/main/java/org/RoadRunner/drive/opmode/StraightTest.java b/TeamCode/src/main/java/org/RoadRunner/drive/opmode/StraightTest.java deleted file mode 100644 index 8e0b0ca..0000000 --- a/TeamCode/src/main/java/org/RoadRunner/drive/opmode/StraightTest.java +++ /dev/null @@ -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()) ; - } -} diff --git a/TeamCode/src/main/java/org/RoadRunner/drive/opmode/TrackWidthTuner.java b/TeamCode/src/main/java/org/RoadRunner/drive/opmode/TrackWidthTuner.java deleted file mode 100644 index 9c3f0e2..0000000 --- a/TeamCode/src/main/java/org/RoadRunner/drive/opmode/TrackWidthTuner.java +++ /dev/null @@ -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(); - } - } -} diff --git a/TeamCode/src/main/java/org/RoadRunner/drive/opmode/TrackingWheelForwardOffsetTuner.java b/TeamCode/src/main/java/org/RoadRunner/drive/opmode/TrackingWheelForwardOffsetTuner.java deleted file mode 100644 index 7d649dd..0000000 --- a/TeamCode/src/main/java/org/RoadRunner/drive/opmode/TrackingWheelForwardOffsetTuner.java +++ /dev/null @@ -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(); - } - } -} diff --git a/TeamCode/src/main/java/org/RoadRunner/drive/opmode/TrackingWheelLateralDistanceTuner.java b/TeamCode/src/main/java/org/RoadRunner/drive/opmode/TrackingWheelLateralDistanceTuner.java deleted file mode 100644 index 56a9021..0000000 --- a/TeamCode/src/main/java/org/RoadRunner/drive/opmode/TrackingWheelLateralDistanceTuner.java +++ /dev/null @@ -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(); - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/RoadRunner/drive/opmode/TurnTest.java b/TeamCode/src/main/java/org/RoadRunner/drive/opmode/TurnTest.java deleted file mode 100644 index 66cd571..0000000 --- a/TeamCode/src/main/java/org/RoadRunner/drive/opmode/TurnTest.java +++ /dev/null @@ -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)); - } -} diff --git a/TeamCode/src/main/java/org/RoadRunner/trajectorysequence/EmptySequenceException.java b/TeamCode/src/main/java/org/RoadRunner/trajectorysequence/EmptySequenceException.java deleted file mode 100644 index 04ba85d..0000000 --- a/TeamCode/src/main/java/org/RoadRunner/trajectorysequence/EmptySequenceException.java +++ /dev/null @@ -1,4 +0,0 @@ -package org.RoadRunner.trajectorysequence; - - -public class EmptySequenceException extends RuntimeException { } diff --git a/TeamCode/src/main/java/org/RoadRunner/trajectorysequence/TrajectorySequence.java b/TeamCode/src/main/java/org/RoadRunner/trajectorysequence/TrajectorySequence.java deleted file mode 100644 index 7609952..0000000 --- a/TeamCode/src/main/java/org/RoadRunner/trajectorysequence/TrajectorySequence.java +++ /dev/null @@ -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 sequenceList; - - public TrajectorySequence(List 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(); - } -} diff --git a/TeamCode/src/main/java/org/RoadRunner/trajectorysequence/TrajectorySequenceBuilder.java b/TeamCode/src/main/java/org/RoadRunner/trajectorysequence/TrajectorySequenceBuilder.java deleted file mode 100644 index 61f44ad..0000000 --- a/TeamCode/src/main/java/org/RoadRunner/trajectorysequence/TrajectorySequenceBuilder.java +++ /dev/null @@ -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 sequenceSegments; - - private final List temporalMarkers; - private final List displacementMarkers; - private final List 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 globalMarkers = convertMarkersToGlobal( - sequenceSegments, - temporalMarkers, displacementMarkers, spatialMarkers - ); - - return new TrajectorySequence(projectGlobalMarkersToLocalSegments(globalMarkers, sequenceSegments)); - } - - private List convertMarkersToGlobal( - List sequenceSegments, - List temporalMarkers, - List displacementMarkers, - List spatialMarkers - ) { - ArrayList 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 projectGlobalMarkersToLocalSegments(List markers, List 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 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 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 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 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 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 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(); - } -} diff --git a/TeamCode/src/main/java/org/RoadRunner/trajectorysequence/TrajectorySequenceRunner.java b/TeamCode/src/main/java/org/RoadRunner/trajectorysequence/TrajectorySequenceRunner.java deleted file mode 100644 index 0a5a023..0000000 --- a/TeamCode/src/main/java/org/RoadRunner/trajectorysequence/TrajectorySequenceRunner.java +++ /dev/null @@ -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 remainingMarkers = new ArrayList<>(); - - private final FtcDashboard dashboard; - private final LinkedList 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) { - } -} diff --git a/TeamCode/src/main/java/org/RoadRunner/trajectorysequence/sequencesegment/SequenceSegment.java b/TeamCode/src/main/java/org/RoadRunner/trajectorysequence/sequencesegment/SequenceSegment.java deleted file mode 100644 index 59dc8d9..0000000 --- a/TeamCode/src/main/java/org/RoadRunner/trajectorysequence/sequencesegment/SequenceSegment.java +++ /dev/null @@ -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 markers; - - protected SequenceSegment( - double duration, - Pose2d startPose, Pose2d endPose, - List 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 getMarkers() { - return markers; - } -} diff --git a/TeamCode/src/main/java/org/RoadRunner/trajectorysequence/sequencesegment/TrajectorySegment.java b/TeamCode/src/main/java/org/RoadRunner/trajectorysequence/sequencesegment/TrajectorySegment.java deleted file mode 100644 index dd424f8..0000000 --- a/TeamCode/src/main/java/org/RoadRunner/trajectorysequence/sequencesegment/TrajectorySegment.java +++ /dev/null @@ -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; - } -} diff --git a/TeamCode/src/main/java/org/RoadRunner/trajectorysequence/sequencesegment/TurnSegment.java b/TeamCode/src/main/java/org/RoadRunner/trajectorysequence/sequencesegment/TurnSegment.java deleted file mode 100644 index 1eb2e6c..0000000 --- a/TeamCode/src/main/java/org/RoadRunner/trajectorysequence/sequencesegment/TurnSegment.java +++ /dev/null @@ -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 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; - } -} diff --git a/TeamCode/src/main/java/org/RoadRunner/trajectorysequence/sequencesegment/WaitSegment.java b/TeamCode/src/main/java/org/RoadRunner/trajectorysequence/sequencesegment/WaitSegment.java deleted file mode 100644 index 9ed6e53..0000000 --- a/TeamCode/src/main/java/org/RoadRunner/trajectorysequence/sequencesegment/WaitSegment.java +++ /dev/null @@ -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 markers) { - super(seconds, pose, pose, markers); - } -} diff --git a/TeamCode/src/main/java/org/RoadRunner/util/AssetsTrajectoryManager.java b/TeamCode/src/main/java/org/RoadRunner/util/AssetsTrajectoryManager.java deleted file mode 100644 index 85b3e42..0000000 --- a/TeamCode/src/main/java/org/RoadRunner/util/AssetsTrajectoryManager.java +++ /dev/null @@ -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(); - } -} diff --git a/TeamCode/src/main/java/org/RoadRunner/util/AxesSigns.java b/TeamCode/src/main/java/org/RoadRunner/util/AxesSigns.java deleted file mode 100644 index 51ff239..0000000 --- a/TeamCode/src/main/java/org/RoadRunner/util/AxesSigns.java +++ /dev/null @@ -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); - } - } -} diff --git a/TeamCode/src/main/java/org/RoadRunner/util/AxisDirection.java b/TeamCode/src/main/java/org/RoadRunner/util/AxisDirection.java deleted file mode 100644 index 9d21ca1..0000000 --- a/TeamCode/src/main/java/org/RoadRunner/util/AxisDirection.java +++ /dev/null @@ -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 -} diff --git a/TeamCode/src/main/java/org/RoadRunner/util/BNO055IMUUtil.java b/TeamCode/src/main/java/org/RoadRunner/util/BNO055IMUUtil.java deleted file mode 100644 index 660de56..0000000 --- a/TeamCode/src/main/java/org/RoadRunner/util/BNO055IMUUtil.java +++ /dev/null @@ -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 this post. - * Reference the BNO055 Datasheet 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); - } -} diff --git a/TeamCode/src/main/java/org/RoadRunner/util/DashboardUtil.java b/TeamCode/src/main/java/org/RoadRunner/util/DashboardUtil.java deleted file mode 100644 index 7a14c6c..0000000 --- a/TeamCode/src/main/java/org/RoadRunner/util/DashboardUtil.java +++ /dev/null @@ -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 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); - } -} diff --git a/TeamCode/src/main/java/org/RoadRunner/util/Encoder.java b/TeamCode/src/main/java/org/RoadRunner/util/Encoder.java deleted file mode 100644 index a1873cc..0000000 --- a/TeamCode/src/main/java/org/RoadRunner/util/Encoder.java +++ /dev/null @@ -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); - } -} diff --git a/TeamCode/src/main/java/org/RoadRunner/util/LoggingUtil.java b/TeamCode/src/main/java/org/RoadRunner/util/LoggingUtil.java deleted file mode 100644 index 896fc4f..0000000 --- a/TeamCode/src/main/java/org/RoadRunner/util/LoggingUtil.java +++ /dev/null @@ -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 logFiles, File dir) { - for (File file : dir.listFiles()) { - if (file.isDirectory()) { - buildLogList(logFiles, file); - } else { - logFiles.add(file); - } - } - } - - private static void pruneLogsIfNecessary() { - List 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); - } -} diff --git a/TeamCode/src/main/java/org/RoadRunner/util/LynxModuleUtil.java b/TeamCode/src/main/java/org/RoadRunner/util/LynxModuleUtil.java deleted file mode 100644 index a2dd8b6..0000000 --- a/TeamCode/src/main/java/org/RoadRunner/util/LynxModuleUtil.java +++ /dev/null @@ -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 { - 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 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 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()); - } - } -} diff --git a/TeamCode/src/main/java/org/RoadRunner/util/RegressionUtil.java b/TeamCode/src/main/java/org/RoadRunner/util/RegressionUtil.java deleted file mode 100644 index 97d9b64..0000000 --- a/TeamCode/src/main/java/org/RoadRunner/util/RegressionUtil.java +++ /dev/null @@ -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 numericalDerivative(List x, List y) { - List 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 timeSamples, List positionSamples, - List 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 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 timeSamples, List positionSamples, - List 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 velSamples = numericalDerivative(timeSamples, positionSamples); - List 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()); - } -} diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java index e2c0254..f956beb 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java @@ -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 { diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightStateAutoEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightStateAutoEngine.java new file mode 100644 index 0000000..d9de5b5 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightStateAutoEngine.java @@ -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")); + + + + + + + + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java index 0dc4255..7e0f482 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java @@ -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() { diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java index 86ac710..39a15da 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java @@ -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); } diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java index 9ec5bf0..5a6866d 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java @@ -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.