File content as of revision 0:64bf61ea2ead:

#include "mbed.h"
#include "MODSERIAL.h"
#include "MMA8451Q.h"
#define MMA8451_I2C_ADDRESS (0x1d<<1)

// 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 A 0x01
#define X 0x04
#define Y 0x08
#define B 0x02
#define LB 0x10 
#define RB 0x20
#define Back 0x40
#define Start 0x80
#define Back_A 0x41
#define Back_B 0x42


// Factores de conversión de los análogos
#define F_LT_RT 0.000030637
#define F_analog 0.000015258
#define F_arrows 0.003174603

// Definición de el periodo de las señales PWM
#define P_M 20.0
#define P_S1 20.0
#define P_S2 20.0

PwmOut Motor(PTD4);            // Puerto PWM para manejar el motor Brushless
PwmOut Direc(PTA12);             // Puerto PWM para manejar el servo de direccion
PwmOut Elev(PTA4);              // Puerto PWM para manejar el servo de elevacion

float datosZ=0;
float datosX=0;
float datosY=0;
float angleXZ;
float angleYZ;

// 
float p1=0;                     
float p2=0;
float p3=0;

float p1a=0;
float p2a=0;
float p3a=0;

float LT;
float RT;
int I_cont=0;
int cont=0;
int Inicio=0;
float Med_M=0;
float Med_S1=0;
float Med_S2=0;
float C_In;
float C_S2;
float C_Stop;
float C_Load;
float T_In;
float T_Load;
float T_Stop;
float T_S2;
float T_PWM;

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


void AcCurve(void);           //Función de aceleración del motor principal
void AcLoadCurve(void);           //Función de aceleración del motor principal
void StopCurve(void);


//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,0xB5,0xE7,0x51,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,d2,d3,d4,d5,d6,d7,d8,d9;

// Declaración de los análogos
int Left_Xaxis,Left_Yaxis,LT_RT,Arrows;

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;
    xbee.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) {
    
    myled = !myled;
    
    
    //Definición de variables de arranque y parada
    T_In=3.0;
    T_Load=6.0;
    T_Stop=1.0;
    T_S2=2.0;
    Med_M=1.26;
    Med_S1=1.66;
    Med_S2=1.5;
    T_PWM=0.100;
    
    
    xbeeRead();     //Leer datos que llegan del control 
    
        
switch(d1)          //El dato número 1 (d1) contiene los valores discretos más importantes del control 
{

  case LB:          //Si el dato1 es L1 (0x40) el barco debe frenar    
    p1=(1.0/P_M);
    break;
  case RB:          //Si el dato1 es R1 (0x80) el barco debe acelerar al máximo
    p1=(Med_M-1.0)+(Med_M);
    break;
  case X:
    p3=(Med_S2/P_S2);
    break;
  case Y:
    p2=((Med_S1+0.03)/P_S1);
    break;
  default:
    Left_Xaxis = ((unsigned int)d2 << 8) | (unsigned int) d3; //Concatena el dato 2 (d2) y el dato 3 (d3) para obtener A1_EjeX
    
    if ((Left_Xaxis<=32000)|(Left_Xaxis>=33000)){ p2=(1.0/P_S1)+(F_analog*Left_Xaxis)*((Med_S1-1.0)*(2.0/P_S1));}
    else {p2=((Med_S1+0.03)/P_S1);}
    
    //Left_Yaxis = ((unsigned int)d4 << 8) | (unsigned int) d5; //Concatena el dato 4 (d4) y el dato 5 (d5) para obtener A1_EjeY
    
    //p3=0.05+(0.00001625*A1_Y)*0.05;
    
    LT_RT = ((unsigned int)d6 << 8) | (unsigned int) d7; //Concatena el dato 6 (d6) y el dato 7 (d7) para obtener A2_EjeX
        
        
    Arrows = ((unsigned int)d8 << 8) | (unsigned int) d9; //Concatena el dato 8 (d8) y el dato 9 (d9) para obtener A2_EjeY
    
  break;
}
    


/////////////Tareas lentas///////////////////////

//Envío de datos
        if(datoNuevo){           
        xbeeSend();
        datoNuevo = 0;
        }

//Actualización del valor de PWM        
        if (pwmNuevo){
            
    if ((Inicio==0)&&(d1==Back_A)){ AcCurve();}
    else if ((Inicio==0)&&(d1==Back_B)){ AcLoadCurve();}
    else if ((Inicio==1)&&(d1==Start)){ StopCurve(); Inicio=0;}
    else if (Inicio==1){
     //LT y RT vienen en un valor que va desde 128 hasta 65408. LT va desde 32768 hasta 65408 y RT va desde 128 hasta 32768.
    // Con este if se identifica si se presionó LT y RT. Para luego escalar a valores desde 0 hasta 1
    if (LT_RT>32768){LT=(-1*LT_RT+65536)*F_LT_RT;p1=((LT*(Med_M-1.0))+1.0)/P_M;}                  //p1 se escala desde 1 a 1,6 para el freno
    else if (LT_RT<32768){RT=(-1*LT_RT+32768)*F_LT_RT;p1=((RT*(Med_M-1.0))+(Med_M))/P_M;}      //p1 se escala desde 1,6 a 2,2 para la aceleración
    else if (LT_RT==32768){p1=(Med_M/P_M);}                                  //p1 es 1,6 si no presiona ni LT ni RT
                        }     
        
        
        if (cont<=10){p2=Med_S1/P_S1; p3=1.5/P_S2; cont++;}
        else {cont=12;}
         
        if (p1 != p1a) {Motor.write(p1);}
        
        if (p2 != p2a) {Direc.write(p2);}
        
        C_S2=(Med_S2-1.05555)*(T_PWM/T_S2);
        
        
               
        if ((Arrows==315)|(Arrows==0)|(Arrows==45)){p3=p3+C_S2/P_S2;} 
        else if ((Arrows==225)|(Arrows==180)|(Arrows==135)) {p3=p3-C_S2/P_S2;}
        
        if (p3<=1.05555/P_S2){p3=1.05555/P_S2;}
        if (p3>=1.94444/P_S2){p3=1.94444/P_S2;}
        
        if (p3 != p3a) {Elev.write(p3);}
        
        p1a=p1;
        p2a=p2;
        p3a=p3;
        
        datosX = acc.getAccX();             // datos a almacenar
        datosY = acc.getAccY();             // datos a almacenar
        datosZ = acc.getAccZ();             // datos a almacenar
        
        pwmNuevo=0;               
        }
        
       

    }
}


