Branch number two: Electric boogaloo

src/main/java/frc/lib/advantagekit/LoggedIO.java

@@ -0,0 +1,5 @@
+package frc.lib.advantagekit;
+
+public interface LoggedIO<T> {
+    public void updateInputs(T inputs);
+}

src/main/java/frc/lib/utils/CanSparkMaxSetup.java

@@ -0,0 +1,15 @@
+package frc.lib.utils;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMaxLowLevel.PeriodicFrame;
+
+public class CanSparkMaxSetup {
+  public void setupSparkMaxSlow(CANSparkMax sparkMax) {
+    // sparkMax.setPeriodicFramePeriod(PeriodicFrame.kStatus1, 500);
+    sparkMax.setPeriodicFramePeriod(PeriodicFrame.kStatus2, 20);
+    sparkMax.setPeriodicFramePeriod(PeriodicFrame.kStatus3, 500);
+    // sparkMax.setPeriodicFramePeriod(PeriodicFrame.kStatus4, 500);
+    sparkMax.setPeriodicFramePeriod(PeriodicFrame.kStatus5, 500);
+    sparkMax.setPeriodicFramePeriod(PeriodicFrame.kStatus6, 500);
+  }
+}

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

@@ -4,6 +4,11 @@
 
 package frc.robot;
 
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.util.Units;
+import frc.robot.util.TunableNumber;
+
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
  * constants. This class should not be used for any other purpose. All constants should be declared
@@ -13,4 +18,44 @@
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
+    public static final boolean tuningMode = true;
+
+    public static final class DriveConstants {
+        public static final class Pigeon2AccelConstants {
+            public static final double kMultiplier = 1;
+            public static final int kFixedShift = 14;
+            public static final int kX = 0;
+            public static final int kY = 1;
+            public static final int kZ = 2;
+        }
+
+
+        //wheel dims
+        public static final double kWheelDiameter = Units.inchesToMeters(3.7);
+        public static final double kWheelBase = Units.inchesToMeters(23);
+        public static final double kTrackWidth = Units.inchesToMeters(23);
+
+        //module offsets
+        public static Translation2d[] kModuleTranslations = {
+            new Translation2d(kWheelBase / 2.0, kTrackWidth / 2.0), // front left
+            new Translation2d(kWheelBase / 2.0, -kTrackWidth / 2.0), // front right
+            new Translation2d(-kWheelBase / 2.0, kTrackWidth / 2.0), // rear left
+            new Translation2d(-kWheelBase / 2.0, -kTrackWidth / 2.0) // rear right
+        };
+
+        public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics(kModuleTranslations);
+
+    }
+
+    public static final class ModuleConstants {
+        public static final TunableNumber kDriveP = new TunableNumber("Drive P", 0.1);
+        public static final TunableNumber kDriveI = new TunableNumber("Drive I", 0.0);
+        public static final TunableNumber kDriveD = new TunableNumber("Drive D", 0.00);
+        public static final TunableNumber kDriveFF = new TunableNumber("Drive FF", 2.96);
+
+        public static final TunableNumber kTurningP = new TunableNumber("TurnP", 0.6);
+        public static final TunableNumber kTurningI = new TunableNumber("Turn I", 0.00);
+        public static final TunableNumber kTurningD = new TunableNumber("Turn D", 0.005);
+
+    }
 }

src/main/java/frc/robot/Main.java

@@ -12,15 +12,14 @@
  * call.
  */
 public final class Main {
-    private Main() {
-    }
+  private Main() {}
 
-    /**
-     * Main initialization function. Do not perform any initialization here.
-     *
-     * <p>If you change your main robot class, change the parameter type.
-     */
-    public static void main(String... args) {
-        RobotBase.startRobot(Robot::new);
-    }
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
 }

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

