bootje_v4 -

Users » pd0wm » Code » bootje_v4

verwerking/pid.cpp@0:bec310bde899, 2011-09-27 (annotated)

User	Revision	Line number	New contents of line
pd0wm	0:bec310bde899	1	#include "pid.h"
pd0wm	0:bec310bde899	2
pd0wm	0:bec310bde899	3	Pid::Pid(Goto * p, Kompas * k, Roer * r){
pd0wm	0:bec310bde899	4	sp = p;
pd0wm	0:bec310bde899	5	pv = k;
pd0wm	0:bec310bde899	6	output = r;
pd0wm	0:bec310bde899	7	dt = 0.1;
pd0wm	0:bec310bde899	8
pd0wm	0:bec310bde899	9	kp = 1.2;
pd0wm	0:bec310bde899	10	ki = 1;
pd0wm	0:bec310bde899	11	kd = 0.1;
pd0wm	0:bec310bde899	12
pd0wm	0:bec310bde899	13	prev_error = 0;
pd0wm	0:bec310bde899	14	}
pd0wm	0:bec310bde899	15
pd0wm	0:bec310bde899	16	void Pid::update(void){
pd0wm	0:bec310bde899	17	double error = verschil(pv->get(),sp->get());
pd0wm	0:bec310bde899	18
pd0wm	0:bec310bde899	19	p = error * kp;
pd0wm	0:bec310bde899	20	i = i + error * dt;
pd0wm	0:bec310bde899	21	if (i > 25)
pd0wm	0:bec310bde899	22	i = 25;
pd0wm	0:bec310bde899	23	if (i < -25)
pd0wm	0:bec310bde899	24	i = -25;
pd0wm	0:bec310bde899	25	i *= ki;
pd0wm	0:bec310bde899	26
pd0wm	0:bec310bde899	27	d = (error - prev_error)/dt * kd;
pd0wm	0:bec310bde899	28	o = p+i+d + 50;
pd0wm	0:bec310bde899	29
pd0wm	0:bec310bde899	30	if (o > 100)
pd0wm	0:bec310bde899	31	o = 100;
pd0wm	0:bec310bde899	32	if (0 < 0)
pd0wm	0:bec310bde899	33	o = 0;
pd0wm	0:bec310bde899	34
pd0wm	0:bec310bde899	35
pd0wm	0:bec310bde899	36	output->set(o);
pd0wm	0:bec310bde899	37	prev_error = error;
pd0wm	0:bec310bde899	38
pd0wm	0:bec310bde899	39
pd0wm	0:bec310bde899	40	}