Poep Hoofd / Mbed 2 deprecated PoolRobot_Code

Dependencies:   HIDScope mbed MODSERIAL QEI

Motor.cpp

Committer:
Alex_Kyrl
Date:
2017-10-30
Revision:
12:69a9cf74583e
Parent:
11:dd1976534a03
Child:
13:559f8946f16d

File content as of revision 12:69a9cf74583e:

#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
    _direction= 1;
    low_PWM = 0.6; 
}

Motor::Motor(PinName a , PinName b, PinName direction , PinName PWM , float freq ,  float safe , float low )  : _Encoder(a,b,NC,32),  _direction(direction) , _PWM(PWM)  
{
    _PWM=0;
    _Encoder.QEI::reset(); 
    frequency=freq;
    set_period(frequency);
    angle= 90;
    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 = 90 + ((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;
}

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

}