virtualmech / Mbed 2 deprecated SistemaMicrocontrolador3

sistemaMicrontrolador3

Dependencies:   mbed

main.cpp@0:390287d3dcb6, 2016-12-19 (annotated)

Committer:
JuanManuelAmador
Date:
Mon Dec 19 15:13:15 2016 +0000
Revision:
0:390287d3dcb6
sistemaMicrocontrolador3;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JuanManuelAmador 0:390287d3dcb6 1 /***************************************
JuanManuelAmador 0:390287d3dcb6 2 VIRTUALMECH
JuanManuelAmador 0:390287d3dcb6 3 Autor: Juan Manuel Amador Olivares
JuanManuelAmador 0:390287d3dcb6 4 Fecha: 16/9/2015
JuanManuelAmador 0:390287d3dcb6 5 ***************************************/
JuanManuelAmador 0:390287d3dcb6 6
JuanManuelAmador 0:390287d3dcb6 7 /***************************************
JuanManuelAmador 0:390287d3dcb6 8 Adquisición de datos IMU y Láser
JuanManuelAmador 0:390287d3dcb6 9 ***************************************/
JuanManuelAmador 0:390287d3dcb6 10 #include "mbed.h"
JuanManuelAmador 0:390287d3dcb6 11 #include "Buffering.h"
JuanManuelAmador 0:390287d3dcb6 12 #include "Bufferinguint.h"
JuanManuelAmador 0:390287d3dcb6 13
JuanManuelAmador 0:390287d3dcb6 14 #define TAMPAQUETEIMU 34 // Tamaño del paquete recibido de la IMU
JuanManuelAmador 0:390287d3dcb6 15 #define TAMENVIOIMU 30 // Tamaño del paquete enviado al PC con los datos de la IMU
JuanManuelAmador 0:390287d3dcb6 16 #define TAMENVIOLASER 8 // Tamaño del paquete enviado al PC con los datos del sensor de distancia
JuanManuelAmador 0:390287d3dcb6 17
JuanManuelAmador 0:390287d3dcb6 18 /********************
JuanManuelAmador 0:390287d3dcb6 19 Funciones
JuanManuelAmador 0:390287d3dcb6 20 ********************/
JuanManuelAmador 0:390287d3dcb6 21 void startStreamingIMU(); // Manda los bytes a la IMU necesarios para que comience a enviar datos por streaming
JuanManuelAmador 0:390287d3dcb6 22 void envioPaquete(unsigned char paquete[], int nElementos); // Envía por el puerto serie un paquete de datos
JuanManuelAmador 0:390287d3dcb6 23 /********************/
JuanManuelAmador 0:390287d3dcb6 24
JuanManuelAmador 0:390287d3dcb6 25 DigitalOut led(LED1); // Led indicador. Se enciende cuando se están adquiriendo datos
JuanManuelAmador 0:390287d3dcb6 26
JuanManuelAmador 0:390287d3dcb6 27 // Puertos UART a utilizar
JuanManuelAmador 0:390287d3dcb6 28 Serial pLaser(p9, p10);
JuanManuelAmador 0:390287d3dcb6 29 Serial pIMU(p28, p27);
JuanManuelAmador 0:390287d3dcb6 30 Serial pc(USBTX, USBRX);
JuanManuelAmador 0:390287d3dcb6 31
JuanManuelAmador 0:390287d3dcb6 32 // Temporizador para el control del tiempo
JuanManuelAmador 0:390287d3dcb6 33 Timer t;
JuanManuelAmador 0:390287d3dcb6 34 unsigned int auxtime = 0; // Variable auxiliar para la lectura del tiempo
JuanManuelAmador 0:390287d3dcb6 35 unsigned char byteIN, byteINanterior, byteINbuff, byteINanteriorbuff; // Variables auxiliares para guardar bytes recibidos de la IMU
JuanManuelAmador 0:390287d3dcb6 36 unsigned char byteINL, byteINbuffL; // Variable auxiliar para guardar bytes recibidos del laser
JuanManuelAmador 0:390287d3dcb6 37
JuanManuelAmador 0:390287d3dcb6 38 // Variables para la reconstrucción de los datos de la IMU
JuanManuelAmador 0:390287d3dcb6 39 unsigned int nBytes = TAMPAQUETEIMU;
JuanManuelAmador 0:390287d3dcb6 40 unsigned char paqueteEnvio[TAMENVIOIMU]; // 12 bytes para las aceleraciones (bytes cada eje) y otros 4 para la marca de tiempo y otros 12 para las velocidades angulares más un byte 'i' que indica que es el paquete de una IMU
JuanManuelAmador 0:390287d3dcb6 41 /*union datoCompuesto{
JuanManuelAmador 0:390287d3dcb6 42 char B[4];
JuanManuelAmador 0:390287d3dcb6 43 float unidos;
JuanManuelAmador 0:390287d3dcb6 44 } dato;*/
JuanManuelAmador 0:390287d3dcb6 45
JuanManuelAmador 0:390287d3dcb6 46 // Variables para la reconstrucción datos del láser
JuanManuelAmador 0:390287d3dcb6 47 unsigned int distancia; // Sólo se usarán los 2 bytes menos significativos
JuanManuelAmador 0:390287d3dcb6 48 unsigned char paqueteEnvioL[TAMENVIOLASER];
JuanManuelAmador 0:390287d3dcb6 49 char fbyte = 0; // Indica si el byte leído es el primero delos dos enviados por el láser (tomando valor true) o el segundo (tomando valor false)
JuanManuelAmador 0:390287d3dcb6 50
JuanManuelAmador 0:390287d3dcb6 51 // Buffers dónde se guardarán los bytes provenientes de los sensores
JuanManuelAmador 0:390287d3dcb6 52 Buffering IMUbuff;
JuanManuelAmador 0:390287d3dcb6 53 Bufferinguint IMUtime;
JuanManuelAmador 0:390287d3dcb6 54 Buffering LASERbuff;
JuanManuelAmador 0:390287d3dcb6 55 Bufferinguint LASERtime;
JuanManuelAmador 0:390287d3dcb6 56
JuanManuelAmador 0:390287d3dcb6 57 // Variables semáforo para que no se mezclen los bytes de distintos paquetes
JuanManuelAmador 0:390287d3dcb6 58 // Estas variables indican de que sensor se pueden leer y procesar los bytes de los paquetes recibidos.
JuanManuelAmador 0:390287d3dcb6 59 // Independientemente de estas variables se recibirán los bytes de los distintos sensores, que se irán guardando en sus respectivos buffer,
JuanManuelAmador 0:390287d3dcb6 60 // y se irá guardando la marca de tiempo cuando se detecte la llegada de un nuevo paquete.
JuanManuelAmador 0:390287d3dcb6 61 bool enviandoIMU = false;
JuanManuelAmador 0:390287d3dcb6 62 bool enviandoLASER = false;
JuanManuelAmador 0:390287d3dcb6 63 bool adquiriendo = false;
JuanManuelAmador 0:390287d3dcb6 64
JuanManuelAmador 0:390287d3dcb6 65 // Variable de control de bytes enviados
JuanManuelAmador 0:390287d3dcb6 66 unsigned int bytesEnviados;
JuanManuelAmador 0:390287d3dcb6 67
JuanManuelAmador 0:390287d3dcb6 68 int main() {
JuanManuelAmador 0:390287d3dcb6 69 // Inicialización de puertos UART
JuanManuelAmador 0:390287d3dcb6 70 pc.baud(460800); // Configuración del puerto conectado al PC
JuanManuelAmador 0:390287d3dcb6 71 pIMU.baud(460800); // Configuración del puerto conectado a la IMU
JuanManuelAmador 0:390287d3dcb6 72 pLaser.baud(115200); // Configuración del puerto conectado al sensor de distancia
JuanManuelAmador 0:390287d3dcb6 73
JuanManuelAmador 0:390287d3dcb6 74 pc.printf("Listo para recibir datos.\n\r");
JuanManuelAmador 0:390287d3dcb6 75
JuanManuelAmador 0:390287d3dcb6 76 while(1) { // Bucle infinito
JuanManuelAmador 0:390287d3dcb6 77 // Se comprueba la llegada de nuevos bytes procedentes del PC
JuanManuelAmador 0:390287d3dcb6 78 if(pc.readable()){
JuanManuelAmador 0:390287d3dcb6 79 char c = pc.getc(); // Se lee el byte
JuanManuelAmador 0:390287d3dcb6 80 if(c == 'a'){ // Si es el carácter 'a', indica el inicio de la adquisición
JuanManuelAmador 0:390287d3dcb6 81 adquiriendo = true;
JuanManuelAmador 0:390287d3dcb6 82 t.reset();
JuanManuelAmador 0:390287d3dcb6 83 t.start();
JuanManuelAmador 0:390287d3dcb6 84 startStreamingIMU();
JuanManuelAmador 0:390287d3dcb6 85 bytesEnviados = 0;
JuanManuelAmador 0:390287d3dcb6 86 led = 1;
JuanManuelAmador 0:390287d3dcb6 87 }else if(c == 's'){ // Si es el carácter 'a', indica el final de la adquisición
JuanManuelAmador 0:390287d3dcb6 88 adquiriendo = false;
JuanManuelAmador 0:390287d3dcb6 89 led = 0;
JuanManuelAmador 0:390287d3dcb6 90 t.stop();
JuanManuelAmador 0:390287d3dcb6 91 t.reset();
JuanManuelAmador 0:390287d3dcb6 92 pc.printf("\n\nBytes enviados: %u\n\n", bytesEnviados);
JuanManuelAmador 0:390287d3dcb6 93 }
JuanManuelAmador 0:390287d3dcb6 94 }
JuanManuelAmador 0:390287d3dcb6 95
JuanManuelAmador 0:390287d3dcb6 96 // Se reciben los bytes de la IMU, se guardan en un buffer y se guarda una marca de tiempo si corresponde
JuanManuelAmador 0:390287d3dcb6 97 if(pIMU.readable()){
JuanManuelAmador 0:390287d3dcb6 98 byteINanterior = byteIN;
JuanManuelAmador 0:390287d3dcb6 99 byteIN = pIMU.getc();
JuanManuelAmador 0:390287d3dcb6 100 IMUbuff.put(byteIN); // Se guarda el byte en el buffer
JuanManuelAmador 0:390287d3dcb6 101 if(byteIN == 0x65 && byteINanterior == 0x75){// Si el byte recibido es un 75 en hex, este byte marca la llegada de una nueva medida
JuanManuelAmador 0:390287d3dcb6 102 // Se guarda la marca de tiempo
JuanManuelAmador 0:390287d3dcb6 103 IMUtime.put(t.read_us());
JuanManuelAmador 0:390287d3dcb6 104 }
JuanManuelAmador 0:390287d3dcb6 105 }
JuanManuelAmador 0:390287d3dcb6 106
JuanManuelAmador 0:390287d3dcb6 107 // Se reciben los bytes del LASER, se guardan en un buffer y se guarda una marca de tiempo si corresponde
JuanManuelAmador 0:390287d3dcb6 108 if(pLaser.readable()){
JuanManuelAmador 0:390287d3dcb6 109 byteINL = pLaser.getc();
JuanManuelAmador 0:390287d3dcb6 110 LASERbuff.put(byteINL); // Se guarda el byte en el buffer
JuanManuelAmador 0:390287d3dcb6 111 if(byteINL > 127){ // Los bytes con el bit más significativo a 1 son los primeros bytes d elos paquetes
JuanManuelAmador 0:390287d3dcb6 112 // por lo que se guarda la marca de tiempo correspondiente
JuanManuelAmador 0:390287d3dcb6 113 LASERtime.put(t.read_us());
JuanManuelAmador 0:390287d3dcb6 114 }
JuanManuelAmador 0:390287d3dcb6 115 }
JuanManuelAmador 0:390287d3dcb6 116
JuanManuelAmador 0:390287d3dcb6 117 if(adquiriendo){ // Sólo se reconstruye y envían datos cuando se ha iniciado la adquisición
JuanManuelAmador 0:390287d3dcb6 118 if(!IMUbuff.isEmpty() && enviandoLASER == false){ // Si no se está enviando un paquete del láser y existen bytes por leer
JuanManuelAmador 0:390287d3dcb6 119 byteINanteriorbuff = byteINbuff;
JuanManuelAmador 0:390287d3dcb6 120 byteINbuff = IMUbuff.get();
JuanManuelAmador 0:390287d3dcb6 121 // Reconstrucción del dato
JuanManuelAmador 0:390287d3dcb6 122 if(byteINbuff == 0x65 && byteINanteriorbuff == 0x75 && nBytes >= (TAMPAQUETEIMU-1)){ // Si el byte recibido es un 75 en hex, este byte marca la llegada de una nueva medida
JuanManuelAmador 0:390287d3dcb6 123 // Se pone el contador de bytes de un paquete a 0
JuanManuelAmador 0:390287d3dcb6 124 pc.putc('I'); // Se envían los dos bytes que indican el comienzo del paquete
JuanManuelAmador 0:390287d3dcb6 125 pc.putc('I'); //antes de enviar el primer bytes de datos
JuanManuelAmador 0:390287d3dcb6 126 nBytes = 1;
JuanManuelAmador 0:390287d3dcb6 127 enviandoIMU = true; // Se está enviando un paquete de IMU
JuanManuelAmador 0:390287d3dcb6 128
JuanManuelAmador 0:390287d3dcb6 129 }else{ // Todo lo que no sea 0x75 es parte del resto del dato
JuanManuelAmador 0:390287d3dcb6 130 // Se suma un byte mas
JuanManuelAmador 0:390287d3dcb6 131 nBytes++;
JuanManuelAmador 0:390287d3dcb6 132 // Para reconstruir el dato se mandan primero los bytes más significativos (que también es el orden en el que llegan)
JuanManuelAmador 0:390287d3dcb6 133 switch(nBytes){
JuanManuelAmador 0:390287d3dcb6 134 case 6:
JuanManuelAmador 0:390287d3dcb6 135 pc.putc(byteINbuff);
JuanManuelAmador 0:390287d3dcb6 136 break;
JuanManuelAmador 0:390287d3dcb6 137 case 7:
JuanManuelAmador 0:390287d3dcb6 138 pc.putc(byteINbuff);
JuanManuelAmador 0:390287d3dcb6 139 break;
JuanManuelAmador 0:390287d3dcb6 140 case 8:
JuanManuelAmador 0:390287d3dcb6 141 pc.putc(byteINbuff);
JuanManuelAmador 0:390287d3dcb6 142 break;
JuanManuelAmador 0:390287d3dcb6 143 case 9:
JuanManuelAmador 0:390287d3dcb6 144 pc.putc(byteINbuff);
JuanManuelAmador 0:390287d3dcb6 145 break;
JuanManuelAmador 0:390287d3dcb6 146 case 10:
JuanManuelAmador 0:390287d3dcb6 147 pc.putc(byteINbuff);
JuanManuelAmador 0:390287d3dcb6 148 break;
JuanManuelAmador 0:390287d3dcb6 149 case 11:
JuanManuelAmador 0:390287d3dcb6 150 pc.putc(byteINbuff);
JuanManuelAmador 0:390287d3dcb6 151 break;
JuanManuelAmador 0:390287d3dcb6 152 case 12:
JuanManuelAmador 0:390287d3dcb6 153 pc.putc(byteINbuff);
JuanManuelAmador 0:390287d3dcb6 154 break;
JuanManuelAmador 0:390287d3dcb6 155 case 13:
JuanManuelAmador 0:390287d3dcb6 156 pc.putc(byteINbuff);
JuanManuelAmador 0:390287d3dcb6 157 break;
JuanManuelAmador 0:390287d3dcb6 158 case 14:
JuanManuelAmador 0:390287d3dcb6 159 pc.putc(byteINbuff);
JuanManuelAmador 0:390287d3dcb6 160 break;
JuanManuelAmador 0:390287d3dcb6 161 case 15:
JuanManuelAmador 0:390287d3dcb6 162 pc.putc(byteINbuff);
JuanManuelAmador 0:390287d3dcb6 163 break;
JuanManuelAmador 0:390287d3dcb6 164 case 16:
JuanManuelAmador 0:390287d3dcb6 165 pc.putc(byteINbuff);
JuanManuelAmador 0:390287d3dcb6 166 break;
JuanManuelAmador 0:390287d3dcb6 167 case 17:
JuanManuelAmador 0:390287d3dcb6 168 pc.putc(byteINbuff);
JuanManuelAmador 0:390287d3dcb6 169 break;
JuanManuelAmador 0:390287d3dcb6 170 case 20:
JuanManuelAmador 0:390287d3dcb6 171 pc.putc(byteINbuff);
JuanManuelAmador 0:390287d3dcb6 172 break;
JuanManuelAmador 0:390287d3dcb6 173 case 21:
JuanManuelAmador 0:390287d3dcb6 174 pc.putc(byteINbuff);
JuanManuelAmador 0:390287d3dcb6 175 break;
JuanManuelAmador 0:390287d3dcb6 176 case 22:
JuanManuelAmador 0:390287d3dcb6 177 pc.putc(byteINbuff);
JuanManuelAmador 0:390287d3dcb6 178 break;
JuanManuelAmador 0:390287d3dcb6 179 case 23:
JuanManuelAmador 0:390287d3dcb6 180 pc.putc(byteINbuff);
JuanManuelAmador 0:390287d3dcb6 181 break;
JuanManuelAmador 0:390287d3dcb6 182 case 24:
JuanManuelAmador 0:390287d3dcb6 183 pc.putc(byteINbuff);
JuanManuelAmador 0:390287d3dcb6 184 break;
JuanManuelAmador 0:390287d3dcb6 185 case 25:
JuanManuelAmador 0:390287d3dcb6 186 pc.putc(byteINbuff);
JuanManuelAmador 0:390287d3dcb6 187 break;
JuanManuelAmador 0:390287d3dcb6 188 case 26:
JuanManuelAmador 0:390287d3dcb6 189 pc.putc(byteINbuff);
JuanManuelAmador 0:390287d3dcb6 190 break;
JuanManuelAmador 0:390287d3dcb6 191 case 27:
JuanManuelAmador 0:390287d3dcb6 192 pc.putc(byteINbuff);
JuanManuelAmador 0:390287d3dcb6 193 break;
JuanManuelAmador 0:390287d3dcb6 194 case 28:
JuanManuelAmador 0:390287d3dcb6 195 pc.putc(byteINbuff);
JuanManuelAmador 0:390287d3dcb6 196 break;
JuanManuelAmador 0:390287d3dcb6 197 case 29:
JuanManuelAmador 0:390287d3dcb6 198 pc.putc(byteINbuff);
JuanManuelAmador 0:390287d3dcb6 199 break;
JuanManuelAmador 0:390287d3dcb6 200 case 30:
JuanManuelAmador 0:390287d3dcb6 201 pc.putc(byteINbuff);
JuanManuelAmador 0:390287d3dcb6 202 break;
JuanManuelAmador 0:390287d3dcb6 203 case 31:
JuanManuelAmador 0:390287d3dcb6 204 pc.putc(byteINbuff);
JuanManuelAmador 0:390287d3dcb6 205 auxtime = IMUtime.get(); // Se recupera la marca de tiempo del paquete y se envía dividido en 4 bytes
JuanManuelAmador 0:390287d3dcb6 206 paqueteEnvio[3] = auxtime;
JuanManuelAmador 0:390287d3dcb6 207 auxtime >>= 8;
JuanManuelAmador 0:390287d3dcb6 208 paqueteEnvio[2] = auxtime;
JuanManuelAmador 0:390287d3dcb6 209 auxtime >>= 8;
JuanManuelAmador 0:390287d3dcb6 210 paqueteEnvio[1] = auxtime;
JuanManuelAmador 0:390287d3dcb6 211 auxtime >>= 8;
JuanManuelAmador 0:390287d3dcb6 212 paqueteEnvio[0] = auxtime;
JuanManuelAmador 0:390287d3dcb6 213 pc.putc(paqueteEnvio[0]);
JuanManuelAmador 0:390287d3dcb6 214 pc.putc(paqueteEnvio[1]);
JuanManuelAmador 0:390287d3dcb6 215 pc.putc(paqueteEnvio[2]);
JuanManuelAmador 0:390287d3dcb6 216 pc.putc(paqueteEnvio[3]);
JuanManuelAmador 0:390287d3dcb6 217 enviandoIMU = false; // Se ha terminado de enviar el paquete de la IMU
JuanManuelAmador 0:390287d3dcb6 218 bytesEnviados += TAMENVIOIMU;
JuanManuelAmador 0:390287d3dcb6 219 break;
JuanManuelAmador 0:390287d3dcb6 220 }
JuanManuelAmador 0:390287d3dcb6 221 }
JuanManuelAmador 0:390287d3dcb6 222 }
JuanManuelAmador 0:390287d3dcb6 223
JuanManuelAmador 0:390287d3dcb6 224 if(!LASERbuff.isEmpty() && enviandoIMU == false){
JuanManuelAmador 0:390287d3dcb6 225 byteINbuffL = LASERbuff.get();
JuanManuelAmador 0:390287d3dcb6 226 // Si el byte recibido tiene el bit más significativo a 1 es el byte más significativo
JuanManuelAmador 0:390287d3dcb6 227 if(byteINbuffL > 127){
JuanManuelAmador 0:390287d3dcb6 228 fbyte = 1;
JuanManuelAmador 0:390287d3dcb6 229 distancia = byteINbuffL;
JuanManuelAmador 0:390287d3dcb6 230 distancia &=~0x80; // El bit mas significativo hay que ponerlo a 0
JuanManuelAmador 0:390287d3dcb6 231 enviandoLASER = true; // Se comienza a enviar el paquete del láser
JuanManuelAmador 0:390287d3dcb6 232 }else{ // Recibido byte menos significativo
JuanManuelAmador 0:390287d3dcb6 233 if (fbyte == 1){ // Si se recibió un byte más significativo puede reconstruirse el valor medido por el laser
JuanManuelAmador 0:390287d3dcb6 234 auxtime = LASERtime.get(); // Se lee el tiempo cuando en el instante de llegada del primer byte del paquete
JuanManuelAmador 0:390287d3dcb6 235 fbyte = 0;
JuanManuelAmador 0:390287d3dcb6 236 distancia <<= 7;
JuanManuelAmador 0:390287d3dcb6 237 distancia += byteINbuffL; // Unidades de ingenieria
JuanManuelAmador 0:390287d3dcb6 238 //distancia = distancia*0.0498; // (mm) La conversión a mm se hace en el PC para no tener que enviar un tipo float
JuanManuelAmador 0:390287d3dcb6 239 // que guardara los decimales de la operación
JuanManuelAmador 0:390287d3dcb6 240 // Los dos primeros bytes byte de los paquetes del laser seran una 'L'
JuanManuelAmador 0:390287d3dcb6 241 paqueteEnvioL[0] = 'L';
JuanManuelAmador 0:390287d3dcb6 242 paqueteEnvioL[1] = 'L';
JuanManuelAmador 0:390287d3dcb6 243 paqueteEnvioL[3] = distancia;
JuanManuelAmador 0:390287d3dcb6 244 distancia >>= 8;
JuanManuelAmador 0:390287d3dcb6 245 paqueteEnvioL[2] = distancia;
JuanManuelAmador 0:390287d3dcb6 246 // Se añaden también los 4 bytes del tiempo y se envía el paquete (siempre el byte más significativo se envía primero)
JuanManuelAmador 0:390287d3dcb6 247 paqueteEnvioL[7] = auxtime;
JuanManuelAmador 0:390287d3dcb6 248 auxtime >>= 8;
JuanManuelAmador 0:390287d3dcb6 249 paqueteEnvioL[6] = auxtime;
JuanManuelAmador 0:390287d3dcb6 250 auxtime >>= 8;
JuanManuelAmador 0:390287d3dcb6 251 paqueteEnvioL[5] = auxtime;
JuanManuelAmador 0:390287d3dcb6 252 auxtime >>= 8;
JuanManuelAmador 0:390287d3dcb6 253 paqueteEnvioL[4] = auxtime;
JuanManuelAmador 0:390287d3dcb6 254 envioPaquete(paqueteEnvioL, TAMENVIOLASER);
JuanManuelAmador 0:390287d3dcb6 255 enviandoLASER = false; // Se ha terminado de enviar el paquete del láser
JuanManuelAmador 0:390287d3dcb6 256 bytesEnviados += TAMENVIOLASER;
JuanManuelAmador 0:390287d3dcb6 257 }
JuanManuelAmador 0:390287d3dcb6 258 }
JuanManuelAmador 0:390287d3dcb6 259
JuanManuelAmador 0:390287d3dcb6 260 }
JuanManuelAmador 0:390287d3dcb6 261 } // if(adquiriendo)
JuanManuelAmador 0:390287d3dcb6 262 } // while(1)
JuanManuelAmador 0:390287d3dcb6 263 } // main
JuanManuelAmador 0:390287d3dcb6 264
JuanManuelAmador 0:390287d3dcb6 265 void envioPaquete(unsigned char paquete[], int nElementos){ // Envía por el puerto serie un paquete de datos
JuanManuelAmador 0:390287d3dcb6 266 for(int i = 0; i < nElementos; i++){
JuanManuelAmador 0:390287d3dcb6 267 pc.putc(paquete[i]);
JuanManuelAmador 0:390287d3dcb6 268 }
JuanManuelAmador 0:390287d3dcb6 269 }
JuanManuelAmador 0:390287d3dcb6 270
JuanManuelAmador 0:390287d3dcb6 271 void startStreamingIMU(){
JuanManuelAmador 0:390287d3dcb6 272 // Se envia un paquete de datos a la IMU que indica el comienzo de lectura de medidas
JuanManuelAmador 0:390287d3dcb6 273 pIMU.putc(0x75); // K
JuanManuelAmador 0:390287d3dcb6 274 pIMU.putc(0x65); // A
JuanManuelAmador 0:390287d3dcb6 275 pIMU.putc(0x0C); // alt + 12 ♀
JuanManuelAmador 0:390287d3dcb6 276 pIMU.putc(0x05); // alt + 5 ♣
JuanManuelAmador 0:390287d3dcb6 277 pIMU.putc(0x05); // alt + 5 ♣
JuanManuelAmador 0:390287d3dcb6 278 pIMU.putc(0x11); // alt + 11 ♂
JuanManuelAmador 0:390287d3dcb6 279 pIMU.putc(0x01); // alt + 1 ☺
JuanManuelAmador 0:390287d3dcb6 280 pIMU.putc(0x01); // alt + 1 ☺
JuanManuelAmador 0:390287d3dcb6 281 pIMU.putc(0x01); // alt + 1 ☺
JuanManuelAmador 0:390287d3dcb6 282 pIMU.putc(0x04); // alt + 4 ♦
JuanManuelAmador 0:390287d3dcb6 283 pIMU.putc(0x1A); // alt + 26 →
JuanManuelAmador 0:390287d3dcb6 284 pc.printf("Paquete de inicio enviado.\n\r");
JuanManuelAmador 0:390287d3dcb6 285 }