37#define DBGSERIAL Serial
54PID wpid(&
win, &
wout, &
wset, 1.0, 0.01, 0.01, DIRECT);
55PID apid(&
ain, &
aout, &
aset, 0.5, 0.05, 0.02, DIRECT);
74 ServoPD(
long Kp,
long Kd,
long zero,
long range,
long scalebits = 8) :
75 itsKp(Kp), itsKd(Kd), itsPos(zero << scalebits),
76 itsPrevTarget(zero << scalebits), itsZero(zero << scalebits),
77 itsRange(range << scalebits), itsScaleBits(scalebits)
83 itsPos = (pos << itsScaleBits);
89 return (itsPos >> itsScaleBits);
94 targetpos <<= itsScaleBits;
95 long diff = itsKp * targetpos + itsKd * (targetpos - itsPrevTarget);
96 itsPos += (diff >> 16);
97 itsPos = constrain(itsPos, itsZero - itsRange, itsZero + itsRange);
98 itsServo.write(itsPos >> itsScaleBits);
99 itsPrevTarget = targetpos;
104 targetpos <<= itsScaleBits;
105 itsPos = constrain(itsPos, itsZero - itsRange, itsZero + itsRange);
106 itsPrevTarget = targetpos;
111 return itsPos >> itsScaleBits;
116 itsPos = rawval << itsScaleBits;
117 itsServo.write(rawval);
122 long itsKp, itsKd, itsPos, itsPrevTarget, itsZero, itsRange, itsScaleBits;
150 wpid.SetMode(AUTOMATIC);
151 wpid.SetSampleTime(10);
152 wpid.SetOutputLimits(-120, 120);
155 apid.SetMode(AUTOMATIC);
156 apid.SetSampleTime(10);
157 apid.SetOutputLimits(-50, 50);
166 char * tok = strtok(
instr,
" \r\n");
167 int state = 0;
int id, targx, targy, targw, targh;
182 case 0:
if (strcmp(tok,
"N2") == 0) state = 1;
else state = 1000;
break;
183 case 1:
id = atoi(&tok[1]); state = 2;
break;
184 case 2: targx = atoi(tok); state = 3;
break;
185 case 3: targy = atoi(tok); state = 4;
break;
186 case 4: targw = atoi(tok); state = 5;
break;
187 case 5: targh = atoi(tok); state = 6;
break;
190 tok = strtok(0,
" \r\n");
225 if (left > 100) left = 100;
else if (left < -100) left = -100;
226 if (right > 100) right = 100;
else if (right < -100) right = -100;
228 long motleft = (left *
leftgain) >> 8;
229 long motright = (right *
rightgain) >> 8;
235 }
else if (motleft < 0) {
238 analogWrite(
LPWMPIN, -motleft);
247 analogWrite(
RPWMPIN, motright);
248 }
else if (motright < 0) {
251 analogWrite(
RPWMPIN, -motright);
void motor(long left, long right)
ServoPD tiltservo(300, 100, TILTZERO, TILTRANGE)
ServoPD panservo(400, 200, PANZERO, PANRANGE)
void update(long targetpos)
void attach(int pin, int pos)
void reset(long targetpos)
ServoPD(long Kp, long Kd, long zero, long range, long scalebits=8)