Diff: Motor_tryout.cpp

Revision:

30:390cab7cd6e6

Parent:

29:7eb028b359a1

Child:

31:3c13f1c35ee5

diff -r 7eb028b359a1 -r 390cab7cd6e6 Motor_tryout.cpp
--- a/Motor_tryout.cpp	Tue Oct 29 12:45:05 2019 +0000
+++ b/Motor_tryout.cpp	Thu Oct 31 09:56:54 2019 +0000
@@ -13,6 +13,10 @@
 PwmOut motor3_pwm(PTC4);
 DigitalOut motor3_dir(PTC12);
 
+AnalogIn potmeter1(A1);
+
+
+
 QEI Encoder1(D12,D13,NC,64,QEI::X4_ENCODING);
 QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING);
 QEI Encoder3(D2,D3,NC,64,QEI::X4_ENCODING);
@@ -65,23 +69,117 @@
 float oldtheta3=0;
 
 // Constant values for PI
-double Kp = 17.5;
-double Ki = 1.02;
-double Ts = 0.0025;
-float new_steps1 = 0;
+float Kp;
+float Kd;
+
+float Ts = 0.0025;
 float error1 = 0;
+float error2 = 0;
+float error3 = 0;
 
-float u_k = 0;
-float u_i = 0;
+float newmotor1 = 1;
+float newmotor2 = 1;
+float newmotor3 = 1;
+
+float u_k1 = 0;
+float u_k2 = 0;
+float u_k3 = 0;
+
+float u_d1 = 0;
+float u_d2 = 0;
+float u_d3 = 0;
+
+float lasterror1 = 0;
+float lasterror2 = 0;
+float lasterror3 = 0;
+
+float derivative1 = 0;
+float derivative2 = 0;
+float derivative3 = 0;
+
+bool timer = false;
+
+Ticker      motor_timer; 
+void motor()
+{ 
+    counts1 = Encoder1.getPulses();
+    counts2 = Encoder2.getPulses();
+    counts3 = Encoder3.getPulses();
+
+//    angle1 = counts1/(8400.0)*2.0*pie; 
+//    angle2 = counts2/(8400.0)*2.0*pie; 
+//    angle3 = counts3/(8400.0)*2.0*pie; 
+
+//    sinewave = sin(time_sin);
 
-Ticker pulses;
-void getpulses()   {
-        counts1 = Encoder1.getPulses();
-        counts2 = Encoder2.getPulses();
-        counts3 = Encoder3.getPulses();
+    lasterror1 = error1;
+    lasterror2 = error2;
+    lasterror3 = error3;
+    
+    error1 = counts1 - steps1;
+    error2 = counts2 - steps2;
+    error3 = counts3 - steps3;
+    
+    derivative1 = error1 - lasterror1;
+    derivative2 = error2 - lasterror2;
+    derivative3 = error3 - lasterror3;
+    
+    Kp = 50;
+    Kd = potmeter1 * 100;
+    
+    
+    // Proportional part
+    u_k1 = Kp * error1;
+    u_k2 = Kp * error2;
+    u_k3 = Kp * error3;
+    
+    u_d1 = Kd * derivative1;
+    u_d2 = Kd * derivative2;
+    u_d3 = Kd * derivative3;
+
+    newmotor1 = u_k1 + u_d1;
+    newmotor2 = u_k2 + u_d2;
+    newmotor3 = u_k3 + u_d3;
+
+    if (newmotor1>1.0f){
+        newmotor1 =1.0f;
         }
+    if (newmotor1<-1.0f){
+        newmotor1 = -1.0f;
+        }
+    
+    if (newmotor2>1.0f){
+        newmotor2 =1.0f;
+        }
+    if (newmotor2<-1.0f){
+        newmotor2 = -1.0f;
+        }
+        
+    if (newmotor3>1.0f){
+        newmotor3 =1.0f;
+        }
+    if (newmotor3<-1.0f){
+        newmotor3 = -1.0f;
+        }
+    
+    if (timer == true)  {
+    motor1_pwm.write(fabs(newmotor1));
+    motor1_dir.write(newmotor1>0);
+    
+    motor2_pwm.write(fabs(newmotor2));
+    motor2_dir.write(newmotor2<0);
+    
+    motor3_pwm.write(fabs(newmotor3));
+    motor3_dir.write(newmotor3>0);
+    }
+    
+    else  {
+    motor1_pwm.write(0);
+    motor2_pwm.write(0);
+    motor3_pwm.write(0);
+        }
+}
       
-
      
 float delta_calcangleyz(float x00, float y00, float z00)   {
     float y2 = y00 + le;
@@ -132,20 +230,6 @@
 }
             
 //double error;
-Ticker piticker;
-void PIcontroller()
-{
-    error1 = steps1-counts1;
-    static float error1_integral = 0;
-    
-    // Proportional part
-    u_k = Kp * error1;
-    
-    // Integral part
-    error1_integral = error1_integral + error1 * Ts;
-    u_i = Ki * error1_integral;
-    new_steps1 = u_k + u_i;
-}           
 
 // Calculate the steps from angle
 float anglestep(float angle)    {
@@ -165,94 +249,111 @@
     theta2 = theta2 - oldtheta2;
     theta3 = theta3 - oldtheta3;    
                 
-    steps1 = anglestep(theta1);
-    steps2 = anglestep(theta2);
-    steps3 = anglestep(theta3);
+    steps1 = steps1 + anglestep(theta1);
+    steps2 = steps2 + anglestep(theta2);
+    steps3 = steps3 + anglestep(theta3);
     pc.printf("\n\rsteps1 %f, steps2 %f, steps3 %f", steps1, steps2, steps3);
+    pc.printf("\n\rcounts1 %f, counts2 %f, counts3 %f", counts1, counts2, counts3);
+
     
 
-    pc.printf("\n\r error1: %f", error1);
-    pc.printf("\n\rnew steps1: %f", new_steps1);
+//    pc.printf("\n\r error1: %f", error1);
+
 
     // Set the direction of the motors.   
-    if (theta1 < 0) {
-        motor1_dir.write(1); 
-    }
-        else {
-            motor1_dir.write(0);
-        }
-    
-    if (theta2 < 0) {
-        motor2_dir.write(0); 
-    }
-        else {
-            motor2_dir.write(1);
-        }
-    
-    if (theta3 < 0) {
-        motor3_dir.write(0); 
-    }
-        else {
-            motor3_dir.write(1);
-        }
-    
-    int frequency_pwm = 10000; //10 kHz PWM
-    motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f
-    motor1_pwm.write(0.57); // write Duty Cycle  
-    
-    motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f
-    motor2_pwm.write(0.57); // write Duty Cycle  
-    
-    motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f
-    motor3_pwm.write(0.57); // write Duty Cycle  
-    
-    check1 = true;
-    check2 = true;
-    check3 = true; 
-
-    Encoder1.reset();
-    Encoder2.reset();
-    Encoder3.reset(); 
+ //   if (theta1 < 0) {
+//        motor1_dir.write(1); 
+//    }
+//        else {
+//            motor1_dir.write(0);
+//        }
+//    
+//    if (theta2 < 0) {
+//        motor2_dir.write(0); 
+//    }
+//        else {
+//            motor2_dir.write(1);
+//        }
+//    
+//    if (theta3 < 0) {
+//        motor3_dir.write(0); 
+//    }
+//        else {
+//            motor3_dir.write(1);
+//        }
+//    
+//    int frequency_pwm = 10000; //10 kHz PWM
+//    motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f
+//    motor1_pwm.write(0.57); // write Duty Cycle  
+//    
+//    motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f
+//    motor2_pwm.write(0.57); // write Duty Cycle  
+//    
+//    motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f
+//    motor3_pwm.write(0.57); // write Duty Cycle  
+//    
+//    check1 = true;
+//    check2 = true;
+//    check3 = true; 
+//
+//    Encoder1.reset();
+//    Encoder2.reset();
+//    Encoder3.reset(); 
+//    
 }
 
 // Omschrijving
-float movefunction() {   
-       
-    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\r 1 is false");
-            motor1_pwm.write(0);
-            check1=false;  
-            pc.printf("\n\rcounts1 %i", counts1);
-            }
-            
-        if (abs(counts2)>=abs(steps2))   {
-            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\r 3 is false");
-            motor3_pwm.write(0);
-            check3=false;
-            pc.printf("\n\rcounts3 %i", counts3);
-            }
-    }
-        pc.printf("\n\rcounts1 %i, counts2 %i, counts3 %i", counts1, counts2, counts3);
-
-            wait(3.0);
-        pc.printf("\n\rcounts1 %i, counts2 %i, counts3 %i", counts1, counts2, counts3);
-}
+float movestop() {  
+wait(0.6);
+timer = false;
+} 
+//       
+//    while (check1 || check2 || check3)    {
+//
+//        int frequency_pwm = 10000; //10 kHz PWM
+//        motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f
+//        motor1_pwm.write(newmotor1); // write Duty Cycle
+//        
+//        motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f
+//        motor2_pwm.write(newmotor2); // write Duty Cycle
+//        
+//        motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f
+//        motor3_pwm.write(newmotor3); // write Duty Cycle
+//        
+//        if (error1==0)  {
+//            check1=false;
+//            }
+//        if (error2==0)  {
+//            check2=false;
+//            }
+//        if (error3==0)  {
+//            check3=false;
+//            }
+//
+////       if(abs(counts1)>=abs(steps1)) {
+////            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\r 2 is false");
+////            motor2_pwm.write(0);
+////            check2=false;
+////            pc.printf("\n\rcounts2 %i", counts2);
+////            }
+////            
+////        if (abs(counts3)>=abs(steps3))  {
+////            pc.printf("\n\r 3 is false");
+////            motor3_pwm.write(0);
+////            check3=false;
+////            pc.printf("\n\rcounts3 %i", counts3);
+////            }
+//    }
+//            wait(1.0);
+//        pc.printf("\n\rdone moving");
+//}
 
 
 ///////////////////
