virtualmech
Programa mbed1 excelencia
Dependencies: mbed
excelenciaSP1.cpp@0:a5908bca4740, 2017-01-18 (annotated)
- Committer:
- JuanManuelAmador
- Date:
- Wed Jan 18 12:58:15 2017 +0000
- Revision:
- 0:a5908bca4740
primer commit
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| JuanManuelAmador | 0:a5908bca4740 | 1 | /*************************************** |
| JuanManuelAmador | 0:a5908bca4740 | 2 | VIRTUALMECH |
| JuanManuelAmador | 0:a5908bca4740 | 3 | Autor: Juan Manuel Amador Olivares |
| JuanManuelAmador | 0:a5908bca4740 | 4 | Fecha: 16/9/2015 |
| JuanManuelAmador | 0:a5908bca4740 | 5 | ***************************************/ |
| JuanManuelAmador | 0:a5908bca4740 | 6 | |
| JuanManuelAmador | 0:a5908bca4740 | 7 | /*************************************** |
| JuanManuelAmador | 0:a5908bca4740 | 8 | Adquisición de datos IMU y Láser |
| JuanManuelAmador | 0:a5908bca4740 | 9 | ***************************************/ |
| JuanManuelAmador | 0:a5908bca4740 | 10 | #include "mbed.h" |
| JuanManuelAmador | 0:a5908bca4740 | 11 | #include "Buffering.h" |
| JuanManuelAmador | 0:a5908bca4740 | 12 | #include "BufferBig.h" |
| JuanManuelAmador | 0:a5908bca4740 | 13 | #include "Bufferinguint.h" |
| JuanManuelAmador | 0:a5908bca4740 | 14 | |
| JuanManuelAmador | 0:a5908bca4740 | 15 | #define TAMPAQUETEIMU 34 // Tamaño del paquete recibido de la IMU |
| JuanManuelAmador | 0:a5908bca4740 | 16 | #define TAMENVIOIMU 30 // Tamaño del paquete enviado al PC con los datos de la IMU |
| JuanManuelAmador | 0:a5908bca4740 | 17 | #define TAMENVIOLASER 8 // Tamaño del paquete enviado al PC con los datos del sensor de distancia |
| JuanManuelAmador | 0:a5908bca4740 | 18 | |
| JuanManuelAmador | 0:a5908bca4740 | 19 | /******************** |
| JuanManuelAmador | 0:a5908bca4740 | 20 | Funciones |
| JuanManuelAmador | 0:a5908bca4740 | 21 | ********************/ |
| JuanManuelAmador | 0:a5908bca4740 | 22 | void startStreamingIMU(); // Manda los bytes a la IMU necesarios para que comience a enviar datos por streaming |
| JuanManuelAmador | 0:a5908bca4740 | 23 | void envioPaquete(unsigned char paquete[], int nElementos); // Envía por el puerto serie un paquete de datos |
| JuanManuelAmador | 0:a5908bca4740 | 24 | void envioDatos(); // Envio al PC los bytes de datos cuando es posible |
| JuanManuelAmador | 0:a5908bca4740 | 25 | void flushSerialBuffer1(void); |
| JuanManuelAmador | 0:a5908bca4740 | 26 | void flushSerialBuffer2(void); |
| JuanManuelAmador | 0:a5908bca4740 | 27 | void flushSerialBuffer3(void); |
| JuanManuelAmador | 0:a5908bca4740 | 28 | /********************/ |
| JuanManuelAmador | 0:a5908bca4740 | 29 | |
| JuanManuelAmador | 0:a5908bca4740 | 30 | DigitalOut led(LED1); // Led indicador. Se enciende cuando se están adquiriendo datos |
| JuanManuelAmador | 0:a5908bca4740 | 31 | |
| JuanManuelAmador | 0:a5908bca4740 | 32 | // Puertos UART a utilizar |
| JuanManuelAmador | 0:a5908bca4740 | 33 | Serial pLaser(p9, p10); |
| JuanManuelAmador | 0:a5908bca4740 | 34 | Serial pIMU(p28, p27); |
| JuanManuelAmador | 0:a5908bca4740 | 35 | Serial pIMU2(p13, p14); |
| JuanManuelAmador | 0:a5908bca4740 | 36 | Serial pc(USBTX, USBRX); |
| JuanManuelAmador | 0:a5908bca4740 | 37 | |
| JuanManuelAmador | 0:a5908bca4740 | 38 | // Temporizador para el control del tiempo |
| JuanManuelAmador | 0:a5908bca4740 | 39 | Timer t; |
| JuanManuelAmador | 0:a5908bca4740 | 40 | unsigned int auxtime = 0; // Variable auxiliar para la lectura del tiempo |
| JuanManuelAmador | 0:a5908bca4740 | 41 | unsigned char byteIN, byteINanterior, byteINbuff, byteINanteriorbuff; // Variables auxiliares para guardar bytes recibidos de la IMU |
| JuanManuelAmador | 0:a5908bca4740 | 42 | unsigned char byteIN2, byteINanterior2, byteINbuff2, byteINanteriorbuff2; // Variables auxiliares para guardar bytes recibidos de la IMU2 |
| JuanManuelAmador | 0:a5908bca4740 | 43 | unsigned char byteINL, byteINbuffL; // Variable auxiliar para guardar bytes recibidos del laser |
| JuanManuelAmador | 0:a5908bca4740 | 44 | |
| JuanManuelAmador | 0:a5908bca4740 | 45 | // Variables para la reconstrucción de los datos de la IMU |
| JuanManuelAmador | 0:a5908bca4740 | 46 | unsigned int nBytes = TAMPAQUETEIMU; |
| JuanManuelAmador | 0:a5908bca4740 | 47 | 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:a5908bca4740 | 48 | |
| JuanManuelAmador | 0:a5908bca4740 | 49 | // Variables para la reconstrucción de los datos de la IMU2 |
| JuanManuelAmador | 0:a5908bca4740 | 50 | unsigned int nBytes2 = TAMPAQUETEIMU; |
| JuanManuelAmador | 0:a5908bca4740 | 51 | unsigned char paqueteEnvio2[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 IMU2 |
| JuanManuelAmador | 0:a5908bca4740 | 52 | /*union datoCompuesto{ |
| JuanManuelAmador | 0:a5908bca4740 | 53 | char B[4]; |
| JuanManuelAmador | 0:a5908bca4740 | 54 | float unidos; |
| JuanManuelAmador | 0:a5908bca4740 | 55 | } dato;*/ |
| JuanManuelAmador | 0:a5908bca4740 | 56 | |
| JuanManuelAmador | 0:a5908bca4740 | 57 | // Variables para la reconstrucción datos del láser |
| JuanManuelAmador | 0:a5908bca4740 | 58 | unsigned int distancia; // Sólo se usarán los 2 bytes menos significativos |
| JuanManuelAmador | 0:a5908bca4740 | 59 | unsigned char paqueteEnvioL[TAMENVIOLASER]; |
| JuanManuelAmador | 0:a5908bca4740 | 60 | 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:a5908bca4740 | 61 | |
| JuanManuelAmador | 0:a5908bca4740 | 62 | // Buffers dónde se guardarán los bytes provenientes de los sensores |
| JuanManuelAmador | 0:a5908bca4740 | 63 | Buffering IMUbuff; |
| JuanManuelAmador | 0:a5908bca4740 | 64 | Bufferinguint IMUtime; |
| JuanManuelAmador | 0:a5908bca4740 | 65 | Buffering IMUbuff2; |
| JuanManuelAmador | 0:a5908bca4740 | 66 | Bufferinguint IMUtime2; |
| JuanManuelAmador | 0:a5908bca4740 | 67 | Buffering LASERbuff; |
| JuanManuelAmador | 0:a5908bca4740 | 68 | Bufferinguint LASERtime; |
| JuanManuelAmador | 0:a5908bca4740 | 69 | |
| JuanManuelAmador | 0:a5908bca4740 | 70 | // Buffer dónde se guardarán los datos antes de ser enviados |
| JuanManuelAmador | 0:a5908bca4740 | 71 | BufferBig envioBuff; |
| JuanManuelAmador | 0:a5908bca4740 | 72 | char continuaEnviando; |
| JuanManuelAmador | 0:a5908bca4740 | 73 | |
| JuanManuelAmador | 0:a5908bca4740 | 74 | // Variables semáforo para que no se mezclen los bytes de distintos paquetes |
| JuanManuelAmador | 0:a5908bca4740 | 75 | // Estas variables indican de que sensor se pueden leer y procesar los bytes de los paquetes recibidos. |
| JuanManuelAmador | 0:a5908bca4740 | 76 | // Independientemente de estas variables se recibirán los bytes de los distintos sensores, que se irán guardando en sus respectivos buffer, |
| JuanManuelAmador | 0:a5908bca4740 | 77 | // y se irá guardando la marca de tiempo cuando se detecte la llegada de un nuevo paquete. |
| JuanManuelAmador | 0:a5908bca4740 | 78 | bool enviandoIMU = false; |
| JuanManuelAmador | 0:a5908bca4740 | 79 | bool enviandoIMU2 = false; |
| JuanManuelAmador | 0:a5908bca4740 | 80 | bool enviandoLASER = false; |
| JuanManuelAmador | 0:a5908bca4740 | 81 | bool adquiriendo = false; |
| JuanManuelAmador | 0:a5908bca4740 | 82 | |
| JuanManuelAmador | 0:a5908bca4740 | 83 | // Variable de control de bytes enviados |
| JuanManuelAmador | 0:a5908bca4740 | 84 | unsigned int bytesEnviados; |
| JuanManuelAmador | 0:a5908bca4740 | 85 | unsigned int selIMUleer; |
| JuanManuelAmador | 0:a5908bca4740 | 86 | |
| JuanManuelAmador | 0:a5908bca4740 | 87 | int main() { |
| JuanManuelAmador | 0:a5908bca4740 | 88 | // Inicialización de puertos UART |
| JuanManuelAmador | 0:a5908bca4740 | 89 | pc.baud(921600); // Configuración del puerto conectado al PC |
| JuanManuelAmador | 0:a5908bca4740 | 90 | pIMU.baud(460800); // Configuración del puerto conectado a la IMU |
| JuanManuelAmador | 0:a5908bca4740 | 91 | pIMU2.baud(460800); // Configuración del puerto conectado a la IMU2 |
| JuanManuelAmador | 0:a5908bca4740 | 92 | pLaser.baud(115200); // Configuración del puerto conectado al sensor de distancia |
| JuanManuelAmador | 0:a5908bca4740 | 93 | |
| JuanManuelAmador | 0:a5908bca4740 | 94 | //pc.printf("Listo para recibir datos.\n\r"); |
| JuanManuelAmador | 0:a5908bca4740 | 95 | |
| JuanManuelAmador | 0:a5908bca4740 | 96 | while(1) { // Bucle infinito |
| JuanManuelAmador | 0:a5908bca4740 | 97 | // Se comprueba la llegada de nuevos bytes procedentes del PC |
| JuanManuelAmador | 0:a5908bca4740 | 98 | if(pc.readable()){ |
| JuanManuelAmador | 0:a5908bca4740 | 99 | char c = pc.getc(); // Se lee el byte |
| JuanManuelAmador | 0:a5908bca4740 | 100 | if(c == 'a'){ // Si es el carácter 'a', indica el inicio de la adquisición |
| JuanManuelAmador | 0:a5908bca4740 | 101 | adquiriendo = true; |
| JuanManuelAmador | 0:a5908bca4740 | 102 | t.reset(); |
| JuanManuelAmador | 0:a5908bca4740 | 103 | t.start(); |
| JuanManuelAmador | 0:a5908bca4740 | 104 | startStreamingIMU(); |
| JuanManuelAmador | 0:a5908bca4740 | 105 | bytesEnviados = 0; |
| JuanManuelAmador | 0:a5908bca4740 | 106 | led = 1; |
| JuanManuelAmador | 0:a5908bca4740 | 107 | }else if(c == 's'){ // Si es el carácter 'a', indica el final de la adquisición |
| JuanManuelAmador | 0:a5908bca4740 | 108 | adquiriendo = false; |
| JuanManuelAmador | 0:a5908bca4740 | 109 | led = 0; |
| JuanManuelAmador | 0:a5908bca4740 | 110 | t.stop(); |
| JuanManuelAmador | 0:a5908bca4740 | 111 | t.reset(); |
| JuanManuelAmador | 0:a5908bca4740 | 112 | |
| JuanManuelAmador | 0:a5908bca4740 | 113 | // Puertos serie |
| JuanManuelAmador | 0:a5908bca4740 | 114 | flushSerialBuffer1(); |
| JuanManuelAmador | 0:a5908bca4740 | 115 | flushSerialBuffer2(); |
| JuanManuelAmador | 0:a5908bca4740 | 116 | flushSerialBuffer3(); |
| JuanManuelAmador | 0:a5908bca4740 | 117 | |
| JuanManuelAmador | 0:a5908bca4740 | 118 | // Buffer en RAM de mbed |
| JuanManuelAmador | 0:a5908bca4740 | 119 | while(!IMUbuff.isEmpty()){ |
| JuanManuelAmador | 0:a5908bca4740 | 120 | IMUbuff.get(); |
| JuanManuelAmador | 0:a5908bca4740 | 121 | } |
| JuanManuelAmador | 0:a5908bca4740 | 122 | while(!IMUtime.isEmpty()){ |
| JuanManuelAmador | 0:a5908bca4740 | 123 | IMUtime.get(); |
| JuanManuelAmador | 0:a5908bca4740 | 124 | } |
| JuanManuelAmador | 0:a5908bca4740 | 125 | while(!IMUbuff2.isEmpty()){ |
| JuanManuelAmador | 0:a5908bca4740 | 126 | IMUbuff2.get(); |
| JuanManuelAmador | 0:a5908bca4740 | 127 | } |
| JuanManuelAmador | 0:a5908bca4740 | 128 | while(!IMUtime2.isEmpty()){ |
| JuanManuelAmador | 0:a5908bca4740 | 129 | IMUtime2.get(); |
| JuanManuelAmador | 0:a5908bca4740 | 130 | } |
| JuanManuelAmador | 0:a5908bca4740 | 131 | while(!LASERbuff.isEmpty()){ |
| JuanManuelAmador | 0:a5908bca4740 | 132 | LASERbuff.get(); |
| JuanManuelAmador | 0:a5908bca4740 | 133 | } |
| JuanManuelAmador | 0:a5908bca4740 | 134 | while(!LASERtime.isEmpty()){ |
| JuanManuelAmador | 0:a5908bca4740 | 135 | LASERtime.get(); |
| JuanManuelAmador | 0:a5908bca4740 | 136 | } |
| JuanManuelAmador | 0:a5908bca4740 | 137 | while(!envioBuff.isEmpty()){ |
| JuanManuelAmador | 0:a5908bca4740 | 138 | envioBuff.get(); |
| JuanManuelAmador | 0:a5908bca4740 | 139 | } |
| JuanManuelAmador | 0:a5908bca4740 | 140 | /*pc.printf("\n\nBytes enviados: %u\n\n", bytesEnviados); |
| JuanManuelAmador | 0:a5908bca4740 | 141 | pc.printf("\n\nDatos en el buffer de envio: %u, %u, %u, %u\n\n", envioBuff.getDif(),IMUbuff.getDif(), IMUbuff2.getDif(),LASERbuff.getDif());*/ |
| JuanManuelAmador | 0:a5908bca4740 | 142 | } |
| JuanManuelAmador | 0:a5908bca4740 | 143 | } |
| JuanManuelAmador | 0:a5908bca4740 | 144 | if(adquiriendo){ // Sólo se reconstruye y envían datos cuando se ha iniciado la adquisición |
| JuanManuelAmador | 0:a5908bca4740 | 145 | |
| JuanManuelAmador | 0:a5908bca4740 | 146 | if(pIMU.readable()){ |
| JuanManuelAmador | 0:a5908bca4740 | 147 | byteINanterior = byteIN; |
| JuanManuelAmador | 0:a5908bca4740 | 148 | byteIN = pIMU.getc(); |
| JuanManuelAmador | 0:a5908bca4740 | 149 | IMUbuff.put(byteIN); // Se guarda el byte en el buffer |
| JuanManuelAmador | 0:a5908bca4740 | 150 | 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:a5908bca4740 | 151 | // Se guarda la marca de tiempo |
| JuanManuelAmador | 0:a5908bca4740 | 152 | IMUtime.put(t.read_us()); |
| JuanManuelAmador | 0:a5908bca4740 | 153 | } |
| JuanManuelAmador | 0:a5908bca4740 | 154 | } |
| JuanManuelAmador | 0:a5908bca4740 | 155 | |
| JuanManuelAmador | 0:a5908bca4740 | 156 | // Se reciben los bytes de la IMU2, se guardan en un buffer y se guarda una marca de tiempo si corresponde |
| JuanManuelAmador | 0:a5908bca4740 | 157 | if(pIMU2.readable()){ |
| JuanManuelAmador | 0:a5908bca4740 | 158 | byteINanterior2 = byteIN2; |
| JuanManuelAmador | 0:a5908bca4740 | 159 | byteIN2 = pIMU2.getc(); |
| JuanManuelAmador | 0:a5908bca4740 | 160 | IMUbuff2.put(byteIN2); // Se guarda el byte en el buffer |
| JuanManuelAmador | 0:a5908bca4740 | 161 | if(byteIN2 == 0x65 && byteINanterior2 == 0x75){// Si el byte recibido es un 75 en hex, este byte marca la llegada de una nueva medida |
| JuanManuelAmador | 0:a5908bca4740 | 162 | // Se guarda la marca de tiempo |
| JuanManuelAmador | 0:a5908bca4740 | 163 | IMUtime2.put(t.read_us()); |
| JuanManuelAmador | 0:a5908bca4740 | 164 | } |
| JuanManuelAmador | 0:a5908bca4740 | 165 | } |
| JuanManuelAmador | 0:a5908bca4740 | 166 | |
| JuanManuelAmador | 0:a5908bca4740 | 167 | // Se reciben los bytes del LASER, se guardan en un buffer y se guarda una marca de tiempo si corresponde |
| JuanManuelAmador | 0:a5908bca4740 | 168 | if(pLaser.readable()){ |
| JuanManuelAmador | 0:a5908bca4740 | 169 | byteINL = pLaser.getc(); |
| JuanManuelAmador | 0:a5908bca4740 | 170 | LASERbuff.put(byteINL); // Se guarda el byte en el buffer |
| JuanManuelAmador | 0:a5908bca4740 | 171 | if(byteINL > 127){ // Los bytes con el bit más significativo a 1 son los primeros bytes d elos paquetes |
| JuanManuelAmador | 0:a5908bca4740 | 172 | // por lo que se guarda la marca de tiempo correspondiente |
| JuanManuelAmador | 0:a5908bca4740 | 173 | LASERtime.put(t.read_us()); |
| JuanManuelAmador | 0:a5908bca4740 | 174 | } |
| JuanManuelAmador | 0:a5908bca4740 | 175 | } |
| JuanManuelAmador | 0:a5908bca4740 | 176 | |
| JuanManuelAmador | 0:a5908bca4740 | 177 | if(!IMUbuff.isEmpty() && enviandoIMU2 == false && enviandoLASER == false){ // Si no se está enviando un paquete del láser y existen bytes por leer |
| JuanManuelAmador | 0:a5908bca4740 | 178 | byteINanteriorbuff = byteINbuff; |
| JuanManuelAmador | 0:a5908bca4740 | 179 | byteINbuff = IMUbuff.get(); |
| JuanManuelAmador | 0:a5908bca4740 | 180 | // Reconstrucción del dato |
| JuanManuelAmador | 0:a5908bca4740 | 181 | 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:a5908bca4740 | 182 | // Se pone el contador de bytes de un paquete a 0 |
| JuanManuelAmador | 0:a5908bca4740 | 183 | //pc.putc('I'); // Se envían los dos bytes que indican el comienzo del paquete |
| JuanManuelAmador | 0:a5908bca4740 | 184 | //pc.putc('I'); //antes de enviar el primer bytes de datos |
| JuanManuelAmador | 0:a5908bca4740 | 185 | envioBuff.put('I'); |
| JuanManuelAmador | 0:a5908bca4740 | 186 | envioBuff.put('I'); |
| JuanManuelAmador | 0:a5908bca4740 | 187 | nBytes = 1; |
| JuanManuelAmador | 0:a5908bca4740 | 188 | enviandoIMU = true; // Se está enviando un paquete de IMU |
| JuanManuelAmador | 0:a5908bca4740 | 189 | |
| JuanManuelAmador | 0:a5908bca4740 | 190 | }else{ // Todo lo que no sea 0x75 es parte del resto del dato |
| JuanManuelAmador | 0:a5908bca4740 | 191 | // Se suma un byte mas |
| JuanManuelAmador | 0:a5908bca4740 | 192 | nBytes++; |
| JuanManuelAmador | 0:a5908bca4740 | 193 | // Para reconstruir el dato se mandan primero los bytes más significativos (que también es el orden en el que llegan) |
| JuanManuelAmador | 0:a5908bca4740 | 194 | switch(nBytes){ |
| JuanManuelAmador | 0:a5908bca4740 | 195 | case 6: |
| JuanManuelAmador | 0:a5908bca4740 | 196 | //pc.putc(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 197 | envioBuff.put(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 198 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 199 | case 7: |
| JuanManuelAmador | 0:a5908bca4740 | 200 | //pc.putc(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 201 | envioBuff.put(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 202 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 203 | case 8: |
| JuanManuelAmador | 0:a5908bca4740 | 204 | //pc.putc(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 205 | envioBuff.put(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 206 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 207 | case 9: |
| JuanManuelAmador | 0:a5908bca4740 | 208 | //pc.putc(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 209 | envioBuff.put(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 210 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 211 | case 10: |
| JuanManuelAmador | 0:a5908bca4740 | 212 | //pc.putc(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 213 | envioBuff.put(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 214 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 215 | case 11: |
| JuanManuelAmador | 0:a5908bca4740 | 216 | //pc.putc(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 217 | envioBuff.put(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 218 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 219 | case 12: |
| JuanManuelAmador | 0:a5908bca4740 | 220 | //pc.putc(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 221 | envioBuff.put(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 222 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 223 | case 13: |
| JuanManuelAmador | 0:a5908bca4740 | 224 | //pc.putc(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 225 | envioBuff.put(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 226 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 227 | case 14: |
| JuanManuelAmador | 0:a5908bca4740 | 228 | //pc.putc(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 229 | envioBuff.put(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 230 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 231 | case 15: |
| JuanManuelAmador | 0:a5908bca4740 | 232 | //pc.putc(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 233 | envioBuff.put(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 234 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 235 | case 16: |
| JuanManuelAmador | 0:a5908bca4740 | 236 | //pc.putc(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 237 | envioBuff.put(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 238 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 239 | case 17: |
| JuanManuelAmador | 0:a5908bca4740 | 240 | //pc.putc(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 241 | envioBuff.put(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 242 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 243 | case 20: |
| JuanManuelAmador | 0:a5908bca4740 | 244 | //pc.putc(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 245 | envioBuff.put(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 246 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 247 | case 21: |
| JuanManuelAmador | 0:a5908bca4740 | 248 | //pc.putc(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 249 | envioBuff.put(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 250 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 251 | case 22: |
| JuanManuelAmador | 0:a5908bca4740 | 252 | //pc.putc(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 253 | envioBuff.put(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 254 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 255 | case 23: |
| JuanManuelAmador | 0:a5908bca4740 | 256 | //pc.putc(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 257 | envioBuff.put(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 258 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 259 | case 24: |
| JuanManuelAmador | 0:a5908bca4740 | 260 | //pc.putc(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 261 | envioBuff.put(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 262 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 263 | case 25: |
| JuanManuelAmador | 0:a5908bca4740 | 264 | //pc.putc(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 265 | envioBuff.put(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 266 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 267 | case 26: |
| JuanManuelAmador | 0:a5908bca4740 | 268 | //pc.putc(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 269 | envioBuff.put(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 270 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 271 | case 27: |
| JuanManuelAmador | 0:a5908bca4740 | 272 | //pc.putc(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 273 | envioBuff.put(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 274 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 275 | case 28: |
| JuanManuelAmador | 0:a5908bca4740 | 276 | //pc.putc(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 277 | envioBuff.put(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 278 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 279 | case 29: |
| JuanManuelAmador | 0:a5908bca4740 | 280 | //pc.putc(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 281 | envioBuff.put(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 282 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 283 | case 30: |
| JuanManuelAmador | 0:a5908bca4740 | 284 | //pc.putc(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 285 | envioBuff.put(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 286 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 287 | case 31: |
| JuanManuelAmador | 0:a5908bca4740 | 288 | //pc.putc(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 289 | envioBuff.put(byteINbuff); |
| JuanManuelAmador | 0:a5908bca4740 | 290 | |
| JuanManuelAmador | 0:a5908bca4740 | 291 | auxtime = IMUtime.get(); // Se recupera la marca de tiempo del paquete y se envía dividido en 4 bytes |
| JuanManuelAmador | 0:a5908bca4740 | 292 | paqueteEnvio[3] = auxtime; |
| JuanManuelAmador | 0:a5908bca4740 | 293 | auxtime >>= 8; |
| JuanManuelAmador | 0:a5908bca4740 | 294 | paqueteEnvio[2] = auxtime; |
| JuanManuelAmador | 0:a5908bca4740 | 295 | auxtime >>= 8; |
| JuanManuelAmador | 0:a5908bca4740 | 296 | paqueteEnvio[1] = auxtime; |
| JuanManuelAmador | 0:a5908bca4740 | 297 | auxtime >>= 8; |
| JuanManuelAmador | 0:a5908bca4740 | 298 | paqueteEnvio[0] = auxtime; |
| JuanManuelAmador | 0:a5908bca4740 | 299 | /*pc.putc(paqueteEnvio[0]); |
| JuanManuelAmador | 0:a5908bca4740 | 300 | pc.putc(paqueteEnvio[1]); |
| JuanManuelAmador | 0:a5908bca4740 | 301 | pc.putc(paqueteEnvio[2]); |
| JuanManuelAmador | 0:a5908bca4740 | 302 | pc.putc(paqueteEnvio[3]);*/ |
| JuanManuelAmador | 0:a5908bca4740 | 303 | envioBuff.put(paqueteEnvio[0]); |
| JuanManuelAmador | 0:a5908bca4740 | 304 | envioBuff.put(paqueteEnvio[1]); |
| JuanManuelAmador | 0:a5908bca4740 | 305 | envioBuff.put(paqueteEnvio[2]); |
| JuanManuelAmador | 0:a5908bca4740 | 306 | envioBuff.put(paqueteEnvio[3]); |
| JuanManuelAmador | 0:a5908bca4740 | 307 | enviandoIMU = false; // Se ha terminado de enviar el paquete de la IMU |
| JuanManuelAmador | 0:a5908bca4740 | 308 | bytesEnviados += TAMENVIOIMU; |
| JuanManuelAmador | 0:a5908bca4740 | 309 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 310 | } |
| JuanManuelAmador | 0:a5908bca4740 | 311 | } |
| JuanManuelAmador | 0:a5908bca4740 | 312 | } |
| JuanManuelAmador | 0:a5908bca4740 | 313 | |
| JuanManuelAmador | 0:a5908bca4740 | 314 | if(!IMUbuff2.isEmpty() && enviandoIMU == false && enviandoLASER == false){ // Si no se está enviando un paquete del láser y existen bytes por leer |
| JuanManuelAmador | 0:a5908bca4740 | 315 | byteINanteriorbuff2 = byteINbuff2; |
| JuanManuelAmador | 0:a5908bca4740 | 316 | byteINbuff2 = IMUbuff2.get(); |
| JuanManuelAmador | 0:a5908bca4740 | 317 | // Reconstrucción del dato |
| JuanManuelAmador | 0:a5908bca4740 | 318 | if(byteINbuff2 == 0x65 && byteINanteriorbuff2 == 0x75 && nBytes2 >= (TAMPAQUETEIMU-1)){ // Si el byte recibido es un 75 en hex, este byte marca la llegada de una nueva medida |
| JuanManuelAmador | 0:a5908bca4740 | 319 | // Se pone el contador de bytes de un paquete a 0 |
| JuanManuelAmador | 0:a5908bca4740 | 320 | //pc.putc('I'); // Se envían los dos bytes que indican el comienzo del paquete |
| JuanManuelAmador | 0:a5908bca4740 | 321 | //pc.putc('I'); //antes de enviar el primer bytes de datos |
| JuanManuelAmador | 0:a5908bca4740 | 322 | envioBuff.put('H'); |
| JuanManuelAmador | 0:a5908bca4740 | 323 | envioBuff.put('H'); |
| JuanManuelAmador | 0:a5908bca4740 | 324 | nBytes2 = 1; |
| JuanManuelAmador | 0:a5908bca4740 | 325 | enviandoIMU2 = true; // Se está enviando un paquete de IMU |
| JuanManuelAmador | 0:a5908bca4740 | 326 | |
| JuanManuelAmador | 0:a5908bca4740 | 327 | }else{ // Todo lo que no sea 0x75 es parte del resto del dato |
| JuanManuelAmador | 0:a5908bca4740 | 328 | // Se suma un byte mas |
| JuanManuelAmador | 0:a5908bca4740 | 329 | nBytes2++; |
| JuanManuelAmador | 0:a5908bca4740 | 330 | // Para reconstruir el dato se mandan primero los bytes más significativos (que también es el orden en el que llegan) |
| JuanManuelAmador | 0:a5908bca4740 | 331 | switch(nBytes2){ |
| JuanManuelAmador | 0:a5908bca4740 | 332 | case 6: |
| JuanManuelAmador | 0:a5908bca4740 | 333 | //pc.putc(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 334 | envioBuff.put(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 335 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 336 | case 7: |
| JuanManuelAmador | 0:a5908bca4740 | 337 | //pc.putc(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 338 | envioBuff.put(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 339 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 340 | case 8: |
| JuanManuelAmador | 0:a5908bca4740 | 341 | //pc.putc(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 342 | envioBuff.put(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 343 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 344 | case 9: |
| JuanManuelAmador | 0:a5908bca4740 | 345 | //pc.putc(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 346 | envioBuff.put(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 347 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 348 | case 10: |
| JuanManuelAmador | 0:a5908bca4740 | 349 | //pc.putc(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 350 | envioBuff.put(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 351 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 352 | case 11: |
| JuanManuelAmador | 0:a5908bca4740 | 353 | //pc.putc(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 354 | envioBuff.put(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 355 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 356 | case 12: |
| JuanManuelAmador | 0:a5908bca4740 | 357 | //pc.putc(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 358 | envioBuff.put(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 359 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 360 | case 13: |
| JuanManuelAmador | 0:a5908bca4740 | 361 | //pc.putc(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 362 | envioBuff.put(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 363 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 364 | case 14: |
| JuanManuelAmador | 0:a5908bca4740 | 365 | //pc.putc(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 366 | envioBuff.put(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 367 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 368 | case 15: |
| JuanManuelAmador | 0:a5908bca4740 | 369 | //pc.putc(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 370 | envioBuff.put(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 371 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 372 | case 16: |
| JuanManuelAmador | 0:a5908bca4740 | 373 | //pc.putc(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 374 | envioBuff.put(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 375 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 376 | case 17: |
| JuanManuelAmador | 0:a5908bca4740 | 377 | //pc.putc(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 378 | envioBuff.put(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 379 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 380 | case 20: |
| JuanManuelAmador | 0:a5908bca4740 | 381 | //pc.putc(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 382 | envioBuff.put(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 383 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 384 | case 21: |
| JuanManuelAmador | 0:a5908bca4740 | 385 | //pc.putc(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 386 | envioBuff.put(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 387 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 388 | case 22: |
| JuanManuelAmador | 0:a5908bca4740 | 389 | //pc.putc(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 390 | envioBuff.put(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 391 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 392 | case 23: |
| JuanManuelAmador | 0:a5908bca4740 | 393 | //pc.putc(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 394 | envioBuff.put(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 395 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 396 | case 24: |
| JuanManuelAmador | 0:a5908bca4740 | 397 | //pc.putc(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 398 | envioBuff.put(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 399 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 400 | case 25: |
| JuanManuelAmador | 0:a5908bca4740 | 401 | //pc.putc(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 402 | envioBuff.put(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 403 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 404 | case 26: |
| JuanManuelAmador | 0:a5908bca4740 | 405 | //pc.putc(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 406 | envioBuff.put(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 407 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 408 | case 27: |
| JuanManuelAmador | 0:a5908bca4740 | 409 | //pc.putc(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 410 | envioBuff.put(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 411 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 412 | case 28: |
| JuanManuelAmador | 0:a5908bca4740 | 413 | //pc.putc(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 414 | envioBuff.put(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 415 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 416 | case 29: |
| JuanManuelAmador | 0:a5908bca4740 | 417 | //pc.putc(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 418 | envioBuff.put(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 419 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 420 | case 30: |
| JuanManuelAmador | 0:a5908bca4740 | 421 | //pc.putc(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 422 | envioBuff.put(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 423 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 424 | case 31: |
| JuanManuelAmador | 0:a5908bca4740 | 425 | //pc.putc(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 426 | envioBuff.put(byteINbuff2); |
| JuanManuelAmador | 0:a5908bca4740 | 427 | |
| JuanManuelAmador | 0:a5908bca4740 | 428 | auxtime = IMUtime2.get(); // Se recupera la marca de tiempo del paquete y se envía dividido en 4 bytes |
| JuanManuelAmador | 0:a5908bca4740 | 429 | paqueteEnvio2[3] = auxtime; |
| JuanManuelAmador | 0:a5908bca4740 | 430 | auxtime >>= 8; |
| JuanManuelAmador | 0:a5908bca4740 | 431 | paqueteEnvio2[2] = auxtime; |
| JuanManuelAmador | 0:a5908bca4740 | 432 | auxtime >>= 8; |
| JuanManuelAmador | 0:a5908bca4740 | 433 | paqueteEnvio2[1] = auxtime; |
| JuanManuelAmador | 0:a5908bca4740 | 434 | auxtime >>= 8; |
| JuanManuelAmador | 0:a5908bca4740 | 435 | paqueteEnvio2[0] = auxtime; |
| JuanManuelAmador | 0:a5908bca4740 | 436 | envioBuff.put(paqueteEnvio2[0]); |
| JuanManuelAmador | 0:a5908bca4740 | 437 | envioBuff.put(paqueteEnvio2[1]); |
| JuanManuelAmador | 0:a5908bca4740 | 438 | envioBuff.put(paqueteEnvio2[2]); |
| JuanManuelAmador | 0:a5908bca4740 | 439 | envioBuff.put(paqueteEnvio2[3]); |
| JuanManuelAmador | 0:a5908bca4740 | 440 | enviandoIMU2 = false; // Se ha terminado de enviar el paquete de la IMU |
| JuanManuelAmador | 0:a5908bca4740 | 441 | bytesEnviados += TAMENVIOIMU; |
| JuanManuelAmador | 0:a5908bca4740 | 442 | break; |
| JuanManuelAmador | 0:a5908bca4740 | 443 | } |
| JuanManuelAmador | 0:a5908bca4740 | 444 | } |
| JuanManuelAmador | 0:a5908bca4740 | 445 | } |
| JuanManuelAmador | 0:a5908bca4740 | 446 | |
| JuanManuelAmador | 0:a5908bca4740 | 447 | if(!LASERbuff.isEmpty() && enviandoIMU == false && enviandoIMU2 == false){ |
| JuanManuelAmador | 0:a5908bca4740 | 448 | byteINbuffL = LASERbuff.get(); |
| JuanManuelAmador | 0:a5908bca4740 | 449 | // Si el byte recibido tiene el bit más significativo a 1 es el byte más significativo |
| JuanManuelAmador | 0:a5908bca4740 | 450 | if(byteINbuffL > 127){ |
| JuanManuelAmador | 0:a5908bca4740 | 451 | fbyte = 1; |
| JuanManuelAmador | 0:a5908bca4740 | 452 | distancia = byteINbuffL; |
| JuanManuelAmador | 0:a5908bca4740 | 453 | distancia &=~0x80; // El bit mas significativo hay que ponerlo a 0 |
| JuanManuelAmador | 0:a5908bca4740 | 454 | enviandoLASER = true; // Se comienza a enviar el paquete del láser |
| JuanManuelAmador | 0:a5908bca4740 | 455 | }else{ // Recibido byte menos significativo |
| JuanManuelAmador | 0:a5908bca4740 | 456 | if (fbyte == 1){ // Si se recibió un byte más significativo puede reconstruirse el valor medido por el laser |
| JuanManuelAmador | 0:a5908bca4740 | 457 | auxtime = LASERtime.get(); // Se lee el tiempo cuando en el instante de llegada del primer byte del paquete |
| JuanManuelAmador | 0:a5908bca4740 | 458 | fbyte = 0; |
| JuanManuelAmador | 0:a5908bca4740 | 459 | distancia <<= 7; |
| JuanManuelAmador | 0:a5908bca4740 | 460 | distancia += byteINbuffL; // Unidades de ingenieria |
| JuanManuelAmador | 0:a5908bca4740 | 461 | //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:a5908bca4740 | 462 | // que guardara los decimales de la operación |
| JuanManuelAmador | 0:a5908bca4740 | 463 | // Los dos primeros bytes byte de los paquetes del laser seran una 'L' |
| JuanManuelAmador | 0:a5908bca4740 | 464 | paqueteEnvioL[0] = 'L'; |
| JuanManuelAmador | 0:a5908bca4740 | 465 | paqueteEnvioL[1] = 'L'; |
| JuanManuelAmador | 0:a5908bca4740 | 466 | paqueteEnvioL[3] = distancia; |
| JuanManuelAmador | 0:a5908bca4740 | 467 | distancia >>= 8; |
| JuanManuelAmador | 0:a5908bca4740 | 468 | paqueteEnvioL[2] = distancia; |
| JuanManuelAmador | 0:a5908bca4740 | 469 | // 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:a5908bca4740 | 470 | paqueteEnvioL[7] = auxtime; |
| JuanManuelAmador | 0:a5908bca4740 | 471 | auxtime >>= 8; |
| JuanManuelAmador | 0:a5908bca4740 | 472 | paqueteEnvioL[6] = auxtime; |
| JuanManuelAmador | 0:a5908bca4740 | 473 | auxtime >>= 8; |
| JuanManuelAmador | 0:a5908bca4740 | 474 | paqueteEnvioL[5] = auxtime; |
| JuanManuelAmador | 0:a5908bca4740 | 475 | auxtime >>= 8; |
| JuanManuelAmador | 0:a5908bca4740 | 476 | paqueteEnvioL[4] = auxtime; |
| JuanManuelAmador | 0:a5908bca4740 | 477 | envioPaquete(paqueteEnvioL, TAMENVIOLASER); |
| JuanManuelAmador | 0:a5908bca4740 | 478 | enviandoLASER = false; // Se ha terminado de enviar el paquete del láser |
| JuanManuelAmador | 0:a5908bca4740 | 479 | bytesEnviados += TAMENVIOLASER; |
| JuanManuelAmador | 0:a5908bca4740 | 480 | } |
| JuanManuelAmador | 0:a5908bca4740 | 481 | } |
| JuanManuelAmador | 0:a5908bca4740 | 482 | |
| JuanManuelAmador | 0:a5908bca4740 | 483 | } |
| JuanManuelAmador | 0:a5908bca4740 | 484 | envioDatos(); |
| JuanManuelAmador | 0:a5908bca4740 | 485 | } // if(adquiriendo) |
| JuanManuelAmador | 0:a5908bca4740 | 486 | } // while(1) |
| JuanManuelAmador | 0:a5908bca4740 | 487 | } // main |
| JuanManuelAmador | 0:a5908bca4740 | 488 | |
| JuanManuelAmador | 0:a5908bca4740 | 489 | // Envia todos los datos que pueda hasta que ya no se puede escribir en el puerto o no haya más datos que enviar |
| JuanManuelAmador | 0:a5908bca4740 | 490 | void envioDatos(){ |
| JuanManuelAmador | 0:a5908bca4740 | 491 | continuaEnviando = 1; |
| JuanManuelAmador | 0:a5908bca4740 | 492 | while(continuaEnviando){ |
| JuanManuelAmador | 0:a5908bca4740 | 493 | if (!envioBuff.isEmpty()){ |
| JuanManuelAmador | 0:a5908bca4740 | 494 | if(pc.writeable()){ |
| JuanManuelAmador | 0:a5908bca4740 | 495 | pc.putc(envioBuff.get()); |
| JuanManuelAmador | 0:a5908bca4740 | 496 | /*envioBuff.get(); |
| JuanManuelAmador | 0:a5908bca4740 | 497 | pc.putc('2');*/ |
| JuanManuelAmador | 0:a5908bca4740 | 498 | }else{ |
| JuanManuelAmador | 0:a5908bca4740 | 499 | continuaEnviando = 0; |
| JuanManuelAmador | 0:a5908bca4740 | 500 | } |
| JuanManuelAmador | 0:a5908bca4740 | 501 | }else{ |
| JuanManuelAmador | 0:a5908bca4740 | 502 | continuaEnviando = 0; |
| JuanManuelAmador | 0:a5908bca4740 | 503 | } |
| JuanManuelAmador | 0:a5908bca4740 | 504 | } |
| JuanManuelAmador | 0:a5908bca4740 | 505 | } |
| JuanManuelAmador | 0:a5908bca4740 | 506 | |
| JuanManuelAmador | 0:a5908bca4740 | 507 | void envioPaquete(unsigned char paquete[], int nElementos){ // Envía por el puerto serie un paquete de datos |
| JuanManuelAmador | 0:a5908bca4740 | 508 | for(int i = 0; i < nElementos; i++){ |
| JuanManuelAmador | 0:a5908bca4740 | 509 | //pc.putc(paquete[i]); |
| JuanManuelAmador | 0:a5908bca4740 | 510 | envioBuff.put(paquete[i]); |
| JuanManuelAmador | 0:a5908bca4740 | 511 | } |
| JuanManuelAmador | 0:a5908bca4740 | 512 | } |
| JuanManuelAmador | 0:a5908bca4740 | 513 | |
| JuanManuelAmador | 0:a5908bca4740 | 514 | void startStreamingIMU(){ |
| JuanManuelAmador | 0:a5908bca4740 | 515 | // Se envia un paquete de datos a la IMU que indica el comienzo de lectura de medidas |
| JuanManuelAmador | 0:a5908bca4740 | 516 | pIMU.putc(0x75); // K |
| JuanManuelAmador | 0:a5908bca4740 | 517 | pIMU.putc(0x65); // A |
| JuanManuelAmador | 0:a5908bca4740 | 518 | pIMU.putc(0x0C); // alt + 12 ♀ |
| JuanManuelAmador | 0:a5908bca4740 | 519 | pIMU.putc(0x05); // alt + 5 ♣ |
| JuanManuelAmador | 0:a5908bca4740 | 520 | pIMU.putc(0x05); // alt + 5 ♣ |
| JuanManuelAmador | 0:a5908bca4740 | 521 | pIMU.putc(0x11); // alt + 11 ♂ |
| JuanManuelAmador | 0:a5908bca4740 | 522 | pIMU.putc(0x01); // alt + 1 ☺ |
| JuanManuelAmador | 0:a5908bca4740 | 523 | pIMU.putc(0x01); // alt + 1 ☺ |
| JuanManuelAmador | 0:a5908bca4740 | 524 | pIMU.putc(0x01); // alt + 1 ☺ |
| JuanManuelAmador | 0:a5908bca4740 | 525 | pIMU.putc(0x04); // alt + 4 ♦ |
| JuanManuelAmador | 0:a5908bca4740 | 526 | pIMU.putc(0x1A); // alt + 26 → |
| JuanManuelAmador | 0:a5908bca4740 | 527 | |
| JuanManuelAmador | 0:a5908bca4740 | 528 | pIMU2.putc(0x75); // K |
| JuanManuelAmador | 0:a5908bca4740 | 529 | pIMU2.putc(0x65); // A |
| JuanManuelAmador | 0:a5908bca4740 | 530 | pIMU2.putc(0x0C); // alt + 12 ♀ |
| JuanManuelAmador | 0:a5908bca4740 | 531 | pIMU2.putc(0x05); // alt + 5 ♣ |
| JuanManuelAmador | 0:a5908bca4740 | 532 | pIMU2.putc(0x05); // alt + 5 ♣ |
| JuanManuelAmador | 0:a5908bca4740 | 533 | pIMU2.putc(0x11); // alt + 11 ♂ |
| JuanManuelAmador | 0:a5908bca4740 | 534 | pIMU2.putc(0x01); // alt + 1 ☺ |
| JuanManuelAmador | 0:a5908bca4740 | 535 | pIMU2.putc(0x01); // alt + 1 ☺ |
| JuanManuelAmador | 0:a5908bca4740 | 536 | pIMU2.putc(0x01); // alt + 1 ☺ |
| JuanManuelAmador | 0:a5908bca4740 | 537 | pIMU2.putc(0x04); // alt + 4 ♦ |
| JuanManuelAmador | 0:a5908bca4740 | 538 | pIMU2.putc(0x1A); // alt + 26 → |
| JuanManuelAmador | 0:a5908bca4740 | 539 | //pc.printf("Paquete de inicio enviado.\n\r"); |
| JuanManuelAmador | 0:a5908bca4740 | 540 | } |
| JuanManuelAmador | 0:a5908bca4740 | 541 | |
| JuanManuelAmador | 0:a5908bca4740 | 542 | void flushSerialBuffer1(void) { char char1 = 0; while (pIMU.readable()) { char1 = pIMU.getc(); } return; } |
| JuanManuelAmador | 0:a5908bca4740 | 543 | void flushSerialBuffer2(void) { char char1 = 0; while (pIMU2.readable()) { char1 = pIMU2.getc(); } return; } |
| JuanManuelAmador | 0:a5908bca4740 | 544 | void flushSerialBuffer3(void) { char char1 = 0; while (pLaser.readable()) { char1 = pLaser.getc(); } return; } |