Diff: DC_Motor.cpp

Revision:

2:7e97b8a38a27

Parent:

1:31c78f4d5084

Child:

3:75845d88357a

--- a/DC_Motor.cpp	Thu Apr 05 18:20:21 2018 +0000
+++ b/DC_Motor.cpp	Sun Apr 08 17:07:36 2018 +0000
@@ -57,8 +57,9 @@
 float pwm1_duty = 0.0f;
 
 float v_err_old_1 = 0.0;
-float Kp_1 = 10;  //need to be tested
-float Ki_1 = 10;  //need to be tested
+float Kp_1 = 50;  //need to be tested
+float Ki_1 = 100;  //need to be tested
+float ctrl_output_old_1 = 0.0f;
 
 
 //motor 2
@@ -75,8 +76,9 @@
 float pwm2_duty = 0.0f;
 
 float v_err_old_2 = 0.0;
-float Kp_2 = 10;
-float Ki_2 = 10;
+float Kp_2 = 50;
+float Ki_2 = 100;
+float ctrl_output_old_2 = 0.0f;
 
 
 //****************************************************************************** End of Variables
@@ -104,11 +106,12 @@
     
       
     v_err_1 = v_ref_1 - speed_1 ;
-    v_ierr_1 = v_err_old_1 + v_err_1 ;
+    
+    ctrl_output_1 = (ctrl_output_old_1) + (Kp_1+Ki_1*Ts*0.5)*(v_err_1) + (-Kp_1+Ki_1)*0.5*(v_ierr_1);
     
-    ctrl_output_1 = (v_err_1)*Kp_1 + (v_ierr_1)*Ki_1 ;
-    
-    v_err_old_1 = v_err_1 ;
+    v_ierr_1 =  v_err_1 ;
+    ctrl_output_old_1 = ctrl_output_1;
+
     
     ///////////////////////////
     
@@ -126,11 +129,11 @@
     // Code for PI controller //
       
     v_err_2 = v_ref_2 - speed_2 ;
-    v_ierr_2 = v_err_old_2 + v_err_2 ;
     
+    ctrl_output_2 = (ctrl_output_old_2) + (Kp_2+Ki_2*Ts*0.5)*(v_err_2) + (-Kp_2+Ki_2)*0.5*(v_ierr_2);
     
-    ctrl_output_2 = (v_err_2)*Kp_2 + (v_ierr_2)*Ki_2 ;
-    v_err_old_2 = v_err_2 ;
+    v_ierr_2 =  v_err_2 ;
+    ctrl_output_old_2 = ctrl_output_2;
     
     ///////////////////////////    
     
@@ -179,25 +182,25 @@
     //////////////////////////////////
     switch(motor_state_1)
     {
-        case 1:
+        case '1':
             if(motor_state_old_1 == 4) 
             count_1--;           
             else if(motor_state_old_1 == 2) 
             count_1++;  
             break; 
-        case 2:
+        case '2':
             if(motor_state_old_1 == 1) 
             count_1--;                 
             else if(motor_state_old_1 == 3) 
             count_1++;  
             break; 
-        case 3:
+        case '3':
             if(motor_state_old_1== 2)  
             count_1--;                 
             else if(motor_state_old_1 == 4) 
             count_1++;  
             break; 
-        case 4:
+        case '4':
             if(motor_state_old_1 == 3) 
             count_1--;  
             else if(motor_state_old_1 == 1)