Diff: Motor_tryout.cpp

Revision:

29:7eb028b359a1

Parent:

28:43a1d67ff8ea

Child:

30:390cab7cd6e6

--- a/Motor_tryout.cpp	Tue Oct 29 12:05:47 2019 +0000
+++ b/Motor_tryout.cpp	Tue Oct 29 12:45:05 2019 +0000
@@ -71,6 +71,9 @@
 float new_steps1 = 0;
 float error1 = 0;
 
+float u_k = 0;
+float u_i = 0;
+
 Ticker pulses;
 void getpulses()   {
         counts1 = Encoder1.getPulses();
@@ -129,20 +132,19 @@
 }
             
 //double error;
-float PI_controller()
+Ticker piticker;
+void PIcontroller()
 {
-    float error1 = steps1-counts1;
-    static double error1_integral = 0;
+    error1 = steps1-counts1;
+    static float error1_integral = 0;
     
     // Proportional part
-    double u_k = Kp * error1;
+    u_k = Kp * error1;
     
     // Integral part
     error1_integral = error1_integral + error1 * Ts;
-    double u_i = Ki * error1_integral;
-       
-    // Sum all parts and return it
-    return u_k + u_i;
+    u_i = Ki * error1_integral;
+    new_steps1 = u_k + u_i;
 }           
 
 // Calculate the steps from angle
@@ -170,7 +172,6 @@
     
 
     pc.printf("\n\r error1: %f", error1);
-    new_steps1 = PI_controller();
     pc.printf("\n\rnew steps1: %f", new_steps1);
 
     // Set the direction of the motors.   
@@ -219,23 +220,29 @@
        
     while (check1 || check2 || check3)    {
         pc.printf("\n\rcounts1 %i, counts2 %i, counts3 %i", counts1, counts2, counts3);
+        
+        pc.printf("\n\rerror1: %f", error1); 
+         pc.printf("\n\ru_k: %f", u_k);   
+         pc.printf("\n\ru_i: %f", u_i); 
+         pc.printf("\n\ru_k + u_i: %f", (u_k+u_i)); 
+         pc.printf("\n\r"); 
 
         if(abs(counts1)>=abs(steps1)) {
-            pc.printf("\n 1 is false");
+            pc.printf("\n\r 1 is false");
             motor1_pwm.write(0);
             check1=false;  
             pc.printf("\n\rcounts1 %i", counts1);
             }
             
         if (abs(counts2)>=abs(steps2))   {
-            pc.printf("\n 2 is false");
+            pc.printf("\n\r 2 is false");
             motor2_pwm.write(0);
             check2=false;
             pc.printf("\n\rcounts2 %i", counts2);
             }
             
         if (abs(counts3)>=abs(steps3))  {
-            pc.printf("\n 3 is false");
+            pc.printf("\n\r 3 is false");
             motor3_pwm.write(0);
             check3=false;
             pc.printf("\n\rcounts3 %i", counts3);
@@ -258,7 +265,10 @@
     char cc = pc.getc();
 
     while(true){
+    pc.printf("\n\r----------------------------------------------------------");
+    pc.printf("\n\r----------------------------------------------------------");
         pulses.attach(&getpulses, 1.0/10000);
+        piticker.attach(&PIcontroller, 1.0/1000);
         
         Encoder1.reset();
         Encoder2.reset();
@@ -344,5 +354,6 @@
         }
     // end while                                           
     }
+     
 // end main
 }  
\ No newline at end of file