Diff: Motor.cpp

Revision:

16:a2a73d57d556

Parent:

14:f561498eee28

Child:

21:59431788a42d

--- a/Motor.cpp	Wed Nov 01 13:32:43 2017 +0000
+++ b/Motor.cpp	Thu Nov 02 15:07:53 2017 +0000
@@ -13,9 +13,12 @@
     _direction= POS;
     low_PWM = 0.6; 
     initial_angle = 90;
+    Ki = 2.0 ;
+    Kd = 0.5 ;
+    Kp = 3; 
 }
 
-Motor::Motor(PinName a , PinName b, PinName direction , PinName PWM , float freq ,  float safe , float low , int pos  , int ini)  : _Encoder(a,b,NC,32),  _direction(direction) , _PWM(PWM)  
+Motor::Motor(PinName a , PinName b, PinName direction , PinName PWM , float freq ,  float safe , float low , int pos  , int ini , float p , float i , float d)  : _Encoder(a,b,NC,32),  _direction(direction) , _PWM(PWM)  
 {
     _PWM=0;
     _Encoder.QEI::reset(); 
@@ -27,6 +30,10 @@
     POS = pos;  
     _direction= POS;
     low_PWM= low;
+    Kp = p;
+    Ki = i;
+    Kd = d;
+    
 }
 
 
@@ -49,8 +56,7 @@
     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
+    double ScaleVal = 1;//Kp/(360.0*5);     //For initial testing purposes, set scaling so that maximum error (360 * M1_KP) equals 100% Duty Cycle
 
     der_error = (error-prev_error)/sample_time;
     
@@ -98,8 +104,6 @@
             _PWM = fabs(error);    
     }
     
-    
-          
 
     
     return _PWM;