Lesson 4

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) {

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{
 

src/main/java/vv/subsystems/climber/Climber.java → 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;

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;

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;
+    }
+}

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());
+    }
+}

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();
+    }
+}

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<XboxControllerSim> consumer) {
+        consumer.accept(sim);
+        sim.notifyNewData();
+    }
+}

src/main/java/vv/subsystems/drivetrain/Drivetrain.java → 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;
 

src/main/java/vv/subsystems/drivetrain/DrivetrainFactory.java → 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;

src/main/java/vv/pid/VVConstraints.java → 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;

src/main/java/vv/pid/VVPIDControllers.java → 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;

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");    
+    }
+}