File content as of revision 0:ee7e9405e1c7:

#include "MD_PID.h"

MD_PID::MD_PID( MD *md, QEI *qei,
                double k, double ti, double td,
				double max_speed)
                : _md(md), _qei(qei), _pid(new PI_D(k, ti, td))
{
    _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::PI_D(present_vel, target_vel, interval);
    _pid->set_state(present_vel, target_vel);
    _duty = -1 * _pid->get_control();
    
    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();
}