Cache swerve module CANCoder feedback coefficients

- Add a proper logging system
This commit is contained in:
nathanrsxtn
2022-01-30 00:39:17 -07:00
parent 7e3af39918
commit bb1c58764e
6 changed files with 136 additions and 8 deletions
@@ -4,10 +4,10 @@
package frc4388.robot.subsystems;
import edu.wpi.first.wpilibj.motorcontrol.Spark;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import java.util.logging.Logger;
import edu.wpi.first.wpilibj.motorcontrol.Spark;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.LEDConstants;
import frc4388.utility.LEDPatterns;
@@ -27,7 +27,7 @@ public class LED extends SubsystemBase {
m_LEDController = LEDController;
setPattern(LEDConstants.DEFAULT_PATTERN);
updateLED();
System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good.");
Logger.getLogger(LED.class.getName()).finer("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good.");
}
@Override
@@ -27,13 +27,14 @@ public class SwerveModule extends SubsystemBase {
private static double kEncoderTicksPerRotation = 4096;
private SwerveModuleState state;
private double canCoderFeedbackCoefficient;
/** Creates a new SwerveModule. */
public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder, double offset) {
this.driveMotor = driveMotor;
this.angleMotor = angleMotor;
this.canCoder = canCoder;
canCoderFeedbackCoefficient = canCoder.configGetFeedbackCoefficient();
TalonFXConfiguration angleTalonFXConfiguration = new TalonFXConfiguration();
@@ -82,7 +83,7 @@ public class SwerveModule extends SubsystemBase {
// 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() / canCoder.configGetFeedbackCoefficient();
double currentTicks = canCoder.getPosition() / canCoderFeedbackCoefficient;
double desiredTicks = currentTicks + deltaTicks;
if (!ignoreAngle){
angleMotor.set(TalonFXControlMode.Position, desiredTicks);