Eduardo Ospina / Mbed 2 deprecated Receptor_Hydros_FlySky_send

Receptor

Dependencies:   FRDM_MMA8451Q MODSERIAL mbed

main.cpp@0:079fc298efbb, 2015-06-10 (annotated)

Committer:
edospinas
Date:
Wed Jun 10 00:29:00 2015 +0000
Revision:
0:079fc298efbb
h

Who changed what in which revision?

UserRevisionLine numberNew contents of line
edospinas 0:079fc298efbb 1 #include "mbed.h"
edospinas 0:079fc298efbb 2 #include "MODSERIAL.h"
edospinas 0:079fc298efbb 3 #include "MMA8451Q.h"
edospinas 0:079fc298efbb 4 #define MMA8451_I2C_ADDRESS (0x1d<<1)
edospinas 0:079fc298efbb 5
edospinas 0:079fc298efbb 6
edospinas 0:079fc298efbb 7 // Definición de el periodo de las señales PWM
edospinas 0:079fc298efbb 8 #define P_M 20
edospinas 0:079fc298efbb 9 #define P_S1 20
edospinas 0:079fc298efbb 10 #define F_C 0.00001525878
edospinas 0:079fc298efbb 11
edospinas 0:079fc298efbb 12 float p1 = 0.0;
edospinas 0:079fc298efbb 13 float p2 = 0.0;
edospinas 0:079fc298efbb 14
edospinas 0:079fc298efbb 15 PwmOut Motor(PTD4); // Puerto PWM para manejar el motor Brushless
edospinas 0:079fc298efbb 16 PwmOut Direc(PTC9); // Puerto PWM para manejar el servo de direccion
edospinas 0:079fc298efbb 17
edospinas 0:079fc298efbb 18 // Map function
edospinas 0:079fc298efbb 19 float map(float x, float in_min, float in_max, float out_min, float out_max)
edospinas 0:079fc298efbb 20 {
edospinas 0:079fc298efbb 21 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
edospinas 0:079fc298efbb 22 }
edospinas 0:079fc298efbb 23
edospinas 0:079fc298efbb 24 int Baudrate=57600; //Se denomina un baudrate (bits/segundo) de 57600
edospinas 0:079fc298efbb 25
edospinas 0:079fc298efbb 26 Serial pc(USBTX, USBRX);
edospinas 0:079fc298efbb 27 Serial Arduino(PTD3, PTD2);
edospinas 0:079fc298efbb 28
edospinas 0:079fc298efbb 29 // Se configuran los puertos de conexión del acelerometro y TX Rx serial.
edospinas 0:079fc298efbb 30 MMA8451Q acc(PTE25, PTE24, MMA8451_I2C_ADDRESS);
edospinas 0:079fc298efbb 31 MODSERIAL xbee(PTC4, PTC3); // tx, rx
edospinas 0:079fc298efbb 32
edospinas 0:079fc298efbb 33
edospinas 0:079fc298efbb 34 // Definición de los botones del control de Xbox
edospinas 0:079fc298efbb 35 #define ON 0x00
edospinas 0:079fc298efbb 36 #define OFF 0x01
edospinas 0:079fc298efbb 37
edospinas 0:079fc298efbb 38 // Factores de conversión de los análogos
edospinas 0:079fc298efbb 39 #define F_int 0.00003051757
edospinas 0:079fc298efbb 40
edospinas 0:079fc298efbb 41
edospinas 0:079fc298efbb 42 float datosZ=0;
edospinas 0:079fc298efbb 43 float datosX=0;
edospinas 0:079fc298efbb 44 float datosY=0;
edospinas 0:079fc298efbb 45 float angleYX;
edospinas 0:079fc298efbb 46 float angleZX;
edospinas 0:079fc298efbb 47
edospinas 0:079fc298efbb 48 int count;
edospinas 0:079fc298efbb 49
edospinas 0:079fc298efbb 50 Ticker envioTask; // Tarea programada para que se envíen datos cada determinado tiempo
edospinas 0:079fc298efbb 51 Ticker pwmTask; // Tarea programada para que se actualice el PWM cada determinado tiempo
edospinas 0:079fc298efbb 52
edospinas 0:079fc298efbb 53 char datoNuevo;
edospinas 0:079fc298efbb 54 char pwmNuevo;
edospinas 0:079fc298efbb 55
edospinas 0:079fc298efbb 56 /*Definición de funciones*/
edospinas 0:079fc298efbb 57 void TxSend(void); //Función de interrupción
edospinas 0:079fc298efbb 58 void WaitPWM(void); //Función de interrupción
edospinas 0:079fc298efbb 59 void xbeeRead(void); //Función de lectura de paquetes
edospinas 0:079fc298efbb 60 void xbeeSend(); //Función de escritura de paquetes
edospinas 0:079fc298efbb 61 void initPWM(void); //Función de inicialización de los PWM
edospinas 0:079fc298efbb 62
edospinas 0:079fc298efbb 63 //Definición de los datos fijos de la trama de envío
edospinas 0:079fc298efbb 64 int trama[17]={0x7E,0x00,0x11,0x10,0x01,0x00,0x13,0xA2,0x00,0x40,0xAF,0x5A,0xB6,0xFF,0xFE,0x00,0x00};
edospinas 0:079fc298efbb 65
edospinas 0:079fc298efbb 66 int i,entrada[8]; //Definición de los datos útiles que se enviarán
edospinas 0:079fc298efbb 67
edospinas 0:079fc298efbb 68 // Declaración de los datos útiles recibidos
edospinas 0:079fc298efbb 69 int d1[2],d2[2],d3[2],d4[2],d5[2],d6[2];
edospinas 0:079fc298efbb 70
edospinas 0:079fc298efbb 71 // Declaración de los análogos
edospinas 0:079fc298efbb 72 int Yaw,Pitch,Roll,Throttle, Aux_A, Aux_B;
edospinas 0:079fc298efbb 73
edospinas 0:079fc298efbb 74 int V[24]; //Se nombra un vector que tendrá los datos que se sumarán para calcular el valor del checksum
edospinas 0:079fc298efbb 75 /*El tamaño de este depende del número de datos útiles, para:
edospinas 0:079fc298efbb 76 1 dato 17
edospinas 0:079fc298efbb 77 2 datos 18
edospinas 0:079fc298efbb 78 3 datos 19
edospinas 0:079fc298efbb 79 4 datos 20
edospinas 0:079fc298efbb 80 5 datos 21
edospinas 0:079fc298efbb 81 .
edospinas 0:079fc298efbb 82 .
edospinas 0:079fc298efbb 83 .
edospinas 0:079fc298efbb 84
edospinas 0:079fc298efbb 85 */
edospinas 0:079fc298efbb 86
edospinas 0:079fc298efbb 87
edospinas 0:079fc298efbb 88 int baudrate=19200; //Se denomina un baudrate (bits/segundo) de 57600
edospinas 0:079fc298efbb 89
edospinas 0:079fc298efbb 90 DigitalOut myled(LED1);
edospinas 0:079fc298efbb 91
edospinas 0:079fc298efbb 92
edospinas 0:079fc298efbb 93
edospinas 0:079fc298efbb 94
edospinas 0:079fc298efbb 95 /*************************************************************************
edospinas 0:079fc298efbb 96 FUNCIÓN: main()
edospinas 0:079fc298efbb 97 PROPÓSITO: ESTA ES LA FUNCIÓN PRINCIPAL DEL PROGRAMA
edospinas 0:079fc298efbb 98 **************************************************************************/
edospinas 0:079fc298efbb 99
edospinas 0:079fc298efbb 100 int main() {
edospinas 0:079fc298efbb 101
edospinas 0:079fc298efbb 102 datoNuevo = 0;
edospinas 0:079fc298efbb 103 pwmNuevo=0;
edospinas 0:079fc298efbb 104 p1 = 0.0;
edospinas 0:079fc298efbb 105 p2 = 0.0;
edospinas 0:079fc298efbb 106
edospinas 0:079fc298efbb 107 xbee.baud(baudrate);
edospinas 0:079fc298efbb 108 Arduino.baud(baudrate);
edospinas 0:079fc298efbb 109 //Setup serial connection
edospinas 0:079fc298efbb 110 pc.baud(Baudrate);
edospinas 0:079fc298efbb 111
edospinas 0:079fc298efbb 112 initPWM();
edospinas 0:079fc298efbb 113
edospinas 0:079fc298efbb 114
edospinas 0:079fc298efbb 115 trama[2]=0x16; // Este valor es del número de datos desde luego de la longitud hasta antes del checksum,
edospinas 0:079fc298efbb 116 // en este caso son 3 datos y se tiene una longitud de 17 datos= 0x11
edospinas 0:079fc298efbb 117 /*
edospinas 0:079fc298efbb 118 La longitud varía según el número de datos útiles a enviar, para:
edospinas 0:079fc298efbb 119 1 dato 0F
edospinas 0:079fc298efbb 120 2 datos 10
edospinas 0:079fc298efbb 121 3 datos 11
edospinas 0:079fc298efbb 122 4 datos 12
edospinas 0:079fc298efbb 123 5 datos 13
edospinas 0:079fc298efbb 124 .
edospinas 0:079fc298efbb 125 .
edospinas 0:079fc298efbb 126 .
edospinas 0:079fc298efbb 127 */
edospinas 0:079fc298efbb 128
edospinas 0:079fc298efbb 129 envioTask.attach_us(&TxSend, 250000);
edospinas 0:079fc298efbb 130 pwmTask.attach_us(&WaitPWM, 100000);
edospinas 0:079fc298efbb 131
edospinas 0:079fc298efbb 132
edospinas 0:079fc298efbb 133 while(1) {
edospinas 0:079fc298efbb 134
edospinas 0:079fc298efbb 135 wait(0.025);
edospinas 0:079fc298efbb 136 xbeeRead(); //Leer datos que llegan del control
edospinas 0:079fc298efbb 137
edospinas 0:079fc298efbb 138 //Print
edospinas 0:079fc298efbb 139 //pc.printf("%c", 0x7E);
edospinas 0:079fc298efbb 140 //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]);
edospinas 0:079fc298efbb 141
edospinas 0:079fc298efbb 142
edospinas 0:079fc298efbb 143
edospinas 0:079fc298efbb 144 Arduino.printf("%c", 0x7E);
edospinas 0:079fc298efbb 145 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]);
edospinas 0:079fc298efbb 146
edospinas 0:079fc298efbb 147 Roll = ((int)d1[1] << 8) | (int) d1[0];
edospinas 0:079fc298efbb 148
edospinas 0:079fc298efbb 149 Pitch = ((int)d2[1] << 8) | (int) d2[0];
edospinas 0:079fc298efbb 150
edospinas 0:079fc298efbb 151 Throttle = ((int)d3[1] << 8) | (int) d3[0];
edospinas 0:079fc298efbb 152
edospinas 0:079fc298efbb 153 if (Throttle > 32768){Throttle = (Throttle - 65536);} //p1 se escala desde 1 a 1,6 para el freno
edospinas 0:079fc298efbb 154
edospinas 0:079fc298efbb 155
edospinas 0:079fc298efbb 156 Yaw = ((int)d4[1] << 8) | (int) d4[0];
edospinas 0:079fc298efbb 157
edospinas 0:079fc298efbb 158 if (Yaw > 32768){Yaw = (Yaw - 65536);}
edospinas 0:079fc298efbb 159
edospinas 0:079fc298efbb 160 Aux_A = ((int)d5[1] << 8) | (int) d5[0];
edospinas 0:079fc298efbb 161
edospinas 0:079fc298efbb 162 Aux_B = ((int)d6[1] << 8) | (int) d6[0];
edospinas 0:079fc298efbb 163
edospinas 0:079fc298efbb 164 /////////////Tareas lentas///////////////////////
edospinas 0:079fc298efbb 165
edospinas 0:079fc298efbb 166
edospinas 0:079fc298efbb 167 if(datoNuevo){
edospinas 0:079fc298efbb 168 //Envío de datos
edospinas 0:079fc298efbb 169 xbeeSend();
edospinas 0:079fc298efbb 170 datoNuevo = 0;
edospinas 0:079fc298efbb 171
edospinas 0:079fc298efbb 172 }
edospinas 0:079fc298efbb 173
edospinas 0:079fc298efbb 174 //Valor promedio
edospinas 0:079fc298efbb 175
edospinas 0:079fc298efbb 176 p1 = map(Throttle, -32768, 32767, 1, 2)/P_M;
edospinas 0:079fc298efbb 177 p2 = map(Yaw, -32768, 32767, 1.1, 1.9)/P_S1;
edospinas 0:079fc298efbb 178 //count = count + 1;
edospinas 0:079fc298efbb 179 //Actualización del valor de PWM
edospinas 0:079fc298efbb 180 if (pwmNuevo){
edospinas 0:079fc298efbb 181
edospinas 0:079fc298efbb 182 //p1 = p1/count;
edospinas 0:079fc298efbb 183
edospinas 0:079fc298efbb 184 pc.printf("%f \n", p2);
edospinas 0:079fc298efbb 185 Motor.write(p1);
edospinas 0:079fc298efbb 186 Direc.write(p2);
edospinas 0:079fc298efbb 187
edospinas 0:079fc298efbb 188 datosX = acc.getAccX(); // datos a almacenar
edospinas 0:079fc298efbb 189 datosY = acc.getAccY(); // datos a almacenar
edospinas 0:079fc298efbb 190 datosZ = acc.getAccZ(); // datos a almacenar
edospinas 0:079fc298efbb 191
edospinas 0:079fc298efbb 192 pwmNuevo = 0;
edospinas 0:079fc298efbb 193 //p1 = 0;
edospinas 0:079fc298efbb 194 count = 0;
edospinas 0:079fc298efbb 195 }
edospinas 0:079fc298efbb 196
edospinas 0:079fc298efbb 197
edospinas 0:079fc298efbb 198
edospinas 0:079fc298efbb 199 }
edospinas 0:079fc298efbb 200 }
edospinas 0:079fc298efbb 201
edospinas 0:079fc298efbb 202
edospinas 0:079fc298efbb 203 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
edospinas 0:079fc298efbb 204 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
edospinas 0:079fc298efbb 205
edospinas 0:079fc298efbb 206
edospinas 0:079fc298efbb 207 /*************************************************************************
edospinas 0:079fc298efbb 208 FUNCIÓN: xbeeRead()
edospinas 0:079fc298efbb 209 PROPÓSITO: ESCRIBIR UN PAQUETE DE DATOS API EN EL XBEE CON LOS DATOS ÚTILES
edospinas 0:079fc298efbb 210 **************************************************************************/
edospinas 0:079fc298efbb 211
edospinas 0:079fc298efbb 212
edospinas 0:079fc298efbb 213 void xbeeSend(){
edospinas 0:079fc298efbb 214
edospinas 0:079fc298efbb 215
edospinas 0:079fc298efbb 216 int checksum=0x00; //Se inicializa el cálculo del checksum por cada iteración
edospinas 0:079fc298efbb 217
edospinas 0:079fc298efbb 218 for (i=3;i<=24;i++){
edospinas 0:079fc298efbb 219
edospinas 0:079fc298efbb 220 //Se nombra un nuevo vector con los datos que va desde luego de la longitud (3) hasta antes del checksum (19)
edospinas 0:079fc298efbb 221 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:079fc298efbb 222 //por ende el condicional tomaría desde 3-16
edospinas 0:079fc298efbb 223 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:079fc298efbb 224 // Si se envían 4 datos útiles la condición sería hasta 20, 5 datos útil iría hasta 21...
edospinas 0:079fc298efbb 225
edospinas 0:079fc298efbb 226 checksum=checksum+V[i]; //Se suma desde el dato siguiente a la longitud de
edospinas 0:079fc298efbb 227 //la trama hasta el último dato útil a enviar
edospinas 0:079fc298efbb 228
edospinas 0:079fc298efbb 229 }
edospinas 0:079fc298efbb 230
edospinas 0:079fc298efbb 231
edospinas 0:079fc298efbb 232 // FORMULA CHECKSUM
edospinas 0:079fc298efbb 233
edospinas 0:079fc298efbb 234 checksum=0xFF-checksum; //Se realiza la operación para la fórmula del ckecksum
edospinas 0:079fc298efbb 235
edospinas 0:079fc298efbb 236
edospinas 0:079fc298efbb 237 // Primero se comprueba que el puerto esté libre para escribir
edospinas 0:079fc298efbb 238 if ((xbee.writeable())){
edospinas 0:079fc298efbb 239 for (i=0;i<=16;i++){xbee.putc(trama[i]);}//Envía todos los datos fijos de la trama de envío
edospinas 0:079fc298efbb 240 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:079fc298efbb 241 }
edospinas 0:079fc298efbb 242
edospinas 0:079fc298efbb 243 }
edospinas 0:079fc298efbb 244
edospinas 0:079fc298efbb 245
edospinas 0:079fc298efbb 246 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
edospinas 0:079fc298efbb 247 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
edospinas 0:079fc298efbb 248
edospinas 0:079fc298efbb 249
edospinas 0:079fc298efbb 250
edospinas 0:079fc298efbb 251 /*************************************************************************
edospinas 0:079fc298efbb 252 FUNCIÓN: xbeeRead()
edospinas 0:079fc298efbb 253 PROPÓSITO: LEER UN PAQUETE DE DATOS DEL XBee Y SACAR DATOS ÚTILES
edospinas 0:079fc298efbb 254 **************************************************************************/
edospinas 0:079fc298efbb 255
edospinas 0:079fc298efbb 256 void xbeeRead(){
edospinas 0:079fc298efbb 257
edospinas 0:079fc298efbb 258 char packetFound = 0; //Esta variable, indicará si se ha encontrado un paquete o no.
edospinas 0:079fc298efbb 259 char thisByte; //Esta variable tomará valores de la trama de datos recibida
edospinas 0:079fc298efbb 260
edospinas 0:079fc298efbb 261 if(xbee.rxBufferGetCount() == 0) //Si en el Buffer no ha llegado ningún dato, el programa volverá a leerlo
edospinas 0:079fc298efbb 262 {
edospinas 0:079fc298efbb 263 return;
edospinas 0:079fc298efbb 264 }
edospinas 0:079fc298efbb 265 while(xbee.rxBufferGetCount() > 14){ //Si llegaron más de 14 palabras al buffer, compruebe que sea la trama correcta
edospinas 0:079fc298efbb 266
edospinas 0:079fc298efbb 267 thisByte = xbee.getc(); //Toma el primer valor de la trama recibida.
edospinas 0:079fc298efbb 268 if(thisByte == 0x7E){ //Si el primer dato de la trama es 0x7E, continúe...
edospinas 0:079fc298efbb 269 thisByte = xbee.getc(); //Toma el MSB de la longitud
edospinas 0:079fc298efbb 270 thisByte = xbee.getc(); //Toma el LSB de la longitud
edospinas 0:079fc298efbb 271 thisByte = xbee.getc(); //Toma el tipo de paquete
edospinas 0:079fc298efbb 272 if(thisByte == 0x90){ //Si recibió que el paquete es de tipo: Rx (0x90), continúe...
edospinas 0:079fc298efbb 273 // packet is rx type
edospinas 0:079fc298efbb 274 packetFound = 1; //Se indica que se encontró la trama de recepción
edospinas 0:079fc298efbb 275 break; //Si se ubica la trama, se sale del ciclo while
edospinas 0:079fc298efbb 276 }
edospinas 0:079fc298efbb 277 }
edospinas 0:079fc298efbb 278 }
edospinas 0:079fc298efbb 279
edospinas 0:079fc298efbb 280 if(packetFound == 1){ // Se encontró un paquete, entonces se toman los siguientes 11 datos.
edospinas 0:079fc298efbb 281 // Estos 11 datos van hasta un dato antes del primer dato útil
edospinas 0:079fc298efbb 282
edospinas 0:079fc298efbb 283 thisByte = xbee.getc();
edospinas 0:079fc298efbb 284 thisByte = xbee.getc();
edospinas 0:079fc298efbb 285 thisByte = xbee.getc();
edospinas 0:079fc298efbb 286 thisByte = xbee.getc();
edospinas 0:079fc298efbb 287 thisByte = xbee.getc();
edospinas 0:079fc298efbb 288 thisByte = xbee.getc();
edospinas 0:079fc298efbb 289 thisByte = xbee.getc();
edospinas 0:079fc298efbb 290 thisByte = xbee.getc();
edospinas 0:079fc298efbb 291 thisByte = xbee.getc();
edospinas 0:079fc298efbb 292 thisByte = xbee.getc();
edospinas 0:079fc298efbb 293 thisByte = xbee.getc();
edospinas 0:079fc298efbb 294
edospinas 0:079fc298efbb 295 // Luego de contar los 11 datos, se comienzan a tomar los datos útiles. En este caso son 3
edospinas 0:079fc298efbb 296
edospinas 0:079fc298efbb 297 thisByte = xbee.getc();
edospinas 0:079fc298efbb 298 d1[1] = thisByte;
edospinas 0:079fc298efbb 299 thisByte = xbee.getc();
edospinas 0:079fc298efbb 300 d1[0] = thisByte;
edospinas 0:079fc298efbb 301 thisByte = xbee.getc();
edospinas 0:079fc298efbb 302 d2[1] = thisByte;
edospinas 0:079fc298efbb 303 thisByte = xbee.getc();
edospinas 0:079fc298efbb 304 d2[0] = thisByte;
edospinas 0:079fc298efbb 305 thisByte = xbee.getc();
edospinas 0:079fc298efbb 306 d3[1] = thisByte;
edospinas 0:079fc298efbb 307 thisByte = xbee.getc();
edospinas 0:079fc298efbb 308 d3[0] = thisByte;
edospinas 0:079fc298efbb 309 thisByte = xbee.getc();
edospinas 0:079fc298efbb 310 d4[1] = thisByte;
edospinas 0:079fc298efbb 311 thisByte = xbee.getc();
edospinas 0:079fc298efbb 312 d4[0] = thisByte;
edospinas 0:079fc298efbb 313 thisByte = xbee.getc();
edospinas 0:079fc298efbb 314 d5[1] = thisByte;
edospinas 0:079fc298efbb 315 thisByte = xbee.getc();
edospinas 0:079fc298efbb 316 d5[0] = thisByte;
edospinas 0:079fc298efbb 317 thisByte = xbee.getc();
edospinas 0:079fc298efbb 318 d6[1] = thisByte;
edospinas 0:079fc298efbb 319 thisByte = xbee.getc();
edospinas 0:079fc298efbb 320 d6[0] = thisByte;
edospinas 0:079fc298efbb 321
edospinas 0:079fc298efbb 322 }
edospinas 0:079fc298efbb 323
edospinas 0:079fc298efbb 324
edospinas 0:079fc298efbb 325 }
edospinas 0:079fc298efbb 326
edospinas 0:079fc298efbb 327
edospinas 0:079fc298efbb 328 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
edospinas 0:079fc298efbb 329 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
edospinas 0:079fc298efbb 330
edospinas 0:079fc298efbb 331
edospinas 0:079fc298efbb 332 // ISR -> Interrupt service ruting - para tomar dato y hacer parpadeo (DEBE SER RAPIDA)
edospinas 0:079fc298efbb 333 void TxSend(void){
edospinas 0:079fc298efbb 334
edospinas 0:079fc298efbb 335 angleZX = atan((datosZ)/(datosX));
edospinas 0:079fc298efbb 336 angleZX = angleZX*(57.2958);
edospinas 0:079fc298efbb 337
edospinas 0:079fc298efbb 338 angleYX = atan((datosY)/(datosX));
edospinas 0:079fc298efbb 339 angleYX = angleYX*(57.2958);
edospinas 0:079fc298efbb 340
edospinas 0:079fc298efbb 341 int d_1 = map(angleYX,-90,90,-32768,32767);
edospinas 0:079fc298efbb 342 int d_2 = map(angleZX,-90,90,-32768,32767);;
edospinas 0:079fc298efbb 343 datosY = 100;
edospinas 0:079fc298efbb 344 datosZ = 1.5;
edospinas 0:079fc298efbb 345
edospinas 0:079fc298efbb 346
edospinas 0:079fc298efbb 347
edospinas 0:079fc298efbb 348 char * b0 = (char *) &d_1;
edospinas 0:079fc298efbb 349 char * b1 = (char *) &d_2;
edospinas 0:079fc298efbb 350 char * b2 = (char *) &datosZ;
edospinas 0:079fc298efbb 351 char * b3 = (char *) &datosY;
edospinas 0:079fc298efbb 352
edospinas 0:079fc298efbb 353 entrada[0]= b0[1];
edospinas 0:079fc298efbb 354 entrada[1]= b0[0];
edospinas 0:079fc298efbb 355 entrada[2]= b1[1];
edospinas 0:079fc298efbb 356 entrada[3]= b1[0];
edospinas 0:079fc298efbb 357 entrada[4]= b2[1];
edospinas 0:079fc298efbb 358 entrada[5]= b2[0];
edospinas 0:079fc298efbb 359 entrada[6]= b3[1];
edospinas 0:079fc298efbb 360 entrada[7]= b3[0];
edospinas 0:079fc298efbb 361
edospinas 0:079fc298efbb 362 datoNuevo = 1; // Indicamos que tenemos un dato nuevo
edospinas 0:079fc298efbb 363 }
edospinas 0:079fc298efbb 364
edospinas 0:079fc298efbb 365 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
edospinas 0:079fc298efbb 366 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
edospinas 0:079fc298efbb 367
edospinas 0:079fc298efbb 368
edospinas 0:079fc298efbb 369 // ISR -> Interrupción para la actualización del PWM
edospinas 0:079fc298efbb 370 void WaitPWM(void){
edospinas 0:079fc298efbb 371 pwmNuevo = 1; // Indicamos que tenemos un dato nuevo
edospinas 0:079fc298efbb 372 }
edospinas 0:079fc298efbb 373
edospinas 0:079fc298efbb 374
edospinas 0:079fc298efbb 375 /*************************************************************************
edospinas 0:079fc298efbb 376 FUNCIÓN: initPWM()
edospinas 0:079fc298efbb 377 PROPÓSITO: Inicializar los PWM
edospinas 0:079fc298efbb 378 **************************************************************************/
edospinas 0:079fc298efbb 379
edospinas 0:079fc298efbb 380 void initPWM(void){
edospinas 0:079fc298efbb 381 Motor.period_ms(P_M);
edospinas 0:079fc298efbb 382 Motor.write(1.0/P_M);
edospinas 0:079fc298efbb 383 Direc.period_ms(P_S1);
edospinas 0:079fc298efbb 384 Direc.write(1.5/P_S1);
edospinas 0:079fc298efbb 385 wait(0.2);
edospinas 0:079fc298efbb 386 }