Diff: Motor.cpp

Revision:

9:22d79a4a0324

Parent:

8:0b7925095416

Child:

11:dd1976534a03

--- a/Motor.cpp	Fri Oct 20 14:38:14 2017 +0000
+++ b/Motor.cpp	Fri Oct 27 11:55:22 2017 +0000
@@ -7,21 +7,19 @@
     _Encoder.QEI::reset(); 
     frequency=50000;
     set_period(frequency);
-    angle=0;
-    control_angle =0; 
+    angle=90; 
     safety_angle = 120; //safety angle in DEGREES
     _direction= 1;
     low_PWM = 0.6; 
 }
 
-Motor::Motor(PinName a , PinName b , PinName c , PinName direction , PinName PWM , int pulse , float freq ,  float safe , float low )  : _Encoder(a,b,c,pulse),  _direction(direction) , _PWM(PWM)  
+Motor::Motor(PinName a , PinName b, PinName direction , PinName PWM , float freq ,  float safe , float low )  : _Encoder(a,b,NC,32),  _direction(direction) , _PWM(PWM)  
 {
     _PWM=0;
     _Encoder.QEI::reset(); 
     frequency=freq;
     set_period(frequency);
-    angle= 0;
-    control_angle = 0; 
+    angle= 90;
     safety_angle = safe;    
     _direction= 1;
     low_PWM= low;
@@ -38,38 +36,36 @@
     
     int n_pulse= _Encoder.QEI::getPulses();  
      
-    angle =  ((n_pulse)*(360/32))/131;                      // get angle    TODO: Change 32 to 4th value of encoder
+    angle = 90 + ((n_pulse)*(360/32))/131;                      // get angle    TODO: Change 32 to 4th value of encoder
     return angle;
 }
 
 void Motor::Control_angle(float ang){
         
     float control_angle=ang;
-    
+    //float step = 360/(32*131);                                // ToDo: changle 32 to 4th value of encoder 
+    //control_angle = control_angle - fmodf(control_angle, step) ;  //changle control angle to posible angles of the sensor
     
-    if(control_angle > safety_angle)
-        control_angle = safety_angle; 
-    if(control_angle < -safety_angle)
-        control_angle = -safety_angle;
+    if(control_angle > safety_angle+90)
+        control_angle = safety_angle+90; 
+    if(control_angle < -safety_angle+90)
+        control_angle = -safety_angle+90;
         
-    float error = angle - control_angle;                //set error probably need some kind of PID
+    double error = angle - control_angle;                //set error probably need some kind of PID
     
     if(error >= 0)
         _direction = 1;
     else
         _direction = 0;
         
-    if(abs(error/(2*safety_angle)) < low_PWM)
+    if( 0.1 < abs(error/180) < low_PWM)
         _PWM = low_PWM;
-    else if(abs(error/(2*safety_angle)) > 1 )
+    else if(abs(error/180) >= 1 )
         _PWM = 1;
+    else if (abs(error/180) < 0.1)
+        _PWM = 0;
     else
-        _PWM = abs(error/(2*safety_angle));
+        _PWM = abs(error/180);
     
 
-}
-float get_angle()
-{
-    
-  //  return angle ;
 }
\ No newline at end of file