Diff: Motor_tryout.cpp

Revision:

27:3eb181cbe183

Parent:

26:432d3519ba86

Child:

28:43a1d67ff8ea

diff -r 432d3519ba86 -r 3eb181cbe183 Motor_tryout.cpp
--- a/Motor_tryout.cpp	Tue Oct 29 10:05:50 2019 +0000
+++ b/Motor_tryout.cpp	Tue Oct 29 11:10:23 2019 +0000
@@ -32,9 +32,7 @@
 int counts1 = 0;
 int counts2 = 0;
 int counts3 = 0;
-int count1old = 0;
-int countdiff = 0;
-int counting1 = 0;
+
 
 const float le = 15.0;
 const float f = 37.5;
@@ -66,16 +64,10 @@
 float oldtheta2=0;
 float oldtheta3=0;
 
-Ticker pulses1;
-void getpulses1()   {
+Ticker pulses;
+void getpulses()   {
         counts1 = Encoder1.getPulses();
-        }
-Ticker pulses2;
-void getpulses2()   {
         counts2 = Encoder2.getPulses();
-        }
-Ticker pulses3;
-void getpulses3()   {
         counts3 = Encoder3.getPulses();
         }
       
@@ -134,8 +126,8 @@
     return steps;
     }
 
-
-float movefunction() {
+float movefunctioninit ()   {
+        
     
     theta1 = delta_calcinverse(x00,y00,z00);
     pc.printf("\n\r de hoeken zijn(%f, %f, %f)", theta1, theta2, theta3);       
@@ -146,10 +138,10 @@
     theta3 = theta3 - oldtheta3;    
                 
     steps1 = anglestep(theta1);
-    pc.printf("\n\rsteps1.1 %f", steps1);
     steps2 = anglestep(theta2);
     steps3 = anglestep(theta3);
-    
+    pc.printf("\n\rsteps1 %f, steps2 %f, steps3 %f", steps1, steps2, steps3);
+
     // Set the direction of the motors.   
     if (theta1 < 0) {
         motor1_dir.write(1); 
@@ -174,85 +166,62 @@
     
     int frequency_pwm = 10000; //10 kHz PWM
     motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f
-    motor1_pwm.write(0.7); // write Duty Cycle  
+    motor1_pwm.write(0.57); // write Duty Cycle  
     
     motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f
-    motor2_pwm.write(0.7); // write Duty Cycle  
+    motor2_pwm.write(0.57); // write Duty Cycle  
     
     motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f
-    motor3_pwm.write(0.7); // write Duty Cycle  
+    motor3_pwm.write(0.57); // write Duty Cycle  
     
     check1 = true;
     check2 = true;
     check3 = true; 
-    
 
- //   for (int i=0; i<100; i++)  {
- //       pc.printf("\n\rcounts1 %i", counts1);
- //       wait(0.001);
- //       }
-    
-         pc.printf("\n\rsteps3.1 %f", steps3);
-        pc.printf("\n\rcounts3.1 %i", counts3);
+    Encoder1.reset();
+    Encoder2.reset();
+    Encoder3.reset(); 
+    }
 
-           
+float movefunction() {   
+       
     while (check1 || check2 || check3)    {
-        pc.printf("\n\rsteps1 %f, steps2 %f, steps3 %f",steps1, steps2, steps3);
         pc.printf("\n\rcounts1 %i, counts2 %i, counts3 %i", counts1, counts2, counts3);
 
-    if (steps1 <= 0)  {
-        if(counts1<=steps1) {
+        if(abs(counts1)>=abs(steps1)) {
             pc.printf("\n 1 is false");
             motor1_pwm.write(0);
             check1=false;
+            pc.printf("\n\rcounts1 %i", counts1);
             }
-        if (counts2<=steps2)   {
+        if (abs(counts2)>=abs(steps2))   {
             pc.printf("\n 2 is false");
             motor2_pwm.write(0);
             check2=false;
+            pc.printf("\n\rcounts2 %i", counts2);
+
             }
-        if (counts3>=abs(steps3))  {
+        if (abs(counts3)>=abs(steps3))  {
             pc.printf("\n 3 is false");
             motor3_pwm.write(0);
             check3=false;
-        pc.printf("\n\rsteps3 %f", steps3);
-        pc.printf("\n\rcounts3 %i", counts3);
-            }
-        }
-        if (steps1 >= 0)  {
-        if(counts1>=steps1) {
-            pc.printf("\n 1 is false");
-            motor1_pwm.write(0);
-            check1=false;
+            pc.printf("\n\rcounts3 %i", counts3);
             }
-        if (counts2>=steps2)   {
-            pc.printf("\n 2 is false");
-            motor2_pwm.write(0);
-            check2=false;
-            }
-        if (counts3<=-steps3)  {
-            pc.printf("\n 3 is false");
-            motor3_pwm.write(0);
-            check3=false;
-            }
-        }
+    }
+        pc.printf("\n\rcounts1 %i, counts2 %i, counts3 %i", counts1, counts2, counts3);
 
-    }
-
-            wait(1.0);
+            wait(3.0);
         pc.printf("\n\rcounts1 %i, counts2 %i, counts3 %i", counts1, counts2, counts3);
 }
 
 int main(void)  {    
-    pulses1.attach(&getpulses1, 1.0/1000);
-    pulses2.attach(&getpulses2, 1.0/1000);
-    pulses3.attach(&getpulses3, 1.0/1000);
 
     pc.baud(115200);
         
     char cc = pc.getc();
 
     while(true)     {
+    pulses.attach(&getpulses, 1.0/10000);
     
     Encoder1.reset();
     Encoder2.reset();
@@ -272,6 +241,7 @@
 if (cc=='d') {
     x0=x0+5.0f;
     if (x0>=-75 && x0<=75)   {
+        movefunctioninit ();
         movefunction ();
         }
             else    {
@@ -282,6 +252,7 @@
 if (cc=='a') {
 x0=x0-5.0f;
 if (x0>=-75 && x0<=75)   {
+    movefunctioninit ();
     movefunction ();
     }
     else    {
@@ -292,6 +263,7 @@
 if (cc=='w') {
 y0=y0+5.0f;
 if (y0>=-75 && y0<=75)   {
+    movefunctioninit ();
     movefunction ();
     }
     else    {
@@ -302,6 +274,7 @@
 if (cc=='s') {
 y0=y0-5.0f;
 if (y0>=-75 && y0<=75)   {
+    movefunctioninit ();
     movefunction ();
     }
     else    {
@@ -312,6 +285,7 @@
 if (cc=='u') {
 z0=z0+5.0f;
 if (z0>=-210 && z0<=-130)   {
+    movefunctioninit ();
     movefunction ();
     }
     else    {
@@ -322,6 +296,7 @@
 if (cc=='j') {
 z0=z0-5.0f;
 if (z0>=-210 && z0<=-130)   {
+    movefunctioninit ();
     movefunction ();
     }
     else    {