File content as of revision 4:38c653bfec5f:

#include "Motor.h"


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

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)  
{
    velocity = vel;  
    _PWM=0;
    _Encoder.QEI::reset(); 
    frequency=freq;
    set_period(frequency);
    angle= 0;
    angle_prev = 0; 
    sample_time = samp;
    safety_angle = safe;    
    POS = pos;
    NEG = !pos;
    _direction=POS;
}

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

void 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
    cout<<angle<<endl;
}

void Motor::control_velocity(float vel){
        
    float control_velocity=vel;
    
    //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;
    }
    else
    {
        _PWM = control_velocity ; 
    }    

   
}

float Motor::get_velocity(){
    
    return velocity;
}

float Motor::get_period(){
    
    return 1/frequency;
    
}