File content as of revision 3:b6e4e5529a52:

#include "PIDController.hpp"

//PIDController::PIDController() {}

void PIDController::updateParams(ControllerParams &cp) {
	setKp(cp.kp);
	setTi(cp.ti);
	setTd(cp.td);
}

void PIDController::calculateCmd() {
	float outputPWM;
	
	float Proportional = kp*dif(this->ref,this->out);
	float Derivative = (kp*td/TE)*dif(this->ref,this->out);
	float Integral = 0;
	
	if(ti == 0) {
		outputPWM = Proportional + Derivative;
	} else {
		Integral = ((kp*TE)/ti)*dif(this->ref,this->out);
		outputPWM = Proportional + Integral + Derivative;
	}
	
	if(outputPWM > 1) {
		outputPWM = outputPWM - Integral;
	} else if(outputPWM < 0) {
		outputPWM = 0;
	}
	
	cmd = outputPWM;
}

void PIDController::setKp(float _kp) {
	kp = _kp;	
}

void PIDController::setTi(float _ti) {
	ti = _ti;
}

void PIDController::setTd(float _td) {
	td = _td;
}

float PIDController::getKp() const {
	return kp;
}

float PIDController::getTi() const {
	return ti;
}

float PIDController::getTd() const {
	return td;
}

PIDController::~PIDController() {}