Jose Eduardo Laruta Espejo / Mbed 2 deprecated maple_heading_pid

Heading control with pid and magnetometer

Dependencies:   HMC5883L mbed motoresDC

main.cpp@0:c4c11b0ac1b9, 2016-06-03 (annotated)

Committer:
tabris2015
Date:
Fri Jun 03 03:57:23 2016 +0000
Revision:
0:c4c11b0ac1b9
First commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tabris2015 0:c4c11b0ac1b9 1 #define MAPLE_MINI
tabris2015 0:c4c11b0ac1b9 2 #include "mbed.h"
tabris2015 0:c4c11b0ac1b9 3 #include "pines.h"
tabris2015 0:c4c11b0ac1b9 4 #include <motoresDC.h>
tabris2015 0:c4c11b0ac1b9 5 #include <HMC5883L.h>
tabris2015 0:c4c11b0ac1b9 6
tabris2015 0:c4c11b0ac1b9 7 HMC5883L brujula;
tabris2015 0:c4c11b0ac1b9 8 InterruptIn boton(PB_8);
tabris2015 0:c4c11b0ac1b9 9 //motores D27 D31 D30 D26 D29 D28
tabris2015 0:c4c11b0ac1b9 10 MotoresDC carro(PWM_L, CTRL1_L, CTRL2_L, PWM_R, CTRL1_R, CTRL2_R);
tabris2015 0:c4c11b0ac1b9 11
tabris2015 0:c4c11b0ac1b9 12 Timer t;
tabris2015 0:c4c11b0ac1b9 13 unsigned long prev_time;
tabris2015 0:c4c11b0ac1b9 14 unsigned long interval = 33;
tabris2015 0:c4c11b0ac1b9 15 double maxOut = 0.5;
tabris2015 0:c4c11b0ac1b9 16 double minOut = -0.5;
tabris2015 0:c4c11b0ac1b9 17 volatile double setpoint = 0.0;
tabris2015 0:c4c11b0ac1b9 18 double iTerm = 0;
tabris2015 0:c4c11b0ac1b9 19 double lastInput;
tabris2015 0:c4c11b0ac1b9 20 double kp = 1.1;
tabris2015 0:c4c11b0ac1b9 21 double ki = 0.01;
tabris2015 0:c4c11b0ac1b9 22 double kd = 0.0;
tabris2015 0:c4c11b0ac1b9 23
tabris2015 0:c4c11b0ac1b9 24 PwmOut led(PB_1);
tabris2015 0:c4c11b0ac1b9 25
tabris2015 0:c4c11b0ac1b9 26 void toggle()
tabris2015 0:c4c11b0ac1b9 27 {
tabris2015 0:c4c11b0ac1b9 28 if(setpoint == 0.0)
tabris2015 0:c4c11b0ac1b9 29 {
tabris2015 0:c4c11b0ac1b9 30 setpoint = 0.5;
tabris2015 0:c4c11b0ac1b9 31 }
tabris2015 0:c4c11b0ac1b9 32 else
tabris2015 0:c4c11b0ac1b9 33 {
tabris2015 0:c4c11b0ac1b9 34 setpoint = 0.0;
tabris2015 0:c4c11b0ac1b9 35 }
tabris2015 0:c4c11b0ac1b9 36
tabris2015 0:c4c11b0ac1b9 37 }
tabris2015 0:c4c11b0ac1b9 38 // functions
tabris2015 0:c4c11b0ac1b9 39 void init_p()
tabris2015 0:c4c11b0ac1b9 40 {
tabris2015 0:c4c11b0ac1b9 41 led = 1;
tabris2015 0:c4c11b0ac1b9 42 carro.detener();
tabris2015 0:c4c11b0ac1b9 43 boton.fall(toggle);
tabris2015 0:c4c11b0ac1b9 44 brujula.init();
tabris2015 0:c4c11b0ac1b9 45 wait_ms(1000);
tabris2015 0:c4c11b0ac1b9 46 led = 0;
tabris2015 0:c4c11b0ac1b9 47 wait_ms(500);
tabris2015 0:c4c11b0ac1b9 48 led = 1;
tabris2015 0:c4c11b0ac1b9 49 t.start();
tabris2015 0:c4c11b0ac1b9 50 wait_ms(2000);
tabris2015 0:c4c11b0ac1b9 51
tabris2015 0:c4c11b0ac1b9 52 led = 0;
tabris2015 0:c4c11b0ac1b9 53 }
tabris2015 0:c4c11b0ac1b9 54
tabris2015 0:c4c11b0ac1b9 55 int main() {
tabris2015 0:c4c11b0ac1b9 56 init_p();
tabris2015 0:c4c11b0ac1b9 57 while(1) {
tabris2015 0:c4c11b0ac1b9 58 unsigned long now = t.read_ms();
tabris2015 0:c4c11b0ac1b9 59 if((now - prev_time) > interval)
tabris2015 0:c4c11b0ac1b9 60 {
tabris2015 0:c4c11b0ac1b9 61 prev_time = now;
tabris2015 0:c4c11b0ac1b9 62
tabris2015 0:c4c11b0ac1b9 63 double input = brujula.getHeading()/ 180.0 -1.0;
tabris2015 0:c4c11b0ac1b9 64 // PID
tabris2015 0:c4c11b0ac1b9 65 double error = setpoint - input;
tabris2015 0:c4c11b0ac1b9 66 if(abs(error) < 0.1) led = 1;
tabris2015 0:c4c11b0ac1b9 67 else led = 0;
tabris2015 0:c4c11b0ac1b9 68
tabris2015 0:c4c11b0ac1b9 69 iTerm += ki * error;
tabris2015 0:c4c11b0ac1b9 70 if(iTerm > maxOut) iTerm = maxOut;
tabris2015 0:c4c11b0ac1b9 71 else if(iTerm < minOut) iTerm = minOut;
tabris2015 0:c4c11b0ac1b9 72
tabris2015 0:c4c11b0ac1b9 73 double dInput = input - lastInput;
tabris2015 0:c4c11b0ac1b9 74
tabris2015 0:c4c11b0ac1b9 75 double output = kp* error + iTerm + kd * dInput;
tabris2015 0:c4c11b0ac1b9 76 if(output > maxOut) output = maxOut;
tabris2015 0:c4c11b0ac1b9 77 else if(output < minOut) output = minOut;
tabris2015 0:c4c11b0ac1b9 78 carro.pivotar(output);
tabris2015 0:c4c11b0ac1b9 79 lastInput = input;
tabris2015 0:c4c11b0ac1b9 80 }
tabris2015 0:c4c11b0ac1b9 81 }
tabris2015 0:c4c11b0ac1b9 82 }