view core/Pid.cpp @ 152:8f98b32d0e23 ClockSync

Last commit on this branch for a while. Overall not very succesful

/*
 * Pid.cpp
 *
 *  Created on: 14 Sep 2015
 *      Author: giulio
 */

#include "Pid.h"

Pid::Pid(){
	integralError = 0;
	pastError = 0;
	error = 0;
	differentialError = 0;
	kp = 5;
	ki = 2;// 2;
	kd = 0.8; //0.8;
	gain = 1;
	idleTimeout = 0;
	timeoutCount = 0;
	output = 0;
//	printf("----------%f %f %f %f %f %f\n", kp, error, ki, integralError, kd, differentialError);
}
void Pid::updateIntegralError(){
	integralError += (error + pastError) * 0.5 * ts;
}
void Pid::updateDifferentialError(){
	differentialError = (error - pastError) / ts;
}
float Pid::setError(float anError){
	pastError = error;
	error = anError;
	ts = 1; //TODO: this should be passed as a parameter, unless setError() gets always called at constant intervals
	updateIntegralError();
	updateDifferentialError();
	output = gain * (kp * error + ki * integralError + kd * differentialError);
//	printf("%f %f %f %f %f %f %f\n", output, kp, error, ki, integralError, kd, differentialError);
	return output;
}
float Pid::getOutput(){
	return output;
}
void Pid::setProportionalGain(float proportionalGain){
	kp = proportionalGain;
}
void Pid::setDerivativeGain(float derivativeGain){
	kd = derivativeGain;
}
void Pid::setIntegralGain(float integralGain){
	ki = integralGain;
}
void Pid::setGlobalGain(float globalGain){
	gain = globalGain;
}
float Pid::getProportionalGain(){
	return kp;
}
float Pid::getDerivativeGain(){
	return kd;
}
float Pid::getIntegralGain(){
	return ki;
}
float Pid::getGlobalGain(){
	return gain;
}
float Pid::getIntegralError(){
	return integralError;
}