Diff: mover.cpp

Revision:

0:22b33429aff5

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mover.cpp	Thu Nov 22 01:19:55 2018 +0000
@@ -0,0 +1,251 @@
+#include "mover.h"
+#include "mbed.h"
+#include "math.h"
+
+PwmOut myServo1(PB_13);//DFA
+PwmOut myServo2(PB_14);//DFB
+PwmOut myServo3(PB_1);//DTA
+PwmOut myServo4(PB_9);//DTB
+PwmOut myServo5(PB_2);//IFA
+PwmOut myServo6(PA_11);//IFB
+PwmOut myServo7(PC_6);//ITA
+PwmOut myServo8(PC_8);//ITB
+
+
+  
+
+uint8_t  ss_time= 255;     // tiempo  de espera para moverse 1 mm en microsegundos
+
+void put_sstime(uint8_t vtime){
+    ss_time=vtime;
+    
+}
+
+int coord2us(float coord)
+{
+    if(0 <= coord <= MAXPOS)
+        return int(750+coord*1900/180);// u6
+    return 750;
+
+}
+
+
+ 
+
+
+void mover_ser(uint8_t motor, uint8_t pos){
+     int pulseX = coord2us(pos);
+     switch(motor){
+         
+    case 1:        
+    
+    myServo1.pulsewidth_us(pulseX) ; 
+  
+    
+    break;
+    
+    case 2:        
+    
+    myServo2.pulsewidth_us(pulseX);
+    
+    break;
+    
+    case 3:        
+    
+    myServo3.pulsewidth_us(pulseX);
+    
+    break;
+    
+    case 4:        
+    
+    myServo4.pulsewidth_us(pulseX);
+    
+    break;
+    
+    case 5:        
+    
+    myServo5.pulsewidth_us(pulseX);
+    
+    break;
+    
+    case 6:  myServo6.pulsewidth_us(pulseX); break;
+    
+    case 7:        
+    
+    myServo7.pulsewidth_us(pulseX);
+    
+    break;
+    
+    case 8:        
+    
+    myServo8.pulsewidth_us(pulseX);
+    
+    break;
+    }
+         
+     
+
+}
+
+void init_servo()
+{
+   myServo1.period_ms(20);
+   myServo2.period_ms(20);
+   myServo3.period_ms(20);
+   myServo4.period_ms(20);
+   myServo5.period_ms(20);
+   myServo6.period_ms(20);
+   myServo7.period_ms(20);
+   myServo8.period_ms(20);
+
+}
+void mover_ser2(uint8_t nmotor, uint8_t pos){
+     switch(nmotor){
+    case 1:        
+    if(pos==1){
+    myServo1.pulsewidth_us(2650);
+    }
+    else if(pos==2){
+    myServo1.pulsewidth_us(1500);
+    }
+    else if(pos==3){
+    myServo2.pulsewidth_us(2650);
+    }
+    else if(pos==4){
+    myServo2.pulsewidth_us(1600);
+    }
+    else{
+        printf("comando fuera de las selecciones posibles");
+        }
+    
+    
+    break;
+    
+    case 2:        
+    
+    if(pos==1){
+    myServo3.pulsewidth_us(2650);
+    }
+    else if(pos==2){
+    myServo3.pulsewidth_us(908);
+    }
+    else if(pos==3){
+    myServo4.pulsewidth_us(2650);
+    }
+    else if(pos==4){
+    myServo4.pulsewidth_us(1600);
+    }
+    else{
+        printf("comando fuera de las selecciones posibles");
+        }
+    
+    
+    break;
+    
+    case 3:        
+    
+   if(pos==1){
+    myServo5.pulsewidth_us(2650);
+    }
+    else if(pos==2){
+    myServo5.pulsewidth_us(1200);
+    }
+    else if(pos==3){
+    myServo6.pulsewidth_us(908);
+    }
+    else if(pos==4){
+    myServo6.pulsewidth_us(1600);
+    }
+    else{
+        printf("comando fuera de las selecciones posibles");
+        }
+    
+    
+    break;
+    
+    case 4:        
+    
+    if(pos==1){
+    myServo7.pulsewidth_us(2650);
+    }
+    else if(pos==2){
+    myServo7.pulsewidth_us(908);
+    }
+    else if(pos==3){
+    myServo8.pulsewidth_us(908);
+    }
+    else if(pos==4){
+    myServo8.pulsewidth_us(1800);
+    }
+    else{
+        printf("comando fuera de las selecciones posibles");
+        }
+    
+    
+    break;
+    
+    }
+    }
+     void mover_ser3(){
+        myServo1.pulsewidth_us(600);
+        myServo3.pulsewidth_us(2500);
+        myServo5.pulsewidth_us(2500);
+        myServo7.pulsewidth_us(700);
+    
+
+       
+        myServo2.pulsewidth_us(1400);
+        myServo4.pulsewidth_us(2400);
+        myServo6.pulsewidth_us(1700);
+        myServo8.pulsewidth_us(300);
+        wait(1);
+        myServo1.pulsewidth_us(1500);
+        wait(1);        
+        myServo2.pulsewidth_us(700);
+        wait(1);  
+        myServo1.pulsewidth_us(600);
+        wait(1);
+        myServo5.pulsewidth_us(1600);
+        wait(1);
+        myServo6.pulsewidth_us(2400);
+        wait(1);
+        myServo5.pulsewidth_us(2500);
+        
+        wait(1);
+        myServo7.pulsewidth_us(1700);
+        wait(1);
+        myServo8.pulsewidth_us(1400);
+        wait(1);
+        myServo7.pulsewidth_us(700);
+        
+        wait(1);
+        myServo3.pulsewidth_us(1600);
+        wait(1);
+        myServo4.pulsewidth_us(1700);
+        wait(1);
+        myServo3.pulsewidth_us(2500);
+        wait(1);
+        myServo2.pulsewidth_us(1400);
+        myServo4.pulsewidth_us(2400);
+        myServo6.pulsewidth_us(1700);
+        myServo8.pulsewidth_us(300);
+        
+        wait(1);
+     
+       
+   
+               
+        }
+    
+ 
+      
+    
+
+        
+        
+        
+        
+        
+        
+        
+        
\ No newline at end of file