FairportRobotics
/
RoboLib
mirror of https://github.com/FairportRobotics/RoboLib.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Working logically

This commit is contained in:
Tyler Wilcox 2026-01-27 20:46:13 -05:00
parent bc53ef3b38
commit ae742134fe
3 changed files with 54 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveBuilder.java
View File

@ -54,7 +54,7 @@ public class SwerveBuilder {
}
public SwerveDriveSubsystem build(){
return new SwerveDriveSubsystem(pigeonId, maxLinearVelMetersSecond, maxAngularVelRadiansSecond, modules.toArray(size -> new SwerveModule[size]));
return new SwerveDriveSubsystem(pigeonId, canBusName, maxLinearVelMetersSecond, maxAngularVelRadiansSecond, modules.toArray(size -> new SwerveModule[size]));
}
public class SwerveModuleBuilder {
@ -63,10 +63,12 @@ public class SwerveBuilder {
private double driveKP;
private double driveKI;
private double driveKD;
private double driveKV;
private int steerMotorId;
private double steerKP;
private double steerKI;
private double steerKD;
private double steerKV;
private int steerEncoderId;
private double steerOffset;
private Translation2d moduleLocation;
@ -98,6 +100,11 @@ public class SwerveBuilder {
return this;
}
public SwerveModuleBuilder withDriveKV(double driveKV){
this.driveKV = driveKV;
return this;
}
public SwerveModuleBuilder withSteerMotorId(int steerMotorId){
this.steerMotorId = steerMotorId;
return this;
@ -118,6 +125,11 @@ public class SwerveBuilder {
return this;
}
public SwerveModuleBuilder withSteerKV(double steerKV){
this.steerKV = steerKV;
return this;
}
public SwerveModuleBuilder withSteerEncoderId(int steerEncoderId){
this.steerEncoderId = steerEncoderId;
return this;
@ -144,7 +156,7 @@ public class SwerveBuilder {
}
public SwerveModule build() {
return new SwerveModule(driveMotorId, driveKP, driveKI, driveKD, steerMotorId, steerKP, steerKI, steerKD,
return new SwerveModule(driveMotorId, driveKP, driveKI, driveKD, driveKV, steerMotorId, steerKP, steerKI, steerKD, steerKV,
steerEncoderId, steerOffset, canBusName, moduleLocation, gearRatio, wheelDiameterInMeters, moduleName);
}

10
lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveDriveSubsystem.java
View File

@ -34,7 +34,7 @@ public class SwerveDriveSubsystem extends SubsystemBase{
private double mLastSimTime = 0;
public SwerveDriveSubsystem(int pigeonId, double maxLinearVelMetersSecond, double maxAngularVelRadiansSecond, SwerveModule... modules){
public SwerveDriveSubsystem(int pigeonId, String canBusName, double maxLinearVelMetersSecond, double maxAngularVelRadiansSecond, SwerveModule... modules){
MAX_LINEAR_SPEED = maxLinearVelMetersSecond;
MAX_ANGULAR_SPEED = maxAngularVelRadiansSecond;
@ -45,7 +45,7 @@ public class SwerveDriveSubsystem extends SubsystemBase{
Arrays.stream(modules).map((m) -> m.getModuleLocation()).toArray(size -> new Translation2d[size])
);
gyro = new Pigeon2(pigeonId);
gyro = new Pigeon2(pigeonId, canBusName);
poseEstimator = new SwerveDrivePoseEstimator3d(driveKiniematics, new Rotation3d(Rotation2d.fromRotations(getCurrentYaw().magnitude())), getModulePositions(), Pose3d.kZero);
chassisSpeeds = new ChassisSpeeds();
@ -56,9 +56,9 @@ public class SwerveDriveSubsystem extends SubsystemBase{
super.periodic();
SwerveModuleState[] moduleStates = driveKiniematics.toSwerveModuleStates(chassisSpeeds, centerOfRotation);
for(int i=0; i<modules.length;i++){
moduleStates[i].optimize(modules[i].getSteerRotations());
}
// for(int i=0; i<modules.length;i++){
// moduleStates[i].optimize(modules[i].getSteerRotations());
// }
poseEstimator.updateWithTime(Utils.getCurrentTimeSeconds(), new Rotation3d(Rotation2d.fromRotations(getCurrentYaw().magnitude())), getModulePositions());

53
lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveModule.java
View File

@ -11,12 +11,14 @@ 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 com.ctre.phoenix6.signals.SensorDirectionValue;
import com.ctre.phoenix6.signals.StaticFeedforwardSignValue;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.units.measure.Angle;
public class SwerveModule {
@ -45,10 +47,12 @@ public class SwerveModule {
double driveKP,
double driveKI,
double driveKD,
double driveKV,
int steerMotorId,
double steerKP,
double steerKI,
double steerKD,
double steerKV,
int steerEncoderId,
double steerOffset,
String canBusName,
@ -57,20 +61,21 @@ public class SwerveModule {
double wheelDiameterInMeters,
String moduleName
){
driveMotor = new TalonFX(driveMotorId, canBusName);
driveMotor.getVelocity().setUpdateFrequency(CAN_UPDATE_FREQUENCY);
driveMotor.optimizeBusUtilization();
driveMotor.getConfigurator().apply(generateDriveTalonConfiguration(driveKP, driveKI, driveKD));
steerMotor = new TalonFX(steerMotorId, canBusName);
steerMotor.optimizeBusUtilization();
steerMotor.getConfigurator().apply(generateSteerTalonConfiguration(steerKP, steerKI, steerKD));
steerEncoder = new CANcoder(steerEncoderId, canBusName);
steerEncoder.getPosition().setUpdateFrequency(CAN_UPDATE_FREQUENCY);
steerEncoder.optimizeBusUtilization();
steerEncoder.getConfigurator().apply(generateCanCoderConfiguration(steerOffset));
driveMotor = new TalonFX(driveMotorId, canBusName);
driveMotor.getVelocity().setUpdateFrequency(CAN_UPDATE_FREQUENCY);
driveMotor.optimizeBusUtilization();
driveMotor.getConfigurator().apply(generateDriveTalonConfiguration(driveKP, driveKI, driveKD, driveKV));
steerMotor = new TalonFX(steerMotorId, canBusName);
steerMotor.optimizeBusUtilization();
steerMotor.getConfigurator().apply(generateSteerTalonConfiguration(steerKP, steerKI, steerKD, steerKV));
this.moduleLocation = moduleLocation;
this.gearRatio = gearRatio;
@ -79,7 +84,7 @@ public class SwerveModule {
this.moduleName = moduleName;
}
public void setSteerRotations(double rotations){
public void setSteerRotations(Angle rotations){
steerMotor.setControl(steerRequest.withSlot(0).withPosition(rotations));
}
@ -90,7 +95,7 @@ public class SwerveModule {
public void setDriveSpeed(double speed){
// Convert wheel speed to rotor speed
// Circumfrence of wheel / speed * gear ratio
double rotorVelocity = ((Math.PI * wheelDiameterInMeters) / speed) * gearRatio;
double rotorVelocity = (speed / (Math.PI * wheelDiameterInMeters)) * gearRatio;
driveMotor.setControl(driveRequest.withSlot(0).withVelocity(rotorVelocity));
}
@ -107,7 +112,7 @@ public class SwerveModule {
* @return
*/
public double getDriveWheelSpeed(){
return (driveMotor.getVelocity().refresh().getValueAsDouble() / gearRatio) * (Math.PI * wheelDiameterInMeters);
return (driveMotor.getVelocity().refresh().getValueAsDouble() * gearRatio) / (Math.PI * wheelDiameterInMeters);
}
public Translation2d getModuleLocation(){
@ -115,7 +120,7 @@ public class SwerveModule {
}
public void setModuleState(SwerveModuleState state){
this.setSteerRotations(state.angle.getRotations());
this.setSteerRotations(state.angle.getMeasure());
this.setDriveSpeed(state.speedMetersPerSecond);
moduleState = state;
}
@ -133,34 +138,46 @@ public class SwerveModule {
}
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 generateDriveTalonConfiguration(double kP, double kI, double kD){
private TalonFXConfiguration generateDriveTalonConfiguration(double kP, double kI, double kD, double kV){
return new TalonFXConfiguration().withSlot0(
new Slot0Configs()
.withKP(kP)
.withKI(kI)
.withKD(kD)
.withKV(kV)
);
}
private TalonFXConfiguration generateSteerTalonConfiguration(double kP, double kI, double kD){
private TalonFXConfiguration generateSteerTalonConfiguration(double kP, double kI, double kD, double kV){
return new TalonFXConfiguration().withSlot0(
new Slot0Configs()
.withKP(kP)
.withKI(kI)
.withKD(kD)
.withKS(kV)
.withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign)
).withFeedback(
new FeedbackConfigs()
.withFeedbackRemoteSensorID(steerEncoder.getDeviceID())
.withFeedbackSensorSource(FeedbackSensorSourceValue.RemoteCANcoder)
.withFeedbackSensorSource(FeedbackSensorSourceValue.FusedCANcoder)
.withSensorToMechanismRatio(1.0)
.withRotorToSensorRatio(12.8)
).withClosedLoopGeneral(
new ClosedLoopGeneralConfigs()
.withContinuousWrap(true)
);
}
private CANcoderConfiguration generateCanCoderConfiguration(double steerOffset){
return new CANcoderConfiguration().withMagnetSensor(new MagnetSensorConfigs().withMagnetOffset(steerOffset));
return new CANcoderConfiguration().withMagnetSensor(
new MagnetSensorConfigs()
.withMagnetOffset(steerOffset)
.withSensorDirection(SensorDirectionValue.CounterClockwise_Positive)
);
}
}