Hydrocontest
Prueba 1
Dependencies: FRDM_MMA8451Q MODSERIAL mbed
Diff: main.cpp
- Revision:
- 0:64bf61ea2ead
- Child:
- 1:f1cf6444beba
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Jul 01 21:45:31 2014 +0000
@@ -0,0 +1,506 @@
+#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
+}
+