File content as of revision 0:079fc298efbb:

#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);
}