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@0:4b1e8862ad53, 2018-11-14 (annotated)

Committer:
andrescas
Date:
Wed Nov 14 20:37:28 2018 +0000
Revision:
0:4b1e8862ad53
Child:
1:9430a3a15f7a
bjbj

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 0:4b1e8862ad53 211 myServo2.pulsewidth_us(1800);
andrescas 0:4b1e8862ad53 212 wait(0.5);
andrescas 0:4b1e8862ad53 213 myServo8.pulsewidth_us(1600);
andrescas 0:4b1e8862ad53 214 wait(0.5);
andrescas 0:4b1e8862ad53 215 myServo4.pulsewidth_us(1400);
andrescas 0:4b1e8862ad53 216 wait(0.5);
andrescas 0:4b1e8862ad53 217 myServo6.pulsewidth_us(1600);
andrescas 0:4b1e8862ad53 218 wait(0.5);
andrescas 0:4b1e8862ad53 219
andrescas 0:4b1e8862ad53 220 /* myServo1.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 221 wait(0.5);
andrescas 0:4b1e8862ad53 222 myServo3.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 223 wait(0.5);
andrescas 0:4b1e8862ad53 224 myServo5.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 225 wait(0.5);
andrescas 0:4b1e8862ad53 226 myServo7.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 227 wait(1);*/
andrescas 0:4b1e8862ad53 228
andrescas 0:4b1e8862ad53 229 pc.printf("rojo --%d\n",rojo);
andrescas 0:4b1e8862ad53 230
andrescas 0:4b1e8862ad53 231 }
andrescas 0:4b1e8862ad53 232 }
andrescas 0:4b1e8862ad53 233 }
andrescas 0:4b1e8862ad53 234
andrescas 0:4b1e8862ad53 235 if (rojo>=200 and rojo <=450) { // lee el verde --------------------------------------
andrescas 0:4b1e8862ad53 236 if (verde>=200 and verde <=450) {
andrescas 0:4b1e8862ad53 237 if (azul>=200 and azul <=450) {
andrescas 0:4b1e8862ad53 238
andrescas 0:4b1e8862ad53 239
andrescas 0:4b1e8862ad53 240 // GIRO A LA DERECHA
andrescas 0:4b1e8862ad53 241 pc.printf("GIRO A LA DERECHA --%d\n",verde);
andrescas 0:4b1e8862ad53 242 myServo1.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 243 myServo3.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 244 myServo5.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 245 myServo7.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 246 wait(0.5);
andrescas 0:4b1e8862ad53 247
andrescas 0:4b1e8862ad53 248 myServo1.pulsewidth_us(2000);
andrescas 0:4b1e8862ad53 249 wait(0.5);
andrescas 0:4b1e8862ad53 250 myServo2.pulsewidth_us(1000);
andrescas 0:4b1e8862ad53 251 wait(0.5);
andrescas 0:4b1e8862ad53 252 myServo1.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 253 wait(0.5);
andrescas 0:4b1e8862ad53 254
andrescas 0:4b1e8862ad53 255 myServo7.pulsewidth_us(2000);
andrescas 0:4b1e8862ad53 256 wait(0.5);
andrescas 0:4b1e8862ad53 257 myServo8.pulsewidth_us(1000);
andrescas 0:4b1e8862ad53 258 wait(0.5);
andrescas 0:4b1e8862ad53 259 myServo7.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 260 wait(0.5);
andrescas 0:4b1e8862ad53 261
andrescas 0:4b1e8862ad53 262 myServo5.pulsewidth_us(2000);
andrescas 0:4b1e8862ad53 263 wait(0.5);
andrescas 0:4b1e8862ad53 264 myServo6.pulsewidth_us(1000);
andrescas 0:4b1e8862ad53 265 wait(0.5);
andrescas 0:4b1e8862ad53 266 myServo5.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 267 wait(0.5);
andrescas 0:4b1e8862ad53 268
andrescas 0:4b1e8862ad53 269 myServo3.pulsewidth_us(2000);
andrescas 0:4b1e8862ad53 270 wait(0.5);
andrescas 0:4b1e8862ad53 271 myServo4.pulsewidth_us(1000);
andrescas 0:4b1e8862ad53 272 wait(0.5);
andrescas 0:4b1e8862ad53 273 myServo3.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 274 wait(0.5);
andrescas 0:4b1e8862ad53 275
andrescas 0:4b1e8862ad53 276
andrescas 0:4b1e8862ad53 277 myServo2.pulsewidth_us(2200);
andrescas 0:4b1e8862ad53 278 myServo6.pulsewidth_us(2200);
andrescas 0:4b1e8862ad53 279 wait(0.5);
andrescas 0:4b1e8862ad53 280 myServo4.pulsewidth_us(2200);
andrescas 0:4b1e8862ad53 281 myServo8.pulsewidth_us(2200);
andrescas 0:4b1e8862ad53 282 wait(1);
andrescas 0:4b1e8862ad53 283
andrescas 0:4b1e8862ad53 284 pc.printf("verde --%d\n",verde);
andrescas 0:4b1e8862ad53 285
andrescas 0:4b1e8862ad53 286 }
andrescas 0:4b1e8862ad53 287 }
andrescas 0:4b1e8862ad53 288 }
andrescas 0:4b1e8862ad53 289
andrescas 0:4b1e8862ad53 290 if (rojo>=400 and rojo <=800) { // lee el azul------------------------------------
andrescas 0:4b1e8862ad53 291 if (verde>=200 and verde <=500) {
andrescas 0:4b1e8862ad53 292 if (azul>=1 and azul <=400) {
andrescas 0:4b1e8862ad53 293
andrescas 0:4b1e8862ad53 294
andrescas 0:4b1e8862ad53 295
andrescas 0:4b1e8862ad53 296 // GIRO A LA IZQUIERDA
andrescas 0:4b1e8862ad53 297 pc.printf("GIRO A LA IZQUIERDA --%d\n",azul);
andrescas 0:4b1e8862ad53 298
andrescas 0:4b1e8862ad53 299 myServo1.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 300 myServo3.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 301 myServo5.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 302 myServo7.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 303 wait(1);
andrescas 0:4b1e8862ad53 304
andrescas 0:4b1e8862ad53 305 myServo1.pulsewidth_us(2000);
andrescas 0:4b1e8862ad53 306 wait(0.5);
andrescas 0:4b1e8862ad53 307 myServo2.pulsewidth_us(2200);
andrescas 0:4b1e8862ad53 308 wait(0.5);
andrescas 0:4b1e8862ad53 309 myServo1.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 310 wait(0.5);
andrescas 0:4b1e8862ad53 311
andrescas 0:4b1e8862ad53 312 myServo7.pulsewidth_us(2000);
andrescas 0:4b1e8862ad53 313 wait(0.5);
andrescas 0:4b1e8862ad53 314 myServo8.pulsewidth_us(2200);
andrescas 0:4b1e8862ad53 315 wait(0.5);
andrescas 0:4b1e8862ad53 316 myServo7.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 317 wait(0.5);
andrescas 0:4b1e8862ad53 318
andrescas 0:4b1e8862ad53 319 myServo5.pulsewidth_us(2000);
andrescas 0:4b1e8862ad53 320 wait(0.5);
andrescas 0:4b1e8862ad53 321 myServo6.pulsewidth_us(2200);
andrescas 0:4b1e8862ad53 322 wait(0.5);
andrescas 0:4b1e8862ad53 323 myServo5.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 324 wait(0.5);
andrescas 0:4b1e8862ad53 325
andrescas 0:4b1e8862ad53 326 myServo3.pulsewidth_us(2000);
andrescas 0:4b1e8862ad53 327 wait(0.5);
andrescas 0:4b1e8862ad53 328 myServo4.pulsewidth_us(2200);
andrescas 0:4b1e8862ad53 329 wait(0.5);
andrescas 0:4b1e8862ad53 330 myServo3.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 331 wait(0.5);
andrescas 0:4b1e8862ad53 332
andrescas 0:4b1e8862ad53 333 myServo2.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 334 myServo6.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 335 wait(0.5);
andrescas 0:4b1e8862ad53 336 myServo4.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 337 myServo8.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 338 wait(1);
andrescas 0:4b1e8862ad53 339
andrescas 0:4b1e8862ad53 340 pc.printf("azul --%d\n",azul);
andrescas 0:4b1e8862ad53 341 }
andrescas 0:4b1e8862ad53 342 }
andrescas 0:4b1e8862ad53 343 }
andrescas 0:4b1e8862ad53 344 timeout.attach(&timer_interrupt, 0.5); // reiniciamos el timeout para la siguiente lectura
andrescas 0:4b1e8862ad53 345 }
andrescas 0:4b1e8862ad53 346 void grupomover()
andrescas 0:4b1e8862ad53 347 {
andrescas 0:4b1e8862ad53 348 pc.printf("mueve grupo \n");
andrescas 0:4b1e8862ad53 349 switch (nmotor) {
andrescas 0:4b1e8862ad53 350 case 0x01: {
andrescas 0:4b1e8862ad53 351 myServo1.pulsewidth_us(550);
andrescas 0:4b1e8862ad53 352 myServo2.pulsewidth_us(1800);
andrescas 0:4b1e8862ad53 353 pc.printf("primer caso\n");
andrescas 0:4b1e8862ad53 354 break;
andrescas 0:4b1e8862ad53 355 }
andrescas 0:4b1e8862ad53 356
andrescas 0:4b1e8862ad53 357 case 0x02: {
andrescas 0:4b1e8862ad53 358 myServo3.pulsewidth_us(550);
andrescas 0:4b1e8862ad53 359 myServo4.pulsewidth_us(1800);
andrescas 0:4b1e8862ad53 360 pc.printf("segundo caso \n");
andrescas 0:4b1e8862ad53 361 break ;
andrescas 0:4b1e8862ad53 362 }
andrescas 0:4b1e8862ad53 363
andrescas 0:4b1e8862ad53 364 case 0x03: {
andrescas 0:4b1e8862ad53 365 myServo5.pulsewidth_us(550);
andrescas 0:4b1e8862ad53 366 myServo6.pulsewidth_us(1800);
andrescas 0:4b1e8862ad53 367 pc.printf("tercer caso \n");
andrescas 0:4b1e8862ad53 368 break;
andrescas 0:4b1e8862ad53 369 }
andrescas 0:4b1e8862ad53 370
andrescas 0:4b1e8862ad53 371 case 0x04: {
andrescas 0:4b1e8862ad53 372 myServo7.pulsewidth_us(500);
andrescas 0:4b1e8862ad53 373 myServo8.pulsewidth_us(1800);
andrescas 0:4b1e8862ad53 374 pc.printf("cuarto caso \n");
andrescas 0:4b1e8862ad53 375 break;
andrescas 0:4b1e8862ad53 376 default:
andrescas 0:4b1e8862ad53 377 pc.printf("solo 4 casos error en comando\n");
andrescas 0:4b1e8862ad53 378 break ;
andrescas 0:4b1e8862ad53 379 }
andrescas 0:4b1e8862ad53 380 }
andrescas 0:4b1e8862ad53 381 pc.printf("fin mueve grupo\n");
andrescas 0:4b1e8862ad53 382
andrescas 0:4b1e8862ad53 383 }
andrescas 0:4b1e8862ad53 384 void adelante()
andrescas 0:4b1e8862ad53 385 {
andrescas 0:4b1e8862ad53 386 // PASO DERECHA-DELANTERA
andrescas 0:4b1e8862ad53 387
andrescas 0:4b1e8862ad53 388 myServo8.pulsewidth_us(1000);
andrescas 0:4b1e8862ad53 389 wait(0.5);
andrescas 0:4b1e8862ad53 390 myServo7.pulsewidth_us(2000);
andrescas 0:4b1e8862ad53 391 wait(0.5);
andrescas 0:4b1e8862ad53 392 myServo8.pulsewidth_us(2000);
andrescas 0:4b1e8862ad53 393 wait(0.5);
andrescas 0:4b1e8862ad53 394 myServo7.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 395 wait(0.5);
andrescas 0:4b1e8862ad53 396 myServo8.pulsewidth_us(1000);
andrescas 0:4b1e8862ad53 397 wait(0.5);
andrescas 0:4b1e8862ad53 398
andrescas 0:4b1e8862ad53 399 // PASO IZQUIERDA-TRASERA
andrescas 0:4b1e8862ad53 400
andrescas 0:4b1e8862ad53 401 myServo4.pulsewidth_us(1600);
andrescas 0:4b1e8862ad53 402 wait(0.5);
andrescas 0:4b1e8862ad53 403 myServo3.pulsewidth_us(2000);
andrescas 0:4b1e8862ad53 404 wait(0.5);
andrescas 0:4b1e8862ad53 405 myServo4.pulsewidth_us(800);
andrescas 0:4b1e8862ad53 406 wait(0.5);
andrescas 0:4b1e8862ad53 407 myServo3.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 408 wait(0.5);
andrescas 0:4b1e8862ad53 409 myServo4.pulsewidth_us(1600);
andrescas 0:4b1e8862ad53 410 wait(0.5);
andrescas 0:4b1e8862ad53 411
andrescas 0:4b1e8862ad53 412 // PASO IZQUIERDA-DELANTERA
andrescas 0:4b1e8862ad53 413
andrescas 0:4b1e8862ad53 414 myServo2.pulsewidth_us(2400);
andrescas 0:4b1e8862ad53 415 wait(0.5);
andrescas 0:4b1e8862ad53 416 myServo1.pulsewidth_us(2000);
andrescas 0:4b1e8862ad53 417 wait(0.5);
andrescas 0:4b1e8862ad53 418 myServo2.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 419 wait(0.5);
andrescas 0:4b1e8862ad53 420 myServo1.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 421 wait(0.5);
andrescas 0:4b1e8862ad53 422 myServo2.pulsewidth_us(2400);
andrescas 0:4b1e8862ad53 423 wait(0.5);
andrescas 0:4b1e8862ad53 424
andrescas 0:4b1e8862ad53 425 // PASO DERECHA-TRASERA
andrescas 0:4b1e8862ad53 426
andrescas 0:4b1e8862ad53 427 myServo6.pulsewidth_us(1500);
andrescas 0:4b1e8862ad53 428 wait(0.5);
andrescas 0:4b1e8862ad53 429 myServo5.pulsewidth_us(2000);
andrescas 0:4b1e8862ad53 430 wait(0.5);
andrescas 0:4b1e8862ad53 431 myServo6.pulsewidth_us(2300);
andrescas 0:4b1e8862ad53 432 wait(0.5);
andrescas 0:4b1e8862ad53 433 myServo5.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 434 wait(0.5);
andrescas 0:4b1e8862ad53 435 myServo6.pulsewidth_us(1500);
andrescas 0:4b1e8862ad53 436 wait(0.5);
andrescas 0:4b1e8862ad53 437
andrescas 0:4b1e8862ad53 438 }
andrescas 0:4b1e8862ad53 439
andrescas 0:4b1e8862ad53 440 void atras()
andrescas 0:4b1e8862ad53 441 {
andrescas 0:4b1e8862ad53 442 // PASO DERECHA-DELANTERA
andrescas 0:4b1e8862ad53 443
andrescas 0:4b1e8862ad53 444 myServo8.pulsewidth_us(1000);
andrescas 0:4b1e8862ad53 445 wait(0.5);
andrescas 0:4b1e8862ad53 446 myServo7.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 447 wait(0.5);
andrescas 0:4b1e8862ad53 448 myServo8.pulsewidth_us(2000);
andrescas 0:4b1e8862ad53 449 wait(0.5);
andrescas 0:4b1e8862ad53 450 myServo7.pulsewidth_us(2000);
andrescas 0:4b1e8862ad53 451 wait(0.5);
andrescas 0:4b1e8862ad53 452 myServo8.pulsewidth_us(1000);
andrescas 0:4b1e8862ad53 453 wait(0.5);
andrescas 0:4b1e8862ad53 454 myServo7.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 455 wait(0.5);
andrescas 0:4b1e8862ad53 456
andrescas 0:4b1e8862ad53 457 // PASO IZQUIERDA-TRASERA
andrescas 0:4b1e8862ad53 458
andrescas 0:4b1e8862ad53 459 myServo4.pulsewidth_us(1600);
andrescas 0:4b1e8862ad53 460 wait(0.5);
andrescas 0:4b1e8862ad53 461 myServo3.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 462 wait(0.5);
andrescas 0:4b1e8862ad53 463 myServo4.pulsewidth_us(800);
andrescas 0:4b1e8862ad53 464 wait(0.5);
andrescas 0:4b1e8862ad53 465 myServo3.pulsewidth_us(2000);
andrescas 0:4b1e8862ad53 466 wait(0.5);
andrescas 0:4b1e8862ad53 467 myServo4.pulsewidth_us(1600);
andrescas 0:4b1e8862ad53 468 wait(0.5);
andrescas 0:4b1e8862ad53 469 myServo3.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 470 wait(0.5);
andrescas 0:4b1e8862ad53 471
andrescas 0:4b1e8862ad53 472 // PASO IZQUIERDA-DELANTERA
andrescas 0:4b1e8862ad53 473
andrescas 0:4b1e8862ad53 474 myServo2.pulsewidth_us(2400);
andrescas 0:4b1e8862ad53 475 wait(0.5);
andrescas 0:4b1e8862ad53 476 myServo1.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 477 wait(0.5);
andrescas 0:4b1e8862ad53 478 myServo2.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 479 wait(0.5);
andrescas 0:4b1e8862ad53 480 myServo1.pulsewidth_us(2000);
andrescas 0:4b1e8862ad53 481 wait(0.5);
andrescas 0:4b1e8862ad53 482 myServo2.pulsewidth_us(2400);
andrescas 0:4b1e8862ad53 483 wait(0.5);
andrescas 0:4b1e8862ad53 484 myServo1.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 485 wait(0.5);
andrescas 0:4b1e8862ad53 486
andrescas 0:4b1e8862ad53 487 // PASO DERECHA-TRASERA
andrescas 0:4b1e8862ad53 488
andrescas 0:4b1e8862ad53 489 myServo6.pulsewidth_us(1500);
andrescas 0:4b1e8862ad53 490 wait(0.5);
andrescas 0:4b1e8862ad53 491 myServo5.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 492 wait(0.5);
andrescas 0:4b1e8862ad53 493 myServo6.pulsewidth_us(2300);
andrescas 0:4b1e8862ad53 494 wait(0.5);
andrescas 0:4b1e8862ad53 495 myServo5.pulsewidth_us(2000);
andrescas 0:4b1e8862ad53 496 wait(0.5);
andrescas 0:4b1e8862ad53 497 myServo6.pulsewidth_us(1500);
andrescas 0:4b1e8862ad53 498 wait(0.5);
andrescas 0:4b1e8862ad53 499 myServo5.pulsewidth_us(1200);
andrescas 0:4b1e8862ad53 500 wait(0.5);
andrescas 0:4b1e8862ad53 501
andrescas 0:4b1e8862ad53 502
andrescas 0:4b1e8862ad53 503 }