diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveBuilder.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveBuilder.java index 0eed21a..ca3eb28 100644 --- a/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveBuilder.java +++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveBuilder.java @@ -67,6 +67,7 @@ public class SwerveBuilder { private double driveKI; private double driveKD; private double driveKV; + private boolean driveInverted = false; private int steerMotorId; private double steerKP; private double steerKI; @@ -110,6 +111,11 @@ public class SwerveBuilder { return this; } + public SwerveModuleBuilder withDriveInverted(){ + this.driveInverted = true; + return this; + } + public SwerveModuleBuilder withSteerMotorId(int steerMotorId) { this.steerMotorId = steerMotorId; return this; @@ -150,6 +156,11 @@ public class SwerveBuilder { return this; } + public SwerveModuleBuilder withSteerOffset(double offsetRotations) { + this.steerOffset = offsetRotations; + return this; + } + public SwerveModuleBuilder withModuleLocation(Translation2d moduleLocation) { this.moduleLocation = moduleLocation; return this; @@ -171,7 +182,7 @@ public class SwerveBuilder { } public SwerveModule build() { - return new SwerveModule(driveMotorId, driveKP, driveKI, driveKD, driveKV, steerMotorId, steerKP, steerKI, + return new SwerveModule(driveMotorId, driveKP, driveKI, driveKD, driveKV, driveInverted, steerMotorId, steerKP, steerKI, steerKD, steerKS, steerKV, steerKA, steerEncoderId, steerOffset, canBusName, moduleLocation, gearRatio, wheelDiameterInMeters, moduleName); diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveDriveSubsystem.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveDriveSubsystem.java index 5f65f70..7034431 100644 --- a/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveDriveSubsystem.java +++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveDriveSubsystem.java @@ -54,16 +54,17 @@ public class SwerveDriveSubsystem extends SubsystemBase{ @Override public void periodic() { super.periodic(); - SwerveModuleState[] moduleStates = driveKiniematics.toSwerveModuleStates(chassisSpeeds, centerOfRotation); + SwerveModuleState[] moduleStates = driveKiniematics.toSwerveModuleStates(chassisSpeeds); - // for(int i=0; i m.getModuleLocation()).toArray(Translation2d[]::new); } - public SwerveModuleState[] getModuleStates(){ - return Arrays.stream(modules).map((SwerveModule m) -> m.getModuleState()).toArray(SwerveModuleState[]::new); + public SwerveModuleState[] getRequestedModuleStates(){ + return Arrays.stream(modules).map((SwerveModule m) -> m.getRequestedModuleState()).toArray(SwerveModuleState[]::new); + } + + public SwerveModuleState[] getActualModuleStates(){ + return Arrays.stream(modules).map((SwerveModule m) -> m.getActualModuleState()).toArray(SwerveModuleState[]::new); } public SwerveModule[] getModules(){ @@ -98,11 +103,11 @@ public class SwerveDriveSubsystem extends SubsystemBase{ } public Angle getCurrentYaw(){ - return gyro.getYaw().refresh().getValue(); + return gyro.getRoll().getValue(); } public void setChassisSpeed(ChassisSpeeds chassisSpeeds){ - this.setChassisSpeed(chassisSpeeds, Translation2d.kZero); + this.setChassisSpeed(chassisSpeeds, new Translation2d()); } public void setChassisSpeed(ChassisSpeeds chassisSpeeds, Translation2d centerOfRotation){ diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveModule.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveModule.java index f812215..ad3f057 100644 --- a/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveModule.java +++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveModule.java @@ -1,10 +1,13 @@ package org.fairportrobotics.frc.robolib.DriveSystems.SwerveDrive; +import java.lang.reflect.Modifier; + 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.MotionMagicConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.MotionMagicVoltage; @@ -13,6 +16,8 @@ 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.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; @@ -20,23 +25,22 @@ 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.wpilibj.smartdashboard.SmartDashboard; public class SwerveModule { private final int CAN_UPDATE_FREQUENCY = 50; - private final int TALON_BUILT_IN_ENCODER_UNITS_PER_ROTATION = 2048; - private final int CANCODER_UNITS_PER_ROTATION = 2048; - private final double gearRatio; private final double wheelDiameterInMeters; - private SwerveModuleState moduleState; + private SwerveModuleState requestedModuleState; private TalonFX driveMotor; private VelocityVoltage driveRequest = new VelocityVoltage(0); private TalonFX steerMotor; private MotionMagicVoltage steerRequest = new MotionMagicVoltage(0); + private PositionVoltage steerPosRequ = new PositionVoltage(0); private CANcoder steerEncoder; private String moduleName; @@ -49,6 +53,7 @@ public class SwerveModule { double driveKI, double driveKD, double driveKV, + boolean driveInverted, int steerMotorId, double steerKP, double steerKI, @@ -66,18 +71,19 @@ public class SwerveModule { ){ steerEncoder = new CANcoder(steerEncoderId, canBusName); - steerEncoder.getPosition().setUpdateFrequency(CAN_UPDATE_FREQUENCY); - steerEncoder.optimizeBusUtilization(); + // steerEncoder.getAbsolutePosition().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)); + // driveMotor.getVelocity().setUpdateFrequency(CAN_UPDATE_FREQUENCY); + // driveMotor.optimizeBusUtilization(); + driveMotor.getConfigurator().apply(generateDriveTalonConfiguration(driveKP, driveKI, driveKD, driveKV, driveInverted)); steerMotor = new TalonFX(steerMotorId, canBusName); - steerMotor.optimizeBusUtilization(); + // steerMotor.optimizeBusUtilization(); steerMotor.getConfigurator().apply(generateSteerTalonConfiguration(steerKP, steerKI, steerKD, steerKS, steerKV, steerKA)); + // steerMotor.setNeutralMode(NeutralModeValue.Brake); this.moduleLocation = moduleLocation; @@ -88,7 +94,9 @@ public class SwerveModule { } public void setSteerRotations(Rotation2d rotations){ - steerMotor.setControl(steerRequest.withSlot(0).withPosition(rotations.getMeasure())); + // steerMotor.setVoltage(steerPid.calculate(getSteerRotations().getRadians(), rotations.getRadians())); + // steerMotor.setControl(steerRequest.withSlot(0).withPosition(rotations.getRotations())); + steerMotor.setControl(steerRequest.withPosition(rotations.getRotations())); } public Rotation2d getSteerRotations(){ @@ -124,20 +132,33 @@ public class SwerveModule { return moduleLocation; } - public void setModuleState(SwerveModuleState state){ + public void setRequestedModuleState(SwerveModuleState state){ this.setSteerRotations(state.angle); this.setDriveSpeed(state.speedMetersPerSecond); - moduleState = state; + requestedModuleState = state; } - public SwerveModuleState getModuleState(){ - return moduleState; + public SwerveModuleState getRequestedModuleState(){ + return requestedModuleState; + } + + public SwerveModuleState getActualModuleState() { + return new SwerveModuleState(getDriveWheelSpeed(), getSteerRotations()); } public SwerveModulePosition getModulePosition(){ return new SwerveModulePosition(this.getModuleDistance(), getSteerRotations()); } + public void periodic(){ + SmartDashboard.putNumber(moduleName + " steerReq", steerPosRequ.Position); + SmartDashboard.putNumber(moduleName + " steerPow", steerMotor.getClosedLoopOutput().getValueAsDouble()); + SmartDashboard.putNumber(moduleName + " steer error", steerMotor.getClosedLoopError().getValueAsDouble()); + SmartDashboard.putNumber(moduleName + " steer setpoint", steerMotor.getClosedLoopReference().getValueAsDouble()); + + SmartDashboard.putNumber(moduleName + " steerPos", getSteerRotations().getRotations()); + } + public String getModuleName(){ return moduleName; } @@ -147,14 +168,18 @@ public class SwerveModule { return distance; } - private TalonFXConfiguration generateDriveTalonConfiguration(double kP, double kI, double kD, double kV){ + private TalonFXConfiguration generateDriveTalonConfiguration(double kP, double kI, double kD, double kV, boolean driveInverted){ return new TalonFXConfiguration().withSlot0( new Slot0Configs() .withKP(kP) .withKI(kI) .withKD(kD) .withKV(kV) - ); + ) + .withMotorOutput( + new MotorOutputConfigs() + .withInverted(driveInverted ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive) + ); } private TalonFXConfiguration generateSteerTalonConfiguration(double kP, double kI, double kD, double kS, double kV, double kA){ @@ -169,13 +194,13 @@ public class SwerveModule { .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign) ).withMotionMagic( new MotionMagicConfigs() - .withMotionMagicCruiseVelocity(80) + .withMotionMagicCruiseVelocity(160) .withMotionMagicAcceleration(160) .withMotionMagicJerk(1600) ).withFeedback( new FeedbackConfigs() .withFeedbackRemoteSensorID(steerEncoder.getDeviceID()) - .withFeedbackSensorSource(FeedbackSensorSourceValue.FusedCANcoder) + .withFeedbackSensorSource(FeedbackSensorSourceValue.RemoteCANcoder) .withSensorToMechanismRatio(1.0) .withRotorToSensorRatio(12.8) ).withClosedLoopGeneral( @@ -187,8 +212,9 @@ public class SwerveModule { private CANcoderConfiguration generateCanCoderConfiguration(double steerOffset){ return new CANcoderConfiguration().withMagnetSensor( new MagnetSensorConfigs() - .withMagnetOffset(steerOffset) - .withSensorDirection(SensorDirectionValue.CounterClockwise_Positive) + .withMagnetOffset(-steerOffset) + .withSensorDirection(SensorDirectionValue.Clockwise_Positive) + .withAbsoluteSensorDiscontinuityPoint(0.5) ); }