Hydrocontest
Prueba 1
Dependencies: FRDM_MMA8451Q MODSERIAL mbed
Diff: main.cpp
- Revision:
- 1:f1cf6444beba
- Parent:
- 0:64bf61ea2ead
--- a/main.cpp Tue Jul 01 21:45:31 2014 +0000
+++ b/main.cpp Tue Jul 01 23:01:35 2014 +0000
@@ -31,33 +31,32 @@
#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
+// Salidas PWM
+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;
+// Variables para el acelerometro
+float datosZ;
+float datosX;
+float datosY;
float angleXZ;
float angleYZ;
-//
-float p1=0;
-float p2=0;
-float p3=0;
+// Ciclos de dureza PWM
+float p1;
+float p2;
+float p3;
+float p1a;
+float p2a;
+float p3a;
-float p1a=0;
-float p2a=0;
-float p3a=0;
-
+// Variables del control de xbox
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 Med_M;
+float Med_S1;
+float Med_S2;
float C_In;
float C_S2;
float C_Stop;
@@ -67,35 +66,43 @@
float T_Stop;
float T_S2;
float T_PWM;
+int I_cont;
+int cont;
+int Inicio;
+// Interrupciones
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
+// Banderas para avisar cuando llega un dato nuevo o se va a escribir un valor en el pwm
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 TxSend(void); // Funcion de interrupción
+void WaitPWM(void); // Funcion de interrupción
+void xbeeRead(void); // Funcion de lectura de paquetes
+void xbeeSend(void); // Funcion de escritura de paquetes
+void initPWM(void); // Funcion 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);
+void AcCurve(void); // Funcion de aceleración del motor principal (sin carga)
+void AcLoadCurve(void); // Funcion de aceleración del motor principal (con carga)
+void StopCurve(void); // Funcion de parada
-//Definición de los datos fijos de la trama de envío
+// Definicion de los datos fijos de la trama de envio
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
+int i;
-// Declaración de los datos útiles recibidos
+int entrada[8]; // Definicion de los datos utiles que se enviaran
+
+// Declaracion de los datos utiles recibidos
int d1,d2,d3,d4,d5,d6,d7,d8,d9;
-// Declaración de los análogos
+// Declaracion de los analogos
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
@@ -108,16 +115,11 @@
.
.
.
-
*/
-
-int baudrate=19200; //Se denomina un baudrate (bits/segundo) de 57600
+int baudrate; //Se denomina un baudrate (bits/segundo) de 57600
-DigitalOut myled(LED1);
-
-
-
+DigitalOut status(PTA17); // Led de estado
/*************************************************************************
FUNCIÓN: main()
@@ -125,15 +127,36 @@
**************************************************************************/
int main() {
-
+
+ // Inicializacion de datos del acelerometro
+ datosZ = 0;
+ datosX = 0;
+ datosY = 0;
+
+ // Inicializacion de ciclos de dureza
+ p1 = 0;
+ p2 = 0;
+ p3 = 0;
+ p1a = 0;
+ p2a = 0;
+ p3a = 0;
+
+ // Inicializacion de datos del control del xbox
+ I_cont = 0;
+ cont = 0;
+ Inicio = 0;
+ Med_M = 0;
+ Med_S1 = 0;
+ Med_S2 = 0;
datoNuevo = 0;
- pwmNuevo=0;
+ pwmNuevo = 0;
+
+ // Valor del BaudRate
+ baudrate = 19200;
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
/*
@@ -152,115 +175,117 @@
pwmTask.attach_us(&WaitPWM, 100000);
- while(1) {
-
- myled = !myled;
-
+ while(1) {
+
+ status = !status;
- //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
-
+ 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
+ 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);}
-
+ 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
-
+ 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;
-}
+ 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///////////////////////
+ /////////////Tareas lentas///////////////////////
-//Envío de datos
+ //Envío de datos
if(datoNuevo){
- xbeeSend();
- datoNuevo = 0;
+ 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
- }
+ //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 (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 (p1 != p1a) {
+ Motor.write(p1);
+ }
-
-
- 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 (p2 != p2a) {
+ Direc.write(p2);
+ }
- 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);}
+ 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;
+ }
- p1a=p1;
- p2a=p2;
- p3a=p3;
+ 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);
+ }
- datosX = acc.getAccX(); // datos a almacenar
- datosY = acc.getAccY(); // datos a almacenar
- datosZ = acc.getAccZ(); // datos a almacenar
+ p1a = p1;
+ p2a = p2;
+ p3a = p3;
- pwmNuevo=0;
- }
+ datosX = acc.getAccX(); // datos a almacenar
+ datosY = acc.getAccY(); // datos a almacenar
+ datosZ = acc.getAccZ(); // datos a almacenar
-
-
+ pwmNuevo = 0;
+ }
}
}
@@ -287,26 +312,24 @@
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);
+void AcCurve(void){
+ C_In=(Med_M-1.0)*(T_PWM/T_In);
- p1=(1.0/P_M);
- Motor.write(p1);
- wait(0.250);
+ 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;
- }
+ 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)
@@ -314,124 +337,109 @@
**************************************************************************/
-void AcLoadCurve(void){
+void AcLoadCurve(void){
+ C_Load=(Med_M-1.0)*(T_PWM/T_Load);
-
- C_Load=(Med_M-1.0)*(T_PWM/T_Load);
-
- p1=(1.0/P_M);
- Motor.write(p1);
- wait(0.250);
+ 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;}
- }
+ 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;
- }
+ 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);
+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;}
- }
-
- }
+ 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(){
-
+void xbeeSend(void){
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
+ 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(){
+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
- 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
- {
+ 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
-
+ 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(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
+ 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();
+ for(int j = 1; j >= 11; j++){
+ thisByte = xbee.getc();
+ }
// Luego de contar los 11 datos, se comienzan a tomar los datos útiles. En este caso son 3
thisByte = xbee.getc();
@@ -452,28 +460,14 @@
d8 = thisByte;
thisByte = xbee.getc();
d9 = thisByte;
- }
-
-
- }
-
+ }
+}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
-// ISR -> Interrupt service ruting - para tomar dato y hacer parpadeo (DEBE SER RAPIDA)
+// ISR -> Interrupt service routine - 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);