diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f3f4db2..b2fa9d8 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,8 +4,11 @@ package frc.robot; +import org.littletonrobotics.conduit.ConduitApi; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.inputs.LoggedDriverStation; +import org.littletonrobotics.junction.inputs.LoggedSystemStats; import org.littletonrobotics.junction.networktables.NT4Publisher; import com.ctre.phoenix6.hardware.TalonFX; @@ -70,5 +73,12 @@ public void simulationInit() { @Override public void simulationPeriodic() { + refreshAkitData(); + } + + private void refreshAkitData() { + ConduitApi.getInstance().captureData(); + LoggedDriverStation.periodic(); + LoggedSystemStats.periodic(); } } diff --git a/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveAbsoluteEncoder/SwerveAbsoluteEncoderIOCANcoder.java b/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveAbsoluteEncoder/SwerveAbsoluteEncoderIOCANcoder.java index f3417df..1e6ee65 100644 --- a/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveAbsoluteEncoder/SwerveAbsoluteEncoderIOCANcoder.java +++ b/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveAbsoluteEncoder/SwerveAbsoluteEncoderIOCANcoder.java @@ -43,6 +43,7 @@ public SwerveAbsoluteEncoderIOCANcoder(int ID, double encoderOffset) { // .withSensorDirection(SensorDirectionValue.CounterClockwise_Positive) // ); // configurator.apply(magnetSensorConfiguration.withMagnetOffset(encoderOffset/360)); + // CaNcoder.getConfigurator().withSensorDirection(getS) } @@ -52,7 +53,7 @@ public void updateInputs(SwerveAbsoluteEncoderIOInputs inputs) { inputs.absoluteEncoderOffset = absoluteEncoderOffset; inputs.inverted = inverted; - inputs.turnAbsolutePosition = MathUtil.angleModulus(Units.degreesToRadians(CaNcoder.getAbsolutePosition().getValueAsDouble() * 360 + absoluteEncoderOffset)); + inputs.turnAbsolutePosition = MathUtil.angleModulus(Units.degreesToRadians((CaNcoder.getAbsolutePosition().getValueAsDouble() * 360) + absoluteEncoderOffset)); } @Override diff --git a/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIOTalonFX.java b/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIOTalonFX.java index e0c201b..184580f 100644 --- a/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIOTalonFX.java +++ b/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIOTalonFX.java @@ -100,6 +100,7 @@ public SwerveMotorIOTalonFX(int canID, String canbus, double gearRatio) { feedbackController = new PIDController(0, 0, 0); FeedbackConfigs feedback = new FeedbackConfigs(); + feedback.SensorToMechanismRatio = 1; configurator.apply(feedback); timestampQueue = OdometryThread.getInstance().makeTimestampQueue();