34 #define SERIAL Serial1
37 #define DBGSERIAL Serial
54 PID wpid(&
win, &
wout, &
wset, 1.0, 0.01, 0.01, DIRECT);
55 PID 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);