Receptor
Dependencies: FRDM_MMA8451Q MODSERIAL mbed
Diff: main.cpp
- Revision:
- 0:079fc298efbb
diff -r 000000000000 -r 079fc298efbb main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Jun 10 00:29:00 2015 +0000 @@ -0,0 +1,386 @@ +#include "mbed.h" +#include "MODSERIAL.h" +#include "MMA8451Q.h" +#define MMA8451_I2C_ADDRESS (0x1d<<1) + + +// Definición de el periodo de las señales PWM +#define P_M 20 +#define P_S1 20 +#define F_C 0.00001525878 + +float p1 = 0.0; +float p2 = 0.0; + +PwmOut Motor(PTD4); // Puerto PWM para manejar el motor Brushless +PwmOut Direc(PTC9); // Puerto PWM para manejar el servo de direccion + +// Map function +float map(float x, float in_min, float in_max, float out_min, float out_max) +{ + return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; +} + +int Baudrate=57600; //Se denomina un baudrate (bits/segundo) de 57600 + +Serial pc(USBTX, USBRX); +Serial Arduino(PTD3, PTD2); + +// Se configuran los puertos de conexión del acelerometro y TX Rx serial. +MMA8451Q acc(PTE25, PTE24, MMA8451_I2C_ADDRESS); +MODSERIAL xbee(PTC4, PTC3); // tx, rx + + +// Definición de los botones del control de Xbox +#define ON 0x00 +#define OFF 0x01 + +// Factores de conversión de los análogos +#define F_int 0.00003051757 + + +float datosZ=0; +float datosX=0; +float datosY=0; +float angleYX; +float angleZX; + +int count; + +Ticker envioTask; // Tarea programada para que se envíen datos cada determinado tiempo +Ticker pwmTask; // Tarea programada para que se actualice el PWM cada determinado tiempo + +char datoNuevo; +char pwmNuevo; + +/*Definición de funciones*/ +void TxSend(void); //Función de interrupción +void WaitPWM(void); //Función de interrupción +void xbeeRead(void); //Función de lectura de paquetes +void xbeeSend(); //Función de escritura de paquetes +void initPWM(void); //Función de inicialización de los PWM + +//Definición de los datos fijos de la trama de envío +int trama[17]={0x7E,0x00,0x11,0x10,0x01,0x00,0x13,0xA2,0x00,0x40,0xAF,0x5A,0xB6,0xFF,0xFE,0x00,0x00}; + +int i,entrada[8]; //Definición de los datos útiles que se enviarán + +// Declaración de los datos útiles recibidos +int d1[2],d2[2],d3[2],d4[2],d5[2],d6[2]; + +// Declaración de los análogos +int Yaw,Pitch,Roll,Throttle, Aux_A, Aux_B; + +int V[24]; //Se nombra un vector que tendrá los datos que se sumarán para calcular el valor del checksum + /*El tamaño de este depende del número de datos útiles, para: + 1 dato 17 + 2 datos 18 + 3 datos 19 + 4 datos 20 + 5 datos 21 + . + . + . + + */ + + +int baudrate=19200; //Se denomina un baudrate (bits/segundo) de 57600 + +DigitalOut myled(LED1); + + + + +/************************************************************************* +FUNCIÓN: main() +PROPÓSITO: ESTA ES LA FUNCIÓN PRINCIPAL DEL PROGRAMA +**************************************************************************/ + +int main() { + + datoNuevo = 0; + pwmNuevo=0; + p1 = 0.0; + p2 = 0.0; + + xbee.baud(baudrate); + Arduino.baud(baudrate); + //Setup serial connection + pc.baud(Baudrate); + + initPWM(); + + + trama[2]=0x16; // Este valor es del número de datos desde luego de la longitud hasta antes del checksum, + // en este caso son 3 datos y se tiene una longitud de 17 datos= 0x11 +/* +La longitud varía según el número de datos útiles a enviar, para: +1 dato 0F +2 datos 10 +3 datos 11 +4 datos 12 +5 datos 13 + . + . + . +*/ + + envioTask.attach_us(&TxSend, 250000); + pwmTask.attach_us(&WaitPWM, 100000); + + + while(1) { + + wait(0.025); + xbeeRead(); //Leer datos que llegan del control + + //Print + //pc.printf("%c", 0x7E); + //pc.printf("%c%c%c%c%c%c%c%c%c%c%c%c",d1[1],d1[0],d2[1],d2[0],d3[1],d3[0],d4[1],d4[0],d5[1],d5[0],d6[1],d6[0]); + + + + Arduino.printf("%c", 0x7E); + Arduino.printf("%c%c%c%c%c%c%c%c%c%c%c%c",d1[1],d1[0],d2[1],d2[0],d3[1],d3[0],d4[1],d4[0],d5[1],d5[0],d6[1],d6[0]); + + Roll = ((int)d1[1] << 8) | (int) d1[0]; + + Pitch = ((int)d2[1] << 8) | (int) d2[0]; + + Throttle = ((int)d3[1] << 8) | (int) d3[0]; + + if (Throttle > 32768){Throttle = (Throttle - 65536);} //p1 se escala desde 1 a 1,6 para el freno + + + Yaw = ((int)d4[1] << 8) | (int) d4[0]; + + if (Yaw > 32768){Yaw = (Yaw - 65536);} + + Aux_A = ((int)d5[1] << 8) | (int) d5[0]; + + Aux_B = ((int)d6[1] << 8) | (int) d6[0]; + +/////////////Tareas lentas/////////////////////// + + + if(datoNuevo){ + //Envío de datos + xbeeSend(); + datoNuevo = 0; + + } + + //Valor promedio + + p1 = map(Throttle, -32768, 32767, 1, 2)/P_M; + p2 = map(Yaw, -32768, 32767, 1.1, 1.9)/P_S1; + //count = count + 1; +//Actualización del valor de PWM + if (pwmNuevo){ + + //p1 = p1/count; + + pc.printf("%f \n", p2); + Motor.write(p1); + Direc.write(p2); + + datosX = acc.getAccX(); // datos a almacenar + datosY = acc.getAccY(); // datos a almacenar + datosZ = acc.getAccZ(); // datos a almacenar + + pwmNuevo = 0; + //p1 = 0; + count = 0; + } + + + + } +} + + +///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + +/************************************************************************* +FUNCIÓN: xbeeRead() +PROPÓSITO: ESCRIBIR UN PAQUETE DE DATOS API EN EL XBEE CON LOS DATOS ÚTILES +**************************************************************************/ + + +void xbeeSend(){ + + + int checksum=0x00; //Se inicializa el cálculo del checksum por cada iteración + +for (i=3;i<=24;i++){ + + //Se nombra un nuevo vector con los datos que va desde luego de la longitud (3) hasta antes del checksum (19) + if(i<=16){V[i]=trama[i];} //El número de datos de los datos fijos de la trama de envío es 17, y la longitud es el dato 2 + //por ende el condicional tomaría desde 3-16 + else if((i>16) && (i<=24)){V[i]=entrada[i-17];}//El número de datos útiles a enviar es de 3, estos corresponderían a las posiciones 17,18 y 19 + // Si se envían 4 datos útiles la condición sería hasta 20, 5 datos útil iría hasta 21... + + checksum=checksum+V[i]; //Se suma desde el dato siguiente a la longitud de + //la trama hasta el último dato útil a enviar + + } + + +// FORMULA CHECKSUM + +checksum=0xFF-checksum; //Se realiza la operación para la fórmula del ckecksum + + +// Primero se comprueba que el puerto esté libre para escribir +if ((xbee.writeable())){ + for (i=0;i<=16;i++){xbee.putc(trama[i]);}//Envía todos los datos fijos de la trama de envío + xbee.printf("%c%c%c%c%c%c%c%c%c", entrada[0], entrada[1],entrada[2],entrada[3],entrada[4], entrada[5],entrada[6],entrada[7],checksum);//Envía el resto de datos (datos variables) de la trama de envío + } + +} + + +///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + + +/************************************************************************* +FUNCIÓN: xbeeRead() +PROPÓSITO: LEER UN PAQUETE DE DATOS DEL XBee Y SACAR DATOS ÚTILES +**************************************************************************/ + + void xbeeRead(){ + + char packetFound = 0; //Esta variable, indicará si se ha encontrado un paquete o no. + char thisByte; //Esta variable tomará valores de la trama de datos recibida + + if(xbee.rxBufferGetCount() == 0) //Si en el Buffer no ha llegado ningún dato, el programa volverá a leerlo + { + return; + } + while(xbee.rxBufferGetCount() > 14){ //Si llegaron más de 14 palabras al buffer, compruebe que sea la trama correcta + + thisByte = xbee.getc(); //Toma el primer valor de la trama recibida. + if(thisByte == 0x7E){ //Si el primer dato de la trama es 0x7E, continúe... + thisByte = xbee.getc(); //Toma el MSB de la longitud + thisByte = xbee.getc(); //Toma el LSB de la longitud + thisByte = xbee.getc(); //Toma el tipo de paquete + if(thisByte == 0x90){ //Si recibió que el paquete es de tipo: Rx (0x90), continúe... + // packet is rx type + packetFound = 1; //Se indica que se encontró la trama de recepción + break; //Si se ubica la trama, se sale del ciclo while + } + } + } + + if(packetFound == 1){ // Se encontró un paquete, entonces se toman los siguientes 11 datos. + // Estos 11 datos van hasta un dato antes del primer dato útil + + thisByte = xbee.getc(); + thisByte = xbee.getc(); + thisByte = xbee.getc(); + thisByte = xbee.getc(); + thisByte = xbee.getc(); + thisByte = xbee.getc(); + thisByte = xbee.getc(); + thisByte = xbee.getc(); + thisByte = xbee.getc(); + thisByte = xbee.getc(); + thisByte = xbee.getc(); + + // Luego de contar los 11 datos, se comienzan a tomar los datos útiles. En este caso son 3 + + thisByte = xbee.getc(); + d1[1] = thisByte; + thisByte = xbee.getc(); + d1[0] = thisByte; + thisByte = xbee.getc(); + d2[1] = thisByte; + thisByte = xbee.getc(); + d2[0] = thisByte; + thisByte = xbee.getc(); + d3[1] = thisByte; + thisByte = xbee.getc(); + d3[0] = thisByte; + thisByte = xbee.getc(); + d4[1] = thisByte; + thisByte = xbee.getc(); + d4[0] = thisByte; + thisByte = xbee.getc(); + d5[1] = thisByte; + thisByte = xbee.getc(); + d5[0] = thisByte; + thisByte = xbee.getc(); + d6[1] = thisByte; + thisByte = xbee.getc(); + d6[0] = thisByte; + + } + + + } + + +///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + +// ISR -> Interrupt service ruting - para tomar dato y hacer parpadeo (DEBE SER RAPIDA) +void TxSend(void){ + + angleZX = atan((datosZ)/(datosX)); + angleZX = angleZX*(57.2958); + + angleYX = atan((datosY)/(datosX)); + angleYX = angleYX*(57.2958); + + int d_1 = map(angleYX,-90,90,-32768,32767); + int d_2 = map(angleZX,-90,90,-32768,32767);; + datosY = 100; + datosZ = 1.5; + + + + char * b0 = (char *) &d_1; + char * b1 = (char *) &d_2; + char * b2 = (char *) &datosZ; + char * b3 = (char *) &datosY; + + entrada[0]= b0[1]; + entrada[1]= b0[0]; + entrada[2]= b1[1]; + entrada[3]= b1[0]; + entrada[4]= b2[1]; + entrada[5]= b2[0]; + entrada[6]= b3[1]; + entrada[7]= b3[0]; + + datoNuevo = 1; // Indicamos que tenemos un dato nuevo +} + +///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + +// ISR -> Interrupción para la actualización del PWM +void WaitPWM(void){ + pwmNuevo = 1; // Indicamos que tenemos un dato nuevo +} + + +/************************************************************************* +FUNCIÓN: initPWM() +PROPÓSITO: Inicializar los PWM +**************************************************************************/ + +void initPWM(void){ + Motor.period_ms(P_M); + Motor.write(1.0/P_M); + Direc.period_ms(P_S1); + Direc.write(1.5/P_S1); + wait(0.2); +}