candle.clearAllAnimations();
lightsOff();
- System.out.println("CANdle features: " + featureConf + ", LED config: " + ledConf);
+ // System.out.println("CANdle features: " + featureConf + ", LED config: " + ledConf);
SmartDashboard.putData("LED Disable", new InstantCommand(() -> {forceOff = true; lightsOff();}).ignoringDisable(true));
SmartDashboard.putData("LED Enable", new InstantCommand(() -> forceOff = false).ignoringDisable(true));
if (gyroBiasEstimator.getSampleCount() >= GyroBiasConstants.MIN_SAMPLES) {
double fullBias = gyroBiasEstimator.getAndResetBias();
double bias = gyroBiasEstimator.applyPartialCorrection(fullBias);
- System.out.println("bias: " + bias);
- System.out.println("FullBias"+ fullBias);
-
+
if (Math.abs(bias) > GyroBiasConstants.MIN_CORRECTION_RAD) {
gyroIO.setYaw(new Rotation2d(currentGyroYaw + bias));
}