Eduardo Ospina
Receptor
Dependencies: FRDM_MMA8451Q MODSERIAL mbed
Diff: main.cpp
- Revision:
- 0:079fc298efbb
--- /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);
+}