File content as of revision 16:a2a73d57d556:

#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;
    Ki = 2.0 ;
    Kd = 0.5 ;
    Kp = 3; 
}

Motor::Motor(PinName a , PinName b, PinName direction , PinName PWM , float freq ,  float safe , float low , int pos  , int ini , float p , float i , float d)  : _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;
    Kp = p;
    Ki = i;
    Kd = d;
    
}


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;
}

double Motor::Control_PID(float error){
    
    static double int_error ; 
    double der_error ;
    double sample_time = 0.002 ;
    double prev_error = 0 ;
    double ScaleVal = 1;//Kp/(360.0*5);     //For initial testing purposes, set scaling so that maximum error (360 * M1_KP) equals 100% Duty Cycle

    der_error = (error-prev_error)/sample_time;
    
    int_error += sample_time*error;
    
    prev_error = error ;
    return (Kp*error + Ki*int_error + Kd*der_error)*ScaleVal;
        
    
}

float Motor::Control_angle(float ang){
        
    float control_angle=ang;
    double error = Control_PID(angle - control_angle);                //set error probably need some kind of PID


    if(angle - control_angle >= 0)
        _direction = POS;
    else
        _direction = !POS; 
        
    //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 + 90 and error >= 0)
    {
        control_angle = safety_angle + 90; 
        _PWM = 0;
    }
    else if(control_angle < -safety_angle + 90 and error <= 0)
    {
        control_angle = -safety_angle + 90;
        _PWM = 0;
    }
    else
    {    
        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;

}