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@13:d09934a46cf8, 2017-06-12 (annotated)

Committer:
javierjsp
Date:
Mon Jun 12 12:35:32 2017 +0000
Revision:
13:d09934a46cf8
Parent:
12:5fb00cff7a0e 
puesto m2 completo

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 11:7674323a52d6 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 11:7674323a52d6 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 9:2c4bf2c1ccdc 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 7:cb59c58cab78 39 int posiciones[10][6]={0}; // declaramos los arrays de posiciones
javierjsp 0:b7978deb977b 40 char dato=0;
javierjsp 8:79701df1b502 41 float V=1;
javierjsp 0:b7978deb977b 42
javierjsp 0:b7978deb977b 43 // declaramos el prototipo de las funciones que utilizaremos
javierjsp 1:7c07ddc42a4c 44 void enviar_bt(); // prototipo de la funcion que ejecutara el ticker1
javierjsp 6:644b77b2111e 45 void encenderLEDs(); // prototipo de la funcion que ejecutara el ticker1
javierjsp 0:b7978deb977b 46 void sethome(); // prototipo de la funcion que nos calibrara la posicion hard home de referencia
javierjsp 8:79701df1b502 47 void posicion(int); // prototipo de la funcion que nos llevara hasta la posicion especificada
javierjsp 0:b7978deb977b 48 void mover(int,int,float); // prototipo de la funcion que movera cada motor
javierjsp 0:b7978deb977b 49 void frenar(int); // prototipo de la funcion que frenara cada motor
javierjsp 0:b7978deb977b 50 void pinza(int); // prototipo de la funcion que abrira (-1) o cerrara (1) la pinza
javierjsp 6:644b77b2111e 51 void movCinta(); // prototipo de la funcion que avanzara la cinta hasta detectar un objeto
javierjsp 8:79701df1b502 52 float map(long,long,long,float,long); // prototipo de la funcion que hace una redimension de los parametros
javierjsp 0:b7978deb977b 53
javierjsp 0:b7978deb977b 54 int main()
javierjsp 0:b7978deb977b 55 {
javierjsp 8:79701df1b502 56 m1.period(0.001); // establece el periodo del PWM, equivalente a una frecuencia de 1KHz.
javierjsp 8:79701df1b502 57 m2.period(0.001);
javierjsp 8:79701df1b502 58 m3.period(0.001);
javierjsp 8:79701df1b502 59 m4.period(0.001);
javierjsp 8:79701df1b502 60 m5.period(0.001);
javierjsp 8:79701df1b502 61 m6.period(0.001);
javierjsp 0:b7978deb977b 62
javierjsp 6:644b77b2111e 63 bt.baud(9600); // velocidad de transmision del bluetooth a 9600 baudios
javierjsp 0:b7978deb977b 64
javierjsp 8:79701df1b502 65 ticker1.attach(&enviar_bt, 0.2); // asignamos la funcion "enviar_bt" al protocolo de interrupcion del "ticker1" y le damos un intervalo de 0.2s
javierjsp 8:79701df1b502 66 ticker2.attach(&encenderLEDs, 0.1); // asignamos la funcion "encenderLEDs" al protocolo de interrupcion del "ticker1" y le damos un intervalo de 0.2s
javierjsp 8:79701df1b502 67
javierjsp 10:3366c05c1827 68 wait(1);
javierjsp 4:9ea8a9271732 69
javierjsp 6:644b77b2111e 70 sethome(); // establecemos la posicion de referencia
javierjsp 0:b7978deb977b 71
javierjsp 0:b7978deb977b 72 while(1)
javierjsp 0:b7978deb977b 73 {
javierjsp 8:79701df1b502 74 if(bt.readable()>0) // si hay un dato disponible:
javierjsp 0:b7978deb977b 75 {
javierjsp 8:79701df1b502 76 dato = bt.getc(); // leemos un caracter
javierjsp 8:79701df1b502 77 if (dato<=10 && dato>=1) // si tiene un valor entre 1 y 10 lo asignamos como velocidad
javierjsp 8:79701df1b502 78 V=map(dato,1,10,0.6,1);
javierjsp 0:b7978deb977b 79 switch(dato)
javierjsp 0:b7978deb977b 80 {
javierjsp 8:79701df1b502 81 case 'A': // si el caracter es A se mueve el motor 1 al derecho
javierjsp 8:79701df1b502 82 mover(1,1,V);
javierjsp 0:b7978deb977b 83 wait(0.2);
javierjsp 0:b7978deb977b 84 frenar(1);
javierjsp 0:b7978deb977b 85 break;
javierjsp 8:79701df1b502 86 case 'B': // si el caracter es B se mueve el motor 1 al inverso
javierjsp 8:79701df1b502 87 mover(1,-1,V);
javierjsp 0:b7978deb977b 88 wait(0.2);
javierjsp 0:b7978deb977b 89 frenar(1);
javierjsp 0:b7978deb977b 90 break;
javierjsp 8:79701df1b502 91 case 'C': // si el caracter es C se mueve el motor 2 al derecho
javierjsp 8:79701df1b502 92 mover(2,1,V);
javierjsp 0:b7978deb977b 93 wait(0.2);
javierjsp 0:b7978deb977b 94 frenar(2);
javierjsp 0:b7978deb977b 95 break;
javierjsp 8:79701df1b502 96 case 'D': // si el caracter es D se mueve el motor 2 al inverso
javierjsp 8:79701df1b502 97 mover(2,-1,V);
javierjsp 0:b7978deb977b 98 wait(0.2);
javierjsp 0:b7978deb977b 99 frenar(2);
javierjsp 0:b7978deb977b 100 break;
javierjsp 8:79701df1b502 101 case 'E': // si el caracter es E se mueve el motor 3 al derecho
javierjsp 8:79701df1b502 102 mover(3,1,V);
javierjsp 0:b7978deb977b 103 wait(0.2);
javierjsp 0:b7978deb977b 104 frenar(3);
javierjsp 0:b7978deb977b 105 break;
javierjsp 8:79701df1b502 106 case 'F': // si el caracter es F se mueve el motor 3 al inverso
javierjsp 8:79701df1b502 107 mover(3,-1,V);
javierjsp 0:b7978deb977b 108 wait(0.2);
javierjsp 0:b7978deb977b 109 frenar(3);
javierjsp 0:b7978deb977b 110 break;
javierjsp 8:79701df1b502 111 case 'G': // si el caracter es G se mueve el motor 4 Y 5 al derecho
javierjsp 8:79701df1b502 112 mover(14,1,V);
javierjsp 0:b7978deb977b 113 wait(0.2);
javierjsp 8:79701df1b502 114 frenar(14);
javierjsp 0:b7978deb977b 115 break;
javierjsp 8:79701df1b502 116 case 'H': // si el caracter es H se mueve el motor 4 Y 5 al inverso
javierjsp 8:79701df1b502 117 mover(14,-1,V);
javierjsp 0:b7978deb977b 118 wait(0.2);
javierjsp 8:79701df1b502 119 frenar(14);
javierjsp 0:b7978deb977b 120 break;
javierjsp 8:79701df1b502 121 case 'I': // si el caracter es I se mueve el motor 4 Y 5 al derecho
javierjsp 8:79701df1b502 122 mover(15,1,V);
javierjsp 0:b7978deb977b 123 wait(0.2);
javierjsp 8:79701df1b502 124 frenar(15);
javierjsp 0:b7978deb977b 125 break;
javierjsp 8:79701df1b502 126 case 'J': // si el caracter es J se mueve el motor 4 Y 5 al inverso
javierjsp 8:79701df1b502 127 mover(15,-1,V);
javierjsp 0:b7978deb977b 128 wait(0.2);
javierjsp 8:79701df1b502 129 frenar(15);
javierjsp 0:b7978deb977b 130 break;
javierjsp 8:79701df1b502 131 case 'K': // si el caracter es K se abre la pinza
javierjsp 0:b7978deb977b 132 pinza(-1);
javierjsp 0:b7978deb977b 133 break;
javierjsp 8:79701df1b502 134 case 'L': // si el caracter es L se cierra la pinza
javierjsp 0:b7978deb977b 135 pinza(1);
javierjsp 0:b7978deb977b 136 break;
javierjsp 8:79701df1b502 137 case 'M': // si el caracter es M se avanza la cinta
javierjsp 6:644b77b2111e 138 break;
javierjsp 8:79701df1b502 139 case 'N': // si el caracter es N se mueve a la posicion home
javierjsp 8:79701df1b502 140 posicion(0);
javierjsp 8:79701df1b502 141 pinza(-1);
javierjsp 8:79701df1b502 142 break;
javierjsp 0:b7978deb977b 143 }
javierjsp 0:b7978deb977b 144 }
javierjsp 0:b7978deb977b 145 }
javierjsp 0:b7978deb977b 146 }
javierjsp 0:b7978deb977b 147
javierjsp 8:79701df1b502 148 float map(long x, long in_min, long in_max, float out_min, long out_max) // funcion que redimensiona los parametros
javierjsp 8:79701df1b502 149 {
javierjsp 8:79701df1b502 150 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
javierjsp 8:79701df1b502 151 }
javierjsp 8:79701df1b502 152
javierjsp 0:b7978deb977b 153 void enviar_bt() // definimos la funcion que ejecutara el ticker
javierjsp 0:b7978deb977b 154 {
javierjsp 8:79701df1b502 155 pulsos[1]=enc1.getPulses(); // cargamos los contadores de los encoders
javierjsp 4:9ea8a9271732 156 pulsos[2]=enc2.getPulses();
javierjsp 4:9ea8a9271732 157 pulsos[3]=enc3.getPulses();
javierjsp 4:9ea8a9271732 158 pulsos[4]=enc4.getPulses();
javierjsp 4:9ea8a9271732 159 pulsos[5]=enc5.getPulses();
javierjsp 8:79701df1b502 160 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 los valores del array "pulsos" por bluetooth
javierjsp 0:b7978deb977b 161 }
javierjsp 0:b7978deb977b 162
javierjsp 6:644b77b2111e 163 void encenderLEDs()
javierjsp 1:7c07ddc42a4c 164 {
javierjsp 8:79701df1b502 165 if(mp1==0)
javierjsp 8:79701df1b502 166 led1=1;
javierjsp 8:79701df1b502 167 else if (mp1==1)
javierjsp 10:3366c05c1827 168 led1==0;
javierjsp 8:79701df1b502 169 if(mp2==0)
javierjsp 8:79701df1b502 170 led3=1;
javierjsp 8:79701df1b502 171 else if (mp2==1)
javierjsp 10:3366c05c1827 172 led3==0;
javierjsp 4:9ea8a9271732 173 }
javierjsp 4:9ea8a9271732 174
javierjsp 0:b7978deb977b 175 void sethome()
javierjsp 0:b7978deb977b 176 {
javierjsp 0:b7978deb977b 177 led2=1;
javierjsp 0:b7978deb977b 178 // posicionar base
javierjsp 0:b7978deb977b 179 int enc=0;
javierjsp 6:644b77b2111e 180 int dir=-1; // mover primero hacia delante
javierjsp 0:b7978deb977b 181 while (mp1==1) // Mientras no se pulse el microswitch
javierjsp 0:b7978deb977b 182 {
javierjsp 0:b7978deb977b 183 enc=enc1.getPulses(); // leemos el contador del encoder
javierjsp 13:d09934a46cf8 184 mover(1,dir,1); // movemos el motor en la dirección inicial
javierjsp 8:79701df1b502 185 wait(0.01); // esperamos 0.01s
javierjsp 0:b7978deb977b 186 if (enc==enc1.getPulses()) // Si el contador no se ha incrementado, hemos llegado al extremo
javierjsp 6:644b77b2111e 187 dir=1; // se cambia la dirección del motor
javierjsp 0:b7978deb977b 188 }
javierjsp 0:b7978deb977b 189 frenar(1); // el motor ha llegado a la posicion adecuada y se frena
javierjsp 0:b7978deb977b 190
javierjsp 0:b7978deb977b 191 // posicionar brazo
javierjsp 6:644b77b2111e 192 dir=-1; // mover primero hacia delante
javierjsp 13:d09934a46cf8 193 while (dir==-1) // mientras la direccion sea hacia delante:
javierjsp 13:d09934a46cf8 194 {
javierjsp 13:d09934a46cf8 195 while (mp2==0) // Mientras se mantenga pulsado el boton
javierjsp 13:d09934a46cf8 196 mover(2,dir,1); // se seguira moviendo el motor en la direccion inicial
javierjsp 13:d09934a46cf8 197 dir=1; // se cambia a direccion
javierjsp 13:d09934a46cf8 198 while (mp2==1) // Mientras el MP esté sin pulsar:
javierjsp 13:d09934a46cf8 199 {
javierjsp 13:d09934a46cf8 200 enc=enc2.getPulses(); // leemos el valor de los encoders
javierjsp 13:d09934a46cf8 201 mover(2,dir,1); // movemos el motor
javierjsp 13:d09934a46cf8 202 wait(0.05); // esperamos 0.01s
javierjsp 13:d09934a46cf8 203 if(enc==enc2.getPulses()) // Si el contador no ha cambiado
javierjsp 13:d09934a46cf8 204 {
javierjsp 13:d09934a46cf8 205 if (dir==-1)
javierjsp 13:d09934a46cf8 206 dir=1; // cambiamos la dirección
javierjsp 13:d09934a46cf8 207 else if (dir==1)
javierjsp 13:d09934a46cf8 208 dir=-1;
javierjsp 13:d09934a46cf8 209 }
javierjsp 13:d09934a46cf8 210 }
javierjsp 13:d09934a46cf8 211 }
javierjsp 0:b7978deb977b 212 frenar(2); // el motor ha llegado a la posicion adecuada y se frena
javierjsp 0:b7978deb977b 213
javierjsp 0:b7978deb977b 214 // posicionar antebrazo
javierjsp 13:d09934a46cf8 215 dir=-1; // mover primero hacia delante
javierjsp 13:d09934a46cf8 216 while (dir==-1) // mientras la direccion sea hacia delante:
javierjsp 0:b7978deb977b 217 {
javierjsp 0:b7978deb977b 218 while (mp3==0) // Mientras se mantenga pulsado el boton
javierjsp 13:d09934a46cf8 219 mover(3,dir,1); // se seguira moviendo el motor en la direccion inicial
javierjsp 13:d09934a46cf8 220 dir=1; // se cambia a direccion
javierjsp 0:b7978deb977b 221 while (mp3==1) // Mientras el MP esté sin pulsar:
javierjsp 0:b7978deb977b 222 {
javierjsp 0:b7978deb977b 223 enc=enc3.getPulses(); // leemos el valor de los encoders
javierjsp 13:d09934a46cf8 224 mover(3,dir,1); // movemos el motor
javierjsp 13:d09934a46cf8 225 wait(0.05); // esperamos 0.01s
javierjsp 0:b7978deb977b 226 if(enc==enc3.getPulses()) // Si el contador no ha cambiado
javierjsp 4:9ea8a9271732 227 {
javierjsp 4:9ea8a9271732 228 if (dir==-1)
javierjsp 0:b7978deb977b 229 dir=1; // cambiamos la dirección
javierjsp 4:9ea8a9271732 230 else if (dir==1)
javierjsp 4:9ea8a9271732 231 dir=-1;
javierjsp 4:9ea8a9271732 232 }
javierjsp 0:b7978deb977b 233 }
javierjsp 0:b7978deb977b 234 }
javierjsp 0:b7978deb977b 235 frenar(3); // el motor ha llegado a la posicion adecuada y se frena
javierjsp 0:b7978deb977b 236
javierjsp 0:b7978deb977b 237 // posicionamiento del giro de la muñeca
javierjsp 6:644b77b2111e 238 dir=-1; // mover primero hacia delante
javierjsp 6:644b77b2111e 239 while (dir==-1) // Mientras que la dirección del motor sea 1:
javierjsp 0:b7978deb977b 240 {
javierjsp 0:b7978deb977b 241 while (mp4==0) // Mientras el MP esté pulsado
javierjsp 13:d09934a46cf8 242 mover(14,dir,1); // movemos los motores 4 y 5
javierjsp 6:644b77b2111e 243 dir=1;
javierjsp 0:b7978deb977b 244 while (mp4==1) // Mientras el MP esté sin pulsar:
javierjsp 0:b7978deb977b 245 {
javierjsp 0:b7978deb977b 246 enc=enc5.getPulses(); // leemos el valor del los encoders
javierjsp 13:d09934a46cf8 247 mover(14,dir,1); // movemos los motores
javierjsp 13:d09934a46cf8 248 wait(0.01); // esperamos 0.01s
javierjsp 0:b7978deb977b 249 if(enc==enc5.getPulses()) // Si el contador no ha cambiado
javierjsp 4:9ea8a9271732 250 {
javierjsp 4:9ea8a9271732 251 if (dir==-1)
javierjsp 0:b7978deb977b 252 dir=1; // cambiamos la dirección
javierjsp 4:9ea8a9271732 253 else if (dir==1)
javierjsp 4:9ea8a9271732 254 dir=-1;
javierjsp 4:9ea8a9271732 255 }
javierjsp 0:b7978deb977b 256 }
javierjsp 0:b7978deb977b 257 }
javierjsp 8:79701df1b502 258 frenar(14); // la muñeca ha llegado a la posicion adecuada y se frenan los motores
javierjsp 0:b7978deb977b 259
javierjsp 0:b7978deb977b 260 // posicionamiento de la rotacion de la muñeca
javierjsp 4:9ea8a9271732 261 while (mp5==1) // Mientras no se pulse el MS
javierjsp 0:b7978deb977b 262 {
javierjsp 13:d09934a46cf8 263 mover(15,1,1); // se mueven los motores 4 y 5
javierjsp 13:d09934a46cf8 264 wait(0.01); // esperamos 0.01s
javierjsp 0:b7978deb977b 265 }
javierjsp 8:79701df1b502 266 frenar(15); // frenamos los motores
javierjsp 8:79701df1b502 267
javierjsp 0:b7978deb977b 268 pinza(-1); // abrimos la pinza
javierjsp 8:79701df1b502 269
javierjsp 8:79701df1b502 270 // ponemos el contador de los encoders a 0
javierjsp 0:b7978deb977b 271 frenar(0); // frenamos todos los motoes
javierjsp 0:b7978deb977b 272 enc1.reset(); // reseteamos los encoders
javierjsp 0:b7978deb977b 273 enc2.reset();
javierjsp 0:b7978deb977b 274 enc3.reset();
javierjsp 0:b7978deb977b 275 enc4.reset();
javierjsp 0:b7978deb977b 276 enc5.reset();
javierjsp 0:b7978deb977b 277 enc6.reset();
javierjsp 0:b7978deb977b 278 led2=0;
javierjsp 0:b7978deb977b 279 }
javierjsp 0:b7978deb977b 280
javierjsp 1:7c07ddc42a4c 281 void posicion(int pos)
javierjsp 0:b7978deb977b 282 {
javierjsp 8:79701df1b502 283 frenar(0); // nos aseguramos de que los motores estan parados
javierjsp 8:79701df1b502 284 pulsos[1]=enc1.getPulses(); // leemos los valores de los conadores de los encoders
javierjsp 8:79701df1b502 285 pulsos[2]=enc2.getPulses();
javierjsp 8:79701df1b502 286 pulsos[3]=enc3.getPulses();
javierjsp 8:79701df1b502 287 pulsos[4]=enc4.getPulses();
javierjsp 8:79701df1b502 288 pulsos[5]=enc5.getPulses();
javierjsp 8:79701df1b502 289 int dir[6]={0,0,0,0,0,0}; // declaramos un array para las direcciones
javierjsp 8:79701df1b502 290 int flag[6]={1,1,1,1,1,1}; // declaramos un array para banderas
javierjsp 8:79701df1b502 291 int i;
javierjsp 8:79701df1b502 292 for (i=1;i<6;i++) // establecemos la direccion a la que tiene que ir cada motor
javierjsp 8:79701df1b502 293 {
javierjsp 8:79701df1b502 294 if (posiciones[pos][i]<pulsos[i])
javierjsp 8:79701df1b502 295 dir[i]=-1;
javierjsp 8:79701df1b502 296 else if (posiciones[pos][i]>pulsos[i])
javierjsp 8:79701df1b502 297 dir[i]=1;
javierjsp 8:79701df1b502 298 else if (posiciones[pos][i]==pulsos[i])
javierjsp 8:79701df1b502 299 flag[i]=0;
javierjsp 8:79701df1b502 300 }
javierjsp 8:79701df1b502 301 while (flag[1]+flag[2]+flag[3]+flag[4]+flag[5]) // mientras que alguna de las banderas este activada:
javierjsp 8:79701df1b502 302 {
javierjsp 8:79701df1b502 303 for (i=1;i<6;i++) // repetimos para cada uno de los motores
javierjsp 8:79701df1b502 304 {
javierjsp 8:79701df1b502 305 if (flag[i]==1) // si la bandera del motor i esta activa
javierjsp 8:79701df1b502 306 {
javierjsp 8:79701df1b502 307 mover(i,dir[i],V); // movemos el motor en la direccion establecida
javierjsp 8:79701df1b502 308 if (dir[i]==1 && pulsos[i]>=posiciones[pos][i]) // si ha llegado a la posicion deseada
javierjsp 8:79701df1b502 309 {
javierjsp 8:79701df1b502 310 frenar(i); // frenamos el motor i
javierjsp 8:79701df1b502 311 flag[i]=0; // desactivamos su bandera
javierjsp 8:79701df1b502 312 }
javierjsp 8:79701df1b502 313 if (dir[i]==-1 && pulsos[i]<=posiciones[pos][i]) // si ha llegado a la posicion deseada
javierjsp 8:79701df1b502 314 {
javierjsp 8:79701df1b502 315 frenar(i); // frenamos el motor i
javierjsp 8:79701df1b502 316 flag[i]=0; // desactivamos su bandera
javierjsp 8:79701df1b502 317 }
javierjsp 8:79701df1b502 318 }
javierjsp 8:79701df1b502 319 }
javierjsp 8:79701df1b502 320 wait(0.001); // esperamos 0.001s
javierjsp 8:79701df1b502 321 pulsos[1]=enc1.getPulses(); // leemos la nueva posicion de los encoders
javierjsp 8:79701df1b502 322 pulsos[2]=enc2.getPulses();
javierjsp 8:79701df1b502 323 pulsos[3]=enc3.getPulses();
javierjsp 8:79701df1b502 324 pulsos[4]=enc4.getPulses();
javierjsp 8:79701df1b502 325 pulsos[5]=enc5.getPulses();
javierjsp 8:79701df1b502 326 }
javierjsp 8:79701df1b502 327 wait(0.1); // esperamos 0.1s
javierjsp 0:b7978deb977b 328 }
javierjsp 0:b7978deb977b 329
javierjsp 0:b7978deb977b 330 void mover(int m, int dir, float v)
javierjsp 0:b7978deb977b 331 {
javierjsp 0:b7978deb977b 332 if((dir==1 || dir==(-1))&&(v>0 && v<=1)) //comprueva que los parametros pasados estan dentro de los valores deseados
javierjsp 0:b7978deb977b 333 {
javierjsp 0:b7978deb977b 334 switch (m)
javierjsp 0:b7978deb977b 335 {
javierjsp 0:b7978deb977b 336 case 0: // en caso de que el valor sea 0 movemos todos los motores
javierjsp 0:b7978deb977b 337 m1.speed(dir*v);
javierjsp 0:b7978deb977b 338 m2.speed(dir*v);
javierjsp 0:b7978deb977b 339 m3.speed(dir*v);
javierjsp 0:b7978deb977b 340 m4.speed(dir*v);
javierjsp 0:b7978deb977b 341 m5.speed(dir*v);
javierjsp 5:053c61912254 342 break;
javierjsp 0:b7978deb977b 343 case 1: // en caso de que el valor sea 1 movemos el motor 1 (base)
javierjsp 0:b7978deb977b 344 m1.speed(dir*v);
javierjsp 0:b7978deb977b 345 break;
javierjsp 0:b7978deb977b 346 case 2: // en caso de que el valor sea 2 movemos el motor 2 (hombro)
javierjsp 0:b7978deb977b 347 m2.speed(dir*v);
javierjsp 0:b7978deb977b 348 break;
javierjsp 0:b7978deb977b 349 case 3: // en caso de que el valor sea 3 movemos el motor 3 (codo)
javierjsp 0:b7978deb977b 350 m3.speed(dir*v);
javierjsp 0:b7978deb977b 351 break;
javierjsp 8:79701df1b502 352 case 4: // en caso de que el valor sea 4 movemos el motor 4
javierjsp 8:79701df1b502 353 m4.speed(dir*v);
javierjsp 5:053c61912254 354 break;
javierjsp 8:79701df1b502 355 case 5: // en caso de que el valor sea 5 movemos el motor 5
javierjsp 0:b7978deb977b 356 m5.speed(dir*v);
javierjsp 5:053c61912254 357 break;
javierjsp 0:b7978deb977b 358 case 6: // en caso de que el valor sea 6 movemos el motor 6 (pinza)
javierjsp 0:b7978deb977b 359 m6.speed(dir*v);
javierjsp 0:b7978deb977b 360 break;
javierjsp 8:79701df1b502 361 case 14: // en caso de que el valor sea 14 movemos el motor 4 en un sentido y el 5 en el otro (girar muñeca)
javierjsp 8:79701df1b502 362 m4.speed((-1)*dir*v);
javierjsp 8:79701df1b502 363 m5.speed(dir*v);
javierjsp 8:79701df1b502 364 break;
javierjsp 8:79701df1b502 365 case 15: // en caso de que el valor sea 15 movemos los motores 4 y 5 en el mismo sentido (rotar muñeca)
javierjsp 8:79701df1b502 366 m4.speed(dir*v);
javierjsp 8:79701df1b502 367 m5.speed(dir*v);
javierjsp 0:b7978deb977b 368 }
javierjsp 0:b7978deb977b 369 }
javierjsp 0:b7978deb977b 370 else // si los parametros no son correctos encendemos el LED3
javierjsp 0:b7978deb977b 371 led3=1;
javierjsp 0:b7978deb977b 372 }
javierjsp 0:b7978deb977b 373
javierjsp 0:b7978deb977b 374 void pinza(int a)
javierjsp 0:b7978deb977b 375 {
javierjsp 0:b7978deb977b 376 int enc=0, encOld=0;
javierjsp 8:79701df1b502 377 mover(6,a,1); // movemos la pinza
javierjsp 3:fa1d943cf383 378 encOld=enc6.getPulses(); // leemos la posicion del encoder
javierjsp 3:fa1d943cf383 379 wait(0.1); // espera 0.1s
javierjsp 3:fa1d943cf383 380 enc=enc6.getPulses(); // leemos la posicion nueva del encoder
javierjsp 3:fa1d943cf383 381 while(enc != encOld) // mientras la posicion antigua sea distinta de la nueva seguimos leyendo
javierjsp 0:b7978deb977b 382 {
javierjsp 0:b7978deb977b 383 wait(0.1); // espera 0.1s
javierjsp 0:b7978deb977b 384 encOld=enc; // guardamos la posicion antigua del encoder
javierjsp 0:b7978deb977b 385 enc=enc6.getPulses(); // leemos la posicion nueva del encoder
javierjsp 3:fa1d943cf383 386 }
javierjsp 0:b7978deb977b 387 frenar(6); // cuando la pinza ha llegado al tope frenamos la pinza
javierjsp 0:b7978deb977b 388 }
javierjsp 0:b7978deb977b 389
javierjsp 0:b7978deb977b 390 void frenar(int m)
javierjsp 0:b7978deb977b 391 {
javierjsp 0:b7978deb977b 392 switch(m)
javierjsp 0:b7978deb977b 393 {
javierjsp 0:b7978deb977b 394 case 0: // en caso de que el valor sea 0 frenamos todos los motores
javierjsp 0:b7978deb977b 395 m1.brake();
javierjsp 0:b7978deb977b 396 m2.brake();
javierjsp 0:b7978deb977b 397 m3.brake();
javierjsp 0:b7978deb977b 398 m4.brake();
javierjsp 0:b7978deb977b 399 m5.brake();
javierjsp 0:b7978deb977b 400 m6.brake();
javierjsp 0:b7978deb977b 401 break;
javierjsp 0:b7978deb977b 402 case 1: // en caso de que el valor sea 1 frenamos el motor 1
javierjsp 0:b7978deb977b 403 m1.brake();
javierjsp 0:b7978deb977b 404 break;
javierjsp 0:b7978deb977b 405 case 2: // en caso de que el valor sea 2 frenamos el motor 2
javierjsp 0:b7978deb977b 406 m2.brake();
javierjsp 0:b7978deb977b 407 break;
javierjsp 0:b7978deb977b 408 case 3: // en caso de que el valor sea 3 frenamos el motor 3
javierjsp 0:b7978deb977b 409 m3.brake();
javierjsp 0:b7978deb977b 410 break;
javierjsp 8:79701df1b502 411 case 4: // en caso de que el valor sea 4 frenamos el motor 4
javierjsp 0:b7978deb977b 412 m4.brake();
javierjsp 0:b7978deb977b 413 break;
javierjsp 8:79701df1b502 414 case 5: // en caso de que el valor sea 5 frenamos el motor 5
javierjsp 0:b7978deb977b 415 m5.brake();
javierjsp 0:b7978deb977b 416 break;
javierjsp 0:b7978deb977b 417 case 6: // en caso de que el valor sea 6 frenamos el motor 6
javierjsp 0:b7978deb977b 418 m6.brake();
javierjsp 0:b7978deb977b 419 break;
javierjsp 8:79701df1b502 420 case 14: // en caso de que el valor sea 4 frenamos los motores 4 y 5
javierjsp 8:79701df1b502 421 m4.brake();
javierjsp 8:79701df1b502 422 m5.brake();
javierjsp 8:79701df1b502 423 break;
javierjsp 8:79701df1b502 424 case 15: // en caso de que el valor sea 5 frenamos los motores 4 y 5
javierjsp 8:79701df1b502 425 m4.brake();
javierjsp 8:79701df1b502 426 m5.brake();
javierjsp 0:b7978deb977b 427 }
javierjsp 6:644b77b2111e 428 }