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