Diff: Motor.cpp

Revision:

14:f561498eee28

Parent:

13:559f8946f16d

Child:

16:a2a73d57d556

--- a/Motor.cpp	Wed Nov 01 07:11:00 2017 +0000
+++ b/Motor.cpp	Wed Nov 01 13:08:04 2017 +0000
@@ -43,42 +43,65 @@
     return angle;
 }
 
+double Motor::Control_PID(float error){
+    
+    static double int_error ; 
+    double der_error ;
+    double sample_time = 0.002 ;
+    double prev_error = 0 ;
+    double Ki = 2.0 , Kd = 0.5 , Kp = 3; // set Controller gains motor 1 Kp Kd Ki
+    double ScaleVal = 1.0/(360*2*Kp);     //For initial testing purposes, set scaling so that maximum error (360 * M1_KP) equals 100% Duty Cycle
+
+    der_error = (error-prev_error)/sample_time;
+    
+    int_error += sample_time*error;
+    
+    prev_error = error ;
+    return (Kp*error + Ki*int_error + Kd*der_error)*ScaleVal;
+        
+    
+}
+
 float Motor::Control_angle(float ang){
         
     float control_angle=ang;
+    double error = Control_PID(angle - control_angle);                //set error probably need some kind of PID
+
+
+    if(angle - control_angle >= 0)
+        _direction = POS;
+    else
+        _direction = !POS; 
+        
     //float step = 360.0/(32*131);                               
    // control_angle -= remainder(control_angle , step);   //changle control angle to posible angles of the sensor
 
-    if(control_angle > safety_angle + initial_angle )
+    if(control_angle > safety_angle + 90 and error >= 0)
     {
-        control_angle = safety_angle + initial_angle; 
+        control_angle = safety_angle + 90; 
         _PWM = 0;
     }
-    else if(control_angle < -safety_angle + initial_angle)
+    else if(control_angle < -safety_angle + 90 and error <= 0)
     {
-        control_angle = -safety_angle + initial_angle;
+        control_angle = -safety_angle + 90;
         _PWM = 0;
     }
     else
     {    
-        
-        double error = angle - control_angle;                //set error probably need some kind of PID
+        if( 0.01 < fabs(error) and fabs(error) < low_PWM)
+            _PWM = low_PWM;
+        else if( fabs(error) >= 1 )
+            _PWM = 1;
+        else if( fabs(error) <= 0.01 )
+            _PWM = 0;
+        else
+            _PWM = fabs(error);    
+    }
     
-        if(error >= 0)
-            _direction = POS;
-        else
-            _direction = !POS;    
+    
+          
 
     
-        if( 0.01 < fabs(error) and fabs(error) < low_PWM)
-            _PWM = low_PWM;
-        else if(fabs(error) >= 1 )
-            _PWM = 1;
-        else if(fabs(error) <= 0.01)
-            _PWM = 0;
-        else
-            _PWM = fabs(error);
-    }
     return _PWM;
 
 }
\ No newline at end of file