Diff: Motor_tryout.cpp

Revision:

28:43a1d67ff8ea

Parent:

27:3eb181cbe183

Child:

29:7eb028b359a1

--- a/Motor_tryout.cpp	Tue Oct 29 11:10:23 2019 +0000
+++ b/Motor_tryout.cpp	Tue Oct 29 12:05:47 2019 +0000
@@ -64,6 +64,13 @@
 float oldtheta2=0;
 float oldtheta3=0;
 
+// Constant values for PI
+double Kp = 17.5;
+double Ki = 1.02;
+double Ts = 0.0025;
+float new_steps1 = 0;
+float error1 = 0;
+
 Ticker pulses;
 void getpulses()   {
         counts1 = Encoder1.getPulses();
@@ -71,7 +78,8 @@
         counts3 = Encoder3.getPulses();
         }
       
-        
+
+     
 float delta_calcangleyz(float x00, float y00, float z00)   {
     float y2 = y00 + le;
     float y1 = f;
@@ -94,8 +102,10 @@
                     }
                 float theta1 = (beta - alpha)*180.0/pi;
                 return theta1;
+    }
 }
-}
+
+
 
 float delta_calcinverse(float x00, float y00, float z00) {
     theta1 = theta2 = theta3 = 0;
@@ -118,16 +128,32 @@
     return theta1, theta2, theta3;
 }
             
-            
+//double error;
+float PI_controller()
+{
+    float error1 = steps1-counts1;
+    static double error1_integral = 0;
+    
+    // Proportional part
+    double 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;
+}           
 
+// Calculate the steps from angle
 float anglestep(float angle)    {
     float steps;
     steps = angle / 360 * 8400;
     return steps;
     }
 
-float movefunctioninit ()   {
-        
+// Omschrijving
+float movefunctioninit ()   {   
     
     theta1 = delta_calcinverse(x00,y00,z00);
     pc.printf("\n\r de hoeken zijn(%f, %f, %f)", theta1, theta2, theta3);       
@@ -141,28 +167,33 @@
     steps2 = anglestep(theta2);
     steps3 = anglestep(theta3);
     pc.printf("\n\rsteps1 %f, steps2 %f, steps3 %f", steps1, steps2, steps3);
+    
+
+    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.   
     if (theta1 < 0) {
         motor1_dir.write(1); 
     }
-    else {
-        motor1_dir.write(0);
-    }
+        else {
+            motor1_dir.write(0);
+        }
     
     if (theta2 < 0) {
         motor2_dir.write(0); 
     }
-    else {
-        motor2_dir.write(1);
-    }
+        else {
+            motor2_dir.write(1);
+        }
     
     if (theta3 < 0) {
         motor3_dir.write(0); 
     }
-    else {
-        motor3_dir.write(1);
-    }
+        else {
+            motor3_dir.write(1);
+        }
     
     int frequency_pwm = 10000; //10 kHz PWM
     motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f
@@ -181,8 +212,9 @@
     Encoder1.reset();
     Encoder2.reset();
     Encoder3.reset(); 
-    }
+}
 
