PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   
3. #include <Servo.h>
   1 ?& l8 h0 J. F1 R0 F  x
4. 
   
5. #include "MePort.h"
   7 g0 O$ E, m( Y
6. #include "MeUltrasonic.h"
   " P0 l& o: n2 \' o# Q( c
7. #include "MeDCMotor.h"
   0 ]1 m, V: q4 M1 G" m0 W8 ~
8. 
   
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    8 C4 }: _+ L$ ]& {) x( e0 F
11. float MaxPower = 180;
    
12. float MinPower = 120;
    ! ~6 n) d5 o$ z8 ^
13. float Error,ErrorAcc,ErrorDec;
    4 ~* U! {( s& v" p! f, h0 x
14. 
    
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    4 G. w: m1 Q7 s* N
18. 
    8 u( J. {) X6 v
19. float nPower;
    9 ~- b* C6 z) ?: |0 L& b0 M. n2 b- p
20. MePort lightsensor_6(6);
    8 l2 Y( r& p. ~: n/ k) F# t3 z
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    2 t$ X  _2 o1 _' |7 v
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    / q. p* c) R- L
26. 
    ) V& x1 f" u0 q- X* B1 Z
27. void setup(){
    " R4 F, S- o8 f+ }/ T% w2 r
28.     lightsensor_6.dWrite1(1);
    * M% r8 \: x5 X5 j" V
29.     nPower = 160;
    
30.     Error=0;
    " Z- R  Q, C/ S( S: Q- s: o8 o
31.     ErrorAcc=0;
    0 `3 ?9 [: k8 I1 K+ ^
32. }
    
33. 
    
34. void loop(){
      K6 D6 I( f- ?  O2 k4 b4 e! U( z
35.   unsigned long StartTime = millis();
    , Y( {. r# v9 P2 W
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    $ s5 _4 ?2 r+ p7 q
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    7 o$ w1 T  P. e  d7 [2 G
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.       }
    
45.     } else{
    9 ?3 I; v; E1 v! E5 F5 w
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    
48.       }
      ?' D$ ^" B4 I7 ~
49.     }
    
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    ! ~& I  J5 r# r5 |4 @
52.   }else{
    
53.     motor_9.run(0);
    + p8 u& y5 [. G9 m
54.     motor_10.run(0);
    0 M+ g( H8 a& v3 z
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    
57. }
    0 A( ^/ C- ^; }) C, u9 }* p
58. 
    - j* w$ _. J& E. c" M/ _4 ?# X) X% G
59. void MotoL(int Power){
    3 N% l- c8 c$ k5 q5 H& R
60.   if (Power > MaxSpeed){
    1 W) U8 p- V; H2 @, c' D8 q# F
61.      Power = MaxSpeed;
    
62.   }
    1 n" ?0 P6 n$ ~: Q% D9 \
63.   if (Power < -MaxSpeed){
    $ ~) k4 ]; H& C5 c$ F8 x" h
64.      Power = -MaxSpeed;
    % r: c( B1 Y5 X* c6 d
65.   }
    
66.   motor_9.run(Power);
    ' D6 [. Z; K' z0 s
67. }  
    
68. 
    
69. void MotoR(int Power){
    5 @. ?0 ]3 v. d$ ?- `# A4 s, H
70.   if (Power > MaxSpeed){
    & @. Q5 ]: H/ H/ s
71.      Power = MaxSpeed;
    - w1 ]- H% i% ]. o  l; {- t
72.   }
    
73.   if (Power < -MaxSpeed){
    $ E# @* M: d8 @+ M/ w2 @
74.      Power = -MaxSpeed;
    
75.   }
    ' H6 B5 U' W1 A% Z0 ^: n4 i0 h) _
76.   motor_10.run(Power);
    
77. }  

複製代碼

+ p% i! y8 U7 T8 g5 Q0 C您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
! c8 `* d) V* \, J9 D雖然已使用較繁雜的方式處理了跑出黑線外的狀況
( a0 X! Y* b  x$ `" K8 d" ?# V
但基於想追求更精簡的程式所以還是想請問一下
3 I0 M( j4 L- B: x  Q9 U1 V就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理


2 K; B: q: z. }以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   
3. int interror = 0;
   , A3 ~+ G* Q! u
4. int olderror = 0;
   
5. double values;
   
6. 
   / H9 W5 g8 s4 [. W: z
7. 
   : ~4 z, k9 e9 Y( R1 l( ~' s& E, k% Q
8. void CNY70()
     H. m4 v( S0 S! p) Z' l
9. {
   & {0 h+ j  m$ P- J
10.   valuesRR = analogRead(RR)
    + t7 X8 ~1 x: c9 O, d8 R
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    % H$ D+ ]3 ^* p; Q8 r, u
13.   valuesML = analogRead(ML);
    $ k/ d# J& C& {
14.   valuesLL = analogRead(LL);
    , l" y& [4 k% U( s/ P; Q
15. 
    % Y% V: s6 S! T6 R
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    + z& s4 G2 S5 X0 c. v% D2 b2 g3 W
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    0 H( y) O9 R5 \$ O' W
25.     valuesLL = CNY70Val;
    
26. 
    * j8 W5 Z; a' n, m  g
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    
29. 
    
30. void Car()
    " D5 E* K8 a2 l+ P
31. {
    - L+ T7 V! D1 e+ e
32.   while (1) {
    ( V4 F+ a, H9 \
33.     CNY70();
    
34. 
    
35.     int error = ((int)values);
    
36.     interror += error;
    - h/ ]; u* e3 K/ [8 s
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    ( [) ]2 Y! \5 b" R3 U6 B9 Y: h
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    4 e) z' s. \" |! Q3 d& p
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    . y- a* x6 a) v2 P" Z- O# P
45. 
    
46.     if (power > 0)
    ! Q0 h# u2 _0 x- P
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    1 v) a  Q* w+ @5 a1 W
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    1 u3 Z5 R4 P$ `# K3 Y1 k. x0 p
50.   }
      U, Y/ v2 b" G- Z3 d8 m' `. b% p7 d
51. }

複製代碼

感謝分享

感謝分享