Diff: main.cpp

Revision:

5:ee733f4b9ea7

Parent:

4:136a0aeb7d51

--- a/main.cpp	Fri Feb 22 18:44:32 2019 +0000
+++ b/main.cpp	Fri Mar 01 12:44:03 2019 +0000
@@ -14,6 +14,11 @@
 
 Serial command(USBTX, USBRX);
 PwmOut myservo1(PB_4);
+PwmOut myservo2(PB_5);
+PwmOut myservo3(PB_10);
+PwmOut myservo4(PB_3);
+PwmOut myservo5(PB_6);
+
 /*INGRESE L A CONFIGURACION DE LOS MOTORES*/
 
 #define INITCMD 0xFF
@@ -22,14 +27,13 @@
 
 uint8_t N_motor;    // almacena el número de motor
 uint8_t N_grados;    // almacena los grados que se mueve el motor
-
+uint8_t N_tm;        //tipo de motor
 
 // definición de las funciones
 void setup_uart();
 void setup_servo();
 
-void mover_servo(uint8_t motor, uint8_t gradss);
-uint32_t degrees2usec(uint8_t grados){
+void mover_servo(uint8_t motor, uint8_t grados);
 
 void leer_datos();
 
@@ -39,7 +43,7 @@
 
     setup_uart();
     setup_servo();
-    //command.printf("inicio de programa");
+   // command.printf("inicio de programa");
     while(1){    
         leer_datos();
         mover_servo(N_motor, N_grados);
@@ -55,6 +59,14 @@
 void setup_servo(){
     myservo1.period_ms(20);
     myservo1.pulsewidth_us(1000);
+    myservo2.period_ms(20);
+    myservo2.pulsewidth_us(1000);
+    myservo3.period_ms(20);
+    myservo3.pulsewidth_us(1000);
+    myservo4.period_ms(20);
+    myservo4.pulsewidth_us(1000);
+    myservo5.period_ms(20);
+    myservo5.pulsewidth_us(1000);
 }
 
 void leer_datos(){
@@ -64,7 +76,15 @@
     
 }
 
-
+uint32_t degrees2usec(uint8_t grados,uint8_t tm){
+// Retorno el valor en usegundos, donde
+//     y − y1 = m(x − x1 )
+    if(grados <= DEGREES_MAX)
+        if (tm==0)return int(700+grados*1700/180);// u6
+        else return int(700+grados*1700/180);// u6
+    return 700;
+    
+}
 
 
 void mover_servo(uint8_t motor, uint8_t grados){
@@ -74,18 +94,17 @@
 /* complementar el código necesario
    tip: deben pasar  grados a microsegundo */
 
-
-//      dpulse=degrees2usec(grados);
-//       myservo1.pulsewidth_us(dpulse);
-
+    dpulse=degrees2usec(grados);
+    if (motor==1)
+        myservo1.pulsewidth_us(dpulse);
+    if (motor==2)
+        myservo2.pulsewidth_us(dpulse);
+    if (motor==3)
+        myservo3.pulsewidth_us(dpulse);
+    if (motor==4)
+        myservo4.pulsewidth_us(dpulse);
+    if (motor==5)
+        myservo5.pulsewidth_us(dpulse);
 }
 
 
-uint32_t degrees2usec(uint8_t grados){
-// Retorno el valor en usegundos, donde
-//     y − y1 = m(x − x1 )
-    if(grados <= DEGREES_MAX)
-        return int(750+grados*1900/180);// u6
-    return 750;
-    
-}
\ No newline at end of file