Patricio Silva / Mbed 2 deprecated c3auto_c

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@1:6e374a9b1bf5, 2019-10-29 (annotated)

Committer:
elpatosilva
Date:
Tue Oct 29 22:44:00 2019 +0000
Revision:
1:6e374a9b1bf5
Parent:
0:749a196e6ebe
Child:
2:5bf1990918aa
Sensores funcionando

Who changed what in which revision?

UserRevisionLine numberNew contents of line
elpatosilva 0:749a196e6ebe 1 #include "mbed.h"
elpatosilva 0:749a196e6ebe 2 #include "lib/Pserial.h"
elpatosilva 0:749a196e6ebe 3 #include "lib/Servo.h"
elpatosilva 0:749a196e6ebe 4 #include "lib/Sensor.h"
elpatosilva 0:749a196e6ebe 5
elpatosilva 0:749a196e6ebe 6 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
elpatosilva 0:749a196e6ebe 7 // Conf IO
elpatosilva 0:749a196e6ebe 8 DigitalOut led1(LED1); // HeartBeat
elpatosilva 0:749a196e6ebe 9 PwmOut cg90(PTD1);
elpatosilva 0:749a196e6ebe 10 AnalogIn opticLeftIn(PTB2);
elpatosilva 0:749a196e6ebe 11 AnalogIn opticRigthIn(PTB3);
elpatosilva 0:749a196e6ebe 12 DigitalOut ultrasonicTrigger(PTA1);
elpatosilva 1:6e374a9b1bf5 13 DigitalIn ultrasonicEcho(PTC0);
elpatosilva 1:6e374a9b1bf5 14 DigitalIn speedLeft(PTC7);
elpatosilva 1:6e374a9b1bf5 15 DigitalIn speedRigth(PTC5);
elpatosilva 1:6e374a9b1bf5 16 InterruptIn speedLeftInterrupt(PTA2);
elpatosilva 1:6e374a9b1bf5 17 InterruptIn speedRigthInterrupt(PTB23);
elpatosilva 1:6e374a9b1bf5 18
elpatosilva 0:749a196e6ebe 19
elpatosilva 0:749a196e6ebe 20 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
elpatosilva 0:749a196e6ebe 21 // Timers y relacionados
elpatosilva 0:749a196e6ebe 22 Ticker timer100ms;
elpatosilva 0:749a196e6ebe 23 LowPowerTimer timer1ms;
elpatosilva 0:749a196e6ebe 24 uint32_t maxUsTicker;
elpatosilva 0:749a196e6ebe 25
elpatosilva 0:749a196e6ebe 26 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
elpatosilva 0:749a196e6ebe 27 // Prototipos locales
elpatosilva 0:749a196e6ebe 28 void heartBeat();
elpatosilva 0:749a196e6ebe 29 void heartBeatSetFlag();
elpatosilva 0:749a196e6ebe 30 void serialReceivePc();
elpatosilva 0:749a196e6ebe 31 void cmdExec();
elpatosilva 0:749a196e6ebe 32 void sendOpticData();
elpatosilva 0:749a196e6ebe 33 void sendUltrasonicData();
elpatosilva 0:749a196e6ebe 34 void sendSpeedData();
elpatosilva 1:6e374a9b1bf5 35 void riseSpeedLeftInterrupt();
elpatosilva 1:6e374a9b1bf5 36 void riseSpeedRigthInterrupt();
elpatosilva 0:749a196e6ebe 37
elpatosilva 0:749a196e6ebe 38
elpatosilva 0:749a196e6ebe 39 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
elpatosilva 0:749a196e6ebe 40 // Constantes locales
elpatosilva 0:749a196e6ebe 41
elpatosilva 0:749a196e6ebe 42 // Version del FW
elpatosilva 1:6e374a9b1bf5 43 const uint8_t FIRMWARE_VERSION[] = "V0.10";
elpatosilva 0:749a196e6ebe 44
elpatosilva 0:749a196e6ebe 45 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
elpatosilva 0:749a196e6ebe 46 // Definiciones locales
elpatosilva 0:749a196e6ebe 47
elpatosilva 0:749a196e6ebe 48 // Estructura para uso auxiliar. Arduino es little endian
elpatosilva 0:749a196e6ebe 49 typedef union{
elpatosilva 0:749a196e6ebe 50 uint8_t u8[4];
elpatosilva 0:749a196e6ebe 51 uint16_t u16[2];
elpatosilva 0:749a196e6ebe 52 int8_t i8[2];
elpatosilva 0:749a196e6ebe 53 uint32_t u32;
elpatosilva 0:749a196e6ebe 54 int32_t i32;
elpatosilva 0:749a196e6ebe 55 }_work;
elpatosilva 0:749a196e6ebe 56
elpatosilva 0:749a196e6ebe 57
elpatosilva 0:749a196e6ebe 58 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
elpatosilva 0:749a196e6ebe 59 // Variables y objetos Locales
elpatosilva 0:749a196e6ebe 60 Serial pc(USBTX, USBRX, 19200);
elpatosilva 0:749a196e6ebe 61 _Pserial pserial;
elpatosilva 0:749a196e6ebe 62
elpatosilva 0:749a196e6ebe 63 _Sensor rigthOpticSensor;
elpatosilva 0:749a196e6ebe 64 _Sensor leftOpticSensor;
elpatosilva 0:749a196e6ebe 65 _Sensor ultrasonicSensor;
elpatosilva 0:749a196e6ebe 66 _Sensor rigthSpeedSensor;
elpatosilva 0:749a196e6ebe 67 _Sensor leftSpeedSensor;
elpatosilva 0:749a196e6ebe 68
elpatosilva 0:749a196e6ebe 69 _Servo servo;
elpatosilva 0:749a196e6ebe 70
elpatosilva 0:749a196e6ebe 71 //_Motor motorLeft
elpatosilva 0:749a196e6ebe 72 //_Motor motorRigth
elpatosilva 0:749a196e6ebe 73
elpatosilva 0:749a196e6ebe 74 /*
elpatosilva 0:749a196e6ebe 75 * Control de los sensores, tiempos de muestreo
elpatosilva 0:749a196e6ebe 76 */
elpatosilva 0:749a196e6ebe 77 uint16_t opticTimeInterval;
elpatosilva 0:749a196e6ebe 78 uint16_t ultrasonicTimeInterval;
elpatosilva 0:749a196e6ebe 79 uint16_t motorSpeedTimeInterval;
elpatosilva 0:749a196e6ebe 80
elpatosilva 0:749a196e6ebe 81 // Auxiliar
elpatosilva 0:749a196e6ebe 82 _work work;
elpatosilva 0:749a196e6ebe 83
elpatosilva 0:749a196e6ebe 84 // Flag del heartBeat()
elpatosilva 0:749a196e6ebe 85 bool heartBeatFlag;
elpatosilva 0:749a196e6ebe 86
elpatosilva 0:749a196e6ebe 87 extern _txbuffer TX;
elpatosilva 0:749a196e6ebe 88 extern _rxbuffer RX;
elpatosilva 0:749a196e6ebe 89 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
elpatosilva 0:749a196e6ebe 90 ///////////////// MAIN
elpatosilva 0:749a196e6ebe 91
elpatosilva 0:749a196e6ebe 92 int main(){
elpatosilva 0:749a196e6ebe 93 maxUsTicker = us_ticker_read();
elpatosilva 0:749a196e6ebe 94 timer100ms.attach(&heartBeatSetFlag, 0.1);
elpatosilva 0:749a196e6ebe 95 heartBeatFlag = false;
elpatosilva 0:749a196e6ebe 96 timer1ms.start();
elpatosilva 0:749a196e6ebe 97 setup(&rigthOpticSensor, &opticRigthIn);
elpatosilva 0:749a196e6ebe 98 setup(&leftOpticSensor, &opticLeftIn);
elpatosilva 0:749a196e6ebe 99 setup(&ultrasonicSensor, &ultrasonicTrigger, &ultrasonicEcho);
elpatosilva 1:6e374a9b1bf5 100
elpatosilva 1:6e374a9b1bf5 101 setup(&rigthSpeedSensor, &speedRigth, 32);
elpatosilva 1:6e374a9b1bf5 102 setup(&leftSpeedSensor, &speedLeft, 32);
elpatosilva 1:6e374a9b1bf5 103 speedLeftInterrupt.rise(&riseSpeedLeftInterrupt);
elpatosilva 1:6e374a9b1bf5 104 speedRigthInterrupt.rise(&riseSpeedRigthInterrupt);
elpatosilva 1:6e374a9b1bf5 105
elpatosilva 0:749a196e6ebe 106
elpatosilva 0:749a196e6ebe 107 opticTimeInterval = 0;
elpatosilva 0:749a196e6ebe 108 ultrasonicTimeInterval = 0;
elpatosilva 0:749a196e6ebe 109 motorSpeedTimeInterval = 0;
elpatosilva 0:749a196e6ebe 110
elpatosilva 0:749a196e6ebe 111 pc.attach(&serialReceivePc, Serial::RxIrq);
elpatosilva 0:749a196e6ebe 112 serialSetup(&pserial, &pc);
elpatosilva 0:749a196e6ebe 113
elpatosilva 0:749a196e6ebe 114 setup(&servo, &cg90);
elpatosilva 0:749a196e6ebe 115 while(1){
elpatosilva 0:749a196e6ebe 116 // heartBeat
elpatosilva 0:749a196e6ebe 117 if(heartBeatFlag) heartBeat();
elpatosilva 0:749a196e6ebe 118
elpatosilva 0:749a196e6ebe 119 // Overflow de us_ticker_read(), reseteo todo lo que utiliza este dato
elpatosilva 0:749a196e6ebe 120 if(maxUsTicker > us_ticker_read()){
elpatosilva 0:749a196e6ebe 121 maxUsTicker = us_ticker_read();
elpatosilva 0:749a196e6ebe 122 serialCmdClean(&pserial);
elpatosilva 0:749a196e6ebe 123 }
elpatosilva 0:749a196e6ebe 124
elpatosilva 0:749a196e6ebe 125 // envio datos que pueda haber en el buffer de salida
elpatosilva 0:749a196e6ebe 126 serialSubmit(&pserial);
elpatosilva 0:749a196e6ebe 127
elpatosilva 0:749a196e6ebe 128 // proceso comando presente en el buffer
elpatosilva 0:749a196e6ebe 129 if(pserial.RX.cmd)
elpatosilva 0:749a196e6ebe 130 cmdExec();
elpatosilva 0:749a196e6ebe 131
elpatosilva 0:749a196e6ebe 132 // Busco si hay un nuevo header en el buffer
elpatosilva 0:749a196e6ebe 133 serialGetFrame(&pserial);
elpatosilva 0:749a196e6ebe 134
elpatosilva 0:749a196e6ebe 135 // Ejecuto tareas de sensores
elpatosilva 0:749a196e6ebe 136 if(opticTimeInterval){
elpatosilva 0:749a196e6ebe 137 sendOpticData();
elpatosilva 0:749a196e6ebe 138 }
elpatosilva 0:749a196e6ebe 139
elpatosilva 0:749a196e6ebe 140 if(ultrasonicTimeInterval){
elpatosilva 0:749a196e6ebe 141 sendUltrasonicData();
elpatosilva 0:749a196e6ebe 142 }
elpatosilva 0:749a196e6ebe 143
elpatosilva 0:749a196e6ebe 144 if(motorSpeedTimeInterval){
elpatosilva 0:749a196e6ebe 145 sendSpeedData();
elpatosilva 0:749a196e6ebe 146 }
elpatosilva 0:749a196e6ebe 147 }
elpatosilva 0:749a196e6ebe 148 }
elpatosilva 0:749a196e6ebe 149
elpatosilva 0:749a196e6ebe 150
elpatosilva 0:749a196e6ebe 151
elpatosilva 0:749a196e6ebe 152 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
elpatosilva 0:749a196e6ebe 153 /// Funciones locales
elpatosilva 0:749a196e6ebe 154
elpatosilva 0:749a196e6ebe 155 /*
elpatosilva 0:749a196e6ebe 156 * Interrupcion, datos en el puerto serie con la PC
elpatosilva 0:749a196e6ebe 157 */
elpatosilva 0:749a196e6ebe 158 void serialReceivePc(){
elpatosilva 0:749a196e6ebe 159 serialReceive(&pserial);
elpatosilva 0:749a196e6ebe 160 }
elpatosilva 0:749a196e6ebe 161
elpatosilva 0:749a196e6ebe 162
elpatosilva 0:749a196e6ebe 163 /*
elpatosilva 0:749a196e6ebe 164 * Señal de activo, dos parpadeos de 100ms cada dos segundos
elpatosilva 1:6e374a9b1bf5 165 * El timer me fueza a pasar pr el wile(1) del main, sino no tiene mucho sentido un heartbeat por interrupciones
elpatosilva 0:749a196e6ebe 166 */
elpatosilva 0:749a196e6ebe 167 void heartBeatSetFlag(){
elpatosilva 0:749a196e6ebe 168 heartBeatFlag = true;
elpatosilva 0:749a196e6ebe 169 }
elpatosilva 0:749a196e6ebe 170
elpatosilva 1:6e374a9b1bf5 171 /*
elpatosilva 1:6e374a9b1bf5 172 * Interrupciones de los pin digitales de los sensores
elpatosilva 1:6e374a9b1bf5 173 */
elpatosilva 1:6e374a9b1bf5 174 void riseSpeedLeftInterrupt(){
elpatosilva 1:6e374a9b1bf5 175 notify(&leftSpeedSensor);
elpatosilva 1:6e374a9b1bf5 176 }
elpatosilva 1:6e374a9b1bf5 177 void riseSpeedRigthInterrupt(){
elpatosilva 1:6e374a9b1bf5 178 notify(&rigthSpeedSensor);
elpatosilva 1:6e374a9b1bf5 179 }
elpatosilva 1:6e374a9b1bf5 180
elpatosilva 1:6e374a9b1bf5 181
elpatosilva 0:749a196e6ebe 182 void heartBeat(){
elpatosilva 0:749a196e6ebe 183 static int count = 0;
elpatosilva 0:749a196e6ebe 184 switch (count++){
elpatosilva 0:749a196e6ebe 185 case 0:
elpatosilva 0:749a196e6ebe 186 case 2:
elpatosilva 0:749a196e6ebe 187 led1 = 0;
elpatosilva 0:749a196e6ebe 188 break;
elpatosilva 0:749a196e6ebe 189 case 1:
elpatosilva 0:749a196e6ebe 190 case 3:
elpatosilva 0:749a196e6ebe 191 led1 = 1;
elpatosilva 0:749a196e6ebe 192 break;
elpatosilva 0:749a196e6ebe 193 case 20:
elpatosilva 0:749a196e6ebe 194 count = 0;
elpatosilva 0:749a196e6ebe 195 break;
elpatosilva 0:749a196e6ebe 196 }
elpatosilva 0:749a196e6ebe 197 heartBeatFlag = false;
elpatosilva 0:749a196e6ebe 198 }
elpatosilva 0:749a196e6ebe 199
elpatosilva 0:749a196e6ebe 200
elpatosilva 0:749a196e6ebe 201
elpatosilva 0:749a196e6ebe 202 /*
elpatosilva 0:749a196e6ebe 203 * Envio datos del sensor optico a la velocidad especificada
elpatosilva 0:749a196e6ebe 204 */
elpatosilva 0:749a196e6ebe 205 void sendOpticData(){
elpatosilva 0:749a196e6ebe 206 static unsigned long stime = timer1ms.read_ms();
elpatosilva 0:749a196e6ebe 207 if(timer1ms.read_ms() > stime+(opticTimeInterval*10)){
elpatosilva 0:749a196e6ebe 208 work.u16[0] = opticTimeInterval;
elpatosilva 0:749a196e6ebe 209 serialEnqueueHeader(&pserial, 12);
elpatosilva 0:749a196e6ebe 210 serialEnqueueData(&pserial, 0xA0); // Comando
elpatosilva 0:749a196e6ebe 211 serialEnqueueData(&pserial, work.u8[0]); // Intervalo actual (2 bytes)
elpatosilva 0:749a196e6ebe 212 serialEnqueueData(&pserial, work.u8[1]);
elpatosilva 0:749a196e6ebe 213 serialEnqueueData(&pserial, 0); // Sensor exterior izquierdo (2 bytes)
elpatosilva 0:749a196e6ebe 214 serialEnqueueData(&pserial, 0);
elpatosilva 0:749a196e6ebe 215 work.u16[0] = getValue(&leftOpticSensor);
elpatosilva 0:749a196e6ebe 216 work.u16[1] = getValue(&rigthOpticSensor);
elpatosilva 0:749a196e6ebe 217 serialEnqueueData(&pserial, work.u8[0]); // Sensor interior izquierdo (2 bytes)
elpatosilva 0:749a196e6ebe 218 serialEnqueueData(&pserial, work.u8[1]);
elpatosilva 0:749a196e6ebe 219 serialEnqueueData(&pserial, work.u8[2]); // Sensor interior drecho (2 bytes)
elpatosilva 0:749a196e6ebe 220 serialEnqueueData(&pserial, work.u8[3]);
elpatosilva 0:749a196e6ebe 221 serialEnqueueData(&pserial, 0); // Sensor exterior derecho (2 bytes)
elpatosilva 0:749a196e6ebe 222 serialEnqueueData(&pserial, 0);
elpatosilva 0:749a196e6ebe 223 serialEnqueueChksum(&pserial);
elpatosilva 0:749a196e6ebe 224 stime = timer1ms.read_ms();
elpatosilva 0:749a196e6ebe 225 }
elpatosilva 0:749a196e6ebe 226 }
elpatosilva 0:749a196e6ebe 227
elpatosilva 0:749a196e6ebe 228
elpatosilva 0:749a196e6ebe 229
elpatosilva 0:749a196e6ebe 230 /*
elpatosilva 0:749a196e6ebe 231 * Envio datos del sensor ultrasonico a la velocidad especificada
elpatosilva 0:749a196e6ebe 232 * Los tiempos de llamada deberian estar espaciados al menos 50ms
elpatosilva 0:749a196e6ebe 233 */
elpatosilva 0:749a196e6ebe 234 void sendUltrasonicData(){
elpatosilva 0:749a196e6ebe 235 static uint32_t stime = timer1ms.read_ms();
elpatosilva 1:6e374a9b1bf5 236
elpatosilva 1:6e374a9b1bf5 237 // Utilizando PING
elpatosilva 1:6e374a9b1bf5 238 /*if(timer1ms.read_ms() > stime+(ultrasonicTimeInterval*10)){
elpatosilva 1:6e374a9b1bf5 239 pserial.port->putc(0x1);
elpatosilva 1:6e374a9b1bf5 240 uint16_t distance = ping(&ultrasonicSensor, 1000);
elpatosilva 1:6e374a9b1bf5 241 pserial.port->putc(0xff);
elpatosilva 1:6e374a9b1bf5 242 work.u16[0] = distance;
elpatosilva 1:6e374a9b1bf5 243 pserial.port->putc(work.u8[1]);
elpatosilva 1:6e374a9b1bf5 244 pserial.port->putc(work.u8[0]);
elpatosilva 1:6e374a9b1bf5 245 serialEnqueueHeader(&pserial, 6);
elpatosilva 1:6e374a9b1bf5 246 serialEnqueueData(&pserial, 0xA1); // Comando
elpatosilva 1:6e374a9b1bf5 247 work.u16[0] = ultrasonicTimeInterval;
elpatosilva 1:6e374a9b1bf5 248 serialEnqueueData(&pserial, work.u8[0]); // Intervalo actual (2 bytes)
elpatosilva 1:6e374a9b1bf5 249 serialEnqueueData(&pserial, work.u8[1]);
elpatosilva 1:6e374a9b1bf5 250 work.u16[0] = distance;
elpatosilva 1:6e374a9b1bf5 251 serialEnqueueData(&pserial, work.u8[0]); // Distancia medida (2 bytes)
elpatosilva 1:6e374a9b1bf5 252 serialEnqueueData(&pserial, work.u8[1]);
elpatosilva 1:6e374a9b1bf5 253 serialEnqueueChksum(&pserial);
elpatosilva 1:6e374a9b1bf5 254 stime = timer1ms.read_ms();
elpatosilva 1:6e374a9b1bf5 255 }*/
elpatosilva 1:6e374a9b1bf5 256
elpatosilva 1:6e374a9b1bf5 257 // Utilizando init y isReady
elpatosilva 1:6e374a9b1bf5 258 if(ultrasonicSensor.stage){
elpatosilva 0:749a196e6ebe 259 uint16_t distance = isReady(&ultrasonicSensor);
elpatosilva 1:6e374a9b1bf5 260 if(distance > 0){ // Ya tengo la distancia, la envio
elpatosilva 0:749a196e6ebe 261 work.u16[0] = ultrasonicTimeInterval;
elpatosilva 1:6e374a9b1bf5 262 serialEnqueueHeader(&pserial, 6);
elpatosilva 0:749a196e6ebe 263 serialEnqueueData(&pserial, 0xA1); // Comando
elpatosilva 0:749a196e6ebe 264 serialEnqueueData(&pserial, work.u8[0]); // Intervalo actual (2 bytes)
elpatosilva 0:749a196e6ebe 265 serialEnqueueData(&pserial, work.u8[1]);
elpatosilva 0:749a196e6ebe 266
elpatosilva 1:6e374a9b1bf5 267 work.u16[0] = distance;
elpatosilva 0:749a196e6ebe 268 serialEnqueueData(&pserial, work.u8[0]); // Distancia medida (2 bytes)
elpatosilva 0:749a196e6ebe 269 serialEnqueueData(&pserial, work.u8[1]);
elpatosilva 0:749a196e6ebe 270
elpatosilva 0:749a196e6ebe 271 serialEnqueueChksum(&pserial);
elpatosilva 0:749a196e6ebe 272 }
elpatosilva 0:749a196e6ebe 273 }
elpatosilva 0:749a196e6ebe 274
elpatosilva 1:6e374a9b1bf5 275 if(timer1ms.read_ms() > stime+(ultrasonicTimeInterval*10)){
elpatosilva 0:749a196e6ebe 276 reset(&ultrasonicSensor);
elpatosilva 1:6e374a9b1bf5 277 init(&ultrasonicSensor);
elpatosilva 0:749a196e6ebe 278 stime = timer1ms.read_ms();
elpatosilva 0:749a196e6ebe 279 }
elpatosilva 0:749a196e6ebe 280 }
elpatosilva 0:749a196e6ebe 281
elpatosilva 0:749a196e6ebe 282
elpatosilva 0:749a196e6ebe 283
elpatosilva 1:6e374a9b1bf5 284 void sendSpeedData(){
elpatosilva 1:6e374a9b1bf5 285 static unsigned long stime = timer1ms.read_ms();
elpatosilva 1:6e374a9b1bf5 286 if(timer1ms.read_ms() > stime+(motorSpeedTimeInterval*10)){
elpatosilva 1:6e374a9b1bf5 287 work.u16[0] = motorSpeedTimeInterval;
elpatosilva 1:6e374a9b1bf5 288 serialEnqueueHeader(&pserial, 8);
elpatosilva 1:6e374a9b1bf5 289 serialEnqueueData(&pserial, 0xA4); // Comando
elpatosilva 1:6e374a9b1bf5 290 serialEnqueueData(&pserial, work.u8[0]); // Intervalo actual (2 bytes)
elpatosilva 1:6e374a9b1bf5 291 serialEnqueueData(&pserial, work.u8[1]);
elpatosilva 1:6e374a9b1bf5 292 work.u16[0] = getRpm(&leftSpeedSensor);
elpatosilva 1:6e374a9b1bf5 293 work.u16[1] = getRpm(&rigthSpeedSensor);
elpatosilva 1:6e374a9b1bf5 294 serialEnqueueData(&pserial, work.u8[0]); // Sensor izquierdo (2 bytes)
elpatosilva 1:6e374a9b1bf5 295 serialEnqueueData(&pserial, work.u8[1]);
elpatosilva 1:6e374a9b1bf5 296 serialEnqueueData(&pserial, work.u8[2]); // Sensor drecho (2 bytes)
elpatosilva 1:6e374a9b1bf5 297 serialEnqueueData(&pserial, work.u8[3]);
elpatosilva 1:6e374a9b1bf5 298 serialEnqueueChksum(&pserial);
elpatosilva 1:6e374a9b1bf5 299 stime = timer1ms.read_ms();
elpatosilva 1:6e374a9b1bf5 300 }
elpatosilva 1:6e374a9b1bf5 301 }
elpatosilva 0:749a196e6ebe 302
elpatosilva 0:749a196e6ebe 303 /*
elpatosilva 0:749a196e6ebe 304 * Se ha recibido un comando por puerto serie, lo interpreto y respondo
elpatosilva 0:749a196e6ebe 305 * Si no hay lugar en el buffer lo espero TXCMDTIME us
elpatosilva 0:749a196e6ebe 306 */
elpatosilva 0:749a196e6ebe 307 void cmdExec(){
elpatosilva 0:749a196e6ebe 308 bool ok = false;
elpatosilva 0:749a196e6ebe 309 switch(pserial.RX.buf[pserial.RX.cmdIndex]){
elpatosilva 0:749a196e6ebe 310 // ping
elpatosilva 0:749a196e6ebe 311 case 0xF0:{
elpatosilva 0:749a196e6ebe 312 ok = serialEnqueueHeader(&pserial, 3) && serialEnqueueData(&pserial, 0xF0) && serialEnqueueData(&pserial, 0x0D) && serialEnqueueChksum(&pserial);
elpatosilva 0:749a196e6ebe 313 break;
elpatosilva 0:749a196e6ebe 314 }
elpatosilva 0:749a196e6ebe 315 // Version de Firmware
elpatosilva 0:749a196e6ebe 316 case 0xF1:{
elpatosilva 0:749a196e6ebe 317 ok = serialEnqueueHeader(&pserial, 13+sizeof(FIRMWARE_VERSION)+sizeof(__DATE__)+sizeof(__TIME__)) && serialEnqueueData(&pserial, 0xF1);
elpatosilva 0:749a196e6ebe 318 for (int i = 0 ; i < sizeof(FIRMWARE_VERSION) ; i++)
elpatosilva 0:749a196e6ebe 319 ok = ok && serialEnqueueData(&pserial, FIRMWARE_VERSION[i]);
elpatosilva 0:749a196e6ebe 320 ok = ok && serialEnqueueData(&pserial, '_') && serialEnqueueData(&pserial, 'b') && serialEnqueueData(&pserial, 'u') && serialEnqueueData(&pserial, 'i') && serialEnqueueData(&pserial, 'l') && serialEnqueueData(&pserial, 'd') && serialEnqueueData(&pserial, '_');
elpatosilva 0:749a196e6ebe 321 for (int i = 0 ; i < sizeof(__DATE__) ; i++)
elpatosilva 0:749a196e6ebe 322 ok = ok && serialEnqueueData(&pserial, __DATE__[i]);
elpatosilva 0:749a196e6ebe 323 ok = ok && serialEnqueueData(&pserial, '_') && serialEnqueueData(&pserial, 'a') && serialEnqueueData(&pserial, 't') && serialEnqueueData(&pserial, '_');
elpatosilva 0:749a196e6ebe 324 for (int i = 0 ; i < sizeof(__TIME__) ; i++)
elpatosilva 0:749a196e6ebe 325 ok = ok && serialEnqueueData(&pserial, __TIME__[i]);
elpatosilva 0:749a196e6ebe 326 ok = ok && serialEnqueueChksum(&pserial);
elpatosilva 0:749a196e6ebe 327 break;
elpatosilva 0:749a196e6ebe 328 }
elpatosilva 0:749a196e6ebe 329 // Sensores Opticos
elpatosilva 0:749a196e6ebe 330 case 0xA0:{
elpatosilva 0:749a196e6ebe 331 work.u8[0] = pserial.RX.buf[pserial.RX.cmdIndex+1];
elpatosilva 0:749a196e6ebe 332 work.u8[1] = pserial.RX.buf[pserial.RX.cmdIndex+2];
elpatosilva 0:749a196e6ebe 333 opticTimeInterval = work.u16[0];
elpatosilva 0:749a196e6ebe 334 ok = true;
elpatosilva 0:749a196e6ebe 335 break;
elpatosilva 0:749a196e6ebe 336 }
elpatosilva 0:749a196e6ebe 337 // Sensor Ultrasonico
elpatosilva 0:749a196e6ebe 338 case 0xA1:{
elpatosilva 0:749a196e6ebe 339 work.u8[0] = pserial.RX.buf[pserial.RX.cmdIndex+1];
elpatosilva 0:749a196e6ebe 340 work.u8[1] = pserial.RX.buf[pserial.RX.cmdIndex+2];
elpatosilva 0:749a196e6ebe 341 ultrasonicTimeInterval = work.u16[0];
elpatosilva 0:749a196e6ebe 342 ok = true;
elpatosilva 0:749a196e6ebe 343 break;
elpatosilva 0:749a196e6ebe 344 }
elpatosilva 0:749a196e6ebe 345 // Sensores de horquilla
elpatosilva 0:749a196e6ebe 346 case 0xA4:{
elpatosilva 0:749a196e6ebe 347 work.u8[0] = pserial.RX.buf[pserial.RX.cmdIndex+1];
elpatosilva 0:749a196e6ebe 348 work.u8[1] = pserial.RX.buf[pserial.RX.cmdIndex+2];
elpatosilva 0:749a196e6ebe 349 motorSpeedTimeInterval = work.u16[0];
elpatosilva 0:749a196e6ebe 350 ok = true;
elpatosilva 0:749a196e6ebe 351 break;
elpatosilva 0:749a196e6ebe 352 }
elpatosilva 0:749a196e6ebe 353 default:{
elpatosilva 0:749a196e6ebe 354 ok = serialEnqueueHeader(&pserial, 2) && serialEnqueueData(&pserial, 0xFF) && serialEnqueueChksum(&pserial);
elpatosilva 0:749a196e6ebe 355 }
elpatosilva 0:749a196e6ebe 356 }
elpatosilva 0:749a196e6ebe 357 if (ok || us_ticker_read() > pserial.TX.cmdTime + TXCMDTIME){
elpatosilva 0:749a196e6ebe 358 pserial.RX.cmd = false;
elpatosilva 0:749a196e6ebe 359 pserial.RX.indexR = pserial.RX.cmdIndex+pserial.RX.payloadSize;
elpatosilva 0:749a196e6ebe 360 pserial.TX.cmdTime = 0;
elpatosilva 0:749a196e6ebe 361 }
elpatosilva 0:749a196e6ebe 362 }