Javier Sanchez Peña / Mbed 2 deprecated lectura_encoders_proyecto_brazo_robotico

establece la posicion de referencia del brazo robotico, manda la posicion de los encoders por bluetooth y permite el control remoto a traves de un dispositivo android.

Dependencies:   Motor QEI mbed

main.cpp@6:644b77b2111e, 2017-06-09 (annotated)

Committer:
javierjsp
Date:
Fri Jun 09 01:06:34 2017 +0000
Revision:
6:644b77b2111e
Parent:
5:053c61912254
Child:
7:cb59c58cab78
retoques

Who changed what in which revision?

UserRevisionLine numberNew contents of line
javierjsp 0:b7978deb977b 1 #include "mbed.h"
javierjsp 0:b7978deb977b 2 #include "Motor.h"
javierjsp 0:b7978deb977b 3 #include "QEI.h"
javierjsp 0:b7978deb977b 4
javierjsp 0:b7978deb977b 5 // declaramos los encoders
javierjsp 0:b7978deb977b 6 QEI enc1(PG_4, PG_6, NC, 480, QEI::X4_ENCODING); // (P0, P1, indice, PulsosPorRev, TipoDeEncoder)
javierjsp 0:b7978deb977b 7 QEI enc2(PG_7, PG_5, NC, 480, QEI::X4_ENCODING);
javierjsp 0:b7978deb977b 8 QEI enc3(PD_10, PG_8, NC, 480, QEI::X4_ENCODING);
javierjsp 0:b7978deb977b 9 QEI enc4(PG_14, PF_11, NC, 480, QEI::X4_ENCODING);
javierjsp 0:b7978deb977b 10 QEI enc5(PF_12, PF_15, NC, 480, QEI::X4_ENCODING);
javierjsp 0:b7978deb977b 11 QEI enc6(PF_13, PF_3, NC, 240, QEI::X4_ENCODING);
javierjsp 0:b7978deb977b 12
javierjsp 0:b7978deb977b 13 // declaramos los motores
javierjsp 0:b7978deb977b 14 Motor m1(PA_5, PC_8, PC_6); // (pwm, derecho, inverso)
javierjsp 0:b7978deb977b 15 Motor m2(PA_6, PC_5, PD_8);
javierjsp 0:b7978deb977b 16 Motor m3(PA_7, PA_12, PA_11);
javierjsp 0:b7978deb977b 17 Motor m4(PB_6, PB_12, PB_11);
javierjsp 0:b7978deb977b 18 Motor m5(PC_7, PB_2, PB_1);
javierjsp 0:b7978deb977b 19 Motor m6(PB_9, PB_15, PB_13);
javierjsp 1:7c07ddc42a4c 20 Motor cinta(PB_8, PC_4, PF_5);
javierjsp 0:b7978deb977b 21
javierjsp 0:b7978deb977b 22 // declaramos las entradas de los micropulsadores
javierjsp 0:b7978deb977b 23 DigitalIn mp1(PA_9, PullUp); // (pin, modo)
javierjsp 0:b7978deb977b 24 DigitalIn mp2(PA_8, PullUp);
javierjsp 0:b7978deb977b 25 DigitalIn mp3(PB_10, PullUp);
javierjsp 0:b7978deb977b 26 DigitalIn mp4(PB_4, PullUp);
javierjsp 0:b7978deb977b 27 DigitalIn mp5(PB_5, PullUp);
javierjsp 1:7c07ddc42a4c 28 DigitalIn sensor(PB_3, PullUp);
javierjsp 0:b7978deb977b 29
javierjsp 0:b7978deb977b 30 // declaramos el bluetooth
javierjsp 0:b7978deb977b 31 Serial bt(PA_2, PA_3); // (RX, TX) (puerto serial 2)
javierjsp 0:b7978deb977b 32
javierjsp 1:7c07ddc42a4c 33 // declaramos otras variables que utilizaremos
javierjsp 6:644b77b2111e 34 Ticker ticker1, ticker2; // iniciamos dos tickers
javierjsp 0:b7978deb977b 35 DigitalOut led1 (LED1); // declaramos el LED1
javierjsp 0:b7978deb977b 36 DigitalOut led2 (LED2); // declaramos el LED2
javierjsp 0:b7978deb977b 37 DigitalOut led3 (LED3); // declaramos el LED3
javierjsp 0:b7978deb977b 38 int pulsos[6]={0,0,0,0,0,0}; // iniciamos el array pulsos
javierjsp 6:644b77b2111e 39 int posiciones[10][6]; // declaramos los arrays de posiciones
javierjsp 6:644b77b2111e 40 for (int i=0; i<10; i++)
javierjsp 6:644b77b2111e 41 for (int j=0; j<10; j++)
javierjsp 6:644b77b2111e 42 posiciones[i][j];
javierjsp 0:b7978deb977b 43 char dato=0;
javierjsp 0:b7978deb977b 44
javierjsp 0:b7978deb977b 45 // declaramos el prototipo de las funciones que utilizaremos
javierjsp 1:7c07ddc42a4c 46 void enviar_bt(); // prototipo de la funcion que ejecutara el ticker1
javierjsp 6:644b77b2111e 47 void encenderLEDs(); // prototipo de la funcion que ejecutara el ticker1
javierjsp 0:b7978deb977b 48 void sethome(); // prototipo de la funcion que nos calibrara la posicion hard home de referencia
javierjsp 1:7c07ddc42a4c 49 void posicion(); // prototipo de la funcion que nos llevara hasta la posicion especificada
javierjsp 0:b7978deb977b 50 void mover(int,int,float); // prototipo de la funcion que movera cada motor
javierjsp 0:b7978deb977b 51 void frenar(int); // prototipo de la funcion que frenara cada motor
javierjsp 0:b7978deb977b 52 void pinza(int); // prototipo de la funcion que abrira (-1) o cerrara (1) la pinza
javierjsp 6:644b77b2111e 53 void movCinta(); // prototipo de la funcion que avanzara la cinta hasta detectar un objeto
javierjsp 0:b7978deb977b 54
javierjsp 0:b7978deb977b 55 int main()
javierjsp 0:b7978deb977b 56 {
javierjsp 6:644b77b2111e 57 m1.period(0.0001); // establece el periodo del PWM, equivalente a una frecuencia de 10KHz.
javierjsp 6:644b77b2111e 58 m2.period(0.0001);
javierjsp 6:644b77b2111e 59 m3.period(0.0001);
javierjsp 6:644b77b2111e 60 m4.period(0.0001);
javierjsp 6:644b77b2111e 61 m5.period(0.0001);
javierjsp 6:644b77b2111e 62 m6.period(0.0001);
javierjsp 0:b7978deb977b 63
javierjsp 6:644b77b2111e 64 bt.baud(9600); // velocidad de transmision del bluetooth a 9600 baudios
javierjsp 0:b7978deb977b 65
javierjsp 1:7c07ddc42a4c 66 ticker1.attach(&enviar_bt, 0.5); // asignamos la funcion "enviar_bt" al protocolo de interrupcion del "ticker1" y le damos un intervalo de 0.5s
javierjsp 6:644b77b2111e 67 ticker2.attach(&encenderLEDs, 0.05); // asignamos la funcion "encenderLED1" al protocolo de interrupcion del "ticker2" y le damos un intervalo de 0.05s
javierjsp 4:9ea8a9271732 68
javierjsp 6:644b77b2111e 69 sethome(); // establecemos la posicion de referencia
javierjsp 0:b7978deb977b 70
javierjsp 0:b7978deb977b 71 while(1)
javierjsp 0:b7978deb977b 72 {
javierjsp 0:b7978deb977b 73 if(bt.readable()>0)
javierjsp 0:b7978deb977b 74 {
javierjsp 0:b7978deb977b 75 dato = bt.getc();
javierjsp 0:b7978deb977b 76 switch(dato)
javierjsp 0:b7978deb977b 77 {
javierjsp 0:b7978deb977b 78 case 'A':
javierjsp 0:b7978deb977b 79 mover(1,1,1);
javierjsp 0:b7978deb977b 80 wait(0.2);
javierjsp 0:b7978deb977b 81 frenar(1);
javierjsp 0:b7978deb977b 82 break;
javierjsp 0:b7978deb977b 83 case 'B':
javierjsp 0:b7978deb977b 84 mover(1,-1,1);
javierjsp 0:b7978deb977b 85 wait(0.2);
javierjsp 0:b7978deb977b 86 frenar(1);
javierjsp 0:b7978deb977b 87 break;
javierjsp 0:b7978deb977b 88 case 'C':
javierjsp 0:b7978deb977b 89 mover(2,1,1);
javierjsp 0:b7978deb977b 90 wait(0.2);
javierjsp 0:b7978deb977b 91 frenar(2);
javierjsp 0:b7978deb977b 92 break;
javierjsp 0:b7978deb977b 93 case 'D':
javierjsp 0:b7978deb977b 94 mover(2,-1,1);
javierjsp 0:b7978deb977b 95 wait(0.2);
javierjsp 0:b7978deb977b 96 frenar(2);
javierjsp 0:b7978deb977b 97 break;
javierjsp 0:b7978deb977b 98 case 'E':
javierjsp 0:b7978deb977b 99 mover(3,1,1);
javierjsp 0:b7978deb977b 100 wait(0.2);
javierjsp 0:b7978deb977b 101 frenar(3);
javierjsp 0:b7978deb977b 102 break;
javierjsp 0:b7978deb977b 103 case 'F':
javierjsp 0:b7978deb977b 104 mover(3,-1,1);
javierjsp 0:b7978deb977b 105 wait(0.2);
javierjsp 0:b7978deb977b 106 frenar(3);
javierjsp 0:b7978deb977b 107 break;
javierjsp 0:b7978deb977b 108 case 'G':
javierjsp 0:b7978deb977b 109 mover(4,1,1);
javierjsp 0:b7978deb977b 110 wait(0.2);
javierjsp 0:b7978deb977b 111 frenar(4);
javierjsp 0:b7978deb977b 112 break;
javierjsp 0:b7978deb977b 113 case 'H':
javierjsp 0:b7978deb977b 114 mover(4,-1,1);
javierjsp 0:b7978deb977b 115 wait(0.2);
javierjsp 0:b7978deb977b 116 frenar(4);
javierjsp 0:b7978deb977b 117 break;
javierjsp 0:b7978deb977b 118 case 'I':
javierjsp 0:b7978deb977b 119 mover(5,1,1);
javierjsp 0:b7978deb977b 120 wait(0.2);
javierjsp 0:b7978deb977b 121 frenar(5);
javierjsp 0:b7978deb977b 122 break;
javierjsp 0:b7978deb977b 123 case 'J':
javierjsp 0:b7978deb977b 124 mover(5,-1,1);
javierjsp 0:b7978deb977b 125 wait(0.2);
javierjsp 0:b7978deb977b 126 frenar(5);
javierjsp 0:b7978deb977b 127 break;
javierjsp 0:b7978deb977b 128 case 'K':
javierjsp 0:b7978deb977b 129 pinza(-1);
javierjsp 0:b7978deb977b 130 break;
javierjsp 0:b7978deb977b 131 case 'L':
javierjsp 0:b7978deb977b 132 pinza(1);
javierjsp 0:b7978deb977b 133 break;
javierjsp 6:644b77b2111e 134 case 'M':
javierjsp 6:644b77b2111e 135 movCinta();
javierjsp 6:644b77b2111e 136 break;
javierjsp 0:b7978deb977b 137 }
javierjsp 0:b7978deb977b 138 }
javierjsp 0:b7978deb977b 139 }
javierjsp 0:b7978deb977b 140 }
javierjsp 0:b7978deb977b 141
javierjsp 0:b7978deb977b 142 void enviar_bt() // definimos la funcion que ejecutara el ticker
javierjsp 0:b7978deb977b 143 {
javierjsp 4:9ea8a9271732 144 pulsos[1]=enc1.getPulses();
javierjsp 4:9ea8a9271732 145 pulsos[2]=enc2.getPulses();
javierjsp 4:9ea8a9271732 146 pulsos[3]=enc3.getPulses();
javierjsp 4:9ea8a9271732 147 pulsos[4]=enc4.getPulses();
javierjsp 4:9ea8a9271732 148 pulsos[5]=enc5.getPulses();
javierjsp 6:644b77b2111e 149 bt.printf(" pulsos encoder 1:%d\n pulsos encoder 2:%d\n pulsos encoder 3:%d\n pulsos encoder 4:%d\n pulsos encoder 5:%d\n", pulsos[1],pulsos[2],pulsos[3],pulsos[4],pulsos[5]); // envia el valor de "pulsos" por bluetooth
javierjsp 0:b7978deb977b 150 }
javierjsp 0:b7978deb977b 151
javierjsp 6:644b77b2111e 152 void encenderLEDs()
javierjsp 1:7c07ddc42a4c 153 {
javierjsp 4:9ea8a9271732 154 if (mp5==0) led1=1; // si se pulsa el mp1 se enciende el LED1
javierjsp 1:7c07ddc42a4c 155 else led1=0; // si no se apaga
javierjsp 4:9ea8a9271732 156 if (mp4==0) led3=1; // si se pulsa el mp1 se enciende el LED1
javierjsp 4:9ea8a9271732 157 else led3=0; // si no se apaga
javierjsp 4:9ea8a9271732 158 }
javierjsp 4:9ea8a9271732 159
javierjsp 0:b7978deb977b 160 void sethome()
javierjsp 0:b7978deb977b 161 {
javierjsp 0:b7978deb977b 162 led2=1;
javierjsp 0:b7978deb977b 163 // posicionar base
javierjsp 0:b7978deb977b 164 int enc=0;
javierjsp 6:644b77b2111e 165 int dir=-1; // mover primero hacia delante
javierjsp 0:b7978deb977b 166 while (mp1==1) // Mientras no se pulse el microswitch
javierjsp 0:b7978deb977b 167 {
javierjsp 0:b7978deb977b 168 enc=enc1.getPulses(); // leemos el contador del encoder
javierjsp 4:9ea8a9271732 169 mover(1,dir,0.7); // movemos el motor en la dirección inicial
javierjsp 6:644b77b2111e 170 wait(0.02); // esperamos 0.01s
javierjsp 0:b7978deb977b 171 if (enc==enc1.getPulses()) // Si el contador no se ha incrementado, hemos llegado al extremo
javierjsp 6:644b77b2111e 172 dir=1; // se cambia la dirección del motor
javierjsp 0:b7978deb977b 173 }
javierjsp 0:b7978deb977b 174 frenar(1); // el motor ha llegado a la posicion adecuada y se frena
javierjsp 0:b7978deb977b 175
javierjsp 0:b7978deb977b 176 // posicionar brazo
javierjsp 6:644b77b2111e 177 dir=-1; // mover primero hacia delante
javierjsp 6:644b77b2111e 178 while (dir==-1) // mientras la direccion sea hacia delante:
javierjsp 0:b7978deb977b 179 {
javierjsp 0:b7978deb977b 180 while (mp2==0) // Mientras se mantenga pulsado el boton
javierjsp 4:9ea8a9271732 181 mover(2,dir,0.7); // se seguira moviendo el motor en la direccion inicial
javierjsp 6:644b77b2111e 182 dir=1; // se cambia a direccion
javierjsp 0:b7978deb977b 183 while (mp2==1) // Mientras el MP esté sin pulsar:
javierjsp 0:b7978deb977b 184 {
javierjsp 0:b7978deb977b 185 enc=enc2.getPulses(); // leemos el valor de los encoders
javierjsp 4:9ea8a9271732 186 mover(2,dir,0.7); // movemos el motor
javierjsp 6:644b77b2111e 187 wait(0.02); // esperamos 0.01s
javierjsp 0:b7978deb977b 188 if(enc==enc2.getPulses()) // Si el contador no ha cambiado
javierjsp 4:9ea8a9271732 189 {
javierjsp 4:9ea8a9271732 190 if (dir==-1)
javierjsp 0:b7978deb977b 191 dir=1; // cambiamos la dirección
javierjsp 4:9ea8a9271732 192 else if (dir==1)
javierjsp 4:9ea8a9271732 193 dir=-1;
javierjsp 4:9ea8a9271732 194 }
javierjsp 0:b7978deb977b 195 }
javierjsp 0:b7978deb977b 196 }
javierjsp 0:b7978deb977b 197 frenar(2); // el motor ha llegado a la posicion adecuada y se frena
javierjsp 0:b7978deb977b 198
javierjsp 0:b7978deb977b 199 // posicionar antebrazo
javierjsp 6:644b77b2111e 200 dir=-1; // mover primero hacia delante
javierjsp 6:644b77b2111e 201 while (dir==-1) // mientras la direccion sea hacia delante:
javierjsp 0:b7978deb977b 202 {
javierjsp 0:b7978deb977b 203 while (mp3==0) // Mientras se mantenga pulsado el boton
javierjsp 4:9ea8a9271732 204 mover(3,dir,0.7); // se seguira moviendo el motor en la direccion inicial
javierjsp 6:644b77b2111e 205 dir=1; // se cambia a direccion
javierjsp 0:b7978deb977b 206 while (mp3==1) // Mientras el MP esté sin pulsar:
javierjsp 0:b7978deb977b 207 {
javierjsp 0:b7978deb977b 208 enc=enc3.getPulses(); // leemos el valor de los encoders
javierjsp 4:9ea8a9271732 209 mover(3,dir,0.7); // movemos el motor
javierjsp 6:644b77b2111e 210 wait(0.02); // esperamos 0.01s
javierjsp 0:b7978deb977b 211 if(enc==enc3.getPulses()) // Si el contador no ha cambiado
javierjsp 4:9ea8a9271732 212 {
javierjsp 4:9ea8a9271732 213 if (dir==-1)
javierjsp 0:b7978deb977b 214 dir=1; // cambiamos la dirección
javierjsp 4:9ea8a9271732 215 else if (dir==1)
javierjsp 4:9ea8a9271732 216 dir=-1;
javierjsp 4:9ea8a9271732 217 }
javierjsp 0:b7978deb977b 218 }
javierjsp 0:b7978deb977b 219 }
javierjsp 0:b7978deb977b 220 frenar(3); // el motor ha llegado a la posicion adecuada y se frena
javierjsp 0:b7978deb977b 221
javierjsp 0:b7978deb977b 222 // posicionamiento del giro de la muñeca
javierjsp 6:644b77b2111e 223 dir=-1; // mover primero hacia delante
javierjsp 6:644b77b2111e 224 while (dir==-1) // Mientras que la dirección del motor sea 1:
javierjsp 0:b7978deb977b 225 {
javierjsp 0:b7978deb977b 226 while (mp4==0) // Mientras el MP esté pulsado
javierjsp 4:9ea8a9271732 227 mover(4,dir,0.7); // movemos los motores 4 y 5
javierjsp 6:644b77b2111e 228 dir=1;
javierjsp 0:b7978deb977b 229 while (mp4==1) // Mientras el MP esté sin pulsar:
javierjsp 0:b7978deb977b 230 {
javierjsp 0:b7978deb977b 231 enc=enc5.getPulses(); // leemos el valor del los encoders
javierjsp 4:9ea8a9271732 232 mover(4,dir,0.7); // movemos los motores
javierjsp 6:644b77b2111e 233 wait(0.02); // esperamos 0.01s
javierjsp 0:b7978deb977b 234 if(enc==enc5.getPulses()) // Si el contador no ha cambiado
javierjsp 4:9ea8a9271732 235 {
javierjsp 4:9ea8a9271732 236 if (dir==-1)
javierjsp 0:b7978deb977b 237 dir=1; // cambiamos la dirección
javierjsp 4:9ea8a9271732 238 else if (dir==1)
javierjsp 4:9ea8a9271732 239 dir=-1;
javierjsp 4:9ea8a9271732 240 }
javierjsp 0:b7978deb977b 241 }
javierjsp 0:b7978deb977b 242 }
javierjsp 0:b7978deb977b 243 frenar(4); // la muñeca ha llegado a la posicion adecuada y se frenan los motores
javierjsp 0:b7978deb977b 244
javierjsp 0:b7978deb977b 245 // posicionamiento de la rotacion de la muñeca
javierjsp 4:9ea8a9271732 246 while (mp5==1) // Mientras no se pulse el MS
javierjsp 0:b7978deb977b 247 {
javierjsp 4:9ea8a9271732 248 mover(5,1,0.7); // se mueven los motores 4 y 5
javierjsp 6:644b77b2111e 249 wait(0.02); // esperamos 0.01s
javierjsp 0:b7978deb977b 250 }
javierjsp 0:b7978deb977b 251 frenar(5); // frenamos los motores
javierjsp 0:b7978deb977b 252 pinza(-1); // abrimos la pinza
javierjsp 0:b7978deb977b 253 frenar(0); // frenamos todos los motoes
javierjsp 0:b7978deb977b 254 enc1.reset(); // reseteamos los encoders
javierjsp 0:b7978deb977b 255 enc2.reset();
javierjsp 0:b7978deb977b 256 enc3.reset();
javierjsp 0:b7978deb977b 257 enc4.reset();
javierjsp 0:b7978deb977b 258 enc5.reset();
javierjsp 0:b7978deb977b 259 enc6.reset();
javierjsp 0:b7978deb977b 260 led2=0;
javierjsp 0:b7978deb977b 261 }
javierjsp 0:b7978deb977b 262
javierjsp 1:7c07ddc42a4c 263 void posicion(int pos)
javierjsp 0:b7978deb977b 264 {
javierjsp 0:b7978deb977b 265
javierjsp 0:b7978deb977b 266 }
javierjsp 0:b7978deb977b 267
javierjsp 0:b7978deb977b 268 void mover(int m, int dir, float v)
javierjsp 0:b7978deb977b 269 {
javierjsp 0:b7978deb977b 270 if((dir==1 || dir==(-1))&&(v>0 && v<=1)) //comprueva que los parametros pasados estan dentro de los valores deseados
javierjsp 0:b7978deb977b 271 {
javierjsp 0:b7978deb977b 272 switch (m)
javierjsp 0:b7978deb977b 273 {
javierjsp 0:b7978deb977b 274 case 0: // en caso de que el valor sea 0 movemos todos los motores
javierjsp 0:b7978deb977b 275 m1.speed(dir*v);
javierjsp 0:b7978deb977b 276 m2.speed(dir*v);
javierjsp 0:b7978deb977b 277 m3.speed(dir*v);
javierjsp 0:b7978deb977b 278 m4.speed(dir*v);
javierjsp 0:b7978deb977b 279 m5.speed(dir*v);
javierjsp 5:053c61912254 280 break;
javierjsp 0:b7978deb977b 281 case 1: // en caso de que el valor sea 1 movemos el motor 1 (base)
javierjsp 0:b7978deb977b 282 m1.speed(dir*v);
javierjsp 0:b7978deb977b 283 break;
javierjsp 0:b7978deb977b 284 case 2: // en caso de que el valor sea 2 movemos el motor 2 (hombro)
javierjsp 0:b7978deb977b 285 m2.speed(dir*v);
javierjsp 0:b7978deb977b 286 break;
javierjsp 0:b7978deb977b 287 case 3: // en caso de que el valor sea 3 movemos el motor 3 (codo)
javierjsp 0:b7978deb977b 288 m3.speed(dir*v);
javierjsp 0:b7978deb977b 289 break;
javierjsp 0:b7978deb977b 290 case 4: // en caso de que el valor sea 4 movemos el motor 4 en un sentido y el 5 en el otro (girar muñeca)
javierjsp 0:b7978deb977b 291 m4.speed((-1)*dir*v);
javierjsp 0:b7978deb977b 292 m5.speed(dir*v);
javierjsp 5:053c61912254 293 break;
javierjsp 0:b7978deb977b 294 case 5: // en caso de que el valor sea 5 movemos los motores 4 y 5 en el mismo sentido (rotar muñeca)
javierjsp 0:b7978deb977b 295 m4.speed(dir*v);
javierjsp 0:b7978deb977b 296 m5.speed(dir*v);
javierjsp 5:053c61912254 297 break;
javierjsp 0:b7978deb977b 298 case 6: // en caso de que el valor sea 6 movemos el motor 6 (pinza)
javierjsp 0:b7978deb977b 299 m6.speed(dir*v);
javierjsp 0:b7978deb977b 300 break;
javierjsp 0:b7978deb977b 301 }
javierjsp 0:b7978deb977b 302 }
javierjsp 0:b7978deb977b 303 else // si los parametros no son correctos encendemos el LED3
javierjsp 0:b7978deb977b 304 led3=1;
javierjsp 0:b7978deb977b 305 }
javierjsp 0:b7978deb977b 306
javierjsp 0:b7978deb977b 307 void pinza(int a)
javierjsp 0:b7978deb977b 308 {
javierjsp 0:b7978deb977b 309 int enc=0, encOld=0;
javierjsp 5:053c61912254 310 mover(6,a,1); // movemos la pinza
javierjsp 3:fa1d943cf383 311 encOld=enc6.getPulses(); // leemos la posicion del encoder
javierjsp 3:fa1d943cf383 312 wait(0.1); // espera 0.1s
javierjsp 3:fa1d943cf383 313 enc=enc6.getPulses(); // leemos la posicion nueva del encoder
javierjsp 3:fa1d943cf383 314 while(enc != encOld) // mientras la posicion antigua sea distinta de la nueva seguimos leyendo
javierjsp 0:b7978deb977b 315 {
javierjsp 0:b7978deb977b 316 wait(0.1); // espera 0.1s
javierjsp 0:b7978deb977b 317 encOld=enc; // guardamos la posicion antigua del encoder
javierjsp 0:b7978deb977b 318 enc=enc6.getPulses(); // leemos la posicion nueva del encoder
javierjsp 3:fa1d943cf383 319 }
javierjsp 0:b7978deb977b 320 frenar(6); // cuando la pinza ha llegado al tope frenamos la pinza
javierjsp 0:b7978deb977b 321 }
javierjsp 0:b7978deb977b 322
javierjsp 0:b7978deb977b 323 void frenar(int m)
javierjsp 0:b7978deb977b 324 {
javierjsp 0:b7978deb977b 325 switch(m)
javierjsp 0:b7978deb977b 326 {
javierjsp 0:b7978deb977b 327 case 0: // en caso de que el valor sea 0 frenamos todos los motores
javierjsp 0:b7978deb977b 328 m1.brake();
javierjsp 0:b7978deb977b 329 m2.brake();
javierjsp 0:b7978deb977b 330 m3.brake();
javierjsp 0:b7978deb977b 331 m4.brake();
javierjsp 0:b7978deb977b 332 m5.brake();
javierjsp 0:b7978deb977b 333 m6.brake();
javierjsp 0:b7978deb977b 334 break;
javierjsp 0:b7978deb977b 335 case 1: // en caso de que el valor sea 1 frenamos el motor 1
javierjsp 0:b7978deb977b 336 m1.brake();
javierjsp 0:b7978deb977b 337 break;
javierjsp 0:b7978deb977b 338 case 2: // en caso de que el valor sea 2 frenamos el motor 2
javierjsp 0:b7978deb977b 339 m2.brake();
javierjsp 0:b7978deb977b 340 break;
javierjsp 0:b7978deb977b 341 case 3: // en caso de que el valor sea 3 frenamos el motor 3
javierjsp 0:b7978deb977b 342 m3.brake();
javierjsp 0:b7978deb977b 343 break;
javierjsp 0:b7978deb977b 344 case 4: // en caso de que el valor sea 4 frenamos los motores 4 y 5
javierjsp 0:b7978deb977b 345 m4.brake();
javierjsp 0:b7978deb977b 346 m5.brake();
javierjsp 0:b7978deb977b 347 break;
javierjsp 0:b7978deb977b 348 case 5: // en caso de que el valor sea 5 frenamos los motores 4 y 5
javierjsp 0:b7978deb977b 349 m4.brake();
javierjsp 0:b7978deb977b 350 m5.brake();
javierjsp 0:b7978deb977b 351 break;
javierjsp 0:b7978deb977b 352 case 6: // en caso de que el valor sea 6 frenamos el motor 6
javierjsp 0:b7978deb977b 353 m6.brake();
javierjsp 0:b7978deb977b 354 break;
javierjsp 0:b7978deb977b 355 }
javierjsp 6:644b77b2111e 356 }
javierjsp 6:644b77b2111e 357 void movCinta()
javierjsp 6:644b77b2111e 358 {
javierjsp 6:644b77b2111e 359 while (sensor==1)
javierjsp 6:644b77b2111e 360 cinta.speed(1);
javierjsp 6:644b77b2111e 361 cinta.brake();
javierjsp 0:b7978deb977b 362 }
javierjsp 0:b7978deb977b 363
javierjsp 0:b7978deb977b 364
javierjsp 0:b7978deb977b 365
javierjsp 0:b7978deb977b 366
javierjsp 6:644b77b2111e 367