Revision 8:31276b85a9a1, committed 2019-10-30

Comitter:

HestervdVen

Date:

Wed Oct 30 11:35:03 2019 +0000

Parent:

7:14c0c10a0090

Commit message:

Kp, Ki en Kd getweaked door trial and error.

Changed in this revision

--- a/main.cpp	Fri Oct 25 10:46:43 2019 +0000
+++ b/main.cpp	Wed Oct 30 11:35:03 2019 +0000
@@ -11,7 +11,6 @@
 #define M_PI acos(-1.0)
 
 Serial pc(USBTX, USBRX);
-Ticker EncTicker; // Ticker for encoder
 
 PwmOut msignal_1(D6); //Signal to motor controller 
 PwmOut msignal_2(D5);
@@ -19,10 +18,10 @@
 DigitalOut direction_2(D4);
 DigitalIn enc_1a(D10); //D10 to ENC1 A
 DigitalIn enc_1b(D9); //D9 to ENC1 B
-DigitalIn enc_2a(D11); //D11 to ENC2 A
-DigitalIn enc_2b(D12); //D12 to ENC2 B
+DigitalIn enc_2a(D2); //D11 to ENC2 A
+DigitalIn enc_2b(D1); //D12 to ENC2 B
 QEI Encoder_1(D10,D9,NC,64,QEI::X4_ENCODING); 
-QEI Encoder_2(D11,D12,NC,64,QEI::X4_ENCODING);
+QEI Encoder_2(D2,D1,NC,64,QEI::X4_ENCODING);
 
 //These seemed the best values after trying them out and using wikipedia's info
 const float kp_1 = 1;  
@@ -33,32 +32,23 @@
 const float kd_2 = 0.1;
 const float t_s = 0.0025; //sample time in sec; same for both motors
 
-/*
-//inputs for LPF; still to fill in!
-float a1;
-float a2;
-float b1;
-float b2;
-float c1;
-float c2;
-float d1;
-float d2;
-float e1;
-float e2;
-*/
 
-float currentAngle_1 = 0;
-float currentAngle_2 = 0;
+volatile float currentAngle_1 = 0;
+volatile float currentAngle_2 = 0;
+float error_prev_1 = 0;
+float error_prev_2 = 0;
+float error_integral_1 = 0;
+float error_integral_2 = 0;
 const float period = 10.0; //sets period to 10ms
 const float to_rads =(2*M_PI); //2*Pi
 const float CountsPerRevolution = 8400; // counts of the encoder per revolution (Property of motor)
 const float ShaftResolution = (to_rads)/(CountsPerRevolution); // calculates constant for relation between counts and radians (rad/count)
 volatile float width;
-volatile float width_percent = 0.5;
+volatile float width_percent = 0.5; //higher value is higher motor speed
 volatile float on_time_1;
 volatile float off_time_1;
-volatile int counts_1 = 0; // counts of Encoder_1
-volatile int counts_2 = 0; // counts of Encoder_2
+volatile float counts_1 = 0; // counts of Encoder_1; THIS HAS TO BE A FLOAT TO WORK
+volatile float counts_2 = 0; // counts of Encoder_2
 bool dir_1; //true = CCW, false = CW
 bool dir_2; //true = CCW, false = CW
 bool moving_1 = false;
@@ -70,13 +60,13 @@
     cin >> setpoint1;   //input on keyboard
     return setpoint1;
     }
-    
+   
 float getSetpoint_2(){
     float setpoint2; //setpoint
     cin >> setpoint2;   //input on keyboard
     return setpoint2;
     }
-    
+
 //calculates error that goes into controller
 float getError(float setpoint, float angle){
     float errorSum = setpoint - angle; 
@@ -96,7 +86,7 @@
       //pc.printf("Negative movement\r\n");
         }
      else{
-        direction_1 = 0;
+//direction_1 = 0;
         moving_1 = false;
       //pc.printf("No movement\r\n");
         }
@@ -106,21 +96,22 @@
 //checks if the motor is turning and in what direction
 void checkMovement_2(double outcome){
     if (outcome >= 0.1){
-        dir_2 = true;   //CCW rotation
+        dir_2 = false;   //CCW rotation
         moving_2 = true;
       //pc.printf("Positive movement\r\n");
         }
     else if (outcome <= -0.1){
-        dir_2 = false;     //CW rotation
+        dir_2 = true;     //CW rotation
         moving_2 = true;
       //pc.printf("Negative movement\r\n");
         }
      else{
-        direction_2 = 0;
+      //  direction_2 = 0;
         moving_2 = false;
       //pc.printf("No movement\r\n");
         }
     }   
