PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   
3. #include <Servo.h>
   + e. ~# t/ u& z2 \
4. 
   
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   + T: u: K- u4 S  e& H& m# B: [
7. #include "MeDCMotor.h"
   ; D- q) z( j" e# C! w6 I
8. 
   8 S2 T1 o# \4 \# T$ o" ?
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    ) R/ y) v+ g' g% B- u# t& J
11. float MaxPower = 180;
    
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    , ^+ L. O7 ]; L, c* m9 c" y- O" ^
14. 
    4 [$ D& `3 C& c7 t
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    2 p  ?5 n$ w  w$ o% N! p+ k! }) I
17. float Ki=0.000201;
    % X/ q) B1 T& W9 @4 k  k0 b
18. 
    
19. float nPower;
    , |& \0 p" N2 g8 R: l
20. MePort lightsensor_6(6);
    4 ?% h( B7 t4 r
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    ; @5 i: G$ m% E
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    $ X1 \4 t9 L, ?) r  P0 G9 m9 t
26. 
    ) T7 v  Z7 c6 F& S0 _8 J
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    * G0 H/ f9 y1 R
29.     nPower = 160;
    . V, g" e1 E* S% O5 z+ C
30.     Error=0;
    
31.     ErrorAcc=0;
    
32. }
    
33. 
    
34. void loop(){
    
35.   unsigned long StartTime = millis();
    
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    - S- ]* D4 N7 v2 f
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    / J8 q4 X4 t* @$ I; `+ ^+ m
41.     if(nError < 80 && nError > -80){
    4 g' E, j. h4 U) l% H8 k% f) i
42.       if(nPower < MaxPower){
    2 _; e+ A+ b% O& _2 I2 N
43.         nPower += 3;
      G# n6 k8 I7 e& ^. h* q7 [; d  a
44.       }
    
45.     } else{
    : k& ], x: u4 q1 E5 M  D
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    
48.       }
    
49.     }
    
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    
52.   }else{
    
53.     motor_9.run(0);
    ! ^( N* \) w5 C
54.     motor_10.run(0);
    : m8 s5 g# ?* O" h3 l
55.   }
    6 ], q) `; @7 P& g7 f7 R. @
56.   do{}while(millis() - StartTime < interval);
    9 b- j9 ?* s+ J
57. }
    1 b! |2 M( I# G
58. 
      [( Y0 e2 Z! O( t! \/ c4 j
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    
62.   }
    + P/ D8 ~' z0 s/ C
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    ! w# d0 J/ w( {0 v
65.   }
    
66.   motor_9.run(Power);
    - J" f% M0 K  s
67. }  
    7 o8 K9 X* A5 |% O. }% _
68. 
    4 q" l3 H9 L% f! ^' d
69. void MotoR(int Power){
    ! l0 w) v) d; h$ L3 ~/ ?! L8 R
70.   if (Power > MaxSpeed){
    8 I& i* A5 H$ t& M7 |4 j5 z# N
71.      Power = MaxSpeed;
    3 k" U& A  b8 L2 c
72.   }
    
73.   if (Power < -MaxSpeed){
    0 o/ M- _# w- G2 T+ v* U
74.      Power = -MaxSpeed;
    * }! a8 G, E; X, M3 Q. A! U
75.   }
    
76.   motor_10.run(Power);
    - q" {- d. v0 D' J' J6 ^) F
77. }  

複製代碼

* j& e% E% o. W6 N. Q

您好，不知是否能向您請教。
8 U) e+ V2 a0 c) ]  l$ Z目前和宋修賢老師在處理Ardui Car
9 O1 X6 b3 Z% h" k5 `+ e; U3 C雖然已使用較繁雜的方式處理了跑出黑線外的狀況

5 k: v& N2 v# {' H1 n" }9 `% U但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
2 x( z4 _# D: z% A% [. R" f不知道您是否願意教我可以如何處理


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

1. int MotoSpeed = 250;
   4 s! d  _% t( E4 F% x
2. double CNY70Val = 1000;
   . j) V% X# Z1 R% Y% C
3. int interror = 0;
   
4. int olderror = 0;
   : }) r* e) W& c
5. double values;
   4 m3 z1 I3 S+ |6 R) {5 r
6. 
   . s  K& s) z5 i3 S+ G
7. 
   
8. void CNY70()
   
9. {
   & h3 n% K1 X* m2 [! J) x
10.   valuesRR = analogRead(RR)
    3 c& p2 S4 _7 t" S2 z+ ~
11.   valuesMR = analogRead(MR);
    2 p2 R1 F" y( H
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    9 B3 D8 l4 H+ U0 {8 O- w8 o
14.   valuesLL = analogRead(LL);
    + y0 m: o& P# [$ n# \2 B2 b3 a
15. 
    * A9 K1 P8 ~. W
16.   if (valuesRR > CNY70Val)
    , O2 \" [; u- c# s  k
17.     valuesRR = CNY70Val;
    & ^" a. k9 X# h5 L
18.   if (valuesMR > CNY70Val)
    ' t9 a$ u) f  J3 k, r9 M
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    8 e& F# ~! i1 B& `
21.     valuesMM = CNY70Val;
    # A7 M, Q( n  V3 V5 J4 p
22.   if (valuesML > CNY70Val)
    9 R! U) x9 x- G( ]( f. I
23.     valuesML = CNY70Val;
    9 N7 C% Y# f4 T
24.   if (valuesLL > CNY70Val)
      D, n2 f/ }* e: m) z6 L0 e+ g
25.     valuesLL = CNY70Val;
    $ ]4 K  a' R  S4 S- T
26. 
    ' p) b- L+ l7 _& v$ k
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    " e0 A6 o( c+ `$ b4 t/ d  [
29. 
    
30. void Car()
    
31. {
    
32.   while (1) {
    
33.     CNY70();
    
34. 
    ; Q/ A) s1 U  q1 m
35.     int error = ((int)values);
    0 u1 R# j9 b: }. u2 Z. c
36.     interror += error;
    
37.     int lasterror = error - olderror;
    / U4 S2 p# g7 O" M' u# {
38.     olderror = error;
    $ f& m: i! w! p  a) g' K/ r
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
      v; N6 l% F' G, M8 @
43.     if (power < -MotoSpeed)
    5 g/ v/ S5 Y$ }  O3 d0 H
44.       power = -MotoSpeed;
    * T. N3 b" R$ K4 ?' _
45. 
    . R' c; g9 M# t. w1 j
46.     if (power > 0)
    # i) L; w# g! B$ x& y; W
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    ) A# i: Z9 [* n8 X
50.   }
      R) W  g0 I) r, Y, C- j
51. }

複製代碼

感謝分享

感謝分享