PID 循跡自走車範例

1. #include <Arduino.h>
     R4 j! G5 Q: @% L6 k( O
2. #include <Wire.h>
   
3. #include <Servo.h>
   
4. 
   
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   7 [' _" _' v5 B' r
8. 
   # I+ w0 K; h5 w7 d" d
9. //double Input, Output ;
   + c; N9 x7 i7 s' M
10. float MaxSpeed = 255;
    8 b7 l% ]5 K; G. n, @* s* B
11. float MaxPower = 180;
    + x: C9 m- r& y
12. float MinPower = 120;
    , c; g6 a1 p8 G- @: v5 u! X1 c
13. float Error,ErrorAcc,ErrorDec;
    4 y# T0 }1 p- D$ x1 b& e
14. 
    
15. float Kp=0.14;
    2 e) A( u0 w# d. X3 |  Q9 k
16. float Kd=0.00020;//23;
    : p9 h5 f4 `  ]- ?1 w
17. float Ki=0.000201;
    ! X# T4 O3 o' V" F$ U
18. 
    / u9 \7 {( S6 |& l% j% f
19. float nPower;
    . I4 ]+ E4 T5 {1 r* X4 ?
20. MePort lightsensor_6(6);
    # H3 q: F& D- ^  e2 ~
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    ( d3 `8 q0 q5 x! R( R" v! p3 p
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
      [; r# n- _1 Y4 i# Z( {& v
26. 
    
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    : G+ V9 O. b3 J6 r6 K
30.     Error=0;
    
31.     ErrorAcc=0;
    
32. }
    
33. 
    # p. K$ r( O) M0 B" F
34. void loop(){
    5 k% b* i1 {& U; `. R
35.   unsigned long StartTime = millis();
    
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    - j4 y/ n0 S1 v7 N2 p2 f
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    ! ?  I2 ^# A+ Z, `' c; @1 h) g' I* l# E# J! b
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    4 j1 c9 v! c% {
42.       if(nPower < MaxPower){
    , ?/ [7 \( p/ x/ U! @5 ~7 s
43.         nPower += 3;
      ], q! }7 E. g" j5 c
44.       }
    6 p) w# E; C" z/ q. j( z
45.     } else{
    
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    8 U! U4 m( K3 w4 N. R" d
48.       }
    
49.     }
    
50.     MotoL(nPower-Error);
      N- ~+ f) c% B/ u8 v; U2 x' h
51.     MotoR(nPower+Error);   
    
52.   }else{
    
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    ! b$ u6 F  S1 E: T( R
55.   }
    8 R* z8 |# M! V3 Q, {
56.   do{}while(millis() - StartTime < interval);
    & L1 |- E4 Y- D& f4 a8 R% `4 C
57. }
    & w, Z' a" k5 N( i0 W& E. P
58. 
    
59. void MotoL(int Power){
    / M+ `+ K; c3 z0 w5 W0 y, ?8 `
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
      T8 a7 U$ f1 F5 |! O
62.   }
    
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    , `; y5 w6 j! O
65.   }
    . r& [! w# y, l2 k3 a
66.   motor_9.run(Power);
    / ?/ p2 c+ C! W3 ]
67. }  
    
68. 
    
69. void MotoR(int Power){
    % k4 n/ Q% P$ N5 g! w, s  n: O
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    
72.   }
    ' ?6 C# x  T; c" V% i' N% @( z
73.   if (Power < -MaxSpeed){
    % G* O! t' |0 T9 D2 Z( M
74.      Power = -MaxSpeed;
    
75.   }
    " U- N6 [# V. h& W. ^+ [2 V
76.   motor_10.run(Power);
    8 X, ~% K( H& z0 X
77. }  

複製代碼

  h( `6 G/ L) b+ V. b+ u
( o0 _- @0 `( j

% V9 n8 U7 ^( y% ]您好，不知是否能向您請教。
  v/ L$ E0 s7 u* L8 v. Y3 G目前和宋修賢老師在處理Ardui Car
3 a4 q2 R1 w8 H雖然已使用較繁雜的方式處理了跑出黑線外的狀況

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

; v7 ?5 ^4 U# V以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   
3. int interror = 0;
   
4. int olderror = 0;
   ! Y% o1 t, Q2 e' `' D- d
5. double values;
   8 k1 n  l7 r( }  u, G. S4 y
6. 
   * S# [* i4 G  J, c8 I* N
7. 
   * @* j, c( K3 \/ h. l
8. void CNY70()
   8 M2 y. V8 c8 K2 t' W  N+ e6 f
9. {
   
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    : u3 n9 B6 T( M; g" R5 |- r3 h% ]4 T
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    
15. 
    ' Y5 Z# Y, E6 @0 M: ^, B
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    8 c2 k) e; q2 [- v! Z% Z+ X9 ]
18.   if (valuesMR > CNY70Val)
    6 y0 w) g+ y- ]9 V# _* n. A5 B
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    6 ?3 b. J4 t) Z
22.   if (valuesML > CNY70Val)
    8 s: L/ I3 ~0 ^
23.     valuesML = CNY70Val;
    6 E# P% Q0 B5 w, @" m- b; _
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    
26. 
    3 W; h" A# ^* S$ ?: R$ G
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    * R' X# W: w, V. [1 H
28. }
    $ P* L) D& L0 d) s7 w* }
29. 
    
30. void Car()
    # {4 V, j* @% j) K* ], [' G
31. {
    
32.   while (1) {
    3 \' A: {; a+ W) N
33.     CNY70();
    & N$ K0 Z1 F% y
34. 
    
35.     int error = ((int)values);
    8 K0 K4 K8 H7 f2 S6 P* M
36.     interror += error;
    
37.     int lasterror = error - olderror;
    $ J- b8 X& X0 q1 ~/ e
38.     olderror = error;
    ) |6 C. {/ ?6 D( p; ]7 {+ P
39.     int power = error / 5 + interror / 10000 + lasterror;
    2 t% v+ ^5 s# u( P4 D
40. 
    
41.     if (power > MotoSpeed)
    2 M+ m2 }+ A6 X  g2 j$ |
42.       power = MotoSpeed;
    # E' X& K$ O9 W4 i8 I0 _
43.     if (power < -MotoSpeed)
    $ Y* d$ _7 ?% ]$ u, [2 y) I/ P7 w
44.       power = -MotoSpeed;
    , d) F; k. g5 i7 Q% K( k
45. 
    1 H: B8 U% j: W  X1 L! x
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    0 R/ G1 |  t5 O0 B1 c
50.   }
    . h, P0 D8 s% h% f  A. @
51. }

複製代碼

, f3 [4 L7 |( Y! N% o& S$ M+ w

感謝分享

感謝分享