Voltage ClawController::update(Angle encoder_angle, bool min_hall, bool max_hall, bool enabled) {
Voltage voltage = 0*V;
Angle current_angle = encoder_angle + _offset;
SmartDashboard::PutNumber("Controller angle", current_angle.to(rad));
Angle error_p = (_goal - current_angle);
SmartDashboard::PutNumber("Proportional angle", error_p.to(rad));
AngularVelocity error_d = (error_p - _error_last) / dt;
switch (_state) {
case CALIBRATING:
if (min_hall) {
voltage = 0*V;
_offset = -encoder_angle;
_state = RUNNING;
}
else {
voltage = -2*V;
}
break;
case RUNNING:
voltage = k_proportional * error_p + k_derivative * error_d;
_error_last = encoder_angle;
break;
case INITIALIZING:
if (enabled) {
_state = CALIBRATING;
}
break;
case ESTOP:
break;
}
SmartDashboard::PutNumber("Claw Voltage", voltage.to(V));
return rangeify(-12.0*V, voltage, 12.0*V);
}
auto rangeify(Container& container)
{
return rangeify(container.begin(), container.end());
}
