Javier Sanchez Peña / Mbed 2 deprecated bluetooth_brazo_robotico

control del brazo robotico por bluetooth mediante un dispositivo android

Dependencies:   Motor QEI mbed

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?

UserRevisionLine numberNew 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