The unified diff between revisions [23a3e9a5..] and [64de686d..] is displayed below. It can also be downloaded as a raw diff.
This diff has been restricted to the following files: 'motor.c'
# # old_revision [23a3e9a50b4034343e3bd217d2c225dcaec064dd] # new_revision [64de686d701acb9539dc52fe0bff299405612ab0] # # add_file "motor.c" # content [b9920bb4788532a73cb93301787e4d0540cc3d56] # ============================================================ --- /dev/null +++ motor.c b9920bb4788532a73cb93301787e4d0540cc3d56 @@ -0,0 +1,154 @@ +/* motor.c */ + +#include "stick.h" +#include "timer.h" +#include "dcm.h" +#include "uart.h" +#include "status.h" + +float integral[3] = {0.0f, 0.0f, 0.0f}; +float last[3]; + +float throttle = 0.0f; + +#define Kp 0.2 +#define Ki 0.04 +#define Kd 0.08 +#define Ka 0.0 + +#define Kp_y 0.2 +#define Ki_y 0.00 +#define Kd_y 0.00 +#define Ka_y 0.0 + + +/* + * Perform a PID loop iteration. + * roll and pitch are absolute values + * yaw is, currently, a rate. + */ +void motor_pid_update(float troll, float mroll, + float tpitch, float mpitch, + float tyaw, float myaw) +{ + float derivative[3]; + float out[3]; + float motor[3]; + float roll, pitch, yaw; + float error, max_error; + float min_motor; + int i; + + roll = troll - mroll; + pitch = tpitch - mpitch; + yaw = tyaw - myaw; + +#if 0 + if ((stick_counter % 100) == 0) { + putstr("{"); + putint_s((int)(tyaw * 10000)); + putstr(", "); + putint_s((int)(myaw * 10000)); + putstr("}\r\n"); + } +#endif + + integral[0] += roll * delta_t; + integral[1] += pitch * delta_t; + integral[2] += yaw * delta_t; + + /* The measurements are the opposite sign to the error */ + derivative[0] = (-mroll - last[0]) / delta_t; + derivative[1] = (-mpitch - last[1]) / delta_t; + derivative[2] = (-myaw - last[2]) / delta_t; + + last[0] = -mroll; + last[1] = -mpitch; + last[2] = -myaw; + + out[0] = roll * Kp + integral[0] * Ki + derivative[0] * Kd; + out[1] = pitch * Kp + integral[1] * Ki + derivative[1] * Kd; + out[2] = yaw * Kp_y + integral[2] * Ki_y + derivative[2] * Kd_y; + + if (status_armed()) { + /* Front right */ + motor[0] = throttle + out[0] + out[1] + out[2]; + /* Front left */ + motor[1] = throttle - out[0] + out[1] - out[2]; + /* Rear left */ + motor[2] = throttle - out[0] - out[1] + out[2]; + /* Rear right */ + motor[3] = throttle + out[0] - out[1] - out[2]; + } else { + motor[0] = 0.0; + motor[1] = 0.0; + motor[2] = 0.0; + motor[3] = 0.0; + } + + max_error = 0.0; + min_motor = 1.0; + for (i = 0; i < 3; i++) { + if (motor[i] < 0.0) + motor[i] = 0.0; + if (motor[i] > 1.0f) { + error = motor[i] - 1.0f; + if (error > max_error) + max_error = error; + } + if (motor[i] < min_motor) + min_motor = motor[i]; + } + + if (max_error > 0.0) { + for (i = 0; i < 3; i++) { + motor[i] -= max_error; + if (motor[i] < 0.0) + motor[i] = 0.0; + } + } + + if (throttle <= 0.0) { + motor[0] = 0.0; + motor[1] = 0.0; + motor[2] = 0.0; + motor[3] = 0.0; + integral[0] = 0.0; + integral[1] = 0.0; + integral[2] = 0.0; + } + if (max_error < min_motor) { + float new_throttle2, new_out[3]; + new_throttle2 = (motor[0] + motor[1] + motor[2] + motor[3])/2.0; + new_out[0] = (motor[0] + motor[3] - new_throttle2)/2.0; + new_out[1] = (motor[0] + motor[1] - new_throttle2)/2.0; + new_out[2] = (motor[0] + motor[2] - new_throttle2)/2.0; + + /* Anti-windup */ + for (i = 0; i < 3; i++) { + if (new_out[i] > 1.0) + integral[i] -= (new_out[i]-1.0) * Ka; + if (new_out[i] < 0.0) + integral[i] -= (new_out[i]) * Ka; + } + } + + timer_set_pwm_value(0, (int)(motor[0] * PWM_MAX)); + timer_set_pwm_value(1, (int)(motor[1] * PWM_MAX)); + timer_set_pwm_value(2, (int)(motor[2] * PWM_MAX)); + timer_set_pwm_value(3, (int)(motor[3] * PWM_MAX)); +} + +void motor_kill(void) { + throttle = 0.0; + timer_set_pwm_value(0, 0); + timer_set_pwm_value(1, 0); + timer_set_pwm_value(2, 0); + timer_set_pwm_value(3, 0); +} + +void motor_set_throttle(float t) { + if (status_armed()) + throttle = t; +} +