查看: 23331|回覆: 3

PID 循跡自走車範例

 薦 [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>
    ; r! F$ A* z" m9 o8 ]
  2. #include <Wire.h>
    + }) p: o: h4 Y: }$ y1 ]) T. C2 T9 W
  3. #include <Servo.h>
    + A" A( }& B9 a
  4. ; B/ J4 F* [/ A5 G% O* A
  5. #include "MePort.h"1 s. D3 n) c- |# E' t! J
  6. #include "MeUltrasonic.h"
    # \4 A) i' B- |$ x6 i4 e
  7. #include "MeDCMotor.h": p2 O4 M* g2 y& f+ J" s
  8. + x# W- Z, p. n" W6 {% ?  c
  9. //double Input, Output ;
    9 U9 R% k+ k) i7 D1 A2 F( B* r
  10. float MaxSpeed = 255;
    8 G: J# w1 y8 P% a% u, O6 o( A+ {, b
  11. float MaxPower = 180;7 f& z: `2 c2 X- v2 |( r
  12. float MinPower = 120;+ W% X5 o9 v) N+ |1 w
  13. float Error,ErrorAcc,ErrorDec;# A! X* n0 U% F2 ^) P
  14. # A* E% h6 E0 b$ M
  15. float Kp=0.14;: J8 g( {$ E' e; ~, r7 f
  16. float Kd=0.00020;//23;
    ; [! [7 z0 W6 Q8 I* b4 S/ i
  17. float Ki=0.000201;
    ) I7 e; ~# z/ G: }( x8 `
  18. ' S, Y1 w( A6 x
  19. float nPower;
    % \/ G4 k9 F% Y( c5 t) l  `; X+ |# ~
  20. MePort lightsensor_6(6);! P+ Z5 a: p; n1 _3 h2 \/ \. J6 \; }
  21. MePort lightsensor_8(8);# N( e6 |* y2 D: v
  22. MeDCMotor motor_9(9);
    . J! t, Q2 C) m6 W. h3 |! r
  23. MeDCMotor motor_10(10);& H( |' Y0 T0 K4 X. F
  24. unsigned long previousMillis = 0;
      t; \1 e( f3 w
  25. const long interval = 1;, Z; X, e5 c. {3 @

  26. - b$ s1 Q: G' K  A+ r
  27. void setup(){
    0 O+ U8 M# r; L6 D3 _8 I
  28.     lightsensor_6.dWrite1(1);! e2 f8 ~: v: \5 ~
  29.     nPower = 160;
    0 {* P; p  F7 h6 e% r) {
  30.     Error=0;
    * }0 t# B" f& L9 I
  31.     ErrorAcc=0;
    , h% H9 k  V8 K7 t3 ]
  32. }. l) z( r8 d) z* r3 ]6 Y# a

  33. & R+ G' M2 E6 x6 H4 C8 U
  34. void loop(){
    5 A! ]5 [6 ^9 O6 F: a
  35.   unsigned long StartTime = millis();- ?% w: |3 q+ e5 f$ w4 ^2 e
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){. I! E( F  i- I+ z. N6 t, v, V6 \6 I
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();2 t: t) k& Z0 W2 C3 W$ s& `% `* N
  38.     ErrorAcc +=  nError*Kd ;
      i+ b7 U! |' d' F% q: d' v1 d
  39.     ErrorDec -=  nError*Ki ;( [# q, h) s5 [" x0 y/ Q& _
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);1 U& y. a( Z: |2 ~/ M' ~" @
  41.     if(nError < 80 && nError > -80){
    ' F. Q2 c# g1 O# H
  42.       if(nPower < MaxPower){
    2 J% k, o; F. b0 D( i
  43.         nPower += 3;7 G! T" r% |, [! ?- a, k- c! H
  44.       }7 S+ r) ?, F/ O9 S
  45.     } else{9 L# P3 y& Q5 k6 V
  46.       if(nPower > MinPower){
    - F& i% r6 b0 v! |2 R
  47.         nPower -= 2;
    1 E9 ~# q* P6 o: X
  48.       }
    6 d" r# ]5 j" i$ ?3 A
  49.     }
    & q. q; u+ \: E7 Q! Y: G2 P7 [" F
  50.     MotoL(nPower-Error);9 ^' N) p+ e" j
  51.     MotoR(nPower+Error);   
    8 k% A0 K; \# c/ ^% O* u
  52.   }else{* `# w; E! I8 m' K% E
  53.     motor_9.run(0);1 ~* M( k. s" h
  54.     motor_10.run(0);
    & A! k- K' W+ y2 t/ z3 T% e2 o% N/ U
  55.   }+ B7 |7 ~) _# y+ o
  56.   do{}while(millis() - StartTime < interval);
    3 g- Z1 m9 [9 v
  57. }
    . @) |# n- a7 b8 s. W
  58. $ P* K+ j! ~3 [1 e
  59. void MotoL(int Power){
    0 F0 p- q1 ]+ S, T" t% I" }% o( a
  60.   if (Power > MaxSpeed){
    4 k. y8 x7 P; t% z; r
  61.      Power = MaxSpeed;* S& K, O( v: H
  62.   } 9 w% [# {% f" T' @, V# n0 S& \
  63.   if (Power < -MaxSpeed){
    - B5 {. }' C3 N4 B+ e1 n" Z1 ?
  64.      Power = -MaxSpeed;; ^3 f8 P3 F7 e& b! M
  65.   }
    ) S- y% ]/ {) Y- j+ Q  ]
  66.   motor_9.run(Power);8 ~% W6 t9 u% Q, i
  67. }  , T5 p- Q( S$ @% `

  68. 9 K1 y) d! u! z. E! w
  69. void MotoR(int Power){
    9 p- d2 O1 ~5 t" x9 k* k
  70.   if (Power > MaxSpeed){
    - W  t6 f% s: W' S9 y9 M$ n
  71.      Power = MaxSpeed;
    0 B8 w# P+ w$ ~
  72.   } ' N6 j7 X" H: K7 }: m4 l' @
  73.   if (Power < -MaxSpeed){
    7 Q; G% S- p  [- `& L
  74.      Power = -MaxSpeed;7 v4 M! }- i1 w# r2 a2 X2 l
  75.   }
    $ [5 W; B. {) g* I# x# z" q8 Q
  76.   motor_10.run(Power);. L0 ?6 ^( l( P
  77. }  
複製代碼
6 N8 }. h+ e# s% l% S& M

+ [$ |4 x  S. Y8 i6 V
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯 * i! O3 ^! L2 D2 O1 j" C7 X

$ D5 Q/ D# j3 t5 j) B您好,不知是否能向您請教。
7 T7 z  V2 D) V9 H- W8 [( o目前和宋修賢老師在處理Ardui Car
" k# L4 b, w. m; X雖然已使用較繁雜的方式處理了跑出黑線外的狀況* C  D7 v* c. W
$ X/ i4 k" |0 d" s% @
但基於想追求更精簡的程式所以還是想請問一下
- P0 Y4 j! {3 L6 I+ \7 E. L- y就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝
' u% ?' K6 Q$ u$ D# M8 i# z不知道您是否願意教我可以如何處理
1 o) O! z0 A0 C+ V  f7 ], p$ f5 [) q! ^- @; |% J1 z" |# I$ w( {

: D8 _/ K2 w7 U, P8 W; o/ Z0 D) l以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;% E5 m7 Q1 z% N! @8 n0 P, `# k: v
  2. double CNY70Val = 1000;5 e1 v6 ]! w- [3 _, Z8 T& c
  3. int interror = 0;% p" ~. g7 b% c. J9 d
  4. int olderror = 0;& N! |! ]; }0 A9 p$ D3 E9 o( u
  5. double values;/ W& m/ H: o, F

  6. + R7 o5 \6 S9 B3 p3 x

  7. - k0 y- G( N8 z- D: K
  8. void CNY70()
    # V/ A9 p+ U: s' ]( K$ q& W
  9. {
    7 L& a. U, Z0 U. v/ A; {
  10.   valuesRR = analogRead(RR)
    ! y. B3 x# k: m
  11.   valuesMR = analogRead(MR);
    & [/ A1 y6 B& E2 w! ?) |7 r0 a
  12.   valuesMM = analogRead(MM);& Z, D4 z% P9 p3 i1 o( g$ T! V& b
  13.   valuesML = analogRead(ML);8 j! |$ K) _8 n
  14.   valuesLL = analogRead(LL);- B7 Y3 G# ^( t' a. N8 Q! b; ^6 e
  15. , }$ i1 Q$ {+ h0 t/ R
  16.   if (valuesRR > CNY70Val)7 H5 u7 R# _" a8 V1 s
  17.     valuesRR = CNY70Val;
    + F' w2 k; @, V4 P- h4 i7 P
  18.   if (valuesMR > CNY70Val)
    . {7 I& w: r$ |/ h1 I* p! V
  19.     valuesMR = CNY70Val;
    1 ^2 Z4 N, k* N) r' C' q1 `4 {
  20.   if (valuesMM > CNY70Val)
    7 H" [% \- {5 [+ [
  21.     valuesMM = CNY70Val;
    7 F0 p* I: \# t
  22.   if (valuesML > CNY70Val)
    ' d* a3 ^' G* ^6 J! A+ A- d
  23.     valuesML = CNY70Val;: ?/ `: `& x% O  F5 M: a7 d. Z; I
  24.   if (valuesLL > CNY70Val)* u1 E5 A3 l8 m" }% g4 }: L9 s
  25.     valuesLL = CNY70Val;8 y  Z0 _5 B8 n4 k! L( _7 U
  26. . o, M; n0 x1 h# @* w
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;. ?7 s8 w7 p, j! H# c, }
  28. }
    & O! N& l0 @& x  P

  29.   P6 d3 b. a. [! D( \
  30. void Car(): c( i& s$ ^: K
  31. {6 W5 F- {2 u& z( a  d  E! @3 T
  32.   while (1) {) [: B/ H9 n; B, b
  33.     CNY70();! ~( G8 c; j: ]3 t

  34. & f2 u: g" i+ K, V, x
  35.     int error = ((int)values);" K; G% o0 v3 {3 D
  36.     interror += error;$ F4 d3 t4 g# k; M( Q$ V1 u2 o7 B
  37.     int lasterror = error - olderror;+ _3 s, Q9 I. ?4 }) H
  38.     olderror = error;
    9 V0 r. F  @; a( y8 w: U/ w9 U4 ?' a
  39.     int power = error / 5 + interror / 10000 + lasterror;
      c+ Y  R% }0 U- q, C+ N

  40. * Y2 W, x! @6 c* i- ], B3 \" S( P
  41.     if (power > MotoSpeed)# R; l4 \: A8 j; l# s
  42.       power = MotoSpeed;
    ( R: g4 T) c: }2 T
  43.     if (power < -MotoSpeed)
    3 y# P9 l7 }( G- p' c
  44.       power = -MotoSpeed;! z, T% j. `% c* P7 U7 Z
  45. 0 ~5 d6 Q" R* s9 U% U: a
  46.     if (power > 0)& T) Z1 m% A' s+ E, E3 a' r% q
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。
    % T. J) o1 v5 B/ u9 u
  48.     else5 C, f) B3 a2 g* j& n4 g' _4 a
  49.       Speed(MotoSpeed + power, MotoSpeed);! m( O& l' n0 d$ j& ^3 G( |) m
  50.   }
    $ [- ^' F8 \$ m# R
  51. }
複製代碼
. o+ s' N9 b5 a

, h& b  B& L: ^
您需要登入後纔可以回帖 登入 | 立即註冊

本版積分規則

Powered by Discuz! X5.0 © 2001-2026 Discuz! Team.

在本版發帖返回頂部