diff --git a/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/MeepMeepTesting.java b/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/MeepMeepTesting.java index 08fbc8d..adfeabd 100644 --- a/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/MeepMeepTesting.java +++ b/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/MeepMeepTesting.java @@ -1,6 +1,9 @@ package com.example.meepmeeptesting; import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; +import com.acmerobotics.roadrunner.trajectory.constraints.MecanumVelocityConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.ProfileAccelerationConstraint; import com.noahbres.meepmeep.MeepMeep; import com.noahbres.meepmeep.roadrunner.DefaultBotBuilder; import com.noahbres.meepmeep.roadrunner.entity.RoadRunnerBotEntity; @@ -14,15 +17,15 @@ public static void main(String[] args) { .setConstraints(60, 60, Math.toRadians(180), Math.toRadians(180), 15) .followTrajectorySequence(drive -> // add your trajectory here: - drive.trajectorySequenceBuilder(new Pose2d(0, 0, 0)) - .forward(30) - .turn(Math.toRadians(90)) - .forward(30) - .turn(Math.toRadians(90)) - .forward(30) - .turn(Math.toRadians(90)) - .forward(30) - .turn(Math.toRadians(90)) + drive.trajectorySequenceBuilder(new Pose2d(10, -60, -Math.PI/2)) + .lineToLinearHeading(new Pose2d(15, -40, Math.toRadians(225))) + .addTemporalMarker(() -> {}) + .waitSeconds(0.5) + .lineTo(new Vector2d(25, -55)) + .lineToLinearHeading(new Pose2d(40, -35, 0)) + .lineToLinearHeading(new Pose2d(50, -40)) + .addTemporalMarker(() -> {}) + .waitSeconds(0.5) .build() ); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autos/DefaultGil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autos/DefaultGil.java index 2f22ded..3daa670 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autos/DefaultGil.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autos/DefaultGil.java @@ -17,7 +17,9 @@ public void runOpMode() throws InterruptedException { } public abstract Alliance getAlliance(); + public abstract FieldStartingLocation getFieldStartingLocation(); + public abstract void run() throws InterruptedException; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/DriveConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/DriveConstants.java index dc04254..90fe47a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/DriveConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/DriveConstants.java @@ -20,8 +20,8 @@ public class DriveConstants { /* * These are motor constants that should be listed online for your motors. */ - public static final double TICKS_PER_REV = 1; - public static final double MAX_RPM = 1; + public static final double TICKS_PER_REVOLUTION = 537.7; + public static final double MAX_RPM = 312; /* * Set RUN_USING_ENCODER to true to enable built-in hub velocity control using drive encoders. @@ -31,9 +31,9 @@ public class DriveConstants { * 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)); + public static final boolean RUN_USING_ENCODER = false; + public static PIDFCoefficients MOTOR_VELOCITY_PID = new PIDFCoefficients(1, 0, 0, + getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REVOLUTION)); /* * These are physical constants that can be determined from your robot (including the track @@ -43,9 +43,9 @@ public class DriveConstants { * 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 = 2; // in - public static double GEAR_RATIO = 1; // output (wheel) speed / input (motor) speed - public static double TRACK_WIDTH = 1; // in + public static double WHEEL_RADIUS = 1.8; // in + public static double GEAR_RATIO = 1.05; // output (wheel) speed / input (motor) speed + public static double TRACK_WIDTH = 325; // in /* * These are the feedforward parameters used to model the drive motor behavior. If you are using @@ -53,9 +53,9 @@ public class DriveConstants { * 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; + public static double kV = 0.00095; + public static double kA = 0.0003; + public static double kStatic = -0.07; /* * These values are used to generate the trajectories for you robot. To ensure proper operation, @@ -64,14 +64,16 @@ public class DriveConstants { * small and gradually increase them later after everything is working. All distance units are * inches. */ - public static double MAX_VEL = 30; - public static double MAX_ACCEL = 30; - public static double MAX_ANG_VEL = Math.toRadians(60); - public static double MAX_ANG_ACCEL = Math.toRadians(60); + public static double MAX_VELOCITY = 900; + public static double MAX_ACCELERATION = 500; + public static double MAX_ANGULAR_VELOCITY = 18.016340020937598; + public static double MAX_ANGULAR_ACCELERATION = Math.toRadians(60); + + public static double MAX_BATTERY_VOLTAGE = 12; public static double encoderTicksToInches(double ticks) { - return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV; + return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REVOLUTION; } public static double rpmToVelocity(double rpm) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/SampleMecanumDrive.java index 9539f1f..bdbb3ec 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/SampleMecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/SampleMecanumDrive.java @@ -21,6 +21,7 @@ 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; @@ -29,17 +30,21 @@ import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequence; import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder; import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceRunner; +import org.firstinspires.ftc.teamcode.roadrunner.util.AxisDirection; +import org.firstinspires.ftc.teamcode.roadrunner.util.BNO055IMUUtil; import org.firstinspires.ftc.teamcode.roadrunner.util.LynxModuleUtil; +import org.firstinspires.ftc.teamcode.subsystems.chassis.ChassisConstants; import java.util.ArrayList; import java.util.Arrays; import java.util.List; -import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.MAX_ACCEL; -import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.MAX_ANG_ACCEL; -import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.MAX_ANG_VEL; -import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.MAX_VEL; -import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.MOTOR_VELO_PID; +import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.MAX_ACCELERATION; +import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.MAX_ANGULAR_ACCELERATION; +import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.MAX_ANGULAR_VELOCITY; +import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.MAX_BATTERY_VOLTAGE; +import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.MAX_VELOCITY; +import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.MOTOR_VELOCITY_PID; import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.RUN_USING_ENCODER; import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.TRACK_WIDTH; import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.encoderTicksToInches; @@ -53,10 +58,10 @@ @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 PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(65, 0, 0); + public static PIDCoefficients HEADING_PID = new PIDCoefficients(0.25, 0, 0); - public static double LATERAL_MULTIPLIER = 1; + public static double LATERAL_MULTIPLIER = 1.0856210671340743708609271523179; public static double VX_WEIGHT = 1; public static double VY_WEIGHT = 1; @@ -64,8 +69,10 @@ public class SampleMecanumDrive extends MecanumDrive { 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 static final TrajectoryVelocityConstraint VEL_CONSTRAINT = getVelocityConstraint(MAX_VELOCITY, MAX_ANGULAR_VELOCITY, TRACK_WIDTH); + private static final TrajectoryAccelerationConstraint ACCEL_CONSTRAINT = getAccelerationConstraint(MAX_ACCELERATION); + + private static final double ACHIEVABLE_MAX_MOTOR_RPM_FRACTION = 1.0; private TrajectoryFollower follower; @@ -94,8 +101,9 @@ public SampleMecanumDrive(HardwareMap hardwareMap) { BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); parameters.angleUnit = BNO055IMU.AngleUnit.RADIANS; imu.initialize(parameters); + BNO055IMUUtil.remapZAxis(imu, AxisDirection.NEG_Y); - // TODO: If the hub containing the IMU you are using is mounted so that the "REV" logo does + // 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 @@ -117,16 +125,16 @@ public SampleMecanumDrive(HardwareMap hardwareMap) { // 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, "leftFront"); - leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); - rightRear = hardwareMap.get(DcMotorEx.class, "rightRear"); - rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); + leftFront = hardwareMap.get(DcMotorEx.class, ChassisConstants.FRONT_LEFT_ID); + leftRear = hardwareMap.get(DcMotorEx.class, ChassisConstants.BACK_LEFT_ID); + rightRear = hardwareMap.get(DcMotorEx.class, ChassisConstants.BACK_RIGHT_ID); + rightFront = hardwareMap.get(DcMotorEx.class, ChassisConstants.FRONT_RIGHT_ID); motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront); for (DcMotorEx motor : motors) { MotorConfigurationType motorConfigurationType = motor.getMotorType().clone(); - motorConfigurationType.setAchieveableMaxRPMFraction(1.0); + motorConfigurationType.setAchieveableMaxRPMFraction(ACHIEVABLE_MAX_MOTOR_RPM_FRACTION); motor.setMotorType(motorConfigurationType); } @@ -136,11 +144,13 @@ public SampleMecanumDrive(HardwareMap hardwareMap) { setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - if (RUN_USING_ENCODER && MOTOR_VELO_PID != null) { - setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID); + if (RUN_USING_ENCODER && MOTOR_VELOCITY_PID != null) { + setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELOCITY_PID); } - // TODO: reverse any motors using DcMotor.setDirection() + // reverse any motors using DcMotor.setDirection() + rightFront.setDirection(DcMotorSimple.Direction.REVERSE); + rightRear.setDirection(DcMotorSimple.Direction.REVERSE); // TODO: if desired, use setLocalizer() to change the localization method // for instance, setLocalizer(new ThreeTrackingWheelLocalizer(...)); @@ -164,7 +174,7 @@ public TrajectorySequenceBuilder trajectorySequenceBuilder(Pose2d startPose) { return new TrajectorySequenceBuilder( startPose, VEL_CONSTRAINT, ACCEL_CONSTRAINT, - MAX_ANG_VEL, MAX_ANG_ACCEL + MAX_ANGULAR_VELOCITY, MAX_ANGULAR_ACCELERATION ); } @@ -237,7 +247,7 @@ public void setZeroPowerBehavior(DcMotor.ZeroPowerBehavior zeroPowerBehavior) { public void setPIDFCoefficients(DcMotor.RunMode runMode, PIDFCoefficients coefficients) { PIDFCoefficients compensatedCoefficients = new PIDFCoefficients( coefficients.p, coefficients.i, coefficients.d, - coefficients.f * 12 / batteryVoltageSensor.getVoltage() + coefficients.f * MAX_BATTERY_VOLTAGE / batteryVoltageSensor.getVoltage() ); for (DcMotorEx motor : motors) { @@ -304,7 +314,7 @@ public Double getExternalHeadingVelocity() { // expected). This bug does NOT affect orientation. // // See https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/251 for details. - return (double) -imu.getAngularVelocity().xRotationRate; + return (double) imu.getAngularVelocity().yRotationRate; } public static TrajectoryVelocityConstraint getVelocityConstraint(double maxVel, double maxAngularVel, double trackWidth) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/SampleTankDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/SampleTankDrive.java index 4b526bc..fc06f56 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/SampleTankDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/SampleTankDrive.java @@ -34,11 +34,11 @@ import java.util.Arrays; import java.util.List; -import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.MAX_ACCEL; -import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.MAX_ANG_ACCEL; -import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.MAX_ANG_VEL; -import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.MAX_VEL; -import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.MOTOR_VELO_PID; +import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.MAX_ACCELERATION; +import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.MAX_ANGULAR_ACCELERATION; +import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.MAX_ANGULAR_VELOCITY; +import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.MAX_VELOCITY; +import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.MOTOR_VELOCITY_PID; import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.RUN_USING_ENCODER; import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.TRACK_WIDTH; import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.encoderTicksToInches; @@ -61,8 +61,8 @@ public class SampleTankDrive extends TankDrive { 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 static final TrajectoryVelocityConstraint VEL_CONSTRAINT = getVelocityConstraint(MAX_VELOCITY, MAX_ANGULAR_VELOCITY, TRACK_WIDTH); + private static final TrajectoryAccelerationConstraint accelConstraint = getAccelerationConstraint(MAX_ACCELERATION); private TrajectoryFollower follower; @@ -135,8 +135,8 @@ public SampleTankDrive(HardwareMap hardwareMap) { setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - if (RUN_USING_ENCODER && MOTOR_VELO_PID != null) { - setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID); + if (RUN_USING_ENCODER && MOTOR_VELOCITY_PID != null) { + setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELOCITY_PID); } // TODO: reverse any motors using DcMotor.setDirection() @@ -163,7 +163,7 @@ public TrajectorySequenceBuilder trajectorySequenceBuilder(Pose2d startPose) { return new TrajectorySequenceBuilder( startPose, VEL_CONSTRAINT, accelConstraint, - MAX_ANG_VEL, MAX_ANG_ACCEL + MAX_ANGULAR_VELOCITY, MAX_ANGULAR_ACCELERATION ); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/AutomaticFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/AutomaticFeedforwardTuner.java index 4ab9d98..84a98ef 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/AutomaticFeedforwardTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/AutomaticFeedforwardTuner.java @@ -6,6 +6,7 @@ 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.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.util.RobotLog; @@ -28,6 +29,7 @@ * 4. Adjust the encoder data based on the velocity tuning data and find kA with another linear * regression. */ +@Disabled @Config @Autonomous(group = "drive") public class AutomaticFeedforwardTuner extends LinearOpMode { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/BackAndForth.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/BackAndForth.java index d60f125..a2d3fb6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/BackAndForth.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/BackAndForth.java @@ -4,6 +4,7 @@ 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.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; @@ -24,6 +25,7 @@ * 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 { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/DriveVelocityPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/DriveVelocityPIDTuner.java index fc4ae0c..798e3ab 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/DriveVelocityPIDTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/DriveVelocityPIDTuner.java @@ -9,6 +9,7 @@ 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.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.util.RobotLog; @@ -42,6 +43,7 @@ * 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. */ +@Disabled @Config @Autonomous(group = "drive") public class DriveVelocityPIDTuner extends LinearOpMode { @@ -56,7 +58,7 @@ enum 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, DriveConstants.MAX_VEL, DriveConstants.MAX_ACCEL); + return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, DriveConstants.MAX_VELOCITY, DriveConstants.MAX_ACCELERATION); } @Override @@ -72,12 +74,12 @@ public void runOpMode() { Mode mode = Mode.TUNING_MODE; - double lastKp = DriveConstants.MOTOR_VELO_PID.p; - double lastKi = DriveConstants.MOTOR_VELO_PID.i; - double lastKd = DriveConstants.MOTOR_VELO_PID.d; - double lastKf = DriveConstants.MOTOR_VELO_PID.f; + double lastKp = DriveConstants.MOTOR_VELOCITY_PID.p; + double lastKi = DriveConstants.MOTOR_VELOCITY_PID.i; + double lastKd = DriveConstants.MOTOR_VELOCITY_PID.d; + double lastKf = DriveConstants.MOTOR_VELOCITY_PID.f; - drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, DriveConstants.MOTOR_VELO_PID); + drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, DriveConstants.MOTOR_VELOCITY_PID); NanoClock clock = NanoClock.system(); @@ -150,14 +152,14 @@ public void runOpMode() { break; } - if (lastKp != DriveConstants.MOTOR_VELO_PID.p || lastKd != DriveConstants.MOTOR_VELO_PID.d - || lastKi != DriveConstants.MOTOR_VELO_PID.i || lastKf != DriveConstants.MOTOR_VELO_PID.f) { - drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, DriveConstants.MOTOR_VELO_PID); + if (lastKp != DriveConstants.MOTOR_VELOCITY_PID.p || lastKd != DriveConstants.MOTOR_VELOCITY_PID.d + || lastKi != DriveConstants.MOTOR_VELOCITY_PID.i || lastKf != DriveConstants.MOTOR_VELOCITY_PID.f) { + drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, DriveConstants.MOTOR_VELOCITY_PID); - lastKp = DriveConstants.MOTOR_VELO_PID.p; - lastKi = DriveConstants.MOTOR_VELO_PID.i; - lastKd = DriveConstants.MOTOR_VELO_PID.d; - lastKf = DriveConstants.MOTOR_VELO_PID.f; + lastKp = DriveConstants.MOTOR_VELOCITY_PID.p; + lastKi = DriveConstants.MOTOR_VELOCITY_PID.i; + lastKd = DriveConstants.MOTOR_VELOCITY_PID.d; + lastKf = DriveConstants.MOTOR_VELOCITY_PID.f; } telemetry.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/FollowerPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/FollowerPIDTuner.java index ad75e5f..0dec553 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/FollowerPIDTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/FollowerPIDTuner.java @@ -3,6 +3,7 @@ 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.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; @@ -21,6 +22,7 @@ * 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. */ +@Disabled @Config @Autonomous(group = "drive") public class FollowerPIDTuner extends LinearOpMode { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/LocalizationTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/LocalizationTest.java index a669224..246397f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/LocalizationTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/LocalizationTest.java @@ -1,6 +1,7 @@ package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode; import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; @@ -14,6 +15,7 @@ * 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). */ +@Disabled @TeleOp(group = "drive") public class LocalizationTest extends LinearOpMode { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/ManualFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/ManualFeedforwardTuner.java index 4f41ce3..d29f5bd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/ManualFeedforwardTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/ManualFeedforwardTuner.java @@ -10,7 +10,9 @@ 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.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.VoltageSensor; import com.qualcomm.robotcore.util.RobotLog; import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; @@ -33,6 +35,7 @@ * 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. */ +@Disabled @Config @Autonomous(group = "drive") public class ManualFeedforwardTuner extends LinearOpMode { @@ -53,11 +56,12 @@ enum 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, DriveConstants.MAX_VEL, DriveConstants.MAX_ACCEL); + return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, DriveConstants.MAX_VELOCITY, DriveConstants.MAX_ACCELERATION); } @Override public void runOpMode() { + VoltageSensor voltageSensor = hardwareMap.voltageSensor.iterator().next(); if (DriveConstants.RUN_USING_ENCODER) { RobotLog.setGlobalErrorMsg("Feedforward constants usually don't need to be tuned " + "when using the built-in drive motor velocity PID."); @@ -106,7 +110,7 @@ public void runOpMode() { MotionState motionState = activeProfile.get(profileTime); double targetPower = Kinematics.calculateMotorFeedforward(motionState.getV(), motionState.getA(), DriveConstants.kV, DriveConstants.kA, DriveConstants.kStatic); - drive.setDrivePower(new Pose2d(targetPower, 0, 0)); + drive.setDrivePower(new Pose2d(targetPower * DriveConstants.MAX_BATTERY_VOLTAGE / voltageSensor.getVoltage(), 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."); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/MaxAngularVeloTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/MaxAngularVeloTuner.java index 35ac969..c2af7b5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/MaxAngularVeloTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/MaxAngularVeloTuner.java @@ -5,6 +5,7 @@ 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.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.util.ElapsedTime; @@ -21,6 +22,7 @@ * Further fine tuning of MAX_ANG_VEL may be desired. */ +@Disabled @Config @Autonomous(group = "drive") public class MaxAngularVeloTuner extends LinearOpMode { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/MaxVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/MaxVelocityTuner.java index 7542e35..3ce375f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/MaxVelocityTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/MaxVelocityTuner.java @@ -5,6 +5,7 @@ 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.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.VoltageSensor; @@ -23,6 +24,7 @@ *

* Further fine tuning of kF may be desired. */ +@Disabled @Config @Autonomous(group = "drive") public class MaxVelocityTuner extends LinearOpMode { @@ -78,7 +80,7 @@ public void runOpMode() throws InterruptedException { } private double veloInchesToTicks(double inchesPerSec) { - return inchesPerSec / (2 * Math.PI * DriveConstants.WHEEL_RADIUS) / DriveConstants.GEAR_RATIO * DriveConstants.TICKS_PER_REV; + return inchesPerSec / (2 * Math.PI * DriveConstants.WHEEL_RADIUS) / DriveConstants.GEAR_RATIO * DriveConstants.TICKS_PER_REVOLUTION; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/SplineTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/SplineTest.java index 130c59e..4f68667 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/SplineTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/SplineTest.java @@ -4,6 +4,7 @@ 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.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/StrafeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/StrafeTest.java index b9bf5d6..d3ff2a9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/StrafeTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/StrafeTest.java @@ -6,6 +6,7 @@ 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.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; @@ -13,6 +14,7 @@ /* * This is a simple routine to test translational drive capabilities. */ +@Disabled @Config @Autonomous(group = "drive") public class StrafeTest extends LinearOpMode { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/StraightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/StraightTest.java index 69617d3..7bcc46b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/StraightTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/StraightTest.java @@ -6,6 +6,7 @@ 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.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; @@ -13,6 +14,7 @@ /* * This is a simple routine to test translational drive capabilities. */ +@Disabled @Config @Autonomous(group = "drive") public class StraightTest extends LinearOpMode { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/TrackWidthTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/TrackWidthTuner.java index 0e23cd0..f37d17a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/TrackWidthTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/TrackWidthTuner.java @@ -6,6 +6,7 @@ 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.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.util.MovingStatistics; @@ -22,6 +23,7 @@ * 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. */ +@Disabled @Config @Autonomous(group = "drive") public class TrackWidthTuner extends LinearOpMode { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/TrackingWheelForwardOffsetTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/TrackingWheelForwardOffsetTuner.java index 2a27911..0f62e99 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/TrackingWheelForwardOffsetTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/TrackingWheelForwardOffsetTuner.java @@ -6,6 +6,7 @@ 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.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.util.MovingStatistics; import com.qualcomm.robotcore.util.RobotLog; @@ -33,6 +34,7 @@ * for the forward offset. You can run this procedure as many times as necessary until a * satisfactory result is produced. */ +@Disabled @Config @Autonomous(group="drive") public class TrackingWheelForwardOffsetTuner extends LinearOpMode { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/TrackingWheelLateralDistanceTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/TrackingWheelLateralDistanceTuner.java index fba1908..ec04c9b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/TrackingWheelLateralDistanceTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/TrackingWheelLateralDistanceTuner.java @@ -3,6 +3,7 @@ import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.util.Angle; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.util.RobotLog; @@ -61,6 +62,7 @@ * slightly but your heading will still be fine. This does not affect your overall tracking * precision. The heading should still line up. */ +@Disabled @Config @TeleOp(group = "drive") public class TrackingWheelLateralDistanceTuner extends LinearOpMode { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/TurnTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/TurnTest.java index 3648f44..45ebd7d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/TurnTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/TurnTest.java @@ -2,6 +2,7 @@ import com.acmerobotics.dashboard.config.Config; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; @@ -9,6 +10,7 @@ /* * This is a simple routine to test turning capabilities. */ +@Disabled @Config @Autonomous(group = "drive") public class TurnTest extends LinearOpMode {