PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   : o( s9 [! A% p' I2 X" u/ w
3. #include <Servo.h>
   
4. 
   * }( e; `; I, L. y2 O: B1 n; `# @
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   . P% K8 [# c/ T8 Z: s% U9 {& `
7. #include "MeDCMotor.h"
     ]1 K6 U; K6 j+ n6 S- A
8. 
   
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    
12. float MinPower = 120;
    , b8 T5 _& b# G% C7 y
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    5 o0 _; O  v# Y+ m
15. float Kp=0.14;
    & {3 k8 T: H1 G9 h0 X  |
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);
    
24. unsigned long previousMillis = 0;
    0 R: k" f* m5 a5 e
25. const long interval = 1;
      r& W9 H% M' ^* P# C
26. 
    
27. void setup(){
    7 ~) X# `( L# s0 t& Y& K& P" a% g* C
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    
30.     Error=0;
    
31.     ErrorAcc=0;
    2 D1 ~3 \$ r) Z5 @& m( |
32. }
    
33. 
    
34. void loop(){
    ' @, k7 V( U1 \* M6 l4 d! w
35.   unsigned long StartTime = millis();
    
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    8 n- `* N# ?2 Z- e
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    , y/ X2 a! I! G6 k7 `
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    4 e, s8 F2 p$ c- X. F4 W
42.       if(nPower < MaxPower){
    + g- H/ V. h/ w, [4 `8 t
43.         nPower += 3;
      ?/ A9 ]. G0 y. D
44.       }
    
45.     } else{
    
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    
48.       }
    5 r, M! L7 s2 t0 f& ^5 e, i
49.     }
    
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    
52.   }else{
    
53.     motor_9.run(0);
    4 J; D5 F) W$ m; B7 E, ^/ V% z
54.     motor_10.run(0);
    + J. P3 L0 z, j# T
55.   }
    ( [7 {2 X/ }/ e/ \
56.   do{}while(millis() - StartTime < interval);
    " @  |, I( i' d* ~3 B* ^3 X; M4 h
57. }
    
58. 
    # t8 D6 e! R; R
59. void MotoL(int Power){
    " ^  I8 T: D+ i
60.   if (Power > MaxSpeed){
    1 |: y$ _! {/ U4 V4 j, O5 }, L
61.      Power = MaxSpeed;
    9 ]2 d3 X! M: O0 b3 z
62.   }
    ! ~( @# \8 M2 i3 b3 S. y
63.   if (Power < -MaxSpeed){
    % v; L8 E# S9 V
64.      Power = -MaxSpeed;
    
65.   }
    
66.   motor_9.run(Power);
    2 o" U% Z, O9 s0 A: D
67. }  
    
68. 
    
69. void MotoR(int Power){
    . L  N% w& Q" L1 Z1 O9 Q( @
70.   if (Power > MaxSpeed){
    5 N, d* _& c: r; w: @0 z  [
71.      Power = MaxSpeed;
    ) b! }0 O7 {/ ]  C
72.   }
    
73.   if (Power < -MaxSpeed){
    + c# x# m/ m) z
74.      Power = -MaxSpeed;
    5 I% b, Z7 ^: `8 r0 z2 z4 {
75.   }
    
76.   motor_10.run(Power);
    
77. }  

複製代碼

: t- r8 Q+ F! y0 w
/ s% Y( S; t4 z* U# ~1 M

7 w- j% B/ k+ d7 I2 u) U( E
您好，不知是否能向您請教。
  f+ }; t5 y1 `+ v目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況
* n4 b  r' _, Z3 r: q0 N
但基於想追求更精簡的程式所以還是想請問一下
) _( ^% N; P$ a  C3 ~就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
& N* l9 o; G9 }! N) w1 E, Z不知道您是否願意教我可以如何處理
+ K0 @! w6 }  Y; e3 D. @
- F  i+ ^! A& ]3 c5 i) O; y
0 N" V1 n0 f) }! F4 _4 M9 u5 E以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   : @$ z. P! a: Y) f, _# D" J5 e9 P( \
2. double CNY70Val = 1000;
   0 d& P4 X* D, v+ K0 `' d
3. int interror = 0;
   
4. int olderror = 0;
   + a+ m* e1 c- e: x6 S1 i
5. double values;
   ' Z3 E! t4 s5 K9 i* y* I
6. 
   + B7 q: c" P4 v7 L6 K0 d
7. 
   
8. void CNY70()
   
9. {
   ! o6 M# t9 ]9 _2 m- Z" L
10.   valuesRR = analogRead(RR)
    / p1 J/ U; m$ D- _
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
      {' Y- K- w5 i6 N+ ?
14.   valuesLL = analogRead(LL);
    
15. 
    ; d( M: G! z7 v: t& U& u! [
16.   if (valuesRR > CNY70Val)
    " w0 }; L% Q+ f, {) Z* p, T- C4 S: G
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    * R1 V+ m$ _/ ?' U0 X$ i
19.     valuesMR = CNY70Val;
    3 S5 Q8 a7 E* @4 A2 y' I
20.   if (valuesMM > CNY70Val)
    * N# Y! Y' g0 K, X
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    ! J1 i/ F( t5 I2 ^& X$ e( ?
23.     valuesML = CNY70Val;
    2 D3 ?# {4 ]2 D5 m
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    % {7 [" |# k, w. G7 x
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
      _+ v% s: z" ^2 ~" D6 D: A
28. }
    1 H) o  \2 A: R8 ]) H# H; W
29. 
    - d; h# z4 ?; H
30. void Car()
    
31. {
    6 R! s* o# Z" R( I& X$ i
32.   while (1) {
    
33.     CNY70();
      }8 i  Z% ?8 B' ]9 M' ?' E7 g
34. 
    
35.     int error = ((int)values);
    
36.     interror += error;
    
37.     int lasterror = error - olderror;
    1 I2 p5 Z+ Y0 O$ a! e% x9 `" m; ?
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    + }7 ]' E. @' l
40. 
    % T# D, B# Y3 e( L
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    
45. 
    8 _$ s) M* a3 E. s: n" u+ A
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    / x1 q% M! V; }1 e- M
49.       Speed(MotoSpeed + power, MotoSpeed);
    : J( ]/ U7 ~  D. @
50.   }
    
51. }

複製代碼

" D  {  Y7 i6 B/ B8 p! W6 p
& ]& m1 u2 ]2 W2 a

感謝分享

感謝分享