File content as of revision 13:559f8946f16d:

#include "Motor.h"


Motor::Motor() : _Encoder(D13,D12,NC,32),  _direction(D7) , _PWM(D6)  
{
    _PWM=0;
    _Encoder.QEI::reset(); 
    frequency=50000;
    set_period(frequency);
    angle=90; 
    safety_angle = 120; //safety angle in DEGREES
    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 , int pos  , int ini)  : _Encoder(a,b,NC,32),  _direction(direction) , _PWM(PWM)  
{
    _PWM=0;
    _Encoder.QEI::reset(); 
    frequency=freq;
    set_period(frequency);
    initial_angle = ini ;
    angle = initial_angle;
    safety_angle = safe;  
    POS = pos;  
    _direction= POS;
    low_PWM= low;
}


void Motor::set_period(float freq){
    frequency=freq;
    _PWM.period(1/frequency);
    
} 

float Motor::set_angle(){
    
    int n_pulse= _Encoder.QEI::getPulses();  
         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;
}

float Motor::Control_angle(float ang){
        
    float control_angle=ang;
    //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
    {    
        
        double error = angle - control_angle;                //set error probably need some kind of PID
    
        if(error >= 0)
            _direction = POS;
        else
            _direction = !POS;    

    
        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;

}