PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   + [0 D4 \$ _, X
3. #include <Servo.h>
   + Y: R2 k4 l, T' ]* Z) V1 A
4. 
   ' ?; Z" v# k* g2 ~1 M
5. #include "MePort.h"
   / T1 g/ {& |: a( p. W4 ~6 z
6. #include "MeUltrasonic.h"
   4 K, w4 M/ C& e/ {
7. #include "MeDCMotor.h"
   
8. 
   : V' ~4 F+ d! [  u4 i/ J
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    , ?! ~# c( Q. ~8 q  |0 e9 L# }
14. 
    
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    5 M0 f, s" U! w* {
17. float Ki=0.000201;
    
18. 
    8 N& h8 V9 R5 C
19. float nPower;
    
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    # e$ M$ i2 z" ^8 I. g/ D8 X
25. const long interval = 1;
    
26. 
    0 t. l/ E; l+ O# W* t/ ~
27. void setup(){
    ) ?; z6 E1 H+ `0 I2 `, ]
28.     lightsensor_6.dWrite1(1);
    ; r6 u/ e2 V6 X# y
29.     nPower = 160;
    4 v. L3 ~4 S# W. v
30.     Error=0;
    3 H& m: i" p* h0 i
31.     ErrorAcc=0;
    
32. }
    
33. 
    
34. void loop(){
    3 t* g8 b8 @- j2 [- e) n6 A
35.   unsigned long StartTime = millis();
    ' Q5 i1 @" b4 e$ U! c6 G5 F' J& c
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    2 g- _6 I! q# f/ M/ I/ }2 {: l
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    # F' @6 P0 Y! N2 x
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    ' a; Y$ L! I; h7 t# v) S* \5 g
43.         nPower += 3;
    , o% g" N8 `1 b8 G; c
44.       }
    
45.     } else{
    
46.       if(nPower > MinPower){
    ) K) e7 w; @" f* \+ A8 f4 S
47.         nPower -= 2;
    & l- L3 j' M) c! A& I6 l: F7 O
48.       }
    & d5 k/ ]$ ^' S  l$ V. m
49.     }
    7 j3 J$ m- N7 m$ w$ C2 ^0 P6 I
50.     MotoL(nPower-Error);
    - Z; q6 o5 `. p/ e- H( q
51.     MotoR(nPower+Error);   
    
52.   }else{
    ; m; X" X; ~! H& c# o
53.     motor_9.run(0);
    ! C4 ]* D, o! W- r% ?$ v% G/ ^0 m1 q+ L
54.     motor_10.run(0);
    
55.   }
    6 M! B0 E, G) B+ L/ s: V
56.   do{}while(millis() - StartTime < interval);
    . G' t) w% T  d- q
57. }
    : Z+ w1 y$ M# c8 `% X
58. 
    ! t! Z) Z, G' W- E
59. void MotoL(int Power){
      D  @) @+ z2 Q
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    
62.   }
    
63.   if (Power < -MaxSpeed){
    3 G3 Q: ^' t2 u# [0 R. [! T
64.      Power = -MaxSpeed;
    
65.   }
    
66.   motor_9.run(Power);
    # A  X3 n3 e4 t6 d6 L. u- p/ v
67. }  
    ( E2 l9 S' k1 `$ f
68. 
    6 b2 R$ I" [! v4 Z. e  P% ]$ X
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    7 i8 Q- V" @( g2 U8 N
71.      Power = MaxSpeed;
    
72.   }
    + t* o9 }4 U' l$ d& A) r2 x, f% y
73.   if (Power < -MaxSpeed){
    7 R2 `" y5 E9 ]+ E
74.      Power = -MaxSpeed;
    " H- [2 k- g! P; g- @6 ^
75.   }
    ; T- a0 A8 d+ i( f% f
76.   motor_10.run(Power);
    7 S2 M2 ^  A& {% f
77. }  

複製代碼

5 I' n* `' _7 m7 s( z6 e# S
" k' z  K  Z& Z% J& u2 Q- X

1 D2 F. o( l0 E' Z$ J
您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況
+ g! G) O; ^: J7 z3 J! T
. W# g6 @0 M7 f但基於想追求更精簡的程式所以還是想請問一下
+ Y3 h# }3 z* p: V6 Y就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理

* Z  _+ m: W& B$ O0 h9 n. v
以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   - k8 @# \2 H" u# A8 M! `; _
3. int interror = 0;
   
4. int olderror = 0;
   
5. double values;
   5 \/ u( [: T3 p; h& c: Y
6. 
   ) q# x% {% }. O/ k! q
7. 
   * f/ P" z- q8 H2 e
8. void CNY70()
   
9. {
   
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    - J+ B7 K" k9 q+ y/ s6 ]
14.   valuesLL = analogRead(LL);
    
15. 
    0 P8 _& w6 X. O% f
16.   if (valuesRR > CNY70Val)
    4 G  z5 c' e- o3 x8 K  A/ m0 S( E
17.     valuesRR = CNY70Val;
    8 c  l' @6 h+ E2 t; C! S) Q9 M
18.   if (valuesMR > CNY70Val)
    . e8 P7 B3 i+ Y
19.     valuesMR = CNY70Val;
    " ?! `7 I' e; F
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    3 a& l9 D/ [) g
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    , E- c$ x" b! U5 R  i
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    
26. 
    ; d% q- C3 E+ C4 {" |4 a: P2 X0 q
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    
29. 
    ; s. R) l. k4 b" u- R
30. void Car()
    ) ~# m/ b2 _6 x, T  I
31. {
    
32.   while (1) {
    
33.     CNY70();
    
34. 
    : j- x$ w6 Q+ P9 R8 r
35.     int error = ((int)values);
    * R- u. I% O, r: y8 |# E2 B6 u
36.     interror += error;
    
37.     int lasterror = error - olderror;
    " Q! v2 J7 V- `) D$ x
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    ) w/ o$ Y3 H0 c9 O) O( N
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    , y: N& \& }3 [" y" O
44.       power = -MotoSpeed;
    
45. 
    
46.     if (power > 0)
    5 g4 {& H# Q4 z( o$ P) ~, q8 P. u
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    ) y; S, n% T0 `
50.   }
    
51. }

複製代碼

& F% w& L3 R& X0 k) \- U

感謝分享

感謝分享