PID 循跡自走車範例

1. #include <Arduino.h>
   2 |- _8 c7 w% [/ l* q# H
2. #include <Wire.h>
   
3. #include <Servo.h>
   0 a5 ^9 x/ z3 h
4. 
   6 h2 a/ S2 `% m) f' k4 Q; N$ z
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   , W+ ?1 y1 w, N  R6 Z
7. #include "MeDCMotor.h"
   
8. 
   % z3 u8 ~/ c+ [3 d
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    # @& B: l; B& \6 c' X
11. float MaxPower = 180;
    
12. float MinPower = 120;
    - `5 g1 [, j5 o+ f' \  Q. i; E2 J" A6 r
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    - E8 F' ~, K7 e; b# V% r& U
18. 
    8 B6 M3 e/ W5 ?: |3 N# x" C
19. float nPower;
    
20. MePort lightsensor_6(6);
    - A1 A) D+ ]0 u. ~
21. MePort lightsensor_8(8);
    & @; W' m  U, w, E# t$ m
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    3 J" I& a- j5 K  Q4 |
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    # I$ l0 e4 b; d: `
26. 
    7 A, ~. }9 W8 M# U3 [
27. void setup(){
    - e2 p% l8 `) x; ~
28.     lightsensor_6.dWrite1(1);
    : T" x7 L: c/ u0 G6 m
29.     nPower = 160;
    $ Y& {- ]1 D) w; d1 T8 q
30.     Error=0;
    + m% q0 `" ]- A+ {
31.     ErrorAcc=0;
    
32. }
    
33. 
    
34. void loop(){
    
35.   unsigned long StartTime = millis();
      J$ f1 a  n! k* o8 n  ?% h
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    + h+ j2 d( K5 ~( n& o
38.     ErrorAcc +=  nError*Kd ;
    / a. Z- Q- B( {$ @+ `8 ?5 x
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    " q* y8 \* C/ r: @+ }
42.       if(nPower < MaxPower){
    : o) u  H6 y3 u4 |# y5 O
43.         nPower += 3;
    7 J- ~8 o2 P# x
44.       }
    
45.     } else{
    
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    : E8 v! k! Y4 E2 i9 r" J& r; ]* e
48.       }
    
49.     }
    
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    
52.   }else{
    
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    
57. }
    8 v, k% I; {/ d
58. 
    
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    5 z/ N  }6 i/ a
61.      Power = MaxSpeed;
    
62.   }
      f, L8 A$ h/ M/ p
63.   if (Power < -MaxSpeed){
    + w: F: u5 ?* j9 U& }( W& e
64.      Power = -MaxSpeed;
    9 @$ V6 w* x0 P
65.   }
    & j: b  Q( u2 r- C# G: A9 N; s
66.   motor_9.run(Power);
    9 q0 Z- z; [) T9 G% C- G% Q' k* Z
67. }  
    % ?* g1 M) L9 c$ {' ^( _
68. 
    
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
      K( q: A% M8 n) x! ^* h, h0 @
72.   }
    ! B) x5 B( Q8 e( k
73.   if (Power < -MaxSpeed){
    2 B7 @! x4 O$ W( }+ a2 E( f, J9 T
74.      Power = -MaxSpeed;
    % ]6 @5 x( c# s8 W. y& i/ l
75.   }
      L$ E4 R0 M  {7 r! i2 q
76.   motor_10.run(Power);
    
77. }  

複製代碼

4 v) `6 ]2 K4 Q. X9 T: g
6 y" Q$ `$ h  r0 p/ G  C% J) C

- i& ^7 D: d. v( h$ Z. x( L2 e
您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
" X. j* S( M: B  e+ [& A雖然已使用較繁雜的方式處理了跑出黑線外的狀況
) P: q/ w) M: |; B$ d- S' e
但基於想追求更精簡的程式所以還是想請問一下
$ k+ o2 Q, a$ U8 C* E* e就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
" Y( }+ o8 C0 g. Q; |3 i# m不知道您是否願意教我可以如何處理
- o1 s) D3 f0 A) F" G. n

以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   7 S! X5 o3 l" ~( ]8 \  E
2. double CNY70Val = 1000;
   
3. int interror = 0;
   6 r# I$ x6 K! V5 `  D; d# _  R0 L
4. int olderror = 0;
   
5. double values;
   $ Q" o& P! R- g9 j! s9 \
6. 
   
7. 
   
8. void CNY70()
   
9. {
   # [; \% Y! j( J0 i9 [2 R8 O0 u- m
10.   valuesRR = analogRead(RR)
    ; A2 I0 \  o7 F  \: f
11.   valuesMR = analogRead(MR);
    7 m# x2 T& I6 `% h9 E% R/ P
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    $ P; F1 ]7 o& v& b: L
15. 
    & o& p! A0 ^' w% E
16.   if (valuesRR > CNY70Val)
    . ^* B5 V4 X7 g& h, U. K
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    # G4 w! V1 g& u: Z" |/ u' D
23.     valuesML = CNY70Val;
      K  U% \5 u8 G
24.   if (valuesLL > CNY70Val)
    6 k0 o! f1 P7 z+ W) B: J
25.     valuesLL = CNY70Val;
    
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    $ f0 Q8 \, _" ~6 F) s6 K' r
29. 
      V. [' E  U) w" E  w5 s
30. void Car()
    - i( ?, d5 }/ L) R$ y
31. {
    
32.   while (1) {
    0 }$ b; }' V5 @9 I6 K, a
33.     CNY70();
    
34. 
      U$ k$ @/ l  S: I" O: W
35.     int error = ((int)values);
    1 i( C: v( [% G8 W# Q
36.     interror += error;
    ! K% i7 R1 D5 _' N. m- w7 X3 C
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    ' l0 d! B  t( @* H* i
40. 
    
41.     if (power > MotoSpeed)
    % |4 v; r% X9 c& z% a
42.       power = MotoSpeed;
    ; d7 J( X+ X0 l! a  x9 W2 d
43.     if (power < -MotoSpeed)
    % g9 K3 J' u' B* g
44.       power = -MotoSpeed;
    1 R. L0 m0 o) W5 v2 o% r
45. 
    
46.     if (power > 0)
    , B+ z3 \% O5 s' n
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    0 e4 v9 }3 F8 |
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    8 X# `" A& |  ]$ B
51. }

複製代碼

感謝分享

感謝分享