設為首頁收藏本站

圓創力科技.MakeBlock TW

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

掃一掃,訪問微社區

查看: 7735|回復: 2

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>; |5 a4 `; @& J+ T& u0 U2 ]  A1 P* ~
  2. #include <Wire.h>
      S- ?' t7 v% r5 V
  3. #include <Servo.h>
    & J* o0 c' n9 o0 D* `

  4. ; X9 l; {' t/ i* _. E/ t' {+ v/ J
  5. #include "MePort.h"
    2 L- {/ r2 m- Y; U" e6 X: M5 v
  6. #include "MeUltrasonic.h"% S, i9 c4 D1 I2 w8 B6 Q. q, r
  7. #include "MeDCMotor.h". Z6 Q6 P7 ?/ Z1 J
  8. ' u7 R/ U' I# a: e, g
  9. //double Input, Output ;
    0 X+ F6 M  @/ v; a
  10. float MaxSpeed = 255;+ b/ l$ M4 ~  U9 B4 G; Z) g
  11. float MaxPower = 180;
    ( p$ c2 [, H3 D- t# r- D
  12. float MinPower = 120;3 r: |3 z# H+ j) Q- @
  13. float Error,ErrorAcc,ErrorDec;
    & m; M" T* }6 K# A

  14. 7 P% N0 n+ g' G
  15. float Kp=0.14;( x! K- E4 Y% T
  16. float Kd=0.00020;//23;+ ]% l0 {. T7 k& D
  17. float Ki=0.000201;7 O# m) P! Q" U+ r% \

  18. 3 `' o  k/ m  R& Q7 a! t1 H
  19. float nPower;! P, }5 @$ {' `" j; j
  20. MePort lightsensor_6(6);# l% r  v6 b, r; C* m* G8 `
  21. MePort lightsensor_8(8);
    ; b9 u& h: x* [! \
  22. MeDCMotor motor_9(9);2 F, Q9 U. A( S( s# j' k, m
  23. MeDCMotor motor_10(10);' R+ v) g0 H+ V( [3 E
  24. unsigned long previousMillis = 0;
    6 |, n0 ^1 G- u8 g8 z+ x, X
  25. const long interval = 1;
    6 ~, j! o; |+ V0 p+ j

  26. 2 ~5 @: q) ~% i7 L$ H) \1 f
  27. void setup(){
    1 U* _( P0 ?+ W, K3 G
  28.     lightsensor_6.dWrite1(1);
    6 h& N% T% F% i; }
  29.     nPower = 160;5 u6 F3 \/ f* s8 {. a$ u+ V0 P  E
  30.     Error=0;
    # g! h+ x# F" \8 V! z0 y& \
  31.     ErrorAcc=0;
    4 n' R3 ]8 I6 R3 ]6 {0 U
  32. }+ F" ]9 M! W( n, S( T8 a
  33. 2 q3 ?2 j# N3 p. [; T
  34. void loop(){
    $ T% x6 g' G, F
  35.   unsigned long StartTime = millis();
    4 [5 [" u( I. A9 a- R4 V
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    ! ~, k9 i- }& j3 P
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    ) I. {5 n$ F. h5 w: x0 {7 A
  38.     ErrorAcc +=  nError*Kd ;
    ; M' C! s" _  T: J) ~$ |" A
  39.     ErrorDec -=  nError*Ki ;
    6 ]7 A( s$ _, p8 J% y0 b; C
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    & a. M" b: L9 }( ]9 o0 A! M
  41.     if(nError < 80 && nError > -80){: Y; ]" f8 o9 {8 F
  42.       if(nPower < MaxPower){+ D! r& [6 j9 n6 T2 P. n
  43.         nPower += 3;; H; [+ L9 f& m7 y
  44.       }3 j- {  B% H* o8 b0 y0 }
  45.     } else{5 r' Z8 o( t/ n; s
  46.       if(nPower > MinPower){
    " y5 ?7 R8 ]- @1 ^( R. T/ z& j, w
  47.         nPower -= 2;
    1 L1 a. D/ @6 E* p* j& H: P
  48.       }) Y* H4 }; @* g/ e8 u1 ~+ l8 N7 E
  49.     } / {. L8 r3 s+ R% J  p* |, K6 U
  50.     MotoL(nPower-Error);
    / @# l1 u9 F7 a( T, q  A
  51.     MotoR(nPower+Error);    0 A" p' q8 h5 R9 C3 J; V
  52.   }else{
    9 _9 [3 P6 d4 K; s
  53.     motor_9.run(0);. M. [1 ~+ A' x" y  O, d
  54.     motor_10.run(0);
    ; `6 o# j2 t: P2 M7 X
  55.   }  B* T5 O( r) j  A' B" Z4 @# L
  56.   do{}while(millis() - StartTime < interval);
    : I7 E# w' C6 Y; [5 a& x; ?" o
  57. }
    1 s  @3 o) ?) U6 A; M
  58. * }, x5 x/ D( ?+ x3 D7 G0 @8 J
  59. void MotoL(int Power){* g- \5 b% j4 L+ B5 Q: y
  60.   if (Power > MaxSpeed){
    + t: p' `/ J: {. k
  61.      Power = MaxSpeed;. u/ z! ^/ p( I4 }. z2 p! G
  62.   } ! x  q# c' y, e4 {* V! }2 h% ?7 f' I
  63.   if (Power < -MaxSpeed){0 _# j. ?4 u% t/ U
  64.      Power = -MaxSpeed;4 K1 u0 p4 V# T, X" q# f1 }$ x
  65.   } / m) v: o3 @  N1 Q& P
  66.   motor_9.run(Power);
    & Y5 Z6 \$ D; W' O8 t( ?
  67. }  2 f' ]  U8 x6 \4 N& @1 g$ T

  68. 4 c$ d, j- U& h2 l0 P0 ?5 z
  69. void MotoR(int Power){4 x- }' V: w% b" U; L+ r7 a3 x
  70.   if (Power > MaxSpeed){
    ' }/ e* j& p1 W7 X# d" F. v
  71.      Power = MaxSpeed;3 {9 Y. z+ r) u; P5 x2 J
  72.   }
    , M& p  [: l+ \3 M/ }
  73.   if (Power < -MaxSpeed){
    ! z; R7 {7 a8 E7 L% r- w( `. m
  74.      Power = -MaxSpeed;
    . G+ d, `. `# A" p0 F6 }. _, Z
  75.   } " H3 o6 v2 i  r: p( Z) I8 [8 @
  76.   motor_10.run(Power);
    , ^/ P, ^& d9 x- Z: u+ Y
  77. }  
複製代碼
, z& \; }1 M3 R. ]* @
6 c0 _- \* B0 ^! j
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯
0 F- ]9 _1 ]# U1 c' U, a8 S
0 v+ F! F: h0 ]9 z$ |/ G您好,不知是否能向您請教。
% J2 \+ ^5 R; Q$ ]目前和宋修賢老師在處理Ardui Car# L5 O0 u, j# ]) y  n; i) ?2 o
雖然已使用較繁雜的方式處理了跑出黑線外的狀況, K% }; s% @8 L* V$ c) ~; O

3 h8 j5 d! v% c" W! i但基於想追求更精簡的程式所以還是想請問一下
, c! l, \4 _- `5 r就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝
9 k# \: p% O- [. x% z1 f0 v3 L( p不知道您是否願意教我可以如何處理
9 O: X7 C; u9 a" e! {" F$ x) ]3 d. `. x7 _+ L/ `* H
, p+ Z0 p2 G7 o/ x% R. \
以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;
    + S: b. n& w# T2 H
  2. double CNY70Val = 1000;! E9 e5 ]! G- N2 _' W4 i& j. T- X
  3. int interror = 0;
    ; E! D+ i9 i& z! D) ?
  4. int olderror = 0;' u7 c6 C8 B' Z- j3 Z
  5. double values;
    8 B+ j7 u9 p9 ]$ o2 A; s% H
  6. 7 v, U* c, e3 w' E/ A* Y3 I2 }
  7. & Q# {+ X. z4 g# \8 C& {8 o
  8. void CNY70()' K. ^: q5 Z1 g! k! `, B; \' E1 |
  9. {- F  Z- S: Q- {2 ~4 T/ L. P
  10.   valuesRR = analogRead(RR)
    ) w0 S4 U4 f- @3 g2 @
  11.   valuesMR = analogRead(MR);9 G3 S, k& x7 |8 I7 s5 A/ S( ?
  12.   valuesMM = analogRead(MM);( o2 e( H2 K- ?4 N2 A; g0 r
  13.   valuesML = analogRead(ML);
    ) V8 A; o$ H9 s" c" L
  14.   valuesLL = analogRead(LL);
    ; D4 E" t/ m& {7 e; P4 Z4 Y
  15. # ~% X: Z0 X/ b
  16.   if (valuesRR > CNY70Val)5 i9 a* l% ], C' K# `9 _1 B5 Z, w
  17.     valuesRR = CNY70Val;
    3 ?" n7 P) ^- x# Z, O! ~% h9 [8 T: Y
  18.   if (valuesMR > CNY70Val)# [, A0 ?( [! z) i7 {
  19.     valuesMR = CNY70Val;) r0 q2 m: A: G5 V- Q% i# i
  20.   if (valuesMM > CNY70Val)
    9 f9 e; [( C2 q
  21.     valuesMM = CNY70Val;8 t: ^% o5 Y+ f8 m5 v9 m, T9 c
  22.   if (valuesML > CNY70Val): d6 s" B8 i6 a1 Z, X0 g) T
  23.     valuesML = CNY70Val;5 L& |( V: }/ J) S) F
  24.   if (valuesLL > CNY70Val)
    * l6 ~3 S  V) E& V1 K* e
  25.     valuesLL = CNY70Val;! e4 B* m3 c/ V) M8 I7 ^$ Y

  26. $ j- Q' L) d, r/ i8 k, u: N
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
      c. l' m1 \2 h% e! X
  28. }3 m1 ?# X+ S5 g; K2 p

  29. 7 z. |2 w* F7 y1 M1 A' ~* J( d1 V  P
  30. void Car()5 _0 M# o. {4 u
  31. {& D  r( J& }. m1 R/ D
  32.   while (1) {; a5 ]5 O- A" f2 N* J# U$ K' t  o' J
  33.     CNY70();& F( g5 k2 n* X1 C7 E  q
  34. 0 r3 j2 E) K2 Z: M& K
  35.     int error = ((int)values);
    2 c/ S3 R! h# ]- P0 T- Q
  36.     interror += error;$ ]6 d, M; j/ ~& O
  37.     int lasterror = error - olderror;: n6 K0 u! D+ q! ~& g  G' ]& [' N
  38.     olderror = error;( _! k0 u- c1 O  G9 b
  39.     int power = error / 5 + interror / 10000 + lasterror;  B8 M; y" O8 f$ @) o( e

  40. $ w9 E, K+ ^& n8 n8 ~$ \9 @
  41.     if (power > MotoSpeed)- L) ~1 Y9 S! `5 a
  42.       power = MotoSpeed;
    $ B% U! x) W& _+ N' G2 {! B+ |
  43.     if (power < -MotoSpeed)( h$ L4 O) U) t9 S/ e
  44.       power = -MotoSpeed;1 i( ^# q: Z6 Y% h; P

  45. : O4 Z" u: P( f! h1 n: D
  46.     if (power > 0)/ Y! C+ i/ q% v5 ]6 d. k
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。
    9 Q4 S  x- c2 B# y
  48.     else
    ' ^  r' x6 C: h  [0 ~( k3 d- K& X
  49.       Speed(MotoSpeed + power, MotoSpeed);9 k( Q7 t2 ^) ]4 R0 G
  50.   }
    % N3 S4 h' b5 w3 j* s) H9 ]
  51. }
複製代碼

1 p& m% j' O* L3 \) E, X4 f. x) m6 m; O) Z( d
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

QQ|Archiver|手機版|小黑屋|圓創力科技有限公司 IOP Robotic Technology Co.,Ltd Tel: 07-3924582   

GMT+8, 2019-4-22 12:42 , Processed in 0.046517 second(s), 17 queries .

Powered by Discuz! X3.4

© 2001-2017 Comsenz Inc.

快速回復 返回頂部 返回列表