Team4388/Swerve-Drive
1
0
Fork 0
mirror of https://github.com/Team4388/Swerve-Drive.git synced 2026-06-08 16:28:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

Basic swerve stuff

Browse Source 
Also separated arcade from swerve everywhere in the code.
This commit is contained in:
aarav18
2021-11-15 17:52:28 -07:00
parent 0bc2442ec1
commit a3d2b5f029
8 changed files with 58 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.templates/CommandTest.java
+1 -1
View File
@@ -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
+3 -3
View File
@@ -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
+7 -1
View File
@@ -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
+6 -6
View File
@@ -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"
src/main/java/frc4388/robot/RobotMap.java
+9 -9
View File
@@ -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();
src/main/java/frc4388/robot/subsystems/Drive.java → src/main/java/frc4388/robot/subsystems/ArcadeDrive.java
+4 -4
View File
@@ -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
+23 -7
View File
@@ -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*/)
// }
}
src/test/java/frc4388/utility/RobotGyroUtilityTest.java
+5 -5
View File
@@ -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();