Javier Sanchez Peña / Mbed 2 deprecated proyecto_brazo_robotico

control de un brazo robotico SCORBOT ER-III a traves de bluetooth con un dispositivo android.

Dependencies:   Motor QEI mbed

main.cpp@5:ae0bdb3aad14, 2017-09-04 (annotated)

Committer:
javierjsp
Date:
Mon Sep 04 18:03:19 2017 +0000
Revision:
5:ae0bdb3aad14
Parent:
4:ceef4dee8284 
version final presentada con la cinta, la sonda y el calefactor incluidos.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
javierjsp 1:bf0631515d8c 1 #include "mbed.h" // declaramos las librerias de funciones
javierjsp 0:cd0af1b08faf 2 #include "Motor.h"
javierjsp 0:cd0af1b08faf 3 #include "QEI.h"
javierjsp 0:cd0af1b08faf 4
javierjsp 0:cd0af1b08faf 5 // declaramos los encoders
javierjsp 0:cd0af1b08faf 6 QEI enc1(PG_4, PG_6, NC, 480, QEI::X4_ENCODING); // (P0, P1, indice, PulsosPorRev, TipoDeEncoder)
javierjsp 0:cd0af1b08faf 7 QEI enc2(PG_7, PG_5, NC, 480, QEI::X4_ENCODING);
javierjsp 0:cd0af1b08faf 8 QEI enc3(PD_10, PG_8, NC, 480, QEI::X4_ENCODING);
javierjsp 0:cd0af1b08faf 9 QEI enc4(PG_14, PF_11, NC, 480, QEI::X4_ENCODING);
javierjsp 0:cd0af1b08faf 10 QEI enc5(PF_12, PF_15, NC, 480, QEI::X4_ENCODING);
javierjsp 0:cd0af1b08faf 11 QEI enc6(PF_13, PF_3, NC, 240, QEI::X4_ENCODING);
javierjsp 0:cd0af1b08faf 12
javierjsp 0:cd0af1b08faf 13 // declaramos los motores
javierjsp 0:cd0af1b08faf 14 Motor m1(PA_5, PC_8, PC_6); // (pwm, derecho, inverso)
javierjsp 0:cd0af1b08faf 15 Motor m2(PA_6, PC_5, PD_8);
javierjsp 0:cd0af1b08faf 16 Motor m3(PA_7, PA_12, PA_11);
javierjsp 0:cd0af1b08faf 17 Motor m4(PB_6, PB_12, PB_11);
javierjsp 0:cd0af1b08faf 18 Motor m5(PC_7, PB_2, PB_1);
javierjsp 0:cd0af1b08faf 19 Motor m6(PB_9, PB_15, PB_13);
javierjsp 0:cd0af1b08faf 20 Motor cinta(PB_8, PC_4, PF_5);
javierjsp 0:cd0af1b08faf 21
javierjsp 0:cd0af1b08faf 22 // declaramos las entradas de los micropulsadores
javierjsp 0:cd0af1b08faf 23 DigitalIn mp1(PA_9, PullUp); // (pin, modo)
javierjsp 0:cd0af1b08faf 24 DigitalIn mp2(PA_8, PullUp);
javierjsp 0:cd0af1b08faf 25 DigitalIn mp3(PB_10, PullUp);
javierjsp 0:cd0af1b08faf 26 DigitalIn mp4(PB_4, PullUp);
javierjsp 0:cd0af1b08faf 27 DigitalIn mp5(PB_5, PullUp);
javierjsp 4:ceef4dee8284 28
javierjsp 4:ceef4dee8284 29 // Declaramos los pines del sensor, la sonda y el calefactor
javierjsp 0:cd0af1b08faf 30 DigitalIn sensor(PB_3, PullUp);
javierjsp 4:ceef4dee8284 31 AnalogIn sonda(PF_10);
javierjsp 4:ceef4dee8284 32 PwmOut calef(PE_10);
javierjsp 0:cd0af1b08faf 33
javierjsp 0:cd0af1b08faf 34 // declaramos el bluetooth
javierjsp 4:ceef4dee8284 35 Serial bt(PA_2, PA_3); // (TX, RX) (puerto serial 2)
javierjsp 0:cd0af1b08faf 36
javierjsp 0:cd0af1b08faf 37 // declaramos otras variables que utilizaremos
javierjsp 1:bf0631515d8c 38 Ticker ticker1; // iniciamos un ticker
javierjsp 0:cd0af1b08faf 39 DigitalOut led1 (LED1); // declaramos el LED1
javierjsp 0:cd0af1b08faf 40 DigitalOut led2 (LED2); // declaramos el LED2
javierjsp 0:cd0af1b08faf 41 DigitalOut led3 (LED3); // declaramos el LED3
javierjsp 0:cd0af1b08faf 42 int pulsos[6]={0,0,0,0,0,0}; // iniciamos el array pulsos
javierjsp 4:ceef4dee8284 43 int posiciones[20][6]={{0,0,0,0,0,0},{0,-3200,0,0,0,0},{0,-3200,1820,1396,0,0},{0,0,1661,1117,0,0},{0,3000,2050,1200,0,0}, // declaramos los arrays de posiciones
javierjsp 4:ceef4dee8284 44 {0,3000,0,0,0,0},{0,5200,0,0,0,0},{0,5200,2100,2000,0,0},{0,0,0,0,0,0},{0,-1988,-900,-400,396,-395}, // sustituye aqui los valores por los de los encoders
javierjsp 4:ceef4dee8284 45 {0,-1700,737,819,-53,-835},{0,-713,1690,1616,180,-955},{0,240,953,-2014,-1540,-2247},{0,950,65,-3970,863,-2050},{0,2200,1040,240,-348,-836}, // y deja el primer valor de cada posicion a 0
javierjsp 4:ceef4dee8284 46 {0,2150,1370,-2066,-200,-980},{0,3150,1370,-2066,1893,1116},{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0}};
javierjsp 0:cd0af1b08faf 47 char dato=0;
javierjsp 0:cd0af1b08faf 48 float V=1;
javierjsp 4:ceef4dee8284 49 float t=0;
javierjsp 0:cd0af1b08faf 50
javierjsp 0:cd0af1b08faf 51 // declaramos el prototipo de las funciones que utilizaremos
javierjsp 0:cd0af1b08faf 52 void enviar_bt(); // prototipo de la funcion que ejecutara el ticker1
javierjsp 0:cd0af1b08faf 53 void sethome(); // prototipo de la funcion que nos calibrara la posicion hard home de referencia
javierjsp 0:cd0af1b08faf 54 void posicion(int); // prototipo de la funcion que nos llevara hasta la posicion especificada
javierjsp 0:cd0af1b08faf 55 void mover(int,int,float); // prototipo de la funcion que movera cada motor
javierjsp 0:cd0af1b08faf 56 void frenar(int); // prototipo de la funcion que frenara cada motor
javierjsp 0:cd0af1b08faf 57 void pinza(int); // prototipo de la funcion que abrira (-1) o cerrara (1) la pinza
javierjsp 0:cd0af1b08faf 58 void movCinta(); // prototipo de la funcion que avanzara la cinta hasta detectar un objeto
javierjsp 4:ceef4dee8284 59 void calefactor(); // prototipo de la funcion que controlara el calefactor
javierjsp 1:bf0631515d8c 60 float map(float,float,float,float,float); // prototipo de la funcion que hace una redimension de los parametros
javierjsp 0:cd0af1b08faf 61
javierjsp 0:cd0af1b08faf 62 int main()
javierjsp 0:cd0af1b08faf 63 {
javierjsp 4:ceef4dee8284 64 m1.period(0.001); // establece el periodo del PWM de los motores, equivalente a una frecuencia de 1KHz.
javierjsp 0:cd0af1b08faf 65 m2.period(0.001);
javierjsp 0:cd0af1b08faf 66 m3.period(0.001);
javierjsp 0:cd0af1b08faf 67 m4.period(0.001);
javierjsp 0:cd0af1b08faf 68 m5.period(0.001);
javierjsp 0:cd0af1b08faf 69 m6.period(0.001);
javierjsp 0:cd0af1b08faf 70
javierjsp 4:ceef4dee8284 71 calef.period(1); // establece el periodo del PWM
javierjsp 4:ceef4dee8284 72 calef.write(0.05); // establece el ciclo de trabajo al 10%
javierjsp 4:ceef4dee8284 73
javierjsp 0:cd0af1b08faf 74 bt.baud(9600); // velocidad de transmision del bluetooth a 9600 baudios
javierjsp 0:cd0af1b08faf 75
javierjsp 4:ceef4dee8284 76 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 4:ceef4dee8284 77
javierjsp 4:ceef4dee8284 78 wait(5);
javierjsp 0:cd0af1b08faf 79 sethome(); // establecemos la posicion de referencia
javierjsp 0:cd0af1b08faf 80
javierjsp 0:cd0af1b08faf 81 while(1)
javierjsp 0:cd0af1b08faf 82 {
javierjsp 0:cd0af1b08faf 83 if(bt.readable()>0) // si hay un dato disponible:
javierjsp 0:cd0af1b08faf 84 {
javierjsp 0:cd0af1b08faf 85 dato = bt.getc(); // leemos un caracter
javierjsp 0:cd0af1b08faf 86 if (dato<=10 && dato>=1) // si tiene un valor entre 1 y 10 lo asignamos como velocidad
javierjsp 0:cd0af1b08faf 87 V=map(dato,1,10,0.6,1);
javierjsp 0:cd0af1b08faf 88 switch(dato)
javierjsp 0:cd0af1b08faf 89 {
javierjsp 0:cd0af1b08faf 90 case 'A': // si el caracter es A se mueve el motor 1 al derecho
javierjsp 0:cd0af1b08faf 91 mover(1,1,V);
javierjsp 0:cd0af1b08faf 92 wait(0.2);
javierjsp 0:cd0af1b08faf 93 frenar(1);
javierjsp 0:cd0af1b08faf 94 break;
javierjsp 0:cd0af1b08faf 95 case 'B': // si el caracter es B se mueve el motor 1 al inverso
javierjsp 0:cd0af1b08faf 96 mover(1,-1,V);
javierjsp 0:cd0af1b08faf 97 wait(0.2);
javierjsp 0:cd0af1b08faf 98 frenar(1);
javierjsp 0:cd0af1b08faf 99 break;
javierjsp 0:cd0af1b08faf 100 case 'C': // si el caracter es C se mueve el motor 2 al derecho
javierjsp 0:cd0af1b08faf 101 mover(2,1,V);
javierjsp 0:cd0af1b08faf 102 wait(0.2);
javierjsp 0:cd0af1b08faf 103 frenar(2);
javierjsp 0:cd0af1b08faf 104 break;
javierjsp 0:cd0af1b08faf 105 case 'D': // si el caracter es D se mueve el motor 2 al inverso
javierjsp 0:cd0af1b08faf 106 mover(2,-1,V);
javierjsp 0:cd0af1b08faf 107 wait(0.2);
javierjsp 0:cd0af1b08faf 108 frenar(2);
javierjsp 0:cd0af1b08faf 109 break;
javierjsp 0:cd0af1b08faf 110 case 'E': // si el caracter es E se mueve el motor 3 al derecho
javierjsp 0:cd0af1b08faf 111 mover(3,1,V);
javierjsp 0:cd0af1b08faf 112 wait(0.2);
javierjsp 0:cd0af1b08faf 113 frenar(3);
javierjsp 0:cd0af1b08faf 114 break;
javierjsp 0:cd0af1b08faf 115 case 'F': // si el caracter es F se mueve el motor 3 al inverso
javierjsp 0:cd0af1b08faf 116 mover(3,-1,V);
javierjsp 0:cd0af1b08faf 117 wait(0.2);
javierjsp 0:cd0af1b08faf 118 frenar(3);
javierjsp 0:cd0af1b08faf 119 break;
javierjsp 0:cd0af1b08faf 120 case 'G': // si el caracter es G se mueve el motor 4 Y 5 al derecho
javierjsp 0:cd0af1b08faf 121 mover(14,1,V);
javierjsp 0:cd0af1b08faf 122 wait(0.2);
javierjsp 0:cd0af1b08faf 123 frenar(14);
javierjsp 0:cd0af1b08faf 124 break;
javierjsp 0:cd0af1b08faf 125 case 'H': // si el caracter es H se mueve el motor 4 Y 5 al inverso
javierjsp 0:cd0af1b08faf 126 mover(14,-1,V);
javierjsp 0:cd0af1b08faf 127 wait(0.2);
javierjsp 0:cd0af1b08faf 128 frenar(14);
javierjsp 0:cd0af1b08faf 129 break;
javierjsp 0:cd0af1b08faf 130 case 'I': // si el caracter es I se mueve el motor 4 Y 5 al derecho
javierjsp 0:cd0af1b08faf 131 mover(15,1,V);
javierjsp 0:cd0af1b08faf 132 wait(0.2);
javierjsp 0:cd0af1b08faf 133 frenar(15);
javierjsp 0:cd0af1b08faf 134 break;
javierjsp 0:cd0af1b08faf 135 case 'J': // si el caracter es J se mueve el motor 4 Y 5 al inverso
javierjsp 0:cd0af1b08faf 136 mover(15,-1,V);
javierjsp 0:cd0af1b08faf 137 wait(0.2);
javierjsp 0:cd0af1b08faf 138 frenar(15);
javierjsp 0:cd0af1b08faf 139 break;
javierjsp 0:cd0af1b08faf 140 case 'K': // si el caracter es K se abre la pinza
javierjsp 0:cd0af1b08faf 141 pinza(-1);
javierjsp 0:cd0af1b08faf 142 break;
javierjsp 0:cd0af1b08faf 143 case 'L': // si el caracter es L se cierra la pinza
javierjsp 0:cd0af1b08faf 144 pinza(1);
javierjsp 0:cd0af1b08faf 145 break;
javierjsp 1:bf0631515d8c 146 case 'M': // si el caracter es M se mueve a la posicion home
javierjsp 0:cd0af1b08faf 147 posicion(0);
javierjsp 0:cd0af1b08faf 148 pinza(-1);
javierjsp 0:cd0af1b08faf 149 break;
javierjsp 1:bf0631515d8c 150 case 'N': // si el caracter es N se mueve a la posicion 1
javierjsp 1:bf0631515d8c 151 posicion(1);
javierjsp 1:bf0631515d8c 152 break;
javierjsp 1:bf0631515d8c 153 case 'O': // si el caracter es O se mueve a la posicion 2
javierjsp 1:bf0631515d8c 154 posicion(2);
javierjsp 1:bf0631515d8c 155 break;
javierjsp 1:bf0631515d8c 156 case 'P': // si el caracter es P se mueve a la posicion 3
javierjsp 1:bf0631515d8c 157 posicion(3);
javierjsp 1:bf0631515d8c 158 break;
javierjsp 1:bf0631515d8c 159 case 'Q': // si el caracter es Q se mueve a la posicion 4
javierjsp 1:bf0631515d8c 160 posicion(4);
javierjsp 1:bf0631515d8c 161 break;
javierjsp 1:bf0631515d8c 162 case 'R': // si el caracter es R se mueve a la posicion 5
javierjsp 4:ceef4dee8284 163 sethome();
javierjsp 1:bf0631515d8c 164 break;
javierjsp 1:bf0631515d8c 165 case 'S': // si el caracter es S se mueve a la posicion 6
javierjsp 4:ceef4dee8284 166 posicion(1);
javierjsp 4:ceef4dee8284 167 posicion(2);
javierjsp 4:ceef4dee8284 168 pinza(1);
javierjsp 4:ceef4dee8284 169 posicion(0);
javierjsp 4:ceef4dee8284 170 posicion(3);
javierjsp 4:ceef4dee8284 171 pinza(-1);
javierjsp 4:ceef4dee8284 172 posicion(0);
javierjsp 1:bf0631515d8c 173 break;
javierjsp 1:bf0631515d8c 174 case 'T': // si el caracter es T se mueve a la posicion 7
javierjsp 4:ceef4dee8284 175 posicion(1);
javierjsp 4:ceef4dee8284 176 posicion(2);
javierjsp 4:ceef4dee8284 177 pinza(1);
javierjsp 4:ceef4dee8284 178 posicion(0);
javierjsp 4:ceef4dee8284 179 posicion(5);
javierjsp 4:ceef4dee8284 180 posicion(4);
javierjsp 4:ceef4dee8284 181 calefactor();
javierjsp 4:ceef4dee8284 182 posicion(5);
javierjsp 4:ceef4dee8284 183 posicion(6);
javierjsp 1:bf0631515d8c 184 posicion(7);
javierjsp 4:ceef4dee8284 185 pinza(-1);
javierjsp 4:ceef4dee8284 186 posicion(6);
javierjsp 4:ceef4dee8284 187 posicion(0);
javierjsp 1:bf0631515d8c 188 break;
javierjsp 1:bf0631515d8c 189 case 'U': // si el caracter es U se mueve a la posicion 8
javierjsp 4:ceef4dee8284 190 posicion(9);
javierjsp 4:ceef4dee8284 191 posicion(10);
javierjsp 4:ceef4dee8284 192 posicion(11);
javierjsp 4:ceef4dee8284 193 posicion(12);
javierjsp 4:ceef4dee8284 194 pinza(1);
javierjsp 4:ceef4dee8284 195 posicion(13);
javierjsp 4:ceef4dee8284 196 posicion(14);
javierjsp 4:ceef4dee8284 197 pinza(-1);
javierjsp 4:ceef4dee8284 198 posicion(15);
javierjsp 4:ceef4dee8284 199 posicion(16);
javierjsp 4:ceef4dee8284 200 pinza(1);
javierjsp 4:ceef4dee8284 201 posicion(0);
javierjsp 4:ceef4dee8284 202 pinza(-1);
javierjsp 1:bf0631515d8c 203 break;
javierjsp 1:bf0631515d8c 204 case 'V': // si el caracter es V se mueve a la posicion 9
javierjsp 1:bf0631515d8c 205 posicion(9);
javierjsp 1:bf0631515d8c 206 break;
javierjsp 0:cd0af1b08faf 207 }
javierjsp 0:cd0af1b08faf 208 }
javierjsp 0:cd0af1b08faf 209 }
javierjsp 0:cd0af1b08faf 210 }
javierjsp 0:cd0af1b08faf 211
javierjsp 1:bf0631515d8c 212 float map(float x, float in_min, float in_max, float out_min, float out_max) // funcion que redimensiona los parametros
javierjsp 0:cd0af1b08faf 213 {
javierjsp 0:cd0af1b08faf 214 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
javierjsp 0:cd0af1b08faf 215 }
javierjsp 0:cd0af1b08faf 216
javierjsp 0:cd0af1b08faf 217 void enviar_bt() // definimos la funcion que ejecutara el ticker
javierjsp 0:cd0af1b08faf 218 {
javierjsp 0:cd0af1b08faf 219 pulsos[1]=enc1.getPulses(); // cargamos los contadores de los encoders
javierjsp 0:cd0af1b08faf 220 pulsos[2]=enc2.getPulses();
javierjsp 0:cd0af1b08faf 221 pulsos[3]=enc3.getPulses();
javierjsp 0:cd0af1b08faf 222 pulsos[4]=enc4.getPulses();
javierjsp 0:cd0af1b08faf 223 pulsos[5]=enc5.getPulses();
javierjsp 4:ceef4dee8284 224 t= map(sonda.read(),0,1,-10,50);
javierjsp 4:ceef4dee8284 225 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 temperatura de la sonda:%.2f\n ", pulsos[1],pulsos[2],pulsos[3],pulsos[4],pulsos[5],t); // envia los valores del array "pulsos" por bluetooth
javierjsp 0:cd0af1b08faf 226 }
javierjsp 0:cd0af1b08faf 227
javierjsp 0:cd0af1b08faf 228 void sethome()
javierjsp 0:cd0af1b08faf 229 {
javierjsp 0:cd0af1b08faf 230 led2=1;
javierjsp 4:ceef4dee8284 231
javierjsp 0:cd0af1b08faf 232 // posicionar base
javierjsp 0:cd0af1b08faf 233 int enc=0;
javierjsp 0:cd0af1b08faf 234 int dir=-1; // mover primero hacia delante
javierjsp 0:cd0af1b08faf 235 while (mp1==1) // Mientras no se pulse el microswitch
javierjsp 0:cd0af1b08faf 236 {
javierjsp 0:cd0af1b08faf 237 enc=enc1.getPulses(); // leemos el contador del encoder
javierjsp 4:ceef4dee8284 238 mover(1,dir,1); // movemos el motor en la dirección inicial
javierjsp 4:ceef4dee8284 239 wait(0.05); // esperamos 0.01s
javierjsp 0:cd0af1b08faf 240 if (enc==enc1.getPulses()) // Si el contador no se ha incrementado, hemos llegado al extremo
javierjsp 3:0b3d5b1850aa 241 {
javierjsp 3:0b3d5b1850aa 242 if (dir==-1)
javierjsp 3:0b3d5b1850aa 243 dir=1; // cambiamos la dirección
javierjsp 3:0b3d5b1850aa 244 else if (dir==1)
javierjsp 3:0b3d5b1850aa 245 dir=-1;
javierjsp 4:ceef4dee8284 246 } // se cambia la dirección del motor
javierjsp 0:cd0af1b08faf 247 }
javierjsp 0:cd0af1b08faf 248 frenar(1); // el motor ha llegado a la posicion adecuada y se frena
javierjsp 0:cd0af1b08faf 249
javierjsp 0:cd0af1b08faf 250 // posicionar brazo
javierjsp 0:cd0af1b08faf 251 dir=-1; // mover primero hacia delante
javierjsp 0:cd0af1b08faf 252 while (dir==-1) // mientras la direccion sea hacia delante:
javierjsp 0:cd0af1b08faf 253 {
javierjsp 0:cd0af1b08faf 254 while (mp2==0) // Mientras se mantenga pulsado el boton
javierjsp 4:ceef4dee8284 255 mover(2,dir,1); // se seguira moviendo el motor en la direccion inicial
javierjsp 0:cd0af1b08faf 256 dir=1; // se cambia a direccion
javierjsp 0:cd0af1b08faf 257 while (mp2==1) // Mientras el MP esté sin pulsar:
javierjsp 0:cd0af1b08faf 258 {
javierjsp 0:cd0af1b08faf 259 enc=enc2.getPulses(); // leemos el valor de los encoders
javierjsp 4:ceef4dee8284 260 mover(2,dir,1); // movemos el motor
javierjsp 4:ceef4dee8284 261 wait(0.05); // esperamos 0.01s
javierjsp 0:cd0af1b08faf 262 if(enc==enc2.getPulses()) // Si el contador no ha cambiado
javierjsp 0:cd0af1b08faf 263 {
javierjsp 0:cd0af1b08faf 264 if (dir==-1)
javierjsp 0:cd0af1b08faf 265 dir=1; // cambiamos la dirección
javierjsp 0:cd0af1b08faf 266 else if (dir==1)
javierjsp 0:cd0af1b08faf 267 dir=-1;
javierjsp 0:cd0af1b08faf 268 }
javierjsp 0:cd0af1b08faf 269 }
javierjsp 0:cd0af1b08faf 270 }
javierjsp 0:cd0af1b08faf 271 frenar(2); // el motor ha llegado a la posicion adecuada y se frena
javierjsp 0:cd0af1b08faf 272
javierjsp 0:cd0af1b08faf 273 // posicionar antebrazo
javierjsp 4:ceef4dee8284 274 dir=-1; // mover primero hacia delante
javierjsp 4:ceef4dee8284 275 while (dir==-1) // mientras la direccion sea hacia delante:
javierjsp 0:cd0af1b08faf 276 {
javierjsp 0:cd0af1b08faf 277 while (mp3==0) // Mientras se mantenga pulsado el boton
javierjsp 4:ceef4dee8284 278 mover(3,dir,1); // se seguira moviendo el motor en la direccion inicial
javierjsp 4:ceef4dee8284 279 dir=1; // se cambia a direccion
javierjsp 0:cd0af1b08faf 280 while (mp3==1) // Mientras el MP esté sin pulsar:
javierjsp 0:cd0af1b08faf 281 {
javierjsp 0:cd0af1b08faf 282 enc=enc3.getPulses(); // leemos el valor de los encoders
javierjsp 4:ceef4dee8284 283 mover(3,dir,1); // movemos el motor
javierjsp 0:cd0af1b08faf 284 wait(0.01); // esperamos 0.01s
javierjsp 0:cd0af1b08faf 285 if(enc==enc3.getPulses()) // Si el contador no ha cambiado
javierjsp 0:cd0af1b08faf 286 {
javierjsp 0:cd0af1b08faf 287 if (dir==-1)
javierjsp 0:cd0af1b08faf 288 dir=1; // cambiamos la dirección
javierjsp 0:cd0af1b08faf 289 else if (dir==1)
javierjsp 0:cd0af1b08faf 290 dir=-1;
javierjsp 0:cd0af1b08faf 291 }
javierjsp 0:cd0af1b08faf 292 }
javierjsp 0:cd0af1b08faf 293 }
javierjsp 0:cd0af1b08faf 294 frenar(3); // el motor ha llegado a la posicion adecuada y se frena
javierjsp 0:cd0af1b08faf 295
javierjsp 0:cd0af1b08faf 296 // posicionamiento del giro de la muñeca
javierjsp 4:ceef4dee8284 297 dir=-1; // mover primero hacia delante
javierjsp 4:ceef4dee8284 298 while (dir==-1) // Mientras que la dirección del motor sea 1:
javierjsp 0:cd0af1b08faf 299 {
javierjsp 0:cd0af1b08faf 300 while (mp4==0) // Mientras el MP esté pulsado
javierjsp 4:ceef4dee8284 301 mover(14,dir,1); // movemos los motores 4 y 5
javierjsp 0:cd0af1b08faf 302 dir=1;
javierjsp 0:cd0af1b08faf 303 while (mp4==1) // Mientras el MP esté sin pulsar:
javierjsp 0:cd0af1b08faf 304 {
javierjsp 0:cd0af1b08faf 305 enc=enc5.getPulses(); // leemos el valor del los encoders
javierjsp 4:ceef4dee8284 306 mover(14,dir,1); // movemos los motores
javierjsp 0:cd0af1b08faf 307 wait(0.01); // esperamos 0.01s
javierjsp 0:cd0af1b08faf 308 if(enc==enc5.getPulses()) // Si el contador no ha cambiado
javierjsp 0:cd0af1b08faf 309 {
javierjsp 0:cd0af1b08faf 310 if (dir==-1)
javierjsp 0:cd0af1b08faf 311 dir=1; // cambiamos la dirección
javierjsp 0:cd0af1b08faf 312 else if (dir==1)
javierjsp 0:cd0af1b08faf 313 dir=-1;
javierjsp 0:cd0af1b08faf 314 }
javierjsp 0:cd0af1b08faf 315 }
javierjsp 0:cd0af1b08faf 316 }
javierjsp 4:ceef4dee8284 317 frenar(14); // la muñeca ha llegado a la posicion adecuada y se frenan los motores
javierjsp 0:cd0af1b08faf 318
javierjsp 0:cd0af1b08faf 319 // posicionamiento de la rotacion de la muñeca
javierjsp 0:cd0af1b08faf 320 while (mp5==1) // Mientras no se pulse el MS
javierjsp 0:cd0af1b08faf 321 {
javierjsp 4:ceef4dee8284 322 mover(15,1,1); // se mueven los motores 4 y 5
javierjsp 2:547c71a1ce67 323 wait(0.01); // esperamos 0.01s
javierjsp 0:cd0af1b08faf 324 }
javierjsp 4:ceef4dee8284 325 frenar(15); // frenamos los motores
javierjsp 4:ceef4dee8284 326 mover(15,1,1); // giramos la pinza para que quede perpendicular al eje
javierjsp 4:ceef4dee8284 327 wait(0.25);
javierjsp 4:ceef4dee8284 328 frenar(15);
javierjsp 0:cd0af1b08faf 329
javierjsp 0:cd0af1b08faf 330 pinza(-1); // abrimos la pinza
javierjsp 0:cd0af1b08faf 331
javierjsp 0:cd0af1b08faf 332 // ponemos el contador de los encoders a 0
javierjsp 0:cd0af1b08faf 333 frenar(0); // frenamos todos los motoes
javierjsp 0:cd0af1b08faf 334 enc1.reset(); // reseteamos los encoders
javierjsp 0:cd0af1b08faf 335 enc2.reset();
javierjsp 0:cd0af1b08faf 336 enc3.reset();
javierjsp 0:cd0af1b08faf 337 enc4.reset();
javierjsp 0:cd0af1b08faf 338 enc5.reset();
javierjsp 0:cd0af1b08faf 339 enc6.reset();
javierjsp 0:cd0af1b08faf 340 led2=0;
javierjsp 0:cd0af1b08faf 341 }
javierjsp 0:cd0af1b08faf 342
javierjsp 0:cd0af1b08faf 343 void posicion(int pos)
javierjsp 0:cd0af1b08faf 344 {
javierjsp 0:cd0af1b08faf 345 frenar(0); // nos aseguramos de que los motores estan parados
javierjsp 0:cd0af1b08faf 346 pulsos[1]=enc1.getPulses(); // leemos los valores de los conadores de los encoders
javierjsp 0:cd0af1b08faf 347 pulsos[2]=enc2.getPulses();
javierjsp 0:cd0af1b08faf 348 pulsos[3]=enc3.getPulses();
javierjsp 0:cd0af1b08faf 349 pulsos[4]=enc4.getPulses();
javierjsp 0:cd0af1b08faf 350 pulsos[5]=enc5.getPulses();
javierjsp 0:cd0af1b08faf 351 int dir[6]={0,0,0,0,0,0}; // declaramos un array para las direcciones
javierjsp 0:cd0af1b08faf 352 int flag[6]={1,1,1,1,1,1}; // declaramos un array para banderas
javierjsp 0:cd0af1b08faf 353 int i;
javierjsp 0:cd0af1b08faf 354 for (i=1;i<6;i++) // establecemos la direccion a la que tiene que ir cada motor
javierjsp 0:cd0af1b08faf 355 {
javierjsp 0:cd0af1b08faf 356 if (posiciones[pos][i]<pulsos[i])
javierjsp 0:cd0af1b08faf 357 dir[i]=-1;
javierjsp 0:cd0af1b08faf 358 else if (posiciones[pos][i]>pulsos[i])
javierjsp 0:cd0af1b08faf 359 dir[i]=1;
javierjsp 0:cd0af1b08faf 360 else if (posiciones[pos][i]==pulsos[i])
javierjsp 0:cd0af1b08faf 361 flag[i]=0;
javierjsp 0:cd0af1b08faf 362 }
javierjsp 0:cd0af1b08faf 363 while (flag[1]+flag[2]+flag[3]+flag[4]+flag[5]) // mientras que alguna de las banderas este activada:
javierjsp 0:cd0af1b08faf 364 {
javierjsp 0:cd0af1b08faf 365 for (i=1;i<6;i++) // repetimos para cada uno de los motores
javierjsp 0:cd0af1b08faf 366 {
javierjsp 0:cd0af1b08faf 367 if (flag[i]==1) // si la bandera del motor i esta activa
javierjsp 0:cd0af1b08faf 368 {
javierjsp 0:cd0af1b08faf 369 mover(i,dir[i],V); // movemos el motor en la direccion establecida
javierjsp 0:cd0af1b08faf 370 if (dir[i]==1 && pulsos[i]>=posiciones[pos][i]) // si ha llegado a la posicion deseada
javierjsp 0:cd0af1b08faf 371 {
javierjsp 0:cd0af1b08faf 372 frenar(i); // frenamos el motor i
javierjsp 0:cd0af1b08faf 373 flag[i]=0; // desactivamos su bandera
javierjsp 0:cd0af1b08faf 374 }
javierjsp 0:cd0af1b08faf 375 if (dir[i]==-1 && pulsos[i]<=posiciones[pos][i]) // si ha llegado a la posicion deseada
javierjsp 0:cd0af1b08faf 376 {
javierjsp 0:cd0af1b08faf 377 frenar(i); // frenamos el motor i
javierjsp 0:cd0af1b08faf 378 flag[i]=0; // desactivamos su bandera
javierjsp 0:cd0af1b08faf 379 }
javierjsp 0:cd0af1b08faf 380 }
javierjsp 0:cd0af1b08faf 381 }
javierjsp 0:cd0af1b08faf 382 pulsos[1]=enc1.getPulses(); // leemos la nueva posicion de los encoders
javierjsp 0:cd0af1b08faf 383 pulsos[2]=enc2.getPulses();
javierjsp 0:cd0af1b08faf 384 pulsos[3]=enc3.getPulses();
javierjsp 0:cd0af1b08faf 385 pulsos[4]=enc4.getPulses();
javierjsp 0:cd0af1b08faf 386 pulsos[5]=enc5.getPulses();
javierjsp 0:cd0af1b08faf 387 }
javierjsp 0:cd0af1b08faf 388 wait(0.1); // esperamos 0.1s
javierjsp 0:cd0af1b08faf 389 }
javierjsp 0:cd0af1b08faf 390
javierjsp 0:cd0af1b08faf 391 void mover(int m, int dir, float v)
javierjsp 0:cd0af1b08faf 392 {
javierjsp 0:cd0af1b08faf 393 if((dir==1 || dir==(-1))&&(v>0 && v<=1)) //comprueva que los parametros pasados estan dentro de los valores deseados
javierjsp 0:cd0af1b08faf 394 {
javierjsp 0:cd0af1b08faf 395 switch (m)
javierjsp 0:cd0af1b08faf 396 {
javierjsp 0:cd0af1b08faf 397 case 0: // en caso de que el valor sea 0 movemos todos los motores
javierjsp 0:cd0af1b08faf 398 m1.speed(dir*v);
javierjsp 0:cd0af1b08faf 399 m2.speed(dir*v);
javierjsp 0:cd0af1b08faf 400 m3.speed(dir*v);
javierjsp 0:cd0af1b08faf 401 m4.speed(dir*v);
javierjsp 0:cd0af1b08faf 402 m5.speed(dir*v);
javierjsp 0:cd0af1b08faf 403 break;
javierjsp 0:cd0af1b08faf 404 case 1: // en caso de que el valor sea 1 movemos el motor 1 (base)
javierjsp 0:cd0af1b08faf 405 m1.speed(dir*v);
javierjsp 0:cd0af1b08faf 406 break;
javierjsp 0:cd0af1b08faf 407 case 2: // en caso de que el valor sea 2 movemos el motor 2 (hombro)
javierjsp 0:cd0af1b08faf 408 m2.speed(dir*v);
javierjsp 0:cd0af1b08faf 409 break;
javierjsp 0:cd0af1b08faf 410 case 3: // en caso de que el valor sea 3 movemos el motor 3 (codo)
javierjsp 0:cd0af1b08faf 411 m3.speed(dir*v);
javierjsp 0:cd0af1b08faf 412 break;
javierjsp 0:cd0af1b08faf 413 case 4: // en caso de que el valor sea 4 movemos el motor 4
javierjsp 0:cd0af1b08faf 414 m4.speed(dir*v);
javierjsp 0:cd0af1b08faf 415 break;
javierjsp 0:cd0af1b08faf 416 case 5: // en caso de que el valor sea 5 movemos el motor 5
javierjsp 0:cd0af1b08faf 417 m5.speed(dir*v);
javierjsp 0:cd0af1b08faf 418 break;
javierjsp 0:cd0af1b08faf 419 case 6: // en caso de que el valor sea 6 movemos el motor 6 (pinza)
javierjsp 0:cd0af1b08faf 420 m6.speed(dir*v);
javierjsp 0:cd0af1b08faf 421 break;
javierjsp 4:ceef4dee8284 422 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 0:cd0af1b08faf 423 m4.speed((-1)*dir*v);
javierjsp 0:cd0af1b08faf 424 m5.speed(dir*v);
javierjsp 0:cd0af1b08faf 425 break;
javierjsp 4:ceef4dee8284 426 case 15: // en caso de que el valor sea 15 movemos los motores 4 y 5 en el mismo sentido (rotar muñeca)
javierjsp 0:cd0af1b08faf 427 m4.speed(dir*v);
javierjsp 0:cd0af1b08faf 428 m5.speed(dir*v);
javierjsp 0:cd0af1b08faf 429 }
javierjsp 0:cd0af1b08faf 430 }
javierjsp 0:cd0af1b08faf 431 else // si los parametros no son correctos encendemos el LED3
javierjsp 0:cd0af1b08faf 432 led3=1;
javierjsp 0:cd0af1b08faf 433 }
javierjsp 0:cd0af1b08faf 434
javierjsp 0:cd0af1b08faf 435 void pinza(int a)
javierjsp 0:cd0af1b08faf 436 {
javierjsp 0:cd0af1b08faf 437 int enc=0, encOld=0;
javierjsp 0:cd0af1b08faf 438 mover(6,a,1); // movemos la pinza
javierjsp 0:cd0af1b08faf 439 encOld=enc6.getPulses(); // leemos la posicion del encoder
javierjsp 0:cd0af1b08faf 440 wait(0.1); // espera 0.1s
javierjsp 0:cd0af1b08faf 441 enc=enc6.getPulses(); // leemos la posicion nueva del encoder
javierjsp 0:cd0af1b08faf 442 while(enc != encOld) // mientras la posicion antigua sea distinta de la nueva seguimos leyendo
javierjsp 0:cd0af1b08faf 443 {
javierjsp 0:cd0af1b08faf 444 wait(0.1); // espera 0.1s
javierjsp 0:cd0af1b08faf 445 encOld=enc; // guardamos la posicion antigua del encoder
javierjsp 0:cd0af1b08faf 446 enc=enc6.getPulses(); // leemos la posicion nueva del encoder
javierjsp 0:cd0af1b08faf 447 }
javierjsp 0:cd0af1b08faf 448 frenar(6); // cuando la pinza ha llegado al tope frenamos la pinza
javierjsp 0:cd0af1b08faf 449 }
javierjsp 0:cd0af1b08faf 450
javierjsp 0:cd0af1b08faf 451 void frenar(int m)
javierjsp 0:cd0af1b08faf 452 {
javierjsp 0:cd0af1b08faf 453 switch(m)
javierjsp 0:cd0af1b08faf 454 {
javierjsp 0:cd0af1b08faf 455 case 0: // en caso de que el valor sea 0 frenamos todos los motores
javierjsp 0:cd0af1b08faf 456 m1.brake();
javierjsp 0:cd0af1b08faf 457 m2.brake();
javierjsp 0:cd0af1b08faf 458 m3.brake();
javierjsp 0:cd0af1b08faf 459 m4.brake();
javierjsp 0:cd0af1b08faf 460 m5.brake();
javierjsp 0:cd0af1b08faf 461 m6.brake();
javierjsp 0:cd0af1b08faf 462 break;
javierjsp 0:cd0af1b08faf 463 case 1: // en caso de que el valor sea 1 frenamos el motor 1
javierjsp 0:cd0af1b08faf 464 m1.brake();
javierjsp 0:cd0af1b08faf 465 break;
javierjsp 0:cd0af1b08faf 466 case 2: // en caso de que el valor sea 2 frenamos el motor 2
javierjsp 0:cd0af1b08faf 467 m2.brake();
javierjsp 0:cd0af1b08faf 468 break;
javierjsp 0:cd0af1b08faf 469 case 3: // en caso de que el valor sea 3 frenamos el motor 3
javierjsp 0:cd0af1b08faf 470 m3.brake();
javierjsp 0:cd0af1b08faf 471 break;
javierjsp 0:cd0af1b08faf 472 case 4: // en caso de que el valor sea 4 frenamos el motor 4
javierjsp 0:cd0af1b08faf 473 m4.brake();
javierjsp 0:cd0af1b08faf 474 break;
javierjsp 0:cd0af1b08faf 475 case 5: // en caso de que el valor sea 5 frenamos el motor 5
javierjsp 0:cd0af1b08faf 476 m5.brake();
javierjsp 0:cd0af1b08faf 477 break;
javierjsp 0:cd0af1b08faf 478 case 6: // en caso de que el valor sea 6 frenamos el motor 6
javierjsp 0:cd0af1b08faf 479 m6.brake();
javierjsp 0:cd0af1b08faf 480 break;
javierjsp 4:ceef4dee8284 481 case 14: // en caso de que el valor sea 4 frenamos los motores 4 y 5
javierjsp 0:cd0af1b08faf 482 m4.brake();
javierjsp 0:cd0af1b08faf 483 m5.brake();
javierjsp 0:cd0af1b08faf 484 break;
javierjsp 4:ceef4dee8284 485 case 15: // en caso de que el valor sea 5 frenamos los motores 4 y 5
javierjsp 0:cd0af1b08faf 486 m4.brake();
javierjsp 0:cd0af1b08faf 487 m5.brake();
javierjsp 0:cd0af1b08faf 488 }
javierjsp 0:cd0af1b08faf 489 }
javierjsp 0:cd0af1b08faf 490 void movCinta()
javierjsp 0:cd0af1b08faf 491 {
javierjsp 0:cd0af1b08faf 492 while (sensor==1) // mientras el sensor no se active:
javierjsp 0:cd0af1b08faf 493 cinta.speed(1); // avanza la cinta
javierjsp 0:cd0af1b08faf 494 cinta.brake(); // cuando se ectiva la cinta se para el sensor
javierjsp 4:ceef4dee8284 495 }
javierjsp 4:ceef4dee8284 496
javierjsp 4:ceef4dee8284 497 void calefactor()
javierjsp 4:ceef4dee8284 498 {
javierjsp 5:ae0bdb3aad14 499 while (map(sonda.read(),0,1,-10,50) < 40) // mientras la temperatura sea menor que 40º activa el calefactor
javierjsp 4:ceef4dee8284 500 calef.write(1);
javierjsp 4:ceef4dee8284 501 calef.write(0);
javierjsp 4:ceef4dee8284 502 }
javierjsp 4:ceef4dee8284 503