File content as of revision 0:ee7e9405e1c7:

#include "MD_PID.h"

MD_PID::MD_PID( MD *md, QEI *qei,
                double kp, double ki, double kd,
				double max_speed)
                : _md(md), _qei(qei), PID_Control(kp, ki, kd)
{
    _duty = 0.0;
    set_max_speed(max_speed);
}

void MD_PID::drive(double target_vel, double interval)
{
    double present_vel = read_vel();

    if(fabs(target_vel) > _max_speed){
        if(target_vel >= 0)
            target_vel = _max_speed;
        else
            target_vel = -_max_speed;
    }
    //PID_Control::PID(present_vel, target_vel, interval);
    //PID_Control::VPID(present_vel, target_vel, interval);
    PID_Control::PI_D(present_vel, target_vel, interval);
    _duty = PID_Control::get_control();
    
    //printf("%lf\r\n", _duty);
    
    if( fabs(_duty) > 1 )
        _duty /= fabs(_duty);
        
    _md->drive(_duty);
    //printf("presnet_vel:\t%lf\tduty:\t%lf\r\n", present_vel, _duty);
}

double MD_PID::get_duty(){
	return _duty;
}

void MD_PID::brake(double target)
{
    
}

void MD_PID::free()
{
    
}

void MD_PID::set_max_speed(double max_speed)
{
	_max_speed = max_speed;
}

double MD_PID::read_vel()
{
    return _qei->get_ang_vel();
}