Skip to content

Instantly share code, notes, and snippets.

@mgronhol

mgronhol/servo.c

Created March 4, 2017 10:05
Show Gist options
  • Save mgronhol/63724818018fddb8459d416cff2ace53 to your computer and use it in GitHub Desktop.
Save mgronhol/63724818018fddb8459d416cff2ace53 to your computer and use it in GitHub Desktop.
Download ZIP
Small servo loop
Raw
servo.c
float servo( motor_t *motor, int target ){
int pos_error = target - motor->enc.current_value;
int velocity = motor->enc.current_value - motor->prev_position;
float v_error, out, q;
motor->prev_position = motor->enc.current_value;
/* P for position */
motor->v_target = 0.99f * pos_error;
/* P + lowpass for velocity */
v_error = motor->v_target - velocity;
out = v_error * 0.5f;
q = 0.3f;
out = out * (1-q) + motor->prev_df * q;
motor->prev_df = out;
return out;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment