PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   + l* _3 N" _8 ?: d# H9 F
3. #include <Servo.h>
   1 S4 ]* i9 P/ }
4. 
   0 m; D0 u4 ?6 [5 v# D+ c
5. #include "MePort.h"
   ( e% G9 P. K' Y- p7 U
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   - N: ]% f9 ?' q- H+ G0 E
8. 
   3 x! E1 F1 x" o! Y" G5 O- k
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    3 m! G- r3 f, C. p0 z
11. float MaxPower = 180;
    " @  Y( a2 ?9 |7 o+ y
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    2 A4 c- @9 f3 B4 l* c
14. 
    0 F8 _8 d" {- Z
15. float Kp=0.14;
    ; ~$ z, o$ J( ]* Y' a9 }
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    
18. 
    ' [2 p( l6 Q; [- E* B
19. float nPower;
    
20. MePort lightsensor_6(6);
    ( J5 x' b: ~, _: p/ `' K* z0 f
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;
    
26. 
    
27. void setup(){
    ' t4 S; v% Y1 w9 Y3 }
28.     lightsensor_6.dWrite1(1);
    ) b# `, j( A. I5 _- C/ l
29.     nPower = 160;
    
30.     Error=0;
    ( F9 S7 ?0 u; [* }/ I
31.     ErrorAcc=0;
    2 M$ d  c0 l0 H, P
32. }
    7 I* v/ H3 O6 R; O8 K8 K8 T+ |$ r. S5 V
33. 
    
34. void loop(){
    
35.   unsigned long StartTime = millis();
    8 l, r4 V  X8 u, \5 [
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    + X2 }# G9 m/ U  @
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    & r+ i, r; L2 I0 W) x
38.     ErrorAcc +=  nError*Kd ;
      k! r4 X( @; i
39.     ErrorDec -=  nError*Ki ;
    . O. _+ l7 p# h1 Q4 |/ S% f6 [
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    ! m3 J% l  G" S& F
42.       if(nPower < MaxPower){
    2 A0 W3 j5 b& ^, X+ _* P
43.         nPower += 3;
    
44.       }
    
45.     } else{
    
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
      j% d0 S/ w7 i* R+ j; S: I
48.       }
    
49.     }
    ! X. f* Y$ ~7 T4 c6 g
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    
52.   }else{
    # q' s! M/ h! S
53.     motor_9.run(0);
    $ B, r9 |; k& ~
54.     motor_10.run(0);
    9 y0 Z, h  U+ f6 w) O
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    0 C) G0 i, x1 k
57. }
    
58. 
    
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    
62.   }
    
63.   if (Power < -MaxSpeed){
    , y4 a3 Y+ n' W
64.      Power = -MaxSpeed;
    " ?1 k( B4 o; d2 k8 P' z/ O
65.   }
    
66.   motor_9.run(Power);
    
67. }  
    ( J. o: b/ k* j! g# h6 g% ]
68. 
    
69. void MotoR(int Power){
    4 \! ~. F: ]: s' d8 |
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    
72.   }
    
73.   if (Power < -MaxSpeed){
    3 f3 a  a# h$ B; D' T% k- T1 }! d7 h
74.      Power = -MaxSpeed;
    
75.   }
    # G' q& ?/ u( E( o
76.   motor_10.run(Power);
    : a0 R- a% G* j9 l$ f9 k
77. }  

複製代碼

您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
5 k5 |& _0 y. Y& Y/ ~0 J3 l雖然已使用較繁雜的方式處理了跑出黑線外的狀況
- J0 T# H% m/ Z0 p- Q
5 o  ^) R1 S" A) u但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
0 }( p" D; i4 V" X不知道您是否願意教我可以如何處理

3 y- I. |( ]5 d1 n* q
以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   ) Q9 o6 ?2 h- J% V" c
2. double CNY70Val = 1000;
     n# n) j6 i& I+ y- J1 d- ~8 j
3. int interror = 0;
   ! @! \; [: F& p; a8 V4 V% I5 h
4. int olderror = 0;
   
5. double values;
   + `6 S9 p6 e0 e6 }" \# z
6. 
   
7. 
   
8. void CNY70()
   
9. {
   ' U* z2 E% M4 j" \) Z* `5 A( a
10.   valuesRR = analogRead(RR)
    6 L0 F0 U3 D; _$ {
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    # Q5 ~0 n7 ]/ ~) ^$ w
14.   valuesLL = analogRead(LL);
    : J  q7 U6 p) G+ C' c) W
15. 
    
16.   if (valuesRR > CNY70Val)
    , W. f/ V5 J) V4 R7 q+ |9 Z
17.     valuesRR = CNY70Val;
    7 ~, X5 [% N" {6 w7 u8 t
18.   if (valuesMR > CNY70Val)
    * V4 i1 S) [# l" N$ \* y: v
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    / ?0 A- }3 T+ B4 d3 d6 {
21.     valuesMM = CNY70Val;
    2 Y( M6 p, y5 ?: A- F" |
22.   if (valuesML > CNY70Val)
    " }3 C: V4 z2 t( |9 n
23.     valuesML = CNY70Val;
    1 k  X/ ~' ~, J5 _9 R+ @3 [
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    
26. 
    0 B. _$ O0 l( z8 q
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    5 x/ R/ W7 H2 B( _; x9 G
28. }
    
29. 
    & E# w( e  G# Q% y% v
30. void Car()
    ; t; b% f7 E0 K) w
31. {
    3 k. P$ b- h0 p* U
32.   while (1) {
    
33.     CNY70();
    
34. 
    
35.     int error = ((int)values);
    : ~% t' {+ {% M* H, d" M! w
36.     interror += error;
    : N- {5 x9 i) t9 a+ r7 Q
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    - V" X0 j; m. j! C2 B+ q- S+ o
39.     int power = error / 5 + interror / 10000 + lasterror;
    - k/ t! B6 D8 ~! U, P% W
40. 
    
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    
45. 
    ) t5 V' Z' h* X# s# o
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    . v7 b; a) c4 s: s% e
48.     else
    - x& T4 p$ y% a3 H8 W0 F) z
49.       Speed(MotoSpeed + power, MotoSpeed);
    ; Y/ y- x- Z/ C1 @0 D5 }  d# b/ `9 c
50.   }
    
51. }

複製代碼

0 P, s  _4 c5 y* d# u4 n
! }9 V& M- t% I- }; M$ }

感謝分享

感謝分享