From ccf29dc156372850ed57d9ee9603cec0e7efe6a2 Mon Sep 17 00:00:00 2001 From: Allen Date: Wed, 15 Oct 2025 11:21:45 -0500 Subject: [PATCH] Starting point for Lesson 4 --- src/main/java/vv/Main.java | 2 +- src/main/java/vv/lesson2/Robot.java | 2 +- .../climber/Climber.java | 2 +- .../vv/lesson2/controls/OperatorControls.java | 2 +- src/main/java/vv/lesson4/Robot.java | 45 +++++++ .../lesson4/commands/MakeSwerveRequest.java | 34 +++++ .../lesson4/commands/MoveRobotRelative.java | 67 ++++++++++ .../vv/lesson4/controls/DriverControls.java | 59 +++++++++ .../drivetrain/Drivetrain.java | 2 +- .../drivetrain/DrivetrainFactory.java | 2 +- .../vv/{ => lesson4}/pid/VVConstraints.java | 2 +- .../{ => lesson4}/pid/VVPIDControllers.java | 2 +- src/test/java/vv/lesson4/DrivetrainTests.java | 122 ++++++++++++++++++ 13 files changed, 335 insertions(+), 8 deletions(-) rename src/main/java/vv/{subsystems => lesson2}/climber/Climber.java (99%) create mode 100644 src/main/java/vv/lesson4/Robot.java create mode 100644 src/main/java/vv/lesson4/commands/MakeSwerveRequest.java create mode 100644 src/main/java/vv/lesson4/commands/MoveRobotRelative.java create mode 100644 src/main/java/vv/lesson4/controls/DriverControls.java rename src/main/java/vv/{subsystems => lesson4}/drivetrain/Drivetrain.java (99%) rename src/main/java/vv/{subsystems => lesson4}/drivetrain/DrivetrainFactory.java (99%) rename src/main/java/vv/{ => lesson4}/pid/VVConstraints.java (95%) rename src/main/java/vv/{ => lesson4}/pid/VVPIDControllers.java (97%) create mode 100644 src/test/java/vv/lesson4/DrivetrainTests.java diff --git a/src/main/java/vv/Main.java b/src/main/java/vv/Main.java index e885603..aa7ba39 100644 --- a/src/main/java/vv/Main.java +++ b/src/main/java/vv/Main.java @@ -1,7 +1,7 @@ package vv; import edu.wpi.first.wpilibj.RobotBase; -import vv.lesson3.Robot; +import vv.lesson4.Robot; public class Main { public static void main(String[] args) { diff --git a/src/main/java/vv/lesson2/Robot.java b/src/main/java/vv/lesson2/Robot.java index fa63e4b..c4c1a6d 100644 --- a/src/main/java/vv/lesson2/Robot.java +++ b/src/main/java/vv/lesson2/Robot.java @@ -3,9 +3,9 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.CommandScheduler; import vv.config.VVConfig; +import vv.lesson2.climber.Climber; import vv.lesson2.controls.DriverControls; import vv.lesson2.controls.OperatorControls; -import vv.subsystems.climber.Climber; public class Robot extends TimedRobot{ diff --git a/src/main/java/vv/subsystems/climber/Climber.java b/src/main/java/vv/lesson2/climber/Climber.java similarity index 99% rename from src/main/java/vv/subsystems/climber/Climber.java rename to src/main/java/vv/lesson2/climber/Climber.java index a2f84cd..9ff47d7 100644 --- a/src/main/java/vv/subsystems/climber/Climber.java +++ b/src/main/java/vv/lesson2/climber/Climber.java @@ -1,4 +1,4 @@ -package vv.subsystems.climber; +package vv.lesson2.climber; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; diff --git a/src/main/java/vv/lesson2/controls/OperatorControls.java b/src/main/java/vv/lesson2/controls/OperatorControls.java index d1f18fd..2270013 100644 --- a/src/main/java/vv/lesson2/controls/OperatorControls.java +++ b/src/main/java/vv/lesson2/controls/OperatorControls.java @@ -5,7 +5,7 @@ import edu.wpi.first.wpilibj.simulation.XboxControllerSim; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import vv.config.VVConfig; -import vv.subsystems.climber.Climber; +import vv.lesson2.climber.Climber; public class OperatorControls { private final CommandXboxController controller; diff --git a/src/main/java/vv/lesson4/Robot.java b/src/main/java/vv/lesson4/Robot.java new file mode 100644 index 0000000..65e8081 --- /dev/null +++ b/src/main/java/vv/lesson4/Robot.java @@ -0,0 +1,45 @@ +package vv.lesson4; + +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import vv.config.VVConfig; +import vv.lesson4.controls.DriverControls; +import vv.lesson4.drivetrain.Drivetrain; +import vv.lesson4.drivetrain.DrivetrainFactory; + +public class Robot extends TimedRobot { + + public static Robot instance = null; + + VVConfig config; + DriverControls driverControls; + Drivetrain drivetrain; + + private Robot() { + config = VVConfig.readFromDeployDirectory("practice-robot.properties"); + driverControls = new DriverControls(config); + drivetrain = DrivetrainFactory.createDrivetrain(config); + setupTriggers(); + } + + @Override + public void robotPeriodic() { + CommandScheduler.getInstance().run(); + } + + @Override + public void testInit() { + // Put the command you want to test here + } + + private void setupTriggers() { + driverControls.setupTriggers(config, drivetrain); + } + + public static Robot start() { + if (Robot.instance == null) { + Robot.instance = new Robot(); + } + return Robot.instance; + } +} \ No newline at end of file diff --git a/src/main/java/vv/lesson4/commands/MakeSwerveRequest.java b/src/main/java/vv/lesson4/commands/MakeSwerveRequest.java new file mode 100644 index 0000000..e687646 --- /dev/null +++ b/src/main/java/vv/lesson4/commands/MakeSwerveRequest.java @@ -0,0 +1,34 @@ +package vv.lesson4.commands; + +import com.ctre.phoenix6.swerve.SwerveRequest; + +import edu.wpi.first.wpilibj2.command.Command; +import vv.lesson4.drivetrain.Drivetrain; + +public class MakeSwerveRequest extends Command { + private final Drivetrain drivetrain; + private final SwerveRequest req; + + public MakeSwerveRequest(Drivetrain drivetrain, SwerveRequest req) { + super(); + this.drivetrain = drivetrain; + this.req = req; + addRequirements(drivetrain); + } + + @Override + public void initialize() { + System.out.println("Starting MakeSwerveRequest command"); + } + + @Override + public void execute() { + drivetrain.setControl(req); + } + + @Override + public void end(boolean interrupted) { + System.out.println("Ending MakeSwerveRequest command "); + drivetrain.setControl(new SwerveRequest.Idle()); + } +} diff --git a/src/main/java/vv/lesson4/commands/MoveRobotRelative.java b/src/main/java/vv/lesson4/commands/MoveRobotRelative.java new file mode 100644 index 0000000..d977e0f --- /dev/null +++ b/src/main/java/vv/lesson4/commands/MoveRobotRelative.java @@ -0,0 +1,67 @@ +package vv.lesson4.commands; + +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile.State; +import edu.wpi.first.wpilibj2.command.Command; +import vv.config.VVConfig; +import vv.lesson4.drivetrain.Drivetrain; +import static vv.lesson4.pid.VVPIDControllers.generalRotationController; +import static vv.lesson4.pid.VVPIDControllers.generalTranslationController; + +public class MoveRobotRelative extends Command { + private final boolean debugLogging; + private final Drivetrain drivetrain; + private final Transform2d transform; + private final ProfiledPIDController xController; + private final ProfiledPIDController yController; + private final ProfiledPIDController rotController; + + public MoveRobotRelative(VVConfig config, Drivetrain drivetrain, Transform2d transform) { + this(config, drivetrain, transform, false); + } + + public MoveRobotRelative(VVConfig config, Drivetrain drivetrain, Transform2d transform, boolean debugLogging) { + this.drivetrain = drivetrain; + this.transform = transform; + this.debugLogging = debugLogging; + + this.xController = generalTranslationController(config); + this.yController = generalTranslationController(config); + this.rotController = generalRotationController(config); + addRequirements(drivetrain); + } + + @Override + public void initialize() { + if (debugLogging) { + System.out.println("Initializing MoveRobotRelative command with transform: " + transform); + } + var targetPose = this.drivetrain.getState().Pose.transformBy(transform); + xController.setGoal(new State(targetPose.getX(), 0)); + yController.setGoal(new State(targetPose.getY(), 0)); + rotController.setGoal(new State(targetPose.getRotation().getRadians(), 0)); + } + + @Override + public void execute() { + var currentPose = this.drivetrain.getState().Pose; + var vx = xController.calculate(currentPose.getX()); + var vy = yController.calculate(currentPose.getY()); + var omega = rotController.calculate(currentPose.getRotation().getRadians()); + drivetrain.driveFieldRelative(vx, vy, omega); + } + + @Override + public void end(boolean interrupted) { + if (debugLogging) { + System.out.println("Ending MoveRobotRelative command, interrupted: " + interrupted); + } + drivetrain.stop(); + } + + @Override + public boolean isFinished() { + return xController.atGoal() && yController.atGoal() && rotController.atGoal(); + } +} \ No newline at end of file diff --git a/src/main/java/vv/lesson4/controls/DriverControls.java b/src/main/java/vv/lesson4/controls/DriverControls.java new file mode 100644 index 0000000..90670a8 --- /dev/null +++ b/src/main/java/vv/lesson4/controls/DriverControls.java @@ -0,0 +1,59 @@ +package vv.lesson4.controls; + +import java.util.function.Consumer; + +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.swerve.SwerveRequest; + +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import edu.wpi.first.wpilibj.simulation.XboxControllerSim; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import vv.config.VVConfig; +import vv.lesson4.drivetrain.Drivetrain; + +public class DriverControls { + private final CommandXboxController controller; + private final XboxControllerSim sim; + + public DriverControls( + VVConfig config + ) { + var driverConfig = config.controllers().driver(); + controller = new CommandXboxController(driverConfig.port()); + sim = new XboxControllerSim(controller.getHID()); + } + + public void setupTriggers(VVConfig config, Drivetrain drivetrain) { + var maxLinearSpeed = config.drivetrain().constants().maxLinearSpeed().in(MetersPerSecond); + var maxRotationalSpeed = config.drivetrain().constants().maxRotationsPerSecond().in(RadiansPerSecond); + var translationalDeadband = config.controllers().driver().translationalDeadband(); + var rotationalDeadband = config.controllers().driver().rotationalDeadband(); + + drivetrain.setDefaultCommand( + drivetrain.applyRequest(() -> new SwerveRequest.FieldCentric() + .withDeadband(maxLinearSpeed * translationalDeadband) + .withRotationalDeadband(maxRotationalSpeed * rotationalDeadband) + .withDriveRequestType(DriveRequestType.OpenLoopVoltage) + .withVelocityX(drivetrain.clampLinearVelocity(-controller.getLeftY() * maxLinearSpeed)) + .withVelocityY(drivetrain.clampLinearVelocity(-controller.getLeftX() * maxLinearSpeed)) + .withRotationalRate(drivetrain.clampAngularVelocity(-controller.getRightX() * maxRotationalSpeed)) + ) + .withName("DriveWithController") + ); + + } + + public CommandXboxController controls() { + return controller; + } + + public XboxControllerSim sim() { + return sim; + } + + public void simulate(Consumer consumer) { + consumer.accept(sim); + sim.notifyNewData(); + } +} \ No newline at end of file diff --git a/src/main/java/vv/subsystems/drivetrain/Drivetrain.java b/src/main/java/vv/lesson4/drivetrain/Drivetrain.java similarity index 99% rename from src/main/java/vv/subsystems/drivetrain/Drivetrain.java rename to src/main/java/vv/lesson4/drivetrain/Drivetrain.java index 5dd265b..fb3300e 100644 --- a/src/main/java/vv/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/vv/lesson4/drivetrain/Drivetrain.java @@ -1,5 +1,5 @@ -package vv.subsystems.drivetrain; +package vv.lesson4.drivetrain; import java.util.function.Supplier; diff --git a/src/main/java/vv/subsystems/drivetrain/DrivetrainFactory.java b/src/main/java/vv/lesson4/drivetrain/DrivetrainFactory.java similarity index 99% rename from src/main/java/vv/subsystems/drivetrain/DrivetrainFactory.java rename to src/main/java/vv/lesson4/drivetrain/DrivetrainFactory.java index 6aedaeb..38241c8 100644 --- a/src/main/java/vv/subsystems/drivetrain/DrivetrainFactory.java +++ b/src/main/java/vv/lesson4/drivetrain/DrivetrainFactory.java @@ -1,5 +1,5 @@ -package vv.subsystems.drivetrain; +package vv.lesson4.drivetrain; import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.configs.CANcoderConfiguration; diff --git a/src/main/java/vv/pid/VVConstraints.java b/src/main/java/vv/lesson4/pid/VVConstraints.java similarity index 95% rename from src/main/java/vv/pid/VVConstraints.java rename to src/main/java/vv/lesson4/pid/VVConstraints.java index 8596a31..52d594e 100644 --- a/src/main/java/vv/pid/VVConstraints.java +++ b/src/main/java/vv/lesson4/pid/VVConstraints.java @@ -1,5 +1,5 @@ -package vv.pid; +package vv.lesson4.pid; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import vv.config.VVConfig; diff --git a/src/main/java/vv/pid/VVPIDControllers.java b/src/main/java/vv/lesson4/pid/VVPIDControllers.java similarity index 97% rename from src/main/java/vv/pid/VVPIDControllers.java rename to src/main/java/vv/lesson4/pid/VVPIDControllers.java index 5834abc..120fa15 100644 --- a/src/main/java/vv/pid/VVPIDControllers.java +++ b/src/main/java/vv/lesson4/pid/VVPIDControllers.java @@ -1,4 +1,4 @@ -package vv.pid; +package vv.lesson4.pid; import edu.wpi.first.math.controller.ProfiledPIDController; import vv.config.VVConfig; diff --git a/src/test/java/vv/lesson4/DrivetrainTests.java b/src/test/java/vv/lesson4/DrivetrainTests.java new file mode 100644 index 0000000..54d7632 --- /dev/null +++ b/src/test/java/vv/lesson4/DrivetrainTests.java @@ -0,0 +1,122 @@ +package vv.lesson4; + +import java.util.concurrent.TimeUnit; + +import static org.awaitility.Awaitility.await; +import static org.junit.jupiter.api.Assertions.assertEquals; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Disabled; +import org.junit.jupiter.api.Test; + +import com.ctre.phoenix6.swerve.SwerveRequest; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import vv.lesson4.commands.MakeSwerveRequest; +import vv.lesson4.commands.MoveRobotRelative; +import vv.lesson4.drivetrain.Drivetrain; +import vv.lesson4.drivetrain.DrivetrainFactory; +import static vv.utils.TestSetup.CONFIG; +import static vv.utils.TestSetup.isFinished; +import static vv.utils.TestSetup.resetSimulationState; + +public class DrivetrainTests { + + Drivetrain drivetrain; + + @BeforeEach + @SuppressWarnings("unused") + void beforeEach() { + resetSimulationState(); + drivetrain = DrivetrainFactory.createDrivetrain(CONFIG); + drivetrain.register(); + } + + @Test + @Disabled + /** + * Enable this test to see the inaccuracy of the open-loop request. + * See {@link DrivetrainTests#testMoveForward1MeterCloseLoop} for an + * example of a closed-loop request that works correctly. + */ + void testMoveForward1MeterOpenLoop() { + // Arrange + var targetDistance = 1; + SwerveRequest forwardRequest = new SwerveRequest.RobotCentric() + .withVelocityX(1.0) + .withVelocityY(0.0) + .withRotationalRate(0.0); + + Command cmd = new MakeSwerveRequest(drivetrain, forwardRequest).withTimeout(targetDistance); + double startX = drivetrain.getState().Pose.getMeasureX().baseUnitMagnitude(); + double startY = drivetrain.getState().Pose.getMeasureY().baseUnitMagnitude(); + double startRotation = drivetrain.getState().Pose.getRotation().getDegrees(); + + // Act + drivetrain.logPose(); + CommandScheduler.getInstance().schedule(cmd); + await().atMost(5, TimeUnit.SECONDS).until(() -> { + CommandScheduler.getInstance().run(); + return isFinished(cmd); + }); + drivetrain.logPose(); + + // Assert + double finalX = drivetrain.getState().Pose.getMeasureX().baseUnitMagnitude(); + double finalY = drivetrain.getState().Pose.getMeasureY().baseUnitMagnitude(); + double finalRotation = drivetrain.getState().Pose.getRotation().getDegrees(); + + System.out.println("Rel Error X: " + (finalX - startX - targetDistance)/targetDistance * 100); + + assertEquals(targetDistance, Math.abs(finalX - startX), 0.5, "Didn't move far enough in X"); + assertEquals(startY, finalY, 0.5, "Should not have moved in Y"); + assertEquals(startRotation, finalRotation, 1, "Should not have rotated"); + } + + /** + * A closed-loop version of {@link DrivetrainTests#testMoveForward1MeterOpenLoop}. + * This uses default PID controllers in {@link MoveRobotRelative} to calculate + * the required velocities and to determine whether the the robot has moved far + * enough. + */ + @Test + void testMoveForward1MeterCloseLoop() { + // Arrange + long targetDistance = 1; + var move = new Transform2d(new Translation2d(targetDistance, 0), Rotation2d.kZero); + Command cmd = new MoveRobotRelative(CONFIG, drivetrain, move); + + double startX = drivetrain.getState().Pose.getMeasureX().baseUnitMagnitude(); + double startY = drivetrain.getState().Pose.getMeasureY().baseUnitMagnitude(); + double startRotation = drivetrain.getState().Pose.getRotation().getDegrees(); + drivetrain.logPose(); + + // Act + CommandScheduler.getInstance().schedule(cmd); + try { + await().atMost(5, TimeUnit.SECONDS).until(() -> { + CommandScheduler.getInstance().run(); + return isFinished(cmd); + }); + } catch (Exception e) { + System.err.println("Command did not finish in time: " + e.getMessage()); + } finally { + System.out.println("=== Final Pose ==="); + drivetrain.logPose(); + } + + // Assert + double finalX = drivetrain.getState().Pose.getMeasureX().baseUnitMagnitude(); + double finalY = drivetrain.getState().Pose.getMeasureY().baseUnitMagnitude(); + double finalRotation = drivetrain.getState().Pose.getRotation().getDegrees(); + + System.out.println("Rel Error X: " + (finalX - startX - targetDistance)/targetDistance * 100 + "%"); + + assertEquals(targetDistance, Math.abs(finalX - startX), .1, "Didn't move far enough in X"); + assertEquals(startY, finalY, .1, "Should not have moved in Y"); + assertEquals(startRotation, finalRotation, 5, "Should not have rotated"); + } +}