@@ -6,96 +6,128 @@
 
 import org.littletonrobotics.junction.LoggedRobot;
 
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
 /**
- * The VM is configured to automatically run this class, and to call the functions corresponding to
- * each mode, as described in the TimedRobot documentation. If you change the name of this class or
- * the package after creating this project, you must also update the build.gradle file in the
+ * The VM is configured to automatically run this class, and to call the
+ * functions corresponding to each mode, as described in the TimedRobot
+ * documentation. If you change the name of this class or the package after
+ * creating this project, you must also update the build.gradle file in the
  * project.
  */
 public class Robot extends LoggedRobot {
-    private Command m_autonomousCommand;
-
-    private RobotContainer m_robotContainer;
-
-    /**
-     * This function is run when the robot is first started up and should be used for any
-     * initialization code.
-     */
-    @Override
-    public void robotInit() {
-        // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
-        // autonomous chooser on the dashboard.
-        m_robotContainer = new RobotContainer();
+  private Command autonomousCommand;
+  private Command testCommand;
+  private RobotContainer robotContainer;
+
+  /**
+   * This function is run when the robot is first started up and should be used
+   * for any initialization code.
+   */
+  @Override
+  public void robotInit() {
+
+    if (Robot.isSimulation()) {
+      DriverStation.silenceJoystickConnectionWarning(true);
     }
 
-    /**
-     * This function is called every robot packet, no matter the mode. Use this for items like
-     * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
-     *
-     * <p>This runs after the mode specific periodic functions, but before LiveWindow and
-     * SmartDashboard integrated updating.
-     */
-    @Override
-    public void robotPeriodic() {
-        // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
-        // commands, running already-scheduled commands, removing finished or interrupted commands,
-        // and running subsystem periodic() methods.  This must be called from the robot's periodic
-        // block in order for anything in the Command-based framework to work.
-        CommandScheduler.getInstance().run();
+    // Instantiate our RobotContainer. This will perform all our button bindings,
+    // and put our autonomous chooser on the dashboard.
+    robotContainer = new RobotContainer();
+  }
+
+  /** This function is called periodically during all modes. */
+  @Override
+  public void robotPeriodic() {
+    // Runs the Scheduler. This is responsible for polling buttons, adding
+    // newly-scheduled commands, running already-scheduled commands, removing
+    // finished or interrupted commands, and running subsystem periodic() methods.
+    // 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.updateRobotState();
+  }
+
+  /** This function is called once when the robot is disabled. */
+  @Override
+  public void disabledInit() {
+
+    robotContainer.onDisabled();
+  }
+
+  /** This function is called periodically when disabled. */
+  @Override
+  public void disabledPeriodic() {
+    robotContainer.disabledPeriodic();
+  }
+
+  /**
+   * This autonomous runs the autonomous command selected by your
+   * {@link RobotContainer} class.
+   */
+  @Override
+  public void autonomousInit() {
+    robotContainer.onEnabled();
+    autonomousCommand = robotContainer.getAutonomousCommand();
+
+    // schedule the autonomous command (example)
+    if (autonomousCommand != null) {
+      autonomousCommand.schedule();
     }
-
-    /** This function is called once each time the robot enters Disabled mode. */
-    @Override
-    public void disabledInit() {
+  }
+
+  /** This function is called periodically during autonomous. */
+  @Override
+  public void autonomousPeriodic() {
+  }
+
+  /** This function is called once when teleop is enabled. */
+  @Override
+  public void teleopInit() {
+    // This makes sure that the autonomous stops running when
+    // teleop starts running. If you want the autonomous to
+    // continue until interrupted by another command, remove
+    // this line or comment it out.
+    if (autonomousCommand != null) {
+      autonomousCommand.cancel();
     }
-
-    @Override
-    public void disabledPeriodic() {
+    robotContainer.onEnabled();
+  }
+
+  /** This function is called periodically during operator control. */
+  @Override
+  public void teleopPeriodic() {
+
+  }
+
+  /** This function is called once when test mode is enabled. */
+  @Override
+  public void testInit() {
+    // Cancels all running commands at the start of test mode.
+    CommandScheduler.getInstance().cancelAll();
+    robotContainer.onEnabled();
+    if (testCommand != null) {
+      testCommand.schedule();
+    } else {
+      System.out.println("No test command");
     }
 
-    /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
-    @Override
-    public void autonomousInit() {
-        m_autonomousCommand = m_robotContainer.getAutonomousCommand();
-
-        // schedule the autonomous command (example)
-        if (m_autonomousCommand != null) {
-            m_autonomousCommand.schedule();
-        }
-    }
+  }
 
-    /** This function is called periodically during autonomous. */
-    @Override
-    public void autonomousPeriodic() {
-    }
+  /** This function is called periodically during test mode. */
+  @Override
+  public void testPeriodic() {
+  }
 
-    @Override
-    public void teleopInit() {
-        // This makes sure that the autonomous stops running when
-        // teleop starts running. If you want the autonomous to
-        // continue until interrupted by another command, remove
-        // this line or comment it out.
-        if (m_autonomousCommand != null) {
-            m_autonomousCommand.cancel();
-        }
-    }
+  /** This function is called once when the robot is first started up. */
+  @Override
+  public void simulationInit() {
+  }
 
-    /** This function is called periodically during operator control. */
-    @Override
-    public void teleopPeriodic() {
-    }
-
-    @Override
-    public void testInit() {
-        // Cancels all running commands at the start of test mode.
-        CommandScheduler.getInstance().cancelAll();
-    }
-
-    /** This function is called periodically during test mode. */
-    @Override
-    public void testPeriodic() {
-    }
+  /** This function is called periodically whilst in simulation. */
+  @Override
+  public void simulationPeriodic() {
+  }
 }

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

@@ -39,6 +39,19 @@ private void configureButtonBindings() {
      */
     public Command getAutonomousCommand() {
         // An ExampleCommand will run in autonomous
-        return null; //CHANGE THIS OR DIE TMR
+        return null;
     }
-}
+
+    public void onEnabled() {
+    }
+
+    public void disabledPeriodic() {
+    }
+
+    public void onDisabled() {
+    }
+
+    public void updateRobotState() {
+    }
+
+}

src/main/java/frc/robot/oi/DriverControls.java

@@ -0,0 +1,5 @@
+package frc.robot.oi;
+
+public interface DriverControls {
+
+}

src/main/java/frc/robot/oi/DriverControlsDualFlightStick.java

@@ -0,0 +1,13 @@
+package frc.robot.oi;
+
+import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
+
+public class DriverControlsDualFlightStick implements DriverControls {
+    public CommandJoystick m_leftJoystick;
+    public CommandJoystick m_rightJoystick;
+
+    public DriverControlsDualFlightStick(int leftJoystick, int rightJoystick) {
+      m_leftJoystick = new CommandJoystick(leftJoystick);
+      m_rightJoystick = new CommandJoystick(rightJoystick);
+    }
+}

src/main/java/frc/robot/oi/OperatorControls.java

@@ -0,0 +1,5 @@
+package frc.robot.oi;
+
+public interface OperatorControls {
+
+}

src/main/java/frc/robot/oi/OperatorControlsXbox.java

@@ -0,0 +1,15 @@
+package frc.robot.oi;
+
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.util.EricNubControls;
+
+public class OperatorControlsXbox implements OperatorControls {
+    public CommandXboxController m_controller;
+    public EricNubControls m_controls;
+
+    public OperatorControlsXbox(int xboxControllerPort) {
+      m_controller = new CommandXboxController(xboxControllerPort);
+      m_controls = new EricNubControls();
+    }
+
+}