+
 void PWM_Motor (void) //Calculates on and off times; higher PWM = higher avarage voltage
 {
     width = period * width_percent;
@@ -157,7 +148,7 @@
 //calculates the angle of motor 1
 float angleEncoder(float counts){
     float currentAngle = ShaftResolution * counts;
-    pc.printf("Angle of motor_1: %f radians\r\n",currentAngle);
+    pc.printf("Angle of motor: %f radians\r\n",currentAngle);
     return currentAngle;
     }
     
@@ -167,66 +158,100 @@
 Integral part general formula: u_i = k_i * integral e dt
 Derivative part general formula: u_d = k_d * derivative e */
 
-float PIDControl(float error, float kp, float ki, float kd){
-    static float error_integral = 0;
-    static float error_prev = error;
- //   static BiQuad LPF (a1, b1, c1, d1, e1); 
+float PIDControl_1(float error, float kp, float ki, float kd){
+   //static float error_integral_1 = 0;
+  //  static float error_prev_1 = error;
+    static BiQuad LPF1 (0.0675, 0.1349, 0.0675, -1.1430, 0.4128); 
     
     //proportional
     float u_k = kp * error;
     
     //integral
-    error_integral = error_integral + error * t_s;
-    float u_i = ki * error_integral;
+    error_integral_1 = error_integral_1 + error * t_s;
+    float u_i = ki * error_integral_1;
     
     //differential
-    float error_derivative = (error - error_prev) / t_s;
-   // float filtered_error = LPF.step(error_derivative); //LPF with function of BiQuad library
-   // float u_d = kd * filtered_error;
-    float u_d = kd * error_derivative; //this is very sensitive to noise, hence the LPF above
-    error_prev = error;
+    float error_derivative_1 = (error - error_prev_1) / t_s;
+    float filtered_error = LPF1.step(error_derivative_1); //LPF with function of BiQuad library
+    float u_d = kd * filtered_error;
+    error_prev_1 = error;
     
     return u_k + u_i + u_d;
     }
     
+    float PIDControl_2(float error, float kp, float ki, float kd){
+    //static float error_integral_2 = 0;
+    //static float error_prev_2 = error;
+    //static BiQuad LPF2 (0.0675, 0.1349, 0.0675, -1.1430, 0.4128); 
     
+    //proportional
+    float u_k = kp * error;
+    
+    //integral
+    error_integral_2 = error_integral_2 + error * t_s;    
+    float u_i = ki * error_integral_2;
+    
+        
+    //differential
+    float error_derivative_2 = (error - error_prev_2) / t_s;
+    //float filtered_error = LPF2.step(error_derivative_2); //LPF with function of BiQuad library
+    //float u_d = kd * filtered_error;
+    float u_d = kd * error_derivative_2;
+    error_prev_2 = error;
+    
+    return u_k + u_i + u_d;
+    }
+   
 int main(){   
     pc.baud(SERIAL_BAUD);
+    
    // startmain:
     pc.printf("--------\r\nWelcome!\r\n--------\r\n");
-    pc.printf("Please input Setpoint 1\r\n");
-    float setpoint_1 = getSetpoint_1();
-    pc.printf("Setpoint 1= %f\r\n", setpoint_1);
+    //pc.printf("Please input Setpoint 1\r\n");
+    //float setpoint_1 = getSetpoint_1();
+    //pc.printf("Setpoint 1= %f\r\n", setpoint_1);
     pc.printf("Please input Setpoint 2\r\n");
     float setpoint_2 = getSetpoint_2();
     pc.printf("Setpoint 2= %f\r\n", setpoint_2);
     PWM_Motor();
     
     while(true){
-    
+    /*
     float err_1 = getError(setpoint_1, currentAngle_1);
-    float err_2 = getError(setpoint_2, currentAngle_2);
+    
     pc.printf("Error 1 = %f\r\n",err_1);
-    pc.printf("Error 2 = %f\r\n",err_2);
-    float outcome_1 = PIDControl(err_1, kp_1, ki_1, kd_1);
-    float outcome_2 = PIDControl(err_2, kp_2, ki_2, kd_2);
+   
+    float outcome_1 = PIDControl_1(err_1, kp_1, ki_1, kd_1);
+   
     pc.printf("Outcome 1 = %f\r\n",outcome_1);
-    pc.printf("Outcome 2 = %f\r\n",outcome_2);
+   
     direction_1 = dir_1;
-    direction_2 = dir_2;
+   
     checkMovement_1(outcome_1);
-    checkMovement_2(outcome_2);
+    
     motor_1();
-    motor_2();
+    
     counts_1 = Encoder_1.getPulses(); 
-    counts_2 = Encoder_2.getPulses();
+   
     currentAngle_1 = angleEncoder(counts_1);
-    currentAngle_2 = angleEncoder(counts_2);
+    
+    pc.printf("Counts1 = %f\r\n", counts_1);
+   
+   */
+   
+    float err_2 = getError(setpoint_2, currentAngle_2);
+    pc.printf("Error 2 = %f\r\n",err_2);
+     float outcome_2 = PIDControl_2(err_2, kp_2, ki_2, kd_2);
+      pc.printf("Outcome 2 = %f\r\n",outcome_2);
+       direction_2 = dir_2;
+       checkMovement_2(outcome_2);
+       motor_2();
+        counts_2 = Encoder_2.getPulses();
+        currentAngle_2 = angleEncoder(counts_2);
+        
+    pc.printf("Counts2 = %f\r\n", counts_2);
     pc.printf("-------\r\n-------\r\n");
     
     
-    
-   
-    
     }
 }