From a3d2b5f029604be8de2252be9baae1be46347141 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Mon, 15 Nov 2021 17:52:28 -0700 Subject: [PATCH] Basic swerve stuff Also separated arcade from swerve everywhere in the code. --- .templates/CommandTest.java | 2 +- .templates/UtilityTest.java | 6 ++-- src/main/java/frc4388/robot/Constants.java | 8 ++++- .../java/frc4388/robot/RobotContainer.java | 12 ++++---- src/main/java/frc4388/robot/RobotMap.java | 18 +++++------ .../{Drive.java => ArcadeDrive.java} | 8 ++--- .../frc4388/robot/subsystems/SwerveDrive.java | 30 ++++++++++++++----- .../frc4388/utility/RobotGyroUtilityTest.java | 10 +++---- 8 files changed, 58 insertions(+), 36 deletions(-) rename src/main/java/frc4388/robot/subsystems/{Drive.java => ArcadeDrive.java} (88%) diff --git a/.templates/CommandTest.java b/.templates/CommandTest.java index 72b31df..7e6f123 100644 --- a/.templates/CommandTest.java +++ b/.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 diff --git a/.templates/UtilityTest.java b/.templates/UtilityTest.java index 11c15cd..035e580 100644 --- a/.templates/UtilityTest.java +++ b/.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 diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index d92d66b..22137dd 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/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; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index b7542bc..a490cf2 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/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( - new RunCommand(() -> m_robotDrive.driveWithInput(getDriverController().getLeftYAxis(), - getDriverController().getRightXAxis()), m_robotDrive)); + m_robotArcadeDrive.setDefaultCommand( + new RunCommand(() -> m_robotArcadeDrive.driveWithInput(getDriverController().getLeftYAxis(), + 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" diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 3456b71..ac8d7d5 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/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 */ - public final WPI_TalonFX leftFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_FRONT_CAN_ID); - public final WPI_TalonFX rightFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_FRONT_CAN_ID); - public final WPI_TalonFX leftBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_BACK_CAN_ID); - public final WPI_TalonFX rightBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_BACK_CAN_ID); + /* ArcadeDrive Subsystem */ + public final WPI_TalonFX leftFrontMotor = new WPI_TalonFX(ArcadeDriveConstants.DRIVE_LEFT_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(ArcadeDriveConstants.DRIVE_LEFT_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(); diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/ArcadeDrive.java similarity index 88% rename from src/main/java/frc4388/robot/subsystems/Drive.java rename to src/main/java/frc4388/robot/subsystems/ArcadeDrive.java index 9bbdee7..f06a547 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/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(); } } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 1c42c5e..131cf44 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/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 - var speeds = new ChassisSpeeds(strafeX, strafeY, rotate * SwerveDriveConstants.RotationSpeed /*in rad/s */); + //https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-kinematics.html + 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) - { - var speeds = ChassisSpeeds.fromFieldRelativeSpeeds(awayFromStation, towardLeftBoundary, - rotate * SwerveDriveConstants.RotationSpeed, /*get odometry angle*/) - } + // public void driveFieldRelative(double awayFromStation, double towardLeftBoundary, double rotate) + // { + // var speeds = ChassisSpeeds.fromFieldRelativeSpeeds(awayFromStation, towardLeftBoundary, + // rotate * SwerveDriveConstants.RotationSpeed, /*get odometry angle*/) + // } } \ No newline at end of file diff --git a/src/test/java/frc4388/utility/RobotGyroUtilityTest.java b/src/test/java/frc4388/utility/RobotGyroUtilityTest.java index 8c0b1e5..c6f0433 100644 --- a/src/test/java/frc4388/utility/RobotGyroUtilityTest.java +++ b/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();