Andres Castañeda / Mbed 2 deprecated Entrega

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@1:9430a3a15f7a, 2018-11-14 (annotated)

Committer:
andrescas
Date:
Wed Nov 14 21:13:59 2018 +0000
Revision:
1:9430a3a15f7a
Parent:
0:4b1e8862ad53
Child:
2:e5c7ae640dfd
Entrega

Who changed what in which revision?

UserRevisionLine numberNew contents of line
andrescas 0:4b1e8862ad53 1 #include "mbed.h"
andrescas 0:4b1e8862ad53 2 Serial pc(USBTX, USBRX); // leer comnunicacion serial
andrescas 0:4b1e8862ad53 3 Serial pcBT(D8, D2); // tx, rx
andrescas 0:4b1e8862ad53 4 AnalogIn analog_value(A0); // definimos puerto para leer analogamente el joystick
andrescas 0:4b1e8862ad53 5 PwmOut myServo1(PC_7); // definir puertos de los servos
andrescas 0:4b1e8862ad53 6 PwmOut myServo2(PB_4);
andrescas 0:4b1e8862ad53 7 PwmOut myServo3(PB_5);
andrescas 0:4b1e8862ad53 8 PwmOut myServo4(PB_8);
andrescas 0:4b1e8862ad53 9 PwmOut myServo5(PA_6);
andrescas 0:4b1e8862ad53 10 PwmOut myServo6(PB_9);
andrescas 0:4b1e8862ad53 11 PwmOut myServo7(PA_8);
andrescas 0:4b1e8862ad53 12 PwmOut myServo8(PA_7);
andrescas 0:4b1e8862ad53 13 DigitalOut led(LED1,0); //led indicador de interrupcion
andrescas 0:4b1e8862ad53 14 DigitalOut S0(PA_13); //definimos los puertos del sensor
andrescas 0:4b1e8862ad53 15 DigitalOut S1(PA_14);
andrescas 0:4b1e8862ad53 16 DigitalOut S2(PA_15);
andrescas 0:4b1e8862ad53 17 DigitalOut S3(PC_8);
andrescas 0:4b1e8862ad53 18 DigitalIn leer(PC_9); // puerto donde leemos la frecuencia que envia el sensor
andrescas 0:4b1e8862ad53 19 Timer tiempo; // timer para uso del sensor
andrescas 0:4b1e8862ad53 20 Timeout timeout; //creamos el objeto timeout
andrescas 0:4b1e8862ad53 21 unsigned int lectura(); //respuesta de lectura de sensor
andrescas 0:4b1e8862ad53 22 unsigned int inicio=0, final=0, resultado=0, rojo=0,verde=0,azul=0;// variables que se usan en los procesos
andrescas 0:4b1e8862ad53 23 float meas_r; //variables del joystick
andrescas 0:4b1e8862ad53 24 float meas_v;
andrescas 0:4b1e8862ad53 25 void moving();
andrescas 0:4b1e8862ad53 26 void grupomover();
andrescas 0:4b1e8862ad53 27 void mover_ser(uint8_t nmotor, uint8_t pos);
andrescas 0:4b1e8862ad53 28 void timer_interrupt();
andrescas 0:4b1e8862ad53 29 void read_command(); // definimos los 4 numeros hexadecimales enviados
andrescas 0:4b1e8862ad53 30 void read_commandbt(); // definimos los 4 numeros hexadecimales enviados
andrescas 0:4b1e8862ad53 31 void adelante();
andrescas 0:4b1e8862ad53 32 void atras();
andrescas 0:4b1e8862ad53 33 void init_servo();
andrescas 0:4b1e8862ad53 34 char init_c;
andrescas 0:4b1e8862ad53 35 int read_cc;
andrescas 0:4b1e8862ad53 36 uint32_t grados;
andrescas 0:4b1e8862ad53 37 uint32_t nmotor;
andrescas 0:4b1e8862ad53 38 int endc;
andrescas 0:4b1e8862ad53 39 int main()
andrescas 0:4b1e8862ad53 40 {
andrescas 0:4b1e8862ad53 41 pc.baud(9600); // leer en el monitor serial
andrescas 0:4b1e8862ad53 42 pc.printf("inicio\n");
andrescas 0:4b1e8862ad53 43 timeout.attach(&timer_interrupt,0.5); // llamamos la interrupcion del timeout cada 0.5 segundos
andrescas 0:4b1e8862ad53 44 pc.attach(&read_command, Serial::RxIrq); //llamamos la interrupcion serialmente
andrescas 0:4b1e8862ad53 45 pcBT.attach(&read_commandbt, Serial::RxIrq);//llamamos la interrupcion serialmente
andrescas 0:4b1e8862ad53 46 S0=1,S1=0; //calibracion de lectura del sensor
andrescas 0:4b1e8862ad53 47 S2=0,S3=0;
andrescas 0:4b1e8862ad53 48 init_servo();
andrescas 0:4b1e8862ad53 49 while(1) {
andrescas 0:4b1e8862ad53 50 wait(1);
andrescas 0:4b1e8862ad53 51 pc.printf("modo joystick\n");
andrescas 0:4b1e8862ad53 52 meas_r = analog_value.read(); // se lee la entrada analoga del joystick
andrescas 0:4b1e8862ad53 53 meas_v = meas_r * 3300; // se convierte la variable a 3.3v
andrescas 0:4b1e8862ad53 54 pc.printf("medida = %f = %.0f mV\n", meas_r, meas_v); //muestra en el monitor lectura del joystick
andrescas 0:4b1e8862ad53 55 if (meas_v < 1000) {
andrescas 0:4b1e8862ad53 56 pc.printf("adelante\n");
andrescas 0:4b1e8862ad53 57 adelante();
andrescas 0:4b1e8862ad53 58
andrescas 0:4b1e8862ad53 59 }
andrescas 0:4b1e8862ad53 60 if (meas_v > 3000) {
andrescas 0:4b1e8862ad53 61 pc.printf("atras\n");
andrescas 0:4b1e8862ad53 62 atras();
andrescas 0:4b1e8862ad53 63 }
andrescas 0:4b1e8862ad53 64 }
andrescas 0:4b1e8862ad53 65 }
andrescas 0:4b1e8862ad53 66 unsigned int lectura() // lectura del sensor
andrescas 0:4b1e8862ad53 67 {
andrescas 0:4b1e8862ad53 68 tiempo.start();
andrescas 0:4b1e8862ad53 69 while(leer) {}
andrescas 0:4b1e8862ad53 70 while(!leer) {} // inicia desde el inicio de cambio de flanco
andrescas 0:4b1e8862ad53 71 //pc.printf("sale");
andrescas 0:4b1e8862ad53 72 while(leer) {}
andrescas 0:4b1e8862ad53 73 tiempo.reset();
andrescas 0:4b1e8862ad53 74 inicio=tiempo.read_us();
andrescas 0:4b1e8862ad53 75 while(!leer) {}
andrescas 0:4b1e8862ad53 76 final=tiempo.read_us();
andrescas 0:4b1e8862ad53 77 resultado=(final-inicio);
andrescas 0:4b1e8862ad53 78 return resultado;
andrescas 0:4b1e8862ad53 79 }
andrescas 0:4b1e8862ad53 80 void init_servo() //inicializamos servomotores
andrescas 0:4b1e8862ad53 81 {
andrescas 0:4b1e8862ad53 82 myServo1.period_ms(20); // periodo de cada servo
andrescas 0:4b1e8862ad53 83 myServo2.period_ms(20);
andrescas 0:4b1e8862ad53 84 myServo3.period_ms(20);
andrescas 0:4b1e8862ad53 85 myServo4.period_ms(20);
andrescas 0:4b1e8862ad53 86 myServo5.period_ms(20);
andrescas 0:4b1e8862ad53 87 myServo6.period_ms(20);
andrescas 0:4b1e8862ad53 88 myServo7.period_ms(20);
andrescas 0:4b1e8862ad53 89 myServo8.period_ms(20);
andrescas 0:4b1e8862ad53 90
andrescas 0:4b1e8862ad53 91 }
andrescas 0:4b1e8862ad53 92 int coord2us(float coord)
andrescas 0:4b1e8862ad53 93 {
andrescas 0:4b1e8862ad53 94 if(0 <= coord <= 180)
andrescas 0:4b1e8862ad53 95 return int(750+coord*1750/180);// cambiar de numero de entrada serial a grados
andrescas 0:4b1e8862ad53 96 return 750;
andrescas 0:4b1e8862ad53 97 }
andrescas 0:4b1e8862ad53 98 void read_command()
andrescas 0:4b1e8862ad53 99 {
andrescas 0:4b1e8862ad53 100 init_c=pc.getc();
andrescas 0:4b1e8862ad53 101 if( init_c==0xFF) {
andrescas 0:4b1e8862ad53 102 read_cc=pc.getc();
andrescas 0:4b1e8862ad53 103 nmotor=pc.getc();
andrescas 0:4b1e8862ad53 104 grados=pc.getc();
andrescas 0:4b1e8862ad53 105 endc=pc.getc();
andrescas 0:4b1e8862ad53 106
andrescas 0:4b1e8862ad53 107 if(endc==0xFD) {
andrescas 0:4b1e8862ad53 108 switch (read_cc) {
andrescas 0:4b1e8862ad53 109 case 0x01:
andrescas 0:4b1e8862ad53 110 moving();
andrescas 0:4b1e8862ad53 111 break;
andrescas 0:4b1e8862ad53 112 case 0x02:
andrescas 0:4b1e8862ad53 113 grupomover();
andrescas 0:4b1e8862ad53 114 break;
andrescas 0:4b1e8862ad53 115 default:
andrescas 0:4b1e8862ad53 116 pc.printf("error \nSe espera 0xFEF0 o 0xFFF0 \n");
andrescas 0:4b1e8862ad53 117 break ;
andrescas 0:4b1e8862ad53 118 }
andrescas 0:4b1e8862ad53 119 } else
andrescas 0:4b1e8862ad53 120 pc.printf("Error de codigo de cierre \n");
andrescas 0:4b1e8862ad53 121
andrescas 0:4b1e8862ad53 122 } else
andrescas 0:4b1e8862ad53 123 pc.printf("Error de codigo de inicio. \n");
andrescas 0:4b1e8862ad53 124 }
andrescas 0:4b1e8862ad53 125 void read_commandbt()
andrescas 0:4b1e8862ad53 126 {
andrescas 0:4b1e8862ad53 127 init_c=pc.getc();
andrescas 0:4b1e8862ad53 128 if( init_c==0xFF) {
andrescas 0:4b1e8862ad53 129 read_cc=pc.getc();
andrescas 0:4b1e8862ad53 130 nmotor=pc.getc();
andrescas 0:4b1e8862ad53 131 grados=pc.getc();
andrescas 0:4b1e8862ad53 132 endc=pc.getc();
andrescas 0:4b1e8862ad53 133
andrescas 0:4b1e8862ad53 134 if(endc==0xFD) {
andrescas 0:4b1e8862ad53 135 switch (read_cc) {
andrescas 0:4b1e8862ad53 136 case 0x01:
andrescas 0:4b1e8862ad53 137 moving();
andrescas 0:4b1e8862ad53 138 break;
andrescas 0:4b1e8862ad53 139 case 0x02:
andrescas 0:4b1e8862ad53 140 grupomover();
andrescas 0:4b1e8862ad53 141 break;
andrescas 0:4b1e8862ad53 142 default:
andrescas 0:4b1e8862ad53 143 pc.printf("error \nSe espera 0xFEF0 o 0xFFF0 \n");
andrescas 0:4b1e8862ad53 144 break ;
andrescas 0:4b1e8862ad53 145 }
andrescas 0:4b1e8862ad53 146 } else
andrescas 0:4b1e8862ad53 147 pc.printf("Error de codigo de cierre \n");
andrescas 0:4b1e8862ad53 148
andrescas 0:4b1e8862ad53 149 } else
andrescas 0:4b1e8862ad53 150 pc.printf("Error de codigo de inicio. \n");
andrescas 0:4b1e8862ad53 151
andrescas 0:4b1e8862ad53 152 }
andrescas 0:4b1e8862ad53 153 void moving()
andrescas 0:4b1e8862ad53 154 {
andrescas 0:4b1e8862ad53 155 pc.printf("mueve\n");
andrescas 0:4b1e8862ad53 156 mover_ser(nmotor,grados); // llama el void para mover servo con grados
andrescas 0:4b1e8862ad53 157 pc.printf("fin mueve\n");
andrescas 0:4b1e8862ad53 158
andrescas 0:4b1e8862ad53 159 }
andrescas 0:4b1e8862ad53 160 void mover_ser(uint8_t nmotor, uint8_t grados) // definimos que leen los 8 servos y le asiganamos a cada variable un servo
andrescas 0:4b1e8862ad53 161 {
andrescas 0:4b1e8862ad53 162 int pulsea = coord2us(grados);
andrescas 0:4b1e8862ad53 163 switch (nmotor) {
andrescas 0:4b1e8862ad53 164 case 0x01:
andrescas 0:4b1e8862ad53 165 myServo1.pulsewidth_us(pulsea);
andrescas 0:4b1e8862ad53 166 break; // caso para cada servo
andrescas 0:4b1e8862ad53 167 case 0x02:
andrescas 0:4b1e8862ad53 168 myServo2.pulsewidth_us(pulsea);
andrescas 0:4b1e8862ad53 169 break;
andrescas 0:4b1e8862ad53 170 case 0x03:
andrescas 0:4b1e8862ad53 171 myServo3.pulsewidth_us(pulsea);
andrescas 0:4b1e8862ad53 172 break;
andrescas 0:4b1e8862ad53 173 case 0x04:
andrescas 0:4b1e8862ad53 174 myServo4.pulsewidth_us(pulsea);
andrescas 0:4b1e8862ad53 175 break;
andrescas 0:4b1e8862ad53 176 case 0x05:
andrescas 0:4b1e8862ad53 177 myServo5.pulsewidth_us(pulsea);
andrescas 0:4b1e8862ad53 178 break;
andrescas 0:4b1e8862ad53 179 case 0x06:
andrescas 0:4b1e8862ad53 180 myServo6.pulsewidth_us(pulsea);
andrescas 0:4b1e8862ad53 181 break;
andrescas 0:4b1e8862ad53 182 case 0x07:
andrescas 0:4b1e8862ad53 183 myServo7.pulsewidth_us(pulsea);
andrescas 0:4b1e8862ad53 184 break;
andrescas 0:4b1e8862ad53 185 case 0x08:
andrescas 0:4b1e8862ad53 186 myServo8.pulsewidth_us(pulsea);
andrescas 0:4b1e8862ad53 187 break;
andrescas 0:4b1e8862ad53 188 default:
andrescas 0:4b1e8862ad53 189 pc.printf("no hay mas motores error de comando\n");
andrescas 0:4b1e8862ad53 190 break ;
andrescas 0:4b1e8862ad53 191 }
andrescas 0:4b1e8862ad53 192 }
andrescas 0:4b1e8862ad53 193
andrescas 0:4b1e8862ad53 194 void timer_interrupt() //la respuesta de cada interrupcion externamente por timer
andrescas 0:4b1e8862ad53 195 {
andrescas 0:4b1e8862ad53 196 led = !led;
andrescas 0:4b1e8862ad53 197 S2=0,S3=0;
andrescas 0:4b1e8862ad53 198 rojo=lectura(); //pc.printf("rojo --%d\n",rojo);
andrescas 0:4b1e8862ad53 199 S2=1,S3=1;
andrescas 0:4b1e8862ad53 200 verde=lectura(); //pc.printf("verde --%d\n",verde);
andrescas 0:4b1e8862ad53 201 S2=0,S3=1;
andrescas 0:4b1e8862ad53 202 azul=lectura(); //pc.printf("azul --%d\n",azul);
andrescas 0:4b1e8862ad53 203
andrescas 0:4b1e8862ad53 204 if (rojo>=100 and rojo <=400) { // lee el rojo------------
andrescas 0:4b1e8862ad53 205 if (verde>=450 and verde <=700) {
andrescas 0:4b1e8862ad53 206 if (azul>=300 and azul <=600) {
andrescas 0:4b1e8862ad53 207
andrescas 0:4b1e8862ad53 208
andrescas 0:4b1e8862ad53 209 //POSICION ORIGINAL
andrescas 0:4b1e8862ad53 210 pc.printf("POSICION ORIGINAL --%d\n",rojo);
andrescas 1:9430a3a15f7a 211
andrescas 1:9430a3a15f7a 212 myServo1.pulsewidth_us(1025);
andrescas 1:9430a3a15f7a 213 wait (0.5);
andrescas 1:9430a3a15f7a 214 myServo2.pulsewidth_us(1711);
andrescas 1:9430a3a15f7a 215 wait (0.5);
andrescas 1:9430a3a15f7a 216 myServo1.pulsewidth_us(1412);
andrescas 1:9430a3a15f7a 217 wait (0.5);
andrescas 1:9430a3a15f7a 218 myServo7.pulsewidth_us(1025);
andrescas 1:9430a3a15f7a 219 wait (0.5);
andrescas 1:9430a3a15f7a 220 myServo8.pulsewidth_us(1319);
andrescas 1:9430a3a15f7a 221 wait (0.5);
andrescas 1:9430a3a15f7a 222 myServo7.pulsewidth_us(1417);
andrescas 1:9430a3a15f7a 223 wait (0.5);
andrescas 1:9430a3a15f7a 224 myServo2.pulsewidth_us(1515);
andrescas 1:9430a3a15f7a 225 myServo8.pulsewidth_us(1515);
andrescas 1:9430a3a15f7a 226 wait (0.5);
andrescas 1:9430a3a15f7a 227 myServo3.pulsewidth_us(1221);
andrescas 1:9430a3a15f7a 228 wait (0.5);
andrescas 1:9430a3a15f7a 229 /* myServo2.pulsewidth_us(1800);
andrescas 0:4b1e8862ad53 230 wait(0.5);
andrescas 0:4b1e8862ad53 231 myServo8.pulsewidth_us(1600);
andrescas 0:4b1e8862ad53 232 wait(0.5);
andrescas 0:4b1e8862ad53 233 myServo4.pulsewidth_us(1400);
andrescas 0:4b1e8862ad53 234 wait(0.5);
andrescas 0:4b1e8862ad53 235 myServo6.pulsewidth_us(1600);
andrescas 0:4b1e8862ad53 236 wait(0.5);
andrescas 0:4b1e8862ad53 237
andrescas 1:9430a3a15f7a 238 myServo1.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 239 wait(0.5);
andrescas 0:4b1e8862ad53 240 myServo3.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 241 wait(0.5);
andrescas 0:4b1e8862ad53 242 myServo5.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 243 wait(0.5);
andrescas 0:4b1e8862ad53 244 myServo7.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 245 wait(1);*/
andrescas 0:4b1e8862ad53 246
andrescas 0:4b1e8862ad53 247 pc.printf("rojo --%d\n",rojo);
andrescas 0:4b1e8862ad53 248
andrescas 0:4b1e8862ad53 249 }
andrescas 0:4b1e8862ad53 250 }
andrescas 0:4b1e8862ad53 251 }
andrescas 0:4b1e8862ad53 252
andrescas 0:4b1e8862ad53 253 if (rojo>=200 and rojo <=450) { // lee el verde --------------------------------------
andrescas 0:4b1e8862ad53 254 if (verde>=200 and verde <=450) {
andrescas 0:4b1e8862ad53 255 if (azul>=200 and azul <=450) {
andrescas 0:4b1e8862ad53 256
andrescas 0:4b1e8862ad53 257
andrescas 0:4b1e8862ad53 258 // GIRO A LA DERECHA
andrescas 0:4b1e8862ad53 259 pc.printf("GIRO A LA DERECHA --%d\n",verde);
andrescas 0:4b1e8862ad53 260 myServo1.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 261 myServo3.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 262 myServo5.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 263 myServo7.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 264 wait(0.5);
andrescas 0:4b1e8862ad53 265
andrescas 0:4b1e8862ad53 266 myServo1.pulsewidth_us(2000);
andrescas 0:4b1e8862ad53 267 wait(0.5);
andrescas 0:4b1e8862ad53 268 myServo2.pulsewidth_us(1000);
andrescas 0:4b1e8862ad53 269 wait(0.5);
andrescas 0:4b1e8862ad53 270 myServo1.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 271 wait(0.5);
andrescas 0:4b1e8862ad53 272
andrescas 0:4b1e8862ad53 273 myServo7.pulsewidth_us(2000);
andrescas 0:4b1e8862ad53 274 wait(0.5);
andrescas 0:4b1e8862ad53 275 myServo8.pulsewidth_us(1000);
andrescas 0:4b1e8862ad53 276 wait(0.5);
andrescas 0:4b1e8862ad53 277 myServo7.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 278 wait(0.5);
andrescas 0:4b1e8862ad53 279
andrescas 0:4b1e8862ad53 280 myServo5.pulsewidth_us(2000);
andrescas 0:4b1e8862ad53 281 wait(0.5);
andrescas 0:4b1e8862ad53 282 myServo6.pulsewidth_us(1000);
andrescas 0:4b1e8862ad53 283 wait(0.5);
andrescas 0:4b1e8862ad53 284 myServo5.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 285 wait(0.5);
andrescas 0:4b1e8862ad53 286
andrescas 0:4b1e8862ad53 287 myServo3.pulsewidth_us(2000);
andrescas 0:4b1e8862ad53 288 wait(0.5);
andrescas 0:4b1e8862ad53 289 myServo4.pulsewidth_us(1000);
andrescas 0:4b1e8862ad53 290 wait(0.5);
andrescas 0:4b1e8862ad53 291 myServo3.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 292 wait(0.5);
andrescas 0:4b1e8862ad53 293
andrescas 0:4b1e8862ad53 294
andrescas 0:4b1e8862ad53 295 myServo2.pulsewidth_us(2200);
andrescas 0:4b1e8862ad53 296 myServo6.pulsewidth_us(2200);
andrescas 0:4b1e8862ad53 297 wait(0.5);
andrescas 0:4b1e8862ad53 298 myServo4.pulsewidth_us(2200);
andrescas 0:4b1e8862ad53 299 myServo8.pulsewidth_us(2200);
andrescas 0:4b1e8862ad53 300 wait(1);
andrescas 0:4b1e8862ad53 301
andrescas 0:4b1e8862ad53 302 pc.printf("verde --%d\n",verde);
andrescas 0:4b1e8862ad53 303
andrescas 0:4b1e8862ad53 304 }
andrescas 0:4b1e8862ad53 305 }
andrescas 0:4b1e8862ad53 306 }
andrescas 0:4b1e8862ad53 307
andrescas 0:4b1e8862ad53 308 if (rojo>=400 and rojo <=800) { // lee el azul------------------------------------
andrescas 0:4b1e8862ad53 309 if (verde>=200 and verde <=500) {
andrescas 0:4b1e8862ad53 310 if (azul>=1 and azul <=400) {
andrescas 0:4b1e8862ad53 311
andrescas 0:4b1e8862ad53 312
andrescas 0:4b1e8862ad53 313
andrescas 0:4b1e8862ad53 314 // GIRO A LA IZQUIERDA
andrescas 0:4b1e8862ad53 315 pc.printf("GIRO A LA IZQUIERDA --%d\n",azul);
andrescas 0:4b1e8862ad53 316
andrescas 0:4b1e8862ad53 317 myServo1.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 318 myServo3.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 319 myServo5.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 320 myServo7.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 321 wait(1);
andrescas 0:4b1e8862ad53 322
andrescas 0:4b1e8862ad53 323 myServo1.pulsewidth_us(2000);
andrescas 0:4b1e8862ad53 324 wait(0.5);
andrescas 0:4b1e8862ad53 325 myServo2.pulsewidth_us(2200);
andrescas 0:4b1e8862ad53 326 wait(0.5);
andrescas 0:4b1e8862ad53 327 myServo1.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 328 wait(0.5);
andrescas 0:4b1e8862ad53 329
andrescas 0:4b1e8862ad53 330 myServo7.pulsewidth_us(2000);
andrescas 0:4b1e8862ad53 331 wait(0.5);
andrescas 0:4b1e8862ad53 332 myServo8.pulsewidth_us(2200);
andrescas 0:4b1e8862ad53 333 wait(0.5);
andrescas 0:4b1e8862ad53 334 myServo7.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 335 wait(0.5);
andrescas 0:4b1e8862ad53 336
andrescas 0:4b1e8862ad53 337 myServo5.pulsewidth_us(2000);
andrescas 0:4b1e8862ad53 338 wait(0.5);
andrescas 0:4b1e8862ad53 339 myServo6.pulsewidth_us(2200);
andrescas 0:4b1e8862ad53 340 wait(0.5);
andrescas 0:4b1e8862ad53 341 myServo5.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 342 wait(0.5);
andrescas 0:4b1e8862ad53 343
andrescas 0:4b1e8862ad53 344 myServo3.pulsewidth_us(2000);
andrescas 0:4b1e8862ad53 345 wait(0.5);
andrescas 0:4b1e8862ad53 346 myServo4.pulsewidth_us(2200);
andrescas 0:4b1e8862ad53 347 wait(0.5);
andrescas 0:4b1e8862ad53 348 myServo3.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 349 wait(0.5);
andrescas 0:4b1e8862ad53 350
andrescas 0:4b1e8862ad53 351 myServo2.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 352 myServo6.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 353 wait(0.5);
andrescas 0:4b1e8862ad53 354 myServo4.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 355 myServo8.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 356 wait(1);
andrescas 0:4b1e8862ad53 357
andrescas 0:4b1e8862ad53 358 pc.printf("azul --%d\n",azul);
andrescas 0:4b1e8862ad53 359 }
andrescas 0:4b1e8862ad53 360 }
andrescas 0:4b1e8862ad53 361 }
andrescas 0:4b1e8862ad53 362 timeout.attach(&timer_interrupt, 0.5); // reiniciamos el timeout para la siguiente lectura
andrescas 0:4b1e8862ad53 363 }
andrescas 0:4b1e8862ad53 364 void grupomover()
andrescas 0:4b1e8862ad53 365 {
andrescas 0:4b1e8862ad53 366 pc.printf("mueve grupo \n");
andrescas 0:4b1e8862ad53 367 switch (nmotor) {
andrescas 0:4b1e8862ad53 368 case 0x01: {
andrescas 0:4b1e8862ad53 369 myServo1.pulsewidth_us(550);
andrescas 0:4b1e8862ad53 370 myServo2.pulsewidth_us(1800);
andrescas 0:4b1e8862ad53 371 pc.printf("primer caso\n");
andrescas 0:4b1e8862ad53 372 break;
andrescas 0:4b1e8862ad53 373 }
andrescas 0:4b1e8862ad53 374
andrescas 0:4b1e8862ad53 375 case 0x02: {
andrescas 0:4b1e8862ad53 376 myServo3.pulsewidth_us(550);
andrescas 0:4b1e8862ad53 377 myServo4.pulsewidth_us(1800);
andrescas 0:4b1e8862ad53 378 pc.printf("segundo caso \n");
andrescas 0:4b1e8862ad53 379 break ;
andrescas 0:4b1e8862ad53 380 }
andrescas 0:4b1e8862ad53 381
andrescas 0:4b1e8862ad53 382 case 0x03: {
andrescas 0:4b1e8862ad53 383 myServo5.pulsewidth_us(550);
andrescas 0:4b1e8862ad53 384 myServo6.pulsewidth_us(1800);
andrescas 0:4b1e8862ad53 385 pc.printf("tercer caso \n");
andrescas 0:4b1e8862ad53 386 break;
andrescas 0:4b1e8862ad53 387 }
andrescas 0:4b1e8862ad53 388
andrescas 0:4b1e8862ad53 389 case 0x04: {
andrescas 0:4b1e8862ad53 390 myServo7.pulsewidth_us(500);
andrescas 0:4b1e8862ad53 391 myServo8.pulsewidth_us(1800);
andrescas 0:4b1e8862ad53 392 pc.printf("cuarto caso \n");
andrescas 0:4b1e8862ad53 393 break;
andrescas 0:4b1e8862ad53 394 default:
andrescas 0:4b1e8862ad53 395 pc.printf("solo 4 casos error en comando\n");
andrescas 0:4b1e8862ad53 396 break ;
andrescas 0:4b1e8862ad53 397 }
andrescas 0:4b1e8862ad53 398 }
andrescas 0:4b1e8862ad53 399 pc.printf("fin mueve grupo\n");
andrescas 0:4b1e8862ad53 400
andrescas 0:4b1e8862ad53 401 }
andrescas 0:4b1e8862ad53 402 void adelante()
andrescas 0:4b1e8862ad53 403 {
andrescas 0:4b1e8862ad53 404 // PASO DERECHA-DELANTERA
andrescas 0:4b1e8862ad53 405
andrescas 0:4b1e8862ad53 406 myServo8.pulsewidth_us(1000);
andrescas 0:4b1e8862ad53 407 wait(0.5);
andrescas 0:4b1e8862ad53 408 myServo7.pulsewidth_us(2000);
andrescas 0:4b1e8862ad53 409 wait(0.5);
andrescas 0:4b1e8862ad53 410 myServo8.pulsewidth_us(2000);
andrescas 0:4b1e8862ad53 411 wait(0.5);
andrescas 0:4b1e8862ad53 412 myServo7.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 413 wait(0.5);
andrescas 0:4b1e8862ad53 414 myServo8.pulsewidth_us(1000);
andrescas 0:4b1e8862ad53 415 wait(0.5);
andrescas 0:4b1e8862ad53 416
andrescas 0:4b1e8862ad53 417 // PASO IZQUIERDA-TRASERA
andrescas 0:4b1e8862ad53 418
andrescas 0:4b1e8862ad53 419 myServo4.pulsewidth_us(1600);
andrescas 0:4b1e8862ad53 420 wait(0.5);
andrescas 0:4b1e8862ad53 421 myServo3.pulsewidth_us(2000);
andrescas 0:4b1e8862ad53 422 wait(0.5);
andrescas 0:4b1e8862ad53 423 myServo4.pulsewidth_us(800);
andrescas 0:4b1e8862ad53 424 wait(0.5);
andrescas 0:4b1e8862ad53 425 myServo3.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 426 wait(0.5);
andrescas 0:4b1e8862ad53 427 myServo4.pulsewidth_us(1600);
andrescas 0:4b1e8862ad53 428 wait(0.5);
andrescas 0:4b1e8862ad53 429
andrescas 0:4b1e8862ad53 430 // PASO IZQUIERDA-DELANTERA
andrescas 0:4b1e8862ad53 431
andrescas 0:4b1e8862ad53 432 myServo2.pulsewidth_us(2400);
andrescas 0:4b1e8862ad53 433 wait(0.5);
andrescas 0:4b1e8862ad53 434 myServo1.pulsewidth_us(2000);
andrescas 0:4b1e8862ad53 435 wait(0.5);
andrescas 0:4b1e8862ad53 436 myServo2.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 437 wait(0.5);
andrescas 0:4b1e8862ad53 438 myServo1.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 439 wait(0.5);
andrescas 0:4b1e8862ad53 440 myServo2.pulsewidth_us(2400);
andrescas 0:4b1e8862ad53 441 wait(0.5);
andrescas 0:4b1e8862ad53 442
andrescas 0:4b1e8862ad53 443 // PASO DERECHA-TRASERA
andrescas 0:4b1e8862ad53 444
andrescas 0:4b1e8862ad53 445 myServo6.pulsewidth_us(1500);
andrescas 0:4b1e8862ad53 446 wait(0.5);
andrescas 0:4b1e8862ad53 447 myServo5.pulsewidth_us(2000);
andrescas 0:4b1e8862ad53 448 wait(0.5);
andrescas 0:4b1e8862ad53 449 myServo6.pulsewidth_us(2300);
andrescas 0:4b1e8862ad53 450 wait(0.5);
andrescas 0:4b1e8862ad53 451 myServo5.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 452 wait(0.5);
andrescas 0:4b1e8862ad53 453 myServo6.pulsewidth_us(1500);
andrescas 0:4b1e8862ad53 454 wait(0.5);
andrescas 0:4b1e8862ad53 455
andrescas 0:4b1e8862ad53 456 }
andrescas 0:4b1e8862ad53 457
andrescas 0:4b1e8862ad53 458 void atras()
andrescas 0:4b1e8862ad53 459 {
andrescas 0:4b1e8862ad53 460 // PASO DERECHA-DELANTERA
andrescas 0:4b1e8862ad53 461
andrescas 0:4b1e8862ad53 462 myServo8.pulsewidth_us(1000);
andrescas 0:4b1e8862ad53 463 wait(0.5);
andrescas 0:4b1e8862ad53 464 myServo7.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 465 wait(0.5);
andrescas 0:4b1e8862ad53 466 myServo8.pulsewidth_us(2000);
andrescas 0:4b1e8862ad53 467 wait(0.5);
andrescas 0:4b1e8862ad53 468 myServo7.pulsewidth_us(2000);
andrescas 0:4b1e8862ad53 469 wait(0.5);
andrescas 0:4b1e8862ad53 470 myServo8.pulsewidth_us(1000);
andrescas 0:4b1e8862ad53 471 wait(0.5);
andrescas 0:4b1e8862ad53 472 myServo7.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 473 wait(0.5);
andrescas 0:4b1e8862ad53 474
andrescas 0:4b1e8862ad53 475 // PASO IZQUIERDA-TRASERA
andrescas 0:4b1e8862ad53 476
andrescas 0:4b1e8862ad53 477 myServo4.pulsewidth_us(1600);
andrescas 0:4b1e8862ad53 478 wait(0.5);
andrescas 0:4b1e8862ad53 479 myServo3.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 480 wait(0.5);
andrescas 0:4b1e8862ad53 481 myServo4.pulsewidth_us(800);
andrescas 0:4b1e8862ad53 482 wait(0.5);
andrescas 0:4b1e8862ad53 483 myServo3.pulsewidth_us(2000);
andrescas 0:4b1e8862ad53 484 wait(0.5);
andrescas 0:4b1e8862ad53 485 myServo4.pulsewidth_us(1600);
andrescas 0:4b1e8862ad53 486 wait(0.5);
andrescas 0:4b1e8862ad53 487 myServo3.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 488 wait(0.5);
andrescas 0:4b1e8862ad53 489
andrescas 0:4b1e8862ad53 490 // PASO IZQUIERDA-DELANTERA
andrescas 0:4b1e8862ad53 491
andrescas 0:4b1e8862ad53 492 myServo2.pulsewidth_us(2400);
andrescas 0:4b1e8862ad53 493 wait(0.5);
andrescas 0:4b1e8862ad53 494 myServo1.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 495 wait(0.5);
andrescas 0:4b1e8862ad53 496 myServo2.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 497 wait(0.5);
andrescas 0:4b1e8862ad53 498 myServo1.pulsewidth_us(2000);
andrescas 0:4b1e8862ad53 499 wait(0.5);
andrescas 0:4b1e8862ad53 500 myServo2.pulsewidth_us(2400);
andrescas 0:4b1e8862ad53 501 wait(0.5);
andrescas 0:4b1e8862ad53 502 myServo1.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 503 wait(0.5);
andrescas 0:4b1e8862ad53 504
andrescas 0:4b1e8862ad53 505 // PASO DERECHA-TRASERA
andrescas 0:4b1e8862ad53 506
andrescas 0:4b1e8862ad53 507 myServo6.pulsewidth_us(1500);
andrescas 0:4b1e8862ad53 508 wait(0.5);
andrescas 0:4b1e8862ad53 509 myServo5.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 510 wait(0.5);
andrescas 0:4b1e8862ad53 511 myServo6.pulsewidth_us(2300);
andrescas 0:4b1e8862ad53 512 wait(0.5);
andrescas 0:4b1e8862ad53 513 myServo5.pulsewidth_us(2000);
andrescas 0:4b1e8862ad53 514 wait(0.5);
andrescas 0:4b1e8862ad53 515 myServo6.pulsewidth_us(1500);
andrescas 0:4b1e8862ad53 516 wait(0.5);
andrescas 0:4b1e8862ad53 517 myServo5.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 518 wait(0.5);
andrescas 0:4b1e8862ad53 519
andrescas 0:4b1e8862ad53 520
andrescas 0:4b1e8862ad53 521 }