Javier Sanchez Peña
/
bluetooth_brazo_robotico
control del brazo robotico por bluetooth mediante un dispositivo android
main.cpp@1:cc623c325046, 2017-06-09 (annotated)
- Committer:
- javierjsp
- Date:
- Fri Jun 09 00:42:22 2017 +0000
- Revision:
- 1:cc623c325046
- Parent:
- 0:8110163d3863
peque?os retoques
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
javierjsp | 0:8110163d3863 | 1 | #include "mbed.h" |
javierjsp | 0:8110163d3863 | 2 | #include "Motor.h" |
javierjsp | 0:8110163d3863 | 3 | #include "QEI.h" |
javierjsp | 0:8110163d3863 | 4 | |
javierjsp | 0:8110163d3863 | 5 | // declaramos los encoders |
javierjsp | 0:8110163d3863 | 6 | QEI enc1(PG_4, PG_6, NC, 480, QEI::X4_ENCODING); // (P0, P1, indice, PulsosPorRev, TipoDeEncoder) |
javierjsp | 0:8110163d3863 | 7 | QEI enc2(PG_7, PG_5, NC, 480, QEI::X4_ENCODING); |
javierjsp | 0:8110163d3863 | 8 | QEI enc3(PD_10, PG_8, NC, 480, QEI::X4_ENCODING); |
javierjsp | 0:8110163d3863 | 9 | QEI enc4(PG_14, PF_11, NC, 480, QEI::X4_ENCODING); |
javierjsp | 0:8110163d3863 | 10 | QEI enc5(PF_12, PF_15, NC, 480, QEI::X4_ENCODING); |
javierjsp | 0:8110163d3863 | 11 | QEI enc6(PF_13, PF_3, NC, 240, QEI::X4_ENCODING); |
javierjsp | 0:8110163d3863 | 12 | |
javierjsp | 0:8110163d3863 | 13 | // declaramos los motores |
javierjsp | 0:8110163d3863 | 14 | Motor m1(PA_5, PC_8, PC_6); // (pwm, derecho, inverso) |
javierjsp | 0:8110163d3863 | 15 | Motor m2(PA_6, PC_5, PD_8); |
javierjsp | 0:8110163d3863 | 16 | Motor m3(PA_7, PA_12, PA_11); |
javierjsp | 0:8110163d3863 | 17 | Motor m4(PB_6, PB_12, PB_11); |
javierjsp | 0:8110163d3863 | 18 | Motor m5(PC_7, PB_2, PB_1); |
javierjsp | 0:8110163d3863 | 19 | Motor m6(PB_9, PB_15, PB_13); |
javierjsp | 0:8110163d3863 | 20 | Motor cinta(PB_8, PC_4, PF_5); |
javierjsp | 0:8110163d3863 | 21 | |
javierjsp | 0:8110163d3863 | 22 | // declaramos las entradas de los micropulsadores |
javierjsp | 0:8110163d3863 | 23 | DigitalIn mp1(PA_9, PullUp); // (pin, modo) |
javierjsp | 0:8110163d3863 | 24 | DigitalIn mp2(PA_8, PullUp); |
javierjsp | 0:8110163d3863 | 25 | DigitalIn mp3(PB_10, PullUp); |
javierjsp | 0:8110163d3863 | 26 | DigitalIn mp4(PB_4, PullUp); |
javierjsp | 0:8110163d3863 | 27 | DigitalIn mp5(PB_5, PullUp); |
javierjsp | 0:8110163d3863 | 28 | DigitalIn sensor(PB_3, PullUp); |
javierjsp | 0:8110163d3863 | 29 | |
javierjsp | 0:8110163d3863 | 30 | // declaramos el bluetooth |
javierjsp | 0:8110163d3863 | 31 | Serial bt(PA_2, PA_3); // (RX, TX) (puerto serial 2) |
javierjsp | 0:8110163d3863 | 32 | |
javierjsp | 0:8110163d3863 | 33 | // declaramos otras variables que utilizaremos |
javierjsp | 0:8110163d3863 | 34 | Ticker ticker1; // iniciamos tickers |
javierjsp | 0:8110163d3863 | 35 | DigitalOut led1 (LED1); // declaramos el LED1 |
javierjsp | 0:8110163d3863 | 36 | DigitalOut led2 (LED2); // declaramos el LED2 |
javierjsp | 0:8110163d3863 | 37 | DigitalOut led3 (LED3); // declaramos el LED3 |
javierjsp | 0:8110163d3863 | 38 | int pulsos[6]={0,0,0,0,0,0}; // iniciamos el array pulsos |
javierjsp | 0:8110163d3863 | 39 | int posiciones[10]; // declaramos el array de posiciones |
javierjsp | 0:8110163d3863 | 40 | char dato=0; |
javierjsp | 0:8110163d3863 | 41 | |
javierjsp | 0:8110163d3863 | 42 | // declaramos el prototipo de las funciones que utilizaremos |
javierjsp | 0:8110163d3863 | 43 | void enviar_bt(); // prototipo de la funcion que ejecutara el ticker1 |
javierjsp | 0:8110163d3863 | 44 | void posicion(); // prototipo de la funcion que nos llevara hasta la posicion especificada |
javierjsp | 0:8110163d3863 | 45 | void mover(int,int,float); // prototipo de la funcion que movera cada motor |
javierjsp | 0:8110163d3863 | 46 | void frenar(int); // prototipo de la funcion que frenara cada motor |
javierjsp | 0:8110163d3863 | 47 | void pinza(int); // prototipo de la funcion que abrira (-1) o cerrara (1) la pinza |
javierjsp | 1:cc623c325046 | 48 | void movCinta(); // prototipo de la funcion que avanzara la cinta hasta detectar un objeto |
javierjsp | 0:8110163d3863 | 49 | |
javierjsp | 0:8110163d3863 | 50 | int main() |
javierjsp | 0:8110163d3863 | 51 | { |
javierjsp | 1:cc623c325046 | 52 | m1.period(0.0001); // establece el periodo del PWM, equivalente a una frecuencia de 10KHz. |
javierjsp | 1:cc623c325046 | 53 | m2.period(0.0001); |
javierjsp | 1:cc623c325046 | 54 | m3.period(0.0001); |
javierjsp | 1:cc623c325046 | 55 | m4.period(0.0001); |
javierjsp | 1:cc623c325046 | 56 | m5.period(0.0001); |
javierjsp | 1:cc623c325046 | 57 | m6.period(0.0001); |
javierjsp | 0:8110163d3863 | 58 | |
javierjsp | 0:8110163d3863 | 59 | bt.baud(9600); // velocidad de transmision del bluetooth a 9600 baudios |
javierjsp | 0:8110163d3863 | 60 | |
javierjsp | 1:cc623c325046 | 61 | 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 | 0:8110163d3863 | 62 | |
javierjsp | 0:8110163d3863 | 63 | while(1) |
javierjsp | 0:8110163d3863 | 64 | { |
javierjsp | 0:8110163d3863 | 65 | if(bt.readable()>0) |
javierjsp | 0:8110163d3863 | 66 | { |
javierjsp | 0:8110163d3863 | 67 | dato = bt.getc(); |
javierjsp | 0:8110163d3863 | 68 | switch(dato) |
javierjsp | 0:8110163d3863 | 69 | { |
javierjsp | 0:8110163d3863 | 70 | case 'A': |
javierjsp | 0:8110163d3863 | 71 | mover(1,1,1); |
javierjsp | 0:8110163d3863 | 72 | wait(0.2); |
javierjsp | 0:8110163d3863 | 73 | frenar(1); |
javierjsp | 0:8110163d3863 | 74 | break; |
javierjsp | 0:8110163d3863 | 75 | case 'B': |
javierjsp | 0:8110163d3863 | 76 | mover(1,-1,1); |
javierjsp | 0:8110163d3863 | 77 | wait(0.2); |
javierjsp | 0:8110163d3863 | 78 | frenar(1); |
javierjsp | 0:8110163d3863 | 79 | break; |
javierjsp | 0:8110163d3863 | 80 | case 'C': |
javierjsp | 0:8110163d3863 | 81 | mover(2,1,1); |
javierjsp | 0:8110163d3863 | 82 | wait(0.2); |
javierjsp | 0:8110163d3863 | 83 | frenar(2); |
javierjsp | 0:8110163d3863 | 84 | break; |
javierjsp | 0:8110163d3863 | 85 | case 'D': |
javierjsp | 0:8110163d3863 | 86 | mover(2,-1,1); |
javierjsp | 0:8110163d3863 | 87 | wait(0.2); |
javierjsp | 0:8110163d3863 | 88 | frenar(2); |
javierjsp | 0:8110163d3863 | 89 | break; |
javierjsp | 0:8110163d3863 | 90 | case 'E': |
javierjsp | 0:8110163d3863 | 91 | mover(3,1,1); |
javierjsp | 0:8110163d3863 | 92 | wait(0.2); |
javierjsp | 0:8110163d3863 | 93 | frenar(3); |
javierjsp | 0:8110163d3863 | 94 | break; |
javierjsp | 0:8110163d3863 | 95 | case 'F': |
javierjsp | 0:8110163d3863 | 96 | mover(3,-1,1); |
javierjsp | 0:8110163d3863 | 97 | wait(0.2); |
javierjsp | 0:8110163d3863 | 98 | frenar(3); |
javierjsp | 0:8110163d3863 | 99 | break; |
javierjsp | 0:8110163d3863 | 100 | case 'G': |
javierjsp | 0:8110163d3863 | 101 | mover(4,1,1); |
javierjsp | 0:8110163d3863 | 102 | wait(0.2); |
javierjsp | 0:8110163d3863 | 103 | frenar(4); |
javierjsp | 0:8110163d3863 | 104 | break; |
javierjsp | 0:8110163d3863 | 105 | case 'H': |
javierjsp | 0:8110163d3863 | 106 | mover(4,-1,1); |
javierjsp | 0:8110163d3863 | 107 | wait(0.2); |
javierjsp | 0:8110163d3863 | 108 | frenar(4); |
javierjsp | 0:8110163d3863 | 109 | break; |
javierjsp | 0:8110163d3863 | 110 | case 'I': |
javierjsp | 0:8110163d3863 | 111 | mover(5,1,1); |
javierjsp | 0:8110163d3863 | 112 | wait(0.2); |
javierjsp | 0:8110163d3863 | 113 | frenar(5); |
javierjsp | 0:8110163d3863 | 114 | break; |
javierjsp | 0:8110163d3863 | 115 | case 'J': |
javierjsp | 0:8110163d3863 | 116 | mover(5,-1,1); |
javierjsp | 0:8110163d3863 | 117 | wait(0.2); |
javierjsp | 0:8110163d3863 | 118 | frenar(5); |
javierjsp | 0:8110163d3863 | 119 | break; |
javierjsp | 0:8110163d3863 | 120 | case 'K': |
javierjsp | 0:8110163d3863 | 121 | pinza(-1); |
javierjsp | 0:8110163d3863 | 122 | break; |
javierjsp | 0:8110163d3863 | 123 | case 'L': |
javierjsp | 0:8110163d3863 | 124 | pinza(1); |
javierjsp | 0:8110163d3863 | 125 | break; |
javierjsp | 1:cc623c325046 | 126 | case 'M': |
javierjsp | 1:cc623c325046 | 127 | movCinta(); |
javierjsp | 1:cc623c325046 | 128 | break; |
javierjsp | 0:8110163d3863 | 129 | } |
javierjsp | 0:8110163d3863 | 130 | } |
javierjsp | 0:8110163d3863 | 131 | } |
javierjsp | 0:8110163d3863 | 132 | } |
javierjsp | 0:8110163d3863 | 133 | |
javierjsp | 0:8110163d3863 | 134 | void enviar_bt() // definimos la funcion que ejecutara el ticker |
javierjsp | 0:8110163d3863 | 135 | { |
javierjsp | 0:8110163d3863 | 136 | pulsos[1]=enc1.getPulses(); |
javierjsp | 0:8110163d3863 | 137 | pulsos[2]=enc2.getPulses(); |
javierjsp | 0:8110163d3863 | 138 | pulsos[3]=enc3.getPulses(); |
javierjsp | 0:8110163d3863 | 139 | pulsos[4]=enc4.getPulses(); |
javierjsp | 0:8110163d3863 | 140 | pulsos[5]=enc5.getPulses(); |
javierjsp | 0:8110163d3863 | 141 | 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:8110163d3863 | 142 | } |
javierjsp | 0:8110163d3863 | 143 | |
javierjsp | 0:8110163d3863 | 144 | void posicion(int pos) |
javierjsp | 0:8110163d3863 | 145 | { |
javierjsp | 0:8110163d3863 | 146 | |
javierjsp | 0:8110163d3863 | 147 | } |
javierjsp | 0:8110163d3863 | 148 | |
javierjsp | 0:8110163d3863 | 149 | void mover(int m, int dir, float v) |
javierjsp | 0:8110163d3863 | 150 | { |
javierjsp | 0:8110163d3863 | 151 | if((dir==1 || dir==(-1))&&(v>0 && v<=1)) //comprueva que los parametros pasados estan dentro de los valores deseados |
javierjsp | 0:8110163d3863 | 152 | { |
javierjsp | 0:8110163d3863 | 153 | switch (m) |
javierjsp | 0:8110163d3863 | 154 | { |
javierjsp | 0:8110163d3863 | 155 | case 0: // en caso de que el valor sea 0 movemos todos los motores |
javierjsp | 0:8110163d3863 | 156 | m1.speed(dir*v); |
javierjsp | 0:8110163d3863 | 157 | m2.speed(dir*v); |
javierjsp | 0:8110163d3863 | 158 | m3.speed(dir*v); |
javierjsp | 0:8110163d3863 | 159 | m4.speed(dir*v); |
javierjsp | 0:8110163d3863 | 160 | m5.speed(dir*v); |
javierjsp | 0:8110163d3863 | 161 | break; |
javierjsp | 0:8110163d3863 | 162 | case 1: // en caso de que el valor sea 1 movemos el motor 1 (base) |
javierjsp | 0:8110163d3863 | 163 | m1.speed(dir*v); |
javierjsp | 0:8110163d3863 | 164 | break; |
javierjsp | 0:8110163d3863 | 165 | case 2: // en caso de que el valor sea 2 movemos el motor 2 (hombro) |
javierjsp | 0:8110163d3863 | 166 | m2.speed(dir*v); |
javierjsp | 0:8110163d3863 | 167 | break; |
javierjsp | 0:8110163d3863 | 168 | case 3: // en caso de que el valor sea 3 movemos el motor 3 (codo) |
javierjsp | 0:8110163d3863 | 169 | m3.speed(dir*v); |
javierjsp | 0:8110163d3863 | 170 | break; |
javierjsp | 0:8110163d3863 | 171 | 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:8110163d3863 | 172 | m4.speed((-1)*dir*v); |
javierjsp | 0:8110163d3863 | 173 | m5.speed(dir*v); |
javierjsp | 0:8110163d3863 | 174 | break; |
javierjsp | 0:8110163d3863 | 175 | 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:8110163d3863 | 176 | m4.speed(dir*v); |
javierjsp | 0:8110163d3863 | 177 | m5.speed(dir*v); |
javierjsp | 0:8110163d3863 | 178 | break; |
javierjsp | 0:8110163d3863 | 179 | case 6: // en caso de que el valor sea 6 movemos el motor 6 (pinza) |
javierjsp | 0:8110163d3863 | 180 | m6.speed(dir*v); |
javierjsp | 0:8110163d3863 | 181 | break; |
javierjsp | 0:8110163d3863 | 182 | } |
javierjsp | 0:8110163d3863 | 183 | } |
javierjsp | 0:8110163d3863 | 184 | else // si los parametros no son correctos encendemos el LED3 |
javierjsp | 0:8110163d3863 | 185 | led3=1; |
javierjsp | 0:8110163d3863 | 186 | } |
javierjsp | 0:8110163d3863 | 187 | |
javierjsp | 0:8110163d3863 | 188 | void pinza(int a) |
javierjsp | 0:8110163d3863 | 189 | { |
javierjsp | 0:8110163d3863 | 190 | int enc=0, encOld=0; |
javierjsp | 0:8110163d3863 | 191 | mover(6,a,1); // movemos la pinza |
javierjsp | 0:8110163d3863 | 192 | encOld=enc6.getPulses(); // leemos la posicion del encoder |
javierjsp | 0:8110163d3863 | 193 | wait(0.1); // espera 0.1s |
javierjsp | 0:8110163d3863 | 194 | enc=enc6.getPulses(); // leemos la posicion nueva del encoder |
javierjsp | 0:8110163d3863 | 195 | while(enc != encOld) // mientras la posicion antigua sea distinta de la nueva seguimos leyendo |
javierjsp | 0:8110163d3863 | 196 | { |
javierjsp | 0:8110163d3863 | 197 | wait(0.1); // espera 0.1s |
javierjsp | 0:8110163d3863 | 198 | encOld=enc; // guardamos la posicion antigua del encoder |
javierjsp | 0:8110163d3863 | 199 | enc=enc6.getPulses(); // leemos la posicion nueva del encoder |
javierjsp | 0:8110163d3863 | 200 | } |
javierjsp | 0:8110163d3863 | 201 | frenar(6); // cuando la pinza ha llegado al tope frenamos la pinza |
javierjsp | 0:8110163d3863 | 202 | } |
javierjsp | 0:8110163d3863 | 203 | |
javierjsp | 0:8110163d3863 | 204 | void frenar(int m) |
javierjsp | 0:8110163d3863 | 205 | { |
javierjsp | 0:8110163d3863 | 206 | switch(m) |
javierjsp | 0:8110163d3863 | 207 | { |
javierjsp | 0:8110163d3863 | 208 | case 0: // en caso de que el valor sea 0 frenamos todos los motores |
javierjsp | 0:8110163d3863 | 209 | m1.brake(); |
javierjsp | 0:8110163d3863 | 210 | m2.brake(); |
javierjsp | 0:8110163d3863 | 211 | m3.brake(); |
javierjsp | 0:8110163d3863 | 212 | m4.brake(); |
javierjsp | 0:8110163d3863 | 213 | m5.brake(); |
javierjsp | 0:8110163d3863 | 214 | m6.brake(); |
javierjsp | 0:8110163d3863 | 215 | break; |
javierjsp | 0:8110163d3863 | 216 | case 1: // en caso de que el valor sea 1 frenamos el motor 1 |
javierjsp | 0:8110163d3863 | 217 | m1.brake(); |
javierjsp | 0:8110163d3863 | 218 | break; |
javierjsp | 0:8110163d3863 | 219 | case 2: // en caso de que el valor sea 2 frenamos el motor 2 |
javierjsp | 0:8110163d3863 | 220 | m2.brake(); |
javierjsp | 0:8110163d3863 | 221 | break; |
javierjsp | 0:8110163d3863 | 222 | case 3: // en caso de que el valor sea 3 frenamos el motor 3 |
javierjsp | 0:8110163d3863 | 223 | m3.brake(); |
javierjsp | 0:8110163d3863 | 224 | break; |
javierjsp | 0:8110163d3863 | 225 | case 4: // en caso de que el valor sea 4 frenamos los motores 4 y 5 |
javierjsp | 0:8110163d3863 | 226 | m4.brake(); |
javierjsp | 0:8110163d3863 | 227 | m5.brake(); |
javierjsp | 0:8110163d3863 | 228 | break; |
javierjsp | 0:8110163d3863 | 229 | case 5: // en caso de que el valor sea 5 frenamos los motores 4 y 5 |
javierjsp | 0:8110163d3863 | 230 | m4.brake(); |
javierjsp | 0:8110163d3863 | 231 | m5.brake(); |
javierjsp | 0:8110163d3863 | 232 | break; |
javierjsp | 0:8110163d3863 | 233 | case 6: // en caso de que el valor sea 6 frenamos el motor 6 |
javierjsp | 0:8110163d3863 | 234 | m6.brake(); |
javierjsp | 0:8110163d3863 | 235 | break; |
javierjsp | 0:8110163d3863 | 236 | } |
javierjsp | 0:8110163d3863 | 237 | } |
javierjsp | 1:cc623c325046 | 238 | void movCinta() |
javierjsp | 1:cc623c325046 | 239 | { |
javierjsp | 1:cc623c325046 | 240 | while (sensor==1) |
javierjsp | 1:cc623c325046 | 241 | cinta.speed(1); |
javierjsp | 1:cc623c325046 | 242 | cinta.brake(); |
javierjsp | 1:cc623c325046 | 243 | } |
javierjsp | 0:8110163d3863 | 244 | |
javierjsp | 0:8110163d3863 | 245 | |
javierjsp | 1:cc623c325046 | 246 |