![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Heading control with pid and magnetometer
Dependencies: HMC5883L mbed motoresDC
Diff: main.cpp
- Revision:
- 0:c4c11b0ac1b9
diff -r 000000000000 -r c4c11b0ac1b9 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Jun 03 03:57:23 2016 +0000 @@ -0,0 +1,82 @@ +#define MAPLE_MINI +#include "mbed.h" +#include "pines.h" +#include <motoresDC.h> +#include <HMC5883L.h> + +HMC5883L brujula; +InterruptIn boton(PB_8); +//motores D27 D31 D30 D26 D29 D28 +MotoresDC carro(PWM_L, CTRL1_L, CTRL2_L, PWM_R, CTRL1_R, CTRL2_R); + +Timer t; +unsigned long prev_time; +unsigned long interval = 33; +double maxOut = 0.5; +double minOut = -0.5; +volatile double setpoint = 0.0; +double iTerm = 0; +double lastInput; +double kp = 1.1; +double ki = 0.01; +double kd = 0.0; + +PwmOut led(PB_1); + +void toggle() +{ + if(setpoint == 0.0) + { + setpoint = 0.5; + } + else + { + setpoint = 0.0; + } + +} +// functions +void init_p() +{ + led = 1; + carro.detener(); + boton.fall(toggle); + brujula.init(); + wait_ms(1000); + led = 0; + wait_ms(500); + led = 1; + t.start(); + wait_ms(2000); + + led = 0; +} + +int main() { + init_p(); + while(1) { + unsigned long now = t.read_ms(); + if((now - prev_time) > interval) + { + prev_time = now; + + double input = brujula.getHeading()/ 180.0 -1.0; + // PID + double error = setpoint - input; + if(abs(error) < 0.1) led = 1; + else led = 0; + + iTerm += ki * error; + if(iTerm > maxOut) iTerm = maxOut; + else if(iTerm < minOut) iTerm = minOut; + + double dInput = input - lastInput; + + double output = kp* error + iTerm + kd * dInput; + if(output > maxOut) output = maxOut; + else if(output < minOut) output = minOut; + carro.pivotar(output); + lastInput = input; + } + } +}