Team4388/2023WayOfTheRobot
1
0
Fork 0
mirror of https://github.com/Team4388/2023WayOfTheRobot.git synced 2026-06-09 08:38:02 -06:00
Code Issues Packages Projects Releases Wiki Activity

integrated gyro + odometry, trying to fix drift

Browse Source
This commit is contained in:
Abhi
2023-02-04 11:22:43 -07:00
parent 05165d6ca6
commit 47c29c9b34
5 changed files with 234 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+24 -5
View File
@@ -20,8 +20,8 @@ import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.utility.Gains;
public class SwerveModule extends SubsystemBase {
public WPI_TalonFX driveMotor;
public WPI_TalonFX angleMotor;
private WPI_TalonFX driveMotor;
private WPI_TalonFX angleMotor;
private CANCoder encoder;
public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS;
@@ -44,6 +44,9 @@ public class SwerveModule extends SubsystemBase {
angleMotor.configAllSettings(angleConfig);
encoder.configMagnetOffset(offset);
driveMotor.setSelectedSensorPosition(0);
driveMotor.config_kP(0, 0.2);
}
/**
@@ -79,6 +82,18 @@ public class SwerveModule extends SubsystemBase {
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);
@@ -117,19 +132,23 @@ public class SwerveModule extends SubsystemBase {
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);
Rotation2d rotationDelta = state.angle.minus(currentRotation); // ? might need to be negative
// 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();
double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient(); // ? why feedback coefficient
angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks);
double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
// double inchesPerSecond = Units.metersToFeet(state.speedMetersPerSecond) * 12;
driveMotor.set(-1 * angleMotor.get() + feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND);
// driveMotor.set(TalonFXControlMode.Velocity, inchesPerSecond * SwerveDriveConstants.Conversions.TICKS_PER_INCH * SwerveDriveConstants.Conversions.SECONDS_TO_TICK_TIME);
// driveMotor.set(0.1);
// double angleCorrection = getAngularVel() * 2.69;
driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
// driveMotor.set(TalonFXControlMode.Velocity, angleCorrection + inchesPerSecond * SwerveDriveConstants.Conversions.TICKS_PER_INCH * SwerveDriveConstants.Conversions.SECONDS_TO_TICK_TIME);
}
public void reset(double position) {