Mario Galeano / Mbed 2 deprecated SpiderProgramming

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@0:17f865015626, 2018-11-20 (annotated)

Committer:
Mario_Galeano
Date:
Tue Nov 20 01:45:51 2018 +0000
Revision:
0:17f865015626
Programaci?n proyecto ara?a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Mario_Galeano 0:17f865015626 1 #include "mbed.h"
Mario_Galeano 0:17f865015626 2 #include <Serial.h>
Mario_Galeano 0:17f865015626 3 #include "Color.h"
Mario_Galeano 0:17f865015626 4
Mario_Galeano 0:17f865015626 5 //*****************************************************************************//
Mario_Galeano 0:17f865015626 6 // Universidad ECCI - Ingenieria mecatronica
Mario_Galeano 0:17f865015626 7 // Sistemas embebidos - Control por telecomandos
Mario_Galeano 0:17f865015626 8 // Mario Esteban Galeano Rodriguez
Mario_Galeano 0:17f865015626 9 //*****************************************************************************//
Mario_Galeano 0:17f865015626 10
Mario_Galeano 0:17f865015626 11 //*****************************************************************************//
Mario_Galeano 0:17f865015626 12 //Declaración de variables
Mario_Galeano 0:17f865015626 13
Mario_Galeano 0:17f865015626 14 uint8_t data [3];
Mario_Galeano 0:17f865015626 15 int color, ciclos;
Mario_Galeano 0:17f865015626 16 char status = 0;
Mario_Galeano 0:17f865015626 17 Serial pc(SERIAL_TX, SERIAL_RX, 9600);
Mario_Galeano 0:17f865015626 18
Mario_Galeano 0:17f865015626 19 //*****************************************************************************//
Mario_Galeano 0:17f865015626 20 // Timer para lectura del sensor
Mario_Galeano 0:17f865015626 21
Mario_Galeano 0:17f865015626 22 Timer timeRead;
Mario_Galeano 0:17f865015626 23 Timer timeSerial;
Mario_Galeano 0:17f865015626 24 AnalogIn value_y (A0);
Mario_Galeano 0:17f865015626 25 AnalogIn value_x(A1);
Mario_Galeano 0:17f865015626 26
Mario_Galeano 0:17f865015626 27 float valor_y;
Mario_Galeano 0:17f865015626 28 float valor_x;
Mario_Galeano 0:17f865015626 29
Mario_Galeano 0:17f865015626 30 //*****************************************************************************//
Mario_Galeano 0:17f865015626 31 // Posiciones movimientos
Mario_Galeano 0:17f865015626 32
Mario_Galeano 0:17f865015626 33 int arriba = 5;
Mario_Galeano 0:17f865015626 34 int abajo = 25;
Mario_Galeano 0:17f865015626 35
Mario_Galeano 0:17f865015626 36 //*****************************************************************************//
Mario_Galeano 0:17f865015626 37 // Otras variables
Mario_Galeano 0:17f865015626 38
Mario_Galeano 0:17f865015626 39 int tiempo = 200;
Mario_Galeano 0:17f865015626 40
Mario_Galeano 0:17f865015626 41 //*****************************************************************************//
Mario_Galeano 0:17f865015626 42 // Distribución pines servomotores
Mario_Galeano 0:17f865015626 43
Mario_Galeano 0:17f865015626 44 // Brazo derecho delante
Mario_Galeano 0:17f865015626 45 PwmOut servo_1 (PA_10); //D0
Mario_Galeano 0:17f865015626 46 PwmOut servo_2 (PB_3); //D1
Mario_Galeano 0:17f865015626 47
Mario_Galeano 0:17f865015626 48 // Brazo derecho atras
Mario_Galeano 0:17f865015626 49 PwmOut servo_3 (PB_5); //D8
Mario_Galeano 0:17f865015626 50 PwmOut servo_4 (PB_4); //D9
Mario_Galeano 0:17f865015626 51
Mario_Galeano 0:17f865015626 52 // Brazo izquierdo adelante
Mario_Galeano 0:17f865015626 53 PwmOut servo_5 (PB_10); //D10
Mario_Galeano 0:17f865015626 54 PwmOut servo_6 (PA_8); //D11
Mario_Galeano 0:17f865015626 55
Mario_Galeano 0:17f865015626 56 // Brazo izquierdo atras
Mario_Galeano 0:17f865015626 57 PwmOut servo_7 (PA_9); //D12
Mario_Galeano 0:17f865015626 58 PwmOut servo_8 (PB_6); //D13
Mario_Galeano 0:17f865015626 59
Mario_Galeano 0:17f865015626 60 //*****************************************************************************//
Mario_Galeano 0:17f865015626 61 // Función para el control de servomotores
Mario_Galeano 0:17f865015626 62
Mario_Galeano 0:17f865015626 63 void controlServo(int n_servo, int degrees)
Mario_Galeano 0:17f865015626 64 {
Mario_Galeano 0:17f865015626 65 int p_width = ((degrees*2000)/180)+500;
Mario_Galeano 0:17f865015626 66
Mario_Galeano 0:17f865015626 67 switch (n_servo)
Mario_Galeano 0:17f865015626 68 {
Mario_Galeano 0:17f865015626 69 case 1:
Mario_Galeano 0:17f865015626 70 servo_1.pulsewidth_us(p_width);
Mario_Galeano 0:17f865015626 71 break;
Mario_Galeano 0:17f865015626 72
Mario_Galeano 0:17f865015626 73 case 2:
Mario_Galeano 0:17f865015626 74 servo_2.pulsewidth_us(p_width);
Mario_Galeano 0:17f865015626 75 break;
Mario_Galeano 0:17f865015626 76
Mario_Galeano 0:17f865015626 77 case 3:
Mario_Galeano 0:17f865015626 78 servo_3.pulsewidth_us(p_width);
Mario_Galeano 0:17f865015626 79 break;
Mario_Galeano 0:17f865015626 80
Mario_Galeano 0:17f865015626 81 case 4:
Mario_Galeano 0:17f865015626 82 servo_4.pulsewidth_us(p_width);
Mario_Galeano 0:17f865015626 83 break;
Mario_Galeano 0:17f865015626 84
Mario_Galeano 0:17f865015626 85 case 5:
Mario_Galeano 0:17f865015626 86 servo_5.pulsewidth_us(p_width);
Mario_Galeano 0:17f865015626 87 break;
Mario_Galeano 0:17f865015626 88
Mario_Galeano 0:17f865015626 89 case 6:
Mario_Galeano 0:17f865015626 90 servo_6.pulsewidth_us(p_width);
Mario_Galeano 0:17f865015626 91 break;
Mario_Galeano 0:17f865015626 92
Mario_Galeano 0:17f865015626 93 case 7:
Mario_Galeano 0:17f865015626 94 servo_7.pulsewidth_us(p_width);
Mario_Galeano 0:17f865015626 95 break;
Mario_Galeano 0:17f865015626 96
Mario_Galeano 0:17f865015626 97 case 8:
Mario_Galeano 0:17f865015626 98 servo_8.pulsewidth_us(p_width);
Mario_Galeano 0:17f865015626 99 break;
Mario_Galeano 0:17f865015626 100
Mario_Galeano 0:17f865015626 101 default:
Mario_Galeano 0:17f865015626 102 break;
Mario_Galeano 0:17f865015626 103 }
Mario_Galeano 0:17f865015626 104 }
Mario_Galeano 0:17f865015626 105
Mario_Galeano 0:17f865015626 106 //*****************************************************************************//
Mario_Galeano 0:17f865015626 107 // Funcion para movimiento de los brazos
Mario_Galeano 0:17f865015626 108
Mario_Galeano 0:17f865015626 109 void controlBrazo (int Brazo, int movimiento)
Mario_Galeano 0:17f865015626 110 {
Mario_Galeano 0:17f865015626 111 if (Brazo == 0x01)
Mario_Galeano 0:17f865015626 112 {
Mario_Galeano 0:17f865015626 113 switch (movimiento)
Mario_Galeano 0:17f865015626 114 {
Mario_Galeano 0:17f865015626 115 case 1:// handRightUp
Mario_Galeano 0:17f865015626 116 controlServo(2, arriba-5);
Mario_Galeano 0:17f865015626 117 break;
Mario_Galeano 0:17f865015626 118
Mario_Galeano 0:17f865015626 119 case 2:// handRightDown
Mario_Galeano 0:17f865015626 120 controlServo(2, abajo-5);
Mario_Galeano 0:17f865015626 121 break;
Mario_Galeano 0:17f865015626 122
Mario_Galeano 0:17f865015626 123 case 3:// handRightForward
Mario_Galeano 0:17f865015626 124 controlServo(1, 70);
Mario_Galeano 0:17f865015626 125 break;
Mario_Galeano 0:17f865015626 126
Mario_Galeano 0:17f865015626 127 case 4:// handRightReverse
Mario_Galeano 0:17f865015626 128 controlServo(1, 15);
Mario_Galeano 0:17f865015626 129 break;
Mario_Galeano 0:17f865015626 130
Mario_Galeano 0:17f865015626 131 default:
Mario_Galeano 0:17f865015626 132 break;
Mario_Galeano 0:17f865015626 133 }
Mario_Galeano 0:17f865015626 134 }
Mario_Galeano 0:17f865015626 135
Mario_Galeano 0:17f865015626 136 else if (Brazo == 0x02)
Mario_Galeano 0:17f865015626 137 {
Mario_Galeano 0:17f865015626 138 switch (movimiento)
Mario_Galeano 0:17f865015626 139 {
Mario_Galeano 0:17f865015626 140 case 1:// legRightUp
Mario_Galeano 0:17f865015626 141 controlServo(4, arriba-5);
Mario_Galeano 0:17f865015626 142 break;
Mario_Galeano 0:17f865015626 143
Mario_Galeano 0:17f865015626 144 case 2:// legRightDown
Mario_Galeano 0:17f865015626 145 controlServo(4, abajo-10);
Mario_Galeano 0:17f865015626 146 break;
Mario_Galeano 0:17f865015626 147
Mario_Galeano 0:17f865015626 148 case 3:// legRightForward
Mario_Galeano 0:17f865015626 149 controlServo(3, 150);
Mario_Galeano 0:17f865015626 150 break;
Mario_Galeano 0:17f865015626 151
Mario_Galeano 0:17f865015626 152 case 4:// legRightReverse
Mario_Galeano 0:17f865015626 153 controlServo(3, 95);
Mario_Galeano 0:17f865015626 154 break;
Mario_Galeano 0:17f865015626 155
Mario_Galeano 0:17f865015626 156 default:
Mario_Galeano 0:17f865015626 157 break;
Mario_Galeano 0:17f865015626 158 }
Mario_Galeano 0:17f865015626 159 }
Mario_Galeano 0:17f865015626 160
Mario_Galeano 0:17f865015626 161 else if (Brazo == 0x03)
Mario_Galeano 0:17f865015626 162 {
Mario_Galeano 0:17f865015626 163 switch (movimiento)
Mario_Galeano 0:17f865015626 164 {
Mario_Galeano 0:17f865015626 165 case 1:// handLeftUp
Mario_Galeano 0:17f865015626 166 controlServo(6, arriba);
Mario_Galeano 0:17f865015626 167 break;
Mario_Galeano 0:17f865015626 168
Mario_Galeano 0:17f865015626 169 case 2:// handLeftDown
Mario_Galeano 0:17f865015626 170 controlServo(6, abajo);
Mario_Galeano 0:17f865015626 171 break;
Mario_Galeano 0:17f865015626 172
Mario_Galeano 0:17f865015626 173 case 3://handLeftForward
Mario_Galeano 0:17f865015626 174 controlServo(5, 60);
Mario_Galeano 0:17f865015626 175 break;
Mario_Galeano 0:17f865015626 176
Mario_Galeano 0:17f865015626 177 case 4:// handLeftReverse
Mario_Galeano 0:17f865015626 178 controlServo(5, 118);
Mario_Galeano 0:17f865015626 179 break;
Mario_Galeano 0:17f865015626 180
Mario_Galeano 0:17f865015626 181 default:
Mario_Galeano 0:17f865015626 182 break;
Mario_Galeano 0:17f865015626 183 }
Mario_Galeano 0:17f865015626 184 }
Mario_Galeano 0:17f865015626 185
Mario_Galeano 0:17f865015626 186 else if (Brazo == 0x04)
Mario_Galeano 0:17f865015626 187 {
Mario_Galeano 0:17f865015626 188 switch (movimiento)
Mario_Galeano 0:17f865015626 189 {
Mario_Galeano 0:17f865015626 190 case 1:// legLeftUp
Mario_Galeano 0:17f865015626 191 controlServo(8, arriba);
Mario_Galeano 0:17f865015626 192 break;
Mario_Galeano 0:17f865015626 193
Mario_Galeano 0:17f865015626 194 case 2:// legLeftDown
Mario_Galeano 0:17f865015626 195 controlServo(8, abajo);
Mario_Galeano 0:17f865015626 196 break;
Mario_Galeano 0:17f865015626 197
Mario_Galeano 0:17f865015626 198 case 3:// legLeftForward
Mario_Galeano 0:17f865015626 199 controlServo(7, 10);
Mario_Galeano 0:17f865015626 200 break;
Mario_Galeano 0:17f865015626 201
Mario_Galeano 0:17f865015626 202 case 4:// legLeftReverse
Mario_Galeano 0:17f865015626 203 controlServo(7, 65);
Mario_Galeano 0:17f865015626 204 break;
Mario_Galeano 0:17f865015626 205
Mario_Galeano 0:17f865015626 206 default:
Mario_Galeano 0:17f865015626 207 break;
Mario_Galeano 0:17f865015626 208 }
Mario_Galeano 0:17f865015626 209 }
Mario_Galeano 0:17f865015626 210 }
Mario_Galeano 0:17f865015626 211
Mario_Galeano 0:17f865015626 212 //*****************************************************************************//
Mario_Galeano 0:17f865015626 213 //Funciones movimientos
Mario_Galeano 0:17f865015626 214
Mario_Galeano 0:17f865015626 215 void inicialPosition ()
Mario_Galeano 0:17f865015626 216 {
Mario_Galeano 0:17f865015626 217 controlServo(2,arriba);
Mario_Galeano 0:17f865015626 218 controlServo(1,50);
Mario_Galeano 0:17f865015626 219 wait_ms(80);
Mario_Galeano 0:17f865015626 220 controlServo(2,abajo-12);
Mario_Galeano 0:17f865015626 221
Mario_Galeano 0:17f865015626 222 controlServo(4,arriba);
Mario_Galeano 0:17f865015626 223 controlServo(3,105);
Mario_Galeano 0:17f865015626 224 wait_ms(80);
Mario_Galeano 0:17f865015626 225 controlServo(4,abajo-12);
Mario_Galeano 0:17f865015626 226
Mario_Galeano 0:17f865015626 227 controlServo(6,arriba);
Mario_Galeano 0:17f865015626 228 controlServo(5,80);
Mario_Galeano 0:17f865015626 229 wait_ms(80);
Mario_Galeano 0:17f865015626 230 controlServo(6,abajo);
Mario_Galeano 0:17f865015626 231
Mario_Galeano 0:17f865015626 232 controlServo(8,arriba);
Mario_Galeano 0:17f865015626 233 controlServo(7,45);
Mario_Galeano 0:17f865015626 234 wait_ms(80);
Mario_Galeano 0:17f865015626 235 controlServo(8,abajo);
Mario_Galeano 0:17f865015626 236 }
Mario_Galeano 0:17f865015626 237
Mario_Galeano 0:17f865015626 238 void forward()
Mario_Galeano 0:17f865015626 239 {
Mario_Galeano 0:17f865015626 240 //*************************************************************//
Mario_Galeano 0:17f865015626 241 // POSICION INICIAL
Mario_Galeano 0:17f865015626 242
Mario_Galeano 0:17f865015626 243 controlServo(5,118); // Brazo 3 atras
Mario_Galeano 0:17f865015626 244 controlServo(7,5); // Brazo 4 adelante
Mario_Galeano 0:17f865015626 245 wait_ms(tiempo); // Tiempo en posicion
Mario_Galeano 0:17f865015626 246
Mario_Galeano 0:17f865015626 247 //*************************************************************//
Mario_Galeano 0:17f865015626 248 // PRIMER PASO
Mario_Galeano 0:17f865015626 249
Mario_Galeano 0:17f865015626 250 controlBrazo(3,1); // brazo 3 arriba
Mario_Galeano 0:17f865015626 251 controlServo(7,50); // Brazo 4 atras
Mario_Galeano 0:17f865015626 252 controlServo(5,50); // Brazo 3 avanza
Mario_Galeano 0:17f865015626 253 controlServo(3,90); // Brazo 2 atras
Mario_Galeano 0:17f865015626 254 wait_ms(tiempo); // tiempo en posicion arriba
Mario_Galeano 0:17f865015626 255
Mario_Galeano 0:17f865015626 256 controlBrazo(3,2); // Brazo 3 baja
Mario_Galeano 0:17f865015626 257 controlServo(1,25); // Brazo 1 atras
Mario_Galeano 0:17f865015626 258 wait_ms(tiempo);
Mario_Galeano 0:17f865015626 259
Mario_Galeano 0:17f865015626 260 //*************************************************************//
Mario_Galeano 0:17f865015626 261 // SEGUNDO PASO
Mario_Galeano 0:17f865015626 262
Mario_Galeano 0:17f865015626 263 controlBrazo(2,1); // Brazo 2 arriba
Mario_Galeano 0:17f865015626 264 controlServo(1,8); // Brazo 1 atras
Mario_Galeano 0:17f865015626 265 controlServo(3,150); // Brazo 2 adelante
Mario_Galeano 0:17f865015626 266 controlServo(7,50); // Brazo 4 atras
Mario_Galeano 0:17f865015626 267 wait_ms(tiempo); // Tiempo en posicion arriba
Mario_Galeano 0:17f865015626 268
Mario_Galeano 0:17f865015626 269 controlBrazo(2,2); // brazo 2 abajo
Mario_Galeano 0:17f865015626 270 controlServo(5,100); // Brazo 3 atras
Mario_Galeano 0:17f865015626 271
Mario_Galeano 0:17f865015626 272 //*************************************************************//
Mario_Galeano 0:17f865015626 273 // TERCER PASO
Mario_Galeano 0:17f865015626 274
Mario_Galeano 0:17f865015626 275 controlBrazo(1,1); // Brazo 1 arriba
Mario_Galeano 0:17f865015626 276 controlServo(3,90); // Brazo 2 atras
Mario_Galeano 0:17f865015626 277 controlServo(1,60); // Brazo 1 adelante
Mario_Galeano 0:17f865015626 278 controlServo(7,65); // Brazo 4 atras
Mario_Galeano 0:17f865015626 279 wait_ms(tiempo);
Mario_Galeano 0:17f865015626 280 controlBrazo(1,2); // Brazo 1 abajo
Mario_Galeano 0:17f865015626 281 controlServo(5,118);
Mario_Galeano 0:17f865015626 282
Mario_Galeano 0:17f865015626 283 //*************************************************************//
Mario_Galeano 0:17f865015626 284 // TERCER PASO
Mario_Galeano 0:17f865015626 285 controlBrazo(4,1);
Mario_Galeano 0:17f865015626 286 controlServo(7,5); // Brazo 4 adelante
Mario_Galeano 0:17f865015626 287 wait_ms(tiempo);
Mario_Galeano 0:17f865015626 288 controlBrazo(4,2);
Mario_Galeano 0:17f865015626 289 }
Mario_Galeano 0:17f865015626 290
Mario_Galeano 0:17f865015626 291 void reverse()
Mario_Galeano 0:17f865015626 292 {
Mario_Galeano 0:17f865015626 293 //*************************************************************//
Mario_Galeano 0:17f865015626 294 // POSICION INICIAL
Mario_Galeano 0:17f865015626 295
Mario_Galeano 0:17f865015626 296 controlServo(5,118); // Brazo 3 atras
Mario_Galeano 0:17f865015626 297 controlServo(7,5); // Brazo 4 adelante
Mario_Galeano 0:17f865015626 298 wait_ms(tiempo); // Tiempo en posicion
Mario_Galeano 0:17f865015626 299
Mario_Galeano 0:17f865015626 300 //*************************************************************//
Mario_Galeano 0:17f865015626 301 // PRIMER PASO
Mario_Galeano 0:17f865015626 302
Mario_Galeano 0:17f865015626 303 controlBrazo(4,1); // brazo 4 arriba
Mario_Galeano 0:17f865015626 304 controlServo(5,60); // Brazo 3 atras
Mario_Galeano 0:17f865015626 305 controlServo(7,65); // Brazo 4 avanza
Mario_Galeano 0:17f865015626 306 controlServo(1,70); // Brazo 1 atras
Mario_Galeano 0:17f865015626 307 wait_ms(tiempo); // tiempo en posicion arriba
Mario_Galeano 0:17f865015626 308
Mario_Galeano 0:17f865015626 309 controlBrazo(4,2); // Brazo 4 abajo
Mario_Galeano 0:17f865015626 310 controlServo(3,120); // Brazo 1 atras
Mario_Galeano 0:17f865015626 311 wait_ms(tiempo);
Mario_Galeano 0:17f865015626 312
Mario_Galeano 0:17f865015626 313 //**************************************************************//
Mario_Galeano 0:17f865015626 314 // SEGUNDO PASO
Mario_Galeano 0:17f865015626 315
Mario_Galeano 0:17f865015626 316 controlBrazo(1,1); // Brazo 1 arriba
Mario_Galeano 0:17f865015626 317 controlServo(5,70); // Brazo 3 atras
Mario_Galeano 0:17f865015626 318 controlServo(1,8); // Brazo 1 adelante
Mario_Galeano 0:17f865015626 319 controlServo(3,150); // Brazo 2 atras
Mario_Galeano 0:17f865015626 320 wait_ms(tiempo); // Tiempo en posicion arriba
Mario_Galeano 0:17f865015626 321
Mario_Galeano 0:17f865015626 322 controlBrazo(1,2); // brazo 1 abajo
Mario_Galeano 0:17f865015626 323 controlServo(7,35); // Brazo 4 atras
Mario_Galeano 0:17f865015626 324
Mario_Galeano 0:17f865015626 325 //*************************************************************//
Mario_Galeano 0:17f865015626 326 // TERCER PASO
Mario_Galeano 0:17f865015626 327
Mario_Galeano 0:17f865015626 328 controlBrazo(2,1); // Brazo 2 arriba
Mario_Galeano 0:17f865015626 329 controlServo(1,70); //Brazo 1 adelante
Mario_Galeano 0:17f865015626 330 controlServo(3,95); //Brazo 2 atras
Mario_Galeano 0:17f865015626 331 controlServo(5,60); // Brazo 4 adelante
Mario_Galeano 0:17f865015626 332 wait_ms(tiempo);
Mario_Galeano 0:17f865015626 333
Mario_Galeano 0:17f865015626 334 controlBrazo(2,2); // Brazo 2 abajo
Mario_Galeano 0:17f865015626 335 controlServo(7,10); // Brazo 4 adelante
Mario_Galeano 0:17f865015626 336
Mario_Galeano 0:17f865015626 337 //*************************************************************//
Mario_Galeano 0:17f865015626 338 // TERCER PASO
Mario_Galeano 0:17f865015626 339
Mario_Galeano 0:17f865015626 340 controlBrazo(3,1); //Brazo 3 arriba
Mario_Galeano 0:17f865015626 341 controlServo(5,118); // Brazo 4 adelante
Mario_Galeano 0:17f865015626 342 wait_ms(tiempo);
Mario_Galeano 0:17f865015626 343 controlBrazo(3,2);
Mario_Galeano 0:17f865015626 344 }
Mario_Galeano 0:17f865015626 345
Mario_Galeano 0:17f865015626 346 void right()
Mario_Galeano 0:17f865015626 347 {
Mario_Galeano 0:17f865015626 348 //*************************************************************//
Mario_Galeano 0:17f865015626 349 // POSICION INICIAL
Mario_Galeano 0:17f865015626 350
Mario_Galeano 0:17f865015626 351 controlServo(5,118); // Brazo 3 atras
Mario_Galeano 0:17f865015626 352 controlServo(7,5); // Brazo 4 adelante
Mario_Galeano 0:17f865015626 353 wait_ms(tiempo); // Tiempo en posicion
Mario_Galeano 0:17f865015626 354
Mario_Galeano 0:17f865015626 355 //*************************************************************//
Mario_Galeano 0:17f865015626 356 // PASO 1
Mario_Galeano 0:17f865015626 357
Mario_Galeano 0:17f865015626 358 controlBrazo(3,1);
Mario_Galeano 0:17f865015626 359 controlServo(5,60);
Mario_Galeano 0:17f865015626 360 controlServo(1,70);
Mario_Galeano 0:17f865015626 361 controlServo(7,40);
Mario_Galeano 0:17f865015626 362 controlServo(3,130);
Mario_Galeano 0:17f865015626 363 wait_ms(tiempo);
Mario_Galeano 0:17f865015626 364 controlBrazo(3,2);
Mario_Galeano 0:17f865015626 365
Mario_Galeano 0:17f865015626 366 // PASO 2
Mario_Galeano 0:17f865015626 367 controlBrazo(1,1);
Mario_Galeano 0:17f865015626 368 controlServo(1,8);
Mario_Galeano 0:17f865015626 369 controlServo(3,150);
Mario_Galeano 0:17f865015626 370 controlServo(5,80);
Mario_Galeano 0:17f865015626 371 controlServo(7,55);
Mario_Galeano 0:17f865015626 372 wait_ms(tiempo);
Mario_Galeano 0:17f865015626 373 controlBrazo(1,2);
Mario_Galeano 0:17f865015626 374
Mario_Galeano 0:17f865015626 375 // PASO 3
Mario_Galeano 0:17f865015626 376 controlBrazo(2,1);
Mario_Galeano 0:17f865015626 377 controlServo(3,95);
Mario_Galeano 0:17f865015626 378 controlServo(1,30);
Mario_Galeano 0:17f865015626 379 controlServo(5,100);
Mario_Galeano 0:17f865015626 380 controlServo(7,65);
Mario_Galeano 0:17f865015626 381 wait_ms(tiempo);
Mario_Galeano 0:17f865015626 382 controlBrazo(2,2);
Mario_Galeano 0:17f865015626 383
Mario_Galeano 0:17f865015626 384 //PASO 4
Mario_Galeano 0:17f865015626 385 controlBrazo(4,1);
Mario_Galeano 0:17f865015626 386 controlServo(7,10);
Mario_Galeano 0:17f865015626 387 controlServo(5,118);
Mario_Galeano 0:17f865015626 388 controlServo(1,45);
Mario_Galeano 0:17f865015626 389 controlServo(3,105);
Mario_Galeano 0:17f865015626 390 wait_ms(tiempo);
Mario_Galeano 0:17f865015626 391 controlBrazo(4,2);
Mario_Galeano 0:17f865015626 392 }
Mario_Galeano 0:17f865015626 393
Mario_Galeano 0:17f865015626 394 void left()
Mario_Galeano 0:17f865015626 395 {
Mario_Galeano 0:17f865015626 396 //*************************************************************//
Mario_Galeano 0:17f865015626 397 // POSICION INICIAL
Mario_Galeano 0:17f865015626 398
Mario_Galeano 0:17f865015626 399 controlServo(1,8); // Brazo 3 atras
Mario_Galeano 0:17f865015626 400 controlServo(3,150); // Brazo 4 adelante
Mario_Galeano 0:17f865015626 401 wait_ms(tiempo); // Tiempo en posicion
Mario_Galeano 0:17f865015626 402
Mario_Galeano 0:17f865015626 403 //*************************************************************//
Mario_Galeano 0:17f865015626 404 // PASO 1
Mario_Galeano 0:17f865015626 405
Mario_Galeano 0:17f865015626 406 controlBrazo(1,1);
Mario_Galeano 0:17f865015626 407 controlServo(3,105);
Mario_Galeano 0:17f865015626 408 controlServo(1,70);
Mario_Galeano 0:17f865015626 409 controlServo(5,60);
Mario_Galeano 0:17f865015626 410 controlServo(7,55);
Mario_Galeano 0:17f865015626 411 wait_ms(tiempo);
Mario_Galeano 0:17f865015626 412 controlBrazo(1,2);
Mario_Galeano 0:17f865015626 413
Mario_Galeano 0:17f865015626 414 // PASO 2
Mario_Galeano 0:17f865015626 415 controlBrazo(3,1);
Mario_Galeano 0:17f865015626 416 controlServo(1,50);
Mario_Galeano 0:17f865015626 417 controlServo(5,118);
Mario_Galeano 0:17f865015626 418 controlServo(3,80);
Mario_Galeano 0:17f865015626 419 controlServo(7,15);
Mario_Galeano 0:17f865015626 420 wait_ms(tiempo);
Mario_Galeano 0:17f865015626 421 controlBrazo(3,2);
Mario_Galeano 0:17f865015626 422
Mario_Galeano 0:17f865015626 423 // PASO 3
Mario_Galeano 0:17f865015626 424 controlBrazo(4,1);
Mario_Galeano 0:17f865015626 425 controlServo(5,100);
Mario_Galeano 0:17f865015626 426 controlServo(7,65);
Mario_Galeano 0:17f865015626 427 controlServo(3,95);
Mario_Galeano 0:17f865015626 428 controlServo(1,30);
Mario_Galeano 0:17f865015626 429 wait_ms(tiempo);
Mario_Galeano 0:17f865015626 430 controlBrazo(4,2);
Mario_Galeano 0:17f865015626 431
Mario_Galeano 0:17f865015626 432 //PASO 4
Mario_Galeano 0:17f865015626 433 controlBrazo(2,1);
Mario_Galeano 0:17f865015626 434 controlServo(1,8);
Mario_Galeano 0:17f865015626 435 controlServo(3,150);
Mario_Galeano 0:17f865015626 436 controlServo(5,80);
Mario_Galeano 0:17f865015626 437 controlServo(7,45);
Mario_Galeano 0:17f865015626 438 wait_ms(tiempo);
Mario_Galeano 0:17f865015626 439 controlBrazo(2,2);
Mario_Galeano 0:17f865015626 440 controlBrazo(1,2);
Mario_Galeano 0:17f865015626 441 controlBrazo(3,2);
Mario_Galeano 0:17f865015626 442 }
Mario_Galeano 0:17f865015626 443
Mario_Galeano 0:17f865015626 444 //*****************************************************************************//
Mario_Galeano 0:17f865015626 445 // Funcion para la lectura de datos
Mario_Galeano 0:17f865015626 446
Mario_Galeano 0:17f865015626 447 uint8_t readComand()
Mario_Galeano 0:17f865015626 448 {
Mario_Galeano 0:17f865015626 449 timeSerial.reset();
Mario_Galeano 0:17f865015626 450 timeSerial.start();
Mario_Galeano 0:17f865015626 451
Mario_Galeano 0:17f865015626 452 while (timeSerial.read_ms() <= 120)
Mario_Galeano 0:17f865015626 453 {
Mario_Galeano 0:17f865015626 454 if(pc.readable())
Mario_Galeano 0:17f865015626 455 {
Mario_Galeano 0:17f865015626 456 char charRead = pc.getc();
Mario_Galeano 0:17f865015626 457
Mario_Galeano 0:17f865015626 458 if (charRead == 0xFF)
Mario_Galeano 0:17f865015626 459 {
Mario_Galeano 0:17f865015626 460 for (int i=0; i<3; i++)
Mario_Galeano 0:17f865015626 461 {
Mario_Galeano 0:17f865015626 462 data[i] = pc.getc();
Mario_Galeano 0:17f865015626 463 }
Mario_Galeano 0:17f865015626 464 }
Mario_Galeano 0:17f865015626 465
Mario_Galeano 0:17f865015626 466 charRead = pc.getc();
Mario_Galeano 0:17f865015626 467
Mario_Galeano 0:17f865015626 468 if (charRead != 0xFD)
Mario_Galeano 0:17f865015626 469 {
Mario_Galeano 0:17f865015626 470 pc.printf("\nError al recibir comando, intente nuevamente\n");
Mario_Galeano 0:17f865015626 471 return 0;
Mario_Galeano 0:17f865015626 472 }
Mario_Galeano 0:17f865015626 473
Mario_Galeano 0:17f865015626 474 return 1;
Mario_Galeano 0:17f865015626 475 }
Mario_Galeano 0:17f865015626 476 }
Mario_Galeano 0:17f865015626 477 return 0;
Mario_Galeano 0:17f865015626 478 }
Mario_Galeano 0:17f865015626 479
Mario_Galeano 0:17f865015626 480 void readSensor()
Mario_Galeano 0:17f865015626 481 {
Mario_Galeano 0:17f865015626 482 valor_y = value_y.read();
Mario_Galeano 0:17f865015626 483 valor_y = valor_y*3300;
Mario_Galeano 0:17f865015626 484 //pc.printf("\nValor Y: %f\n",valor_y);
Mario_Galeano 0:17f865015626 485
Mario_Galeano 0:17f865015626 486 valor_x = value_x.read();
Mario_Galeano 0:17f865015626 487 valor_x = valor_x*3300;
Mario_Galeano 0:17f865015626 488 //pc.printf("\nValor X: %f\n",valor_x);
Mario_Galeano 0:17f865015626 489 }
Mario_Galeano 0:17f865015626 490
Mario_Galeano 0:17f865015626 491 void up (void)
Mario_Galeano 0:17f865015626 492 {
Mario_Galeano 0:17f865015626 493 controlBrazo(1,2);
Mario_Galeano 0:17f865015626 494 controlBrazo(2,1);
Mario_Galeano 0:17f865015626 495 controlBrazo(3,2);
Mario_Galeano 0:17f865015626 496 controlBrazo(4,1);
Mario_Galeano 0:17f865015626 497 }
Mario_Galeano 0:17f865015626 498
Mario_Galeano 0:17f865015626 499 // *********************************************************************************************************************************//
Mario_Galeano 0:17f865015626 500 // Programa principal
Mario_Galeano 0:17f865015626 501
Mario_Galeano 0:17f865015626 502
Mario_Galeano 0:17f865015626 503 int main ()
Mario_Galeano 0:17f865015626 504 {
Mario_Galeano 0:17f865015626 505 inicialPosition ();
Mario_Galeano 0:17f865015626 506 wait(1);
Mario_Galeano 0:17f865015626 507
Mario_Galeano 0:17f865015626 508 while (1)
Mario_Galeano 0:17f865015626 509 {
Mario_Galeano 0:17f865015626 510 // Lectura constante del color
Mario_Galeano 0:17f865015626 511 color = readColor();
Mario_Galeano 0:17f865015626 512
Mario_Galeano 0:17f865015626 513 // lectura del joystick cada 100 ms
Mario_Galeano 0:17f865015626 514 timeRead.start();
Mario_Galeano 0:17f865015626 515
Mario_Galeano 0:17f865015626 516 if (timeRead.read_ms() >= 100)
Mario_Galeano 0:17f865015626 517 {
Mario_Galeano 0:17f865015626 518 readSensor();
Mario_Galeano 0:17f865015626 519 timeRead.reset();
Mario_Galeano 0:17f865015626 520 }
Mario_Galeano 0:17f865015626 521
Mario_Galeano 0:17f865015626 522 while (readComand())
Mario_Galeano 0:17f865015626 523 {
Mario_Galeano 0:17f865015626 524 // Codigo para comandos desde CoolTerm
Mario_Galeano 0:17f865015626 525
Mario_Galeano 0:17f865015626 526 if (data[0] == 0x01)
Mario_Galeano 0:17f865015626 527 {
Mario_Galeano 0:17f865015626 528 // Codigo para movimiento independiente de los servomotores desde CoolTerm
Mario_Galeano 0:17f865015626 529 controlServo(data[1],data[2]);
Mario_Galeano 0:17f865015626 530 pc.printf("\nNumero motor: %d", data[1]);
Mario_Galeano 0:17f865015626 531 pc.printf("\nGrados: %d\n", data[2]);
Mario_Galeano 0:17f865015626 532 }
Mario_Galeano 0:17f865015626 533
Mario_Galeano 0:17f865015626 534 else if(data[0]==0x02)
Mario_Galeano 0:17f865015626 535 {
Mario_Galeano 0:17f865015626 536 // Codigo para movimiento de las extremidades desde CoolTerm
Mario_Galeano 0:17f865015626 537 pc.printf("\nNumero brazo: %d", data[1]);
Mario_Galeano 0:17f865015626 538 pc.printf("\nMovimiento: %d\n", data[2]);
Mario_Galeano 0:17f865015626 539 controlBrazo(data[1],data[2]);
Mario_Galeano 0:17f865015626 540 }
Mario_Galeano 0:17f865015626 541
Mario_Galeano 0:17f865015626 542 else if (data[0] == 3)
Mario_Galeano 0:17f865015626 543 {
Mario_Galeano 0:17f865015626 544 // Codigo para llevar a posicion inicial desde CoolTerm
Mario_Galeano 0:17f865015626 545 inicialPosition ();
Mario_Galeano 0:17f865015626 546 }
Mario_Galeano 0:17f865015626 547
Mario_Galeano 0:17f865015626 548 else if (data[0] == 4)
Mario_Galeano 0:17f865015626 549 {
Mario_Galeano 0:17f865015626 550 // Codigo para lectura de color desde CoolTerm
Mario_Galeano 0:17f865015626 551 color = readColor();
Mario_Galeano 0:17f865015626 552 pc.printf("\nColor: %d\n", color);
Mario_Galeano 0:17f865015626 553 }
Mario_Galeano 0:17f865015626 554 }
Mario_Galeano 0:17f865015626 555
Mario_Galeano 0:17f865015626 556 if (valor_y <= 1000)
Mario_Galeano 0:17f865015626 557 {
Mario_Galeano 0:17f865015626 558 // Codigo para movimiento adelante segun joystick
Mario_Galeano 0:17f865015626 559
Mario_Galeano 0:17f865015626 560 if (readColor() == 1)
Mario_Galeano 0:17f865015626 561 {
Mario_Galeano 0:17f865015626 562 for (int i=0; i<2; i++)
Mario_Galeano 0:17f865015626 563 {
Mario_Galeano 0:17f865015626 564 // Si el color es rojo gira a la derecha
Mario_Galeano 0:17f865015626 565 right();
Mario_Galeano 0:17f865015626 566 }
Mario_Galeano 0:17f865015626 567 }
Mario_Galeano 0:17f865015626 568
Mario_Galeano 0:17f865015626 569 else if(readColor() == 2)
Mario_Galeano 0:17f865015626 570 {
Mario_Galeano 0:17f865015626 571 for (int i=0; i<2; i++)
Mario_Galeano 0:17f865015626 572 {
Mario_Galeano 0:17f865015626 573 // Si el color es azul gira a la izquierda
Mario_Galeano 0:17f865015626 574 left();
Mario_Galeano 0:17f865015626 575 }
Mario_Galeano 0:17f865015626 576 }
Mario_Galeano 0:17f865015626 577
Mario_Galeano 0:17f865015626 578 else
Mario_Galeano 0:17f865015626 579 {
Mario_Galeano 0:17f865015626 580 forward();
Mario_Galeano 0:17f865015626 581 status=1;
Mario_Galeano 0:17f865015626 582 }
Mario_Galeano 0:17f865015626 583 }
Mario_Galeano 0:17f865015626 584
Mario_Galeano 0:17f865015626 585 else if (valor_y >= 2000)
Mario_Galeano 0:17f865015626 586 {
Mario_Galeano 0:17f865015626 587 // Codigo para movimiento atras segun joystick
Mario_Galeano 0:17f865015626 588 reverse();
Mario_Galeano 0:17f865015626 589 status=1;
Mario_Galeano 0:17f865015626 590 }
Mario_Galeano 0:17f865015626 591
Mario_Galeano 0:17f865015626 592 if (valor_x <= 1000)
Mario_Galeano 0:17f865015626 593 {
Mario_Galeano 0:17f865015626 594 // Codigo para giro derecho segun joystick
Mario_Galeano 0:17f865015626 595
Mario_Galeano 0:17f865015626 596 if (readColor() == 3)
Mario_Galeano 0:17f865015626 597 {
Mario_Galeano 0:17f865015626 598 // Si el color es verde se "sienta"
Mario_Galeano 0:17f865015626 599 up();
Mario_Galeano 0:17f865015626 600 }
Mario_Galeano 0:17f865015626 601
Mario_Galeano 0:17f865015626 602 else
Mario_Galeano 0:17f865015626 603 {
Mario_Galeano 0:17f865015626 604 // Si el color es diferente de verde gira a la izquierda
Mario_Galeano 0:17f865015626 605 right();
Mario_Galeano 0:17f865015626 606 status=1;
Mario_Galeano 0:17f865015626 607 }
Mario_Galeano 0:17f865015626 608 }
Mario_Galeano 0:17f865015626 609
Mario_Galeano 0:17f865015626 610 else if (valor_x >= 2000)
Mario_Galeano 0:17f865015626 611 {
Mario_Galeano 0:17f865015626 612 // Codigo para giro izquierdo segun joystick
Mario_Galeano 0:17f865015626 613
Mario_Galeano 0:17f865015626 614 if (readColor() == 3)
Mario_Galeano 0:17f865015626 615 {
Mario_Galeano 0:17f865015626 616 // Si el color es verde se "sienta"
Mario_Galeano 0:17f865015626 617 up();
Mario_Galeano 0:17f865015626 618 }
Mario_Galeano 0:17f865015626 619
Mario_Galeano 0:17f865015626 620 else
Mario_Galeano 0:17f865015626 621 {
Mario_Galeano 0:17f865015626 622 // Si el color es diferente de verde gira a la izquierda
Mario_Galeano 0:17f865015626 623 left();
Mario_Galeano 0:17f865015626 624 status=1;
Mario_Galeano 0:17f865015626 625 }
Mario_Galeano 0:17f865015626 626 }
Mario_Galeano 0:17f865015626 627
Mario_Galeano 0:17f865015626 628 else if(status == 1)
Mario_Galeano 0:17f865015626 629 {
Mario_Galeano 0:17f865015626 630 inicialPosition ();
Mario_Galeano 0:17f865015626 631 status=0;
Mario_Galeano 0:17f865015626 632 }
Mario_Galeano 0:17f865015626 633
Mario_Galeano 0:17f865015626 634 }
Mario_Galeano 0:17f865015626 635 }