Jose Eduardo Laruta Espejo
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
diff -r 000000000000 -r c4c11b0ac1b9 HMC5883L.lib --- /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
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;
+ }
+ }
+}
diff -r 000000000000 -r c4c11b0ac1b9 mbed.bld --- /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
diff -r 000000000000 -r c4c11b0ac1b9 motoresDC.lib --- /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
diff -r 000000000000 -r c4c11b0ac1b9 pines.h --- /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