Final, cambios pendientes
Dependencies: MAG3110 MMA8451Q SLCD eCompass_Lib mbed-rtos mbed
Fork of KL46_eCompass_TiltCompensed_Acel-Mag by
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?
User | Revision | Line number | New 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 | } |