mirror of
https://github.com/Team4388/Robot-Essentials.git
synced 2026-06-09 00:38:01 -06:00
worky
This commit is contained in:
Abhishrek05
@@ -7,175 +7,155 @@ package frc4388.robot.subsystems;
|
||||
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
|
||||
import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
|
||||
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
|
||||
import com.ctre.phoenix.motorcontrol.TalonFXFeedbackDevice;
|
||||
import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
|
||||
import com.ctre.phoenix.sensors.CANCoder;
|
||||
import com.ctre.phoenix.sensors.CANCoderConfiguration;
|
||||
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.kinematics.SwerveModulePosition;
|
||||
import edu.wpi.first.math.kinematics.SwerveModuleState;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.utility.Gains;
|
||||
|
||||
public class SwerveModule extends SubsystemBase {
|
||||
public WPI_TalonFX angleMotor;
|
||||
public WPI_TalonFX driveMotor;
|
||||
private CANCoder canCoder;
|
||||
public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS;
|
||||
|
||||
private static double kEncoderTicksPerRotation = 4096;
|
||||
private SwerveModuleState state;
|
||||
private double canCoderFeedbackCoefficient;
|
||||
|
||||
public long m_currentTime;
|
||||
public long m_lastTime;
|
||||
public double m_deltaTime;
|
||||
|
||||
public double m_currentPos;
|
||||
public double m_lastPos;
|
||||
|
||||
public SwerveModuleState lastState = new SwerveModuleState();
|
||||
public SwerveModuleState currentState;
|
||||
private WPI_TalonFX driveMotor;
|
||||
private WPI_TalonFX angleMotor;
|
||||
private CANCoder encoder;
|
||||
|
||||
public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS;
|
||||
|
||||
/** Creates a new SwerveModule. */
|
||||
public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder, double offset) {
|
||||
public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder encoder, double offset) {
|
||||
this.driveMotor = driveMotor;
|
||||
this.angleMotor = angleMotor;
|
||||
this.canCoder = canCoder;
|
||||
canCoderFeedbackCoefficient = canCoder.configGetFeedbackCoefficient();
|
||||
this.encoder = encoder;
|
||||
|
||||
TalonFXConfiguration angleTalonFXConfiguration = new TalonFXConfiguration();
|
||||
TalonFXConfiguration angleConfig = new TalonFXConfiguration();
|
||||
angleConfig.slot0.kP = swerveGains.kP;
|
||||
angleConfig.slot0.kI = swerveGains.kI;
|
||||
angleConfig.slot0.kD = swerveGains.kD;
|
||||
|
||||
angleTalonFXConfiguration.slot0.kP = m_swerveGains.kP;
|
||||
angleTalonFXConfiguration.slot0.kI = m_swerveGains.kI;
|
||||
angleTalonFXConfiguration.slot0.kD = m_swerveGains.kD;
|
||||
// use the CANcoder as the remote sensor for the primary TalonFX PID
|
||||
angleConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID();
|
||||
angleConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder;
|
||||
angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
|
||||
angleMotor.configAllSettings(angleConfig);
|
||||
|
||||
// Use the CANCoder as the remote sensor for the primary TalonFX PID
|
||||
angleTalonFXConfiguration.remoteFilter0.remoteSensorDeviceID = canCoder.getDeviceID();
|
||||
angleTalonFXConfiguration.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder;
|
||||
angleTalonFXConfiguration.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
|
||||
angleMotor.configAllSettings(angleTalonFXConfiguration);
|
||||
// angleMotor.setInverted(true);
|
||||
// TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration();
|
||||
// driveTalonFXConfiguration.slot0.kP = 0.05;
|
||||
// driveTalonFXConfiguration.slot0.kI = 0.0;
|
||||
// driveTalonFXConfiguration.slot0.kD = 0.0;
|
||||
// driveTalonFXConfiguration.primaryPID.selectedFeedbackSensor =
|
||||
// FeedbackDevice.IntegratedSensor;
|
||||
driveMotor.configFactoryDefault();
|
||||
driveMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 30);
|
||||
driveMotor.configNominalOutputForward(0, 30);
|
||||
driveMotor.configNominalOutputReverse(0, 30);
|
||||
driveMotor.configPeakOutputForward(1, 30);
|
||||
driveMotor.configPeakOutputReverse(-1, 30);
|
||||
driveMotor.configAllowableClosedloopError(0, 0, 30);
|
||||
// driveMotor.setInverted(true);
|
||||
driveMotor.config_kP(0, 0, 30);
|
||||
driveMotor.config_kI(0, 0, 30);
|
||||
driveMotor.config_kD(0, 0, 30);
|
||||
encoder.configMagnetOffset(offset);
|
||||
|
||||
// driveMotor.configAllSettings(driveTalonFXConfiguration);
|
||||
|
||||
CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration();
|
||||
canCoderConfiguration.sensorCoefficient = 0.087890625;
|
||||
canCoderConfiguration.magnetOffsetDegrees = offset;
|
||||
canCoderConfiguration.sensorDirection = true;
|
||||
canCoder.configAllSettings(canCoderConfiguration);
|
||||
|
||||
m_currentTime = System.currentTimeMillis();
|
||||
m_lastTime = System.currentTimeMillis();
|
||||
|
||||
m_lastPos = driveMotor.getSelectedSensorPosition();
|
||||
}
|
||||
|
||||
private Rotation2d getAngle() {
|
||||
// ! Note: This assumes the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees.
|
||||
return Rotation2d.fromDegrees(canCoder.getAbsolutePosition());
|
||||
driveMotor.setSelectedSensorPosition(0);
|
||||
driveMotor.config_kP(0, 0.2);
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the speed + rotation of the swerve module from a SwerveModuleState object
|
||||
*
|
||||
* @param desiredState - A SwerveModuleState representing the desired new state
|
||||
* of the module
|
||||
* Get the drive motor of the SwerveModule
|
||||
* @return the drive motor of the SwerveModule
|
||||
*/
|
||||
public void setDesiredState(SwerveModuleState desiredState, boolean ignoreAngle) {
|
||||
Rotation2d currentRotation = getAngle();
|
||||
// currentRotation.getDegrees());
|
||||
state = SwerveModuleState.optimize(desiredState, currentRotation);
|
||||
|
||||
// Find the difference between our current rotational position + our new
|
||||
// rotational position
|
||||
Rotation2d rotationDelta = state.angle.minus(currentRotation);
|
||||
|
||||
// Find the new absolute position of the module based on the difference in
|
||||
// rotation
|
||||
double deltaTicks = (rotationDelta.getDegrees() / 360.) * kEncoderTicksPerRotation;
|
||||
// Convert the CANCoder from it's position reading back to ticks
|
||||
double currentTicks = canCoder.getPosition() / canCoderFeedbackCoefficient;
|
||||
double desiredTicks = currentTicks + deltaTicks;
|
||||
|
||||
if (!ignoreAngle) {
|
||||
angleMotor.set(TalonFXControlMode.Position, desiredTicks);
|
||||
}
|
||||
|
||||
// Please work
|
||||
double ftPerSec = Units.metersToFeet(state.speedMetersPerSecond);
|
||||
double normFtPerSec = ftPerSec / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC;
|
||||
// double angleCorrection = angleMotor.getSelectedSensorVelocity() * 2.69;
|
||||
driveMotor.set(normFtPerSec);// - angleMotor.get());
|
||||
// driveMotor.set(TalonFXControlMode.Velocity, angleCorrection); // Ratio
|
||||
// between axis = 1/1.75 Ratio of wheel is 5.14/1 ratio of steer is 12.8/1
|
||||
public WPI_TalonFX getDriveMotor() {
|
||||
return this.driveMotor;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get current module state.
|
||||
*
|
||||
* @return The current state of the module in m/s.
|
||||
* Get the angle motor of the SwerveModule
|
||||
* @return the angle motor of the SwerveModule
|
||||
*/
|
||||
public SwerveModuleState getState() {
|
||||
// return state;
|
||||
return new SwerveModuleState(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.INCHES_PER_TICK
|
||||
* SwerveDriveConstants.METERS_PER_INCH * 10, getAngle());
|
||||
public WPI_TalonFX getAngleMotor() {
|
||||
return this.angleMotor;
|
||||
}
|
||||
|
||||
/**
|
||||
* Stop the drive and steer motors of current module.
|
||||
* Get the CANcoder of the SwerveModule
|
||||
* @return the CANcoder of the SwerveModule
|
||||
*/
|
||||
public CANCoder getEncoder() {
|
||||
return this.encoder;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the angle of a SwerveModule as a Rotation2d
|
||||
* @return the angle of a SwerveModule as a Rotation2d
|
||||
*/
|
||||
public Rotation2d getAngle() {
|
||||
// * Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees.
|
||||
return Rotation2d.fromDegrees(encoder.getAbsolutePosition());
|
||||
}
|
||||
|
||||
public double getAngularVel() {
|
||||
return this.angleMotor.getSelectedSensorVelocity();
|
||||
}
|
||||
|
||||
public double getDrivePos() {
|
||||
return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV;
|
||||
}
|
||||
|
||||
public double getDriveVel() {
|
||||
return this.driveMotor.getSelectedSensorVelocity(0);
|
||||
}
|
||||
|
||||
public void stop() {
|
||||
driveMotor.set(0);
|
||||
angleMotor.set(0);
|
||||
}
|
||||
|
||||
public void rotateToAngle(double angle) {
|
||||
this.angleMotor.set(TalonFXControlMode.Position, angle);
|
||||
angleMotor.set(TalonFXControlMode.Position, angle);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
currentState = this.getState();
|
||||
|
||||
Rotation2d currentRotation = getAngle();
|
||||
SmartDashboard.putNumber("Angle Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees());
|
||||
SmartDashboard.putNumber("Drive Motor " + driveMotor.getDeviceID(),
|
||||
((driveMotor.getSelectedSensorPosition() / 2048) * 360) % 360);
|
||||
|
||||
lastState = currentState;
|
||||
/**
|
||||
* Get state of swerve module
|
||||
* @return speed in m/s and angle in degrees
|
||||
*/
|
||||
public SwerveModuleState getState() {
|
||||
return new SwerveModuleState(
|
||||
Units.inchesToMeters(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.Conversions.INCHES_PER_TICK) * SwerveDriveConstants.Conversions.TICK_TIME_TO_SECONDS,
|
||||
getAngle()
|
||||
);
|
||||
}
|
||||
|
||||
public void reset() {
|
||||
canCoder.setPositionToAbsolute();
|
||||
// canCoder.configSensorInitializationStrategy(initializationStrategy)
|
||||
/**
|
||||
* Returns the current position of the SwerveModule
|
||||
* @return The current position of the SwerveModule in meters traveled by the driveMotor and the angle of the angleMotor.
|
||||
*/
|
||||
public SwerveModulePosition getPosition() {
|
||||
return new SwerveModulePosition(Units.inchesToMeters(driveMotor.getSelectedSensorPosition() * SwerveDriveConstants.Conversions.INCHES_PER_TICK), getAngle());
|
||||
}
|
||||
public double getCurrent(){
|
||||
|
||||
/**
|
||||
* Set the speed and rotation of the SwerveModule from a SwerveModuleState object
|
||||
* @param desiredState a SwerveModuleState representing the desired new state of the module
|
||||
*/
|
||||
public void setDesiredState(SwerveModuleState desiredState) {
|
||||
Rotation2d currentRotation = this.getAngle();
|
||||
|
||||
SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation);
|
||||
|
||||
// calculate the difference between our current rotational position and our new rotational position
|
||||
Rotation2d rotationDelta = state.angle.minus(currentRotation);
|
||||
|
||||
// calculate the new absolute position of the SwerveModule based on the difference in rotation
|
||||
double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION;
|
||||
|
||||
// convert the CANCoder from its position reading to ticks
|
||||
double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient();
|
||||
|
||||
angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks);
|
||||
|
||||
double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
|
||||
|
||||
driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
|
||||
}
|
||||
|
||||
public void reset(double position) {
|
||||
encoder.setPositionToAbsolute();
|
||||
}
|
||||
|
||||
public double getCurrent() {
|
||||
return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent();
|
||||
}
|
||||
|
||||
public double getVoltage(){
|
||||
public double getVoltage() {
|
||||
return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage()));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user