Reorganization
This commit is contained in:
Tyler Wilcox
parent
dddd9096a0
commit
568d3b0bf8
|
|
@ -1 +1 @@
|
||||||
Subproject commit 8defb590f40120ea22befb0e0880bcbb2636b502
|
Subproject commit 817515901ca237b9f47e2001f7d65f9e38249eb1
|
||||||
|
|
@ -4,18 +4,13 @@
|
||||||
|
|
||||||
package frc.robot;
|
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.Command;
|
||||||
import edu.wpi.first.wpilibj2.command.Commands;
|
|
||||||
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
||||||
import edu.wpi.first.wpilibj2.command.button.Trigger;
|
import edu.wpi.first.wpilibj2.command.button.Trigger;
|
||||||
import frc.robot.Constants.OperatorConstants;
|
import frc.robot.Constants.OperatorConstants;
|
||||||
import frc.robot.commands.Autos;
|
import frc.robot.commands.Autos;
|
||||||
import frc.robot.commands.ExampleCommand;
|
import frc.robot.commands.ExampleCommand;
|
||||||
|
import frc.robot.subsystems.DriveSubsystem;
|
||||||
import frc.robot.subsystems.ExampleSubsystem;
|
import frc.robot.subsystems.ExampleSubsystem;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|
@ -34,113 +29,13 @@ public class RobotContainer {
|
||||||
|
|
||||||
// private final CommandPS4Controller m_driverController = new CommandPS4Controller(OperatorConstants.kDriverControllerPort);
|
// private final CommandPS4Controller m_driverController = new CommandPS4Controller(OperatorConstants.kDriverControllerPort);
|
||||||
|
|
||||||
public static SwerveDriveSubsystem driveSubsystem;
|
public final DriveSubsystem driveSubsystem = new DriveSubsystem(m_driverController);
|
||||||
|
|
||||||
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;
|
|
||||||
|
|
||||||
/** The container for the robot. Contains subsystems, OI devices, and commands. */
|
/** The container for the robot. Contains subsystems, OI devices, and commands. */
|
||||||
public RobotContainer() {
|
public RobotContainer() {
|
||||||
// Configure the trigger bindings
|
// Configure the trigger bindings
|
||||||
configureBindings();
|
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());
|
// 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.
|
* Use this to pass the autonomous command to the main {@link Robot} class.
|
||||||
*
|
*
|
||||||
|
|
|
||||||
|
|
@ -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();
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
Loading…
Reference in New Issue