Irving Hernandez / Mbed 2 deprecated Ragnarok_2ejes

Final, cambios pendientes

Dependencies:   MAG3110 MMA8451Q SLCD eCompass_Lib mbed-rtos mbed

Fork of KL46_eCompass_TiltCompensed_Acel-Mag by Irving Hernandez

main.cpp@6:93c9844af619, 2015-12-22 (annotated)

Committer:
IrvingHdez
Date:
Tue Dec 22 03:52:59 2015 +0000
Revision:
6:93c9844af619
Parent:
5:5199647e821a 
Final

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JimCarver 0:4e1d43dc608f 1 #include "mbed.h"
JimCarver 2:e4ae1d748311 2 #include "eCompass_Lib.h"
JimCarver 0:4e1d43dc608f 3 #include "MAG3110.h"
JimCarver 0:4e1d43dc608f 4 #include "MMA8451Q.h"
JimCarver 0:4e1d43dc608f 5 #include "rtos.h"
JimCarver 0:4e1d43dc608f 6 #include "SLCD.h"
JimCarver 0:4e1d43dc608f 7
JimCarver 0:4e1d43dc608f 8 #define MMA8451_I2C_ADDRESS (0x1d<<1)
JimCarver 0:4e1d43dc608f 9
IrvingHdez 6:93c9844af619 10 #define espera 0.0001
IrvingHdez 6:93c9844af619 11 #define sp 0.5 // set point
IrvingHdez 6:93c9844af619 12 #define sp0 0.5 // set point
IrvingHdez 6:93c9844af619 13 #define sp1 0.4 // set point
IrvingHdez 6:93c9844af619 14 #define idle 0
IrvingHdez 6:93c9844af619 15 #define full 0.7
IrvingHdez 6:93c9844af619 16 #define k 100 // Divisor
IrvingHdez 6:93c9844af619 17 #define lol 0.0
IrvingHdez 6:93c9844af619 18 #define m -1.333
IrvingHdez 6:93c9844af619 19
IrvingHdez 6:93c9844af619 20 PwmOut motor0(A2); // PITCH arriba
IrvingHdez 6:93c9844af619 21 PwmOut motor1(A3); // ROLL izq
IrvingHdez 6:93c9844af619 22 PwmOut motor2(A4); // ROLL der
IrvingHdez 6:93c9844af619 23 PwmOut motor3(A5); // PITCH abajo
IrvingHdez 6:93c9844af619 24
IrvingHdez 6:93c9844af619 25 /*PwmOut motor1(A2); // PITCH arriba
IrvingHdez 6:93c9844af619 26 PwmOut motor0(A3); // ROLL izq
IrvingHdez 6:93c9844af619 27 PwmOut motor3(A4); // ROLL der
IrvingHdez 6:93c9844af619 28 PwmOut motor2(A5); // PITCH abajo*/
IrvingHdez 6:93c9844af619 29
IrvingHdez 6:93c9844af619 30 DigitalIn enable(SW1);
IrvingHdez 6:93c9844af619 31 DigitalIn btn2(SW3);
IrvingHdez 6:93c9844af619 32 DigitalOut led(LED1);
IrvingHdez 6:93c9844af619 33 DigitalOut led2(LED2);
IrvingHdez 6:93c9844af619 34
IrvingHdez 6:93c9844af619 35 int i=0,flag=0;
IrvingHdez 6:93c9844af619 36 float xm=0, x=0;
IrvingHdez 6:93c9844af619 37 float ym=0, y=0;
IrvingHdez 6:93c9844af619 38
IrvingHdez 6:93c9844af619 39 float pwmVal0=0, pwmVal1=0; // Valores auxiliares para imprimir
IrvingHdez 6:93c9844af619 40 int i0=0,i1=0,i2=0,i3=0;
IrvingHdez 6:93c9844af619 41 float scalerX=0.5;
IrvingHdez 6:93c9844af619 42 float scalerY=0.5;
IrvingHdez 6:93c9844af619 43 float difSample = 0, currSample = 0;
IrvingHdez 6:93c9844af619 44 float c = 0;
JimCarver 0:4e1d43dc608f 45
JimCarver 2:e4ae1d748311 46 eCompass compass;
JimCarver 0:4e1d43dc608f 47 MAG3110 mag( PTE25, PTE24);
JimCarver 0:4e1d43dc608f 48 MMA8451Q acc( PTE25, PTE24, MMA8451_I2C_ADDRESS);
JimCarver 2:e4ae1d748311 49 DigitalOut red(LED_RED);
JimCarver 2:e4ae1d748311 50 DigitalOut green(LED_GREEN);
JimCarver 0:4e1d43dc608f 51 Serial pc(USBTX, USBRX);
JimCarver 0:4e1d43dc608f 52 SLCD slcd;
JimCarver 0:4e1d43dc608f 53
JimCarver 2:e4ae1d748311 54 extern axis6_t axis6;
JimCarver 2:e4ae1d748311 55 extern uint32_t seconds;
JimCarver 2:e4ae1d748311 56 extern uint32_t compass_type;
JimCarver 2:e4ae1d748311 57 extern int32_t tcount;
JimCarver 2:e4ae1d748311 58 extern uint8_t cdebug;
JimCarver 3:0770c275e6e8 59
JimCarver 3:0770c275e6e8 60 MotionSensorDataCounts mag_raw;
JimCarver 3:0770c275e6e8 61 MotionSensorDataCounts acc_raw;
JimCarver 3:0770c275e6e8 62
IrvingHdez 6:93c9844af619 63 // Declaracion de tareas
JimCarver 3:0770c275e6e8 64 Thread *(debugp);
JimCarver 3:0770c275e6e8 65 Thread *(calibrate);
JimCarver 3:0770c275e6e8 66 Thread *(lcdp);
IrvingHdez 6:93c9844af619 67 Thread *(bandera);
IrvingHdez 6:93c9844af619 68 Thread *(controller);
IrvingHdez 6:93c9844af619 69 Thread *(controllerY);
JimCarver 3:0770c275e6e8 70
JimCarver 2:e4ae1d748311 71 //
JimCarver 2:e4ae1d748311 72 // Print data values for debug
JimCarver 2:e4ae1d748311 73 //
JimCarver 2:e4ae1d748311 74 void debug_print(void)
IrvingHdez 6:93c9844af619 75 {
IrvingHdez 6:93c9844af619 76 pc.printf("roll=%d, pitch=%d, yaw=%d\r\n", axis6.roll, axis6.pitch, axis6.yaw);
JimCarver 2:e4ae1d748311 77 }
IrvingHdez 6:93c9844af619 78
JimCarver 2:e4ae1d748311 79 // HAL Map for KL46 Freedom board MMA8451Q & MAG3110 sensors
JimCarver 3:0770c275e6e8 80 void hal_map( MotionSensorDataCounts * acc_data, MotionSensorDataCounts * mag_data)
JimCarver 0:4e1d43dc608f 81 {
JimCarver 0:4e1d43dc608f 82 int16_t t;
JimCarver 3:0770c275e6e8 83 // swap and negate accelerometer x & y
JimCarver 3:0770c275e6e8 84 t = acc_data->y;
JimCarver 3:0770c275e6e8 85 acc_data->y = acc_data->x * -1;
JimCarver 3:0770c275e6e8 86 acc_data->x = t * -1;
JimCarver 3:0770c275e6e8 87
JimCarver 0:4e1d43dc608f 88 // negate magnetometer y
JimCarver 3:0770c275e6e8 89 mag_data->y *= -1;
JimCarver 0:4e1d43dc608f 90
JimCarver 0:4e1d43dc608f 91 }
JimCarver 0:4e1d43dc608f 92
JimCarver 0:4e1d43dc608f 93 //
JimCarver 0:4e1d43dc608f 94 // This is the 50Hz thread where the magic happens
JimCarver 0:4e1d43dc608f 95 //
JimCarver 2:e4ae1d748311 96 int l = 0;
JimCarver 2:e4ae1d748311 97
JimCarver 0:4e1d43dc608f 98 void compass_thread(void const *argument) {
JimCarver 0:4e1d43dc608f 99 // get raw data from the sensors
JimCarver 3:0770c275e6e8 100 mag.getAxis(mag_raw);
JimCarver 3:0770c275e6e8 101 acc.getAxis(acc_raw);
JimCarver 2:e4ae1d748311 102 if(tcount) compass.run( acc_raw, mag_raw); // calculate the eCompass
JimCarver 3:0770c275e6e8 103 if(!(l % 10)) lcdp->signal_set(0x04);
IrvingHdez 6:93c9844af619 104 if(l++ >= 10) { // take care of business once a second//50
JimCarver 0:4e1d43dc608f 105 seconds++;
IrvingHdez 6:93c9844af619 106 bandera->signal_set(0x03);
IrvingHdez 6:93c9844af619 107 controller->signal_set(0x05);
IrvingHdez 6:93c9844af619 108 controllerY->signal_set(0x06);
JimCarver 3:0770c275e6e8 109 calibrate->signal_set(0x2);
IrvingHdez 6:93c9844af619 110 //debugp->signal_set(0x01);
JimCarver 0:4e1d43dc608f 111 l = 0;
JimCarver 2:e4ae1d748311 112 green = !green;
JimCarver 0:4e1d43dc608f 113 }
JimCarver 0:4e1d43dc608f 114 tcount++;
JimCarver 0:4e1d43dc608f 115 }
JimCarver 3:0770c275e6e8 116
JimCarver 3:0770c275e6e8 117 void calibrate_thread(void const *argument) {
JimCarver 3:0770c275e6e8 118 while(true) {
JimCarver 3:0770c275e6e8 119 Thread::signal_wait(0x2);
JimCarver 4:ba1dbfb683fb 120 red = 0;
JimCarver 3:0770c275e6e8 121 compass.calibrate();
JimCarver 4:ba1dbfb683fb 122 red = 1;
JimCarver 3:0770c275e6e8 123 }
JimCarver 3:0770c275e6e8 124 }
JimCarver 3:0770c275e6e8 125
JimCarver 3:0770c275e6e8 126 void print_thread(void const *argument) {
JimCarver 3:0770c275e6e8 127 while (true) {
JimCarver 3:0770c275e6e8 128 // Signal flags that are reported as event are automatically cleared.
JimCarver 3:0770c275e6e8 129 Thread::signal_wait(0x1);
IrvingHdez 6:93c9844af619 130 debug_print();
IrvingHdez 6:93c9844af619 131 }
IrvingHdez 6:93c9844af619 132 }
IrvingHdez 6:93c9844af619 133
IrvingHdez 6:93c9844af619 134 void bandera_thread(void const *argument) {
IrvingHdez 6:93c9844af619 135 while (true) {
IrvingHdez 6:93c9844af619 136 // Signal flags that are reported as event are automatically cleared.
IrvingHdez 6:93c9844af619 137 Thread::signal_wait(0x3);
IrvingHdez 6:93c9844af619 138 //pc.printf("xm=%f\r\n", xm);
IrvingHdez 6:93c9844af619 139 if(!enable)
IrvingHdez 6:93c9844af619 140 {
IrvingHdez 6:93c9844af619 141 flag=!flag;
IrvingHdez 6:93c9844af619 142 wait(0.5);
IrvingHdez 6:93c9844af619 143 }
IrvingHdez 6:93c9844af619 144 else flag=flag;
IrvingHdez 6:93c9844af619 145
IrvingHdez 6:93c9844af619 146 if(flag==1)
IrvingHdez 6:93c9844af619 147 {
IrvingHdez 6:93c9844af619 148 led2=1;
IrvingHdez 6:93c9844af619 149 led=0;
IrvingHdez 6:93c9844af619 150 }
IrvingHdez 6:93c9844af619 151 else
IrvingHdez 6:93c9844af619 152 { // Apaga los motores
IrvingHdez 6:93c9844af619 153 led2=0; // Green off
IrvingHdez 6:93c9844af619 154 led=1; // Red on
IrvingHdez 6:93c9844af619 155 motor0 = lol;
IrvingHdez 6:93c9844af619 156 motor1 = lol;
IrvingHdez 6:93c9844af619 157 motor2 = lol;
IrvingHdez 6:93c9844af619 158 motor3 = lol;
IrvingHdez 6:93c9844af619 159 }
JimCarver 3:0770c275e6e8 160 }
JimCarver 3:0770c275e6e8 161 }
JimCarver 3:0770c275e6e8 162
IrvingHdez 6:93c9844af619 163 // Funciones de Membresia para control por logica difusa
IrvingHdez 6:93c9844af619 164 void controller_thread(void const *argument) {
IrvingHdez 6:93c9844af619 165 while (true) {
IrvingHdez 6:93c9844af619 166 // Signal flags that are reported as event are automatically cleared.
IrvingHdez 6:93c9844af619 167 Thread::signal_wait(0x5);
IrvingHdez 6:93c9844af619 168
IrvingHdez 6:93c9844af619 169 while (flag==1)
IrvingHdez 6:93c9844af619 170 {
IrvingHdez 6:93c9844af619 171 x = axis6.roll;
IrvingHdez 6:93c9844af619 172 xm=x/k;
IrvingHdez 6:93c9844af619 173
IrvingHdez 6:93c9844af619 174 int LE = 10; // Low Error
IrvingHdez 6:93c9844af619 175 int ME = 10; // Medium Error
IrvingHdez 6:93c9844af619 176 int HE = 30; // High Error
IrvingHdez 6:93c9844af619 177
IrvingHdez 6:93c9844af619 178 float m0 = -1.9;
IrvingHdez 6:93c9844af619 179 float m1 = -0.8;
IrvingHdez 6:93c9844af619 180
IrvingHdez 6:93c9844af619 181 if (x > -ME || x < ME)
IrvingHdez 6:93c9844af619 182 {
IrvingHdez 6:93c9844af619 183 motor3 = sp-xm*m1*c;
IrvingHdez 6:93c9844af619 184 wait(espera);
IrvingHdez 6:93c9844af619 185 motor0 = sp+xm*m1*c;
IrvingHdez 6:93c9844af619 186 wait(espera);
IrvingHdez 6:93c9844af619 187 scalerX = 0.6;
IrvingHdez 6:93c9844af619 188 pc.printf("vcxvcvcxvxcvxc");
IrvingHdez 6:93c9844af619 189 }
IrvingHdez 6:93c9844af619 190 if (x < -HE)
IrvingHdez 6:93c9844af619 191 {
IrvingHdez 6:93c9844af619 192 motor3 = idle;
IrvingHdez 6:93c9844af619 193 wait(espera);
IrvingHdez 6:93c9844af619 194 motor0 = full*scalerX*c;
IrvingHdez 6:93c9844af619 195 wait(espera);
IrvingHdez 6:93c9844af619 196 if (scalerX <=0.85)
IrvingHdez 6:93c9844af619 197 scalerX = scalerX+0.1;
IrvingHdez 6:93c9844af619 198 pc.printf("vcxvcvcxvxcvxc");
IrvingHdez 6:93c9844af619 199 }
IrvingHdez 6:93c9844af619 200 if (x > HE)
IrvingHdez 6:93c9844af619 201 {
IrvingHdez 6:93c9844af619 202 motor3 = full*scalerX*c;
IrvingHdez 6:93c9844af619 203 wait(espera);
IrvingHdez 6:93c9844af619 204 motor0 = idle;
IrvingHdez 6:93c9844af619 205 wait(espera);
IrvingHdez 6:93c9844af619 206 if (scalerX <=0.85)
IrvingHdez 6:93c9844af619 207 scalerX = scalerX+0.1;
IrvingHdez 6:93c9844af619 208 pc.printf("vcxvcvcxvxcvxc");
IrvingHdez 6:93c9844af619 209 }
IrvingHdez 6:93c9844af619 210
IrvingHdez 6:93c9844af619 211 i2++;
IrvingHdez 6:93c9844af619 212 if (i2 >= 15) currSample = xm;
IrvingHdez 6:93c9844af619 213 if(i2 >= 30)
IrvingHdez 6:93c9844af619 214 {
IrvingHdez 6:93c9844af619 215 difSample = abs(xm -currSample);
IrvingHdez 6:93c9844af619 216 i2 = 0;
IrvingHdez 6:93c9844af619 217 }
IrvingHdez 6:93c9844af619 218
IrvingHdez 6:93c9844af619 219 if (difSample >= 0.4) c = 0.1;
IrvingHdez 6:93c9844af619 220 if (difSample < 0.4) c = 1;
IrvingHdez 6:93c9844af619 221
IrvingHdez 6:93c9844af619 222 }
IrvingHdez 6:93c9844af619 223 }
IrvingHdez 6:93c9844af619 224 }
IrvingHdez 6:93c9844af619 225
IrvingHdez 6:93c9844af619 226 // Funciones de Membresia para control por logica difusa
IrvingHdez 6:93c9844af619 227 void controllerY_thread(void const *argument) {
IrvingHdez 6:93c9844af619 228 while (true) {
IrvingHdez 6:93c9844af619 229 // Signal flags that are reported as event are automatically cleared.
IrvingHdez 6:93c9844af619 230 Thread::signal_wait(0x6);
IrvingHdez 6:93c9844af619 231
IrvingHdez 6:93c9844af619 232 while (flag==1)
IrvingHdez 6:93c9844af619 233 {
IrvingHdez 6:93c9844af619 234 y = axis6.pitch;
IrvingHdez 6:93c9844af619 235 ym=y/k;
IrvingHdez 6:93c9844af619 236
IrvingHdez 6:93c9844af619 237 int LE = 10; // Low Error
IrvingHdez 6:93c9844af619 238 int ME = 17; // Medium Error
IrvingHdez 6:93c9844af619 239 int HE = 30; // High Error
IrvingHdez 6:93c9844af619 240
IrvingHdez 6:93c9844af619 241 float my = -0.8;
IrvingHdez 6:93c9844af619 242
IrvingHdez 6:93c9844af619 243 if (y > -ME && y < ME)
IrvingHdez 6:93c9844af619 244 {
IrvingHdez 6:93c9844af619 245 motor2 = 0.2;//sp-ym*my;
IrvingHdez 6:93c9844af619 246 wait(espera);
IrvingHdez 6:93c9844af619 247 motor1 = 0.2;//sp+ym*my;
IrvingHdez 6:93c9844af619 248 wait(espera);
IrvingHdez 6:93c9844af619 249 scalerY = 0.6;
IrvingHdez 6:93c9844af619 250 }
IrvingHdez 6:93c9844af619 251 if (y < -ME)
IrvingHdez 6:93c9844af619 252 {
IrvingHdez 6:93c9844af619 253 motor2 = idle;
IrvingHdez 6:93c9844af619 254 wait(espera);
IrvingHdez 6:93c9844af619 255 motor1 = full*scalerY;
IrvingHdez 6:93c9844af619 256 wait(espera);
IrvingHdez 6:93c9844af619 257 if (scalerY <=0.8)
IrvingHdez 6:93c9844af619 258 scalerY = scalerY+0.1;
IrvingHdez 6:93c9844af619 259 }
IrvingHdez 6:93c9844af619 260 if (y > ME)
IrvingHdez 6:93c9844af619 261 {
IrvingHdez 6:93c9844af619 262 motor2 = full*scalerY;
IrvingHdez 6:93c9844af619 263 wait(espera);
IrvingHdez 6:93c9844af619 264 motor1 = idle;
IrvingHdez 6:93c9844af619 265 wait(espera);
IrvingHdez 6:93c9844af619 266 if (scalerY <=0.8)
IrvingHdez 6:93c9844af619 267 scalerY = scalerY+0.1;
IrvingHdez 6:93c9844af619 268 }
IrvingHdez 6:93c9844af619 269 }
IrvingHdez 6:93c9844af619 270 }
IrvingHdez 6:93c9844af619 271 }
JimCarver 3:0770c275e6e8 272
JimCarver 3:0770c275e6e8 273 void lcd_thread(void const *argument) {
JimCarver 3:0770c275e6e8 274 while (true) {
JimCarver 3:0770c275e6e8 275 // Signal flags that are reported as event are automatically cleared.
JimCarver 3:0770c275e6e8 276 Thread::signal_wait(0x4);
IrvingHdez 6:93c9844af619 277 slcd.printf("%04d", axis6.roll); // print the heading (NED compass) to the LCD
JimCarver 3:0770c275e6e8 278 }
JimCarver 3:0770c275e6e8 279 }
JimCarver 3:0770c275e6e8 280
IrvingHdez 6:93c9844af619 281 int main()
IrvingHdez 6:93c9844af619 282 {
IrvingHdez 6:93c9844af619 283 pc.baud (115200);
JimCarver 0:4e1d43dc608f 284 slcd.clear();
JimCarver 0:4e1d43dc608f 285 tcount = 0;
JimCarver 2:e4ae1d748311 286 red = 1;
JimCarver 2:e4ae1d748311 287 green = 1;
JimCarver 2:e4ae1d748311 288 //cdebug = 1; // Set to 1 to disable eCompass in order to observe raw mag data.
JimCarver 0:4e1d43dc608f 289 compass_type = NED_COMPASS;
JimCarver 0:4e1d43dc608f 290 seconds = 0;
IrvingHdez 6:93c9844af619 291 //Thread t1(print_thread);
JimCarver 3:0770c275e6e8 292 Thread t2(calibrate_thread);
IrvingHdez 6:93c9844af619 293 //Thread t3(lcd_thread);
IrvingHdez 6:93c9844af619 294
IrvingHdez 6:93c9844af619 295 Thread t4(bandera_thread);
IrvingHdez 6:93c9844af619 296 Thread t5(controller_thread);
IrvingHdez 6:93c9844af619 297 Thread t6(controllerY_thread);
IrvingHdez 6:93c9844af619 298
IrvingHdez 6:93c9844af619 299 //debugp = &t1;
JimCarver 3:0770c275e6e8 300 calibrate = &t2;
IrvingHdez 6:93c9844af619 301 //lcdp = &t3;
IrvingHdez 6:93c9844af619 302
IrvingHdez 6:93c9844af619 303 bandera = &t4;
IrvingHdez 6:93c9844af619 304 controller = &t5;
IrvingHdez 6:93c9844af619 305 controllerY = &t6;
IrvingHdez 6:93c9844af619 306
JimCarver 3:0770c275e6e8 307 mag.enable();
JimCarver 3:0770c275e6e8 308 acc.enable();
IrvingHdez 6:93c9844af619 309
JimCarver 3:0770c275e6e8 310 mag.getAxis(mag_raw); // flush the magnetmeter
JimCarver 2:e4ae1d748311 311 RtosTimer compass_timer(compass_thread, osTimerPeriodic);
JimCarver 3:0770c275e6e8 312
JimCarver 3:0770c275e6e8 313 /*
JimCarver 3:0770c275e6e8 314 * Thread Priorities
JimCarver 3:0770c275e6e8 315 * Compass Thread, High Priority
JimCarver 3:0770c275e6e8 316 * Compass calibration, Above Normal
JimCarver 3:0770c275e6e8 317 * LED Update, Normal
JimCarver 3:0770c275e6e8 318 * printf to console, Below Normal
JimCarver 3:0770c275e6e8 319 * main(), Normal
JimCarver 3:0770c275e6e8 320 */
JimCarver 3:0770c275e6e8 321
JimCarver 3:0770c275e6e8 322 debugp->set_priority(osPriorityBelowNormal);
JimCarver 3:0770c275e6e8 323 lcdp->set_priority(osPriorityLow);
IrvingHdez 6:93c9844af619 324 calibrate->set_priority(osPriorityAboveNormal);
IrvingHdez 6:93c9844af619 325 bandera->set_priority(osPriorityHigh);
IrvingHdez 6:93c9844af619 326 controller->set_priority(osPriorityAboveNormal);
IrvingHdez 6:93c9844af619 327 controllerY->set_priority(osPriorityAboveNormal);
IrvingHdez 6:93c9844af619 328
IrvingHdez 6:93c9844af619 329 compass_timer.start(10); // Run the Compass every 20ms
JimCarver 4:ba1dbfb683fb 330
JimCarver 2:e4ae1d748311 331 Thread::wait(osWaitForever);
JimCarver 0:4e1d43dc608f 332 }