virtualmech
Programa mbed1 excelencia
Dependencies: mbed
Diff: excelenciaSP1.cpp
- Revision:
- 0:a5908bca4740
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/excelenciaSP1.cpp Wed Jan 18 12:58:15 2017 +0000
@@ -0,0 +1,544 @@
+/***************************************
+ VIRTUALMECH
+ Autor: Juan Manuel Amador Olivares
+ Fecha: 16/9/2015
+ ***************************************/
+
+/***************************************
+ Adquisición de datos IMU y Láser
+ ***************************************/
+#include "mbed.h"
+#include "Buffering.h"
+#include "BufferBig.h"
+#include "Bufferinguint.h"
+
+#define TAMPAQUETEIMU 34 // Tamaño del paquete recibido de la IMU
+#define TAMENVIOIMU 30 // Tamaño del paquete enviado al PC con los datos de la IMU
+#define TAMENVIOLASER 8 // Tamaño del paquete enviado al PC con los datos del sensor de distancia
+
+/********************
+ Funciones
+ ********************/
+void startStreamingIMU(); // Manda los bytes a la IMU necesarios para que comience a enviar datos por streaming
+void envioPaquete(unsigned char paquete[], int nElementos); // Envía por el puerto serie un paquete de datos
+void envioDatos(); // Envio al PC los bytes de datos cuando es posible
+void flushSerialBuffer1(void);
+void flushSerialBuffer2(void);
+void flushSerialBuffer3(void);
+/********************/
+
+DigitalOut led(LED1); // Led indicador. Se enciende cuando se están adquiriendo datos
+
+// Puertos UART a utilizar
+Serial pLaser(p9, p10);
+Serial pIMU(p28, p27);
+Serial pIMU2(p13, p14);
+Serial pc(USBTX, USBRX);
+
+// Temporizador para el control del tiempo
+Timer t;
+unsigned int auxtime = 0; // Variable auxiliar para la lectura del tiempo
+unsigned char byteIN, byteINanterior, byteINbuff, byteINanteriorbuff; // Variables auxiliares para guardar bytes recibidos de la IMU
+unsigned char byteIN2, byteINanterior2, byteINbuff2, byteINanteriorbuff2; // Variables auxiliares para guardar bytes recibidos de la IMU2
+unsigned char byteINL, byteINbuffL; // Variable auxiliar para guardar bytes recibidos del laser
+
+// Variables para la reconstrucción de los datos de la IMU
+unsigned int nBytes = TAMPAQUETEIMU;
+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
+
+// Variables para la reconstrucción de los datos de la IMU2
+unsigned int nBytes2 = TAMPAQUETEIMU;
+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
+/*union datoCompuesto{
+ char B[4];
+ float unidos;
+} dato;*/
+
+// Variables para la reconstrucción datos del láser
+unsigned int distancia; // Sólo se usarán los 2 bytes menos significativos
+unsigned char paqueteEnvioL[TAMENVIOLASER];
+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)
+
+// Buffers dónde se guardarán los bytes provenientes de los sensores
+Buffering IMUbuff;
+Bufferinguint IMUtime;
+Buffering IMUbuff2;
+Bufferinguint IMUtime2;
+Buffering LASERbuff;
+Bufferinguint LASERtime;
+
+// Buffer dónde se guardarán los datos antes de ser enviados
+BufferBig envioBuff;
+char continuaEnviando;
+
+// Variables semáforo para que no se mezclen los bytes de distintos paquetes
+// Estas variables indican de que sensor se pueden leer y procesar los bytes de los paquetes recibidos.
+// Independientemente de estas variables se recibirán los bytes de los distintos sensores, que se irán guardando en sus respectivos buffer,
+// y se irá guardando la marca de tiempo cuando se detecte la llegada de un nuevo paquete.
+bool enviandoIMU = false;
+bool enviandoIMU2 = false;
+bool enviandoLASER = false;
+bool adquiriendo = false;
+
+// Variable de control de bytes enviados
+unsigned int bytesEnviados;
+unsigned int selIMUleer;
+
+int main() {
+ // Inicialización de puertos UART
+ pc.baud(921600); // Configuración del puerto conectado al PC
+ pIMU.baud(460800); // Configuración del puerto conectado a la IMU
+ pIMU2.baud(460800); // Configuración del puerto conectado a la IMU2
+ pLaser.baud(115200); // Configuración del puerto conectado al sensor de distancia
+
+ //pc.printf("Listo para recibir datos.\n\r");
+
+ while(1) { // Bucle infinito
+ // Se comprueba la llegada de nuevos bytes procedentes del PC
+ if(pc.readable()){
+ char c = pc.getc(); // Se lee el byte
+ if(c == 'a'){ // Si es el carácter 'a', indica el inicio de la adquisición
+ adquiriendo = true;
+ t.reset();
+ t.start();
+ startStreamingIMU();
+ bytesEnviados = 0;
+ led = 1;
+ }else if(c == 's'){ // Si es el carácter 'a', indica el final de la adquisición
+ adquiriendo = false;
+ led = 0;
+ t.stop();
+ t.reset();
+
+ // Puertos serie
+ flushSerialBuffer1();
+ flushSerialBuffer2();
+ flushSerialBuffer3();
+
+ // Buffer en RAM de mbed
+ while(!IMUbuff.isEmpty()){
+ IMUbuff.get();
+ }
+ while(!IMUtime.isEmpty()){
+ IMUtime.get();
+ }
+ while(!IMUbuff2.isEmpty()){
+ IMUbuff2.get();
+ }
+ while(!IMUtime2.isEmpty()){
+ IMUtime2.get();
+ }
+ while(!LASERbuff.isEmpty()){
+ LASERbuff.get();
+ }
+ while(!LASERtime.isEmpty()){
+ LASERtime.get();
+ }
+ while(!envioBuff.isEmpty()){
+ envioBuff.get();
+ }
+ /*pc.printf("\n\nBytes enviados: %u\n\n", bytesEnviados);
+ pc.printf("\n\nDatos en el buffer de envio: %u, %u, %u, %u\n\n", envioBuff.getDif(),IMUbuff.getDif(), IMUbuff2.getDif(),LASERbuff.getDif());*/
+ }
+ }
+ if(adquiriendo){ // Sólo se reconstruye y envían datos cuando se ha iniciado la adquisición
+
+ if(pIMU.readable()){
+ byteINanterior = byteIN;
+ byteIN = pIMU.getc();
+ IMUbuff.put(byteIN); // Se guarda el byte en el buffer
+ if(byteIN == 0x65 && byteINanterior == 0x75){// Si el byte recibido es un 75 en hex, este byte marca la llegada de una nueva medida
+ // Se guarda la marca de tiempo
+ IMUtime.put(t.read_us());
+ }
+ }
+
+ // Se reciben los bytes de la IMU2, se guardan en un buffer y se guarda una marca de tiempo si corresponde
+ if(pIMU2.readable()){
+ byteINanterior2 = byteIN2;
+ byteIN2 = pIMU2.getc();
+ IMUbuff2.put(byteIN2); // Se guarda el byte en el buffer
+ if(byteIN2 == 0x65 && byteINanterior2 == 0x75){// Si el byte recibido es un 75 en hex, este byte marca la llegada de una nueva medida
+ // Se guarda la marca de tiempo
+ IMUtime2.put(t.read_us());
+ }
+ }
+
+ // Se reciben los bytes del LASER, se guardan en un buffer y se guarda una marca de tiempo si corresponde
+ if(pLaser.readable()){
+ byteINL = pLaser.getc();
+ LASERbuff.put(byteINL); // Se guarda el byte en el buffer
+ if(byteINL > 127){ // Los bytes con el bit más significativo a 1 son los primeros bytes d elos paquetes
+ // por lo que se guarda la marca de tiempo correspondiente
+ LASERtime.put(t.read_us());
+ }
+ }
+
+ if(!IMUbuff.isEmpty() && enviandoIMU2 == false && enviandoLASER == false){ // Si no se está enviando un paquete del láser y existen bytes por leer
+ byteINanteriorbuff = byteINbuff;
+ byteINbuff = IMUbuff.get();
+ // Reconstrucción del dato
+ 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
+ // Se pone el contador de bytes de un paquete a 0
+ //pc.putc('I'); // Se envían los dos bytes que indican el comienzo del paquete
+ //pc.putc('I'); //antes de enviar el primer bytes de datos
+ envioBuff.put('I');
+ envioBuff.put('I');
+ nBytes = 1;
+ enviandoIMU = true; // Se está enviando un paquete de IMU
+
+ }else{ // Todo lo que no sea 0x75 es parte del resto del dato
+ // Se suma un byte mas
+ nBytes++;
+ // Para reconstruir el dato se mandan primero los bytes más significativos (que también es el orden en el que llegan)
+ switch(nBytes){
+ case 6:
+ //pc.putc(byteINbuff);
+ envioBuff.put(byteINbuff);
+ break;
+ case 7:
+ //pc.putc(byteINbuff);
+ envioBuff.put(byteINbuff);
+ break;
+ case 8:
+ //pc.putc(byteINbuff);
+ envioBuff.put(byteINbuff);
+ break;
+ case 9:
+ //pc.putc(byteINbuff);
+ envioBuff.put(byteINbuff);
+ break;
+ case 10:
+ //pc.putc(byteINbuff);
+ envioBuff.put(byteINbuff);
+ break;
+ case 11:
+ //pc.putc(byteINbuff);
+ envioBuff.put(byteINbuff);
+ break;
+ case 12:
+ //pc.putc(byteINbuff);
+ envioBuff.put(byteINbuff);
+ break;
+ case 13:
+ //pc.putc(byteINbuff);
+ envioBuff.put(byteINbuff);
+ break;
+ case 14:
+ //pc.putc(byteINbuff);
+ envioBuff.put(byteINbuff);
+ break;
+ case 15:
+ //pc.putc(byteINbuff);
+ envioBuff.put(byteINbuff);
+ break;
+ case 16:
+ //pc.putc(byteINbuff);
+ envioBuff.put(byteINbuff);
+ break;
+ case 17:
+ //pc.putc(byteINbuff);
+ envioBuff.put(byteINbuff);
+ break;
+ case 20:
+ //pc.putc(byteINbuff);
+ envioBuff.put(byteINbuff);
+ break;
+ case 21:
+ //pc.putc(byteINbuff);
+ envioBuff.put(byteINbuff);
+ break;
+ case 22:
+ //pc.putc(byteINbuff);
+ envioBuff.put(byteINbuff);
+ break;
+ case 23:
+ //pc.putc(byteINbuff);
+ envioBuff.put(byteINbuff);
+ break;
+ case 24:
+ //pc.putc(byteINbuff);
+ envioBuff.put(byteINbuff);
+ break;
+ case 25:
+ //pc.putc(byteINbuff);
+ envioBuff.put(byteINbuff);
+ break;
+ case 26:
+ //pc.putc(byteINbuff);
+ envioBuff.put(byteINbuff);
+ break;
+ case 27:
+ //pc.putc(byteINbuff);
+ envioBuff.put(byteINbuff);
+ break;
+ case 28:
+ //pc.putc(byteINbuff);
+ envioBuff.put(byteINbuff);
+ break;
+ case 29:
+ //pc.putc(byteINbuff);
+ envioBuff.put(byteINbuff);
+ break;
+ case 30:
+ //pc.putc(byteINbuff);
+ envioBuff.put(byteINbuff);
+ break;
+ case 31:
+ //pc.putc(byteINbuff);
+ envioBuff.put(byteINbuff);
+
+ auxtime = IMUtime.get(); // Se recupera la marca de tiempo del paquete y se envía dividido en 4 bytes
+ paqueteEnvio[3] = auxtime;
+ auxtime >>= 8;
+ paqueteEnvio[2] = auxtime;
+ auxtime >>= 8;
+ paqueteEnvio[1] = auxtime;
+ auxtime >>= 8;
+ paqueteEnvio[0] = auxtime;
+ /*pc.putc(paqueteEnvio[0]);
+ pc.putc(paqueteEnvio[1]);
+ pc.putc(paqueteEnvio[2]);
+ pc.putc(paqueteEnvio[3]);*/
+ envioBuff.put(paqueteEnvio[0]);
+ envioBuff.put(paqueteEnvio[1]);
+ envioBuff.put(paqueteEnvio[2]);
+ envioBuff.put(paqueteEnvio[3]);
+ enviandoIMU = false; // Se ha terminado de enviar el paquete de la IMU
+ bytesEnviados += TAMENVIOIMU;
+ break;
+ }
+ }
+ }
+
+ if(!IMUbuff2.isEmpty() && enviandoIMU == false && enviandoLASER == false){ // Si no se está enviando un paquete del láser y existen bytes por leer
+ byteINanteriorbuff2 = byteINbuff2;
+ byteINbuff2 = IMUbuff2.get();
+ // Reconstrucción del dato
+ 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
+ // Se pone el contador de bytes de un paquete a 0
+ //pc.putc('I'); // Se envían los dos bytes que indican el comienzo del paquete
+ //pc.putc('I'); //antes de enviar el primer bytes de datos
+ envioBuff.put('H');
+ envioBuff.put('H');
+ nBytes2 = 1;
+ enviandoIMU2 = true; // Se está enviando un paquete de IMU
+
+ }else{ // Todo lo que no sea 0x75 es parte del resto del dato
+ // Se suma un byte mas
+ nBytes2++;
+ // Para reconstruir el dato se mandan primero los bytes más significativos (que también es el orden en el que llegan)
+ switch(nBytes2){
+ case 6:
+ //pc.putc(byteINbuff2);
+ envioBuff.put(byteINbuff2);
+ break;
+ case 7:
+ //pc.putc(byteINbuff2);
+ envioBuff.put(byteINbuff2);
+ break;
+ case 8:
+ //pc.putc(byteINbuff2);
+ envioBuff.put(byteINbuff2);
+ break;
+ case 9:
+ //pc.putc(byteINbuff2);
+ envioBuff.put(byteINbuff2);
+ break;
+ case 10:
+ //pc.putc(byteINbuff2);
+ envioBuff.put(byteINbuff2);
+ break;
+ case 11:
+ //pc.putc(byteINbuff2);
+ envioBuff.put(byteINbuff2);
+ break;
+ case 12:
+ //pc.putc(byteINbuff2);
+ envioBuff.put(byteINbuff2);
+ break;
+ case 13:
+ //pc.putc(byteINbuff2);
+ envioBuff.put(byteINbuff2);
+ break;
+ case 14:
+ //pc.putc(byteINbuff2);
+ envioBuff.put(byteINbuff2);
+ break;
+ case 15:
+ //pc.putc(byteINbuff2);
+ envioBuff.put(byteINbuff2);
+ break;
+ case 16:
+ //pc.putc(byteINbuff2);
+ envioBuff.put(byteINbuff2);
+ break;
+ case 17:
+ //pc.putc(byteINbuff2);
+ envioBuff.put(byteINbuff2);
+ break;
+ case 20:
+ //pc.putc(byteINbuff2);
+ envioBuff.put(byteINbuff2);
+ break;
+ case 21:
+ //pc.putc(byteINbuff2);
+ envioBuff.put(byteINbuff2);
+ break;
+ case 22:
+ //pc.putc(byteINbuff2);
+ envioBuff.put(byteINbuff2);
+ break;
+ case 23:
+ //pc.putc(byteINbuff2);
+ envioBuff.put(byteINbuff2);
+ break;
+ case 24:
+ //pc.putc(byteINbuff2);
+ envioBuff.put(byteINbuff2);
+ break;
+ case 25:
+ //pc.putc(byteINbuff2);
+ envioBuff.put(byteINbuff2);
+ break;
+ case 26:
+ //pc.putc(byteINbuff2);
+ envioBuff.put(byteINbuff2);
+ break;
+ case 27:
+ //pc.putc(byteINbuff2);
+ envioBuff.put(byteINbuff2);
+ break;
+ case 28:
+ //pc.putc(byteINbuff2);
+ envioBuff.put(byteINbuff2);
+ break;
+ case 29:
+ //pc.putc(byteINbuff2);
+ envioBuff.put(byteINbuff2);
+ break;
+ case 30:
+ //pc.putc(byteINbuff2);
+ envioBuff.put(byteINbuff2);
+ break;
+ case 31:
+ //pc.putc(byteINbuff2);
+ envioBuff.put(byteINbuff2);
+
+ auxtime = IMUtime2.get(); // Se recupera la marca de tiempo del paquete y se envía dividido en 4 bytes
+ paqueteEnvio2[3] = auxtime;
+ auxtime >>= 8;
+ paqueteEnvio2[2] = auxtime;
+ auxtime >>= 8;
+ paqueteEnvio2[1] = auxtime;
+ auxtime >>= 8;
+ paqueteEnvio2[0] = auxtime;
+ envioBuff.put(paqueteEnvio2[0]);
+ envioBuff.put(paqueteEnvio2[1]);
+ envioBuff.put(paqueteEnvio2[2]);
+ envioBuff.put(paqueteEnvio2[3]);
+ enviandoIMU2 = false; // Se ha terminado de enviar el paquete de la IMU
+ bytesEnviados += TAMENVIOIMU;
+ break;
+ }
+ }
+ }
+
+ if(!LASERbuff.isEmpty() && enviandoIMU == false && enviandoIMU2 == false){
+ byteINbuffL = LASERbuff.get();
+ // Si el byte recibido tiene el bit más significativo a 1 es el byte más significativo
+ if(byteINbuffL > 127){
+ fbyte = 1;
+ distancia = byteINbuffL;
+ distancia &=~0x80; // El bit mas significativo hay que ponerlo a 0
+ enviandoLASER = true; // Se comienza a enviar el paquete del láser
+ }else{ // Recibido byte menos significativo
+ if (fbyte == 1){ // Si se recibió un byte más significativo puede reconstruirse el valor medido por el laser
+ auxtime = LASERtime.get(); // Se lee el tiempo cuando en el instante de llegada del primer byte del paquete
+ fbyte = 0;
+ distancia <<= 7;
+ distancia += byteINbuffL; // Unidades de ingenieria
+ //distancia = distancia*0.0498; // (mm) La conversión a mm se hace en el PC para no tener que enviar un tipo float
+ // que guardara los decimales de la operación
+ // Los dos primeros bytes byte de los paquetes del laser seran una 'L'
+ paqueteEnvioL[0] = 'L';
+ paqueteEnvioL[1] = 'L';
+ paqueteEnvioL[3] = distancia;
+ distancia >>= 8;
+ paqueteEnvioL[2] = distancia;
+ // 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)
+ paqueteEnvioL[7] = auxtime;
+ auxtime >>= 8;
+ paqueteEnvioL[6] = auxtime;
+ auxtime >>= 8;
+ paqueteEnvioL[5] = auxtime;
+ auxtime >>= 8;
+ paqueteEnvioL[4] = auxtime;
+ envioPaquete(paqueteEnvioL, TAMENVIOLASER);
+ enviandoLASER = false; // Se ha terminado de enviar el paquete del láser
+ bytesEnviados += TAMENVIOLASER;
+ }
+ }
+
+ }
+ envioDatos();
+ } // if(adquiriendo)
+ } // while(1)
+} // main
+
+// Envia todos los datos que pueda hasta que ya no se puede escribir en el puerto o no haya más datos que enviar
+void envioDatos(){
+ continuaEnviando = 1;
+ while(continuaEnviando){
+ if (!envioBuff.isEmpty()){
+ if(pc.writeable()){
+ pc.putc(envioBuff.get());
+ /*envioBuff.get();
+ pc.putc('2');*/
+ }else{
+ continuaEnviando = 0;
+ }
+ }else{
+ continuaEnviando = 0;
+ }
+ }
+}
+
+void envioPaquete(unsigned char paquete[], int nElementos){ // Envía por el puerto serie un paquete de datos
+ for(int i = 0; i < nElementos; i++){
+ //pc.putc(paquete[i]);
+ envioBuff.put(paquete[i]);
+ }
+}
+
+void startStreamingIMU(){
+ // Se envia un paquete de datos a la IMU que indica el comienzo de lectura de medidas
+ pIMU.putc(0x75); // K
+ pIMU.putc(0x65); // A
+ pIMU.putc(0x0C); // alt + 12 ♀
+ pIMU.putc(0x05); // alt + 5 ♣
+ pIMU.putc(0x05); // alt + 5 ♣
+ pIMU.putc(0x11); // alt + 11 ♂
+ pIMU.putc(0x01); // alt + 1 ☺
+ pIMU.putc(0x01); // alt + 1 ☺
+ pIMU.putc(0x01); // alt + 1 ☺
+ pIMU.putc(0x04); // alt + 4 ♦
+ pIMU.putc(0x1A); // alt + 26 →
+
+ pIMU2.putc(0x75); // K
+ pIMU2.putc(0x65); // A
+ pIMU2.putc(0x0C); // alt + 12 ♀
+ pIMU2.putc(0x05); // alt + 5 ♣
+ pIMU2.putc(0x05); // alt + 5 ♣
+ pIMU2.putc(0x11); // alt + 11 ♂
+ pIMU2.putc(0x01); // alt + 1 ☺
+ pIMU2.putc(0x01); // alt + 1 ☺
+ pIMU2.putc(0x01); // alt + 1 ☺
+ pIMU2.putc(0x04); // alt + 4 ♦
+ pIMU2.putc(0x1A); // alt + 26 →
+ //pc.printf("Paquete de inicio enviado.\n\r");
+}
+
+void flushSerialBuffer1(void) { char char1 = 0; while (pIMU.readable()) { char1 = pIMU.getc(); } return; }
+void flushSerialBuffer2(void) { char char1 = 0; while (pIMU2.readable()) { char1 = pIMU2.getc(); } return; }
+void flushSerialBuffer3(void) { char char1 = 0; while (pLaser.readable()) { char1 = pLaser.getc(); } return; }
\ No newline at end of file