main.cpp

00001 #include "mbed.h"
00002  
00003 /*****************************************************************************
00004 generar un programa que controle por el puerto serial el grado de 4 servo motores.
00005 por medio de la comunicacion serial el comando es 
00006  
00007 |            |            |             |
00008 |   INITCMD  |     TC     |     PA      |
00009 |    0xFF    | 0x01- 0x02 | 0x00 - 0x06 |
00010  
00011 para enviar los comandos usar el programa Coolterm http://freeware.the-meiers.org/
00012  
00013 # para el servo motor se debe modificar el ciclo util del PWM SEGUN:
00014 #     ________              ________
00015 # ___|        |____________|        |___________
00016 #    <-width->
00017 #    <-------period 20ms--->
00018 # period = 20 ms
00019 # width  = 1000us y 2400us
00020 #  1000us para 0 grados
00021 #  2400us para 180 grados
00022 #
00023  
00024 *****************************************************************************/
00025  
00026  
00027 Serial command(USBTX, USBRX);
00028 PwmOut myservo1(PB_6);
00029 PwmOut myservo2(PA_8);
00030 PwmOut myservo3(PA_11);
00031 PwmOut myservo4(PB_15);
00032 /*INGRESE LA CONFIGURACION DE LOS MOTORES*/
00033  
00034 #define INITCMD 0xFF
00035 #define DEGREES_MAX 180
00036  
00037 // definición de las variables globales 
00038  
00039 uint8_t TC;    // Telecomando - variable almacena el movimiento en leer_datos()
00040 uint8_t PA;    // Parametro - varable almacena los grados a los que se mueven los motores en leer_datos()
00041  
00042  
00043 // definición de las funciones
00044 void setup_uart();
00045 void setup_servo();
00046 
00047 void mover_brazo(uint8_t PA);
00048 void mover_pinza(uint8_t PA);
00049 void mover_servo(uint8_t motor, uint8_t grados);
00050 uint32_t degrees2usec(uint8_t grados);
00051  
00052 void leer_datos();
00053  
00054     
00055     
00056 int main() {
00057  
00058     setup_uart();
00059     setup_servo();
00060     //command.printf("inicio de programa");
00061     while(1){    
00062         leer_datos();
00063         switch(TC){
00064             case 0x01:
00065                 mover_brazo(PA);
00066                 break;
00067             case 0x02:
00068                 mover_pinza(PA);
00069                 break;
00070             default:
00071                 if(TC<=06)
00072                 mover_servo(TC-2, PA);
00073                 break; 
00074         }
00075     }    
00076 }
00077 
00078 void mover_brazo(uint8_t PA){
00079     switch(PA){
00080         case 0x01: //Home
00081             mover_servo(1, 45);
00082             mover_servo(2, 0);
00083             mover_servo(3, 90);
00084             mover_servo(4, 0);
00085             break;
00086         case 0x02: //Material
00087             mover_servo(1, 0);
00088             mover_servo(2, 0);
00089             mover_servo(3, 90);
00090             mover_servo(4, 90);
00091             break;
00092         case 0x03: //Celda 1
00093             mover_servo(1, 180);
00094             mover_servo(2, 0);
00095             mover_servo(3, 90);
00096             mover_servo(4, 90);
00097             break;
00098         case 0x04: //Celda 2
00099             mover_servo(1, 160);
00100             mover_servo(2, 0);
00101             mover_servo(3, 90);
00102             mover_servo(4, 90);
00103             break;
00104         case 0x05: //Celda 3
00105             mover_servo(1, 140);
00106             mover_servo(2, 0);
00107             mover_servo(3, 90);
00108             mover_servo(4, 90);
00109             break;
00110         case 0x06: //Celda 4
00111             mover_servo(1, 120);
00112             mover_servo(2, 0);
00113             mover_servo(3, 90);
00114             mover_servo(4, 90);
00115             break;
00116         default: 
00117             mover_servo(1, 45);
00118             mover_servo(2, 0);
00119             mover_servo(3, 90);
00120             mover_servo(4, 0);
00121             break;      
00122     }
00123 } 
00124 
00125 void mover_pinza(uint8_t PA){ 
00126     switch(PA){
00127         case 0x01:
00128             mover_servo(4, 180);
00129             break;
00130         case 0x02:
00131             mover_servo(4, 45);
00132             break;
00133     }
00134 }
00135  
00136 void setup_uart(){
00137     command.baud(9600);
00138 }
00139  
00140 void setup_servo(){
00141     myservo1.period_ms(20);
00142     myservo1.pulsewidth_us(2000);
00143 }
00144  
00145 void leer_datos(){
00146     while(command.getc()!= INITCMD);
00147     TC=command.getc();
00148     PA=command.getc();
00149 }
00150  
00151  
00152 uint32_t degrees2usec(uint8_t grados){
00153 // Retorna el valor en microsegundos, donde
00154 //      y − y1 = m(x − x1 )
00155 //      y=  b + mx
00156  
00157     if(grados <= DEGREES_MAX)
00158         return int(1000+grados*1400/180);// u6
00159     return 1000;
00160     
00161 }
00162  
00163 void mover_servo(uint8_t motor, uint8_t grados){
00164  
00165         uint32_t dpulse=0;
00166         
00167 /* complementar el código necesario
00168    tip: deben pasar  grados a microsegundo */
00169  
00170  
00171     dpulse=degrees2usec(grados);
00172     if (motor==1)
00173          myservo1.pulsewidth_us(dpulse);
00174     if (motor==2)
00175          myservo2.pulsewidth_us(dpulse);
00176     if (motor==3)
00177          myservo3.pulsewidth_us(dpulse);
00178     if (motor==4)
00179          myservo4.pulsewidth_us(dpulse);         
00180 }