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.
main.cpp@7:cb59c58cab78, 2017-06-09 (annotated)
- Committer:
- javierjsp
- Date:
- Fri Jun 09 01:14:42 2017 +0000
- Revision:
- 7:cb59c58cab78
- Parent:
- 6:644b77b2111e
- Child:
- 8:79701df1b502
a?adida la cinta
Who changed what in which revision?
User | Revision | Line number | New 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 | 7:cb59c58cab78 | 39 | int posiciones[10][6]={0}; // declaramos los arrays de posiciones |
javierjsp | 0:b7978deb977b | 40 | char dato=0; |
javierjsp | 0:b7978deb977b | 41 | |
javierjsp | 0:b7978deb977b | 42 | // declaramos el prototipo de las funciones que utilizaremos |
javierjsp | 1:7c07ddc42a4c | 43 | void enviar_bt(); // prototipo de la funcion que ejecutara el ticker1 |
javierjsp | 6:644b77b2111e | 44 | void encenderLEDs(); // prototipo de la funcion que ejecutara el ticker1 |
javierjsp | 0:b7978deb977b | 45 | void sethome(); // prototipo de la funcion que nos calibrara la posicion hard home de referencia |
javierjsp | 1:7c07ddc42a4c | 46 | void posicion(); // prototipo de la funcion que nos llevara hasta la posicion especificada |
javierjsp | 0:b7978deb977b | 47 | void mover(int,int,float); // prototipo de la funcion que movera cada motor |
javierjsp | 0:b7978deb977b | 48 | void frenar(int); // prototipo de la funcion que frenara cada motor |
javierjsp | 0:b7978deb977b | 49 | void pinza(int); // prototipo de la funcion que abrira (-1) o cerrara (1) la pinza |
javierjsp | 6:644b77b2111e | 50 | void movCinta(); // prototipo de la funcion que avanzara la cinta hasta detectar un objeto |
javierjsp | 0:b7978deb977b | 51 | |
javierjsp | 0:b7978deb977b | 52 | int main() |
javierjsp | 0:b7978deb977b | 53 | { |
javierjsp | 6:644b77b2111e | 54 | m1.period(0.0001); // establece el periodo del PWM, equivalente a una frecuencia de 10KHz. |
javierjsp | 6:644b77b2111e | 55 | m2.period(0.0001); |
javierjsp | 6:644b77b2111e | 56 | m3.period(0.0001); |
javierjsp | 6:644b77b2111e | 57 | m4.period(0.0001); |
javierjsp | 6:644b77b2111e | 58 | m5.period(0.0001); |
javierjsp | 6:644b77b2111e | 59 | m6.period(0.0001); |
javierjsp | 0:b7978deb977b | 60 | |
javierjsp | 6:644b77b2111e | 61 | bt.baud(9600); // velocidad de transmision del bluetooth a 9600 baudios |
javierjsp | 0:b7978deb977b | 62 | |
javierjsp | 1:7c07ddc42a4c | 63 | 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 | 64 | 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 | 65 | |
javierjsp | 6:644b77b2111e | 66 | sethome(); // establecemos la posicion de referencia |
javierjsp | 0:b7978deb977b | 67 | |
javierjsp | 0:b7978deb977b | 68 | while(1) |
javierjsp | 0:b7978deb977b | 69 | { |
javierjsp | 0:b7978deb977b | 70 | if(bt.readable()>0) |
javierjsp | 0:b7978deb977b | 71 | { |
javierjsp | 0:b7978deb977b | 72 | dato = bt.getc(); |
javierjsp | 0:b7978deb977b | 73 | switch(dato) |
javierjsp | 0:b7978deb977b | 74 | { |
javierjsp | 0:b7978deb977b | 75 | case 'A': |
javierjsp | 0:b7978deb977b | 76 | mover(1,1,1); |
javierjsp | 0:b7978deb977b | 77 | wait(0.2); |
javierjsp | 0:b7978deb977b | 78 | frenar(1); |
javierjsp | 0:b7978deb977b | 79 | break; |
javierjsp | 0:b7978deb977b | 80 | case 'B': |
javierjsp | 0:b7978deb977b | 81 | mover(1,-1,1); |
javierjsp | 0:b7978deb977b | 82 | wait(0.2); |
javierjsp | 0:b7978deb977b | 83 | frenar(1); |
javierjsp | 0:b7978deb977b | 84 | break; |
javierjsp | 0:b7978deb977b | 85 | case 'C': |
javierjsp | 0:b7978deb977b | 86 | mover(2,1,1); |
javierjsp | 0:b7978deb977b | 87 | wait(0.2); |
javierjsp | 0:b7978deb977b | 88 | frenar(2); |
javierjsp | 0:b7978deb977b | 89 | break; |
javierjsp | 0:b7978deb977b | 90 | case 'D': |
javierjsp | 0:b7978deb977b | 91 | mover(2,-1,1); |
javierjsp | 0:b7978deb977b | 92 | wait(0.2); |
javierjsp | 0:b7978deb977b | 93 | frenar(2); |
javierjsp | 0:b7978deb977b | 94 | break; |
javierjsp | 0:b7978deb977b | 95 | case 'E': |
javierjsp | 0:b7978deb977b | 96 | mover(3,1,1); |
javierjsp | 0:b7978deb977b | 97 | wait(0.2); |
javierjsp | 0:b7978deb977b | 98 | frenar(3); |
javierjsp | 0:b7978deb977b | 99 | break; |
javierjsp | 0:b7978deb977b | 100 | case 'F': |
javierjsp | 0:b7978deb977b | 101 | mover(3,-1,1); |
javierjsp | 0:b7978deb977b | 102 | wait(0.2); |
javierjsp | 0:b7978deb977b | 103 | frenar(3); |
javierjsp | 0:b7978deb977b | 104 | break; |
javierjsp | 0:b7978deb977b | 105 | case 'G': |
javierjsp | 0:b7978deb977b | 106 | mover(4,1,1); |
javierjsp | 0:b7978deb977b | 107 | wait(0.2); |
javierjsp | 0:b7978deb977b | 108 | frenar(4); |
javierjsp | 0:b7978deb977b | 109 | break; |
javierjsp | 0:b7978deb977b | 110 | case 'H': |
javierjsp | 0:b7978deb977b | 111 | mover(4,-1,1); |
javierjsp | 0:b7978deb977b | 112 | wait(0.2); |
javierjsp | 0:b7978deb977b | 113 | frenar(4); |
javierjsp | 0:b7978deb977b | 114 | break; |
javierjsp | 0:b7978deb977b | 115 | case 'I': |
javierjsp | 0:b7978deb977b | 116 | mover(5,1,1); |
javierjsp | 0:b7978deb977b | 117 | wait(0.2); |
javierjsp | 0:b7978deb977b | 118 | frenar(5); |
javierjsp | 0:b7978deb977b | 119 | break; |
javierjsp | 0:b7978deb977b | 120 | case 'J': |
javierjsp | 0:b7978deb977b | 121 | mover(5,-1,1); |
javierjsp | 0:b7978deb977b | 122 | wait(0.2); |
javierjsp | 0:b7978deb977b | 123 | frenar(5); |
javierjsp | 0:b7978deb977b | 124 | break; |
javierjsp | 0:b7978deb977b | 125 | case 'K': |
javierjsp | 0:b7978deb977b | 126 | pinza(-1); |
javierjsp | 0:b7978deb977b | 127 | break; |
javierjsp | 0:b7978deb977b | 128 | case 'L': |
javierjsp | 0:b7978deb977b | 129 | pinza(1); |
javierjsp | 0:b7978deb977b | 130 | break; |
javierjsp | 6:644b77b2111e | 131 | case 'M': |
javierjsp | 6:644b77b2111e | 132 | movCinta(); |
javierjsp | 6:644b77b2111e | 133 | break; |
javierjsp | 0:b7978deb977b | 134 | } |
javierjsp | 0:b7978deb977b | 135 | } |
javierjsp | 0:b7978deb977b | 136 | } |
javierjsp | 0:b7978deb977b | 137 | } |
javierjsp | 0:b7978deb977b | 138 | |
javierjsp | 0:b7978deb977b | 139 | void enviar_bt() // definimos la funcion que ejecutara el ticker |
javierjsp | 0:b7978deb977b | 140 | { |
javierjsp | 4:9ea8a9271732 | 141 | pulsos[1]=enc1.getPulses(); |
javierjsp | 4:9ea8a9271732 | 142 | pulsos[2]=enc2.getPulses(); |
javierjsp | 4:9ea8a9271732 | 143 | pulsos[3]=enc3.getPulses(); |
javierjsp | 4:9ea8a9271732 | 144 | pulsos[4]=enc4.getPulses(); |
javierjsp | 4:9ea8a9271732 | 145 | pulsos[5]=enc5.getPulses(); |
javierjsp | 6:644b77b2111e | 146 | 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 | 147 | } |
javierjsp | 0:b7978deb977b | 148 | |
javierjsp | 6:644b77b2111e | 149 | void encenderLEDs() |
javierjsp | 1:7c07ddc42a4c | 150 | { |
javierjsp | 4:9ea8a9271732 | 151 | if (mp5==0) led1=1; // si se pulsa el mp1 se enciende el LED1 |
javierjsp | 1:7c07ddc42a4c | 152 | else led1=0; // si no se apaga |
javierjsp | 4:9ea8a9271732 | 153 | if (mp4==0) led3=1; // si se pulsa el mp1 se enciende el LED1 |
javierjsp | 4:9ea8a9271732 | 154 | else led3=0; // si no se apaga |
javierjsp | 4:9ea8a9271732 | 155 | } |
javierjsp | 4:9ea8a9271732 | 156 | |
javierjsp | 0:b7978deb977b | 157 | void sethome() |
javierjsp | 0:b7978deb977b | 158 | { |
javierjsp | 0:b7978deb977b | 159 | led2=1; |
javierjsp | 0:b7978deb977b | 160 | // posicionar base |
javierjsp | 0:b7978deb977b | 161 | int enc=0; |
javierjsp | 6:644b77b2111e | 162 | int dir=-1; // mover primero hacia delante |
javierjsp | 0:b7978deb977b | 163 | while (mp1==1) // Mientras no se pulse el microswitch |
javierjsp | 0:b7978deb977b | 164 | { |
javierjsp | 0:b7978deb977b | 165 | enc=enc1.getPulses(); // leemos el contador del encoder |
javierjsp | 4:9ea8a9271732 | 166 | mover(1,dir,0.7); // movemos el motor en la dirección inicial |
javierjsp | 6:644b77b2111e | 167 | wait(0.02); // esperamos 0.01s |
javierjsp | 0:b7978deb977b | 168 | if (enc==enc1.getPulses()) // Si el contador no se ha incrementado, hemos llegado al extremo |
javierjsp | 6:644b77b2111e | 169 | dir=1; // se cambia la dirección del motor |
javierjsp | 0:b7978deb977b | 170 | } |
javierjsp | 0:b7978deb977b | 171 | frenar(1); // el motor ha llegado a la posicion adecuada y se frena |
javierjsp | 0:b7978deb977b | 172 | |
javierjsp | 0:b7978deb977b | 173 | // posicionar brazo |
javierjsp | 6:644b77b2111e | 174 | dir=-1; // mover primero hacia delante |
javierjsp | 6:644b77b2111e | 175 | while (dir==-1) // mientras la direccion sea hacia delante: |
javierjsp | 0:b7978deb977b | 176 | { |
javierjsp | 0:b7978deb977b | 177 | while (mp2==0) // Mientras se mantenga pulsado el boton |
javierjsp | 4:9ea8a9271732 | 178 | mover(2,dir,0.7); // se seguira moviendo el motor en la direccion inicial |
javierjsp | 6:644b77b2111e | 179 | dir=1; // se cambia a direccion |
javierjsp | 0:b7978deb977b | 180 | while (mp2==1) // Mientras el MP esté sin pulsar: |
javierjsp | 0:b7978deb977b | 181 | { |
javierjsp | 0:b7978deb977b | 182 | enc=enc2.getPulses(); // leemos el valor de los encoders |
javierjsp | 4:9ea8a9271732 | 183 | mover(2,dir,0.7); // movemos el motor |
javierjsp | 6:644b77b2111e | 184 | wait(0.02); // esperamos 0.01s |
javierjsp | 0:b7978deb977b | 185 | if(enc==enc2.getPulses()) // Si el contador no ha cambiado |
javierjsp | 4:9ea8a9271732 | 186 | { |
javierjsp | 4:9ea8a9271732 | 187 | if (dir==-1) |
javierjsp | 0:b7978deb977b | 188 | dir=1; // cambiamos la dirección |
javierjsp | 4:9ea8a9271732 | 189 | else if (dir==1) |
javierjsp | 4:9ea8a9271732 | 190 | dir=-1; |
javierjsp | 4:9ea8a9271732 | 191 | } |
javierjsp | 0:b7978deb977b | 192 | } |
javierjsp | 0:b7978deb977b | 193 | } |
javierjsp | 0:b7978deb977b | 194 | frenar(2); // el motor ha llegado a la posicion adecuada y se frena |
javierjsp | 0:b7978deb977b | 195 | |
javierjsp | 0:b7978deb977b | 196 | // posicionar antebrazo |
javierjsp | 6:644b77b2111e | 197 | dir=-1; // mover primero hacia delante |
javierjsp | 6:644b77b2111e | 198 | while (dir==-1) // mientras la direccion sea hacia delante: |
javierjsp | 0:b7978deb977b | 199 | { |
javierjsp | 0:b7978deb977b | 200 | while (mp3==0) // Mientras se mantenga pulsado el boton |
javierjsp | 4:9ea8a9271732 | 201 | mover(3,dir,0.7); // se seguira moviendo el motor en la direccion inicial |
javierjsp | 6:644b77b2111e | 202 | dir=1; // se cambia a direccion |
javierjsp | 0:b7978deb977b | 203 | while (mp3==1) // Mientras el MP esté sin pulsar: |
javierjsp | 0:b7978deb977b | 204 | { |
javierjsp | 0:b7978deb977b | 205 | enc=enc3.getPulses(); // leemos el valor de los encoders |
javierjsp | 4:9ea8a9271732 | 206 | mover(3,dir,0.7); // movemos el motor |
javierjsp | 6:644b77b2111e | 207 | wait(0.02); // esperamos 0.01s |
javierjsp | 0:b7978deb977b | 208 | if(enc==enc3.getPulses()) // Si el contador no ha cambiado |
javierjsp | 4:9ea8a9271732 | 209 | { |
javierjsp | 4:9ea8a9271732 | 210 | if (dir==-1) |
javierjsp | 0:b7978deb977b | 211 | dir=1; // cambiamos la dirección |
javierjsp | 4:9ea8a9271732 | 212 | else if (dir==1) |
javierjsp | 4:9ea8a9271732 | 213 | dir=-1; |
javierjsp | 4:9ea8a9271732 | 214 | } |
javierjsp | 0:b7978deb977b | 215 | } |
javierjsp | 0:b7978deb977b | 216 | } |
javierjsp | 0:b7978deb977b | 217 | frenar(3); // el motor ha llegado a la posicion adecuada y se frena |
javierjsp | 0:b7978deb977b | 218 | |
javierjsp | 0:b7978deb977b | 219 | // posicionamiento del giro de la muñeca |
javierjsp | 6:644b77b2111e | 220 | dir=-1; // mover primero hacia delante |
javierjsp | 6:644b77b2111e | 221 | while (dir==-1) // Mientras que la dirección del motor sea 1: |
javierjsp | 0:b7978deb977b | 222 | { |
javierjsp | 0:b7978deb977b | 223 | while (mp4==0) // Mientras el MP esté pulsado |
javierjsp | 4:9ea8a9271732 | 224 | mover(4,dir,0.7); // movemos los motores 4 y 5 |
javierjsp | 6:644b77b2111e | 225 | dir=1; |
javierjsp | 0:b7978deb977b | 226 | while (mp4==1) // Mientras el MP esté sin pulsar: |
javierjsp | 0:b7978deb977b | 227 | { |
javierjsp | 0:b7978deb977b | 228 | enc=enc5.getPulses(); // leemos el valor del los encoders |
javierjsp | 4:9ea8a9271732 | 229 | mover(4,dir,0.7); // movemos los motores |
javierjsp | 6:644b77b2111e | 230 | wait(0.02); // esperamos 0.01s |
javierjsp | 0:b7978deb977b | 231 | if(enc==enc5.getPulses()) // Si el contador no ha cambiado |
javierjsp | 4:9ea8a9271732 | 232 | { |
javierjsp | 4:9ea8a9271732 | 233 | if (dir==-1) |
javierjsp | 0:b7978deb977b | 234 | dir=1; // cambiamos la dirección |
javierjsp | 4:9ea8a9271732 | 235 | else if (dir==1) |
javierjsp | 4:9ea8a9271732 | 236 | dir=-1; |
javierjsp | 4:9ea8a9271732 | 237 | } |
javierjsp | 0:b7978deb977b | 238 | } |
javierjsp | 0:b7978deb977b | 239 | } |
javierjsp | 0:b7978deb977b | 240 | frenar(4); // la muñeca ha llegado a la posicion adecuada y se frenan los motores |
javierjsp | 0:b7978deb977b | 241 | |
javierjsp | 0:b7978deb977b | 242 | // posicionamiento de la rotacion de la muñeca |
javierjsp | 4:9ea8a9271732 | 243 | while (mp5==1) // Mientras no se pulse el MS |
javierjsp | 0:b7978deb977b | 244 | { |
javierjsp | 4:9ea8a9271732 | 245 | mover(5,1,0.7); // se mueven los motores 4 y 5 |
javierjsp | 6:644b77b2111e | 246 | wait(0.02); // esperamos 0.01s |
javierjsp | 0:b7978deb977b | 247 | } |
javierjsp | 0:b7978deb977b | 248 | frenar(5); // frenamos los motores |
javierjsp | 0:b7978deb977b | 249 | pinza(-1); // abrimos la pinza |
javierjsp | 0:b7978deb977b | 250 | frenar(0); // frenamos todos los motoes |
javierjsp | 0:b7978deb977b | 251 | enc1.reset(); // reseteamos los encoders |
javierjsp | 0:b7978deb977b | 252 | enc2.reset(); |
javierjsp | 0:b7978deb977b | 253 | enc3.reset(); |
javierjsp | 0:b7978deb977b | 254 | enc4.reset(); |
javierjsp | 0:b7978deb977b | 255 | enc5.reset(); |
javierjsp | 0:b7978deb977b | 256 | enc6.reset(); |
javierjsp | 0:b7978deb977b | 257 | led2=0; |
javierjsp | 0:b7978deb977b | 258 | } |
javierjsp | 0:b7978deb977b | 259 | |
javierjsp | 1:7c07ddc42a4c | 260 | void posicion(int pos) |
javierjsp | 0:b7978deb977b | 261 | { |
javierjsp | 0:b7978deb977b | 262 | |
javierjsp | 0:b7978deb977b | 263 | } |
javierjsp | 0:b7978deb977b | 264 | |
javierjsp | 0:b7978deb977b | 265 | void mover(int m, int dir, float v) |
javierjsp | 0:b7978deb977b | 266 | { |
javierjsp | 0:b7978deb977b | 267 | if((dir==1 || dir==(-1))&&(v>0 && v<=1)) //comprueva que los parametros pasados estan dentro de los valores deseados |
javierjsp | 0:b7978deb977b | 268 | { |
javierjsp | 0:b7978deb977b | 269 | switch (m) |
javierjsp | 0:b7978deb977b | 270 | { |
javierjsp | 0:b7978deb977b | 271 | case 0: // en caso de que el valor sea 0 movemos todos los motores |
javierjsp | 0:b7978deb977b | 272 | m1.speed(dir*v); |
javierjsp | 0:b7978deb977b | 273 | m2.speed(dir*v); |
javierjsp | 0:b7978deb977b | 274 | m3.speed(dir*v); |
javierjsp | 0:b7978deb977b | 275 | m4.speed(dir*v); |
javierjsp | 0:b7978deb977b | 276 | m5.speed(dir*v); |
javierjsp | 5:053c61912254 | 277 | break; |
javierjsp | 0:b7978deb977b | 278 | case 1: // en caso de que el valor sea 1 movemos el motor 1 (base) |
javierjsp | 0:b7978deb977b | 279 | m1.speed(dir*v); |
javierjsp | 0:b7978deb977b | 280 | break; |
javierjsp | 0:b7978deb977b | 281 | case 2: // en caso de que el valor sea 2 movemos el motor 2 (hombro) |
javierjsp | 0:b7978deb977b | 282 | m2.speed(dir*v); |
javierjsp | 0:b7978deb977b | 283 | break; |
javierjsp | 0:b7978deb977b | 284 | case 3: // en caso de que el valor sea 3 movemos el motor 3 (codo) |
javierjsp | 0:b7978deb977b | 285 | m3.speed(dir*v); |
javierjsp | 0:b7978deb977b | 286 | break; |
javierjsp | 0:b7978deb977b | 287 | 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 | 288 | m4.speed((-1)*dir*v); |
javierjsp | 0:b7978deb977b | 289 | m5.speed(dir*v); |
javierjsp | 5:053c61912254 | 290 | break; |
javierjsp | 0:b7978deb977b | 291 | 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 | 292 | m4.speed(dir*v); |
javierjsp | 0:b7978deb977b | 293 | m5.speed(dir*v); |
javierjsp | 5:053c61912254 | 294 | break; |
javierjsp | 0:b7978deb977b | 295 | case 6: // en caso de que el valor sea 6 movemos el motor 6 (pinza) |
javierjsp | 0:b7978deb977b | 296 | m6.speed(dir*v); |
javierjsp | 0:b7978deb977b | 297 | break; |
javierjsp | 0:b7978deb977b | 298 | } |
javierjsp | 0:b7978deb977b | 299 | } |
javierjsp | 0:b7978deb977b | 300 | else // si los parametros no son correctos encendemos el LED3 |
javierjsp | 0:b7978deb977b | 301 | led3=1; |
javierjsp | 0:b7978deb977b | 302 | } |
javierjsp | 0:b7978deb977b | 303 | |
javierjsp | 0:b7978deb977b | 304 | void pinza(int a) |
javierjsp | 0:b7978deb977b | 305 | { |
javierjsp | 0:b7978deb977b | 306 | int enc=0, encOld=0; |
javierjsp | 5:053c61912254 | 307 | mover(6,a,1); // movemos la pinza |
javierjsp | 3:fa1d943cf383 | 308 | encOld=enc6.getPulses(); // leemos la posicion del encoder |
javierjsp | 3:fa1d943cf383 | 309 | wait(0.1); // espera 0.1s |
javierjsp | 3:fa1d943cf383 | 310 | enc=enc6.getPulses(); // leemos la posicion nueva del encoder |
javierjsp | 3:fa1d943cf383 | 311 | while(enc != encOld) // mientras la posicion antigua sea distinta de la nueva seguimos leyendo |
javierjsp | 0:b7978deb977b | 312 | { |
javierjsp | 0:b7978deb977b | 313 | wait(0.1); // espera 0.1s |
javierjsp | 0:b7978deb977b | 314 | encOld=enc; // guardamos la posicion antigua del encoder |
javierjsp | 0:b7978deb977b | 315 | enc=enc6.getPulses(); // leemos la posicion nueva del encoder |
javierjsp | 3:fa1d943cf383 | 316 | } |
javierjsp | 0:b7978deb977b | 317 | frenar(6); // cuando la pinza ha llegado al tope frenamos la pinza |
javierjsp | 0:b7978deb977b | 318 | } |
javierjsp | 0:b7978deb977b | 319 | |
javierjsp | 0:b7978deb977b | 320 | void frenar(int m) |
javierjsp | 0:b7978deb977b | 321 | { |
javierjsp | 0:b7978deb977b | 322 | switch(m) |
javierjsp | 0:b7978deb977b | 323 | { |
javierjsp | 0:b7978deb977b | 324 | case 0: // en caso de que el valor sea 0 frenamos todos los motores |
javierjsp | 0:b7978deb977b | 325 | m1.brake(); |
javierjsp | 0:b7978deb977b | 326 | m2.brake(); |
javierjsp | 0:b7978deb977b | 327 | m3.brake(); |
javierjsp | 0:b7978deb977b | 328 | m4.brake(); |
javierjsp | 0:b7978deb977b | 329 | m5.brake(); |
javierjsp | 0:b7978deb977b | 330 | m6.brake(); |
javierjsp | 0:b7978deb977b | 331 | break; |
javierjsp | 0:b7978deb977b | 332 | case 1: // en caso de que el valor sea 1 frenamos el motor 1 |
javierjsp | 0:b7978deb977b | 333 | m1.brake(); |
javierjsp | 0:b7978deb977b | 334 | break; |
javierjsp | 0:b7978deb977b | 335 | case 2: // en caso de que el valor sea 2 frenamos el motor 2 |
javierjsp | 0:b7978deb977b | 336 | m2.brake(); |
javierjsp | 0:b7978deb977b | 337 | break; |
javierjsp | 0:b7978deb977b | 338 | case 3: // en caso de que el valor sea 3 frenamos el motor 3 |
javierjsp | 0:b7978deb977b | 339 | m3.brake(); |
javierjsp | 0:b7978deb977b | 340 | break; |
javierjsp | 0:b7978deb977b | 341 | case 4: // en caso de que el valor sea 4 frenamos los motores 4 y 5 |
javierjsp | 0:b7978deb977b | 342 | m4.brake(); |
javierjsp | 0:b7978deb977b | 343 | m5.brake(); |
javierjsp | 0:b7978deb977b | 344 | break; |
javierjsp | 0:b7978deb977b | 345 | case 5: // en caso de que el valor sea 5 frenamos los motores 4 y 5 |
javierjsp | 0:b7978deb977b | 346 | m4.brake(); |
javierjsp | 0:b7978deb977b | 347 | m5.brake(); |
javierjsp | 0:b7978deb977b | 348 | break; |
javierjsp | 0:b7978deb977b | 349 | case 6: // en caso de que el valor sea 6 frenamos el motor 6 |
javierjsp | 0:b7978deb977b | 350 | m6.brake(); |
javierjsp | 0:b7978deb977b | 351 | break; |
javierjsp | 0:b7978deb977b | 352 | } |
javierjsp | 6:644b77b2111e | 353 | } |
javierjsp | 6:644b77b2111e | 354 | void movCinta() |
javierjsp | 6:644b77b2111e | 355 | { |
javierjsp | 6:644b77b2111e | 356 | while (sensor==1) |
javierjsp | 6:644b77b2111e | 357 | cinta.speed(1); |
javierjsp | 6:644b77b2111e | 358 | cinta.brake(); |
javierjsp | 0:b7978deb977b | 359 | } |
javierjsp | 0:b7978deb977b | 360 | |
javierjsp | 0:b7978deb977b | 361 | |
javierjsp | 0:b7978deb977b | 362 | |
javierjsp | 0:b7978deb977b | 363 | |
javierjsp | 6:644b77b2111e | 364 |