PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   
3. #include <Servo.h>
   # M5 E$ m% W7 \& [0 n
4. 
   ' Q! L0 J  ]. E5 f
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
     F; p7 B( }) P+ x/ G8 u: A
8. 
   ! D- Y+ D% A7 y5 y6 q
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    , y! s9 G3 N! [  B
12. float MinPower = 120;
    - x$ c- J3 Q" A6 O- m
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    
15. float Kp=0.14;
    7 X6 ]$ t. P' A" M& A
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    
18. 
    
19. float nPower;
    % G1 i8 u  R% P6 D' p) w
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    / ?" s% A: A+ z/ Q/ ?, ^' f
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    % O) d& M& {: U( b
26. 
      e+ M- N6 U2 z8 m0 B- e
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    
30.     Error=0;
    
31.     ErrorAcc=0;
    
32. }
    
33. 
    ' K3 S, W; O5 x- Y2 m# `
34. void loop(){
    
35.   unsigned long StartTime = millis();
    & u/ M9 M5 @4 T
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    7 D* B' f. ~- p6 v
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    9 O0 ^4 p- l% \4 f8 t
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    $ u4 ?  Y/ q% X) Q: i, f
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    % Y1 |' @; t: z5 R4 o  D
43.         nPower += 3;
    3 I6 R  U% h. [! h6 n
44.       }
    ; v* _0 ?5 F7 g1 Y
45.     } else{
      M% d1 B: O; x
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    5 w( p2 X' M7 D0 P/ y
48.       }
    
49.     }
    
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    ) ~; I" r, D# N  U: B
52.   }else{
    
53.     motor_9.run(0);
    3 U! M6 ^$ E6 Q) y4 _' i% p2 r- Q( Z
54.     motor_10.run(0);
    
55.   }
    + B2 C, H9 K! S: Q% W9 X) r
56.   do{}while(millis() - StartTime < interval);
    ' R8 y0 N% f$ |% M; D3 I( G
57. }
    
58. 
    8 w  `0 u7 l2 F9 m$ f
59. void MotoL(int Power){
    ) h! M3 b+ O. m
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    ) e, F0 {. c' r
62.   }
    5 H: e' x( z, L3 ~: A+ m
63.   if (Power < -MaxSpeed){
    4 K! p4 j0 N! ^  G& P! t
64.      Power = -MaxSpeed;
    
65.   }
    
66.   motor_9.run(Power);
    / P4 [; W" U% f$ c& c0 J
67. }  
    
68. 
    ! w% j) I1 O3 t9 I9 ?5 v
69. void MotoR(int Power){
    7 h' Q0 ?: Z( T5 L% R
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    ( m4 j7 t( L. q9 x: p7 ]  B
72.   }
    ; C; n9 d2 F# O: I% ?( X  A( G
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    6 _4 q" j7 P2 O0 e$ O, O3 [: E: S
75.   }
    
76.   motor_10.run(Power);
    
77. }  

複製代碼

8 u5 Z' \" u. l, y( W5 V
2 ~! V/ d+ J5 c3 H% o

您好，不知是否能向您請教。
) d9 z9 F. H3 F8 g! N目前和宋修賢老師在處理Ardui Car
0 K. F2 y  Y1 E  e3 p. e" t" p3 F雖然已使用較繁雜的方式處理了跑出黑線外的狀況

9 i( j. W; d( q- b) \5 c. u但基於想追求更精簡的程式所以還是想請問一下
# R2 Z9 ^) Y" R就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理
: I, {+ x! `4 x% K0 O

  u! K; D. u2 s以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   " }5 U) T( b! D* f4 ?: K& a1 [: A, e
2. double CNY70Val = 1000;
   & Q! H( h1 i+ @6 S$ n
3. int interror = 0;
   : S8 r7 j+ V' }7 u- Y
4. int olderror = 0;
     m) H/ o! J: y
5. double values;
   2 ~& J* Z' r- i1 N# a, O& j0 U
6. 
   9 n! `6 d# X- K( Z+ r2 R# L1 B3 Y3 u/ N
7. 
   
8. void CNY70()
   
9. {
   2 X# e9 h' l+ n  F# c1 ?7 b7 j- H7 `$ N
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    8 Q/ B  E& e0 ]: g1 R: S
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    ) f# `5 }9 L) F7 a
15. 
    1 d1 M2 J1 \0 R: L* a! q* v6 D
16.   if (valuesRR > CNY70Val)
    9 Z$ p) u+ I. S6 h
17.     valuesRR = CNY70Val;
    2 z4 P6 x( V! j$ E8 f
18.   if (valuesMR > CNY70Val)
    0 l: A/ T- Z( v' H
19.     valuesMR = CNY70Val;
    5 ]4 P$ M% B3 l- Q  l3 J' O  S
20.   if (valuesMM > CNY70Val)
    & T; B  ~. ?0 C# e6 @# _+ N
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    8 d0 p! w  l: `1 F1 L9 J) H0 J) W
25.     valuesLL = CNY70Val;
    + M, u2 \& J% E* ^0 p8 w
26. 
    9 y& g) b  F$ _0 ]0 L
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    0 m$ k% {% _& h  d1 A0 `6 a* u. R2 E! B
28. }
    2 T* _9 K2 |3 m" i8 ~% ]+ ]* `6 V! J
29. 
    
30. void Car()
    2 `- G, d2 i- e' Q0 P6 F
31. {
    
32.   while (1) {
    / I& u, e- O; R; ?! y
33.     CNY70();
    5 J( D/ y4 Y& i/ Q4 n! g- s
34. 
    
35.     int error = ((int)values);
    2 y* Y8 d: N) J/ W, ]% G
36.     interror += error;
    
37.     int lasterror = error - olderror;
    6 B4 X- f. a$ g! M
38.     olderror = error;
    * N4 t( E/ [) y1 ]5 r1 c! d
39.     int power = error / 5 + interror / 10000 + lasterror;
    8 F+ {. I9 m9 b  X; I6 R+ ~
40. 
    " x) B& H4 W  p
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    : {1 [- e, m& s6 D
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    
45. 
    2 f) V* |$ I- l5 l& H+ ]& }
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    1 V& o' w( |  o) j/ g6 ]+ n' u' I
50.   }
    ! k! E, h: @1 N  f- o4 `1 I
51. }

複製代碼

感謝分享

感謝分享