Hopeful fixes from the first round of tests
This commit is contained in:
Tyler Wilcox
parent
604e9e7cc5
commit
bc53ef3b38
|
|
@ -9,6 +9,9 @@ public class SwerveBuilder {
|
|||
ArrayList<SwerveModule> modules = new ArrayList<SwerveModule>();
|
||||
String canBusName = "rio";
|
||||
|
||||
private double maxAngularVelRadiansSecond = 1.0;
|
||||
private double maxLinearVelMetersSecond = Math.PI;
|
||||
|
||||
private int pigeonId;
|
||||
|
||||
public SwerveBuilder(){
|
||||
|
|
@ -30,8 +33,28 @@ public class SwerveBuilder {
|
|||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the max linear velocity in meters per second
|
||||
* @param maxVel Max linear velocity in meters per second
|
||||
* @return The swerve builder
|
||||
*/
|
||||
public SwerveBuilder withMaxLinearVelocity(double maxVel){
|
||||
this.maxLinearVelMetersSecond = maxVel;
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the max angular velocity in radians per second
|
||||
* @param maxVel Max angular velocity in radians per second
|
||||
* @return The swerve builder
|
||||
*/
|
||||
public SwerveBuilder withMaxAngularVelocity(double maxVel){
|
||||
this.maxAngularVelRadiansSecond = maxVel;
|
||||
return this;
|
||||
}
|
||||
|
||||
public SwerveDriveSubsystem build(){
|
||||
return new SwerveDriveSubsystem(pigeonId, modules.toArray(size -> new SwerveModule[size]));
|
||||
return new SwerveDriveSubsystem(pigeonId, maxLinearVelMetersSecond, maxAngularVelRadiansSecond, modules.toArray(size -> new SwerveModule[size]));
|
||||
}
|
||||
|
||||
public class SwerveModuleBuilder {
|
||||
|
|
|
|||
|
|
@ -2,6 +2,7 @@ package org.fairportrobotics.frc.robolib.DriveSystems.SwerveDrive;
|
|||
|
||||
import java.util.Arrays;
|
||||
|
||||
import com.ctre.phoenix6.Utils;
|
||||
import com.ctre.phoenix6.hardware.Pigeon2;
|
||||
|
||||
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator3d;
|
||||
|
|
@ -26,12 +27,17 @@ public class SwerveDriveSubsystem extends SubsystemBase{
|
|||
|
||||
private SwerveModule[] modules;
|
||||
|
||||
private final double MAX_LINEAR_SPEED = 3.9;
|
||||
private final double MAX_ANGULAR_SPEED = Math.PI * 2;
|
||||
private double MAX_LINEAR_SPEED = 3.9;
|
||||
private double MAX_ANGULAR_SPEED = Math.PI * 2;
|
||||
|
||||
private Pigeon2 gyro;
|
||||
|
||||
public SwerveDriveSubsystem(int pigeonId, SwerveModule... modules){
|
||||
private double mLastSimTime = 0;
|
||||
|
||||
public SwerveDriveSubsystem(int pigeonId, double maxLinearVelMetersSecond, double maxAngularVelRadiansSecond, SwerveModule... modules){
|
||||
|
||||
MAX_LINEAR_SPEED = maxLinearVelMetersSecond;
|
||||
MAX_ANGULAR_SPEED = maxAngularVelRadiansSecond;
|
||||
|
||||
this.modules = modules;
|
||||
|
||||
|
|
@ -47,19 +53,34 @@ public class SwerveDriveSubsystem extends SubsystemBase{
|
|||
|
||||
@Override
|
||||
public void periodic() {
|
||||
super.periodic();
|
||||
SwerveModuleState[] moduleStates = driveKiniematics.toSwerveModuleStates(chassisSpeeds, centerOfRotation);
|
||||
|
||||
for(int i=0; i<modules.length;i++){
|
||||
moduleStates[i].optimize(modules[i].getCurrentSteerRotations());
|
||||
moduleStates[i].optimize(modules[i].getSteerRotations());
|
||||
}
|
||||
|
||||
poseEstimator.update(new Rotation3d(Rotation2d.fromRotations(getCurrentYaw().magnitude())), getModulePositions());
|
||||
poseEstimator.updateWithTime(Utils.getCurrentTimeSeconds(), new Rotation3d(Rotation2d.fromRotations(getCurrentYaw().magnitude())), getModulePositions());
|
||||
|
||||
for(int i=0;i<modules.length;i++){
|
||||
modules[i].setModuleState(moduleStates[i]);
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void simulationPeriodic() {
|
||||
super.simulationPeriodic();
|
||||
|
||||
double deltaTimeSecond = (mLastSimTime - Utils.getCurrentTimeSeconds());
|
||||
|
||||
if(mLastSimTime != 0){
|
||||
Pose3d oldPose = getRobotPose();
|
||||
poseEstimator.resetPose(new Pose3d(oldPose.getX() + (chassisSpeeds.vxMetersPerSecond * (deltaTimeSecond)), oldPose.getY() + chassisSpeeds.vyMetersPerSecond * (deltaTimeSecond), 0, oldPose.getRotation().plus(new Rotation3d(Rotation2d.fromRadians(chassisSpeeds.omegaRadiansPerSecond * (deltaTimeSecond))))));
|
||||
}
|
||||
|
||||
mLastSimTime = Utils.getCurrentTimeSeconds();
|
||||
}
|
||||
|
||||
public Translation2d[] getModuleLocations(){
|
||||
return Arrays.stream(modules).map((SwerveModule m) -> m.getModuleLocation()).toArray(Translation2d[]::new);
|
||||
}
|
||||
|
|
@ -68,6 +89,14 @@ public class SwerveDriveSubsystem extends SubsystemBase{
|
|||
return Arrays.stream(modules).map((SwerveModule m) -> m.getModuleState()).toArray(SwerveModuleState[]::new);
|
||||
}
|
||||
|
||||
public SwerveModule[] getModules(){
|
||||
return Arrays.stream(modules).toArray(SwerveModule[]::new);
|
||||
}
|
||||
|
||||
public int getNumberOfModules(){
|
||||
return modules.length;
|
||||
}
|
||||
|
||||
public Angle getCurrentYaw(){
|
||||
return gyro.getYaw().refresh().getValue();
|
||||
}
|
||||
|
|
@ -81,7 +110,13 @@ public class SwerveDriveSubsystem extends SubsystemBase{
|
|||
this.centerOfRotation = centerOfRotation;
|
||||
}
|
||||
|
||||
public void setChassisSpeedsFromJoystick(double x, double y, double rot){
|
||||
/**
|
||||
* Set robot speed relative to field
|
||||
* @param x Forward robot speed in meters per second
|
||||
* @param y Left robot speed in meters per second
|
||||
* @param rot CCW robot speed in radians per second
|
||||
*/
|
||||
public void setChassisSpeedsFromJoystickFieldRelative(double x, double y, double rot){
|
||||
this.setChassisSpeed(ChassisSpeeds.fromFieldRelativeSpeeds(
|
||||
MAX_LINEAR_SPEED * x,
|
||||
MAX_LINEAR_SPEED * y,
|
||||
|
|
@ -90,6 +125,25 @@ public class SwerveDriveSubsystem extends SubsystemBase{
|
|||
));
|
||||
}
|
||||
|
||||
/**
|
||||
* Set robot speed relative to robot
|
||||
* @param x Forward robot speed in meters per second
|
||||
* @param y Left robot speed in meters per second
|
||||
* @param rot CCW robot speed in radians per second
|
||||
*/
|
||||
public void setChassisSpeedsFromJoystickRobotRelative(double x, double y, double rot){
|
||||
this.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(
|
||||
MAX_LINEAR_SPEED * x,
|
||||
MAX_LINEAR_SPEED * y,
|
||||
MAX_ANGULAR_SPEED * rot,
|
||||
Rotation2d.fromDegrees(getCurrentYaw().magnitude())
|
||||
));
|
||||
}
|
||||
|
||||
public Pose3d getRobotPose(){
|
||||
return poseEstimator.getEstimatedPosition();
|
||||
}
|
||||
|
||||
private SwerveModulePosition[] getModulePositions(){
|
||||
return Arrays.stream(modules).map((SwerveModule m) -> m.getModulePosition()).toArray(SwerveModulePosition[]::new);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,6 +1,8 @@
|
|||
package org.fairportrobotics.frc.robolib.DriveSystems.SwerveDrive;
|
||||
|
||||
import com.ctre.phoenix6.configs.CANcoderConfiguration;
|
||||
import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs;
|
||||
import com.ctre.phoenix6.configs.FeedbackConfigs;
|
||||
import com.ctre.phoenix6.configs.MagnetSensorConfigs;
|
||||
import com.ctre.phoenix6.configs.Slot0Configs;
|
||||
import com.ctre.phoenix6.configs.TalonFXConfiguration;
|
||||
|
|
@ -8,6 +10,7 @@ import com.ctre.phoenix6.controls.PositionVoltage;
|
|||
import com.ctre.phoenix6.controls.VelocityVoltage;
|
||||
import com.ctre.phoenix6.hardware.CANcoder;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
|
||||
import com.sun.jdi.request.StepRequest;
|
||||
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
|
|
@ -57,11 +60,11 @@ public class SwerveModule {
|
|||
driveMotor = new TalonFX(driveMotorId, canBusName);
|
||||
driveMotor.getVelocity().setUpdateFrequency(CAN_UPDATE_FREQUENCY);
|
||||
driveMotor.optimizeBusUtilization();
|
||||
driveMotor.getConfigurator().apply(generateTalonConfiguration(driveKP, driveKI, driveKD));
|
||||
driveMotor.getConfigurator().apply(generateDriveTalonConfiguration(driveKP, driveKI, driveKD));
|
||||
|
||||
steerMotor = new TalonFX(steerMotorId, canBusName);
|
||||
steerMotor.optimizeBusUtilization();
|
||||
steerMotor.getConfigurator().apply(generateTalonConfiguration(steerKP, steerKI, steerKD));
|
||||
steerMotor.getConfigurator().apply(generateSteerTalonConfiguration(steerKP, steerKI, steerKD));
|
||||
|
||||
steerEncoder = new CANcoder(steerEncoderId, canBusName);
|
||||
steerEncoder.getPosition().setUpdateFrequency(CAN_UPDATE_FREQUENCY);
|
||||
|
|
@ -80,19 +83,33 @@ public class SwerveModule {
|
|||
steerMotor.setControl(steerRequest.withSlot(0).withPosition(rotations));
|
||||
}
|
||||
|
||||
public Rotation2d getCurrentSteerRotations(){
|
||||
public Rotation2d getSteerRotations(){
|
||||
return new Rotation2d(steerEncoder.getPosition().refresh().getValueAsDouble());
|
||||
}
|
||||
|
||||
public void setDriveSpeed(double speed){
|
||||
// Convert wheel speed to rotor speed
|
||||
// Circumfrence of wheel / speed * gear ratio
|
||||
double rotorVelocity = ((Math.PI * wheelDiameterInMeters) / speed) * gearRatio;
|
||||
//driveMotor.setControl(driveRequest.withSlot(0).withVelocity(rotorVelocity));
|
||||
driveMotor.setControl(driveRequest.withSlot(0).withVelocity(rotorVelocity));
|
||||
}
|
||||
|
||||
public double getDriveSpeed(){
|
||||
/**
|
||||
* Get drive motor speed in rotations per second
|
||||
* @return
|
||||
*/
|
||||
public double getDriveMotorSpeed(){
|
||||
return driveMotor.getVelocity().refresh().getValueAsDouble();
|
||||
}
|
||||
|
||||
/**
|
||||
* Get drive wheel speed in meters per second
|
||||
* @return
|
||||
*/
|
||||
public double getDriveWheelSpeed(){
|
||||
return (driveMotor.getVelocity().refresh().getValueAsDouble() / gearRatio) * (Math.PI * wheelDiameterInMeters);
|
||||
}
|
||||
|
||||
public Translation2d getModuleLocation(){
|
||||
return moduleLocation;
|
||||
}
|
||||
|
|
@ -108,16 +125,38 @@ public class SwerveModule {
|
|||
}
|
||||
|
||||
public SwerveModulePosition getModulePosition(){
|
||||
return new SwerveModulePosition(this.getModuleDistance(), getCurrentSteerRotations());
|
||||
return new SwerveModulePosition(this.getModuleDistance(), getSteerRotations());
|
||||
}
|
||||
|
||||
public String getModuleName(){
|
||||
return moduleName;
|
||||
}
|
||||
|
||||
private double getModuleDistance(){
|
||||
double distance = (this.driveMotor.getPosition(true).getValueAsDouble()) / gearRatio / (Math.PI * wheelDiameterInMeters);
|
||||
double distance = (this.driveMotor.getPosition(true).getValueAsDouble()) / gearRatio * (Math.PI * wheelDiameterInMeters);
|
||||
return distance;
|
||||
}
|
||||
|
||||
private TalonFXConfiguration generateTalonConfiguration(double kP, double kI, double kD){
|
||||
return new TalonFXConfiguration().withSlot0(new Slot0Configs().withKP(kP).withKI(kI).withKD(kD));
|
||||
private TalonFXConfiguration generateDriveTalonConfiguration(double kP, double kI, double kD){
|
||||
return new TalonFXConfiguration().withSlot0(
|
||||
new Slot0Configs()
|
||||
.withKP(kP)
|
||||
.withKI(kI)
|
||||
.withKD(kD)
|
||||
);
|
||||
}
|
||||
|
||||
private TalonFXConfiguration generateSteerTalonConfiguration(double kP, double kI, double kD){
|
||||
return new TalonFXConfiguration().withSlot0(
|
||||
new Slot0Configs()
|
||||
.withKP(kP)
|
||||
.withKI(kI)
|
||||
.withKD(kD)
|
||||
).withFeedback(
|
||||
new FeedbackConfigs()
|
||||
.withFeedbackRemoteSensorID(steerEncoder.getDeviceID())
|
||||
.withFeedbackSensorSource(FeedbackSensorSourceValue.RemoteCANcoder)
|
||||
);
|
||||
}
|
||||
|
||||
private CANcoderConfiguration generateCanCoderConfiguration(double steerOffset){
|
||||
|
|
|
|||
Loading…
Reference in New Issue