PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   0 B* i6 L/ d% D, ]6 u' q9 e
3. #include <Servo.h>
   
4. 
   
5. #include "MePort.h"
   # q, Z. a: _! g
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   6 e- _  V9 L- E" O) s
8. 
   
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    ' I1 X, M  {, y
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    
18. 
    
19. float nPower;
    
20. MePort lightsensor_6(6);
    % D7 p: Z# v# ~
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    6 c1 c8 C  l3 t/ o  S' X! e
25. const long interval = 1;
    
26. 
    
27. void setup(){
    5 T& b: y9 G$ ^8 U! V
28.     lightsensor_6.dWrite1(1);
    5 A3 X0 \: c: U) z  l+ T) d6 \
29.     nPower = 160;
    8 W8 H9 r$ @+ u* q- D6 P8 P
30.     Error=0;
    
31.     ErrorAcc=0;
    
32. }
    ' C6 e& l8 U; P; {1 g- {
33. 
    4 w- h, d; w7 q$ T
34. void loop(){
    : q7 L# U- A5 O; R; \$ Z
35.   unsigned long StartTime = millis();
    
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    1 L/ u: U1 D6 I" }4 }
38.     ErrorAcc +=  nError*Kd ;
    ) j9 [2 n6 {; M2 ?
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    4 l+ r- |( j9 l' L
41.     if(nError < 80 && nError > -80){
    # T* u5 N( P& I' E3 d4 V6 t  ]
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    ; S9 j/ p1 ^/ P; S
44.       }
    ; n# c4 {* f1 i1 K! g& D
45.     } else{
    
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    " G" i7 g4 n" w; a7 S# j
48.       }
    : Q( c* N8 |; E+ {
49.     }
    
50.     MotoL(nPower-Error);
    * y& K9 ~* J  r5 C
51.     MotoR(nPower+Error);   
    0 ?) j: }9 n( G9 [  g8 W; E
52.   }else{
    
53.     motor_9.run(0);
    0 g" E9 _& e7 N! W
54.     motor_10.run(0);
      ^* e5 F* m5 i! S. @( z6 \
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    $ u2 J2 B% `! P  u8 R/ T) ]& S
57. }
    ) ~) m5 O! J. f; |
58. 
    / e* C: m0 `# u2 h3 v/ g7 @7 S
59. void MotoL(int Power){
    " r+ b  ^% a: d, I' `
60.   if (Power > MaxSpeed){
    7 c( Z! J3 Z2 M- v- m% t4 g/ n
61.      Power = MaxSpeed;
    
62.   }
    0 d) F0 K; v9 B- H6 g
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    $ t3 ?; X9 F- Q# j
65.   }
    
66.   motor_9.run(Power);
    & {5 C5 e  g7 n8 q9 A! q
67. }  
    ) T( |. p5 {% K1 q4 @
68. 
    
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    # w# g% c/ S# h* c8 d3 A# D
72.   }
    
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    
75.   }
    : F1 C, `+ Y( s3 m' R
76.   motor_10.run(Power);
    ( Q" n8 {. C8 l9 z
77. }  

複製代碼

- g3 {" E6 G; @8 M# a3 \# P

8 K* J; n0 w3 P6 W1 `9 x7 c
您好，不知是否能向您請教。
; i  R& Y# f* N1 f6 u3 v( Q4 D目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況

但基於想追求更精簡的程式所以還是想請問一下
) p" `/ i+ _1 s/ B就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
. h& B6 Z* \* o+ B; f( t) a$ r5 U4 J2 N不知道您是否願意教我可以如何處理


" @( c; }/ Z: @以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   
3. int interror = 0;
   4 i; b. o1 G' x8 k: q
4. int olderror = 0;
   
5. double values;
   0 t2 [" |0 G$ E# H' X# @9 t
6. 
   1 p4 T8 O9 C1 M2 m' u+ \7 `
7. 
   8 s/ o, L" D  ^& g
8. void CNY70()
   ; x- D- J* @% E& L( J, S) X5 B
9. {
   # L* Y8 ~4 l6 q/ O' W9 h
10.   valuesRR = analogRead(RR)
    ) o. X/ h" b. N, p7 Z! `
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    " j; d; M) g; H& H$ v. b
15. 
    1 F. T) n, c# {, a; u$ M
16.   if (valuesRR > CNY70Val)
    ( \5 `, b' y$ ]7 e6 z
17.     valuesRR = CNY70Val;
    2 N% [7 Q' p9 t9 S# T& G* _
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    2 D1 a, s5 [; t, i& P  Q& Z
22.   if (valuesML > CNY70Val)
    0 x+ \5 e/ T9 n9 [
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    , d! o: `' \/ l7 ?! c
25.     valuesLL = CNY70Val;
    7 a" P  Y' f6 g* L2 K  [
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    & o  {, _( W* [# b1 B* }
28. }
    
29. 
    
30. void Car()
    
31. {
    
32.   while (1) {
    
33.     CNY70();
    
34. 
    $ G8 v7 `" h3 {7 ?# x4 |  G; a
35.     int error = ((int)values);
    5 D4 S. Z4 ^% C8 X- r( r
36.     interror += error;
    
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    1 Q9 ]. k" `: s9 P
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
      D; t' e% i' p, `3 g
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    & \4 v0 o: [( |0 X
44.       power = -MotoSpeed;
    : Q* I1 Q+ d# C  Q8 S" L. U
45. 
    # l/ W) D; k. S- c2 K" @
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    ) Z5 G" r6 n! g8 b. L- z
49.       Speed(MotoSpeed + power, MotoSpeed);
    9 w. t( Q( R% Q: C
50.   }
    / Q6 @9 \1 h% r
51. }

複製代碼

5 q. I9 v! I. w9 v- S, I. N; A

感謝分享

感謝分享