PID 循跡自走車範例

1. #include <Arduino.h>
   , G( Y: u. M$ X
2. #include <Wire.h>
   + |) T9 P8 V; J# C3 r; J* B
3. #include <Servo.h>
   4 e, p4 m$ i1 ]' F" z) }7 g+ H) \
4. 
   
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   ) R+ z- {, o& f2 a7 |
7. #include "MeDCMotor.h"
   7 r" h8 S4 D# a9 Y6 s
8. 
   - P) U6 R* \) I( w. @! s) o5 T
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    9 C/ B+ o' c  M+ @3 V
11. float MaxPower = 180;
    
12. float MinPower = 120;
    2 g/ Y! d' d5 E5 E5 }8 f# l+ i  f! J
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    . n) J& S8 Y& k# r
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    ( {# m! t- t5 w8 h% K
18. 
    
19. float nPower;
    
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    # p2 c& D$ C& n$ D- O6 s) V( g
22. MeDCMotor motor_9(9);
    3 P  p6 z! ?% V6 E& ^% I
23. MeDCMotor motor_10(10);
    / x! S6 j3 t1 [" E( |$ Y
24. unsigned long previousMillis = 0;
    ) |6 b: ]( m4 _. G2 [- M
25. const long interval = 1;
    
26. 
    
27. void setup(){
    0 f$ k; n% e$ k/ `  K' K) P
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    7 |/ e" N- k# l6 s9 p
30.     Error=0;
    ) [2 s' f/ \$ e* m2 s
31.     ErrorAcc=0;
    
32. }
    
33. 
    
34. void loop(){
    : \5 T$ A8 o5 m4 ~5 \/ ^5 G& s
35.   unsigned long StartTime = millis();
    
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    9 k$ Z* P  [' g: V1 z  s
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    $ q3 ?3 K1 u" m+ |' Y
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    7 `6 w) F5 |8 g- r! F! P
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    : ~" y$ V4 W! ]: a0 M2 Q. O+ k. Y
43.         nPower += 3;
    
44.       }
    * Q$ D7 d' h" u7 S3 {5 [7 J
45.     } else{
    ; V7 M1 M+ J9 c' X3 u* a
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    ; L8 ]6 b) Q/ q, V$ B% S% E
48.       }
    : D/ D+ d0 \" e. W% X
49.     }
    
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    
52.   }else{
    
53.     motor_9.run(0);
    6 F3 |: R& i# }! g8 G
54.     motor_10.run(0);
    
55.   }
    ; `/ _7 E8 v6 C# j' m$ o7 J+ k
56.   do{}while(millis() - StartTime < interval);
    7 ?) Q0 F8 C/ j: j# j9 ?) ?. R
57. }
    ' c) p% j, \. F& d4 s8 t9 N
58. 
    4 X8 q/ j' m2 \3 e7 p! x
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    4 b4 p( q8 X' }, p& d
62.   }
    
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    + W- Z4 f: R$ e9 h: e, F
65.   }
    " E0 {3 d& P0 |% x
66.   motor_9.run(Power);
    
67. }  
    $ A: \% ~9 G; L1 `6 f0 u% m& u
68. 
    9 p+ Z4 }9 h4 V
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    * a- z7 E5 `& `! `3 L5 ~' e
71.      Power = MaxSpeed;
    
72.   }
    : u4 @  r3 G, A4 R; P: B
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    ' n; ^2 v. O/ ^
75.   }
    
76.   motor_10.run(Power);
    2 v8 ^9 ~$ M2 \2 a. W, d3 J
77. }  

複製代碼

* _# p  U* _% U" R
8 k1 U7 ]+ _, ?- |4 d1 W您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況

但基於想追求更精簡的程式所以還是想請問一下
2 k* [% Z" p8 Z. \& |1 I就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
' _* }6 K/ o2 h2 M0 k& m不知道您是否願意教我可以如何處理


% ?  w2 E; C: y, h# h1 h6 h以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
     ^0 z* l- n+ G' ^
2. double CNY70Val = 1000;
   7 a# J: X0 E6 @  ]- u6 q5 e
3. int interror = 0;
   
4. int olderror = 0;
     h$ s' S$ P9 ^
5. double values;
   
6. 
   
7. 
   " V/ H% m) D- T$ p5 Y
8. void CNY70()
   2 @' }5 W5 P0 t5 G. @9 H# A
9. {
   . c& q6 J. |" ?  X  j! |
10.   valuesRR = analogRead(RR)
    ' `, }- D: c, L4 N! K& I0 ~
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    1 u, o8 j+ I$ }, a% a! y7 E/ i: x
13.   valuesML = analogRead(ML);
    ; L, B, W# n: I3 b9 @
14.   valuesLL = analogRead(LL);
    
15. 
    
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    + b. J( W6 G$ X
19.     valuesMR = CNY70Val;
    2 v/ W  q2 |* A/ a  x
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    * V4 A1 x! Y  O9 \, W: Y
22.   if (valuesML > CNY70Val)
    0 F! g) v/ C& D! W
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    & X- W' a9 _4 k5 A$ D7 m; k* B
25.     valuesLL = CNY70Val;
    ! r8 E. {9 R/ [, @
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    + V. A# Z: R! ]! T0 e
28. }
    
29. 
    
30. void Car()
    
31. {
    $ u; L; E' C# ^; a- h
32.   while (1) {
    
33.     CNY70();
    / a. e2 B, _% p3 \
34. 
    
35.     int error = ((int)values);
    
36.     interror += error;
    
37.     int lasterror = error - olderror;
    & U( D: |! O9 Y, W
38.     olderror = error;
    & M* v& Z% t* R5 W
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    7 W2 O- m6 }9 Q- {; l1 J5 O8 L
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    $ j: x# z3 E( V! S
44.       power = -MotoSpeed;
    
45. 
    8 n( ~6 e7 z$ p! Z5 _; U9 N
46.     if (power > 0)
    " a: K  C$ F- v
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    & Q9 u% P" I9 A2 a
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    5 j4 Y/ M  G" R. Y& B$ ~; Q9 v4 V
51. }

複製代碼

" f. w+ k4 M+ C$ T

感謝分享

感謝分享