PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   : C# T% u3 Z- P# f
3. #include <Servo.h>
   
4. 
   
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   0 W4 F2 b0 y* S6 z! n8 X+ k7 D
7. #include "MeDCMotor.h"
   8 e  l: ?2 b& b' |! k4 L, C  q
8. 
   
9. //double Input, Output ;
   + w  L0 \5 i1 d% I& ?
10. float MaxSpeed = 255;
    7 n- ?# R6 H( K% e' q
11. float MaxPower = 180;
    3 t/ v5 y1 L+ Q  f9 S# m
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    , ^" d8 W! i, q( N, Y
14. 
    & K+ m& n' q1 o* s( Y0 [* f
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    
18. 
    
19. float nPower;
    
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    " W- b/ K5 h' }# N
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    8 k1 T: D. p5 Y4 J8 B; U
26. 
    
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    ; g: F7 l1 X/ q2 F) R6 N
29.     nPower = 160;
    
30.     Error=0;
    6 N) Y& G; a% V! Z
31.     ErrorAcc=0;
    
32. }
    
33. 
    
34. void loop(){
    - T: G- V4 _) e& R! y7 ?/ Z2 F% ]
35.   unsigned long StartTime = millis();
    . a6 r9 N3 S4 k, [& j- @
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    . u% c8 ^5 I; W) M# ]
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    $ y0 }- c$ [$ y
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    $ X2 J0 _1 y3 U
41.     if(nError < 80 && nError > -80){
    . q: ], p8 P. S6 N0 \1 U
42.       if(nPower < MaxPower){
    ( E6 g# y% n, ]# K+ b
43.         nPower += 3;
      @; X8 I0 P( R
44.       }
    ! ^" j0 D" g1 g# C
45.     } else{
    
46.       if(nPower > MinPower){
    : O3 @+ O- u0 k. @4 V
47.         nPower -= 2;
    
48.       }
    , q6 `1 _8 _& f+ F4 x
49.     }
    , h# @# c% s$ x2 P2 z; c5 x
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    
52.   }else{
    : Z7 A; J" N+ F9 s4 |
53.     motor_9.run(0);
    6 o: o8 k; y5 s0 B2 K. ^( i1 l) F
54.     motor_10.run(0);
    ' v) \1 e6 e( x$ a& @; Y! S( u
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    ( ~' _# w7 H9 B
57. }
    5 J) c. W0 [& e
58. 
    " T' ^. l" R. y; X& l5 [1 t
59. void MotoL(int Power){
    ! B. ]. `: H7 k+ |
60.   if (Power > MaxSpeed){
    6 F  k/ N4 f; n8 C0 R7 k! X
61.      Power = MaxSpeed;
    
62.   }
    # M9 J0 _8 L2 v5 w9 _
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    ; T. O. `& ]1 w! ]
65.   }
    " x$ R% W% q3 Y( E$ p# H9 _
66.   motor_9.run(Power);
    + s4 k/ J1 R( z
67. }  
    " _( J- l9 I: _* V* H! P
68. 
    $ i$ d* c- s" g) ^5 J7 B8 \# H& s
69. void MotoR(int Power){
    7 a/ W" y$ b! z
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
      t  K# R. ?1 X# c% C" p2 Y
72.   }
    
73.   if (Power < -MaxSpeed){
    8 Q' `: m, G. e& Y9 B* g
74.      Power = -MaxSpeed;
    
75.   }
    / {. j2 l3 |7 z  r
76.   motor_10.run(Power);
    6 t  W8 J( [7 K# s% Q' g' g! H
77. }  

複製代碼

+ ~; ^9 q2 T/ s6 z* e9 o0 ?6 [2 U% f; i
# Y6 f+ r4 e1 m$ P您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況

0 ^2 F2 l- P, \% }但基於想追求更精簡的程式所以還是想請問一下
2 i; c( o6 j2 ]就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
( O9 b5 a9 D1 @5 P不知道您是否願意教我可以如何處理
: G% `" \. s) |8 H- @

* Y6 [- |) a4 |以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   1 k  N) M6 Q# t- e
3. int interror = 0;
   9 P9 R, ^% ?: J  R$ J' S
4. int olderror = 0;
   6 k% v- L6 m; t3 T
5. double values;
   
6. 
   ) `8 b9 U! M* `0 j  {" }
7. 
   
8. void CNY70()
   
9. {
   . W! {6 f) J( [
10.   valuesRR = analogRead(RR)
    ! G2 P' b" \2 Z
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    ' J, B" H, c. `  Q; q- A
14.   valuesLL = analogRead(LL);
    
15. 
    
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    ; l, S  o7 r) c1 J, L- a
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    , `. B( k: \7 y4 ]2 @, U: D7 U
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    + }. M, q; }( @; W5 J8 _/ T! r6 H( n
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    $ z, [6 }4 G  x
28. }
    
29. 
    
30. void Car()
    4 u( W/ f1 @+ c! `, M8 e
31. {
    " ~) l2 f/ o. Q9 O
32.   while (1) {
    4 U" T" \. X. C& i& b- `
33.     CNY70();
    
34. 
    : f# c, q7 h( h
35.     int error = ((int)values);
    , `) Q9 N+ e* H* u% i/ @
36.     interror += error;
    " V4 S3 g8 n) f# t3 D
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    ; r. F: N4 u0 `+ F* W
40. 
    9 [: h3 l1 m- G
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    
45. 
    2 k; o( x/ C! y5 c5 @5 ^1 W
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    7 y" g6 i3 k" F) L4 g# _' K- L
49.       Speed(MotoSpeed + power, MotoSpeed);
    * I( w/ Q' O9 }- ]) @" K3 p
50.   }
    " o2 O' X0 e$ B4 }  c8 X
51. }

複製代碼

感謝分享

感謝分享