/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

/*************************************************************************
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(Med_S1/P_S1);
    Elev.period_ms(P_S2);
    Elev.write(Med_S2/P_S2);
    wait(0.2);
}

/*************************************************************************
FUNCIÓN: AcCurve(void)
PROPÓSITO:  Función de aceleración del motor a partir de velocidad cero
**************************************************************************/


void AcCurve(void){
    
        
        
        C_In=(Med_M-1.0)*(T_PWM/T_In);
    
        p1=(1.0/P_M);
        Motor.write(p1);
        wait(0.250);
                    
        for (int ii=0;ii<=ceil(T_In/T_PWM);ii++)   {
                    if (p1 < (Med_M/P_M)) {p1=p1+C_In/P_M; Motor.write(p1); wait(T_PWM);}
                    else if (p1 >= (Med_M/P_M)){p1=Med_M/P_M; }            
                    }
                    
                    
        Inicio=1;
                   }
                   
/*************************************************************************
FUNCIÓN: AcLoadCurve(void)
PROPÓSITO:  Función de aceleración del motor a partir de velocidad cero
**************************************************************************/


void AcLoadCurve(void){
    
                
        C_Load=(Med_M-1.0)*(T_PWM/T_Load);
    
        p1=(1.0/P_M);
        Motor.write(p1);
        wait(0.250);
                    
        for (int ii=0;ii<=ceil(T_Load/T_PWM);ii++)   {
                    if (p1 < (Med_M/P_M)) {p1=p1+C_Load/P_M; Motor.write(p1); wait(T_PWM);}
                    else if (p1 >= (Med_M/P_M)){p1=Med_M/P_M;}            
                    }
                    
        Inicio=1;
                   }

 /*************************************************************************
FUNCIÓN: StopCurve(void)
PROPÓSITO: función de parada del motor. 
**************************************************************************/
 
 void StopCurve(void){
    
            C_Stop=(Med_M-1.0)*(T_PWM/T_Stop);
    
     for (int ii=0;ii<=ceil(T_Stop/T_PWM);ii++){
                      if (p1 > 1.0/P_M) {p1=p1-C_Stop/P_M; Motor.write(p1); wait(T_PWM);}
                      else if (p1 <= 1.0/P_M) {p1=1.0/P_M;}
                      }
                        
                      }   
/*************************************************************************
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 =  thisByte;
        thisByte =  xbee.getc();       
        d2 =  thisByte;
        thisByte =  xbee.getc();       
        d3 =  thisByte;
        thisByte =  xbee.getc();       
        d4 =  thisByte;
        thisByte =  xbee.getc();       
        d5 =  thisByte;
        thisByte =  xbee.getc();       
        d6 =  thisByte;
        thisByte =  xbee.getc();       
        d7 =  thisByte;
        thisByte =  xbee.getc();       
        d8 =  thisByte;
        thisByte =  xbee.getc();       
        d9 =  thisByte;
        }
        
        
        }


/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


// ISR -> Interrupt service ruting - para tomar dato y hacer parpadeo (DEBE SER RAPIDA)
void TxSend(void){
    
    //datosX = 1.0;             // datos a almacenar
    //datosY = 1.0;             // datos a almacenar
    //datosZ = 1.0;             // datos a almacenar
      
    //datosX = acc.getAccX();             // datos a almacenar
    //datosY = acc.getAccY();             // datos a almacenar
    //datosZ = acc.getAccZ();             // datos a almacenar
    
    
    angleXZ = atan((datosX)/(datosZ));
    angleXZ = angleXZ*(57.2958);
    
    angleYZ = atan((datosY)/(datosZ));
    angleYZ = angleYZ*(57.2958);
    
    char * b1 = (char *) &angleXZ;
    char * b2 = (char *) &angleYZ;

    entrada[0]= b1[3];
    entrada[1]= b1[2];
    entrada[2]= b1[1];
    entrada[3]= b1[0];
    entrada[4]= b2[3];
    entrada[5]= b2[2];
    entrada[6]= b2[1];
    entrada[7]= b2[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
}