PID Speed Control
In this post, all codes are NOT tested. the PIDs are not tuned yet.
Derived wheel rotation speed from Wheel Count
Have you ever calibrated the wheel counter? Let me give you some data.
1. one revolution = 60 counts
2. wheel diameter = 56 mm
3. sampling time = 2.5 ms
What do you think about the MAXIMUM change in count per 2.5 ms? Doing a simple arithmetic, you will learn that it is not likely to exceed 15 counts per 2.5 ms. In fact, 1 count per 2.5 ms is equivalent to 1.17 m/s. So 2.5 ms is not suitable for deriving the car speed. 20 ms will give a lowest controllable speed of 15 cm/s.
My plan to obtain a speed would be:
1. get the change in count every 2.5 ms and save it for at least 8 cycles.
2. sum up 8 consecutive readings every 2.5 ms to obtain speed as counts per 20 ms.
I will use 1 integer to hold 8 samples. Recall that each sample's value would not exceed 15, so 4 bits is enough. Integer is 32 bits long (stm32f103 is 32-bit MCU), so it can hold 8 samples. The reason is not just to save RAM, it would simplify the operation. New data is saved at the LSB side; old data is shifted out at the MSB side.
/* */
a ee
u i
ee oi a ee }
u i e ee u i e e ou e a ue
u i e u a a e i 5
a
u i e ee
o u 15
ee i ou o a a
ee u a e e a a
e u e u ee
}
i a e
u i e ee a
} e i
cl ss Sp d {
p bl c:
Sp d(v d) { l st = sp d8 = 0; }
ns gn d sp d( ns gn d cnt) { //cnt = n w c nt r v l
ns gn d = cnt - l st; //ch ng n 2. ms
l st = cnt;
ns gn d s, s8 = sp d8;
f r (s = ; s8; s8 >>= 4) s += s8 & ;
sp d8 <<= 4; //sh ft t ld d t
sp d8 |= ; //s v n w d t
r t rn s; //r t rn sp d
}
pr v t :
ns gn d sp d8, l st;
} l ft, r ght;
Speed of Car
What would you choose to represent the speed of the car? Left wheel or right wheel?
My choice is: LEFT + RIGHT
PID Controllers
a i
u i
i u i e u i e i u i e i i
e e
}
i o o i e o
i e a e o a
a e o
a e o
i a A a A
i a I a I
e u e o a i e a
}
oi e e oi a a }
i a e
e u A 1 3 5 7 9 I 1 3 5 7 9}
u i e i
i a a
} ee 1 3 5 1 ee 1 3 1
cl ss P d {
p bl c:
P d( ns gn d p, ns gn d , ns gn d d):p(p), ( ),d(d) {
r s t();
}
nt c ntr l( nt rr r) {
nt d lt = rr r - l st;
l st = rr r;
cc += rr r;
f ( cc > M X) cc = M X;
f ( cc < M N) cc = M N;
r t rn rr r * p + cc / + d lt * d;
}
v d r s t(v d) { cc = l st = 0; }
pr v t :
n m {M X=2 2 4 6 8 , M N=-2 2 4 6 8 };
ns gn d p, , d;
nt cc, l st;
} sp d( 2 4 , ,0), st r( 2 4, ,0);
Please note that the integral coefficient i is inverted as denominator. In this way, floating point mathematics is avoided.
Update PWMs
oi u a e oi a i e e 5
i e ee e e e ou e a ue a e u a e
i i ee e i
i ee o o ee e
i ee o o
i
i
i 999 999
i 999 999
e i
}
v d pd t _pwm(v d) { //c ll t v ry 2. ms
nt lt = l ft.sp d(g tL ft()); //wh n c nt r v l s r pd t d
nt rt = r ght.sp d(g tR ght());
nt sp = sp d.c ntr l(sp d_s t - (lt + rt));
nt st = st r.c ntr l(lt - rt);
lt = sp - st; rt = sp + st; // previous typo mistake: rt = sp + rt
f (lt < 0) lt = 0;
f (rt < 0) rt = 0;
f (lt > 44 ) lt = 44 ;
f (rt > 44 ) rt = 44 ;
pwmL ft(lt); pwmR ght(rt);
}
Call this function at the end of void poll(unsigned).
留言
張貼留言