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@149: 	ki = 2;
giuliomoro@149: 	kd = 0.8;
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@149: 	output= 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@149: 
giuliomoro@149: