Diff: Motor.cpp

Revision:

8:0b7925095416

Parent:

6:452e301a105a

Child:

9:22d79a4a0324

--- a/Motor.cpp	Tue Oct 17 06:29:42 2017 +0000
+++ b/Motor.cpp	Fri Oct 20 14:38:14 2017 +0000
@@ -3,90 +3,73 @@
 
 Motor::Motor() : _Encoder(D13,D12,NC,32),  _direction(D7) , _PWM(D6)  
 {
-    velocity = 0;  
     _PWM=0;
     _Encoder.QEI::reset(); 
     frequency=50000;
     set_period(frequency);
     angle=0;
-    angle_prev =0;
-    sample_time = 0.002;  
-    safety_angle = 90; //safety angle in DEGREES
-    POS=1 , NEG=0;
-    _direction= 1; 
+    control_angle =0; 
+    safety_angle = 120; //safety angle in DEGREES
+    _direction= 1;
+    low_PWM = 0.6; 
 }
 
-Motor::Motor(float vel , PinName a , PinName b , PinName c , PinName direction , PinName PWM , int pulse , float freq , int dir , float samp , float safe , int pos)  : _Encoder(a,b,c,pulse),  _direction(direction) , _PWM(PWM)  
+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)  
 {
-    velocity = vel;  
     _PWM=0;
     _Encoder.QEI::reset(); 
     frequency=freq;
     set_period(frequency);
     angle= 0;
-    angle_prev = 0; 
-    sample_time = samp;
+    control_angle = 0; 
     safety_angle = safe;    
-    POS = pos;
-    NEG = !pos;
-    _direction=POS;
+    _direction= 1;
+    low_PWM= low;
 }
 
-void Motor::change_direction(){
-    
-    _direction=!_direction; //should probably have some call to DigitalOut:: maybe can't use overloaded operators here
-
-}
 
 void Motor::set_period(float freq){
     frequency=freq;
     _PWM.period(1/frequency);
     
 }
-void Motor::set_velocity(){
-    
-    velocity = (angle-angle_prev)/sample_time;              // differentiate angle TODO: needs filtering!
-    angle_prev = angle;
+
+float Motor::set_angle(){
     
-} 
-
-void Motor::set_angle(){
-    
-     int n_pulse= _Encoder.QEI::getPulses();  
+    int n_pulse= _Encoder.QEI::getPulses();  
      
-      angle =  ((n_pulse)*(360/32))/131;                      // get angle    TODO: Change 32 to 4th value of encoder
-    cout<<angle<<endl;
+    angle =  ((n_pulse)*(360/32))/131;                      // get angle    TODO: Change 32 to 4th value of encoder
+    return angle;
 }
 
-void Motor::control_velocity(float vel){
+void Motor::Control_angle(float ang){
         
-    float control_velocity=vel;
+    float control_angle=ang;
     
-    //float error = velocity - control_velocity;                //set error probably need some kind of PID
     
-    if(angle >= (safety_angle-3) and _direction == !POS) 
-    {
-        _PWM = 0;
-    }
-    else if(angle <= -(safety_angle-3) and _direction == !NEG) 
-    {
-        _PWM = 0;
-    }
+    if(control_angle > safety_angle)
+        control_angle = safety_angle; 
+    if(control_angle < -safety_angle)
+        control_angle = -safety_angle;
+        
+    float error = angle - control_angle;                //set error probably need some kind of PID
+    
+    if(error >= 0)
+        _direction = 1;
     else
-    {
-        _PWM = control_velocity ; 
-    }    
-
-   
-}
-
-float Motor::get_velocity(){
+        _direction = 0;
+        
+    if(abs(error/(2*safety_angle)) < low_PWM)
+        _PWM = low_PWM;
+    else if(abs(error/(2*safety_angle)) > 1 )
+        _PWM = 1;
+    else
+        _PWM = abs(error/(2*safety_angle));
     
-    return velocity;
+
 }
-
-float Motor::get_period(){
+float get_angle()
+{
     
-    return 1/frequency;
-    
-} 
\ No newline at end of file
+  //  return angle ;
+}
\ No newline at end of file