File content as of revision 8:0b7925095416:

#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=0;
    control_angle =0; 
    safety_angle = 120; //safety angle in DEGREES
    _direction= 1;
    low_PWM = 0.6; 
}

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)  
{
    _PWM=0;
    _Encoder.QEI::reset(); 
    frequency=freq;
    set_period(frequency);
    angle= 0;
    control_angle = 0; 
    safety_angle = safe;    
    _direction= 1;
    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 =  ((n_pulse)*(360/32))/131;                      // get angle    TODO: Change 32 to 4th value of encoder
    return angle;
}

void Motor::Control_angle(float ang){
        
    float control_angle=ang;
    
    
    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
        _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));
    

}
float get_angle()
{
    
  //  return angle ;
}