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@0:8110163d3863, 2017-06-08 (annotated)

Committer:
javierjsp
Date:
Thu Jun 08 20:59:13 2017 +0000
Revision:
0:8110163d3863
Child:
1:cc623c325046
version funcional

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 0:8110163d3863 48
javierjsp 0:8110163d3863 49 int main()
javierjsp 0:8110163d3863 50 {
javierjsp 0:8110163d3863 51 m1.period(0.001); // establece el periodo del PWM, equivalente a una frecuencia de 10KHz.
javierjsp 0:8110163d3863 52 m2.period(0.001);
javierjsp 0:8110163d3863 53 m3.period(0.001);
javierjsp 0:8110163d3863 54 m4.period(0.001);
javierjsp 0:8110163d3863 55 m5.period(0.001);
javierjsp 0:8110163d3863 56 m6.period(0.001);
javierjsp 0:8110163d3863 57
javierjsp 0:8110163d3863 58 bt.baud(9600); // velocidad de transmision del bluetooth a 9600 baudios
javierjsp 0:8110163d3863 59
javierjsp 0:8110163d3863 60 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 61
javierjsp 0:8110163d3863 62 while(1)
javierjsp 0:8110163d3863 63 {
javierjsp 0:8110163d3863 64 if(bt.readable()>0)
javierjsp 0:8110163d3863 65 {
javierjsp 0:8110163d3863 66 dato = bt.getc();
javierjsp 0:8110163d3863 67 switch(dato)
javierjsp 0:8110163d3863 68 {
javierjsp 0:8110163d3863 69 case 'A':
javierjsp 0:8110163d3863 70 mover(1,1,1);
javierjsp 0:8110163d3863 71 wait(0.2);
javierjsp 0:8110163d3863 72 frenar(1);
javierjsp 0:8110163d3863 73 break;
javierjsp 0:8110163d3863 74 case 'B':
javierjsp 0:8110163d3863 75 mover(1,-1,1);
javierjsp 0:8110163d3863 76 wait(0.2);
javierjsp 0:8110163d3863 77 frenar(1);
javierjsp 0:8110163d3863 78 break;
javierjsp 0:8110163d3863 79 case 'C':
javierjsp 0:8110163d3863 80 mover(2,1,1);
javierjsp 0:8110163d3863 81 wait(0.2);
javierjsp 0:8110163d3863 82 frenar(2);
javierjsp 0:8110163d3863 83 break;
javierjsp 0:8110163d3863 84 case 'D':
javierjsp 0:8110163d3863 85 mover(2,-1,1);
javierjsp 0:8110163d3863 86 wait(0.2);
javierjsp 0:8110163d3863 87 frenar(2);
javierjsp 0:8110163d3863 88 break;
javierjsp 0:8110163d3863 89 case 'E':
javierjsp 0:8110163d3863 90 mover(3,1,1);
javierjsp 0:8110163d3863 91 wait(0.2);
javierjsp 0:8110163d3863 92 frenar(3);
javierjsp 0:8110163d3863 93 break;
javierjsp 0:8110163d3863 94 case 'F':
javierjsp 0:8110163d3863 95 mover(3,-1,1);
javierjsp 0:8110163d3863 96 wait(0.2);
javierjsp 0:8110163d3863 97 frenar(3);
javierjsp 0:8110163d3863 98 break;
javierjsp 0:8110163d3863 99 case 'G':
javierjsp 0:8110163d3863 100 mover(4,1,1);
javierjsp 0:8110163d3863 101 wait(0.2);
javierjsp 0:8110163d3863 102 frenar(4);
javierjsp 0:8110163d3863 103 break;
javierjsp 0:8110163d3863 104 case 'H':
javierjsp 0:8110163d3863 105 mover(4,-1,1);
javierjsp 0:8110163d3863 106 wait(0.2);
javierjsp 0:8110163d3863 107 frenar(4);
javierjsp 0:8110163d3863 108 break;
javierjsp 0:8110163d3863 109 case 'I':
javierjsp 0:8110163d3863 110 mover(5,1,1);
javierjsp 0:8110163d3863 111 wait(0.2);
javierjsp 0:8110163d3863 112 frenar(5);
javierjsp 0:8110163d3863 113 break;
javierjsp 0:8110163d3863 114 case 'J':
javierjsp 0:8110163d3863 115 mover(5,-1,1);
javierjsp 0:8110163d3863 116 wait(0.2);
javierjsp 0:8110163d3863 117 frenar(5);
javierjsp 0:8110163d3863 118 break;
javierjsp 0:8110163d3863 119 case 'K':
javierjsp 0:8110163d3863 120 pinza(-1);
javierjsp 0:8110163d3863 121 break;
javierjsp 0:8110163d3863 122 case 'L':
javierjsp 0:8110163d3863 123 pinza(1);
javierjsp 0:8110163d3863 124 break;
javierjsp 0:8110163d3863 125 }
javierjsp 0:8110163d3863 126 }
javierjsp 0:8110163d3863 127 }
javierjsp 0:8110163d3863 128 }
javierjsp 0:8110163d3863 129
javierjsp 0:8110163d3863 130 void enviar_bt() // definimos la funcion que ejecutara el ticker
javierjsp 0:8110163d3863 131 {
javierjsp 0:8110163d3863 132 pulsos[1]=enc1.getPulses();
javierjsp 0:8110163d3863 133 pulsos[2]=enc2.getPulses();
javierjsp 0:8110163d3863 134 pulsos[3]=enc3.getPulses();
javierjsp 0:8110163d3863 135 pulsos[4]=enc4.getPulses();
javierjsp 0:8110163d3863 136 pulsos[5]=enc5.getPulses();
javierjsp 0:8110163d3863 137 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 138 }
javierjsp 0:8110163d3863 139
javierjsp 0:8110163d3863 140 void posicion(int pos)
javierjsp 0:8110163d3863 141 {
javierjsp 0:8110163d3863 142
javierjsp 0:8110163d3863 143 }
javierjsp 0:8110163d3863 144
javierjsp 0:8110163d3863 145 void mover(int m, int dir, float v)
javierjsp 0:8110163d3863 146 {
javierjsp 0:8110163d3863 147 if((dir==1 || dir==(-1))&&(v>0 && v<=1)) //comprueva que los parametros pasados estan dentro de los valores deseados
javierjsp 0:8110163d3863 148 {
javierjsp 0:8110163d3863 149 switch (m)
javierjsp 0:8110163d3863 150 {
javierjsp 0:8110163d3863 151 case 0: // en caso de que el valor sea 0 movemos todos los motores
javierjsp 0:8110163d3863 152 m1.speed(dir*v);
javierjsp 0:8110163d3863 153 m2.speed(dir*v);
javierjsp 0:8110163d3863 154 m3.speed(dir*v);
javierjsp 0:8110163d3863 155 m4.speed(dir*v);
javierjsp 0:8110163d3863 156 m5.speed(dir*v);
javierjsp 0:8110163d3863 157 break;
javierjsp 0:8110163d3863 158 case 1: // en caso de que el valor sea 1 movemos el motor 1 (base)
javierjsp 0:8110163d3863 159 m1.speed(dir*v);
javierjsp 0:8110163d3863 160 break;
javierjsp 0:8110163d3863 161 case 2: // en caso de que el valor sea 2 movemos el motor 2 (hombro)
javierjsp 0:8110163d3863 162 m2.speed(dir*v);
javierjsp 0:8110163d3863 163 break;
javierjsp 0:8110163d3863 164 case 3: // en caso de que el valor sea 3 movemos el motor 3 (codo)
javierjsp 0:8110163d3863 165 m3.speed(dir*v);
javierjsp 0:8110163d3863 166 break;
javierjsp 0:8110163d3863 167 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 168 m4.speed((-1)*dir*v);
javierjsp 0:8110163d3863 169 m5.speed(dir*v);
javierjsp 0:8110163d3863 170 break;
javierjsp 0:8110163d3863 171 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 172 m4.speed(dir*v);
javierjsp 0:8110163d3863 173 m5.speed(dir*v);
javierjsp 0:8110163d3863 174 break;
javierjsp 0:8110163d3863 175 case 6: // en caso de que el valor sea 6 movemos el motor 6 (pinza)
javierjsp 0:8110163d3863 176 m6.speed(dir*v);
javierjsp 0:8110163d3863 177 break;
javierjsp 0:8110163d3863 178 }
javierjsp 0:8110163d3863 179 }
javierjsp 0:8110163d3863 180 else // si los parametros no son correctos encendemos el LED3
javierjsp 0:8110163d3863 181 led3=1;
javierjsp 0:8110163d3863 182 }
javierjsp 0:8110163d3863 183
javierjsp 0:8110163d3863 184 void pinza(int a)
javierjsp 0:8110163d3863 185 {
javierjsp 0:8110163d3863 186 int enc=0, encOld=0;
javierjsp 0:8110163d3863 187 mover(6,a,1); // movemos la pinza
javierjsp 0:8110163d3863 188 encOld=enc6.getPulses(); // leemos la posicion del encoder
javierjsp 0:8110163d3863 189 wait(0.1); // espera 0.1s
javierjsp 0:8110163d3863 190 enc=enc6.getPulses(); // leemos la posicion nueva del encoder
javierjsp 0:8110163d3863 191 while(enc != encOld) // mientras la posicion antigua sea distinta de la nueva seguimos leyendo
javierjsp 0:8110163d3863 192 {
javierjsp 0:8110163d3863 193 wait(0.1); // espera 0.1s
javierjsp 0:8110163d3863 194 encOld=enc; // guardamos la posicion antigua del encoder
javierjsp 0:8110163d3863 195 enc=enc6.getPulses(); // leemos la posicion nueva del encoder
javierjsp 0:8110163d3863 196 }
javierjsp 0:8110163d3863 197 frenar(6); // cuando la pinza ha llegado al tope frenamos la pinza
javierjsp 0:8110163d3863 198 }
javierjsp 0:8110163d3863 199
javierjsp 0:8110163d3863 200 void frenar(int m)
javierjsp 0:8110163d3863 201 {
javierjsp 0:8110163d3863 202 switch(m)
javierjsp 0:8110163d3863 203 {
javierjsp 0:8110163d3863 204 case 0: // en caso de que el valor sea 0 frenamos todos los motores
javierjsp 0:8110163d3863 205 m1.brake();
javierjsp 0:8110163d3863 206 m2.brake();
javierjsp 0:8110163d3863 207 m3.brake();
javierjsp 0:8110163d3863 208 m4.brake();
javierjsp 0:8110163d3863 209 m5.brake();
javierjsp 0:8110163d3863 210 m6.brake();
javierjsp 0:8110163d3863 211 break;
javierjsp 0:8110163d3863 212 case 1: // en caso de que el valor sea 1 frenamos el motor 1
javierjsp 0:8110163d3863 213 m1.brake();
javierjsp 0:8110163d3863 214 break;
javierjsp 0:8110163d3863 215 case 2: // en caso de que el valor sea 2 frenamos el motor 2
javierjsp 0:8110163d3863 216 m2.brake();
javierjsp 0:8110163d3863 217 break;
javierjsp 0:8110163d3863 218 case 3: // en caso de que el valor sea 3 frenamos el motor 3
javierjsp 0:8110163d3863 219 m3.brake();
javierjsp 0:8110163d3863 220 break;
javierjsp 0:8110163d3863 221 case 4: // en caso de que el valor sea 4 frenamos los motores 4 y 5
javierjsp 0:8110163d3863 222 m4.brake();
javierjsp 0:8110163d3863 223 m5.brake();
javierjsp 0:8110163d3863 224 break;
javierjsp 0:8110163d3863 225 case 5: // en caso de que el valor sea 5 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 6: // en caso de que el valor sea 6 frenamos el motor 6
javierjsp 0:8110163d3863 230 m6.brake();
javierjsp 0:8110163d3863 231 break;
javierjsp 0:8110163d3863 232 }
javierjsp 0:8110163d3863 233 }
javierjsp 0:8110163d3863 234
javierjsp 0:8110163d3863 235