Basic swerve stuff

Also separated arcade from swerve everywhere in the code.

.templates/CommandTest.java

@@ -27,7 +27,7 @@ public class CommandTest {
     @Test
     public void testExample() {
         // Arrange
-        Drive drive = mock(Drive.class);
+        ArcadeDrive drive = mock(ArcadeDrive.class);
         RunCommand command = new RunCommand(() -> drive.driveWithInput(0, 0), drive);
 
         // Act

.templates/UtilityTest.java

@@ -15,7 +15,7 @@ import com.kauailabs.navx.frc.AHRS;
 import org.junit.*;
 
 import frc4388.mocks.MockPigeonIMU;
-import frc4388.robot.Constants.DriveConstants;
+import frc4388.robot.Constants.ArcadeDriveConstants;
 
 /**
  * Based on the RobotGyroUtilityTest class
@@ -27,7 +27,7 @@ public class UtilityTest {
     @Test
     public void testConstructor() {
         // Arrange
-        MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID);
+        MockPigeonIMU pigeon = new MockPigeonIMU(ArcadeDriveConstants.DRIVE_PIGEON_ID);
         AHRS navX = mock(AHRS.class);
         gyroPigeon = new RobotGyro(pigeon);
         gyroNavX = new RobotGyro(navX);
@@ -44,7 +44,7 @@ public class UtilityTest {
     @Test
     public void testHeadingPigeon() {
         // Arrange
-        MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID);
+        MockPigeonIMU pigeon = new MockPigeonIMU(ArcadeDriveConstants.DRIVE_PIGEON_ID);
         gyroPigeon = new RobotGyro(pigeon);
 
         // Act & Assert

src/main/java/frc4388/robot/Constants.java

@@ -18,7 +18,7 @@ import frc4388.utility.LEDPatterns;
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
-    public static final class DriveConstants {
+    public static final class ArcadeDriveConstants {
         public static final int DRIVE_LEFT_FRONT_CAN_ID = 2;
         public static final int DRIVE_RIGHT_FRONT_CAN_ID = 4;
 	    public static final int DRIVE_LEFT_BACK_CAN_ID = 3;
@@ -29,6 +29,12 @@ public final class Constants {
         public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
     }
 
+    public static final class SwerveDriveConstants {
+        public static final double ROTATION_SPEED = 0.1;
+        public static final double WIDTH = 0;
+        public static final double HEIGHT = 0;
+    }
+
     public static final class LEDConstants {
         public static final int LED_SPARK_ID = 0;
 

src/main/java/frc4388/robot/RobotContainer.java

@@ -13,7 +13,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc4388.robot.Constants.*;
-import frc4388.robot.subsystems.Drive;
+import frc4388.robot.subsystems.ArcadeDrive;
 import frc4388.robot.subsystems.LED;
 import frc4388.utility.LEDPatterns;
 import frc4388.utility.controller.IHandController;
@@ -31,7 +31,7 @@ public class RobotContainer {
     private final RobotMap m_robotMap = new RobotMap();
 
     /* Subsystems */
-    private final Drive m_robotDrive = new Drive(m_robotMap.leftFrontMotor, m_robotMap.rightFrontMotor,
+    private final ArcadeDrive m_robotArcadeDrive = new ArcadeDrive(m_robotMap.leftFrontMotor, m_robotMap.rightFrontMotor,
             m_robotMap.leftBackMotor, m_robotMap.rightBackMotor, m_robotMap.driveTrain, m_robotMap.gyroDrive);
 
     private final LED m_robotLED = new LED(m_robotMap.LEDController);
@@ -48,9 +48,9 @@ public class RobotContainer {
 
         /* Default Commands */
         // drives the robot with a two-axis input from the driver controller
-        m_robotDrive.setDefaultCommand(
+        m_robotArcadeDrive.setDefaultCommand(
-                new RunCommand(() -> m_robotDrive.driveWithInput(getDriverController().getLeftYAxis(),
+                new RunCommand(() -> m_robotArcadeDrive.driveWithInput(getDriverController().getLeftYAxis(),
-                        getDriverController().getRightXAxis()), m_robotDrive));
+                        getDriverController().getRightXAxis()), m_robotArcadeDrive));
         // continually sends updates to the Blinkin LED controller to keep the lights on
         m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
     }
@@ -65,7 +65,7 @@ public class RobotContainer {
         /* Driver Buttons */
         // test command to spin the robot while pressing A on the driver controller
         new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
-                .whileHeld(() -> m_robotDrive.driveWithInput(0, 1));
+                .whileHeld(() -> m_robotArcadeDrive.driveWithInput(0, 1));
 
         /* Operator Buttons */
         // activates "Lit Mode"

src/main/java/frc4388/robot/RobotMap.java

@@ -14,7 +14,7 @@ import com.ctre.phoenix.sensors.PigeonIMU;
 
 import edu.wpi.first.wpilibj.Spark;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-import frc4388.robot.Constants.DriveConstants;
+import frc4388.robot.Constants.ArcadeDriveConstants;
 import frc4388.robot.Constants.LEDConstants;
 import frc4388.utility.RobotGyro;
 
@@ -26,7 +26,7 @@ public class RobotMap {
 
     public RobotMap() {
         configureLEDMotorControllers();
-        configureDriveMotorControllers();
+        configureArcadeDriveMotorControllers();
     }
 
     /* LED Subsystem */
@@ -36,15 +36,15 @@ public class RobotMap {
         
     }
 
-    /* Drive Subsystem */
+    /* ArcadeDrive Subsystem */
-    public final WPI_TalonFX leftFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_FRONT_CAN_ID);
+    public final WPI_TalonFX leftFrontMotor = new WPI_TalonFX(ArcadeDriveConstants.DRIVE_LEFT_FRONT_CAN_ID);
-    public final WPI_TalonFX rightFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_FRONT_CAN_ID);
+    public final WPI_TalonFX rightFrontMotor = new WPI_TalonFX(ArcadeDriveConstants.DRIVE_RIGHT_FRONT_CAN_ID);
-    public final WPI_TalonFX leftBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_BACK_CAN_ID);
+    public final WPI_TalonFX leftBackMotor = new WPI_TalonFX(ArcadeDriveConstants.DRIVE_LEFT_BACK_CAN_ID);
-    public final WPI_TalonFX rightBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_BACK_CAN_ID);
+    public final WPI_TalonFX rightBackMotor = new WPI_TalonFX(ArcadeDriveConstants.DRIVE_RIGHT_BACK_CAN_ID);
     public final DifferentialDrive driveTrain = new DifferentialDrive(leftFrontMotor, rightFrontMotor);
-    public final RobotGyro gyroDrive = new RobotGyro(new PigeonIMU(DriveConstants.DRIVE_PIGEON_ID));
+    public final RobotGyro gyroDrive = new RobotGyro(new PigeonIMU(ArcadeDriveConstants.DRIVE_PIGEON_ID));
 
-    void configureDriveMotorControllers() {
+    void configureArcadeDriveMotorControllers() {
 
         /* factory default values */
         leftFrontMotor.configFactoryDefault();

src/main/java/frc4388/robot/subsystems/ArcadeDrive.java

@@ -13,14 +13,14 @@ import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
-import frc4388.robot.Constants.DriveConstants;
+import frc4388.robot.Constants.ArcadeDriveConstants;
 import frc4388.utility.RobotGyro;
 import frc4388.utility.RobotTime;
 
 /**
  * Add your docs here.
  */
-public class Drive extends SubsystemBase {
+public class ArcadeDrive extends SubsystemBase {
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
 
@@ -36,7 +36,7 @@ public class Drive extends SubsystemBase {
   /**
    * Add your docs here.
    */
-  public Drive(WPI_TalonFX leftFrontMotor, WPI_TalonFX rightFrontMotor, WPI_TalonFX leftBackMotor,
+  public ArcadeDrive(WPI_TalonFX leftFrontMotor, WPI_TalonFX rightFrontMotor, WPI_TalonFX leftBackMotor,
       WPI_TalonFX rightBackMotor, DifferentialDrive driveTrain, RobotGyro gyro) {
 
     m_leftFrontMotor = leftFrontMotor;
@@ -51,7 +51,7 @@ public class Drive extends SubsystemBase {
   public void periodic() {
     m_gyro.updatePigeonDeltas();
 
-    if (m_robotTime.m_frameNumber % DriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) {
+    if (m_robotTime.m_frameNumber % ArcadeDriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) {
       updateSmartDashboard();
     }
   }

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -1,3 +1,19 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.subsystems;
+
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
+import frc4388.robot.Constants.SwerveDriveConstants;
+
 public class SwerveDrive
 {
     SwerveDriveKinematics m_kinematics;
@@ -17,8 +33,8 @@ public class SwerveDrive
 
     public void drive(double strafeX, double strafeY, double rotate)
     {
-        //https://jacobmisirian.gitbooks.io/frc-swerve-drive-programming/content/chapter1.html
+        //https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-kinematics.html 
-        var speeds = new ChassisSpeeds(strafeX, strafeY, rotate * SwerveDriveConstants.RotationSpeed /*in rad/s */);
+        var speeds = new ChassisSpeeds(strafeX, strafeY, rotate * SwerveDriveConstants.ROTATION_SPEED /*in rad/s */);
         // Convert to module states
         SwerveModuleState[] moduleStates = m_kinematics.toSwerveModuleStates(speeds);
 
@@ -35,9 +51,9 @@ public class SwerveDrive
         SwerveModuleState backRight = SwerveModuleState.optimize(moduleStates[3], new Rotation2d(/*get encoder positions*/));
     }
 
-    public void driveFieldRelative(double awayFromStation, double towardLeftBoundary, double rotate)
+    // public void driveFieldRelative(double awayFromStation, double towardLeftBoundary, double rotate)
-    {
+    // {
-        var speeds = ChassisSpeeds.fromFieldRelativeSpeeds(awayFromStation, towardLeftBoundary, 
+    //     var speeds = ChassisSpeeds.fromFieldRelativeSpeeds(awayFromStation, towardLeftBoundary, 
-            rotate * SwerveDriveConstants.RotationSpeed, /*get odometry angle*/)
+    //         rotate * SwerveDriveConstants.RotationSpeed, /*get odometry angle*/)
-    }
+    // }
 }

src/test/java/frc4388/utility/RobotGyroUtilityTest.java

@@ -15,7 +15,7 @@ import com.kauailabs.navx.frc.AHRS;
 import org.junit.*;
 
 import frc4388.mocks.MockPigeonIMU;
-import frc4388.robot.Constants.DriveConstants;
+import frc4388.robot.Constants.ArcadeDriveConstants;
 
 /**
  * Add your docs here.
@@ -29,7 +29,7 @@ public class RobotGyroUtilityTest {
     @Test
     public void testConstructor() {
         // Arrange
-        MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID);
+        MockPigeonIMU pigeon = new MockPigeonIMU(ArcadeDriveConstants.DRIVE_PIGEON_ID);
         AHRS navX = mock(AHRS.class);
         gyroPigeon = new RobotGyro(pigeon);
         gyroNavX = new RobotGyro(navX);
@@ -46,7 +46,7 @@ public class RobotGyroUtilityTest {
     @Test
     public void testHeadingPigeon() {
         // Arrange
-        MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID);
+        MockPigeonIMU pigeon = new MockPigeonIMU(ArcadeDriveConstants.DRIVE_PIGEON_ID);
         gyroPigeon = new RobotGyro(pigeon);
 
         // Act & Assert
@@ -64,7 +64,7 @@ public class RobotGyroUtilityTest {
     @Test
     public void testYawPitchRollPigeon() {
         // Arrange
-        MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID);
+        MockPigeonIMU pigeon = new MockPigeonIMU(ArcadeDriveConstants.DRIVE_PIGEON_ID);
         gyroPigeon = new RobotGyro(pigeon);
 
         // Assert
@@ -136,7 +136,7 @@ public class RobotGyroUtilityTest {
     @Test
     public void testRatesPigeon() {
         // Arrange
-        MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID);
+        MockPigeonIMU pigeon = new MockPigeonIMU(ArcadeDriveConstants.DRIVE_PIGEON_ID);
         gyroPigeon = new RobotGyro(pigeon);
         RobotTime robotTime = RobotTime.getInstance();
         gyroPigeon.updatePigeonDeltas();