Diff: Motor.cpp

Revision:

13:559f8946f16d

Parent:

12:69a9cf74583e

Child:

14:f561498eee28

--- a/Motor.cpp	Mon Oct 30 16:28:21 2017 +0000
+++ b/Motor.cpp	Wed Nov 01 07:11:00 2017 +0000
@@ -9,19 +9,23 @@
     set_period(frequency);
     angle=90; 
     safety_angle = 120; //safety angle in DEGREES
-    _direction= 1;
+    POS=1;
+    _direction= POS;
     low_PWM = 0.6; 
+    initial_angle = 90;
 }
 
-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)  
+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)  
 {
     _PWM=0;
     _Encoder.QEI::reset(); 
     frequency=freq;
     set_period(frequency);
-    angle= 90;
-    safety_angle = safe;    
-    _direction= 1;
+    initial_angle = ini ;
+    angle = initial_angle;
+    safety_angle = safe;  
+    POS = pos;  
+    _direction= POS;
     low_PWM= low;
 }
 
@@ -30,43 +34,51 @@
     frequency=freq;
     _PWM.period(1/frequency);
     
-}
+} 
 
 float Motor::set_angle(){
     
     int n_pulse= _Encoder.QEI::getPulses();  
-     
-    angle = 90 + ((n_pulse)*(360/32))/131;                      // get angle  (131 is the gear ratio of the motor in order to work in degrees of the actual arm) 
+         angle = initial_angle - n_pulse*360.0f/(32*131);                 // get angle  (131 is the gear ratio of the motor in order to work in degrees of the actual arm) 
     return angle;
 }
 
-double Motor::Control_angle(float ang){
+float Motor::Control_angle(float ang){
         
     float control_angle=ang;
-    //float step = 360/(32*131);                               
-    //control_angle = control_angle - fmodf(control_angle, step) ;  //changle control angle to posible angles of the sensor
-    
-    if(control_angle > safety_angle+90)
-        control_angle = safety_angle+90; 
-    if(control_angle < -safety_angle+90)
-        control_angle = -safety_angle+90;
-        
-    double error = angle - control_angle;                //set error probably need some kind of PID
-    
-    if(error >= 0)
-        _direction = 1;
+    //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 )
+    {
+        control_angle = safety_angle + initial_angle; 
+        _PWM = 0;
+    }
+    else if(control_angle < -safety_angle + initial_angle)
+    {
+        control_angle = -safety_angle + initial_angle;
+        _PWM = 0;
+    }
     else
-        _direction = 0;    
+    {    
+        
+        double error = angle - control_angle;                //set error probably need some kind of PID
+    
+        if(error >= 0)
+            _direction = POS;
+        else
+            _direction = !POS;    
+
     
-    if( 0.1 < fabs(error) < low_PWM)
-        _PWM = low_PWM;
-    else if(fabs(error) >= 1 )
-        _PWM = 1;
-    else if (fabs(error) < 0.1)
-        _PWM = 0;
-    else
-        _PWM = fabs(error);
-        
-    return error;
+        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