@@ -260,39 +361,47 @@
 ///////////////////
 
 int main(void)  {    
+    motor_timer.attach(&motor, 0.002);
 
     pc.baud(115200);  
     char cc = pc.getc();
+    
+    int frequency_pwm = 10000; //10 kHz PWM
+//    motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f
+//    motor1_pwm.write(newmotor1); // write Duty Cycle
+//        
+//    motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f
+//    motor2_pwm.write(newmotor2); // write Duty Cycle
+//        
+//    motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f
+//    motor3_pwm.write(newmotor3); // write Duty Cycle
 
     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();
-        Encoder3.reset();
-        counts1 = Encoder1.getPulses();
-        counts2 = Encoder2.getPulses();
-        counts3 = Encoder3.getPulses();
+
         
         delta_calcinverse(x0,y0,z0);
     
         oldtheta1 = theta1;
         oldtheta2 = theta2;
         oldtheta3 = theta3; 
-                    
+                            
         char cc = pc.getc();
         
+        timer = true;
+
+        
         if (cc=='d') {
             x0=x0+5.0f;
             if (x0>=-75 && x0<=75){
                 movefunctioninit ();
-                movefunction ();
+                movestop();
             }
             else {
+                timer = false;
                 x0=x0-5.0f;
+                
             }
         }
         
@@ -300,9 +409,10 @@
             x0=x0-5.0f;
             if (x0>=-75 && x0<=75){
                 movefunctioninit ();
-                movefunction ();
+                movestop();
             }
             else {
+                timer = false;
                 x0=x0+5.0f;
             }
         }
