diff --git a/lib/RoboLib b/lib/RoboLib index 8defb59..8175159 160000 --- a/lib/RoboLib +++ b/lib/RoboLib @@ -1 +1 @@ -Subproject commit 8defb590f40120ea22befb0e0880bcbb2636b502 +Subproject commit 817515901ca237b9f47e2001f7d65f9e38249eb1 diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 720fbe9..33a5d7e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,18 +4,13 @@ package frc.robot; -import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveBuilder; -import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveDriveSubsystem; -import org.littletonrobotics.junction.Logger; - -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.OperatorConstants; import frc.robot.commands.Autos; import frc.robot.commands.ExampleCommand; +import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.ExampleSubsystem; /** @@ -34,113 +29,13 @@ public class RobotContainer { // private final CommandPS4Controller m_driverController = new CommandPS4Controller(OperatorConstants.kDriverControllerPort); - public static SwerveDriveSubsystem driveSubsystem; - - private double driveP = 0.1; - private double driveI = 0.0; - private double driveD = 0.01; - private double driveKV = 0.2; - - private double steerP = 50; - private double steerI = 1; - private double steerD = 1; - private double steerKV = 0; + public final DriveSubsystem driveSubsystem = new DriveSubsystem(m_driverController); /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { // Configure the trigger bindings configureBindings(); - SwerveBuilder swerveBuilder = new SwerveBuilder(); - - driveSubsystem = swerveBuilder - .withCanbusName("Drive") - .withPigeonId(20) - .withMaxLinearVelocity(3.0) - .withMaxAngularVelocity(Math.PI * 2) - .withSwerveModule( - swerveBuilder.new SwerveModuleBuilder() - .withDriveMotorId(12) - .withDriveKP(driveP) - .withDriveKI(driveI) - .withDriveKD(driveD) - .withDriveKV(driveKV) - .withSteerMotorId(3) - .withSteerKP(steerP) - .withSteerKI(steerI) - .withSteerKD(steerD) - .withSteerKV(steerKV) - .withSteerOffset(0.048828) - .withSteerEncoderId(10) - .withModuleLocation(new Translation2d(1, 1)) - .withModuleName("Front Left") - .withGearRatio(8.14) - .withWheelDiameter(0.1016) - .build() - ) - .withSwerveModule( - swerveBuilder.new SwerveModuleBuilder() - .withDriveMotorId(5) - .withDriveKP(driveP) - .withDriveKI(driveI) - .withDriveKD(driveD) - .withDriveKV(driveKV) - .withDriveInverted() - .withSteerMotorId(6) - .withSteerKP(steerP) - .withSteerKI(steerI) - .withSteerKD(steerD) - .withSteerKV(steerKV) - .withSteerOffset(0.244875) - .withSteerEncoderId(1) - .withModuleLocation(new Translation2d(1, -1)) - .withModuleName("Front Right") - .withGearRatio(8.14) - .withWheelDiameter(0.1016) - .build()) - .withSwerveModule( - swerveBuilder.new SwerveModuleBuilder() - .withDriveMotorId(8) - .withDriveKP(driveP) - .withDriveKI(driveI) - .withDriveKD(driveD) - .withDriveKV(driveKV) - .withSteerMotorId(2) - .withSteerKP(steerP) - .withSteerKI(steerI) - .withSteerKD(steerD) - .withSteerKV(steerKV) - .withSteerOffset(0.430176) - .withSteerEncoderId(7) - .withModuleLocation(new Translation2d(-1, 1)) - .withModuleName("Back Left") - .withGearRatio(8.14) - .withWheelDiameter(0.1016) - .build()) - .withSwerveModule( - swerveBuilder.new SwerveModuleBuilder() - .withDriveMotorId(9) - .withDriveKP(driveP) - .withDriveKI(driveI) - .withDriveKD(driveD) - .withDriveKV(driveKV) - .withDriveInverted() - .withSteerMotorId(11) - .withSteerKP(steerP) - .withSteerKI(steerI) - .withSteerKD(steerD) - .withSteerKV(steerKV) - .withSteerOffset(0.012695) - .withSteerEncoderId(4) - .withModuleLocation(new Translation2d(-1, -1)) - .withModuleName("Back Right") - .withGearRatio(8.14) - .withWheelDiameter(0.1016) - .build()) - .build(); - - driveSubsystem.setDefaultCommand(getDriveCommand()); - } /** @@ -162,26 +57,6 @@ public class RobotContainer { // m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand()); } - public Command getDriveCommand(){ - return Commands.run(new Runnable() { - - @Override - public void run() { - driveSubsystem.setChassisSpeedsFromJoystickRobotRelative( - -(m_driverController.getLeftY() * Math.abs(m_driverController.getLeftY())), - -(m_driverController.getLeftX() * Math.abs(m_driverController.getLeftX())), - -(m_driverController.getRawAxis(4) * Math.abs(m_driverController.getRawAxis(4)))); - - // driveSubsystem.setChassisSpeed(new ChassisSpeeds(1, 0, 0)); - - Logger.recordOutput("RequestedSwerveState", driveSubsystem.getRequestedModuleStates()); - Logger.recordOutput("ActualSwerveState", driveSubsystem.getActualModuleStates()); - Logger.recordOutput("Pose Estimation", driveSubsystem.getRobotPose()); - } - - }, driveSubsystem); - } - /** * Use this to pass the autonomous command to the main {@link Robot} class. * diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java new file mode 100644 index 0000000..e02055a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -0,0 +1,166 @@ +package frc.robot.subsystems; + +import org.fairportrobotics.frc.posty.TestableSubsystem; +import org.fairportrobotics.frc.posty.test.PostTest; +import static org.fairportrobotics.frc.posty.assertions.Assertions.*; +import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveBuilder; +import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveDriveSystem; +import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveModule; +import org.littletonrobotics.junction.Logger; + +import com.fasterxml.jackson.annotation.JsonCreator.Mode; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; + +public class DriveSubsystem extends TestableSubsystem { + + private SwerveBuilder swerveBuilder = new SwerveBuilder(); + private SwerveDriveSystem driveSystem; + + private double driveP = 0.1; + private double driveI = 0.0; + private double driveD = 0.01; + private double driveKV = 0.2; + + private double steerP = 50; + private double steerI = 1; + private double steerD = 1; + private double steerKV = 0; + + public DriveSubsystem(CommandXboxController xboxController) { + + driveSystem = swerveBuilder + .withCanbusName("Drive") + .withPigeonId(20) + .withMaxLinearVelocity(3.0) + .withMaxAngularVelocity(Math.PI * 2) + .withSwerveModule( + swerveBuilder.new SwerveModuleBuilder() + .withDriveMotorId(12) + .withDriveKP(driveP) + .withDriveKI(driveI) + .withDriveKD(driveD) + .withDriveKV(driveKV) + .withSteerMotorId(3) + .withSteerKP(steerP) + .withSteerKI(steerI) + .withSteerKD(steerD) + .withSteerKV(steerKV) + .withSteerOffset(0.048828) + .withSteerEncoderId(10) + .withModuleLocation(new Translation2d(1, 1)) + .withModuleName("Front Left") + .withGearRatio(8.14) + .withWheelDiameter(0.1016) + .build()) + .withSwerveModule( + swerveBuilder.new SwerveModuleBuilder() + .withDriveMotorId(5) + .withDriveKP(driveP) + .withDriveKI(driveI) + .withDriveKD(driveD) + .withDriveKV(driveKV) + .withDriveInverted() + .withSteerMotorId(6) + .withSteerKP(steerP) + .withSteerKI(steerI) + .withSteerKD(steerD) + .withSteerKV(steerKV) + .withSteerOffset(0.244875) + .withSteerEncoderId(1) + .withModuleLocation(new Translation2d(1, -1)) + .withModuleName("Front Right") + .withGearRatio(8.14) + .withWheelDiameter(0.1016) + .build()) + .withSwerveModule( + swerveBuilder.new SwerveModuleBuilder() + .withDriveMotorId(8) + .withDriveKP(driveP) + .withDriveKI(driveI) + .withDriveKD(driveD) + .withDriveKV(driveKV) + .withSteerMotorId(2) + .withSteerKP(steerP) + .withSteerKI(steerI) + .withSteerKD(steerD) + .withSteerKV(steerKV) + .withSteerOffset(0.430176) + .withSteerEncoderId(7) + .withModuleLocation(new Translation2d(-1, 1)) + .withModuleName("Back Left") + .withGearRatio(8.14) + .withWheelDiameter(0.1016) + .build()) + .withSwerveModule( + swerveBuilder.new SwerveModuleBuilder() + .withDriveMotorId(9) + .withDriveKP(driveP) + .withDriveKI(driveI) + .withDriveKD(driveD) + .withDriveKV(driveKV) + .withDriveInverted() + .withSteerMotorId(11) + .withSteerKP(steerP) + .withSteerKI(steerI) + .withSteerKD(steerD) + .withSteerKV(steerKV) + .withSteerOffset(0.012695) + .withSteerEncoderId(4) + .withModuleLocation(new Translation2d(-1, -1)) + .withModuleName("Back Right") + .withGearRatio(8.14) + .withWheelDiameter(0.1016) + .build()) + .build(); + + this.setDefaultCommand(Commands.run(new Runnable() { + + @Override + public void run() { + driveSystem.setChassisSpeedsFromJoystickRobotRelative( + -(xboxController.getLeftY() * Math.abs(xboxController.getLeftY())), + -(xboxController.getLeftX() * Math.abs(xboxController.getLeftX())), + -(xboxController.getRawAxis(4) * Math.abs(xboxController.getRawAxis(4)))); + + // driveSubsystem.setChassisSpeed(new ChassisSpeeds(1, 0, 0)); + + Logger.recordOutput("RequestedSwerveState", driveSystem.getRequestedModuleStates()); + Logger.recordOutput("ActualSwerveState", driveSystem.getActualModuleStates()); + Logger.recordOutput("Pose Estimation", driveSystem.getRobotPose()); + } + + }, this)); + + } + + @Override + public void periodic() { + super.periodic(); + driveSystem.periodic(); + } + + @Override + public void simulationPeriodic() { + super.simulationPeriodic(); + driveSystem.simulationPeriodic(); + } + + @PostTest() + public void DriveSubsystem_CanDevicesConnected(){ + + SwerveModule[] modules = driveSystem.getModules(); + + for(int i=0;i < modules.length; i++){ + assertThat(modules[i].getDriveMotor().isConnected()).as(modules[i].getModuleName() + " drive motor is not connected").isTrue(); + assertThat(modules[i].getSteerMotor().isConnected()).as(modules[i].getModuleName() + " steer motor is not connected").isTrue(); + assertThat(modules[i].getSteerEncoder().isConnected()).as(modules[i].getModuleName() + " CANCoder is not connected").isTrue(); + } + + assertThat(driveSystem.getGyro().isConnected()).as("Pigeon is not connected").isTrue(); + + } + +}