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