Diff: Motor.cpp

Revision:

11:dd1976534a03

Parent:

9:22d79a4a0324

Child:

12:69a9cf74583e

--- a/Motor.cpp	Fri Oct 27 11:55:22 2017 +0000
+++ b/Motor.cpp	Mon Oct 30 15:32:27 2017 +0000
@@ -7,7 +7,7 @@
     _Encoder.QEI::reset(); 
     frequency=50000;
     set_period(frequency);
-    angle=90; 
+    angle=45; 
     safety_angle = 120; //safety angle in DEGREES
     _direction= 1;
     low_PWM = 0.6; 
@@ -19,7 +19,7 @@
     _Encoder.QEI::reset(); 
     frequency=freq;
     set_period(frequency);
-    angle= 90;
+    angle= 45;
     safety_angle = safe;    
     _direction= 1;
     low_PWM= low;
@@ -36,36 +36,37 @@
     
     int n_pulse= _Encoder.QEI::getPulses();  
      
-    angle = 90 + ((n_pulse)*(360/32))/131;                      // get angle    TODO: Change 32 to 4th value of encoder
+    angle = 45 + ((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) 
     return angle;
 }
 
-void Motor::Control_angle(float ang){
+double Motor::Control_angle(float ang){
         
     float control_angle=ang;
-    //float step = 360/(32*131);                                // ToDo: changle 32 to 4th value of encoder 
+    //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;
+    if(control_angle > safety_angle+45)
+        control_angle = safety_angle+45; 
+    if(control_angle < -safety_angle+45)
+        control_angle = -safety_angle+45;
         
     double error = angle - control_angle;                //set error probably need some kind of PID
     
     if(error >= 0)
         _direction = 1;
     else
-        _direction = 0;
-        
-    if( 0.1 < abs(error/180) < low_PWM)
+        _direction = 0;    
+    
+    if( 0.1 < fabs(error) < low_PWM)
         _PWM = low_PWM;
-    else if(abs(error/180) >= 1 )
+    else if(fabs(error) >= 1 )
         _PWM = 1;
-    else if (abs(error/180) < 0.1)
+    else if (fabs(error) < 0.1)
         _PWM = 0;
     else
-        _PWM = abs(error/180);
-    
+        _PWM = fabs(error);
+        
+    return error;
 
 }
\ No newline at end of file