@@ -311,9 +421,10 @@
             y0=y0+5.0f;
             if (y0>=-75 && y0<=75){
                 movefunctioninit ();
-                movefunction ();
+                movestop();
             }
             else {
+                timer = false;
                 y0=y0-5.0f;
             }
         }
@@ -322,9 +433,10 @@
             y0=y0-5.0f;
             if (y0>=-75 && y0<=75)   {
                 movefunctioninit ();
-                movefunction ();
+                movestop();
                 }
             else {
+                timer = false;
                 y0=y0+5.0f;
             }
         }
@@ -334,9 +446,14 @@
             z0=z0+5.0f;
             if (z0>=-210 && z0<=-130) {
                 movefunctioninit ();
-                movefunction ();
+                movestop();
+//                for (float i=0.0; i<5; i++)  {
+//                pc.printf("\n\r dit is error1 %f, error2 %f, error3 %f", error1, error2, error3);
+//                wait(1.0/10.0);
+//                }
             }
             else{
+                timer = false;
                 z0=z0-5.0f;
             }
         }
@@ -346,9 +463,10 @@
             z0=z0-5.0f;
             if (z0>=-210 && z0<=-130){
                 movefunctioninit ();
-                movefunction ();
+                movestop();
             }
             else {
+                timer = false;
                 z0=z0+5.0f;
             }
         }