diff --git a/FtcRobotController/build.gradle b/FtcRobotController/build.gradle index de6d53fe..1ec0503f 100644 --- a/FtcRobotController/build.gradle +++ b/FtcRobotController/build.gradle @@ -13,7 +13,7 @@ android { buildConfigField "String", "BUILD_TIME", '"' + (new SimpleDateFormat("yyyy-MM-dd'T'HH:mm:ss.SSSZ", Locale.ROOT).format(new Date())) + '"' } - compileSdkVersion 28 + compileSdkVersion 29 compileOptions { sourceCompatibility JavaVersion.VERSION_1_7 diff --git a/FtcRobotController/build.release.gradle b/FtcRobotController/build.release.gradle index c280933e..336a0c2a 100644 --- a/FtcRobotController/build.release.gradle +++ b/FtcRobotController/build.release.gradle @@ -1,11 +1,11 @@ dependencies { - implementation 'org.firstinspires.ftc:Inspection:6.0.1' - implementation 'org.firstinspires.ftc:Blocks:6.0.1' - implementation 'org.firstinspires.ftc:RobotCore:6.0.1' - implementation 'org.firstinspires.ftc:RobotServer:6.0.1' - implementation 'org.firstinspires.ftc:OnBotJava:6.0.1' - implementation 'org.firstinspires.ftc:Hardware:6.0.1' - implementation 'org.firstinspires.ftc:FtcCommon:6.0.1' + implementation 'org.firstinspires.ftc:Inspection:6.1.1' + implementation 'org.firstinspires.ftc:Blocks:6.1.1' + implementation 'org.firstinspires.ftc:RobotCore:6.1.1' + implementation 'org.firstinspires.ftc:RobotServer:6.1.1' + implementation 'org.firstinspires.ftc:OnBotJava:6.1.1' + implementation 'org.firstinspires.ftc:Hardware:6.1.1' + implementation 'org.firstinspires.ftc:FtcCommon:6.1.1' implementation 'androidx.appcompat:appcompat:1.2.0' implementation 'com.acmerobotics.dashboard:dashboard:0.3.10' diff --git a/FtcRobotController/src/main/AndroidManifest.xml b/FtcRobotController/src/main/AndroidManifest.xml index e4755ead..cb1e8d22 100644 --- a/FtcRobotController/src/main/AndroidManifest.xml +++ b/FtcRobotController/src/main/AndroidManifest.xml @@ -2,8 +2,8 @@ + android:versionCode="39" + android:versionName="6.1"> @@ -13,7 +13,8 @@ android:extractNativeLibs="true" android:icon="@drawable/ic_launcher" android:label="@string/app_name" - android:theme="@style/AppThemeRedRC" > + android:theme="@style/AppThemeRedRC" + android:usesCleartextTraffic="true"> poseHistory; @@ -104,7 +112,11 @@ public SampleMecanumDrive(HardwareMap hardwareMap) { turnController = new PIDFController(HEADING_PID); turnController.setInputBounds(0, 2 * Math.PI); - constraints = new MecanumConstraints(BASE_CONSTRAINTS, TRACK_WIDTH); + velConstraint = new MinVelocityConstraint(Arrays.asList( + new AngularVelocityConstraint(MAX_ANG_VEL), + new MecanumVelocityConstraint(MAX_VEL, TRACK_WIDTH) + )); + accelConstraint = new ProfileAccelerationConstraint(MAX_ACCEL); follower = new HolonomicPIDVAFollower(TRANSLATIONAL_PID, TRANSLATIONAL_PID, HEADING_PID, new Pose2d(0.5, 0.5, Math.toRadians(5.0)), 0.5); @@ -158,15 +170,15 @@ public SampleMecanumDrive(HardwareMap hardwareMap) { } public TrajectoryBuilder trajectoryBuilder(Pose2d startPose) { - return new TrajectoryBuilder(startPose, constraints); + return new TrajectoryBuilder(startPose, velConstraint, accelConstraint); } public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, boolean reversed) { - return new TrajectoryBuilder(startPose, reversed, constraints); + return new TrajectoryBuilder(startPose, reversed, velConstraint, accelConstraint); } public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, double startHeading) { - return new TrajectoryBuilder(startPose, startHeading, constraints); + return new TrajectoryBuilder(startPose, startHeading, velConstraint, accelConstraint); } public void turnAsync(double angle) { @@ -177,9 +189,8 @@ public void turnAsync(double angle) { turnProfile = MotionProfileGenerator.generateSimpleMotionProfile( new MotionState(heading, 0, 0, 0), new MotionState(heading + angle, 0, 0, 0), - constraints.maxAngVel, - constraints.maxAngAccel, - constraints.maxAngJerk + MAX_ANG_VEL, + MAX_ANG_ACCEL ); turnStart = clock.seconds(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleTankDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleTankDrive.java index aac23244..2e2d792f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleTankDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleTankDrive.java @@ -18,8 +18,12 @@ import com.acmerobotics.roadrunner.profile.MotionState; import com.acmerobotics.roadrunner.trajectory.Trajectory; import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder; -import com.acmerobotics.roadrunner.trajectory.constraints.DriveConstraints; -import com.acmerobotics.roadrunner.trajectory.constraints.TankConstraints; +import com.acmerobotics.roadrunner.trajectory.constraints.AngularVelocityConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.MinVelocityConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.ProfileAccelerationConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.TankVelocityConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint; import com.acmerobotics.roadrunner.util.NanoClock; import com.qualcomm.hardware.bosch.BNO055IMU; import com.qualcomm.hardware.lynx.LynxModule; @@ -37,7 +41,10 @@ import java.util.Arrays; import java.util.List; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.BASE_CONSTRAINTS; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_ACCEL; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_VEL; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL; import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MOTOR_VELO_PID; import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER; import static org.firstinspires.ftc.teamcode.drive.DriveConstants.TRACK_WIDTH; @@ -73,7 +80,8 @@ public enum Mode { private MotionProfile turnProfile; private double turnStart; - private DriveConstraints constraints; + private TrajectoryVelocityConstraint velConstraint; + private TrajectoryAccelerationConstraint accelConstraint; private TrajectoryFollower follower; private List poseHistory; @@ -96,7 +104,11 @@ public SampleTankDrive(HardwareMap hardwareMap) { turnController = new PIDFController(HEADING_PID); turnController.setInputBounds(0, 2 * Math.PI); - constraints = new TankConstraints(BASE_CONSTRAINTS, TRACK_WIDTH); + velConstraint = new MinVelocityConstraint(Arrays.asList( + new AngularVelocityConstraint(MAX_ANG_VEL), + new TankVelocityConstraint(MAX_VEL, TRACK_WIDTH) + )); + accelConstraint = new ProfileAccelerationConstraint(MAX_ACCEL); follower = new TankPIDVAFollower(AXIAL_PID, CROSS_TRACK_PID, new Pose2d(0.5, 0.5, Math.toRadians(5.0)), 0.5); @@ -153,15 +165,15 @@ public SampleTankDrive(HardwareMap hardwareMap) { } public TrajectoryBuilder trajectoryBuilder(Pose2d startPose) { - return new TrajectoryBuilder(startPose, constraints); + return new TrajectoryBuilder(startPose, velConstraint, accelConstraint); } public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, boolean reversed) { - return new TrajectoryBuilder(startPose, reversed, constraints); + return new TrajectoryBuilder(startPose, reversed, velConstraint, accelConstraint); } public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, double startHeading) { - return new TrajectoryBuilder(startPose, startHeading, constraints); + return new TrajectoryBuilder(startPose, startHeading, velConstraint, accelConstraint); } public void turnAsync(double angle) { @@ -169,9 +181,8 @@ public void turnAsync(double angle) { turnProfile = MotionProfileGenerator.generateSimpleMotionProfile( new MotionState(heading, 0, 0, 0), new MotionState(heading + angle, 0, 0, 0), - constraints.maxAngVel, - constraints.maxAngAccel, - constraints.maxAngJerk + MAX_ANG_VEL, + MAX_ANG_ACCEL ); turnStart = clock.seconds(); mode = Mode.TURN; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/StandardTrackingWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/StandardTrackingWheelLocalizer.java index 7d039db6..c122de94 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/StandardTrackingWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/StandardTrackingWheelLocalizer.java @@ -7,6 +7,7 @@ import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.HardwareMap; + import org.firstinspires.ftc.teamcode.util.Encoder; import java.util.Arrays; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/BackAndForth.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/BackAndForth.java index 73e8e6d7..3ddad2ae 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/BackAndForth.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/BackAndForth.java @@ -4,9 +4,6 @@ import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.trajectory.Trajectory; import com.arcrobotics.ftclib.command.CommandOpMode; -import com.arcrobotics.ftclib.command.InstantCommand; -import com.arcrobotics.ftclib.command.PerpetualCommand; -import com.arcrobotics.ftclib.command.ScheduleCommand; import com.arcrobotics.ftclib.command.SequentialCommandGroup; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/DriveVelocityPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/DriveVelocityPIDTuner.java index 7d4d6ae2..2bd33c44 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/DriveVelocityPIDTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/DriveVelocityPIDTuner.java @@ -10,6 +10,8 @@ import com.acmerobotics.roadrunner.util.NanoClock; import com.arcrobotics.ftclib.command.CommandOpMode; import com.arcrobotics.ftclib.command.InstantCommand; +import com.arcrobotics.ftclib.command.SequentialCommandGroup; +import com.arcrobotics.ftclib.command.WaitUntilCommand; import com.arcrobotics.ftclib.command.button.Button; import com.arcrobotics.ftclib.command.button.GamepadButton; import com.arcrobotics.ftclib.gamepad.GamepadEx; @@ -25,6 +27,8 @@ import java.util.List; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL; import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MOTOR_VELO_PID; import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER; import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV; @@ -71,10 +75,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.BASE_CONSTRAINTS.maxVel, - DriveConstants.BASE_CONSTRAINTS.maxAccel, - DriveConstants.BASE_CONSTRAINTS.maxJerk); + return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, MAX_VEL, MAX_ACCEL); } private MecanumDriveSubsystem drive; @@ -107,17 +108,20 @@ public void initialize() { drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID); - clock = NanoClock.system(); + clock = NanoClock.system(); telemetry.addLine("Ready!"); telemetry.update(); telemetry.clearAll(); - schedule(new InstantCommand(() -> { - movingForwards = true; - activeProfile = generateProfile(true); - profileStart = clock.seconds(); - }), new RunCommand(() -> telemetry.addData("mode", mode))); + schedule(new SequentialCommandGroup( + new WaitUntilCommand(this::isStarted), + new InstantCommand(() -> { + movingForwards = true; + activeProfile = generateProfile(true); + profileStart = clock.seconds(); + }) + ), new RunCommand(() -> telemetry.addData("mode", mode))); xButton = new GamepadButton(gamepad, GamepadKeys.Button.X) .whenPressed(() -> { @@ -166,7 +170,7 @@ public void initialize() { drive.drive(gamepad1.left_stick_y, gamepad1.left_stick_x, gamepad1.right_stick_x); break; } - }, drive).alongWith(new RunCommand(() -> { + if (lastKp != MOTOR_VELO_PID.p || lastKd != MOTOR_VELO_PID.d || lastKi != MOTOR_VELO_PID.i || lastKf != MOTOR_VELO_PID.f) { drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID); @@ -176,8 +180,9 @@ public void initialize() { lastKd = MOTOR_VELO_PID.d; lastKf = MOTOR_VELO_PID.f; } + telemetry.update(); - }, drive))); + })); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/FieldCentricTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/FieldCentricTest.java index 22fe1610..22645e97 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/FieldCentricTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/FieldCentricTest.java @@ -34,10 +34,11 @@ public void initialize() { register(drive); drive.setDefaultCommand(new MecanumDriveCommand( - drive, gamepad::getLeftY, gamepad::getLeftX, gamepad::getRightX + drive, () -> -gamepad.getLeftY(), gamepad::getLeftX, gamepad::getRightX )); schedule(new RunCommand(() -> { + drive.update(); telemetry.addData("Heading", drive.getPoseEstimate().getHeading()); telemetry.update(); })); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/FollowerPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/FollowerPIDTuner.java index bfd29072..38aefde2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/FollowerPIDTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/FollowerPIDTuner.java @@ -3,6 +3,7 @@ import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.arcrobotics.ftclib.command.Command; import com.arcrobotics.ftclib.command.CommandOpMode; import com.arcrobotics.ftclib.command.SequentialCommandGroup; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; @@ -36,6 +37,7 @@ public class FollowerPIDTuner extends CommandOpMode { private MecanumDriveSubsystem drive; private TrajectoryFollowerCommand followerCommand; + private Command trajGroup; private TurnCommand turnCommand; private Pose2d startPose; @@ -47,24 +49,18 @@ public void initialize() { drive.setPoseEstimate(startPose); - Trajectory traj = drive.trajectoryBuilder(startPose) - .forward(DISTANCE) - .build(); - followerCommand = new TrajectoryFollowerCommand(drive, traj); - turnCommand = new TurnCommand(drive, Math.toRadians(90)); - - SequentialCommandGroup trajGroup = new SequentialCommandGroup(followerCommand, turnCommand); - - schedule( - trajGroup.whenFinished( - () -> startPose = traj.end().plus(new Pose2d(0, 0, Math.toRadians(90))) - ), - new RunCommand(() -> { - if (trajGroup.isFinished() || !trajGroup.isScheduled()) { - trajGroup.schedule(); - } - }) - ); + schedule(new RunCommand(() -> { + if (trajGroup == null || trajGroup.isFinished() || !trajGroup.isScheduled()) { + Trajectory traj = drive.trajectoryBuilder(startPose) + .forward(DISTANCE) + .build(); + followerCommand = new TrajectoryFollowerCommand(drive, traj); + turnCommand = new TurnCommand(drive, Math.toRadians(90)); + trajGroup = new SequentialCommandGroup(followerCommand, turnCommand) + .whenFinished(() -> startPose = traj.end().plus(new Pose2d(0, 0, Math.toRadians(90)))); + trajGroup.schedule(); + } + })); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/LocalizationTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/LocalizationTest.java index d22be4cc..dbf9940e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/LocalizationTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/LocalizationTest.java @@ -3,13 +3,9 @@ import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.arcrobotics.ftclib.command.CommandOpMode; -import com.arcrobotics.ftclib.command.InstantCommand; import com.arcrobotics.ftclib.command.PerpetualCommand; -import com.arcrobotics.ftclib.command.ScheduleCommand; import com.arcrobotics.ftclib.gamepad.GamepadEx; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.DcMotor; import org.firstinspires.ftc.teamcode.commands.MecanumDriveCommand; import org.firstinspires.ftc.teamcode.commands.RunCommand; @@ -37,15 +33,16 @@ public class LocalizationTest extends CommandOpMode { public void initialize() { drive = new MecanumDriveSubsystem(new SampleMecanumDrive(hardwareMap), false); - schedule(new PerpetualCommand(new RunCommand( - () -> { - Pose2d poseEstimate = drive.getPoseEstimate(); - telemetry.addData("x", poseEstimate.getX()); - telemetry.addData("y", poseEstimate.getY()); - telemetry.addData("heading", poseEstimate.getHeading()); - telemetry.update(); - } // ignore requirements - ))); + gamepad = new GamepadEx(gamepad1); + + schedule(new RunCommand(() -> { + drive.update(); + Pose2d poseEstimate = drive.getPoseEstimate(); + telemetry.addData("x", poseEstimate.getX()); + telemetry.addData("y", poseEstimate.getY()); + telemetry.addData("heading", poseEstimate.getHeading()); + telemetry.update(); + })); driveCommand = new MecanumDriveCommand( drive, () -> -gamepad.getLeftY(), diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/ManualFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/ManualFeedforwardTuner.java index c9ad23f2..62375dc8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/ManualFeedforwardTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/ManualFeedforwardTuner.java @@ -16,7 +16,6 @@ import com.arcrobotics.ftclib.gamepad.GamepadEx; import com.arcrobotics.ftclib.gamepad.GamepadKeys; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.util.RobotLog; import org.firstinspires.ftc.teamcode.commands.RunCommand; @@ -30,6 +29,8 @@ import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kA; import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kStatic; import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL; /** * This routine is designed to tune the open-loop feedforward coefficients. Although it may seem unnecessary, @@ -75,10 +76,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.BASE_CONSTRAINTS.maxVel, - DriveConstants.BASE_CONSTRAINTS.maxAccel, - DriveConstants.BASE_CONSTRAINTS.maxJerk); + return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, MAX_VEL, MAX_ACCEL); } private NanoClock clock; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxVelocityTuner.java index 18e24284..76daf4cc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxVelocityTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxVelocityTuner.java @@ -32,7 +32,7 @@ public class MaxVelocityTuner extends CommandOpMode { public static double RUNTIME = 2.0; - private ElapsedTime timer; + private ElapsedTime timer = new ElapsedTime(); private double maxVelocity = 0.0; private VoltageSensor batteryVoltageSensor; @@ -50,16 +50,18 @@ public void initialize() { telemetry.addLine("Press start when ready."); telemetry.update(); - schedule(new InstantCommand(() -> { - telemetry.clearAll(); - telemetry.update(); + schedule(new WaitUntilCommand(this::isStarted) + .whenFinished(() -> { + telemetry.clearAll(); + telemetry.update(); - drive.setDrivePower(new Pose2d(1, 0, 0)); - timer = new ElapsedTime(); - }, drive)); + drive.setDrivePower(new Pose2d(1, 0, 0)); + timer.reset(); + }) + ); RunCommand runCommand = new RunCommand(() -> { - // update is called every loop in the periodic method of the drive subsystem + drive.update(); Pose2d poseVelo = Objects.requireNonNull( drive.getPoseVelocity(), @@ -68,10 +70,11 @@ public void initialize() { ); maxVelocity = Math.max(poseVelo.vec().norm(), maxVelocity); - }, drive); + }); - schedule(runCommand.deadlineWith(new WaitUntilCommand(() -> timer.seconds() >= RUNTIME)) - .andThen(new InstantCommand(() -> { + schedule(new WaitUntilCommand(() -> timer.seconds() >= RUNTIME) + .deadlineWith(runCommand) + .whenFinished(() -> { drive.setDrivePower(new Pose2d()); double effectiveKf = DriveConstants.getMotorVelocityF(veloInchesToTicks(maxVelocity)); @@ -79,7 +82,7 @@ public void initialize() { telemetry.addData("Max Velocity", maxVelocity); telemetry.addData("Voltage Compensated kF", effectiveKf * batteryVoltageSensor.getVoltage() / 12); telemetry.update(); - }, drive)) + }) ); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/SplineTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/SplineTest.java index f571713e..07153388 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/SplineTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/SplineTest.java @@ -5,8 +5,8 @@ import com.acmerobotics.roadrunner.trajectory.Trajectory; import com.arcrobotics.ftclib.command.CommandOpMode; import com.arcrobotics.ftclib.command.WaitCommand; +import com.arcrobotics.ftclib.command.WaitUntilCommand; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.commands.TrajectoryFollowerCommand; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; @@ -30,13 +30,14 @@ public void initialize() { .splineTo(new Vector2d(30, 30), 0) .build(); splineFollower = new TrajectoryFollowerCommand(drive, traj); - schedule(splineFollower.andThen(new WaitCommand(2000), - new TrajectoryFollowerCommand(drive, - drive.trajectoryBuilder(traj.end(), true) - .splineTo(new Vector2d(0, 0), Math.toRadians(180)) - .build() - )) - ); + schedule(new WaitUntilCommand(this::isStarted).andThen( + splineFollower.andThen(new WaitCommand(2000), + new TrajectoryFollowerCommand(drive, + drive.trajectoryBuilder(traj.end(), true) + .splineTo(new Vector2d(0, 0), Math.toRadians(180)) + .build() + )) + )); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StrafeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StrafeTest.java index 4807c100..909b346a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StrafeTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StrafeTest.java @@ -2,10 +2,9 @@ import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.acmerobotics.roadrunner.trajectory.Trajectory; import com.arcrobotics.ftclib.command.CommandOpMode; +import com.arcrobotics.ftclib.command.WaitUntilCommand; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.commands.TrajectoryFollowerCommand; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; @@ -33,13 +32,13 @@ public void initialize() { .strafeRight(DISTANCE) .build() ); - schedule(strafeFollower.whenFinished(() -> { + schedule(new WaitUntilCommand(this::isStarted).andThen(strafeFollower.whenFinished(() -> { Pose2d poseEstimate = drive.getPoseEstimate(); telemetry.addData("finalX", poseEstimate.getX()); telemetry.addData("finalY", poseEstimate.getY()); telemetry.addData("finalHeading", poseEstimate.getHeading()); telemetry.update(); - })); + }))); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StraightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StraightTest.java index 3f2ff591..b5d6d184 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StraightTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StraightTest.java @@ -3,6 +3,7 @@ import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.arcrobotics.ftclib.command.CommandOpMode; +import com.arcrobotics.ftclib.command.WaitUntilCommand; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import org.firstinspires.ftc.teamcode.commands.TrajectoryFollowerCommand; @@ -31,13 +32,13 @@ public void initialize() { .forward(DISTANCE) .build() ); - schedule(straightFollower.whenFinished(() -> { + schedule(new WaitUntilCommand(this::isStarted).andThen(straightFollower.whenFinished(() -> { Pose2d poseEstimate = drive.getPoseEstimate(); telemetry.addData("finalX", poseEstimate.getX()); telemetry.addData("finalY", poseEstimate.getY()); telemetry.addData("finalHeading", poseEstimate.getHeading()); telemetry.update(); - })); + }))); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackWidthTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackWidthTuner.java index 92346bd6..3cd5c562 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackWidthTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackWidthTuner.java @@ -7,6 +7,7 @@ import com.acmerobotics.roadrunner.util.Angle; import com.arcrobotics.ftclib.command.CommandOpMode; import com.arcrobotics.ftclib.command.InstantCommand; +import com.arcrobotics.ftclib.command.SequentialCommandGroup; import com.arcrobotics.ftclib.command.WaitUntilCommand; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.util.MovingStatistics; @@ -51,18 +52,23 @@ public void initialize() { // TODO: if you haven't already, set the localizer to something that doesn't depend on // drive encoders for computing the heading + turnCommand = new TurnCommand(drive, Math.toRadians(ANGLE)); + telemetry.addLine("Press play to begin the track width tuner routine"); telemetry.addLine("Make sure your robot has enough clearance to turn smoothly"); telemetry.update(); - InstantCommand setupCommand = new InstantCommand(() -> { - telemetry.clearAll(); - telemetry.addLine("Running..."); - telemetry.update(); + SequentialCommandGroup setupCommand = new SequentialCommandGroup( + new WaitUntilCommand(this::isStarted), + new InstantCommand(() -> { + telemetry.clearAll(); + telemetry.addLine("Running..."); + telemetry.update(); - trackWidthStats = new MovingStatistics(NUM_TRIALS); - trial = 0; - }); + trackWidthStats = new MovingStatistics(NUM_TRIALS); + trial = 0; + } + )); InstantCommand finishCommand = new InstantCommand(() -> { telemetry.clearAll(); @@ -74,34 +80,34 @@ public void initialize() { }); RunCommand tuneCommand = new RunCommand(() -> { - if (turnCommand == null || !turnCommand.isScheduled()) { - if (turnCommand != null) { + if (trial < NUM_TRIALS && (turnCommand == null || !turnCommand.isScheduled())) { + if (headingAccumulator != 0) { double trackWidth = DriveConstants.TRACK_WIDTH * Math.toRadians(ANGLE) / headingAccumulator; trackWidthStats.add(trackWidth); } sleep(DELAY); + trial++; + drive.setPoseEstimate(new Pose2d()); // it is important to handle heading wraparounds headingAccumulator = 0; lastHeading = 0; - turnCommand = new TurnCommand(drive, ANGLE); + turnCommand = new TurnCommand(drive, Math.toRadians(ANGLE)); turnCommand.schedule(); - - trial++; } else { double heading = drive.getPoseEstimate().getHeading(); headingAccumulator += Angle.norm(heading - lastHeading); lastHeading = heading; } - }, drive); + }); schedule(setupCommand.andThen( - tuneCommand - .deadlineWith(new WaitUntilCommand(() -> trial == NUM_TRIALS)) + new WaitUntilCommand(() -> trial == NUM_TRIALS) + .deadlineWith(tuneCommand) .whenFinished(turnCommand::cancel), finishCommand )); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackingWheelLateralDistanceTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackingWheelLateralDistanceTuner.java index c6b2347f..964e0e8c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackingWheelLateralDistanceTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackingWheelLateralDistanceTuner.java @@ -115,24 +115,24 @@ public void initialize() { yButton = new GamepadButton(gamepad, GamepadKeys.Button.Y) .whenPressed(() -> turningFinished = true); - schedule(new RunCommand(() -> { - Pose2d vel = new Pose2d(0, 0, -gamepad1.right_stick_x); - drive.setDrivePower(vel); + schedule(new WaitUntilCommand(() -> turningFinished) + .deadlineWith(new RunCommand(() -> { + Pose2d vel = new Pose2d(0, 0, -gamepad1.right_stick_x); + drive.setDrivePower(vel); - double heading = drive.getPoseEstimate().getHeading(); - double deltaHeading = heading - lastHeading; + double heading = drive.getPoseEstimate().getHeading(); + double deltaHeading = heading - lastHeading; - headingAccumulator += Angle.normDelta(deltaHeading); - lastHeading = heading; + headingAccumulator += Angle.normDelta(deltaHeading); + lastHeading = heading; - telemetry.clearAll(); - telemetry.addLine("Total Heading (deg): " + Math.toDegrees(headingAccumulator)); - telemetry.addLine("Raw Heading (deg): " + Math.toDegrees(heading)); - telemetry.addLine(); - telemetry.addLine("Press Y/△ to conclude routine"); - telemetry.update(); - }, drive) - .deadlineWith(new WaitUntilCommand(() -> turningFinished)) + telemetry.clearAll(); + telemetry.addLine("Total Heading (deg): " + Math.toDegrees(headingAccumulator)); + telemetry.addLine("Raw Heading (deg): " + Math.toDegrees(heading)); + telemetry.addLine(); + telemetry.addLine("Press Y/△ to conclude routine"); + telemetry.update(); + }, drive) .whenFinished(() -> { telemetry.clearAll(); telemetry.addLine("Localizer's total heading: " + Math.toDegrees(headingAccumulator) + "°"); @@ -140,7 +140,7 @@ public void initialize() { (headingAccumulator / (NUM_TURNS * Math.PI * 2)) * StandardTrackingWheelLocalizer.LATERAL_DISTANCE); telemetry.update(); - }) + })) ); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TurnTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TurnTest.java index a07da5c3..edcbbdca 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TurnTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TurnTest.java @@ -4,7 +4,6 @@ import com.acmerobotics.roadrunner.geometry.Pose2d; import com.arcrobotics.ftclib.command.CommandOpMode; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.commands.TurnCommand; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/MecanumDriveSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/MecanumDriveSubsystem.java index 5faa646a..d8c74595 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/MecanumDriveSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/MecanumDriveSubsystem.java @@ -26,13 +26,6 @@ public class MecanumDriveSubsystem extends SubsystemBase { public MecanumDriveSubsystem(SampleMecanumDrive drive, boolean isFieldCentric) { this.drive = drive; fieldCentric = isFieldCentric; - init(); - } - - public void init() { - drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - drive.setMotorPowers(0, 0, 0, 0); - drive.setPoseEstimate(new Pose2d()); } public void setMode(DcMotor.RunMode mode) { @@ -47,8 +40,7 @@ public void setPoseEstimate(Pose2d pose) { drive.setPoseEstimate(pose); } - @Override - public void periodic() { + public void update() { drive.update(); } diff --git a/build.common.gradle b/build.common.gradle index 1fb5968d..907a79ae 100644 --- a/build.common.gradle +++ b/build.common.gradle @@ -23,7 +23,7 @@ def useProguard = false android { - compileSdkVersion 28 + compileSdkVersion 29 signingConfigs { debug { diff --git a/gradlew b/gradlew old mode 100755 new mode 100644 diff --git a/gradlew.bat b/gradlew.bat index 41da462a..8a0b282a 100644 --- a/gradlew.bat +++ b/gradlew.bat @@ -1,95 +1,3 @@ -<<<<<<< HEAD -@if "%DEBUG%" == "" @echo off -@rem ########################################################################## -@rem -@rem Gradle startup script for Windows -@rem -@rem ########################################################################## - -@rem Set local scope for the variables with windows NT shell -if "%OS%"=="Windows_NT" setlocal - -@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. -set DEFAULT_JVM_OPTS= - -set DIRNAME=%~dp0 -if "%DIRNAME%" == "" set DIRNAME=. -set APP_BASE_NAME=%~n0 -set APP_HOME=%DIRNAME% - -@rem Find java.exe -if defined JAVA_HOME goto findJavaFromJavaHome - -set JAVA_EXE=java.exe -%JAVA_EXE% -version >NUL 2>&1 -if "%ERRORLEVEL%" == "0" goto init - -echo. -echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. -echo. -echo Please set the JAVA_HOME variable in your environment to match the -echo location of your Java installation. - -goto fail - -:findJavaFromJavaHome -set JAVA_HOME=%JAVA_HOME:"=% -set JAVA_EXE=%JAVA_HOME%/bin/java.exe - -if exist "%JAVA_EXE%" goto init - -echo. -echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% -echo. -echo Please set the JAVA_HOME variable in your environment to match the -echo location of your Java installation. - -goto fail - -:init -@rem Get command-line arguments, handling Windowz variants - -if not "%OS%" == "Windows_NT" goto win9xME_args -if "%@eval[2+2]" == "4" goto 4NT_args - -:win9xME_args -@rem Slurp the command line arguments. -set CMD_LINE_ARGS= -set _SKIP=2 - -:win9xME_args_slurp -if "x%~1" == "x" goto execute - -set CMD_LINE_ARGS=%* -goto execute - -:4NT_args -@rem Get arguments from the 4NT Shell from JP Software -set CMD_LINE_ARGS=%$ - -:execute -@rem Setup the command line - -set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar - -@rem Execute Gradle -"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %CMD_LINE_ARGS% - -:end -@rem End local scope for the variables with windows NT shell -if "%ERRORLEVEL%"=="0" goto mainEnd - -:fail -rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of -rem the _cmd.exe /c_ return code! -if not "" == "%GRADLE_EXIT_CONSOLE%" exit 1 -exit /b 1 - -:mainEnd -if "%OS%"=="Windows_NT" endlocal - -:omega -======= @if "%DEBUG%" == "" @echo off @rem ########################################################################## @rem @@ -180,4 +88,3 @@ exit /b 1 if "%OS%"=="Windows_NT" endlocal :omega ->>>>>>> c05584b63b22a8823944d2320194644d55030820