+// Omschrijving
 float movefunction() {   
        
     while (check1 || check2 || check3)    {
@@ -191,16 +223,17 @@
         if(abs(counts1)>=abs(steps1)) {
             pc.printf("\n 1 is false");
             motor1_pwm.write(0);
-            check1=false;
+            check1=false;  
             pc.printf("\n\rcounts1 %i", counts1);
             }
+            
         if (abs(counts2)>=abs(steps2))   {
             pc.printf("\n 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");
             motor3_pwm.write(0);
@@ -214,94 +247,102 @@
         pc.printf("\n\rcounts1 %i, counts2 %i, counts3 %i", counts1, counts2, counts3);
 }
 
+
+///////////////////
+// MAIN FUNCTION //
+///////////////////
+
 int main(void)  {    
 
-    pc.baud(115200);
-        
+    pc.baud(115200);  
     char cc = pc.getc();
 
-    while(true)     {
-    pulses.attach(&getpulses, 1.0/10000);
-    
-    Encoder1.reset();
-    Encoder2.reset();
-    Encoder3.reset();
-    counts1 = Encoder1.getPulses();
-    counts2 = Encoder2.getPulses();
-    counts3 = Encoder3.getPulses();
+    while(true){
+        pulses.attach(&getpulses, 1.0/10000);
+        
+        Encoder1.reset();
+        Encoder2.reset();
+        Encoder3.reset();
+        counts1 = Encoder1.getPulses();
+        counts2 = Encoder2.getPulses();
+        counts3 = Encoder3.getPulses();
+        
+        delta_calcinverse(x0,y0,z0);
     
-
-    delta_calcinverse(x0,y0,z0);
-
-    oldtheta1 = theta1;
-    oldtheta2 = theta2;
-    oldtheta3 = theta3;             
-char cc = pc.getc();
-
-if (cc=='d') {
-    x0=x0+5.0f;
-    if (x0>=-75 && x0<=75)   {
-        movefunctioninit ();
-        movefunction ();
+        oldtheta1 = theta1;
+        oldtheta2 = theta2;
+        oldtheta3 = theta3; 
+                    
+        char cc = pc.getc();
+        
+        if (cc=='d') {
+            x0=x0+5.0f;
+            if (x0>=-75 && x0<=75){
+                movefunctioninit ();
+                movefunction ();
+            }
+            else {
+                x0=x0-5.0f;
+            }
         }
-            else    {
-                x0=x0-5.0f;
-                }
-}
-
-if (cc=='a') {
-x0=x0-5.0f;
-if (x0>=-75 && x0<=75)   {
-    movefunctioninit ();
-    movefunction ();
-    }
-    else    {
-            x0=x0+5.0f;
+        
+        if (cc=='a') {
+            x0=x0-5.0f;
+            if (x0>=-75 && x0<=75){
+                movefunctioninit ();
+                movefunction ();
+            }
+            else {
+                x0=x0+5.0f;
             }
-}
-
-if (cc=='w') {
-y0=y0+5.0f;
-if (y0>=-75 && y0<=75)   {
-    movefunctioninit ();
-    movefunction ();
-    }
-    else    {
-            y0=y0-5.0f;
+        }
+        
+        if (cc=='w'){
+            y0=y0+5.0f;
+            if (y0>=-75 && y0<=75){
+                movefunctioninit ();
+                movefunction ();
+            }
+            else {
+                y0=y0-5.0f;
             }
-}
-
-if (cc=='s') {
-y0=y0-5.0f;
-if (y0>=-75 && y0<=75)   {
-    movefunctioninit ();
-    movefunction ();
-    }
-    else    {
-            y0=y0+5.0f;
+        }
+        
+        if (cc=='s'){
+            y0=y0-5.0f;
+            if (y0>=-75 && y0<=75)   {
+                movefunctioninit ();
+                movefunction ();
+                }
+            else {
+                y0=y0+5.0f;
             }
-}
-
-if (cc=='u') {
-z0=z0+5.0f;
-if (z0>=-210 && z0<=-130)   {
-    movefunctioninit ();
-    movefunction ();
-    }
-    else    {
-            z0=z0-5.0f;
+        }
+        
+        if (cc=='u') 
+        {
+            z0=z0+5.0f;
+            if (z0>=-210 && z0<=-130) {
+                movefunctioninit ();
+                movefunction ();
+            }
+            else{
+                z0=z0-5.0f;
             }
-}
-
-if (cc=='j') {
-z0=z0-5.0f;
-if (z0>=-210 && z0<=-130)   {
-    movefunctioninit ();
-    movefunction ();
+        }
+        
+        if (cc=='j') 
+        {
+            z0=z0-5.0f;
+            if (z0>=-210 && z0<=-130){
+                movefunctioninit ();
+                movefunction ();
+            }
+            else {
+                z0=z0+5.0f;
+            }
+        }
+    // end while                                           
     }
-    else    {
-            z0=z0+5.0f;
-            }
-}                                       
-}
+// end main
 }  
\ No newline at end of file