German Ariza / Mbed 2 deprecated CORTE2

Entrega 2 Proyecto Sistemas Embebidos

Dependencies:   mbed

main.cpp@0:4b10df072322, 2019-04-26 (annotated)

Committer:
andriuxone
Date:
Fri Apr 26 16:36:40 2019 +0000
Revision:
0:4b10df072322
Entrega Corte 2 - Sistemas embebidos - Universidad ECCI - Colombia

Who changed what in which revision?

UserRevisionLine numberNew contents of line
andriuxone 0:4b10df072322 1 #include "mbed.h"
andriuxone 0:4b10df072322 2 #include "scolor_TCS3200.h"
andriuxone 0:4b10df072322 3
andriuxone 0:4b10df072322 4 /*****************************************************************************
andriuxone 0:4b10df072322 5 generar un programa que controle por el puerto serial el grado de 4 servo motores.
andriuxone 0:4b10df072322 6 por medio de la comunicacion serial el comando es
andriuxone 0:4b10df072322 7
andriuxone 0:4b10df072322 8 | | | |
andriuxone 0:4b10df072322 9 | INITCMD | N_motor | N_grados |
andriuxone 0:4b10df072322 10 | 0xff | 0x01- 0x04 | 0x00 - 0xb4 |
andriuxone 0:4b10df072322 11
andriuxone 0:4b10df072322 12
andriuxone 0:4b10df072322 13 *****************************************************************************/
andriuxone 0:4b10df072322 14 /*CONSTANTES*/
andriuxone 0:4b10df072322 15 #define INITCMD 0xFF
andriuxone 0:4b10df072322 16 #define DEGREES_MAX 180
andriuxone 0:4b10df072322 17
andriuxone 0:4b10df072322 18 /*TELECOMANDO*/
andriuxone 0:4b10df072322 19 #define AUTOMATIC 0x01
andriuxone 0:4b10df072322 20 #define PINZA 0x02
andriuxone 0:4b10df072322 21 #define M1_MANUAL 0x03
andriuxone 0:4b10df072322 22 #define M2_MANUAL 0x04
andriuxone 0:4b10df072322 23 #define M3_MANUAL 0x05
andriuxone 0:4b10df072322 24 #define M4_MANUAL 0x06
andriuxone 0:4b10df072322 25 #define VELOCIDAD 0x07
andriuxone 0:4b10df072322 26 #define JOYSTICK 0x08
andriuxone 0:4b10df072322 27 #define COLOR 0x09
andriuxone 0:4b10df072322 28
andriuxone 0:4b10df072322 29 /*PARAMETRO*/
andriuxone 0:4b10df072322 30 #define HOME 0x01
andriuxone 0:4b10df072322 31 #define MATERIAL 0x02
andriuxone 0:4b10df072322 32 #define CELDA1 0x03
andriuxone 0:4b10df072322 33 #define CELDA2 0x04
andriuxone 0:4b10df072322 34 #define CELDA3 0x05
andriuxone 0:4b10df072322 35 #define CELDA4 0x06
andriuxone 0:4b10df072322 36 #define ABRIR_PINZA 0x01
andriuxone 0:4b10df072322 37 #define CERRAR_PINZA 0x02
andriuxone 0:4b10df072322 38 #define VELOCIDAD_LENTA 0x01
andriuxone 0:4b10df072322 39 #define VELOCIDAD_MEDIA 0x02
andriuxone 0:4b10df072322 40 #define VELOCIDAD_ALTA 0x03
andriuxone 0:4b10df072322 41 #define INICIA_JOYSTICK 0x01
andriuxone 0:4b10df072322 42 #define PARA_JOYSTICK 0x02
andriuxone 0:4b10df072322 43 #define LEE_COLOR 0x00
andriuxone 0:4b10df072322 44
andriuxone 0:4b10df072322 45
andriuxone 0:4b10df072322 46 /*VARIABLES GLOBALES*/
andriuxone 0:4b10df072322 47 uint8_t N_motor; // variable almacena el número de motor en leer_datos()
andriuxone 0:4b10df072322 48 uint8_t N_grados; // varable almacena los grados que se mueve el motor en leer_datos()
andriuxone 0:4b10df072322 49
andriuxone 0:4b10df072322 50 /*VARIABLE RECIBE COMANDOS*/
andriuxone 0:4b10df072322 51 /*Mueve Base y Eslabon Izquierdo*/
andriuxone 0:4b10df072322 52 Serial pc(USBTX, USBRX);
andriuxone 0:4b10df072322 53 unsigned char telecomando;
andriuxone 0:4b10df072322 54 unsigned char parametro;
andriuxone 0:4b10df072322 55 unsigned char teleJoys;
andriuxone 0:4b10df072322 56 unsigned char paramJoys;
andriuxone 0:4b10df072322 57
andriuxone 0:4b10df072322 58 /*CONFIGURACIÓN SERVOS*/
andriuxone 0:4b10df072322 59 PwmOut myservo1(PB_4);//d5 -- BASE
andriuxone 0:4b10df072322 60 PwmOut myservo2(PB_10);//d6 -- IZQUIERDO
andriuxone 0:4b10df072322 61 PwmOut myservo3(PA_8);//d7 -- DERECHO
andriuxone 0:4b10df072322 62 PwmOut myservo4(PA_9);//d8 -- PINZA
andriuxone 0:4b10df072322 63
andriuxone 0:4b10df072322 64 /*CONFIGURACIÓN JOYSTICK 1*/
andriuxone 0:4b10df072322 65 /*Mueve Base y Eslabon Izquierdo*/
andriuxone 0:4b10df072322 66 AnalogIn analog1_value_x(A0);//A0 -- POSICION X -- BASE
andriuxone 0:4b10df072322 67 AnalogIn analog1_value_y(A1);//A1 -- POSICION Y -- ESLABON INZQUIERDO
andriuxone 0:4b10df072322 68 /*Mueve Eslabon Derecho y Pinza*/
andriuxone 0:4b10df072322 69 AnalogIn analog2_value_x(A2);//A2 -- POSICION X -- ESLABON DERECHO
andriuxone 0:4b10df072322 70 AnalogIn analog2_value_y(A3);
andriuxone 0:4b10df072322 71 DigitalIn sw2(PB_6);//D10 -- BOTON JOYSTICK -- 0-ABRE PINZA , 1-CIERRA PINZA
andriuxone 0:4b10df072322 72
andriuxone 0:4b10df072322 73 /*CONFIGURACION SENSOR DE COLOR*/
andriuxone 0:4b10df072322 74 // S0, S1, S2, S3, OUT
andriuxone 0:4b10df072322 75 //scolor_TCS3200 scolor(PA_8, PB_10, PB_9, PB_5, PB_8);
andriuxone 0:4b10df072322 76 // D2 D3 D14 D4 D15
andriuxone 0:4b10df072322 77 scolor_TCS3200 scolor(PA_10, PB_3, PB_9, PB_5, PB_8);
andriuxone 0:4b10df072322 78
andriuxone 0:4b10df072322 79 /*DEFINICIÓN FUNCIONES*/
andriuxone 0:4b10df072322 80 void setup_uart();
andriuxone 0:4b10df072322 81 void setup_servo();
andriuxone 0:4b10df072322 82 void mover_servo(uint8_t motor, uint8_t gradss);
andriuxone 0:4b10df072322 83 uint32_t degrees2usec(uint8_t grados);
andriuxone 0:4b10df072322 84 void motor_velocidad(uint8_t motor, float valor);
andriuxone 0:4b10df072322 85
andriuxone 0:4b10df072322 86 //MOVIMIENTO ESLABONES POR POSICION
andriuxone 0:4b10df072322 87 void mover_base(int posicion);
andriuxone 0:4b10df072322 88 void mover_izquierdo(int posicion);
andriuxone 0:4b10df072322 89 void mover_derecho(int posicion);
andriuxone 0:4b10df072322 90 void mover_pinza(int posicion);
andriuxone 0:4b10df072322 91
andriuxone 0:4b10df072322 92 void mover_home();
andriuxone 0:4b10df072322 93 void mover_material();
andriuxone 0:4b10df072322 94 void mover_celda1();
andriuxone 0:4b10df072322 95 void mover_celda2();
andriuxone 0:4b10df072322 96 void mover_celda3();
andriuxone 0:4b10df072322 97 void mover_celda4();
andriuxone 0:4b10df072322 98 void abrir_pinza();
andriuxone 0:4b10df072322 99 void cerrar_pinza();
andriuxone 0:4b10df072322 100
andriuxone 0:4b10df072322 101 void mover_servo(uint8_t motor, uint8_t pos);
andriuxone 0:4b10df072322 102 void mover_con_joystick();
andriuxone 0:4b10df072322 103 void leer_color();
andriuxone 0:4b10df072322 104
andriuxone 0:4b10df072322 105 /*INICIO DE PROGRAMA*/
andriuxone 0:4b10df072322 106 int main()
andriuxone 0:4b10df072322 107 {
andriuxone 0:4b10df072322 108 pc.printf("INICIO PROGRAMA...\n\r");
andriuxone 0:4b10df072322 109 setup_uart();//Configuración Velocidad de Transmisión
andriuxone 0:4b10df072322 110 setup_servo();//Configuración de Motores -> periodo = 20 ms
andriuxone 0:4b10df072322 111
andriuxone 0:4b10df072322 112 while(1){
andriuxone 0:4b10df072322 113
andriuxone 0:4b10df072322 114 pc.printf("\nEsperando Telecomando...\n");
andriuxone 0:4b10df072322 115 while(pc.getc() != INITCMD);//0xFF
andriuxone 0:4b10df072322 116 telecomando = pc.getc();
andriuxone 0:4b10df072322 117 parametro= pc.getc();
andriuxone 0:4b10df072322 118
andriuxone 0:4b10df072322 119 pc.printf("Validando Telecomando: ");
andriuxone 0:4b10df072322 120 pc.printf("%02x",telecomando);pc.printf("\n");
andriuxone 0:4b10df072322 121 if(telecomando == AUTOMATIC) {
andriuxone 0:4b10df072322 122 pc.printf("MOVIMIENTO AUTOMATICO\n");
andriuxone 0:4b10df072322 123
andriuxone 0:4b10df072322 124 pc.printf("Validando Parametro: ");
andriuxone 0:4b10df072322 125 pc.printf("%02x",parametro);
andriuxone 0:4b10df072322 126 switch (parametro) {
andriuxone 0:4b10df072322 127 case HOME:
andriuxone 0:4b10df072322 128 pc.printf("\nHOME..\n");
andriuxone 0:4b10df072322 129 mover_home();
andriuxone 0:4b10df072322 130 NVIC_SystemReset();
andriuxone 0:4b10df072322 131 break;
andriuxone 0:4b10df072322 132 case MATERIAL:
andriuxone 0:4b10df072322 133 pc.printf("\nMATERIAL.\n");
andriuxone 0:4b10df072322 134 mover_material();
andriuxone 0:4b10df072322 135 NVIC_SystemReset();
andriuxone 0:4b10df072322 136 break;
andriuxone 0:4b10df072322 137 case CELDA1:
andriuxone 0:4b10df072322 138 pc.printf("\nCELDA1.\n");
andriuxone 0:4b10df072322 139 mover_celda1();
andriuxone 0:4b10df072322 140 NVIC_SystemReset();
andriuxone 0:4b10df072322 141 break;
andriuxone 0:4b10df072322 142 case CELDA2:
andriuxone 0:4b10df072322 143 pc.printf("\nCELDA2.\n");
andriuxone 0:4b10df072322 144 mover_celda2();
andriuxone 0:4b10df072322 145 NVIC_SystemReset();
andriuxone 0:4b10df072322 146 break;
andriuxone 0:4b10df072322 147 case CELDA3:
andriuxone 0:4b10df072322 148 pc.printf("\nCELDA3.\n");
andriuxone 0:4b10df072322 149 mover_celda3();
andriuxone 0:4b10df072322 150 NVIC_SystemReset();
andriuxone 0:4b10df072322 151 break;
andriuxone 0:4b10df072322 152 case CELDA4:
andriuxone 0:4b10df072322 153 pc.printf("\nCELDA4.\n");
andriuxone 0:4b10df072322 154 mover_celda4();
andriuxone 0:4b10df072322 155 NVIC_SystemReset();
andriuxone 0:4b10df072322 156 break;
andriuxone 0:4b10df072322 157 default:
andriuxone 0:4b10df072322 158 pc.printf("No se encontro parametro valido.\n");
andriuxone 0:4b10df072322 159 break ;
andriuxone 0:4b10df072322 160 }//Fin Switch
andriuxone 0:4b10df072322 161
andriuxone 0:4b10df072322 162
andriuxone 0:4b10df072322 163 }else if(telecomando == PINZA){
andriuxone 0:4b10df072322 164 pc.printf("PINZA\n");
andriuxone 0:4b10df072322 165
andriuxone 0:4b10df072322 166 pc.printf("Validando Parametro: ");
andriuxone 0:4b10df072322 167 pc.printf("%02x",parametro);
andriuxone 0:4b10df072322 168 switch (parametro) {
andriuxone 0:4b10df072322 169 case ABRIR_PINZA:
andriuxone 0:4b10df072322 170 pc.printf("\nABRE PINZA..\n");
andriuxone 0:4b10df072322 171 abrir_pinza();
andriuxone 0:4b10df072322 172 break;
andriuxone 0:4b10df072322 173 case CERRAR_PINZA:
andriuxone 0:4b10df072322 174 pc.printf("\nCIERRA PINZA..\n");
andriuxone 0:4b10df072322 175 cerrar_pinza();
andriuxone 0:4b10df072322 176 break;
andriuxone 0:4b10df072322 177 default:
andriuxone 0:4b10df072322 178 pc.printf("No se encontro comando valido.\n");
andriuxone 0:4b10df072322 179 break ;
andriuxone 0:4b10df072322 180 }//Fin Switch
andriuxone 0:4b10df072322 181
andriuxone 0:4b10df072322 182 }else if(telecomando == M1_MANUAL){
andriuxone 0:4b10df072322 183 pc.printf("M1_MANUAL\n");
andriuxone 0:4b10df072322 184
andriuxone 0:4b10df072322 185 pc.printf("Validando Parametro: ");
andriuxone 0:4b10df072322 186 pc.printf("%02x",parametro);
andriuxone 0:4b10df072322 187
andriuxone 0:4b10df072322 188 mover_servo(telecomando, parametro);
andriuxone 0:4b10df072322 189
andriuxone 0:4b10df072322 190 }else if(telecomando == M2_MANUAL){
andriuxone 0:4b10df072322 191 pc.printf("M2_MANUAL\n");
andriuxone 0:4b10df072322 192
andriuxone 0:4b10df072322 193 pc.printf("Validando Parametro: ");
andriuxone 0:4b10df072322 194 pc.printf("%02x",parametro);
andriuxone 0:4b10df072322 195
andriuxone 0:4b10df072322 196 mover_servo(telecomando, parametro);
andriuxone 0:4b10df072322 197 }else if(telecomando == M3_MANUAL){
andriuxone 0:4b10df072322 198 pc.printf("M3_MANUAL\n");
andriuxone 0:4b10df072322 199
andriuxone 0:4b10df072322 200 pc.printf("Validando Parametro: ");
andriuxone 0:4b10df072322 201 pc.printf("%02x",parametro);
andriuxone 0:4b10df072322 202
andriuxone 0:4b10df072322 203 mover_servo(telecomando, parametro);
andriuxone 0:4b10df072322 204
andriuxone 0:4b10df072322 205 }else if(telecomando == M4_MANUAL){
andriuxone 0:4b10df072322 206 pc.printf("M4_MANUAL\n");
andriuxone 0:4b10df072322 207
andriuxone 0:4b10df072322 208 pc.printf("Validando Parametro: ");
andriuxone 0:4b10df072322 209 pc.printf("%02x",parametro);
andriuxone 0:4b10df072322 210
andriuxone 0:4b10df072322 211 mover_servo(telecomando, parametro);
andriuxone 0:4b10df072322 212
andriuxone 0:4b10df072322 213 }else if(telecomando == VELOCIDAD){
andriuxone 0:4b10df072322 214 pc.printf("VELOCIDAD\n");
andriuxone 0:4b10df072322 215
andriuxone 0:4b10df072322 216 pc.printf("Validando Parametro: ");
andriuxone 0:4b10df072322 217 pc.printf("%02x",parametro);
andriuxone 0:4b10df072322 218 switch (parametro) {
andriuxone 0:4b10df072322 219 case VELOCIDAD_LENTA:
andriuxone 0:4b10df072322 220 pc.printf("\nVELOCIDAD_LENTA..\n");
andriuxone 0:4b10df072322 221 break;
andriuxone 0:4b10df072322 222 case VELOCIDAD_MEDIA:
andriuxone 0:4b10df072322 223 pc.printf("\nVELOCIDAD_MEDIA..\n");
andriuxone 0:4b10df072322 224 break;
andriuxone 0:4b10df072322 225 case VELOCIDAD_ALTA:
andriuxone 0:4b10df072322 226 pc.printf("\nVELOCIDAD_ALTA..\n");
andriuxone 0:4b10df072322 227 break;
andriuxone 0:4b10df072322 228 default:
andriuxone 0:4b10df072322 229 pc.printf("No se encontro comando valido.\n");
andriuxone 0:4b10df072322 230 break ;
andriuxone 0:4b10df072322 231 }//Fin Switch
andriuxone 0:4b10df072322 232
andriuxone 0:4b10df072322 233 }else if(telecomando == JOYSTICK){
andriuxone 0:4b10df072322 234 pc.printf("JOYSTICK\n");
andriuxone 0:4b10df072322 235
andriuxone 0:4b10df072322 236 pc.printf("Validando Parametro: ");
andriuxone 0:4b10df072322 237 pc.printf("%02x",parametro);
andriuxone 0:4b10df072322 238 switch (parametro) {
andriuxone 0:4b10df072322 239 case INICIA_JOYSTICK:
andriuxone 0:4b10df072322 240 pc.printf("\nINICIA_JOYSTICK ..\n");
andriuxone 0:4b10df072322 241 mover_con_joystick();
andriuxone 0:4b10df072322 242 break;
andriuxone 0:4b10df072322 243 case PARA_JOYSTICK:
andriuxone 0:4b10df072322 244 pc.printf("\nPARA_JOYSTICK..\n");
andriuxone 0:4b10df072322 245 break;
andriuxone 0:4b10df072322 246 default:
andriuxone 0:4b10df072322 247 pc.printf("No se encontro comando valido.\n");
andriuxone 0:4b10df072322 248 break ;
andriuxone 0:4b10df072322 249 }//Fin Switch
andriuxone 0:4b10df072322 250
andriuxone 0:4b10df072322 251
andriuxone 0:4b10df072322 252 }else if(telecomando == COLOR){
andriuxone 0:4b10df072322 253 pc.printf("COLOR\n");
andriuxone 0:4b10df072322 254
andriuxone 0:4b10df072322 255 leer_color();
andriuxone 0:4b10df072322 256
andriuxone 0:4b10df072322 257 }else {
andriuxone 0:4b10df072322 258 pc.printf("Telecomando No Encontrado...\n");
andriuxone 0:4b10df072322 259 }//Fin Validación Telecomando
andriuxone 0:4b10df072322 260 }//Fin While
andriuxone 0:4b10df072322 261 }// Fin main
andriuxone 0:4b10df072322 262
andriuxone 0:4b10df072322 263 /*CONFIGURA VELOCIDAD DE TRAMSMISIÓN*/
andriuxone 0:4b10df072322 264 void setup_uart()
andriuxone 0:4b10df072322 265 {
andriuxone 0:4b10df072322 266 pc.baud(9600);
andriuxone 0:4b10df072322 267 }
andriuxone 0:4b10df072322 268
andriuxone 0:4b10df072322 269 /*CONFIGURACIÓN DE SERVOS*/
andriuxone 0:4b10df072322 270 void setup_servo()
andriuxone 0:4b10df072322 271 {
andriuxone 0:4b10df072322 272 myservo1.period_ms(20);
andriuxone 0:4b10df072322 273 myservo2.period_ms(20);
andriuxone 0:4b10df072322 274 myservo3.period_ms(20);
andriuxone 0:4b10df072322 275 myservo4.period_ms(20);
andriuxone 0:4b10df072322 276 }
andriuxone 0:4b10df072322 277
andriuxone 0:4b10df072322 278
andriuxone 0:4b10df072322 279 //MOVER BASE
andriuxone 0:4b10df072322 280 void mover_base(int posicion)
andriuxone 0:4b10df072322 281 {
andriuxone 0:4b10df072322 282 myservo1.pulsewidth_us(posicion);
andriuxone 0:4b10df072322 283 }
andriuxone 0:4b10df072322 284 //
andriuxone 0:4b10df072322 285 //MOVER IZQUIERDO
andriuxone 0:4b10df072322 286 void mover_izquierdo(int posicion)
andriuxone 0:4b10df072322 287 {
andriuxone 0:4b10df072322 288 myservo2.pulsewidth_us(posicion);
andriuxone 0:4b10df072322 289 }
andriuxone 0:4b10df072322 290
andriuxone 0:4b10df072322 291 //MOVER DERECHO
andriuxone 0:4b10df072322 292 void mover_derecho(int posicion)
andriuxone 0:4b10df072322 293 {
andriuxone 0:4b10df072322 294 myservo3.pulsewidth_us(posicion);
andriuxone 0:4b10df072322 295 }
andriuxone 0:4b10df072322 296
andriuxone 0:4b10df072322 297 //MOVER PINZA
andriuxone 0:4b10df072322 298 void mover_pinza(int posicion)
andriuxone 0:4b10df072322 299 {
andriuxone 0:4b10df072322 300 myservo4.pulsewidth_us(posicion);
andriuxone 0:4b10df072322 301 }
andriuxone 0:4b10df072322 302
andriuxone 0:4b10df072322 303
andriuxone 0:4b10df072322 304 //MUEVE A POSICIÓN HOME
andriuxone 0:4b10df072322 305 void mover_home()
andriuxone 0:4b10df072322 306 {
andriuxone 0:4b10df072322 307 myservo1.pulsewidth_us(600);
andriuxone 0:4b10df072322 308 wait(1.0);
andriuxone 0:4b10df072322 309 myservo2.pulsewidth_us(600);
andriuxone 0:4b10df072322 310 wait(1.0);
andriuxone 0:4b10df072322 311 myservo3.pulsewidth_us(600);
andriuxone 0:4b10df072322 312 wait(1.0);
andriuxone 0:4b10df072322 313 }
andriuxone 0:4b10df072322 314
andriuxone 0:4b10df072322 315 //MUEVE A POSICIÓN MATERIAL
andriuxone 0:4b10df072322 316 void mover_material()
andriuxone 0:4b10df072322 317 {
andriuxone 0:4b10df072322 318 myservo1.pulsewidth_us(700);
andriuxone 0:4b10df072322 319 wait(1.0);
andriuxone 0:4b10df072322 320 myservo2.pulsewidth_us(650);
andriuxone 0:4b10df072322 321 wait(1.0);
andriuxone 0:4b10df072322 322 myservo3.pulsewidth_us(630);
andriuxone 0:4b10df072322 323 wait(1.0);
andriuxone 0:4b10df072322 324 }
andriuxone 0:4b10df072322 325
andriuxone 0:4b10df072322 326 //MUEVE A POSICIÓN CELDA1
andriuxone 0:4b10df072322 327 void mover_celda1()
andriuxone 0:4b10df072322 328 {
andriuxone 0:4b10df072322 329 myservo1.pulsewidth_us(800);
andriuxone 0:4b10df072322 330 wait(1.0);
andriuxone 0:4b10df072322 331 myservo2.pulsewidth_us(700);
andriuxone 0:4b10df072322 332 wait(1.0);
andriuxone 0:4b10df072322 333 myservo3.pulsewidth_us(660);
andriuxone 0:4b10df072322 334 wait(1.0);
andriuxone 0:4b10df072322 335 }
andriuxone 0:4b10df072322 336
andriuxone 0:4b10df072322 337 //MUEVE A POSICIÓN CELDA2
andriuxone 0:4b10df072322 338 void mover_celda2()
andriuxone 0:4b10df072322 339 {
andriuxone 0:4b10df072322 340 myservo1.pulsewidth_us(900);
andriuxone 0:4b10df072322 341 wait(1.0);
andriuxone 0:4b10df072322 342 myservo2.pulsewidth_us(750);
andriuxone 0:4b10df072322 343 wait(1.0);
andriuxone 0:4b10df072322 344 myservo3.pulsewidth_us(690);
andriuxone 0:4b10df072322 345 wait(1.0);
andriuxone 0:4b10df072322 346 }
andriuxone 0:4b10df072322 347
andriuxone 0:4b10df072322 348 //MUEVE A POSICIÓN CELDA3
andriuxone 0:4b10df072322 349 void mover_celda3()
andriuxone 0:4b10df072322 350 {
andriuxone 0:4b10df072322 351 myservo1.pulsewidth_us(1000);
andriuxone 0:4b10df072322 352 wait(1.0);
andriuxone 0:4b10df072322 353 myservo2.pulsewidth_us(770);
andriuxone 0:4b10df072322 354 wait(1.0);
andriuxone 0:4b10df072322 355 myservo3.pulsewidth_us(720);
andriuxone 0:4b10df072322 356 wait(1.0);
andriuxone 0:4b10df072322 357 }
andriuxone 0:4b10df072322 358
andriuxone 0:4b10df072322 359 //MUEVE A POSICIÓN CELDA4
andriuxone 0:4b10df072322 360 void mover_celda4()
andriuxone 0:4b10df072322 361 {
andriuxone 0:4b10df072322 362 myservo1.pulsewidth_us(1100);
andriuxone 0:4b10df072322 363 wait(1.0);
andriuxone 0:4b10df072322 364 myservo2.pulsewidth_us(780);
andriuxone 0:4b10df072322 365 wait(1.0);
andriuxone 0:4b10df072322 366 myservo3.pulsewidth_us(750);
andriuxone 0:4b10df072322 367 wait(1.0);
andriuxone 0:4b10df072322 368 }
andriuxone 0:4b10df072322 369
andriuxone 0:4b10df072322 370 //ABRE PINZA
andriuxone 0:4b10df072322 371 void abrir_pinza()
andriuxone 0:4b10df072322 372 {
andriuxone 0:4b10df072322 373 myservo4.pulsewidth_us(600);
andriuxone 0:4b10df072322 374 }
andriuxone 0:4b10df072322 375
andriuxone 0:4b10df072322 376 //CIERRA PINZA
andriuxone 0:4b10df072322 377 void cerrar_pinza()
andriuxone 0:4b10df072322 378 {
andriuxone 0:4b10df072322 379 myservo4.pulsewidth_us(900);
andriuxone 0:4b10df072322 380 }
andriuxone 0:4b10df072322 381
andriuxone 0:4b10df072322 382 // Mover Servo con posicion
andriuxone 0:4b10df072322 383 void mover_servo(uint8_t motor, uint8_t pos){
andriuxone 0:4b10df072322 384
andriuxone 0:4b10df072322 385 int position = degrees2usec(pos);
andriuxone 0:4b10df072322 386
andriuxone 0:4b10df072322 387 switch (motor) {
andriuxone 0:4b10df072322 388
andriuxone 0:4b10df072322 389 case 0x01:
andriuxone 0:4b10df072322 390 myservo1.pulsewidth_us(position);
andriuxone 0:4b10df072322 391 break;
andriuxone 0:4b10df072322 392 case 0x02:
andriuxone 0:4b10df072322 393 myservo2.pulsewidth_us(position);
andriuxone 0:4b10df072322 394 break;
andriuxone 0:4b10df072322 395 case 0x03:
andriuxone 0:4b10df072322 396 myservo3.pulsewidth_us(position);
andriuxone 0:4b10df072322 397 break;
andriuxone 0:4b10df072322 398 case 0x04:
andriuxone 0:4b10df072322 399 myservo4.pulsewidth_us(position);
andriuxone 0:4b10df072322 400 break;
andriuxone 0:4b10df072322 401 default: break;
andriuxone 0:4b10df072322 402 }
andriuxone 0:4b10df072322 403
andriuxone 0:4b10df072322 404 }
andriuxone 0:4b10df072322 405
andriuxone 0:4b10df072322 406 /*CONVERTIR GRADOS A SEGUNDOS*/
andriuxone 0:4b10df072322 407 uint32_t degrees2usec(uint8_t grados)
andriuxone 0:4b10df072322 408 {
andriuxone 0:4b10df072322 409 // Retorna el valor en microsegundos, donde
andriuxone 0:4b10df072322 410 // y − y1 = m(x − x1 )
andriuxone 0:4b10df072322 411 // y= b + mx
andriuxone 0:4b10df072322 412
andriuxone 0:4b10df072322 413 if(grados <= DEGREES_MAX)
andriuxone 0:4b10df072322 414 return int(750+grados*1900/180);// u6
andriuxone 0:4b10df072322 415 return 750;
andriuxone 0:4b10df072322 416
andriuxone 0:4b10df072322 417 }
andriuxone 0:4b10df072322 418
andriuxone 0:4b10df072322 419 /*MOVIMIENTOS CON JOYSTICK*/
andriuxone 0:4b10df072322 420 void mover_con_joystick(){
andriuxone 0:4b10df072322 421
andriuxone 0:4b10df072322 422 double base = 0.0;
andriuxone 0:4b10df072322 423 double izquierdo = 0.0;
andriuxone 0:4b10df072322 424 double derecho = 0.0;
andriuxone 0:4b10df072322 425
andriuxone 0:4b10df072322 426 int posBase=600;
andriuxone 0:4b10df072322 427 int posIzquierdo=600;
andriuxone 0:4b10df072322 428 int posDerecho=600;
andriuxone 0:4b10df072322 429
andriuxone 0:4b10df072322 430 while(1){
andriuxone 0:4b10df072322 431 base = analog1_value_x.read() * 3.3;
andriuxone 0:4b10df072322 432 izquierdo = analog1_value_y.read() * 3.3;
andriuxone 0:4b10df072322 433 derecho= analog2_value_x.read() * 3.3;
andriuxone 0:4b10df072322 434
andriuxone 0:4b10df072322 435 //Valida Joystick 1 en X - base
andriuxone 0:4b10df072322 436 if (base < 2.0){
andriuxone 0:4b10df072322 437 posBase= posBase+3;
andriuxone 0:4b10df072322 438 mover_base(posBase);
andriuxone 0:4b10df072322 439 } else if(base > 3.0){
andriuxone 0:4b10df072322 440 posBase= posBase-3;
andriuxone 0:4b10df072322 441 mover_base(posBase);
andriuxone 0:4b10df072322 442 }
andriuxone 0:4b10df072322 443
andriuxone 0:4b10df072322 444 //Valida Joystick 1 en Y - Izquierdo
andriuxone 0:4b10df072322 445 if (izquierdo < 2){
andriuxone 0:4b10df072322 446 posIzquierdo= posIzquierdo+3;
andriuxone 0:4b10df072322 447 mover_izquierdo(posIzquierdo);
andriuxone 0:4b10df072322 448 } else if(izquierdo > 3){
andriuxone 0:4b10df072322 449 posIzquierdo= posIzquierdo-13;
andriuxone 0:4b10df072322 450 mover_izquierdo(posIzquierdo);
andriuxone 0:4b10df072322 451 }
andriuxone 0:4b10df072322 452
andriuxone 0:4b10df072322 453 //Valida Joystic 2 en X - Derecho
andriuxone 0:4b10df072322 454 if (derecho < 2){
andriuxone 0:4b10df072322 455 posDerecho= posDerecho+3;
andriuxone 0:4b10df072322 456 mover_derecho(posDerecho);
andriuxone 0:4b10df072322 457 } else if(derecho > 3){
andriuxone 0:4b10df072322 458 posDerecho= posDerecho-3;
andriuxone 0:4b10df072322 459 mover_derecho(posDerecho);
andriuxone 0:4b10df072322 460 }
andriuxone 0:4b10df072322 461
andriuxone 0:4b10df072322 462 if(sw2.read() == 0){
andriuxone 0:4b10df072322 463 cerrar_pinza();
andriuxone 0:4b10df072322 464 } else if(sw2.read() == 1){
andriuxone 0:4b10df072322 465 abrir_pinza();
andriuxone 0:4b10df072322 466 }
andriuxone 0:4b10df072322 467
andriuxone 0:4b10df072322 468 }
andriuxone 0:4b10df072322 469 }
andriuxone 0:4b10df072322 470
andriuxone 0:4b10df072322 471 //LECTURA DE COLOR
andriuxone 0:4b10df072322 472 void leer_color(){
andriuxone 0:4b10df072322 473
andriuxone 0:4b10df072322 474 long red;
andriuxone 0:4b10df072322 475 long green;
andriuxone 0:4b10df072322 476 long blue;
andriuxone 0:4b10df072322 477 long clear;
andriuxone 0:4b10df072322 478
andriuxone 0:4b10df072322 479 red = scolor.ReadRed();
andriuxone 0:4b10df072322 480 green = scolor.ReadGreen();
andriuxone 0:4b10df072322 481 blue = scolor.ReadBlue();
andriuxone 0:4b10df072322 482 clear = scolor.ReadClear();
andriuxone 0:4b10df072322 483
andriuxone 0:4b10df072322 484 if (red < 100 && green<100 && blue<100){
andriuxone 0:4b10df072322 485 pc.printf("COLOR LEIDO: CLARO\n");
andriuxone 0:4b10df072322 486 pc.printf("%#04x","04");
andriuxone 0:4b10df072322 487 pc.printf("\n");
andriuxone 0:4b10df072322 488 }else if (blue<100){
andriuxone 0:4b10df072322 489 pc.printf("COLOR LEIDO: AZUL\n");
andriuxone 0:4b10df072322 490 pc.printf("%#04x","02");
andriuxone 0:4b10df072322 491 pc.printf("\n");
andriuxone 0:4b10df072322 492 }else if (red < 100){
andriuxone 0:4b10df072322 493 pc.printf("COLOR LEIDO: ROJO\n");
andriuxone 0:4b10df072322 494 pc.printf("%#04x","01");
andriuxone 0:4b10df072322 495 pc.printf("\n");
andriuxone 0:4b10df072322 496 }else if (red > 100 && green>100 && blue>100 && red < 140 && green<140 && blue<140){
andriuxone 0:4b10df072322 497 pc.printf("COLOR LEIDO: VERDE\n");
andriuxone 0:4b10df072322 498 pc.printf("%#04x","03");
andriuxone 0:4b10df072322 499 pc.printf("\n");
andriuxone 0:4b10df072322 500 }
andriuxone 0:4b10df072322 501
andriuxone 0:4b10df072322 502 pc.printf("RED: %5d GREEN: %5d BLUE: %5d CLEAR: %5d \n ", red, green, blue, clear);
andriuxone 0:4b10df072322 503 }