PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   
3. #include <Servo.h>
   7 J& T4 @+ O! F0 e6 {: }) K5 ~
4. 
   ' u+ ~* k/ @2 z8 }( f- O
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   
8. 
   
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    - g. y+ x' E. V$ w$ A
11. float MaxPower = 180;
    
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    ! L5 `# }$ r5 b7 v7 Q
14. 
    - u$ C; D1 _6 |2 W5 s4 s3 y. ^, g
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    1 V" o% Z" k$ R: q
17. float Ki=0.000201;
    
18. 
    
19. float nPower;
    + Z3 m' G  r0 p0 k: F8 D
20. MePort lightsensor_6(6);
    & i- q& K- c5 ^; k
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    ! I9 i/ H* r1 i2 m' c
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    + T; O9 W( ]' E
25. const long interval = 1;
    
26. 
    
27. void setup(){
    ; Y9 B3 F$ D; @/ W+ \- P
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    5 Q/ u, |# f, t- B
30.     Error=0;
    
31.     ErrorAcc=0;
    
32. }
    # \, }7 H, @! [; A. v, x: s
33. 
    
34. void loop(){
    # `' \+ r" ?4 S! c
35.   unsigned long StartTime = millis();
    
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    3 d1 H% C$ u0 a: F% M) M
41.     if(nError < 80 && nError > -80){
    : _7 m' w* G4 U4 ^
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    2 I# x5 Q8 N8 B
44.       }
      t) Y% {  h, y- x! I/ O% |
45.     } else{
    : ~  g# U# w1 s# a/ ?; w7 S
46.       if(nPower > MinPower){
    # }- }" B9 ?+ x* d, y
47.         nPower -= 2;
    3 N7 Q5 h4 A3 T) a
48.       }
    
49.     }
    ; e  {7 i9 ]6 D* }. v$ f3 T& k# r
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    
52.   }else{
    7 v/ n  P0 f% t
53.     motor_9.run(0);
    / d* w  s3 t; z) I5 D  j
54.     motor_10.run(0);
    ! o4 L; P# [: i! P, F) p: V, @$ l
55.   }
    & C. J9 _6 b4 Q
56.   do{}while(millis() - StartTime < interval);
    & j- A# f! x1 B1 }6 c
57. }
    
58. 
    1 n% D. m1 b# z' _. X% m* w1 U
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
      O& K9 T4 S. K, @7 U) A0 o
61.      Power = MaxSpeed;
    
62.   }
    
63.   if (Power < -MaxSpeed){
    ; t& t8 f. r. v
64.      Power = -MaxSpeed;
    
65.   }
    8 S9 |/ J" h+ n* B
66.   motor_9.run(Power);
    * {( O- ~; i2 f/ }
67. }  
    4 L, ?& O; n  y# v" q% A
68. 
    
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    
72.   }
    
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    
75.   }
    0 e# g0 D; r; n1 i. @! S+ F8 K
76.   motor_10.run(Power);
    ; {+ C1 Z% |: o, j: ~
77. }  

複製代碼

, C9 \& }! A* T, }1 f# ~
' A6 G8 y$ u- k- T' w9 T您好，不知是否能向您請教。
4 O) A" W: |. j目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況
/ t3 U9 I, M  [
1 ?) Q  Q" ~; {$ {4 |但基於想追求更精簡的程式所以還是想請問一下
+ P! j# E; Z) c就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理
0 t( C% O) ~$ N; F
# Z! E  ]0 q9 w1 L5 ^
以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   + E: U/ z8 e% T( i
2. double CNY70Val = 1000;
   6 R. e0 g* H: q) D
3. int interror = 0;
   / s. f! K+ |' F
4. int olderror = 0;
   - W; V, C) d" ?+ w2 G# o
5. double values;
   - ]" H; e/ ~. j2 ^
6. 
   
7. 
   - y( u2 j9 y: d4 z
8. void CNY70()
   9 h3 z& a; `4 b, i
9. {
   * H8 N0 k# G" b+ X0 Q  k! K
10.   valuesRR = analogRead(RR)
    / t; R$ K4 U6 G7 A0 t, z5 V
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    # E- b7 q$ n* t% ]% B; o, r
13.   valuesML = analogRead(ML);
    , Y6 K0 r7 ~$ ~2 G& e3 N* T
14.   valuesLL = analogRead(LL);
    ' K$ f9 o" g! T: j- d4 P
15. 
    " v1 X/ S3 z1 s8 A8 m
16.   if (valuesRR > CNY70Val)
    * u- _9 i' ~% @+ f9 y% y
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    ( `3 X" n% v- i* C
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    $ U: V+ k( C  Q6 p7 U) p4 W
24.   if (valuesLL > CNY70Val)
    3 f  i) x6 t( j& q- s" v
25.     valuesLL = CNY70Val;
    
26. 
    $ F, U$ n5 X/ r6 ^+ e
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    6 c% S% F! U) k4 {
28. }
    
29. 
    
30. void Car()
    
31. {
    & o1 |; O& I' f8 d7 L
32.   while (1) {
    ( U' }: |7 ?3 L  Y$ q" z
33.     CNY70();
    
34. 
    
35.     int error = ((int)values);
    ; v+ w+ K& A5 B+ ?# T" i# h" L
36.     interror += error;
    9 y) t- O! V0 T; q5 R6 N
37.     int lasterror = error - olderror;
    ; H! k/ P1 |! N! p
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    , I& m. }* n, L! ]
40. 
    
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    : [+ d  y( F2 U
45. 
    1 @9 _  E$ \9 g5 E5 ]
46.     if (power > 0)
    ' B/ J8 \+ `: j& v+ \" r( V
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    9 t1 s. x, ~) C2 b7 a+ v2 g
48.     else
    : o4 g% Q; V( d+ ]
49.       Speed(MotoSpeed + power, MotoSpeed);
    9 i, N/ C; X- z# ^6 Z9 r5 e
50.   }
    
51. }

複製代碼

4 R" @: w: l. z& ]# K6 g" i) r4 |

感謝分享

感謝分享