|
|
|
@ -1,7 +1,7 @@
|
|
|
|
|
/* motor.c */ |
|
|
|
|
|
|
|
|
|
#include "stick.h" |
|
|
|
|
#include "timer.h" |
|
|
|
|
#include "thrust.h" |
|
|
|
|
#include "dcm.h" |
|
|
|
|
#include "uart.h" |
|
|
|
|
#include "status.h" |
|
|
|
@ -11,8 +11,8 @@ float last[3];
|
|
|
|
|
|
|
|
|
|
float throttle = 0.0f; |
|
|
|
|
|
|
|
|
|
#define Kp 0.2 |
|
|
|
|
#define Ki 0.04 |
|
|
|
|
#define Kp 0.3 |
|
|
|
|
#define Ki 0.02 |
|
|
|
|
#define Kd 0.08 |
|
|
|
|
#define Ka 0.0 |
|
|
|
|
|
|
|
|
@ -57,6 +57,14 @@ void motor_pid_update(float troll, float mroll,
|
|
|
|
|
integral[1] += pitch * delta_t; |
|
|
|
|
integral[2] += yaw * delta_t; |
|
|
|
|
|
|
|
|
|
#define INTEGRAL_LIMIT 1.0f |
|
|
|
|
for (i = 0; i < 3; i++) { |
|
|
|
|
if (integral[i] > INTEGRAL_LIMIT) |
|
|
|
|
integral[i] = INTEGRAL_LIMIT; |
|
|
|
|
if (integral[i] < -INTEGRAL_LIMIT) |
|
|
|
|
integral[i] = -INTEGRAL_LIMIT; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/* The measurements are the opposite sign to the error */ |
|
|
|
|
derivative[0] = (-mroll - last[0]) / delta_t; |
|
|
|
|
derivative[1] = (-mpitch - last[1]) / delta_t; |
|
|
|
@ -133,18 +141,18 @@ void motor_pid_update(float troll, float mroll,
|
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
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)); |
|
|
|
|
set_thrust(0, motor[0]); |
|
|
|
|
set_thrust(1, motor[1]); |
|
|
|
|
set_thrust(2, motor[2]); |
|
|
|
|
set_thrust(3, motor[3]); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
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); |
|
|
|
|
set_thrust(0, 0.0); |
|
|
|
|
set_thrust(1, 0.0); |
|
|
|
|
set_thrust(2, 0.0); |
|
|
|
|
set_thrust(3, 0.0); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void motor_set_throttle(float t) { |
|
|
|
|