PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   
3. #include <Servo.h>
   # b2 b% c6 ?- I; g0 V
4. 
   7 V# R) o5 E$ O1 v
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   ( m* O' A+ Q" Q( C$ B
8. 
   
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
      P- Z1 s# j6 x6 q8 y  g
12. float MinPower = 120;
    ' J$ H  R; h( d
13. float Error,ErrorAcc,ErrorDec;
    * @4 e& c. M5 u" J2 `
14. 
    
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    * G1 g# l3 j! }
18. 
    % t+ u; q, G0 q3 B/ k
19. float nPower;
    / n, A/ k& a* k4 M* ~
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    " K, N$ _6 B# R0 H
22. MeDCMotor motor_9(9);
    - _0 C( W2 S8 R/ z) u$ L
23. MeDCMotor motor_10(10);
    3 ?7 ]( k# g+ Z1 f( c
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    ; H, X1 `: y, H7 h5 W5 i: R
26. 
    
27. void setup(){
    & \$ n$ |5 p% w
28.     lightsensor_6.dWrite1(1);
    2 O4 n& X4 n/ Q! B3 {) K* n
29.     nPower = 160;
    ! T% V5 j2 w: @0 S# L& r
30.     Error=0;
    % Z' d$ G: N; h; c" r, V
31.     ErrorAcc=0;
    
32. }
    
33. 
    5 ^5 T3 E  u7 p; o- S& ?& r
34. void loop(){
    
35.   unsigned long StartTime = millis();
    , k& l1 o/ W. F+ P. t9 w
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    * n# s! U2 P  n2 F; Y
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    ( X# K1 {0 {8 C7 g# d
43.         nPower += 3;
    ; z9 F4 U+ ], B6 S0 b. W: ?- u7 \( F: b
44.       }
    
45.     } else{
    # }) [  Y2 b" i9 E
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    ; U" V5 o) l" m9 m
48.       }
    
49.     }
    
50.     MotoL(nPower-Error);
    ( A1 w+ ]# O: ^0 B8 b  ]
51.     MotoR(nPower+Error);   
    
52.   }else{
    
53.     motor_9.run(0);
    % ^# W9 y3 G+ c3 k4 Z
54.     motor_10.run(0);
    
55.   }
    3 F/ d3 g- m: w# N! [' P5 w
56.   do{}while(millis() - StartTime < interval);
    
57. }
    
58. 
    ! k: h* f4 C" ?6 `# B9 B8 V
59. void MotoL(int Power){
    $ h3 u, ]2 T$ l0 [# h, v( Y
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    % D" V* T0 ]/ {3 a
62.   }
    
63.   if (Power < -MaxSpeed){
    ; v6 Y. G, K3 d8 c1 U% N
64.      Power = -MaxSpeed;
    
65.   }
    " A  r. f# D6 s; f# @& R
66.   motor_9.run(Power);
    
67. }  
    $ x1 u$ m7 O8 m# [
68. 
    
69. void MotoR(int Power){
    % P8 M7 B4 N3 M- m7 k5 k: r
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    
72.   }
    
73.   if (Power < -MaxSpeed){
    , [( a3 ]2 v( x, y! O# X: k1 ^  G( d
74.      Power = -MaxSpeed;
    
75.   }
    2 M/ I5 R( d  T: z) t  M: }
76.   motor_10.run(Power);
    2 d0 |- u2 }) S: R# F) E
77. }  

複製代碼

9 z$ \  L6 T9 u2 `! x
  v/ `, v% q  W0 z  s" W; a$ T

  M: T. P% Z9 r$ }, W: E& V& }您好，不知是否能向您請教。
0 z6 w; U" Z+ I$ P3 e% @- U目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況

但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理


) V. J3 n; E$ F$ D# `+ P. r以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   , E* b; K6 H( C3 ?0 U8 e" K$ ]
2. double CNY70Val = 1000;
   
3. int interror = 0;
   
4. int olderror = 0;
   
5. double values;
   
6. 
   ) Y  i* @) r$ V$ Z( j) ]' @3 e/ p
7. 
   5 C' B/ b$ X* u. K
8. void CNY70()
   
9. {
   : h0 i+ _' x3 y9 V& o; X& J+ e
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    5 m5 `3 P7 y; C5 V2 ]% ?: |
13.   valuesML = analogRead(ML);
    7 j  a8 ^& d; {4 ?, |' X" S
14.   valuesLL = analogRead(LL);
    2 j6 r5 H- K2 X; s, O( O7 J
15. 
    
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    4 I& A. R( F* h5 }  w+ z+ @
21.     valuesMM = CNY70Val;
    / p* c1 r% ?0 Q( q. {$ C
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    2 \# P4 D1 K1 q% ^/ D
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    ( S5 `4 H4 n3 i5 T( }3 K  i1 W
28. }
    
29. 
      O8 A) j9 E8 E. t8 d/ T' @! Y7 Q
30. void Car()
    8 X# \4 U1 [/ X, f2 T. o. A) Q
31. {
    ; R' @/ ]3 |5 O
32.   while (1) {
    6 [9 M1 M4 u& i/ J3 I& C
33.     CNY70();
    
34. 
    
35.     int error = ((int)values);
    - Y. T( g1 w0 O4 Y* s0 {7 N* M' w
36.     interror += error;
    
37.     int lasterror = error - olderror;
    & L. w  Q/ f! i" Q
38.     olderror = error;
    7 A4 H  E1 e/ v7 r- m( w7 D
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    * V. }5 {8 b! _' Z  U
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    , A6 H, m  _6 u& {9 j/ l
45. 
    1 a8 D1 C- Z4 }. Y5 ^* t
46.     if (power > 0)
    * K5 f3 U& B6 v% W. u& }
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    # N9 L& b. o. H( m$ W/ K) E
48.     else
    3 ]. B9 V/ @: B. n
49.       Speed(MotoSpeed + power, MotoSpeed);
    0 d% m% v# ~6 w' W2 |$ `
50.   }
    
51. }

複製代碼

' d& s' M! d, V- T  H" a/ X' B8 d

感謝分享

感謝分享