File content as of revision 0:e83b840a5f86:

/*
 * MDPIDSpeed.cpp
 *
 *  Created on: 2018/08/31
 *      Author: komazawataiyou
 */

#include "MDPIDSpeed.h"

MD_PID_Speed::MD_PID_Speed(MD *md, QEI *encorder, double kp, double ki, double kd)
: Timer_PID(kp, ki, kd), duty_(0.00), target_rpm_(0.00)
{
	this->md = md;
	this->encorder = encorder;

}

MD_PID_Speed::~MD_PID_Speed() {
}

void MD_PID_Speed::drive(double rpm)
{
	double current_rpm = this->get_current_rpm();
	target_rpm_ = rpm;

	duty_ += Timer_PID::PID(current_rpm, rpm);

	for(int i = 1; i < (signed)(sizeof(rpm_log_) / sizeof(double)); i++)
		rpm_log_[i - 1] = rpm_log_[i];
	rpm_log_[sizeof(rpm_log_) / sizeof(double) - 1] = current_rpm;

	md->drive(duty_);
}

void MD_PID_Speed::brake(double rpm)
{

}

void MD_PID_Speed::free()
{

}

bool MD_PID_Speed::target_complete(float allowable_error)
{
	allowable_error = (fabs(allowable_error) < 1.0) ? fabs(allowable_error) : 1.00;
	double rpm_mean = 0.00;
	for(int i = 0; i < (signed)(sizeof(rpm_log_) / sizeof(rpm_log_[0])); i++)
		rpm_mean += rpm_log_[i];
	rpm_mean /= (sizeof(rpm_log_) / sizeof(double));

	return (rpm_mean <= target_rpm_ + allowable_error && rpm_mean >= target_rpm_ - allowable_error);
}

double MD_PID_Speed::get_duty()
{
	return duty_;
}

double MD_PID_Speed::get_current_rpm()
{
	return encorder->speed() * 60;
}

void MD_PID_Speed::reset()
{
	duty_ = 0;
	Timer_PID::reset();
}