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@2:5bf1990918aa, 2019-11-05 (annotated)

Committer:
elpatosilva
Date:
Tue Nov 05 20:07:52 2019 +0000
Revision:
2:5bf1990918aa
Parent:
1:6e374a9b1bf5 
Funciones de auto.Antes de modificaciones para seguir el ultrasoinico y buscar los colores

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 2:5bf1990918aa 5 #include "lib/Motor.h"
elpatosilva 2:5bf1990918aa 6
elpatosilva 2:5bf1990918aa 7 #define WHEEL_ENCODER_SLOTS 32
elpatosilva 2:5bf1990918aa 8 #define WHEEL_PERIM 105
elpatosilva 2:5bf1990918aa 9
elpatosilva 2:5bf1990918aa 10 // Version del FW
elpatosilva 2:5bf1990918aa 11 const uint8_t FIRMWARE_VERSION[] = "V0.13";
elpatosilva 2:5bf1990918aa 12
elpatosilva 0:749a196e6ebe 13
elpatosilva 0:749a196e6ebe 14 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
elpatosilva 0:749a196e6ebe 15 // Conf IO
elpatosilva 2:5bf1990918aa 16 DigitalOut led1(PTB21); // HeartBeat - LedBlue
elpatosilva 2:5bf1990918aa 17
elpatosilva 2:5bf1990918aa 18 PwmOut servoPwm(PTA2);
elpatosilva 2:5bf1990918aa 19 DigitalOut servoDigitalPin(PTC12); // No logro estabilidad en PWM a periodos tan largos como requiere el servo, lo emulo con este pin
elpatosilva 2:5bf1990918aa 20
elpatosilva 0:749a196e6ebe 21 AnalogIn opticLeftIn(PTB2);
elpatosilva 0:749a196e6ebe 22 AnalogIn opticRigthIn(PTB3);
elpatosilva 2:5bf1990918aa 23
elpatosilva 2:5bf1990918aa 24 DigitalOut ultrasonicTrigger(PTB18);
elpatosilva 1:6e374a9b1bf5 25 DigitalIn ultrasonicEcho(PTC0);
elpatosilva 2:5bf1990918aa 26
elpatosilva 1:6e374a9b1bf5 27 DigitalIn speedLeft(PTC7);
elpatosilva 1:6e374a9b1bf5 28 DigitalIn speedRigth(PTC5);
elpatosilva 2:5bf1990918aa 29 InterruptIn speedLeftInterrupt(PTC7); // Interrupciones para los pines de las horquillas
elpatosilva 2:5bf1990918aa 30 InterruptIn speedRigthInterrupt(PTC5);
elpatosilva 2:5bf1990918aa 31
elpatosilva 2:5bf1990918aa 32 DigitalOut motorLeftA(PTB19);
elpatosilva 2:5bf1990918aa 33 DigitalOut motorLeftB(PTC1);
elpatosilva 2:5bf1990918aa 34 PwmOut motorLeftPwm(PTC2);
elpatosilva 2:5bf1990918aa 35 DigitalOut motorRigthA(PTC8);
elpatosilva 2:5bf1990918aa 36 DigitalOut motorRigthB(PTC9);
elpatosilva 2:5bf1990918aa 37 PwmOut motorRigthPwm(PTC3);
elpatosilva 2:5bf1990918aa 38
elpatosilva 2:5bf1990918aa 39 /* Conf IO Blue Pill
elpatosilva 2:5bf1990918aa 40 DigitalOut led1(LED1); // HeartBeat
elpatosilva 2:5bf1990918aa 41 PwmOut cg90(PB_5);
elpatosilva 2:5bf1990918aa 42 AnalogIn opticLeftIn(PA_0);
elpatosilva 2:5bf1990918aa 43 AnalogIn opticRigthIn(PA_1);
elpatosilva 2:5bf1990918aa 44 DigitalOut ultrasonicTrigger(PB_4);
elpatosilva 2:5bf1990918aa 45 DigitalIn ultrasonicEcho(PB_3);
elpatosilva 2:5bf1990918aa 46 DigitalIn speedLeft(PA_15);
elpatosilva 2:5bf1990918aa 47 DigitalIn speedRigth(PA_11);
elpatosilva 2:5bf1990918aa 48 InterruptIn speedLeftInterrupt(PA_15); // Interrupciones para los pines de las horquillas
elpatosilva 2:5bf1990918aa 49 InterruptIn speedRigthInterrupt(PA_12);
elpatosilva 2:5bf1990918aa 50 PwmOut motorLeft(PA_11);
elpatosilva 2:5bf1990918aa 51 PwmOut motorRigth(PA_8);
elpatosilva 2:5bf1990918aa 52 */
elpatosilva 1:6e374a9b1bf5 53
elpatosilva 0:749a196e6ebe 54
elpatosilva 0:749a196e6ebe 55 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
elpatosilva 0:749a196e6ebe 56 // Timers y relacionados
elpatosilva 2:5bf1990918aa 57 Ticker ticker100ms;
elpatosilva 2:5bf1990918aa 58 Ticker ticker20ms;
elpatosilva 2:5bf1990918aa 59 Timer timer1ms;
elpatosilva 0:749a196e6ebe 60 uint32_t maxUsTicker;
elpatosilva 0:749a196e6ebe 61
elpatosilva 0:749a196e6ebe 62 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
elpatosilva 0:749a196e6ebe 63 // Prototipos locales
elpatosilva 0:749a196e6ebe 64 void heartBeat();
elpatosilva 2:5bf1990918aa 65 void pwmServo();
elpatosilva 0:749a196e6ebe 66 void heartBeatSetFlag();
elpatosilva 0:749a196e6ebe 67 void serialReceivePc();
elpatosilva 0:749a196e6ebe 68 void cmdExec();
elpatosilva 0:749a196e6ebe 69 void sendOpticData();
elpatosilva 0:749a196e6ebe 70 void sendUltrasonicData();
elpatosilva 0:749a196e6ebe 71 void sendSpeedData();
elpatosilva 1:6e374a9b1bf5 72 void riseSpeedLeftInterrupt();
elpatosilva 1:6e374a9b1bf5 73 void riseSpeedRigthInterrupt();
elpatosilva 0:749a196e6ebe 74
elpatosilva 0:749a196e6ebe 75
elpatosilva 0:749a196e6ebe 76
elpatosilva 0:749a196e6ebe 77 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
elpatosilva 0:749a196e6ebe 78 // Definiciones locales
elpatosilva 0:749a196e6ebe 79
elpatosilva 0:749a196e6ebe 80 // Estructura para uso auxiliar. Arduino es little endian
elpatosilva 0:749a196e6ebe 81 typedef union{
elpatosilva 0:749a196e6ebe 82 uint8_t u8[4];
elpatosilva 0:749a196e6ebe 83 uint16_t u16[2];
elpatosilva 0:749a196e6ebe 84 int8_t i8[2];
elpatosilva 0:749a196e6ebe 85 uint32_t u32;
elpatosilva 0:749a196e6ebe 86 int32_t i32;
elpatosilva 0:749a196e6ebe 87 }_work;
elpatosilva 0:749a196e6ebe 88
elpatosilva 2:5bf1990918aa 89 // Auxiliar
elpatosilva 2:5bf1990918aa 90 _work work;
elpatosilva 0:749a196e6ebe 91
elpatosilva 0:749a196e6ebe 92 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
elpatosilva 2:5bf1990918aa 93 // Variables Locales
elpatosilva 0:749a196e6ebe 94 Serial pc(USBTX, USBRX, 19200);
elpatosilva 0:749a196e6ebe 95 _Pserial pserial;
elpatosilva 0:749a196e6ebe 96
elpatosilva 0:749a196e6ebe 97 _Sensor rigthOpticSensor;
elpatosilva 0:749a196e6ebe 98 _Sensor leftOpticSensor;
elpatosilva 0:749a196e6ebe 99 _Sensor ultrasonicSensor;
elpatosilva 0:749a196e6ebe 100 _Sensor rigthSpeedSensor;
elpatosilva 0:749a196e6ebe 101 _Sensor leftSpeedSensor;
elpatosilva 0:749a196e6ebe 102
elpatosilva 0:749a196e6ebe 103 _Servo servo;
elpatosilva 0:749a196e6ebe 104
elpatosilva 2:5bf1990918aa 105 _Motor motorLeft;
elpatosilva 2:5bf1990918aa 106 _Motor motorRigth;
elpatosilva 2:5bf1990918aa 107
elpatosilva 0:749a196e6ebe 108
elpatosilva 0:749a196e6ebe 109 /*
elpatosilva 0:749a196e6ebe 110 * Control de los sensores, tiempos de muestreo
elpatosilva 0:749a196e6ebe 111 */
elpatosilva 0:749a196e6ebe 112 uint16_t opticTimeInterval;
elpatosilva 0:749a196e6ebe 113 uint16_t ultrasonicTimeInterval;
elpatosilva 0:749a196e6ebe 114 uint16_t motorSpeedTimeInterval;
elpatosilva 0:749a196e6ebe 115
elpatosilva 2:5bf1990918aa 116 // Indica si el servo está en la posicion a la que fue configurado
elpatosilva 2:5bf1990918aa 117 bool servoIsInPos;
elpatosilva 2:5bf1990918aa 118 volatile bool flipServoDigitalOut;
elpatosilva 2:5bf1990918aa 119 volatile uint32_t flipTimeServoDigitalOut;
elpatosilva 2:5bf1990918aa 120
elpatosilva 2:5bf1990918aa 121 // Velocidad, direccion y timeout solicitada en los motores
elpatosilva 2:5bf1990918aa 122 uint16_t leftMotorRqSpeed;
elpatosilva 2:5bf1990918aa 123 uint16_t rigthMotorRqSpeed;
elpatosilva 2:5bf1990918aa 124 uint32_t leftMotorRqTimeOut;
elpatosilva 2:5bf1990918aa 125 uint32_t rigthMotorRqTimeOut;
elpatosilva 2:5bf1990918aa 126 uint8_t leftMotorRqType;
elpatosilva 2:5bf1990918aa 127 uint8_t rigthMotorRqType;
elpatosilva 2:5bf1990918aa 128
elpatosilva 0:749a196e6ebe 129
elpatosilva 0:749a196e6ebe 130 // Flag del heartBeat()
elpatosilva 0:749a196e6ebe 131 bool heartBeatFlag;
elpatosilva 0:749a196e6ebe 132
elpatosilva 2:5bf1990918aa 133
elpatosilva 0:749a196e6ebe 134 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
elpatosilva 0:749a196e6ebe 135 ///////////////// MAIN
elpatosilva 0:749a196e6ebe 136
elpatosilva 0:749a196e6ebe 137 int main(){
elpatosilva 0:749a196e6ebe 138 maxUsTicker = us_ticker_read();
elpatosilva 2:5bf1990918aa 139 ticker100ms.attach(&heartBeatSetFlag, 0.1);
elpatosilva 2:5bf1990918aa 140 ticker20ms.attach(&pwmServo, 0.02);
elpatosilva 0:749a196e6ebe 141 heartBeatFlag = false;
elpatosilva 0:749a196e6ebe 142 timer1ms.start();
elpatosilva 2:5bf1990918aa 143
elpatosilva 2:5bf1990918aa 144 // Setup de puerto serie
elpatosilva 2:5bf1990918aa 145 pc.attach(&serialReceivePc, Serial::RxIrq);
elpatosilva 2:5bf1990918aa 146 serialSetup(&pserial, &pc);
elpatosilva 2:5bf1990918aa 147
elpatosilva 2:5bf1990918aa 148 // setup de los sensores opticos
elpatosilva 0:749a196e6ebe 149 setup(&rigthOpticSensor, &opticRigthIn);
elpatosilva 0:749a196e6ebe 150 setup(&leftOpticSensor, &opticLeftIn);
elpatosilva 2:5bf1990918aa 151
elpatosilva 2:5bf1990918aa 152 // setup del sensor ultrasonico
elpatosilva 0:749a196e6ebe 153 setup(&ultrasonicSensor, &ultrasonicTrigger, &ultrasonicEcho);
elpatosilva 1:6e374a9b1bf5 154
elpatosilva 2:5bf1990918aa 155 // setup de los sensores de horquilla
elpatosilva 2:5bf1990918aa 156 setup(&leftSpeedSensor, &speedLeft, WHEEL_ENCODER_SLOTS, WHEEL_PERIM);
elpatosilva 2:5bf1990918aa 157 setup(&rigthSpeedSensor, &speedRigth, WHEEL_ENCODER_SLOTS, WHEEL_PERIM);
elpatosilva 1:6e374a9b1bf5 158 speedLeftInterrupt.rise(&riseSpeedLeftInterrupt);
elpatosilva 1:6e374a9b1bf5 159 speedRigthInterrupt.rise(&riseSpeedRigthInterrupt);
elpatosilva 1:6e374a9b1bf5 160
elpatosilva 2:5bf1990918aa 161 // setup del servo
elpatosilva 2:5bf1990918aa 162 setup(&servo, &servoPwm);
elpatosilva 2:5bf1990918aa 163 setMinPulseWidth_us(&servo, 500);
elpatosilva 2:5bf1990918aa 164 setMaxPulseWidth_us(&servo, 2500);
elpatosilva 0:749a196e6ebe 165
elpatosilva 2:5bf1990918aa 166
elpatosilva 2:5bf1990918aa 167 // setup de los driver de motor
elpatosilva 2:5bf1990918aa 168 setup(&motorLeft, &leftSpeedSensor, &motorLeftPwm, &motorLeftA, &motorLeftB);
elpatosilva 2:5bf1990918aa 169 setup(&motorRigth, &rigthSpeedSensor, &motorRigthPwm, &motorRigthA, &motorRigthB);
elpatosilva 2:5bf1990918aa 170
elpatosilva 2:5bf1990918aa 171 // Variables de control
elpatosilva 0:749a196e6ebe 172 opticTimeInterval = 0;
elpatosilva 0:749a196e6ebe 173 ultrasonicTimeInterval = 0;
elpatosilva 0:749a196e6ebe 174 motorSpeedTimeInterval = 0;
elpatosilva 0:749a196e6ebe 175
elpatosilva 2:5bf1990918aa 176 servoIsInPos = true;
elpatosilva 2:5bf1990918aa 177 flipServoDigitalOut = false;
elpatosilva 2:5bf1990918aa 178
elpatosilva 0:749a196e6ebe 179 while(1){
elpatosilva 0:749a196e6ebe 180 // heartBeat
elpatosilva 0:749a196e6ebe 181 if(heartBeatFlag) heartBeat();
elpatosilva 0:749a196e6ebe 182
elpatosilva 0:749a196e6ebe 183 // Overflow de us_ticker_read(), reseteo todo lo que utiliza este dato
elpatosilva 0:749a196e6ebe 184 if(maxUsTicker > us_ticker_read()){
elpatosilva 0:749a196e6ebe 185 maxUsTicker = us_ticker_read();
elpatosilva 0:749a196e6ebe 186 serialCmdClean(&pserial);
elpatosilva 2:5bf1990918aa 187 reset(&ultrasonicSensor);
elpatosilva 2:5bf1990918aa 188 reset(&rigthSpeedSensor);
elpatosilva 2:5bf1990918aa 189 reset(&leftSpeedSensor);
elpatosilva 0:749a196e6ebe 190 }
elpatosilva 0:749a196e6ebe 191
elpatosilva 2:5bf1990918aa 192 // envio datos que pueda haber en el buffer de salida
elpatosilva 2:5bf1990918aa 193 serialSubmit(&pserial);
elpatosilva 0:749a196e6ebe 194
elpatosilva 2:5bf1990918aa 195 // proceso comando presente en el buffer
elpatosilva 2:5bf1990918aa 196 if(pserial.RX.cmd)
elpatosilva 0:749a196e6ebe 197 cmdExec();
elpatosilva 0:749a196e6ebe 198
elpatosilva 2:5bf1990918aa 199 // Busco si hay un nuevo header en el buffer
elpatosilva 2:5bf1990918aa 200 serialGetFrame(&pserial);
elpatosilva 0:749a196e6ebe 201
elpatosilva 2:5bf1990918aa 202 // Ejecuto tareas de sensores
elpatosilva 2:5bf1990918aa 203 if(opticTimeInterval){
elpatosilva 0:749a196e6ebe 204 sendOpticData();
elpatosilva 2:5bf1990918aa 205 }
elpatosilva 0:749a196e6ebe 206
elpatosilva 2:5bf1990918aa 207 if(ultrasonicTimeInterval && servoIsInPos){
elpatosilva 0:749a196e6ebe 208 sendUltrasonicData();
elpatosilva 2:5bf1990918aa 209 }
elpatosilva 0:749a196e6ebe 210
elpatosilva 2:5bf1990918aa 211 if(motorSpeedTimeInterval){
elpatosilva 2:5bf1990918aa 212 sendSpeedData();
elpatosilva 2:5bf1990918aa 213 }
elpatosilva 2:5bf1990918aa 214
elpatosilva 2:5bf1990918aa 215 // Ejecuto tareas con actuadores
elpatosilva 2:5bf1990918aa 216 if(inPos(&servo) && !servoIsInPos){
elpatosilva 2:5bf1990918aa 217 servoIsInPos = true;
elpatosilva 2:5bf1990918aa 218 // Notifico fin de movimiento
elpatosilva 2:5bf1990918aa 219 work.u16[0] = servo.speed;
elpatosilva 2:5bf1990918aa 220 serialEnqueueHeader(&pserial, 6);
elpatosilva 2:5bf1990918aa 221 serialEnqueueData(&pserial, 0xA2);
elpatosilva 2:5bf1990918aa 222 serialEnqueueData(&pserial, servo.currentPos);
elpatosilva 2:5bf1990918aa 223 serialEnqueueData(&pserial, work.u8[0]);
elpatosilva 2:5bf1990918aa 224 serialEnqueueData(&pserial, work.u8[1]);
elpatosilva 2:5bf1990918aa 225 serialEnqueueData(&pserial, 0x0A);
elpatosilva 2:5bf1990918aa 226 serialEnqueueChksum(&pserial);
elpatosilva 2:5bf1990918aa 227 }
elpatosilva 2:5bf1990918aa 228
elpatosilva 2:5bf1990918aa 229 // pseudo PWM del servo
elpatosilva 2:5bf1990918aa 230 if(flipServoDigitalOut && us_ticker_read() > (flipTimeServoDigitalOut+servo.currentPulseWidth)){
elpatosilva 2:5bf1990918aa 231 flipServoDigitalOut = false;
elpatosilva 2:5bf1990918aa 232 servoDigitalPin.write(0);
elpatosilva 2:5bf1990918aa 233 }
elpatosilva 0:749a196e6ebe 234 }
elpatosilva 0:749a196e6ebe 235 }
elpatosilva 0:749a196e6ebe 236
elpatosilva 0:749a196e6ebe 237
elpatosilva 0:749a196e6ebe 238
elpatosilva 0:749a196e6ebe 239 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
elpatosilva 0:749a196e6ebe 240 /// Funciones locales
elpatosilva 0:749a196e6ebe 241
elpatosilva 0:749a196e6ebe 242 /*
elpatosilva 0:749a196e6ebe 243 * Interrupcion, datos en el puerto serie con la PC
elpatosilva 0:749a196e6ebe 244 */
elpatosilva 0:749a196e6ebe 245 void serialReceivePc(){
elpatosilva 0:749a196e6ebe 246 serialReceive(&pserial);
elpatosilva 0:749a196e6ebe 247 }
elpatosilva 0:749a196e6ebe 248
elpatosilva 2:5bf1990918aa 249 /*
elpatosilva 2:5bf1990918aa 250 * pwm a manopla para manejar el faquing servo. Periodo es 20ms, pulsos entre 1 y 2ms. el rango es 180, esta func se ejecuta cada 10us y me da una definicion de 100 llamados cada 1ms
elpatosilva 2:5bf1990918aa 251 */
elpatosilva 2:5bf1990918aa 252 void pwmServo(){
elpatosilva 2:5bf1990918aa 253 flipServoDigitalOut = true;
elpatosilva 2:5bf1990918aa 254 flipTimeServoDigitalOut = us_ticker_read();
elpatosilva 2:5bf1990918aa 255 servoDigitalPin.write(1);
elpatosilva 2:5bf1990918aa 256 }
elpatosilva 0:749a196e6ebe 257
elpatosilva 0:749a196e6ebe 258 /*
elpatosilva 0:749a196e6ebe 259 * Señal de activo, dos parpadeos de 100ms cada dos segundos
elpatosilva 1:6e374a9b1bf5 260 * El timer me fueza a pasar pr el wile(1) del main, sino no tiene mucho sentido un heartbeat por interrupciones
elpatosilva 0:749a196e6ebe 261 */
elpatosilva 0:749a196e6ebe 262 void heartBeatSetFlag(){
elpatosilva 0:749a196e6ebe 263 heartBeatFlag = true;
elpatosilva 0:749a196e6ebe 264 }
elpatosilva 0:749a196e6ebe 265
elpatosilva 1:6e374a9b1bf5 266 /*
elpatosilva 1:6e374a9b1bf5 267 * Interrupciones de los pin digitales de los sensores
elpatosilva 1:6e374a9b1bf5 268 */
elpatosilva 1:6e374a9b1bf5 269 void riseSpeedLeftInterrupt(){
elpatosilva 1:6e374a9b1bf5 270 notify(&leftSpeedSensor);
elpatosilva 1:6e374a9b1bf5 271 }
elpatosilva 1:6e374a9b1bf5 272 void riseSpeedRigthInterrupt(){
elpatosilva 1:6e374a9b1bf5 273 notify(&rigthSpeedSensor);
elpatosilva 1:6e374a9b1bf5 274 }
elpatosilva 1:6e374a9b1bf5 275
elpatosilva 1:6e374a9b1bf5 276
elpatosilva 0:749a196e6ebe 277 void heartBeat(){
elpatosilva 0:749a196e6ebe 278 static int count = 0;
elpatosilva 0:749a196e6ebe 279 switch (count++){
elpatosilva 0:749a196e6ebe 280 case 0:
elpatosilva 0:749a196e6ebe 281 case 2:
elpatosilva 0:749a196e6ebe 282 led1 = 0;
elpatosilva 0:749a196e6ebe 283 break;
elpatosilva 0:749a196e6ebe 284 case 1:
elpatosilva 0:749a196e6ebe 285 case 3:
elpatosilva 0:749a196e6ebe 286 led1 = 1;
elpatosilva 0:749a196e6ebe 287 break;
elpatosilva 0:749a196e6ebe 288 case 20:
elpatosilva 0:749a196e6ebe 289 count = 0;
elpatosilva 0:749a196e6ebe 290 break;
elpatosilva 0:749a196e6ebe 291 }
elpatosilva 0:749a196e6ebe 292 heartBeatFlag = false;
elpatosilva 0:749a196e6ebe 293 }
elpatosilva 0:749a196e6ebe 294
elpatosilva 0:749a196e6ebe 295
elpatosilva 0:749a196e6ebe 296
elpatosilva 0:749a196e6ebe 297 /*
elpatosilva 0:749a196e6ebe 298 * Envio datos del sensor optico a la velocidad especificada
elpatosilva 0:749a196e6ebe 299 */
elpatosilva 0:749a196e6ebe 300 void sendOpticData(){
elpatosilva 0:749a196e6ebe 301 static unsigned long stime = timer1ms.read_ms();
elpatosilva 0:749a196e6ebe 302 if(timer1ms.read_ms() > stime+(opticTimeInterval*10)){
elpatosilva 0:749a196e6ebe 303 work.u16[0] = opticTimeInterval;
elpatosilva 0:749a196e6ebe 304 serialEnqueueHeader(&pserial, 12);
elpatosilva 0:749a196e6ebe 305 serialEnqueueData(&pserial, 0xA0); // Comando
elpatosilva 0:749a196e6ebe 306 serialEnqueueData(&pserial, work.u8[0]); // Intervalo actual (2 bytes)
elpatosilva 0:749a196e6ebe 307 serialEnqueueData(&pserial, work.u8[1]);
elpatosilva 0:749a196e6ebe 308 serialEnqueueData(&pserial, 0); // Sensor exterior izquierdo (2 bytes)
elpatosilva 0:749a196e6ebe 309 serialEnqueueData(&pserial, 0);
elpatosilva 0:749a196e6ebe 310 work.u16[0] = getValue(&leftOpticSensor);
elpatosilva 0:749a196e6ebe 311 work.u16[1] = getValue(&rigthOpticSensor);
elpatosilva 0:749a196e6ebe 312 serialEnqueueData(&pserial, work.u8[0]); // Sensor interior izquierdo (2 bytes)
elpatosilva 0:749a196e6ebe 313 serialEnqueueData(&pserial, work.u8[1]);
elpatosilva 0:749a196e6ebe 314 serialEnqueueData(&pserial, work.u8[2]); // Sensor interior drecho (2 bytes)
elpatosilva 0:749a196e6ebe 315 serialEnqueueData(&pserial, work.u8[3]);
elpatosilva 0:749a196e6ebe 316 serialEnqueueData(&pserial, 0); // Sensor exterior derecho (2 bytes)
elpatosilva 0:749a196e6ebe 317 serialEnqueueData(&pserial, 0);
elpatosilva 0:749a196e6ebe 318 serialEnqueueChksum(&pserial);
elpatosilva 0:749a196e6ebe 319 stime = timer1ms.read_ms();
elpatosilva 0:749a196e6ebe 320 }
elpatosilva 0:749a196e6ebe 321 }
elpatosilva 0:749a196e6ebe 322
elpatosilva 0:749a196e6ebe 323
elpatosilva 0:749a196e6ebe 324
elpatosilva 0:749a196e6ebe 325 /*
elpatosilva 0:749a196e6ebe 326 * Envio datos del sensor ultrasonico a la velocidad especificada
elpatosilva 0:749a196e6ebe 327 * Los tiempos de llamada deberian estar espaciados al menos 50ms
elpatosilva 0:749a196e6ebe 328 */
elpatosilva 0:749a196e6ebe 329 void sendUltrasonicData(){
elpatosilva 0:749a196e6ebe 330 static uint32_t stime = timer1ms.read_ms();
elpatosilva 1:6e374a9b1bf5 331
elpatosilva 1:6e374a9b1bf5 332 // Utilizando init y isReady
elpatosilva 1:6e374a9b1bf5 333 if(ultrasonicSensor.stage){
elpatosilva 0:749a196e6ebe 334 uint16_t distance = isReady(&ultrasonicSensor);
elpatosilva 1:6e374a9b1bf5 335 if(distance > 0){ // Ya tengo la distancia, la envio
elpatosilva 0:749a196e6ebe 336 work.u16[0] = ultrasonicTimeInterval;
elpatosilva 1:6e374a9b1bf5 337 serialEnqueueHeader(&pserial, 6);
elpatosilva 0:749a196e6ebe 338 serialEnqueueData(&pserial, 0xA1); // Comando
elpatosilva 0:749a196e6ebe 339 serialEnqueueData(&pserial, work.u8[0]); // Intervalo actual (2 bytes)
elpatosilva 0:749a196e6ebe 340 serialEnqueueData(&pserial, work.u8[1]);
elpatosilva 0:749a196e6ebe 341
elpatosilva 1:6e374a9b1bf5 342 work.u16[0] = distance;
elpatosilva 0:749a196e6ebe 343 serialEnqueueData(&pserial, work.u8[0]); // Distancia medida (2 bytes)
elpatosilva 0:749a196e6ebe 344 serialEnqueueData(&pserial, work.u8[1]);
elpatosilva 0:749a196e6ebe 345
elpatosilva 0:749a196e6ebe 346 serialEnqueueChksum(&pserial);
elpatosilva 0:749a196e6ebe 347 }
elpatosilva 0:749a196e6ebe 348 }
elpatosilva 0:749a196e6ebe 349
elpatosilva 1:6e374a9b1bf5 350 if(timer1ms.read_ms() > stime+(ultrasonicTimeInterval*10)){
elpatosilva 0:749a196e6ebe 351 reset(&ultrasonicSensor);
elpatosilva 1:6e374a9b1bf5 352 init(&ultrasonicSensor);
elpatosilva 0:749a196e6ebe 353 stime = timer1ms.read_ms();
elpatosilva 0:749a196e6ebe 354 }
elpatosilva 0:749a196e6ebe 355 }
elpatosilva 0:749a196e6ebe 356
elpatosilva 0:749a196e6ebe 357
elpatosilva 0:749a196e6ebe 358
elpatosilva 1:6e374a9b1bf5 359 void sendSpeedData(){
elpatosilva 1:6e374a9b1bf5 360 static unsigned long stime = timer1ms.read_ms();
elpatosilva 1:6e374a9b1bf5 361 if(timer1ms.read_ms() > stime+(motorSpeedTimeInterval*10)){
elpatosilva 1:6e374a9b1bf5 362 work.u16[0] = motorSpeedTimeInterval;
elpatosilva 1:6e374a9b1bf5 363 serialEnqueueHeader(&pserial, 8);
elpatosilva 1:6e374a9b1bf5 364 serialEnqueueData(&pserial, 0xA4); // Comando
elpatosilva 1:6e374a9b1bf5 365 serialEnqueueData(&pserial, work.u8[0]); // Intervalo actual (2 bytes)
elpatosilva 1:6e374a9b1bf5 366 serialEnqueueData(&pserial, work.u8[1]);
elpatosilva 1:6e374a9b1bf5 367 work.u16[0] = getRpm(&leftSpeedSensor);
elpatosilva 1:6e374a9b1bf5 368 work.u16[1] = getRpm(&rigthSpeedSensor);
elpatosilva 1:6e374a9b1bf5 369 serialEnqueueData(&pserial, work.u8[0]); // Sensor izquierdo (2 bytes)
elpatosilva 1:6e374a9b1bf5 370 serialEnqueueData(&pserial, work.u8[1]);
elpatosilva 1:6e374a9b1bf5 371 serialEnqueueData(&pserial, work.u8[2]); // Sensor drecho (2 bytes)
elpatosilva 1:6e374a9b1bf5 372 serialEnqueueData(&pserial, work.u8[3]);
elpatosilva 1:6e374a9b1bf5 373 serialEnqueueChksum(&pserial);
elpatosilva 1:6e374a9b1bf5 374 stime = timer1ms.read_ms();
elpatosilva 1:6e374a9b1bf5 375 }
elpatosilva 1:6e374a9b1bf5 376 }
elpatosilva 0:749a196e6ebe 377
elpatosilva 0:749a196e6ebe 378 /*
elpatosilva 0:749a196e6ebe 379 * Se ha recibido un comando por puerto serie, lo interpreto y respondo
elpatosilva 2:5bf1990918aa 380 * Si ok no se setea a true, el comando se vuelve a prosesar
elpatosilva 2:5bf1990918aa 381 * Por ejemplo para responder el alive, si no hay lugar en el buffer no seteo ok y por lo tanto
elpatosilva 2:5bf1990918aa 382 * el comando se vuelve a procesar. No es indefinido, lo espero TXCMDTIME us
elpatosilva 0:749a196e6ebe 383 */
elpatosilva 0:749a196e6ebe 384 void cmdExec(){
elpatosilva 0:749a196e6ebe 385 bool ok = false;
elpatosilva 0:749a196e6ebe 386 switch(pserial.RX.buf[pserial.RX.cmdIndex]){
elpatosilva 0:749a196e6ebe 387 // ping
elpatosilva 0:749a196e6ebe 388 case 0xF0:{
elpatosilva 0:749a196e6ebe 389 ok = serialEnqueueHeader(&pserial, 3) && serialEnqueueData(&pserial, 0xF0) && serialEnqueueData(&pserial, 0x0D) && serialEnqueueChksum(&pserial);
elpatosilva 0:749a196e6ebe 390 break;
elpatosilva 0:749a196e6ebe 391 }
elpatosilva 0:749a196e6ebe 392 // Version de Firmware
elpatosilva 0:749a196e6ebe 393 case 0xF1:{
elpatosilva 0:749a196e6ebe 394 ok = serialEnqueueHeader(&pserial, 13+sizeof(FIRMWARE_VERSION)+sizeof(__DATE__)+sizeof(__TIME__)) && serialEnqueueData(&pserial, 0xF1);
elpatosilva 0:749a196e6ebe 395 for (int i = 0 ; i < sizeof(FIRMWARE_VERSION) ; i++)
elpatosilva 0:749a196e6ebe 396 ok = ok && serialEnqueueData(&pserial, FIRMWARE_VERSION[i]);
elpatosilva 0:749a196e6ebe 397 ok = ok && serialEnqueueData(&pserial, '_') && serialEnqueueData(&pserial, 'b') && serialEnqueueData(&pserial, 'u') && serialEnqueueData(&pserial, 'i') && serialEnqueueData(&pserial, 'l') && serialEnqueueData(&pserial, 'd') && serialEnqueueData(&pserial, '_');
elpatosilva 0:749a196e6ebe 398 for (int i = 0 ; i < sizeof(__DATE__) ; i++)
elpatosilva 0:749a196e6ebe 399 ok = ok && serialEnqueueData(&pserial, __DATE__[i]);
elpatosilva 0:749a196e6ebe 400 ok = ok && serialEnqueueData(&pserial, '_') && serialEnqueueData(&pserial, 'a') && serialEnqueueData(&pserial, 't') && serialEnqueueData(&pserial, '_');
elpatosilva 0:749a196e6ebe 401 for (int i = 0 ; i < sizeof(__TIME__) ; i++)
elpatosilva 0:749a196e6ebe 402 ok = ok && serialEnqueueData(&pserial, __TIME__[i]);
elpatosilva 0:749a196e6ebe 403 ok = ok && serialEnqueueChksum(&pserial);
elpatosilva 0:749a196e6ebe 404 break;
elpatosilva 0:749a196e6ebe 405 }
elpatosilva 0:749a196e6ebe 406 // Sensores Opticos
elpatosilva 0:749a196e6ebe 407 case 0xA0:{
elpatosilva 0:749a196e6ebe 408 work.u8[0] = pserial.RX.buf[pserial.RX.cmdIndex+1];
elpatosilva 0:749a196e6ebe 409 work.u8[1] = pserial.RX.buf[pserial.RX.cmdIndex+2];
elpatosilva 0:749a196e6ebe 410 opticTimeInterval = work.u16[0];
elpatosilva 0:749a196e6ebe 411 ok = true;
elpatosilva 0:749a196e6ebe 412 break;
elpatosilva 0:749a196e6ebe 413 }
elpatosilva 0:749a196e6ebe 414 // Sensor Ultrasonico
elpatosilva 0:749a196e6ebe 415 case 0xA1:{
elpatosilva 0:749a196e6ebe 416 work.u8[0] = pserial.RX.buf[pserial.RX.cmdIndex+1];
elpatosilva 0:749a196e6ebe 417 work.u8[1] = pserial.RX.buf[pserial.RX.cmdIndex+2];
elpatosilva 0:749a196e6ebe 418 ultrasonicTimeInterval = work.u16[0];
elpatosilva 0:749a196e6ebe 419 ok = true;
elpatosilva 0:749a196e6ebe 420 break;
elpatosilva 0:749a196e6ebe 421 }
elpatosilva 2:5bf1990918aa 422 // Servo
elpatosilva 2:5bf1990918aa 423 case 0xA2:{
elpatosilva 2:5bf1990918aa 424 work.u8[0] = pserial.RX.buf[pserial.RX.cmdIndex+2];
elpatosilva 2:5bf1990918aa 425 work.u8[1] = pserial.RX.buf[pserial.RX.cmdIndex+3];
elpatosilva 2:5bf1990918aa 426 setPos(&servo, pserial.RX.buf[pserial.RX.cmdIndex+1], work.u16[0]);
elpatosilva 2:5bf1990918aa 427 servoIsInPos = false;
elpatosilva 2:5bf1990918aa 428 ok = serialEnqueueHeader(&pserial, 3) &&
elpatosilva 2:5bf1990918aa 429 serialEnqueueData(&pserial, 0xA2) &&
elpatosilva 2:5bf1990918aa 430 serialEnqueueData(&pserial, 0x0D) &&
elpatosilva 2:5bf1990918aa 431 serialEnqueueChksum(&pserial);
elpatosilva 2:5bf1990918aa 432 break;
elpatosilva 2:5bf1990918aa 433 }
elpatosilva 2:5bf1990918aa 434 // Control de motores
elpatosilva 2:5bf1990918aa 435 case 0xA3:{
elpatosilva 2:5bf1990918aa 436 rigthMotorRqType = pserial.RX.buf[pserial.RX.cmdIndex+1];
elpatosilva 2:5bf1990918aa 437 leftMotorRqType = pserial.RX.buf[pserial.RX.cmdIndex+2];
elpatosilva 2:5bf1990918aa 438
elpatosilva 2:5bf1990918aa 439 work.u8[0] = pserial.RX.buf[pserial.RX.cmdIndex+3];
elpatosilva 2:5bf1990918aa 440 work.u8[1] = pserial.RX.buf[pserial.RX.cmdIndex+4];
elpatosilva 2:5bf1990918aa 441 work.u8[2] = pserial.RX.buf[pserial.RX.cmdIndex+5];
elpatosilva 2:5bf1990918aa 442 work.u8[3] = pserial.RX.buf[pserial.RX.cmdIndex+6];
elpatosilva 2:5bf1990918aa 443 rigthMotorRqTimeOut = work.u32;
elpatosilva 2:5bf1990918aa 444
elpatosilva 2:5bf1990918aa 445 work.u8[0] = pserial.RX.buf[pserial.RX.cmdIndex+7];
elpatosilva 2:5bf1990918aa 446 work.u8[1] = pserial.RX.buf[pserial.RX.cmdIndex+8];
elpatosilva 2:5bf1990918aa 447 work.u8[2] = pserial.RX.buf[pserial.RX.cmdIndex+9];
elpatosilva 2:5bf1990918aa 448 work.u8[3] = pserial.RX.buf[pserial.RX.cmdIndex+10];
elpatosilva 2:5bf1990918aa 449 leftMotorRqTimeOut = work.u32;;
elpatosilva 2:5bf1990918aa 450
elpatosilva 2:5bf1990918aa 451 work.u8[0] = pserial.RX.buf[pserial.RX.cmdIndex+11];
elpatosilva 2:5bf1990918aa 452 work.u8[1] = pserial.RX.buf[pserial.RX.cmdIndex+12];
elpatosilva 2:5bf1990918aa 453 work.u8[2] = pserial.RX.buf[pserial.RX.cmdIndex+13];
elpatosilva 2:5bf1990918aa 454 work.u8[3] = pserial.RX.buf[pserial.RX.cmdIndex+14];
elpatosilva 2:5bf1990918aa 455 rigthMotorRqSpeed = work.u16[0];
elpatosilva 2:5bf1990918aa 456 leftMotorRqSpeed = work.u16[1];
elpatosilva 2:5bf1990918aa 457 ok = serialEnqueueHeader(&pserial, 5) &&
elpatosilva 2:5bf1990918aa 458 serialEnqueueData(&pserial, 0xA3) &&
elpatosilva 2:5bf1990918aa 459 serialEnqueueData(&pserial, rigthMotorRqType) &&
elpatosilva 2:5bf1990918aa 460 serialEnqueueData(&pserial, leftMotorRqType) &&
elpatosilva 2:5bf1990918aa 461 serialEnqueueData(&pserial, 0x0D) &&
elpatosilva 2:5bf1990918aa 462 serialEnqueueChksum(&pserial);
elpatosilva 2:5bf1990918aa 463 break;
elpatosilva 2:5bf1990918aa 464 }
elpatosilva 0:749a196e6ebe 465 // Sensores de horquilla
elpatosilva 0:749a196e6ebe 466 case 0xA4:{
elpatosilva 0:749a196e6ebe 467 work.u8[0] = pserial.RX.buf[pserial.RX.cmdIndex+1];
elpatosilva 0:749a196e6ebe 468 work.u8[1] = pserial.RX.buf[pserial.RX.cmdIndex+2];
elpatosilva 0:749a196e6ebe 469 motorSpeedTimeInterval = work.u16[0];
elpatosilva 0:749a196e6ebe 470 ok = true;
elpatosilva 0:749a196e6ebe 471 break;
elpatosilva 0:749a196e6ebe 472 }
elpatosilva 2:5bf1990918aa 473 // Comandos fuera de protocolo
elpatosilva 2:5bf1990918aa 474 // Setear period y pulseWidth del servo (ui16 period, ui16 min, ui16 max)
elpatosilva 2:5bf1990918aa 475 case 0xE0:{
elpatosilva 2:5bf1990918aa 476 work.u8[0] = pserial.RX.buf[pserial.RX.cmdIndex+1];
elpatosilva 2:5bf1990918aa 477 work.u8[1] = pserial.RX.buf[pserial.RX.cmdIndex+2];
elpatosilva 2:5bf1990918aa 478 setPeriod_us(&servo, work.u16[0]);
elpatosilva 2:5bf1990918aa 479 work.u8[0] = pserial.RX.buf[pserial.RX.cmdIndex+3];
elpatosilva 2:5bf1990918aa 480 work.u8[1] = pserial.RX.buf[pserial.RX.cmdIndex+4];
elpatosilva 2:5bf1990918aa 481 work.u8[2] = pserial.RX.buf[pserial.RX.cmdIndex+5];
elpatosilva 2:5bf1990918aa 482 work.u8[3] = pserial.RX.buf[pserial.RX.cmdIndex+6];
elpatosilva 2:5bf1990918aa 483 setMinPulseWidth_us(&servo, work.u16[0]);
elpatosilva 2:5bf1990918aa 484 setMaxPulseWidth_us(&servo, work.u16[1]);
elpatosilva 2:5bf1990918aa 485 ok = serialEnqueueHeader(&pserial, 3) &&
elpatosilva 2:5bf1990918aa 486 serialEnqueueData(&pserial, 0xE0) &&
elpatosilva 2:5bf1990918aa 487 serialEnqueueData(&pserial, 0x0D) &&
elpatosilva 2:5bf1990918aa 488 serialEnqueueChksum(&pserial);
elpatosilva 2:5bf1990918aa 489 ok = true;
elpatosilva 2:5bf1990918aa 490 break;
elpatosilva 2:5bf1990918aa 491 }
elpatosilva 2:5bf1990918aa 492
elpatosilva 0:749a196e6ebe 493 default:{
elpatosilva 0:749a196e6ebe 494 ok = serialEnqueueHeader(&pserial, 2) && serialEnqueueData(&pserial, 0xFF) && serialEnqueueChksum(&pserial);
elpatosilva 0:749a196e6ebe 495 }
elpatosilva 0:749a196e6ebe 496 }
elpatosilva 0:749a196e6ebe 497 if (ok || us_ticker_read() > pserial.TX.cmdTime + TXCMDTIME){
elpatosilva 0:749a196e6ebe 498 pserial.RX.cmd = false;
elpatosilva 0:749a196e6ebe 499 pserial.RX.indexR = pserial.RX.cmdIndex+pserial.RX.payloadSize;
elpatosilva 0:749a196e6ebe 500 pserial.TX.cmdTime = 0;
elpatosilva 0:749a196e6ebe 501 }
elpatosilva 0:749a196e6ebe 502 }