Added friction compensation to the angle adjust.
diff --git a/frc971/control_loops/zeroed_joint.h b/frc971/control_loops/zeroed_joint.h
index 468dc13..0fbe33e 100644
--- a/frc971/control_loops/zeroed_joint.h
+++ b/frc971/control_loops/zeroed_joint.h
@@ -117,6 +117,8 @@
double zeroing_off_speed;
// Maximum voltage to apply when zeroing.
double max_zeroing_voltage;
+ // Deadband voltage.
+ double deadband_voltage;
// Angles where we see a positive edge from the hall effect sensors.
double hall_effect_start_angle[kNumZeroSensors];
};
@@ -432,7 +434,18 @@
break;
}
if (output_enabled) {
- return loop_->voltage();
+ double voltage = loop_->voltage();
+ if (voltage > 0) {
+ voltage += config_data_.deadband_voltage;
+ } else if (voltage < 0) {
+ voltage -= config_data_.deadband_voltage;
+ }
+ if (voltage > 12.0) {
+ voltage = 12.0;
+ } else if (voltage < -12.0) {
+ voltage = -12.0;
+ }
+ return voltage;
} else {
return 0.0;
}