Hydrocontest / Mbed 2 deprecated 01_07_2014_ACC_baudrate_copy

Prueba 1

Dependencies:   FRDM_MMA8451Q MODSERIAL mbed

main.cpp@0:64bf61ea2ead, 2014-07-01 (annotated)

Committer:
edospinas
Date:
Tue Jul 01 21:45:31 2014 +0000
Revision:
0:64bf61ea2ead
Child:
1:f1cf6444beba
prueba hydros

Who changed what in which revision?

UserRevisionLine numberNew contents of line
edospinas 0:64bf61ea2ead 1 #include "mbed.h"
edospinas 0:64bf61ea2ead 2 #include "MODSERIAL.h"
edospinas 0:64bf61ea2ead 3 #include "MMA8451Q.h"
edospinas 0:64bf61ea2ead 4 #define MMA8451_I2C_ADDRESS (0x1d<<1)
edospinas 0:64bf61ea2ead 5
edospinas 0:64bf61ea2ead 6 // Se configuran los puertos de conexión del acelerometro y TX Rx serial.
edospinas 0:64bf61ea2ead 7 MMA8451Q acc(PTE25, PTE24, MMA8451_I2C_ADDRESS);
edospinas 0:64bf61ea2ead 8 MODSERIAL xbee(PTC4, PTC3); // tx, rx
edospinas 0:64bf61ea2ead 9
edospinas 0:64bf61ea2ead 10
edospinas 0:64bf61ea2ead 11 // Definición de los botones del control de Xbox
edospinas 0:64bf61ea2ead 12 #define A 0x01
edospinas 0:64bf61ea2ead 13 #define X 0x04
edospinas 0:64bf61ea2ead 14 #define Y 0x08
edospinas 0:64bf61ea2ead 15 #define B 0x02
edospinas 0:64bf61ea2ead 16 #define LB 0x10
edospinas 0:64bf61ea2ead 17 #define RB 0x20
edospinas 0:64bf61ea2ead 18 #define Back 0x40
edospinas 0:64bf61ea2ead 19 #define Start 0x80
edospinas 0:64bf61ea2ead 20 #define Back_A 0x41
edospinas 0:64bf61ea2ead 21 #define Back_B 0x42
edospinas 0:64bf61ea2ead 22
edospinas 0:64bf61ea2ead 23
edospinas 0:64bf61ea2ead 24 // Factores de conversión de los análogos
edospinas 0:64bf61ea2ead 25 #define F_LT_RT 0.000030637
edospinas 0:64bf61ea2ead 26 #define F_analog 0.000015258
edospinas 0:64bf61ea2ead 27 #define F_arrows 0.003174603
edospinas 0:64bf61ea2ead 28
edospinas 0:64bf61ea2ead 29 // Definición de el periodo de las señales PWM
edospinas 0:64bf61ea2ead 30 #define P_M 20.0
edospinas 0:64bf61ea2ead 31 #define P_S1 20.0
edospinas 0:64bf61ea2ead 32 #define P_S2 20.0
edospinas 0:64bf61ea2ead 33
edospinas 0:64bf61ea2ead 34 PwmOut Motor(PTD4); // Puerto PWM para manejar el motor Brushless
edospinas 0:64bf61ea2ead 35 PwmOut Direc(PTA12); // Puerto PWM para manejar el servo de direccion
edospinas 0:64bf61ea2ead 36 PwmOut Elev(PTA4); // Puerto PWM para manejar el servo de elevacion
edospinas 0:64bf61ea2ead 37
edospinas 0:64bf61ea2ead 38 float datosZ=0;
edospinas 0:64bf61ea2ead 39 float datosX=0;
edospinas 0:64bf61ea2ead 40 float datosY=0;
edospinas 0:64bf61ea2ead 41 float angleXZ;
edospinas 0:64bf61ea2ead 42 float angleYZ;
edospinas 0:64bf61ea2ead 43
edospinas 0:64bf61ea2ead 44 //
edospinas 0:64bf61ea2ead 45 float p1=0;
edospinas 0:64bf61ea2ead 46 float p2=0;
edospinas 0:64bf61ea2ead 47 float p3=0;
edospinas 0:64bf61ea2ead 48
edospinas 0:64bf61ea2ead 49 float p1a=0;
edospinas 0:64bf61ea2ead 50 float p2a=0;
edospinas 0:64bf61ea2ead 51 float p3a=0;
edospinas 0:64bf61ea2ead 52
edospinas 0:64bf61ea2ead 53 float LT;
edospinas 0:64bf61ea2ead 54 float RT;
edospinas 0:64bf61ea2ead 55 int I_cont=0;
edospinas 0:64bf61ea2ead 56 int cont=0;
edospinas 0:64bf61ea2ead 57 int Inicio=0;
edospinas 0:64bf61ea2ead 58 float Med_M=0;
edospinas 0:64bf61ea2ead 59 float Med_S1=0;
edospinas 0:64bf61ea2ead 60 float Med_S2=0;
edospinas 0:64bf61ea2ead 61 float C_In;
edospinas 0:64bf61ea2ead 62 float C_S2;
edospinas 0:64bf61ea2ead 63 float C_Stop;
edospinas 0:64bf61ea2ead 64 float C_Load;
edospinas 0:64bf61ea2ead 65 float T_In;
edospinas 0:64bf61ea2ead 66 float T_Load;
edospinas 0:64bf61ea2ead 67 float T_Stop;
edospinas 0:64bf61ea2ead 68 float T_S2;
edospinas 0:64bf61ea2ead 69 float T_PWM;
edospinas 0:64bf61ea2ead 70
edospinas 0:64bf61ea2ead 71 Ticker envioTask; // Tarea programada para que se envíen datos cada determinado tiempo
edospinas 0:64bf61ea2ead 72 Ticker pwmTask; // Tarea programada para que se actualice el PWM cada determinado tiempo
edospinas 0:64bf61ea2ead 73
edospinas 0:64bf61ea2ead 74 char datoNuevo;
edospinas 0:64bf61ea2ead 75 char pwmNuevo;
edospinas 0:64bf61ea2ead 76
edospinas 0:64bf61ea2ead 77 /*Definición de funciones*/
edospinas 0:64bf61ea2ead 78 void TxSend(void); //Función de interrupción
edospinas 0:64bf61ea2ead 79 void WaitPWM(void); //Función de interrupción
edospinas 0:64bf61ea2ead 80 void xbeeRead(void); //Función de lectura de paquetes
edospinas 0:64bf61ea2ead 81 void xbeeSend(); //Función de escritura de paquetes
edospinas 0:64bf61ea2ead 82 void initPWM(void); //Función de inicialización de los PWM
edospinas 0:64bf61ea2ead 83
edospinas 0:64bf61ea2ead 84
edospinas 0:64bf61ea2ead 85 void AcCurve(void); //Función de aceleración del motor principal
edospinas 0:64bf61ea2ead 86 void AcLoadCurve(void); //Función de aceleración del motor principal
edospinas 0:64bf61ea2ead 87 void StopCurve(void);
edospinas 0:64bf61ea2ead 88
edospinas 0:64bf61ea2ead 89
edospinas 0:64bf61ea2ead 90 //Definición de los datos fijos de la trama de envío
edospinas 0:64bf61ea2ead 91 int trama[17]={0x7E,0x00,0x11,0x10,0x01,0x00,0x13,0xA2,0x00,0x40,0xB5,0xE7,0x51,0xFF,0xFE,0x00,0x00};
edospinas 0:64bf61ea2ead 92
edospinas 0:64bf61ea2ead 93 int i,entrada[8]; //Definición de los datos útiles que se enviarán
edospinas 0:64bf61ea2ead 94
edospinas 0:64bf61ea2ead 95 // Declaración de los datos útiles recibidos
edospinas 0:64bf61ea2ead 96 int d1,d2,d3,d4,d5,d6,d7,d8,d9;
edospinas 0:64bf61ea2ead 97
edospinas 0:64bf61ea2ead 98 // Declaración de los análogos
edospinas 0:64bf61ea2ead 99 int Left_Xaxis,Left_Yaxis,LT_RT,Arrows;
edospinas 0:64bf61ea2ead 100
edospinas 0:64bf61ea2ead 101 int V[24]; //Se nombra un vector que tendrá los datos que se sumarán para calcular el valor del checksum
edospinas 0:64bf61ea2ead 102 /*El tamaño de este depende del número de datos útiles, para:
edospinas 0:64bf61ea2ead 103 1 dato 17
edospinas 0:64bf61ea2ead 104 2 datos 18
edospinas 0:64bf61ea2ead 105 3 datos 19
edospinas 0:64bf61ea2ead 106 4 datos 20
edospinas 0:64bf61ea2ead 107 5 datos 21
edospinas 0:64bf61ea2ead 108 .
edospinas 0:64bf61ea2ead 109 .
edospinas 0:64bf61ea2ead 110 .
edospinas 0:64bf61ea2ead 111
edospinas 0:64bf61ea2ead 112 */
edospinas 0:64bf61ea2ead 113
edospinas 0:64bf61ea2ead 114
edospinas 0:64bf61ea2ead 115 int baudrate=19200; //Se denomina un baudrate (bits/segundo) de 57600
edospinas 0:64bf61ea2ead 116
edospinas 0:64bf61ea2ead 117 DigitalOut myled(LED1);
edospinas 0:64bf61ea2ead 118
edospinas 0:64bf61ea2ead 119
edospinas 0:64bf61ea2ead 120
edospinas 0:64bf61ea2ead 121
edospinas 0:64bf61ea2ead 122 /*************************************************************************
edospinas 0:64bf61ea2ead 123 FUNCIÓN: main()
edospinas 0:64bf61ea2ead 124 PROPÓSITO: ESTA ES LA FUNCIÓN PRINCIPAL DEL PROGRAMA
edospinas 0:64bf61ea2ead 125 **************************************************************************/
edospinas 0:64bf61ea2ead 126
edospinas 0:64bf61ea2ead 127 int main() {
edospinas 0:64bf61ea2ead 128
edospinas 0:64bf61ea2ead 129 datoNuevo = 0;
edospinas 0:64bf61ea2ead 130 pwmNuevo=0;
edospinas 0:64bf61ea2ead 131 xbee.baud(baudrate);
edospinas 0:64bf61ea2ead 132
edospinas 0:64bf61ea2ead 133 initPWM();
edospinas 0:64bf61ea2ead 134
edospinas 0:64bf61ea2ead 135
edospinas 0:64bf61ea2ead 136
edospinas 0:64bf61ea2ead 137 trama[2]=0x16; // Este valor es del número de datos desde luego de la longitud hasta antes del checksum,
edospinas 0:64bf61ea2ead 138 // en este caso son 3 datos y se tiene una longitud de 17 datos= 0x11
edospinas 0:64bf61ea2ead 139 /*
edospinas 0:64bf61ea2ead 140 La longitud varía según el número de datos útiles a enviar, para:
edospinas 0:64bf61ea2ead 141 1 dato 0F
edospinas 0:64bf61ea2ead 142 2 datos 10
edospinas 0:64bf61ea2ead 143 3 datos 11
edospinas 0:64bf61ea2ead 144 4 datos 12
edospinas 0:64bf61ea2ead 145 5 datos 13
edospinas 0:64bf61ea2ead 146 .
edospinas 0:64bf61ea2ead 147 .
edospinas 0:64bf61ea2ead 148 .
edospinas 0:64bf61ea2ead 149 */
edospinas 0:64bf61ea2ead 150
edospinas 0:64bf61ea2ead 151 envioTask.attach_us(&TxSend, 250000);
edospinas 0:64bf61ea2ead 152 pwmTask.attach_us(&WaitPWM, 100000);
edospinas 0:64bf61ea2ead 153
edospinas 0:64bf61ea2ead 154
edospinas 0:64bf61ea2ead 155 while(1) {
edospinas 0:64bf61ea2ead 156
edospinas 0:64bf61ea2ead 157 myled = !myled;
edospinas 0:64bf61ea2ead 158
edospinas 0:64bf61ea2ead 159
edospinas 0:64bf61ea2ead 160 //Definición de variables de arranque y parada
edospinas 0:64bf61ea2ead 161 T_In=3.0;
edospinas 0:64bf61ea2ead 162 T_Load=6.0;
edospinas 0:64bf61ea2ead 163 T_Stop=1.0;
edospinas 0:64bf61ea2ead 164 T_S2=2.0;
edospinas 0:64bf61ea2ead 165 Med_M=1.26;
edospinas 0:64bf61ea2ead 166 Med_S1=1.66;
edospinas 0:64bf61ea2ead 167 Med_S2=1.5;
edospinas 0:64bf61ea2ead 168 T_PWM=0.100;
edospinas 0:64bf61ea2ead 169
edospinas 0:64bf61ea2ead 170
edospinas 0:64bf61ea2ead 171 xbeeRead(); //Leer datos que llegan del control
edospinas 0:64bf61ea2ead 172
edospinas 0:64bf61ea2ead 173
edospinas 0:64bf61ea2ead 174 switch(d1) //El dato número 1 (d1) contiene los valores discretos más importantes del control
edospinas 0:64bf61ea2ead 175 {
edospinas 0:64bf61ea2ead 176
edospinas 0:64bf61ea2ead 177 case LB: //Si el dato1 es L1 (0x40) el barco debe frenar
edospinas 0:64bf61ea2ead 178 p1=(1.0/P_M);
edospinas 0:64bf61ea2ead 179 break;
edospinas 0:64bf61ea2ead 180 case RB: //Si el dato1 es R1 (0x80) el barco debe acelerar al máximo
edospinas 0:64bf61ea2ead 181 p1=(Med_M-1.0)+(Med_M);
edospinas 0:64bf61ea2ead 182 break;
edospinas 0:64bf61ea2ead 183 case X:
edospinas 0:64bf61ea2ead 184 p3=(Med_S2/P_S2);
edospinas 0:64bf61ea2ead 185 break;
edospinas 0:64bf61ea2ead 186 case Y:
edospinas 0:64bf61ea2ead 187 p2=((Med_S1+0.03)/P_S1);
edospinas 0:64bf61ea2ead 188 break;
edospinas 0:64bf61ea2ead 189 default:
edospinas 0:64bf61ea2ead 190 Left_Xaxis = ((unsigned int)d2 << 8) | (unsigned int) d3; //Concatena el dato 2 (d2) y el dato 3 (d3) para obtener A1_EjeX
edospinas 0:64bf61ea2ead 191
edospinas 0:64bf61ea2ead 192 if ((Left_Xaxis<=32000)|(Left_Xaxis>=33000)){ p2=(1.0/P_S1)+(F_analog*Left_Xaxis)*((Med_S1-1.0)*(2.0/P_S1));}
edospinas 0:64bf61ea2ead 193 else {p2=((Med_S1+0.03)/P_S1);}
edospinas 0:64bf61ea2ead 194
edospinas 0:64bf61ea2ead 195 //Left_Yaxis = ((unsigned int)d4 << 8) | (unsigned int) d5; //Concatena el dato 4 (d4) y el dato 5 (d5) para obtener A1_EjeY
edospinas 0:64bf61ea2ead 196
edospinas 0:64bf61ea2ead 197 //p3=0.05+(0.00001625*A1_Y)*0.05;
edospinas 0:64bf61ea2ead 198
edospinas 0:64bf61ea2ead 199 LT_RT = ((unsigned int)d6 << 8) | (unsigned int) d7; //Concatena el dato 6 (d6) y el dato 7 (d7) para obtener A2_EjeX
edospinas 0:64bf61ea2ead 200
edospinas 0:64bf61ea2ead 201
edospinas 0:64bf61ea2ead 202 Arrows = ((unsigned int)d8 << 8) | (unsigned int) d9; //Concatena el dato 8 (d8) y el dato 9 (d9) para obtener A2_EjeY
edospinas 0:64bf61ea2ead 203
edospinas 0:64bf61ea2ead 204 break;
edospinas 0:64bf61ea2ead 205 }
edospinas 0:64bf61ea2ead 206
edospinas 0:64bf61ea2ead 207
edospinas 0:64bf61ea2ead 208
edospinas 0:64bf61ea2ead 209 /////////////Tareas lentas///////////////////////
edospinas 0:64bf61ea2ead 210
edospinas 0:64bf61ea2ead 211 //Envío de datos
edospinas 0:64bf61ea2ead 212 if(datoNuevo){
edospinas 0:64bf61ea2ead 213 xbeeSend();
edospinas 0:64bf61ea2ead 214 datoNuevo = 0;
edospinas 0:64bf61ea2ead 215 }
edospinas 0:64bf61ea2ead 216
edospinas 0:64bf61ea2ead 217 //Actualización del valor de PWM
edospinas 0:64bf61ea2ead 218 if (pwmNuevo){
edospinas 0:64bf61ea2ead 219
edospinas 0:64bf61ea2ead 220 if ((Inicio==0)&&(d1==Back_A)){ AcCurve();}
edospinas 0:64bf61ea2ead 221 else if ((Inicio==0)&&(d1==Back_B)){ AcLoadCurve();}
edospinas 0:64bf61ea2ead 222 else if ((Inicio==1)&&(d1==Start)){ StopCurve(); Inicio=0;}
edospinas 0:64bf61ea2ead 223 else if (Inicio==1){
edospinas 0:64bf61ea2ead 224 //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.
edospinas 0:64bf61ea2ead 225 // Con este if se identifica si se presionó LT y RT. Para luego escalar a valores desde 0 hasta 1
edospinas 0:64bf61ea2ead 226 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
edospinas 0:64bf61ea2ead 227 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
edospinas 0:64bf61ea2ead 228 else if (LT_RT==32768){p1=(Med_M/P_M);} //p1 es 1,6 si no presiona ni LT ni RT
edospinas 0:64bf61ea2ead 229 }
edospinas 0:64bf61ea2ead 230
edospinas 0:64bf61ea2ead 231
edospinas 0:64bf61ea2ead 232 if (cont<=10){p2=Med_S1/P_S1; p3=1.5/P_S2; cont++;}
edospinas 0:64bf61ea2ead 233 else {cont=12;}
edospinas 0:64bf61ea2ead 234
edospinas 0:64bf61ea2ead 235 if (p1 != p1a) {Motor.write(p1);}
edospinas 0:64bf61ea2ead 236
edospinas 0:64bf61ea2ead 237 if (p2 != p2a) {Direc.write(p2);}
edospinas 0:64bf61ea2ead 238
edospinas 0:64bf61ea2ead 239 C_S2=(Med_S2-1.05555)*(T_PWM/T_S2);
edospinas 0:64bf61ea2ead 240
edospinas 0:64bf61ea2ead 241
edospinas 0:64bf61ea2ead 242
edospinas 0:64bf61ea2ead 243 if ((Arrows==315)|(Arrows==0)|(Arrows==45)){p3=p3+C_S2/P_S2;}
edospinas 0:64bf61ea2ead 244 else if ((Arrows==225)|(Arrows==180)|(Arrows==135)) {p3=p3-C_S2/P_S2;}
edospinas 0:64bf61ea2ead 245
edospinas 0:64bf61ea2ead 246 if (p3<=1.05555/P_S2){p3=1.05555/P_S2;}
edospinas 0:64bf61ea2ead 247 if (p3>=1.94444/P_S2){p3=1.94444/P_S2;}
edospinas 0:64bf61ea2ead 248
edospinas 0:64bf61ea2ead 249 if (p3 != p3a) {Elev.write(p3);}
edospinas 0:64bf61ea2ead 250
edospinas 0:64bf61ea2ead 251 p1a=p1;
edospinas 0:64bf61ea2ead 252 p2a=p2;
edospinas 0:64bf61ea2ead 253 p3a=p3;
edospinas 0:64bf61ea2ead 254
edospinas 0:64bf61ea2ead 255 datosX = acc.getAccX(); // datos a almacenar
edospinas 0:64bf61ea2ead 256 datosY = acc.getAccY(); // datos a almacenar
edospinas 0:64bf61ea2ead 257 datosZ = acc.getAccZ(); // datos a almacenar
edospinas 0:64bf61ea2ead 258
edospinas 0:64bf61ea2ead 259 pwmNuevo=0;
edospinas 0:64bf61ea2ead 260 }
edospinas 0:64bf61ea2ead 261
edospinas 0:64bf61ea2ead 262
edospinas 0:64bf61ea2ead 263
edospinas 0:64bf61ea2ead 264 }
edospinas 0:64bf61ea2ead 265 }
edospinas 0:64bf61ea2ead 266
edospinas 0:64bf61ea2ead 267
edospinas 0:64bf61ea2ead 268 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
edospinas 0:64bf61ea2ead 269 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
edospinas 0:64bf61ea2ead 270
edospinas 0:64bf61ea2ead 271 /*************************************************************************
edospinas 0:64bf61ea2ead 272 FUNCIÓN: initPWM()
edospinas 0:64bf61ea2ead 273 PROPÓSITO: Inicializar los PWM
edospinas 0:64bf61ea2ead 274 **************************************************************************/
edospinas 0:64bf61ea2ead 275
edospinas 0:64bf61ea2ead 276 void initPWM(void){
edospinas 0:64bf61ea2ead 277 Motor.period_ms(P_M);
edospinas 0:64bf61ea2ead 278 Motor.write(1.0/P_M);
edospinas 0:64bf61ea2ead 279 Direc.period_ms(P_S1);
edospinas 0:64bf61ea2ead 280 Direc.write(Med_S1/P_S1);
edospinas 0:64bf61ea2ead 281 Elev.period_ms(P_S2);
edospinas 0:64bf61ea2ead 282 Elev.write(Med_S2/P_S2);
edospinas 0:64bf61ea2ead 283 wait(0.2);
edospinas 0:64bf61ea2ead 284 }
edospinas 0:64bf61ea2ead 285
edospinas 0:64bf61ea2ead 286 /*************************************************************************
edospinas 0:64bf61ea2ead 287 FUNCIÓN: AcCurve(void)
edospinas 0:64bf61ea2ead 288 PROPÓSITO: Función de aceleración del motor a partir de velocidad cero
edospinas 0:64bf61ea2ead 289 **************************************************************************/
edospinas 0:64bf61ea2ead 290
edospinas 0:64bf61ea2ead 291
edospinas 0:64bf61ea2ead 292 void AcCurve(void){
edospinas 0:64bf61ea2ead 293
edospinas 0:64bf61ea2ead 294
edospinas 0:64bf61ea2ead 295
edospinas 0:64bf61ea2ead 296 C_In=(Med_M-1.0)*(T_PWM/T_In);
edospinas 0:64bf61ea2ead 297
edospinas 0:64bf61ea2ead 298 p1=(1.0/P_M);
edospinas 0:64bf61ea2ead 299 Motor.write(p1);
edospinas 0:64bf61ea2ead 300 wait(0.250);
edospinas 0:64bf61ea2ead 301
edospinas 0:64bf61ea2ead 302 for (int ii=0;ii<=ceil(T_In/T_PWM);ii++) {
edospinas 0:64bf61ea2ead 303 if (p1 < (Med_M/P_M)) {p1=p1+C_In/P_M; Motor.write(p1); wait(T_PWM);}
edospinas 0:64bf61ea2ead 304 else if (p1 >= (Med_M/P_M)){p1=Med_M/P_M; }
edospinas 0:64bf61ea2ead 305 }
edospinas 0:64bf61ea2ead 306
edospinas 0:64bf61ea2ead 307
edospinas 0:64bf61ea2ead 308 Inicio=1;
edospinas 0:64bf61ea2ead 309 }
edospinas 0:64bf61ea2ead 310
edospinas 0:64bf61ea2ead 311 /*************************************************************************
edospinas 0:64bf61ea2ead 312 FUNCIÓN: AcLoadCurve(void)
edospinas 0:64bf61ea2ead 313 PROPÓSITO: Función de aceleración del motor a partir de velocidad cero
edospinas 0:64bf61ea2ead 314 **************************************************************************/
edospinas 0:64bf61ea2ead 315
edospinas 0:64bf61ea2ead 316
edospinas 0:64bf61ea2ead 317 void AcLoadCurve(void){
edospinas 0:64bf61ea2ead 318
edospinas 0:64bf61ea2ead 319
edospinas 0:64bf61ea2ead 320 C_Load=(Med_M-1.0)*(T_PWM/T_Load);
edospinas 0:64bf61ea2ead 321
edospinas 0:64bf61ea2ead 322 p1=(1.0/P_M);
edospinas 0:64bf61ea2ead 323 Motor.write(p1);
edospinas 0:64bf61ea2ead 324 wait(0.250);
edospinas 0:64bf61ea2ead 325
edospinas 0:64bf61ea2ead 326 for (int ii=0;ii<=ceil(T_Load/T_PWM);ii++) {
edospinas 0:64bf61ea2ead 327 if (p1 < (Med_M/P_M)) {p1=p1+C_Load/P_M; Motor.write(p1); wait(T_PWM);}
edospinas 0:64bf61ea2ead 328 else if (p1 >= (Med_M/P_M)){p1=Med_M/P_M;}
edospinas 0:64bf61ea2ead 329 }
edospinas 0:64bf61ea2ead 330
edospinas 0:64bf61ea2ead 331 Inicio=1;
edospinas 0:64bf61ea2ead 332 }
edospinas 0:64bf61ea2ead 333
edospinas 0:64bf61ea2ead 334 /*************************************************************************
edospinas 0:64bf61ea2ead 335 FUNCIÓN: StopCurve(void)
edospinas 0:64bf61ea2ead 336 PROPÓSITO: función de parada del motor.
edospinas 0:64bf61ea2ead 337 **************************************************************************/
edospinas 0:64bf61ea2ead 338
edospinas 0:64bf61ea2ead 339 void StopCurve(void){
edospinas 0:64bf61ea2ead 340
edospinas 0:64bf61ea2ead 341 C_Stop=(Med_M-1.0)*(T_PWM/T_Stop);
edospinas 0:64bf61ea2ead 342
edospinas 0:64bf61ea2ead 343 for (int ii=0;ii<=ceil(T_Stop/T_PWM);ii++){
edospinas 0:64bf61ea2ead 344 if (p1 > 1.0/P_M) {p1=p1-C_Stop/P_M; Motor.write(p1); wait(T_PWM);}
edospinas 0:64bf61ea2ead 345 else if (p1 <= 1.0/P_M) {p1=1.0/P_M;}
edospinas 0:64bf61ea2ead 346 }
edospinas 0:64bf61ea2ead 347
edospinas 0:64bf61ea2ead 348 }
edospinas 0:64bf61ea2ead 349 /*************************************************************************
edospinas 0:64bf61ea2ead 350 FUNCIÓN: xbeeRead()
edospinas 0:64bf61ea2ead 351 PROPÓSITO: ESCRIBIR UN PAQUETE DE DATOS API EN EL XBEE CON LOS DATOS ÚTILES
edospinas 0:64bf61ea2ead 352 **************************************************************************/
edospinas 0:64bf61ea2ead 353
edospinas 0:64bf61ea2ead 354
edospinas 0:64bf61ea2ead 355 void xbeeSend(){
edospinas 0:64bf61ea2ead 356
edospinas 0:64bf61ea2ead 357 int checksum=0x00; //Se inicializa el cálculo del checksum por cada iteración
edospinas 0:64bf61ea2ead 358
edospinas 0:64bf61ea2ead 359 for (i=3;i<=24;i++){
edospinas 0:64bf61ea2ead 360
edospinas 0:64bf61ea2ead 361 //Se nombra un nuevo vector con los datos que va desde luego de la longitud (3) hasta antes del checksum (19)
edospinas 0:64bf61ea2ead 362 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
edospinas 0:64bf61ea2ead 363 //por ende el condicional tomaría desde 3-16
edospinas 0:64bf61ea2ead 364 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
edospinas 0:64bf61ea2ead 365 // Si se envían 4 datos útiles la condición sería hasta 20, 5 datos útil iría hasta 21...
edospinas 0:64bf61ea2ead 366
edospinas 0:64bf61ea2ead 367 checksum=checksum+V[i]; //Se suma desde el dato siguiente a la longitud de
edospinas 0:64bf61ea2ead 368 //la trama hasta el último dato útil a enviar
edospinas 0:64bf61ea2ead 369
edospinas 0:64bf61ea2ead 370 }
edospinas 0:64bf61ea2ead 371
edospinas 0:64bf61ea2ead 372
edospinas 0:64bf61ea2ead 373 // FORMULA CHECKSUM
edospinas 0:64bf61ea2ead 374
edospinas 0:64bf61ea2ead 375 checksum=0xFF-checksum; //Se realiza la operación para la fórmula del ckecksum
edospinas 0:64bf61ea2ead 376
edospinas 0:64bf61ea2ead 377
edospinas 0:64bf61ea2ead 378 // Primero se comprueba que el puerto esté libre para escribir
edospinas 0:64bf61ea2ead 379 if ((xbee.writeable())){
edospinas 0:64bf61ea2ead 380 for (i=0;i<=16;i++){xbee.putc(trama[i]);}//Envía todos los datos fijos de la trama de envío
edospinas 0:64bf61ea2ead 381 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
edospinas 0:64bf61ea2ead 382 }
edospinas 0:64bf61ea2ead 383
edospinas 0:64bf61ea2ead 384 }
edospinas 0:64bf61ea2ead 385
edospinas 0:64bf61ea2ead 386
edospinas 0:64bf61ea2ead 387 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
edospinas 0:64bf61ea2ead 388 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
edospinas 0:64bf61ea2ead 389
edospinas 0:64bf61ea2ead 390
edospinas 0:64bf61ea2ead 391
edospinas 0:64bf61ea2ead 392 /*************************************************************************
edospinas 0:64bf61ea2ead 393 FUNCIÓN: xbeeRead()
edospinas 0:64bf61ea2ead 394 PROPÓSITO: LEER UN PAQUETE DE DATOS DEL XBee Y SACAR DATOS ÚTILES
edospinas 0:64bf61ea2ead 395 **************************************************************************/
edospinas 0:64bf61ea2ead 396
edospinas 0:64bf61ea2ead 397 void xbeeRead(){
edospinas 0:64bf61ea2ead 398
edospinas 0:64bf61ea2ead 399 char packetFound = 0; //Esta variable, indicará si se ha encontrado un paquete o no.
edospinas 0:64bf61ea2ead 400 char thisByte; //Esta variable tomará valores de la trama de datos recibida
edospinas 0:64bf61ea2ead 401
edospinas 0:64bf61ea2ead 402 if(xbee.rxBufferGetCount() == 0) //Si en el Buffer no ha llegado ningún dato, el programa volverá a leerlo
edospinas 0:64bf61ea2ead 403 {
edospinas 0:64bf61ea2ead 404 return;
edospinas 0:64bf61ea2ead 405 }
edospinas 0:64bf61ea2ead 406 while(xbee.rxBufferGetCount() > 14){ //Si llegaron más de 14 palabras al buffer, compruebe que sea la trama correcta
edospinas 0:64bf61ea2ead 407
edospinas 0:64bf61ea2ead 408 thisByte = xbee.getc(); //Toma el primer valor de la trama recibida.
edospinas 0:64bf61ea2ead 409 if(thisByte == 0x7E){ //Si el primer dato de la trama es 0x7E, continúe...
edospinas 0:64bf61ea2ead 410 thisByte = xbee.getc(); //Toma el MSB de la longitud
edospinas 0:64bf61ea2ead 411 thisByte = xbee.getc(); //Toma el LSB de la longitud
edospinas 0:64bf61ea2ead 412 thisByte = xbee.getc(); //Toma el tipo de paquete
edospinas 0:64bf61ea2ead 413 if(thisByte == 0x90){ //Si recibió que el paquete es de tipo: Rx (0x90), continúe...
edospinas 0:64bf61ea2ead 414 // packet is rx type
edospinas 0:64bf61ea2ead 415 packetFound = 1; //Se indica que se encontró la trama de recepción
edospinas 0:64bf61ea2ead 416 break; //Si se ubica la trama, se sale del ciclo while
edospinas 0:64bf61ea2ead 417 }
edospinas 0:64bf61ea2ead 418 }
edospinas 0:64bf61ea2ead 419 }
edospinas 0:64bf61ea2ead 420
edospinas 0:64bf61ea2ead 421 if(packetFound == 1){ // Se encontró un paquete, entonces se toman los siguientes 11 datos.
edospinas 0:64bf61ea2ead 422 // Estos 11 datos van hasta un dato antes del primer dato útil
edospinas 0:64bf61ea2ead 423
edospinas 0:64bf61ea2ead 424 thisByte = xbee.getc();
edospinas 0:64bf61ea2ead 425 thisByte = xbee.getc();
edospinas 0:64bf61ea2ead 426 thisByte = xbee.getc();
edospinas 0:64bf61ea2ead 427 thisByte = xbee.getc();
edospinas 0:64bf61ea2ead 428 thisByte = xbee.getc();
edospinas 0:64bf61ea2ead 429 thisByte = xbee.getc();
edospinas 0:64bf61ea2ead 430 thisByte = xbee.getc();
edospinas 0:64bf61ea2ead 431 thisByte = xbee.getc();
edospinas 0:64bf61ea2ead 432 thisByte = xbee.getc();
edospinas 0:64bf61ea2ead 433 thisByte = xbee.getc();
edospinas 0:64bf61ea2ead 434 thisByte = xbee.getc();
edospinas 0:64bf61ea2ead 435
edospinas 0:64bf61ea2ead 436 // Luego de contar los 11 datos, se comienzan a tomar los datos útiles. En este caso son 3
edospinas 0:64bf61ea2ead 437 thisByte = xbee.getc();
edospinas 0:64bf61ea2ead 438 d1 = thisByte;
edospinas 0:64bf61ea2ead 439 thisByte = xbee.getc();
edospinas 0:64bf61ea2ead 440 d2 = thisByte;
edospinas 0:64bf61ea2ead 441 thisByte = xbee.getc();
edospinas 0:64bf61ea2ead 442 d3 = thisByte;
edospinas 0:64bf61ea2ead 443 thisByte = xbee.getc();
edospinas 0:64bf61ea2ead 444 d4 = thisByte;
edospinas 0:64bf61ea2ead 445 thisByte = xbee.getc();
edospinas 0:64bf61ea2ead 446 d5 = thisByte;
edospinas 0:64bf61ea2ead 447 thisByte = xbee.getc();
edospinas 0:64bf61ea2ead 448 d6 = thisByte;
edospinas 0:64bf61ea2ead 449 thisByte = xbee.getc();
edospinas 0:64bf61ea2ead 450 d7 = thisByte;
edospinas 0:64bf61ea2ead 451 thisByte = xbee.getc();
edospinas 0:64bf61ea2ead 452 d8 = thisByte;
edospinas 0:64bf61ea2ead 453 thisByte = xbee.getc();
edospinas 0:64bf61ea2ead 454 d9 = thisByte;
edospinas 0:64bf61ea2ead 455 }
edospinas 0:64bf61ea2ead 456
edospinas 0:64bf61ea2ead 457
edospinas 0:64bf61ea2ead 458 }
edospinas 0:64bf61ea2ead 459
edospinas 0:64bf61ea2ead 460
edospinas 0:64bf61ea2ead 461 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
edospinas 0:64bf61ea2ead 462 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
edospinas 0:64bf61ea2ead 463
edospinas 0:64bf61ea2ead 464
edospinas 0:64bf61ea2ead 465 // ISR -> Interrupt service ruting - para tomar dato y hacer parpadeo (DEBE SER RAPIDA)
edospinas 0:64bf61ea2ead 466 void TxSend(void){
edospinas 0:64bf61ea2ead 467
edospinas 0:64bf61ea2ead 468 //datosX = 1.0; // datos a almacenar
edospinas 0:64bf61ea2ead 469 //datosY = 1.0; // datos a almacenar
edospinas 0:64bf61ea2ead 470 //datosZ = 1.0; // datos a almacenar
edospinas 0:64bf61ea2ead 471
edospinas 0:64bf61ea2ead 472 //datosX = acc.getAccX(); // datos a almacenar
edospinas 0:64bf61ea2ead 473 //datosY = acc.getAccY(); // datos a almacenar
edospinas 0:64bf61ea2ead 474 //datosZ = acc.getAccZ(); // datos a almacenar
edospinas 0:64bf61ea2ead 475
edospinas 0:64bf61ea2ead 476
edospinas 0:64bf61ea2ead 477 angleXZ = atan((datosX)/(datosZ));
edospinas 0:64bf61ea2ead 478 angleXZ = angleXZ*(57.2958);
edospinas 0:64bf61ea2ead 479
edospinas 0:64bf61ea2ead 480 angleYZ = atan((datosY)/(datosZ));
edospinas 0:64bf61ea2ead 481 angleYZ = angleYZ*(57.2958);
edospinas 0:64bf61ea2ead 482
edospinas 0:64bf61ea2ead 483 char * b1 = (char *) &angleXZ;
edospinas 0:64bf61ea2ead 484 char * b2 = (char *) &angleYZ;
edospinas 0:64bf61ea2ead 485
edospinas 0:64bf61ea2ead 486 entrada[0]= b1[3];
edospinas 0:64bf61ea2ead 487 entrada[1]= b1[2];
edospinas 0:64bf61ea2ead 488 entrada[2]= b1[1];
edospinas 0:64bf61ea2ead 489 entrada[3]= b1[0];
edospinas 0:64bf61ea2ead 490 entrada[4]= b2[3];
edospinas 0:64bf61ea2ead 491 entrada[5]= b2[2];
edospinas 0:64bf61ea2ead 492 entrada[6]= b2[1];
edospinas 0:64bf61ea2ead 493 entrada[7]= b2[0];
edospinas 0:64bf61ea2ead 494
edospinas 0:64bf61ea2ead 495 datoNuevo = 1; // Indicamos que tenemos un dato nuevo
edospinas 0:64bf61ea2ead 496 }
edospinas 0:64bf61ea2ead 497
edospinas 0:64bf61ea2ead 498 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
edospinas 0:64bf61ea2ead 499 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
edospinas 0:64bf61ea2ead 500
edospinas 0:64bf61ea2ead 501
edospinas 0:64bf61ea2ead 502 // ISR -> Interrupción para la actualización del PWM
edospinas 0:64bf61ea2ead 503 void WaitPWM(void){
edospinas 0:64bf61ea2ead 504 pwmNuevo = 1; // Indicamos que tenemos un dato nuevo
edospinas 0:64bf61ea2ead 505 }
edospinas 0:64bf61ea2ead 506