PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   + ~4 Y! x9 r! c. }
3. #include <Servo.h>
   8 F* l" Y7 _* p8 l/ I
4. 
   
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   
8. 
   $ I, O8 g6 r# I$ r% h* j. E6 d4 v) G' ]* L
9. //double Input, Output ;
   % L, L9 w9 B7 g+ p& S% z: ]
10. float MaxSpeed = 255;
    6 V9 R, U3 G7 u3 E) l
11. float MaxPower = 180;
    4 T0 }7 I2 b# \- a" ]
12. float MinPower = 120;
    ) @' L" f+ ~8 q% X- Y' g
13. float Error,ErrorAcc,ErrorDec;
    - c; D4 q. A0 `* ^3 c* S
14. 
    
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    & a1 u' m3 v- z, x) h) x3 e
18. 
    ; Z1 m) ?8 j5 i' `* I% U1 n- ?7 b
19. float nPower;
    ) o. ]! Z4 p9 |% j, \: _
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    4 \" E+ B( c1 J  y5 L9 R$ B
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    
26. 
    
27. void setup(){
    " F& i! i: ~/ _3 P
28.     lightsensor_6.dWrite1(1);
    & s0 G8 M8 ~5 `) B- y
29.     nPower = 160;
    3 a0 |. w$ A. I3 B, T1 u, Y5 Q
30.     Error=0;
    / S+ j# D' p( k- s( b/ O( u6 S
31.     ErrorAcc=0;
    & {0 [3 L$ Q, y" ~( p' ?
32. }
    , X7 ?- f) U# ]. m# F9 D! x/ w9 G- z
33. 
    
34. void loop(){
    ( J- v  t) M4 U1 T) y
35.   unsigned long StartTime = millis();
    8 X3 g$ H  |& g7 c, C
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
      g5 \; v7 C- S0 r# m, S
38.     ErrorAcc +=  nError*Kd ;
    * m+ m. T8 K: m4 [
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    
44.       }
    0 j+ W8 E2 ~; }: T! x
45.     } else{
    
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    9 G5 _! {. O2 L3 e3 `  ]
48.       }
    ' `$ y8 C1 S. [( n' @
49.     }
    0 H$ u2 a1 e" N+ c7 m7 T* Z7 C. i- B
50.     MotoL(nPower-Error);
    1 ~% I2 a8 ~0 u
51.     MotoR(nPower+Error);   
    
52.   }else{
    
53.     motor_9.run(0);
    / l, `2 e% ]: a9 J) j
54.     motor_10.run(0);
    
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    ) O. F$ u7 ?! Z9 F: s% {
57. }
    6 e% M; t) M+ Z
58. 
    
59. void MotoL(int Power){
    ) m& F+ m: u/ ~! c0 v" s  E
60.   if (Power > MaxSpeed){
    # K( A8 w# e% q' I
61.      Power = MaxSpeed;
    
62.   }
    
63.   if (Power < -MaxSpeed){
    . J) x" G/ h0 s" h& |5 E9 w0 b5 o
64.      Power = -MaxSpeed;
    5 E  n* E+ U/ I* P/ M
65.   }
    
66.   motor_9.run(Power);
    
67. }  
    
68. 
    
69. void MotoR(int Power){
    * G7 x* r, z$ N3 o. I# N; O
70.   if (Power > MaxSpeed){
    * k' p. S- B8 o1 \  S
71.      Power = MaxSpeed;
    
72.   }
    
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    
75.   }
    
76.   motor_10.run(Power);
    , u: \7 T# P) f2 c4 q
77. }  

複製代碼

/ m- U# ], ~% f; I# c
4 a% C! Y! T- d0 r+ f

/ u5 |! v' D' D; K; o4 r. [
" ?! |1 \7 A; ~6 t( a% h您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
+ |9 d1 Q# u5 g1 x雖然已使用較繁雜的方式處理了跑出黑線外的狀況

但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
2 [( {* w" Z4 s, a不知道您是否願意教我可以如何處理
$ b2 C4 Q8 T: a* G  _, V+ g
+ r& q3 u  \) L- v5 R# L! o; q
以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
     [# p1 M- o) t" ^- w5 Q
2. double CNY70Val = 1000;
   1 D- Z6 y$ O; C2 b8 j
3. int interror = 0;
   
4. int olderror = 0;
   
5. double values;
   
6. 
   
7. 
   ; p+ n9 t3 {  @$ y
8. void CNY70()
   4 U3 F1 a( i) K
9. {
   / c' v/ F1 I0 n$ ], |% S
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    7 x& x  D, [4 B- X! a
14.   valuesLL = analogRead(LL);
    
15. 
    , s) N6 Y: u  Z0 ~0 k
16.   if (valuesRR > CNY70Val)
    3 v1 x8 J% `0 h$ F6 P# g  Y
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    $ _, o5 p% v! y: V- |
19.     valuesMR = CNY70Val;
    * w# A) T) G. A* x$ |3 \! @
20.   if (valuesMM > CNY70Val)
    $ V- o* I# K% b1 {! B
21.     valuesMM = CNY70Val;
    $ M0 d1 L* s, Y
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    9 U3 `: W5 [% p2 D
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    + S3 Z3 F) }; z2 z
29. 
    - P. Z3 U5 y1 `0 o
30. void Car()
    
31. {
    
32.   while (1) {
    4 W' B! d  c$ M# j
33.     CNY70();
    
34. 
    
35.     int error = ((int)values);
    
36.     interror += error;
    
37.     int lasterror = error - olderror;
      k! ]% I/ _2 l; W8 g9 R3 h, _+ X& N
38.     olderror = error;
    ) S. E0 r. }4 V
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    / {9 _+ c7 X* l$ b- p
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    0 F& c) w$ A& a; \, d8 y" g
45. 
    
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    ) c, y* w+ s2 \5 _  h( Q6 s& n
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    + n7 _- w' N/ m0 A
51. }

複製代碼

0 n. r$ l8 N4 i0 j' Z

感謝分享

感謝分享