Write code for swerve drivetrain

src/main/java/frc/robot/Constants.java

@@ -15,9 +15,10 @@
  * wherever the constants are needed, to reduce verbosity.
  */
 public final class Constants {
-  public static final Mode currentMode = Mode.SIM;
+    public static final Mode currentMode = Mode.SIM;
+    public static double loopPeriodSecs = 0.02;
 
-  public enum Mode {
+    public enum Mode {
     /** Running on a real robot. */
     REAL,
 

src/main/java/frc/robot/Robot.java

@@ -4,15 +4,16 @@
 
 package frc.robot;
 
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import org.littletonrobotics.junction.LogFileUtil;
 import org.littletonrobotics.junction.LoggedRobot;
 import org.littletonrobotics.junction.Logger;
 import org.littletonrobotics.junction.networktables.NT4Publisher;
 import org.littletonrobotics.junction.wpilog.WPILOGReader;
 import org.littletonrobotics.junction.wpilog.WPILOGWriter;
 
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import java.io.IOException;
 
 /**
  * The VM is configured to automatically run this class, and to call the
@@ -44,7 +45,7 @@ public void robotInit() {
         logger.recordMetadata("GitDirty", "All changes committed");
         break;
       case 1:
-        logger.recordMetadata("GitDirty", "Uncomitted changes");
+        logger.recordMetadata("GitDirty", "Uncommitted changes");
         break;
       default:
         logger.recordMetadata("GitDirty", "Unknown");
@@ -56,6 +57,7 @@ public void robotInit() {
       // Running on a real robot, log to a USB stick
       case REAL:
         logger.addDataReceiver(new WPILOGWriter("/media/sda1/"));
+        logger.addDataReceiver(new WPILOGWriter("/media/sda2/"));
         logger.addDataReceiver(new NT4Publisher());
         break;
 
@@ -82,7 +84,11 @@ public void robotInit() {
 
     // Instantiate our RobotContainer. This will perform all our button bindings,
     // and put our autonomous chooser on the dashboard.
-    robotContainer = new RobotContainer();
+    try {
+      robotContainer = new RobotContainer();
+    } catch (IOException e) {
+      throw new RuntimeException(e);
+    }
   }
 
   /** This function is called periodically during all modes. */
@@ -94,6 +100,7 @@ public void robotPeriodic() {
     // This must be called from the robot's periodic block in order for anything in
     // the Command-based framework to work.
     CommandScheduler.getInstance().run();
+    robotContainer.periodic();
   }
 
   /** This function is called once when the robot is disabled. */

src/main/java/frc/robot/RobotContainer.java

@@ -4,25 +4,25 @@
 
 package frc.robot;
 
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
-import edu.wpi.first.wpilibj2.command.StartEndCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import frc.robot.commands.DriveWithFlywheelAuto;
 import frc.robot.commands.SpinAuto;
-import frc.robot.subsystems.drive.Drive;
-import frc.robot.subsystems.drive.DriveIO;
-import frc.robot.subsystems.drive.DriveIOSim;
-import frc.robot.subsystems.drive.DriveIOSparkMax;
-import frc.robot.subsystems.flywheel.Flywheel;
-import frc.robot.subsystems.flywheel.FlywheelIO;
-import frc.robot.subsystems.flywheel.FlywheelIOSim;
-import frc.robot.subsystems.flywheel.FlywheelIOSparkMax;
+import frc.robot.subsystems.battery.BatteryIO;
+import frc.robot.subsystems.battery.BatteryIOInputsAutoLogged;
+import frc.robot.subsystems.battery.BatteryIORio;
+import frc.robot.subsystems.battery.BatteryIOSim;
+import frc.robot.subsystems.drive.*;
+import org.littletonrobotics.junction.Logger;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
-import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
+
+import java.io.IOException;
+
+import static edu.wpi.first.math.MathUtil.applyDeadband;
 
 /**
  * This class is where the bulk of the robot should be declared. Since
@@ -32,73 +32,120 @@
  * commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
-  // Subsystems
-  private final Drive drive;
-  private final Flywheel flywheel;
-
-  // Controller
-  private final CommandXboxController controller = new CommandXboxController(0);
-
-  // Dashboard inputs
-  private final LoggedDashboardChooser<Command> autoChooser = new LoggedDashboardChooser<>("Auto Choices");
-  private final LoggedDashboardNumber flywheelSpeedInput = new LoggedDashboardNumber("Flywheel Speed", 1500.0);
-
-  /**
-   * The container for the robot. Contains subsystems, OI devices, and commands.
-   */
-  public RobotContainer() {
-    switch (Constants.currentMode) {
-      // Real robot, instantiate hardware IO implementations
-      case REAL:
-        drive = new Drive(new DriveIOSparkMax());
-        flywheel = new Flywheel(new FlywheelIOSparkMax());
-        // drive = new Drive(new DriveIOFalcon500());
-        // flywheel = new Flywheel(new FlywheelIOFalcon500());
-        break;
-
-      // Sim robot, instantiate physics sim IO implementations
-      case SIM:
-        drive = new Drive(new DriveIOSim());
-        flywheel = new Flywheel(new FlywheelIOSim());
-        break;
-
-      // Replayed robot, disable IO implementations
-      default:
-        drive = new Drive(new DriveIO() {
-        });
-        flywheel = new Flywheel(new FlywheelIO() {
-        });
-        break;
+    // Subsystems
+    private final Drive drive;
+//    private final Flywheel flywheel;
+
+    // Controller
+    private final CommandXboxController controller = new CommandXboxController(0);
+
+    // Dashboard inputs
+    private final LoggedDashboardChooser<Command> autoChooser = new LoggedDashboardChooser<>("Auto Choices");
+
+    // Battery IO
+    BatteryIO battery;
+    BatteryIOInputsAutoLogged batteryInputs = new BatteryIOInputsAutoLogged();
+    /**
+     * The container for the robot. Contains subsystems, OI devices, and commands.
+     */
+    public RobotContainer() throws IOException {
+        var FRDriveMotorId = 10;
+        var BLDriveMotorId = 11;
+        var FLDriveMotorId = 12;
+        var BRDriveMotorId = 13;
+
+        var FRTurnMotorId = 14;
+        var BLTurnMotorId = 15;
+        var FLTurnMotorId = 16;
+        var BRTurnMotorId = 17;
+
+        var FRTurnEncoderId = 6;
+        var BLTurnEncoderId = 7;
+        var FLTurnEncoderId = 8;
+        var BRTurnEncoderId = 9;
+
+        var gyroID = 20;
+        switch (Constants.currentMode) {
+            // Real robot, instantiate hardware IO implementations
+            case REAL:
+                battery = new BatteryIORio();
+                drive = new Drive(
+                        new ModuleIOFalcon500(FRTurnMotorId, FRDriveMotorId, false, false, FRTurnEncoderId),
+                        new ModuleIOFalcon500(BLTurnMotorId, BLDriveMotorId, false, false, BLTurnEncoderId),
+                        new ModuleIOFalcon500(FLTurnMotorId, FLDriveMotorId, false, false, FLTurnEncoderId),
+                        new ModuleIOFalcon500(BRTurnMotorId, BRDriveMotorId, false, false, BRTurnEncoderId),
+                        new GyroIOReal(gyroID, "rio"),
+                        new VisionIOPhoton());
+//                flywheel = new Flywheel(new FlywheelIOSparkMax());
+                break;
+
+            // Sim robot, instantiate physics sim IO implementations
+            case SIM:
+                battery = new BatteryIOSim();
+                battery.updateInputs(batteryInputs);
+                var gyroSim = new GyroIOSim(new Translation2d[]{new Translation2d(1.0, 1.0),
+                        new Translation2d(1.0, -1.0),
+                        new Translation2d(-1.0, 1.0),
+                        new Translation2d(-1.0, -1.0)});
+                drive = new Drive(new ModuleIOSim(0, gyroSim, (BatteryIOSim) battery, batteryInputs),
+                        new ModuleIOSim(1, gyroSim, (BatteryIOSim) battery, batteryInputs),
+                        new ModuleIOSim(2, gyroSim, (BatteryIOSim) battery, batteryInputs),
+                        new ModuleIOSim(3, gyroSim, (BatteryIOSim) battery, batteryInputs),
+                        gyroSim,
+                        new VisionIO() {
+                        });
+//                flywheel = new Flywheel(new FlywheelIOSim());
+                break;
+
+            // Replayed robot, disable IO implementations
+            default:
+                battery = new BatteryIO() {
+                };
+                drive = new Drive(new ModuleIO() {
+                }, new ModuleIO() {
+                }, new ModuleIO() {
+                }, new ModuleIO() {
+                }, new GyroIO() {
+                }, new VisionIO() {
+                });
+//                flywheel = new Flywheel(new FlywheelIO() {
+//                });
+                break;
+        }
+
+        // Set up auto routines
+        autoChooser.addDefaultOption("Do Nothing", new InstantCommand());
+        autoChooser.addOption("Spin", new SpinAuto(drive));
+//        autoChooser.addOption("Drive With Flywheel", new DriveWithFlywheelAuto(drive, flywheel));
+
+        // Configure the button bindings
+        configureButtonBindings();
+    }
+
+    /**
+     * Use this method to define your button->command mappings. Buttons can be
+     * created by instantiating a {@link GenericHID} or one of its subclasses
+     * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
+     * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+     */
+    private void configureButtonBindings() {
+        drive.setDefaultCommand(
+                new RunCommand(() -> drive.driveArcade(applyDeadband(controller.getLeftX(), 0.05), applyDeadband(controller.getLeftY(), 0.05), applyDeadband(controller.getRightX(), 0.05), true), drive));
+//        controller.a()
+//                .whileTrue(new StartEndCommand(() -> flywheel.runVelocity(flywheelSpeedInput.get()), flywheel::stop, flywheel));
+    }
+
+    /**
+     * Use this to pass the autonomous command to the main {@link Robot} class.
+     *
+     * @return the command to run in autonomous
+     */
+    public Command getAutonomousCommand() {
+        return autoChooser.get();
     }
 
-    // Set up auto routines
-    autoChooser.addDefaultOption("Do Nothing", new InstantCommand());
-    autoChooser.addOption("Spin", new SpinAuto(drive));
-    autoChooser.addOption("Drive With Flywheel", new DriveWithFlywheelAuto(drive, flywheel));
-
-    // Configure the button bindings
-    configureButtonBindings();
-  }
-
-  /**
-   * Use this method to define your button->command mappings. Buttons can be
-   * created by instantiating a {@link GenericHID} or one of its subclasses
-   * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
-   * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
-   */
-  private void configureButtonBindings() {
-    drive.setDefaultCommand(
-        new RunCommand(() -> drive.driveArcade(-controller.getLeftY(), controller.getLeftX()), drive));
-    controller.a()
-        .whileTrue(new StartEndCommand(() -> flywheel.runVelocity(flywheelSpeedInput.get()), flywheel::stop, flywheel));
-  }
-
-  /**
-   * Use this to pass the autonomous command to the main {@link Robot} class.
-   *
-   * @return the command to run in autonomous
-   */
-  public Command getAutonomousCommand() {
-    return autoChooser.get();
-  }
+    public void periodic() {
+        this.battery.updateInputs(batteryInputs);
+        Logger.getInstance().processInputs("Battery", batteryInputs);
+    }
 }

src/main/java/frc/robot/commands/DriveWithFlywheelAuto.java

@@ -17,7 +17,7 @@ public class DriveWithFlywheelAuto extends SequentialCommandGroup {
    */
   public DriveWithFlywheelAuto(Drive drive, Flywheel flywheel) {
     addCommands(
-        new StartEndCommand(() -> drive.drivePercent(drivePercent, -drivePercent), drive::stop, drive)
+            new StartEndCommand(() -> drive.driveArcade(0.0, drivePercent, 0.0, false), drive::stop, drive)
             .withTimeout(driveDuration),
         new StartEndCommand(() -> flywheel.runVelocity(flywheelSpeed), flywheel::stop, flywheel)
             .withTimeout(flywheelDuration));

src/main/java/frc/robot/commands/SpinAuto.java

@@ -11,7 +11,7 @@ public class SpinAuto extends SequentialCommandGroup {
   /** Creates a new SpinAuto, which spins in place for ten seconds. */
   public SpinAuto(Drive drive) {
     addCommands(
-        new StartEndCommand(() -> drive.drivePercent(drivePercent, -drivePercent), drive::stop, drive)
+            new StartEndCommand(() -> drive.driveArcade(0.0, 0.0, -drivePercent, true), drive::stop, drive)
             .withTimeout(duration));
   }
 }

src/main/java/frc/robot/subsystems/battery/BatteryIO.java

@@ -0,0 +1,13 @@
+package frc.robot.subsystems.battery;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface BatteryIO {
+    default void updateInputs(BatteryIOInputs inputs) {
+    }
+
+    @AutoLog
+    class BatteryIOInputs {
+        public double voltage = 0.0;
+    }
+}

src/main/java/frc/robot/subsystems/battery/BatteryIORio.java

@@ -0,0 +1,10 @@
+package frc.robot.subsystems.battery;
+
+import edu.wpi.first.wpilibj.RobotController;
+
+public class BatteryIORio implements BatteryIO {
+    @Override
+    public void updateInputs(BatteryIOInputs inputs) {
+        inputs.voltage = RobotController.getBatteryVoltage();
+    }
+}

src/main/java/frc/robot/subsystems/battery/BatteryIOSim.java

@@ -0,0 +1,26 @@
+package frc.robot.subsystems.battery;
+
+import edu.wpi.first.wpilibj.simulation.BatterySim;
+
+import java.util.ArrayList;
+import java.util.List;
+
+public class BatteryIOSim implements BatteryIO {
+
+    List<Double> currents = new ArrayList<>();
+
+    @Override
+    public void updateInputs(BatteryIOInputs inputs) {
+        var total = new double[currents.size() == 0 ? 1 : currents.size()];
+        if (currents.size() == 0) total[0] = 0.0;
+        for (int i = 0; i < currents.size(); i++) {
+            total[i] = currents.get(i);
+        }
+        inputs.voltage = BatterySim.calculateDefaultBatteryLoadedVoltage(total);
+        currents.clear();
+    }
+
+    public void addCurrent(double current) {
+        this.currents.add(current);
+    }
+}