![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Heading control with pid and magnetometer
Dependencies: HMC5883L mbed motoresDC
Revision 0:c4c11b0ac1b9, committed 2016-06-03
- Comitter:
- tabris2015
- Date:
- Fri Jun 03 03:57:23 2016 +0000
- Commit message:
- First commit
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/HMC5883L.lib Fri Jun 03 03:57:23 2016 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/users/tabris2015/code/HMC5883L/#502f2b8b17ea
--- /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; + } + } +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Jun 03 03:57:23 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/6c34061e7c34 \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/motoresDC.lib Fri Jun 03 03:57:23 2016 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/tabris2015/code/motoresDC/#b1e9ffb92a0a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/pines.h Fri Jun 03 03:57:23 2016 +0000 @@ -0,0 +1,28 @@ +#ifdef MAPLE_MINI +// pines para los motores + +// Pines para el driver de motores +#define PWM_L PA_8 // d27 +#define CTRL1_L PB_12 // d31 +#define CTRL2_L PB_13 // d30 +#define PWM_R PA_9 // d26 +#define CTRL1_R PB_14 // d29 +#define CTRL2_R PB_15 // d28 + +// pines para el rf + +#define nrf_CE PA_3 +#define nrf_CSN PA_4 +#define spi_SCK PA_5 +#define spi_MOSI PA_7 +#define spi_MISO PA_6 + +// pines para el puerto serial +#define TX3_PIN PB_10 +#define RX3_PIN PB_11 + + +// pines para bus I2C +#define SDA_PIN PB_7 +#define SCL_PIN PB_6 +#endif \ No newline at end of file