Merge pull request #1 from DurhamAcademy/dev

.github/workflows/build.yml

@@ -8,11 +8,16 @@ jobs:
   build:
     name: Build
     runs-on: ubuntu-latest
-    container: wpilib/roborio-cross-ubuntu:2023-22.04
+    container: wpilib/roborio-cross-ubuntu:2023-22.04 #
     steps:
       - name: Checkout repository
         uses: actions/checkout@v2
       - name: Grant execute permission
         run: chmod +x gradlew
       - name: Build robot code
         run: ./gradlew build
+      - uses: actions/upload-artifact@v2  # upload test results
+        if: success() || failure()        # run this step even if previous step failed
+        with:
+          name: test-results
+          path: build/test-results/test/

.gitignore

@@ -164,4 +164,15 @@ bin/
 simgui*.json
 
 # Version file
-src/main/java/frc/robot/BuildConstants.java
+src/main/java/frc/robot/BuildConstants.java
+/*.wpilog
+/.idea/vcs.xml
+/.idea/gradle.xml
+/.idea/jarRepositories.xml
+/.idea/misc.xml
+/.glass/glass.json
+/.idea/modules.xml
+/networktables.json
+/.idea/modules/RewriteFRC2023.main.iml
+/.idea/modules/RewriteFRC2023.test.iml
+/.idea/compiler.xml

build.gradle

@@ -98,8 +98,8 @@ test {
 }
 
 // Simulation configuration (e.g. environment variables).
-wpi.sim.addGui()
-wpi.sim.addDriverstation()
+wpi.sim.addGui().defaultEnabled = true
+wpi.sim.addDriverstation().defaultEnabled = true
 
 // Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar')
 // in order to make them all available at runtime. Also adding the manifest so WPILib

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.REAL;
+    public static final Mode currentMode = Mode.REAL;
+    public static double loopPeriodSecs = 0.02;
 
-  public static 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,26 +4,25 @@
 
 package frc.robot;
 
-import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
-import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
-
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
-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 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.SpinAuto;
+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 java.io.IOException;
+
+import static edu.wpi.first.math.MathUtil.applyDeadband;
 
 /**
  * This class is where the bulk of the robot should be declared. Since
@@ -33,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(FLTurnMotorId, FLDriveMotorId, false, false, FLTurnEncoderId),
+                        new ModuleIOFalcon500(FRTurnMotorId, FRDriveMotorId, false, false, FRTurnEncoderId),
+                        new ModuleIOFalcon500(BLTurnMotorId, BLDriveMotorId, false, false, BLTurnEncoderId),
+                        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();
     }
 
-    // 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();
-  }
+    /**
+     * 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();
+    }
+
+    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();
+    }
+}