From 39f95053b7f8b9368cdbba38e37d3bc8c3ce047c Mon Sep 17 00:00:00 2001 From: hillelv Date: Mon, 17 Jun 2024 15:39:18 +0300 Subject: [PATCH 01/22] hillel - naming --- .../roadrunner/drive/DriveConstants.java | 16 ++++++------ .../roadrunner/drive/SampleMecanumDrive.java | 23 +++++++++------- .../roadrunner/drive/SampleTankDrive.java | 20 +++++++------- .../drive/opmode/DriveVelocityPIDTuner.java | 26 +++++++++---------- .../drive/opmode/ManualFeedforwardTuner.java | 2 +- .../drive/opmode/MaxVelocityTuner.java | 2 +- 6 files changed, 46 insertions(+), 43 deletions(-) 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 4ba53e9..e421e95 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,7 +20,7 @@ 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 TICKS_PER_REVOLUTION = 1; public static final double MAX_RPM = 1; /* @@ -32,8 +32,8 @@ public class DriveConstants { * 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 PIDFCoefficients MOTOR_VELOCITY_PID = new PIDFCoefficients(0, 0, 0, + getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REVOLUTION)); /* * These are physical constants that can be determined from your robot (including the track @@ -64,14 +64,14 @@ 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 = 30; + public static double MAX_ACCELERATION = 30; + public static double MAX_ANGULAR_VELOCITY = Math.toRadians(60); + public static double MAX_ANGULAR_ACCELERATION = Math.toRadians(60); 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 3e53d40..9a1db7f 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 @@ -29,17 +29,19 @@ 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 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_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; @@ -63,8 +65,8 @@ 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 TrajectoryFollower follower; @@ -93,6 +95,7 @@ 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 // not face up, remap the IMU axes so that the z-axis points upward (normal to the floor.) @@ -135,8 +138,8 @@ 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() @@ -163,7 +166,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 ); } 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 7f016df..e0d4761 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; @@ -60,8 +60,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; @@ -134,8 +134,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() @@ -162,7 +162,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/DriveVelocityPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/DriveVelocityPIDTuner.java index af1182a..a49ac6e 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 @@ -55,7 +55,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 @@ -71,12 +71,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(); @@ -149,14 +149,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/ManualFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/ManualFeedforwardTuner.java index 43a33dd..611ee01 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 @@ -52,7 +52,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 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 d6ae556..f6215bd 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 @@ -77,6 +77,6 @@ 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; } } From 10e662b7924a92b420ce405abcf769426c1a5adf Mon Sep 17 00:00:00 2001 From: hillelv Date: Mon, 17 Jun 2024 16:34:36 +0300 Subject: [PATCH 02/22] hillel - more stuff --- .../roadrunner/drive/DriveConstants.java | 2 ++ .../roadrunner/drive/SampleMecanumDrive.java | 16 ++++++++++------ 2 files changed, 12 insertions(+), 6 deletions(-) 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 e421e95..05c2540 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 @@ -69,6 +69,8 @@ public class DriveConstants { public static double MAX_ANGULAR_VELOCITY = Math.toRadians(60); 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_REVOLUTION; 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 9a1db7f..dae1936 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 @@ -32,6 +32,7 @@ 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; @@ -40,6 +41,7 @@ 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; @@ -68,6 +70,8 @@ public class SampleMecanumDrive extends MecanumDrive { 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; private DcMotorEx leftFront, leftRear, rightRear, rightFront; @@ -119,16 +123,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); } @@ -239,7 +243,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) { From 12d754257c88dbda54c8d99fc49754b852310536 Mon Sep 17 00:00:00 2001 From: hillelv Date: Tue, 18 Jun 2024 09:12:00 +0300 Subject: [PATCH 03/22] hillel - constants --- .../ftc/teamcode/roadrunner/drive/DriveConstants.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) 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 05c2540..db90065 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_REVOLUTION = 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. @@ -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 = 19.2; // output (wheel) speed / input (motor) speed + public static double TRACK_WIDTH = 15.5; // in /* * These are the feedforward parameters used to model the drive motor behavior. If you are using From 02584eb640d8d15f2ff76e1c4e823759722c2f10 Mon Sep 17 00:00:00 2001 From: greenblitz4590 <94316058+greenblitz4590@users.noreply.github.com> Date: Tue, 18 Jun 2024 21:01:51 +0300 Subject: [PATCH 04/22] hillel - very long day very little progress --- .../ftc/teamcode/roadrunner/drive/DriveConstants.java | 8 ++++---- .../teamcode/roadrunner/drive/SampleMecanumDrive.java | 9 ++++++--- .../roadrunner/drive/opmode/ManualFeedforwardTuner.java | 4 +++- 3 files changed, 13 insertions(+), 8 deletions(-) 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 db90065..a98e839 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 @@ -31,7 +31,7 @@ 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 final boolean RUN_USING_ENCODER = false; public static PIDFCoefficients MOTOR_VELOCITY_PID = new PIDFCoefficients(0, 0, 0, getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REVOLUTION)); @@ -45,7 +45,7 @@ public class DriveConstants { */ public static double WHEEL_RADIUS = 1.8; // in public static double GEAR_RATIO = 19.2; // output (wheel) speed / input (motor) speed - public static double TRACK_WIDTH = 15.5; // in + public static double TRACK_WIDTH = 14.5; // in /* * These are the feedforward parameters used to model the drive motor behavior. If you are using @@ -64,8 +64,8 @@ public class DriveConstants { * small and gradually increase them later after everything is working. All distance units are * inches. */ - public static double MAX_VELOCITY = 30; - public static double MAX_ACCELERATION = 30; + public static double MAX_VELOCITY = MAX_RPM/60 * WHEEL_RADIUS * GEAR_RATIO * 2*Math.PI * 0.8; + public static double MAX_ACCELERATION = MAX_VELOCITY; public static double MAX_ANGULAR_VELOCITY = Math.toRadians(60); public static double MAX_ANGULAR_ACCELERATION = Math.toRadians(60); 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 dae1936..86fc596 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; @@ -101,7 +102,7 @@ public SampleMecanumDrive(HardwareMap hardwareMap) { 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 @@ -146,7 +147,9 @@ public SampleMecanumDrive(HardwareMap hardwareMap) { 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(...)); @@ -310,7 +313,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/opmode/ManualFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/ManualFeedforwardTuner.java index 611ee01..0e5f68b 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 @@ -11,6 +11,7 @@ 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.VoltageSensor; import com.qualcomm.robotcore.util.RobotLog; import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; @@ -57,6 +58,7 @@ private static MotionProfile generateProfile(boolean movingForward) { @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."); @@ -105,7 +107,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."); From a07896c42f1b93759309dd09a084c9cc858c98a7 Mon Sep 17 00:00:00 2001 From: greenblitz4590 <94316058+greenblitz4590@users.noreply.github.com> Date: Wed, 19 Jun 2024 09:15:05 +0300 Subject: [PATCH 05/22] hillel - welp --- .../ftc/teamcode/roadrunner/drive/DriveConstants.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) 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 a98e839..b35867d 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 @@ -32,7 +32,7 @@ public class DriveConstants { * from DriveVelocityPIDTuner. */ public static final boolean RUN_USING_ENCODER = false; - public static PIDFCoefficients MOTOR_VELOCITY_PID = new PIDFCoefficients(0, 0, 0, + public static PIDFCoefficients MOTOR_VELOCITY_PID = new PIDFCoefficients(1, 0, 0, getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REVOLUTION)); /* @@ -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, @@ -65,7 +65,7 @@ public class DriveConstants { * inches. */ public static double MAX_VELOCITY = MAX_RPM/60 * WHEEL_RADIUS * GEAR_RATIO * 2*Math.PI * 0.8; - public static double MAX_ACCELERATION = MAX_VELOCITY; + public static double MAX_ACCELERATION = 500; public static double MAX_ANGULAR_VELOCITY = Math.toRadians(60); public static double MAX_ANGULAR_ACCELERATION = Math.toRadians(60); From cba3af1b278d7a3e68077f591ddb592642c04b21 Mon Sep 17 00:00:00 2001 From: GreenBlitz Date: Wed, 19 Jun 2024 19:45:15 +0300 Subject: [PATCH 06/22] dana - y good x not --- .../ftc/teamcode/roadrunner/drive/DriveConstants.java | 2 +- .../ftc/teamcode/roadrunner/drive/SampleMecanumDrive.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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 b35867d..60e229a 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 @@ -44,7 +44,7 @@ public class DriveConstants { * convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO. */ public static double WHEEL_RADIUS = 1.8; // in - public static double GEAR_RATIO = 19.2; // output (wheel) speed / input (motor) speed + public static double GEAR_RATIO = 25.285580545014684269287225445047; // output (wheel) speed / input (motor) speed public static double TRACK_WIDTH = 14.5; // in /* 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 86fc596..3605cbc 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 @@ -60,7 +60,7 @@ 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 LATERAL_MULTIPLIER = 1.145705306889935; public static double VX_WEIGHT = 1; public static double VY_WEIGHT = 1; From 959d37016480246b6bfb2bc1c6a4060b7aea282a Mon Sep 17 00:00:00 2001 From: GreenBlitz Date: Wed, 19 Jun 2024 21:15:53 +0300 Subject: [PATCH 07/22] hillel - y better --- .../ftc/teamcode/roadrunner/drive/SampleMecanumDrive.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 3605cbc..3fa760d 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 @@ -60,7 +60,7 @@ 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.145705306889935; + public static double LATERAL_MULTIPLIER = 1.0856210671340743708609271523179; public static double VX_WEIGHT = 1; public static double VY_WEIGHT = 1; From 55b32042b244852bf6fd82d2f83c4487ab2d5b29 Mon Sep 17 00:00:00 2001 From: GreenBlitz Date: Wed, 19 Jun 2024 21:45:20 +0300 Subject: [PATCH 08/22] yoav - staright calibration --- .../ftc/teamcode/roadrunner/drive/DriveConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 60e229a..84f26ee 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 @@ -44,7 +44,7 @@ public class DriveConstants { * convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO. */ public static double WHEEL_RADIUS = 1.8; // in - public static double GEAR_RATIO = 25.285580545014684269287225445047; // output (wheel) speed / input (motor) speed + public static double GEAR_RATIO = 10.405055385014241477642377955606; // output (wheel) speed / input (motor) speed public static double TRACK_WIDTH = 14.5; // in /* From 28a76d77f464206df77e337698eda8fc04577571 Mon Sep 17 00:00:00 2001 From: GreenBlitz Date: Thu, 20 Jun 2024 13:35:59 +0300 Subject: [PATCH 09/22] hillel ana-aref --- .../ftc/teamcode/roadrunner/drive/DriveConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 84f26ee..c0952c1 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 @@ -66,7 +66,7 @@ public class DriveConstants { */ public static double MAX_VELOCITY = MAX_RPM/60 * WHEEL_RADIUS * GEAR_RATIO * 2*Math.PI * 0.8; public static double MAX_ACCELERATION = 500; - public static double MAX_ANGULAR_VELOCITY = Math.toRadians(60); + public static double MAX_ANGULAR_VELOCITY = 0.27222222089767456; public static double MAX_ANGULAR_ACCELERATION = Math.toRadians(60); public static double MAX_BATTERY_VOLTAGE = 12; From 7f7256940bff71b4e1c22ac3128fee69ba9c2578 Mon Sep 17 00:00:00 2001 From: GreenBlitz Date: Thu, 20 Jun 2024 19:40:49 +0300 Subject: [PATCH 10/22] hillel - track width tune --- .../ftc/teamcode/roadrunner/drive/DriveConstants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 c0952c1..7e3d785 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 @@ -45,7 +45,7 @@ public class DriveConstants { */ public static double WHEEL_RADIUS = 1.8; // in public static double GEAR_RATIO = 10.405055385014241477642377955606; // output (wheel) speed / input (motor) speed - public static double TRACK_WIDTH = 14.5; // in + public static double TRACK_WIDTH = 440; // in /* * These are the feedforward parameters used to model the drive motor behavior. If you are using @@ -66,7 +66,7 @@ public class DriveConstants { */ public static double MAX_VELOCITY = MAX_RPM/60 * WHEEL_RADIUS * GEAR_RATIO * 2*Math.PI * 0.8; public static double MAX_ACCELERATION = 500; - public static double MAX_ANGULAR_VELOCITY = 0.27222222089767456; + public static double MAX_ANGULAR_VELOCITY = 18.016340020937598; public static double MAX_ANGULAR_ACCELERATION = Math.toRadians(60); public static double MAX_BATTERY_VOLTAGE = 12; From d5576028e6c2c95d50bef21ebccdd0632d64a959 Mon Sep 17 00:00:00 2001 From: GreenBlitz Date: Thu, 20 Jun 2024 20:57:06 +0300 Subject: [PATCH 11/22] hillel - I'm amazing --- .../ftc/teamcode/roadrunner/drive/DriveConstants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 7e3d785..0488921 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 @@ -44,8 +44,8 @@ public class DriveConstants { * convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO. */ public static double WHEEL_RADIUS = 1.8; // in - public static double GEAR_RATIO = 10.405055385014241477642377955606; // output (wheel) speed / input (motor) speed - public static double TRACK_WIDTH = 440; // 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 From 6e38ddb2badb3ca565cf329ed4dc85f053a0f3e1 Mon Sep 17 00:00:00 2001 From: GreenBlitz Date: Sat, 22 Jun 2024 22:51:56 +0300 Subject: [PATCH 12/22] hillel - looks good --- .../ftc/teamcode/roadrunner/drive/SampleMecanumDrive.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 3fa760d..4be4e55 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 @@ -57,8 +57,8 @@ */ @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(100, 0, 0); + public static PIDCoefficients HEADING_PID = new PIDCoefficients(-17, 0, 0); public static double LATERAL_MULTIPLIER = 1.0856210671340743708609271523179; From 2664302ed3f303fe9c978ce999dbd04cb096cd45 Mon Sep 17 00:00:00 2001 From: GreenBlitz Date: Sat, 22 Jun 2024 23:17:52 +0300 Subject: [PATCH 13/22] hillel - working!!! --- .../ftc/teamcode/roadrunner/drive/SampleMecanumDrive.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 4be4e55..c082d0c 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 @@ -58,7 +58,7 @@ @Config public class SampleMecanumDrive extends MecanumDrive { public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(100, 0, 0); - public static PIDCoefficients HEADING_PID = new PIDCoefficients(-17, 0, 0); + public static PIDCoefficients HEADING_PID = new PIDCoefficients(-10, 0, 0); public static double LATERAL_MULTIPLIER = 1.0856210671340743708609271523179; From ba50d1638ad884b8aec562db83234dcbe7e6af71 Mon Sep 17 00:00:00 2001 From: GreenBlitz Date: Sat, 22 Jun 2024 23:22:20 +0300 Subject: [PATCH 14/22] hillel - merge master --- .../ftc/teamcode/roadrunner/drive/SampleMecanumDrive.java | 2 ++ 1 file changed, 2 insertions(+) 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 c082d0c..d0530d7 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 @@ -57,6 +57,7 @@ */ @Config public class SampleMecanumDrive extends MecanumDrive { + public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(100, 0, 0); public static PIDCoefficients HEADING_PID = new PIDCoefficients(-10, 0, 0); @@ -326,4 +327,5 @@ public static TrajectoryVelocityConstraint getVelocityConstraint(double maxVel, public static TrajectoryAccelerationConstraint getAccelerationConstraint(double maxAccel) { return new ProfileAccelerationConstraint(maxAccel); } + } \ No newline at end of file From 6b7283ec27a9e40b40d7941792faf6bf3b7784f7 Mon Sep 17 00:00:00 2001 From: GreenBlitz Date: Sun, 23 Jun 2024 11:58:34 +0300 Subject: [PATCH 15/22] hillel - disable --- .../roadrunner/drive/opmode/AutomaticFeedforwardTuner.java | 2 ++ .../ftc/teamcode/roadrunner/drive/opmode/BackAndForth.java | 2 ++ .../teamcode/roadrunner/drive/opmode/DriveVelocityPIDTuner.java | 2 ++ .../ftc/teamcode/roadrunner/drive/opmode/FollowerPIDTuner.java | 2 ++ .../ftc/teamcode/roadrunner/drive/opmode/LocalizationTest.java | 2 ++ .../roadrunner/drive/opmode/ManualFeedforwardTuner.java | 2 ++ .../teamcode/roadrunner/drive/opmode/MaxAngularVeloTuner.java | 2 ++ .../ftc/teamcode/roadrunner/drive/opmode/MaxVelocityTuner.java | 2 ++ .../ftc/teamcode/roadrunner/drive/opmode/SplineTest.java | 2 ++ .../ftc/teamcode/roadrunner/drive/opmode/StrafeTest.java | 2 ++ .../ftc/teamcode/roadrunner/drive/opmode/StraightTest.java | 2 ++ .../ftc/teamcode/roadrunner/drive/opmode/TrackWidthTuner.java | 2 ++ .../drive/opmode/TrackingWheelForwardOffsetTuner.java | 2 ++ .../drive/opmode/TrackingWheelLateralDistanceTuner.java | 2 ++ .../ftc/teamcode/roadrunner/drive/opmode/TurnTest.java | 2 ++ 15 files changed, 30 insertions(+) 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..483bf0e 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. */ +@Disabled @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 f7cfb56..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 { 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 966f2d8..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,6 +10,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.VoltageSensor; import com.qualcomm.robotcore.util.RobotLog; @@ -34,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 { 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 ae48dad..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 { 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..cb202eb 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; @@ -11,6 +12,7 @@ /* * This is an example of a more complex path to really test the tuning. */ +@Disabled @Autonomous(group = "drive") public class SplineTest extends LinearOpMode { 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 { From fba90fa372b783c34476f3e92aff885956fb0a74 Mon Sep 17 00:00:00 2001 From: GreenBlitz Date: Sun, 23 Jun 2024 12:25:22 +0300 Subject: [PATCH 16/22] hillel - comment out --- .../ftc/teamcode/autos/DefaultGil.java | 39 +++++++++++++++---- 1 file changed, 32 insertions(+), 7 deletions(-) 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..f5a607c 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 @@ -1,23 +1,48 @@ package org.firstinspires.ftc.teamcode.autos; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.Alliance; import org.firstinspires.ftc.teamcode.FieldStartingLocation; import org.firstinspires.ftc.teamcode.Robot; +import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequence; -public abstract class DefaultGil extends LinearOpMode { +@Autonomous(name = "DefaultGil") +public class DefaultGil extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { - Robot.getInstance().setAlliance(getAlliance()); - Robot.getInstance().setFieldStartingLocation(getFieldStartingLocation()); +// Robot.getInstance().setAlliance(getAlliance()); +// Robot.getInstance().setFieldStartingLocation(getFieldStartingLocation()); Robot.init(hardwareMap); - run(); + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + Pose2d startPose = new Pose2d(0, 0, 0); + + drive.setPoseEstimate(startPose); + + TrajectorySequence trajSeq = 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)) + .build(); + + waitForStart(); + + if (!isStopRequested()) + drive.followTrajectorySequence(trajSeq); } - public abstract Alliance getAlliance(); - public abstract FieldStartingLocation getFieldStartingLocation(); - public abstract void run() throws InterruptedException; +// public abstract Alliance getAlliance(); +// public abstract FieldStartingLocation getFieldStartingLocation(); +// public abstract void run() throws InterruptedException; } From 7bac7b7c4d6810d5d11c01bf5a32599bd49c5e8b Mon Sep 17 00:00:00 2001 From: GreenBlitz Date: Sun, 23 Jun 2024 15:02:51 +0300 Subject: [PATCH 17/22] hillel - all positions --- .../ftc/teamcode/roadrunner/drive/DriveConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 c008cfc..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 @@ -64,7 +64,7 @@ public class DriveConstants { * small and gradually increase them later after everything is working. All distance units are * inches. */ - public static double MAX_VELOCITY = MAX_RPM/60 * WHEEL_RADIUS * GEAR_RATIO * 2*Math.PI * 0.8; + 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); From 8309bf3df035450fe55158b5246fb93018330218 Mon Sep 17 00:00:00 2001 From: GreenBlitz Date: Sun, 23 Jun 2024 16:12:33 +0300 Subject: [PATCH 18/22] hillel - calib --- .../ftc/teamcode/roadrunner/drive/SampleMecanumDrive.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 d0530d7..71c8cd3 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 @@ -58,8 +58,8 @@ @Config public class SampleMecanumDrive extends MecanumDrive { - public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(100, 0, 0); - public static PIDCoefficients HEADING_PID = new PIDCoefficients(-10, 0, 0); + public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(65, 0, 0); + public static PIDCoefficients HEADING_PID = new PIDCoefficients(-19.5, 0, 0); public static double LATERAL_MULTIPLIER = 1.0856210671340743708609271523179; From 5ed7b3ee1255e28e1939adc19ee2668e057460bb Mon Sep 17 00:00:00 2001 From: GreenBlitz Date: Sun, 23 Jun 2024 17:05:06 +0300 Subject: [PATCH 19/22] yoav: uncommitde --- .../meepmeeptesting/MeepMeepTesting.java | 21 +++++++++-------- .../ftc/teamcode/autos/DefaultGil.java | 23 ++++++++++--------- .../roadrunner/drive/opmode/BackAndForth.java | 2 +- .../roadrunner/drive/opmode/SplineTest.java | 1 - 4 files changed, 25 insertions(+), 22 deletions(-) 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 f5a607c..2c0d581 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 @@ -1,6 +1,7 @@ package org.firstinspires.ftc.teamcode.autos; import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; @@ -22,17 +23,17 @@ public void runOpMode() throws InterruptedException { Pose2d startPose = new Pose2d(0, 0, 0); - drive.setPoseEstimate(startPose); - - TrajectorySequence trajSeq = 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.setPoseEstimate(new Pose2d(10, -60, -Math.PI/2)); + + TrajectorySequence trajSeq = 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(); waitForStart(); 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 483bf0e..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 @@ -25,7 +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. */ -@Disabled + @Config @Autonomous(group = "drive") public class BackAndForth extends LinearOpMode { 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 cb202eb..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 @@ -12,7 +12,6 @@ /* * This is an example of a more complex path to really test the tuning. */ -@Disabled @Autonomous(group = "drive") public class SplineTest extends LinearOpMode { From 4120683388738f15fd6cbed8dbf4f8d0989dfb46 Mon Sep 17 00:00:00 2001 From: greenblitz4590 <94316058+greenblitz4590@users.noreply.github.com> Date: Sun, 23 Jun 2024 20:42:49 +0300 Subject: [PATCH 20/22] hillel - shamir --- .../ftc/teamcode/autos/DefaultGil.java | 13 +++++++------ .../roadrunner/drive/SampleMecanumDrive.java | 2 +- 2 files changed, 8 insertions(+), 7 deletions(-) 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 2c0d581..004e356 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 @@ -23,15 +23,16 @@ public void runOpMode() throws InterruptedException { Pose2d startPose = new Pose2d(0, 0, 0); - drive.setPoseEstimate(new Pose2d(10, -60, -Math.PI/2)); + drive.setPoseEstimate(new Pose2d(10, 60, Math.PI/2)); - TrajectorySequence trajSeq = drive.trajectorySequenceBuilder(new Pose2d(10, -60, -Math.PI/2)) - .lineToLinearHeading(new Pose2d(15, -40, Math.toRadians(225))) + TrajectorySequence trajSeq = drive.trajectorySequenceBuilder(new Pose2d(10, 60, Math.PI/2)) + .lineTo(new Vector2d(10, 40)) + .turn(Math.toRadians(-45)) .addTemporalMarker(() -> {}) .waitSeconds(0.5) - .lineTo(new Vector2d(25, -55)) - .lineToLinearHeading(new Pose2d(40, -35, 0)) - .lineToLinearHeading(new Pose2d(50, -40)) + .lineTo(new Vector2d(50, 40)) +// .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/roadrunner/drive/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/SampleMecanumDrive.java index 71c8cd3..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 @@ -59,7 +59,7 @@ public class SampleMecanumDrive extends MecanumDrive { public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(65, 0, 0); - public static PIDCoefficients HEADING_PID = new PIDCoefficients(-19.5, 0, 0); + public static PIDCoefficients HEADING_PID = new PIDCoefficients(0.25, 0, 0); public static double LATERAL_MULTIPLIER = 1.0856210671340743708609271523179; From 0a6a3feb464fbda32c92fe0001c51aa5e40b5a18 Mon Sep 17 00:00:00 2001 From: greenblitz4590 <94316058+greenblitz4590@users.noreply.github.com> Date: Sun, 23 Jun 2024 21:33:53 +0300 Subject: [PATCH 21/22] hillel - not shamir --- .../java/org/firstinspires/ftc/teamcode/autos/DefaultGil.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) 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 004e356..4b37821 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 @@ -26,8 +26,7 @@ public void runOpMode() throws InterruptedException { drive.setPoseEstimate(new Pose2d(10, 60, Math.PI/2)); TrajectorySequence trajSeq = drive.trajectorySequenceBuilder(new Pose2d(10, 60, Math.PI/2)) - .lineTo(new Vector2d(10, 40)) - .turn(Math.toRadians(-45)) + .lineToLinearHeading(new Pose2d(10, 40, Math.toRadians(-135))) .addTemporalMarker(() -> {}) .waitSeconds(0.5) .lineTo(new Vector2d(50, 40)) From 8220984c5e28a77fbb8395f0f87953bad501047c Mon Sep 17 00:00:00 2001 From: greenblitz4590 <94316058+greenblitz4590@users.noreply.github.com> Date: Sun, 23 Jun 2024 22:42:25 +0300 Subject: [PATCH 22/22] toav- def gil --- .../ftc/teamcode/autos/DefaultGil.java | 40 ++++--------------- 1 file changed, 8 insertions(+), 32 deletions(-) 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 4b37821..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 @@ -1,49 +1,25 @@ package org.firstinspires.ftc.teamcode.autos; -import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.acmerobotics.roadrunner.geometry.Vector2d; -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.Alliance; import org.firstinspires.ftc.teamcode.FieldStartingLocation; import org.firstinspires.ftc.teamcode.Robot; -import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; -import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequence; -@Autonomous(name = "DefaultGil") -public class DefaultGil extends LinearOpMode { +public abstract class DefaultGil extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { -// Robot.getInstance().setAlliance(getAlliance()); -// Robot.getInstance().setFieldStartingLocation(getFieldStartingLocation()); + Robot.getInstance().setAlliance(getAlliance()); + Robot.getInstance().setFieldStartingLocation(getFieldStartingLocation()); Robot.init(hardwareMap); - SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); - - Pose2d startPose = new Pose2d(0, 0, 0); - - drive.setPoseEstimate(new Pose2d(10, 60, Math.PI/2)); - - TrajectorySequence trajSeq = drive.trajectorySequenceBuilder(new Pose2d(10, 60, Math.PI/2)) - .lineToLinearHeading(new Pose2d(10, 40, Math.toRadians(-135))) - .addTemporalMarker(() -> {}) - .waitSeconds(0.5) - .lineTo(new Vector2d(50, 40)) -// .lineToLinearHeading(new Pose2d(40, 35, 0)) -// .lineToLinearHeading(new Pose2d(50, 40)) - .addTemporalMarker(() -> {}) - .waitSeconds(0.5) - .build(); + run(); + } - waitForStart(); + public abstract Alliance getAlliance(); - if (!isStopRequested()) - drive.followTrajectorySequence(trajSeq); - } + public abstract FieldStartingLocation getFieldStartingLocation(); -// public abstract Alliance getAlliance(); -// public abstract FieldStartingLocation getFieldStartingLocation(); -// public abstract void run() throws InterruptedException; + public abstract void run() throws InterruptedException; }