PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   
3. #include <Servo.h>
   + ~8 ^# M/ ~: r0 `5 W  N; j
4. 
   
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   
8. 
   5 [9 l$ E9 I1 o! a# Y& o# S, H
9. //double Input, Output ;
   " G& }4 u' t! m+ f
10. float MaxSpeed = 255;
    0 `4 q0 M2 R3 O5 @- Z
11. float MaxPower = 180;
    
12. float MinPower = 120;
    ; j# I! A. e! j
13. float Error,ErrorAcc,ErrorDec;
    8 s  h7 E  j, K9 B- M3 m. t
14. 
    
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    9 A1 x$ ?2 T$ t, |9 v
17. float Ki=0.000201;
    ' T" V3 @3 p1 `! Q
18. 
    ) K$ h! W9 C1 A: T
19. float nPower;
    2 E4 q1 W* `' R; Q3 L
20. MePort lightsensor_6(6);
    5 d0 x% f6 C2 C4 b' u/ ^0 O# Z/ U
21. MePort lightsensor_8(8);
    / k' e: e3 i- y6 y% g
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    ' L- N: p0 |6 {$ G
25. const long interval = 1;
    
26. 
    
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    ; \7 d, [0 D+ N$ h- J- k
29.     nPower = 160;
    
30.     Error=0;
    
31.     ErrorAcc=0;
    
32. }
    ' ^" }0 w& Q2 N) H
33. 
    
34. void loop(){
    
35.   unsigned long StartTime = millis();
    9 G8 b3 y- e7 O  Y, x& U! P
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    / y2 f+ L! U6 e9 ~, h
38.     ErrorAcc +=  nError*Kd ;
    : t5 m" U. q0 f3 n4 h( L
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    - K5 m: T1 p& [. ]
44.       }
    & T% G2 q" x; B, @7 C9 m
45.     } else{
    5 O) Z3 |; u$ V. D* [8 H
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    . z* A: z5 R( v) I4 n
48.       }
    
49.     }
    
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    
52.   }else{
    
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    
55.   }
    2 c8 i& y% u% @6 I# E- `
56.   do{}while(millis() - StartTime < interval);
    
57. }
    
58. 
    
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    1 y: z# }! Q- ]5 N/ K' t- I
61.      Power = MaxSpeed;
    
62.   }
    
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    . E$ \- P' J3 G. j/ |' `, q
65.   }
    5 t7 u4 h4 x2 \+ ?7 O
66.   motor_9.run(Power);
    5 W/ h$ f" m* }+ q
67. }  
    
68. 
    8 x# f2 ], ]( ]& c3 M/ d2 K: C
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    ) t8 C5 P1 U  h4 z8 v
71.      Power = MaxSpeed;
    
72.   }
    1 `2 V; a0 H2 w3 [$ \  T" V. ]  E
73.   if (Power < -MaxSpeed){
    4 R1 [: f" r4 L% x' K  e+ H
74.      Power = -MaxSpeed;
    3 D4 R; d, M% Y, i2 {+ i
75.   }
    
76.   motor_10.run(Power);
    
77. }  

複製代碼

: H* L1 w  ^' A* G您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況
/ B9 R  V3 B& |" p2 M& E
) t  d- a& Z9 S$ X但基於想追求更精簡的程式所以還是想請問一下
  w/ F8 H8 W. E, k( b! e就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理


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

1. int MotoSpeed = 250;
   & D! C! {9 Q7 m$ M
2. double CNY70Val = 1000;
   + X! @% {9 k, H
3. int interror = 0;
   * o# F: P( |/ F$ e1 a
4. int olderror = 0;
   ) o7 \0 h  b& P7 A
5. double values;
   % m- z& b; ?3 b+ [( _8 }. a1 I
6. 
   
7. 
   ! I* X) ~( e) Q. g" v
8. void CNY70()
   ; b) a1 O( P8 @1 K
9. {
   
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    + p$ B( s: s  N$ H1 V
15. 
    
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    $ E8 z" _& l% o# c
18.   if (valuesMR > CNY70Val)
    : b* }5 p; F! Z& e) l6 H+ h
19.     valuesMR = CNY70Val;
    ' g$ L. i" c' }: C( ]  ~
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    ! N9 g: v: y' A2 o8 h1 O! v
22.   if (valuesML > CNY70Val)
    3 x( S4 {9 a* K; ^
23.     valuesML = CNY70Val;
      G8 x; ~7 w: ]- P& q6 p" E8 G
24.   if (valuesLL > CNY70Val)
    6 d  P& P1 O: M8 x. L( S; x* q
25.     valuesLL = CNY70Val;
    
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    4 I4 a: E- m) q) `# u) `) |
28. }
    
29. 
    
30. void Car()
    * h5 r  e: S* c/ g$ l0 W
31. {
    * v) ^, G8 D+ V8 E. Q6 J, O) U
32.   while (1) {
    ) W1 N3 s- M$ v: T( Z
33.     CNY70();
    , f+ r. U/ l- B4 z/ D+ ~
34. 
    
35.     int error = ((int)values);
    
36.     interror += error;
    
37.     int lasterror = error - olderror;
    ( w3 m7 c- O/ m6 k9 l
38.     olderror = error;
    & S: G# }" q5 G' ~! n$ C
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    0 K% C/ _9 y( E$ b
41.     if (power > MotoSpeed)
    ' }3 x' b1 O9 M8 Z( {" D
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    
45. 
    
46.     if (power > 0)
    8 j/ J# u; ^; M$ Y0 q
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    6 C. o0 {/ H8 i& E5 H
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    
51. }

複製代碼

/ Z+ O$ E1 s8 P
+ O! P- y2 S+ a. x/ G3 W8 n) t

感謝分享

感謝分享