PID 循跡自走車範例

1. #include <Arduino.h>
   3 X2 {* q# j# ]5 ~
2. #include <Wire.h>
   
3. #include <Servo.h>
   ' l; ^9 [9 T: C# v% A7 [# `, @$ P
4. 
   5 N  l$ _( U* ~3 r8 P' H- R" X
5. #include "MePort.h"
   ; w% Y' i+ a* o( U$ y9 |
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   1 x* x9 X  j* J1 v* X5 P. l
8. 
   3 A; I- T! p" M& A" a4 |' s
9. //double Input, Output ;
   % ]# a4 M# Q9 V% L! d
10. float MaxSpeed = 255;
    4 r6 k4 ~( I; k# T* R+ }
11. float MaxPower = 180;
    
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    # d. b; [8 ]: E2 A
14. 
    
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    - M  _4 X% A1 k  m0 i1 L
17. float Ki=0.000201;
    
18. 
    2 f7 L# ?# z* P- d1 D: Z* I/ b
19. float nPower;
    
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    3 y5 ?! \3 _6 u9 o+ B4 z9 `
26. 
    
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    8 O" l! n7 T2 T) c
29.     nPower = 160;
    
30.     Error=0;
    
31.     ErrorAcc=0;
    % K5 \2 O& p, l" H3 y* P0 o) J7 G$ {
32. }
    
33. 
    8 Y8 ^' Q0 Y' c; p, ^
34. void loop(){
    
35.   unsigned long StartTime = millis();
    : p0 ], U; x1 B+ M: g
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    " E$ k" n2 D4 H8 f# d3 K) T
38.     ErrorAcc +=  nError*Kd ;
    . N5 Y0 |( g" d
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    - I: @5 m; H' p* Y, ~6 G8 }6 f
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    # D: n. c$ m+ n/ s/ ^
43.         nPower += 3;
    6 t& n% i! E$ [& s
44.       }
    
45.     } else{
    
46.       if(nPower > MinPower){
    ( H8 i6 y+ V, w" J
47.         nPower -= 2;
    & B% }- \: b. C. o) m+ d
48.       }
    
49.     }
    * I8 Z  E3 [  d+ q5 T! w$ y8 w
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    
52.   }else{
    
53.     motor_9.run(0);
    . O& E) a6 ^* n. D) ]: h
54.     motor_10.run(0);
    
55.   }
    8 r' a8 R# Z  C
56.   do{}while(millis() - StartTime < interval);
    
57. }
    : K. T: |! R5 a8 s. L
58. 
    
59. void MotoL(int Power){
    8 I4 w! i8 U0 ]4 a
60.   if (Power > MaxSpeed){
    5 b. {/ k% I; a
61.      Power = MaxSpeed;
    
62.   }
    
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    % @6 J2 w5 A6 n* Y4 k4 o& r& G
65.   }
      ^0 Y( y! u$ p. Z: g. G3 [
66.   motor_9.run(Power);
    
67. }  
    * Z8 {/ w, B- U7 ]3 x6 f- F
68. 
    
69. void MotoR(int Power){
    1 g# a9 e8 l3 H5 x& o, S( {+ ^% Q
70.   if (Power > MaxSpeed){
    , a1 W; B1 ]# n/ q
71.      Power = MaxSpeed;
    + o+ @7 o* M  ^  L  F5 X# j
72.   }
    * w' g% h" r/ P% D
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    & O! t0 |4 O' `9 P
75.   }
    
76.   motor_10.run(Power);
    
77. }  

複製代碼

: r! w& X6 H8 X
您好，不知是否能向您請教。
' E, k; x+ y! f  Q9 v目前和宋修賢老師在處理Ardui Car
  w* K" I9 S0 t) P: Y8 Y6 \雖然已使用較繁雜的方式處理了跑出黑線外的狀況
: w( r7 ~  \8 {5 G5 V' g% m
. O& i& p% K5 D, w, x0 U/ i但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
' |! I2 p' r$ h4 }) o/ K1 @8 T不知道您是否願意教我可以如何處理
4 `% W6 c) W7 c* [& v3 b

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

1. int MotoSpeed = 250;
   8 N1 R( q$ x- |; J: a
2. double CNY70Val = 1000;
   
3. int interror = 0;
   
4. int olderror = 0;
   
5. double values;
   
6. 
   : B' w3 X0 L$ T) t+ C
7. 
   
8. void CNY70()
   : x$ l: k3 v6 ]$ r
9. {
   
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    - O% `2 t" ^6 a4 z2 }7 X" t, u
13.   valuesML = analogRead(ML);
    " A) b" q% q' a; G$ Z4 ^, \0 M) E
14.   valuesLL = analogRead(LL);
    
15. 
    
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    5 V8 P* O. M$ Q; Y; d) `
18.   if (valuesMR > CNY70Val)
    & y+ {: B0 m* {( g
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    , t) U- N% H8 b9 u5 J
21.     valuesMM = CNY70Val;
    1 {) N% I$ d' ]; n
22.   if (valuesML > CNY70Val)
    6 L- q& r% b  T0 O
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    . B" `) {+ g9 [9 R" D
25.     valuesLL = CNY70Val;
    ( G, H1 Z$ [5 ~% X/ v. l
26. 
    4 \8 z! ~7 p. B6 ?
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    1 w, t% ]. X: i# n3 f8 O0 W
28. }
    . O! X8 F6 B  q1 x1 C# ?# e
29. 
    
30. void Car()
    
31. {
    
32.   while (1) {
    7 l% N) C, V0 K" I5 _
33.     CNY70();
    7 n, F0 j4 s# o! }) \* c, w" f5 Z
34. 
    % r7 s8 I* r" {, r# T  J- R: D) U
35.     int error = ((int)values);
    4 t! c8 x/ Y; E( M: o2 M
36.     interror += error;
    
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    8 q; E, O1 s* [) |, _7 q8 m% ~; n2 x
41.     if (power > MotoSpeed)
    . o- G: @9 h: e, @5 F2 N/ B. n  v
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    ' S* a7 U+ r9 l; A+ Q1 v0 i
45. 
    1 i% A8 `2 X* j  S7 H  K& A* f
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    ' s8 F; o7 {# m6 `) c
48.     else
    9 `! t- W0 t$ V
49.       Speed(MotoSpeed + power, MotoSpeed);
    0 L' |% c0 p& e7 i
50.   }
    
51. }

複製代碼

感謝分享

感謝分享