圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21806|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>
    $ j; R: n' L' r% y3 x) ^4 c
  2. #include <Wire.h>& j1 J! T1 m+ H; E
  3. #include <Servo.h>1 y" u% W1 e9 F: b3 o! g; F
  4. ( @) q0 ~+ K! v/ G% W% w$ O4 B
  5. #include "MePort.h"* q1 c2 ?- M* x' \
  6. #include "MeUltrasonic.h"
    9 z! P, [4 Y' B
  7. #include "MeDCMotor.h"2 V: k& r1 D. d& A2 d

  8. 5 O3 a( l4 n, S
  9. //double Input, Output ;
    2 M( a' \+ J) t9 }
  10. float MaxSpeed = 255;5 W5 Q: i0 C* F9 E: q* c, `2 I
  11. float MaxPower = 180;
    ) {/ W& U' q! q' P  M9 f0 h
  12. float MinPower = 120;
    ! J* j" ]# j4 O0 J8 |* ]
  13. float Error,ErrorAcc,ErrorDec;/ |* y- c; ?$ E# n$ y

  14. ) ?. o5 d; o+ J3 B" i$ o8 N0 L
  15. float Kp=0.14;
    7 P5 J  a: e( n: X
  16. float Kd=0.00020;//23;
    5 x: `' s* S, V% f
  17. float Ki=0.000201;* `) f* v* @. D  Z8 I- M
  18. & Z; e: n- y- I8 w, a- n3 h
  19. float nPower;
    5 k! ^; ?# s* d- s+ L: U
  20. MePort lightsensor_6(6);* b% x) V! Z* Q4 x& R
  21. MePort lightsensor_8(8);: u+ c1 _4 B9 K4 b/ G0 k" Y
  22. MeDCMotor motor_9(9);
    / Y" x/ l9 g. ]; p4 k" F$ g) v
  23. MeDCMotor motor_10(10);$ d+ p; f: i3 U2 e% W( d
  24. unsigned long previousMillis = 0;; q! @+ T, k9 v* q4 j3 a# Q- u
  25. const long interval = 1;1 Z3 u4 _7 ~9 E! y) P  V$ v

  26. 8 A% I9 E! \' H$ x
  27. void setup(){
    1 [& Q+ u- V& B% i
  28.     lightsensor_6.dWrite1(1);/ ]6 A4 n  l  S2 H* X0 Y* r$ b
  29.     nPower = 160;, `2 z8 U  @. ^; C
  30.     Error=0;
    ( Q; D* |% L0 \% n9 R
  31.     ErrorAcc=0;' T* z! j5 M9 Q+ P& J& _
  32. }
    6 R$ ~" }$ [+ z! W2 R0 B9 J

  33. % @9 m+ V' I- Z( ?* o  P
  34. void loop(){0 J0 v- Q! d, Z- r/ V% Z
  35.   unsigned long StartTime = millis();
    3 |! |, a9 g. U
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){$ T" ~& q# ?+ X  W8 r
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();2 Z& m' p8 L# n1 j9 Z% y
  38.     ErrorAcc +=  nError*Kd ;3 l$ a' N& F- s& s& U3 p" P
  39.     ErrorDec -=  nError*Ki ;& M: H2 j* B$ l% j6 ~% u
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    9 V( q- n  q( A
  41.     if(nError < 80 && nError > -80){: ~: _) ~) d6 |( W
  42.       if(nPower < MaxPower){
    & c9 c% O' W" c- T# k
  43.         nPower += 3;0 t4 C, f9 _  R# ~
  44.       }
    8 s: y0 M$ h/ y& x+ c
  45.     } else{) k5 h  r4 R0 z! s; k
  46.       if(nPower > MinPower){
    $ |/ ~+ g. n" u, a
  47.         nPower -= 2;+ [3 `: T- y3 ?. w. q0 Y
  48.       }
    ! e$ n4 E* y  e( B8 t0 }
  49.     }
    3 w- g' M, s* ~: o, m" H8 g% v
  50.     MotoL(nPower-Error);* ]. |& E* ]1 A0 O, b( J5 I
  51.     MotoR(nPower+Error);    , }) C8 S: s; n8 X/ p6 m& v8 f( G* Q
  52.   }else{& j  o7 ?( }+ C& r. E2 e
  53.     motor_9.run(0);3 M( {" V- N9 g2 }- |3 h5 @- r
  54.     motor_10.run(0);
    5 ], G* }, \) @0 N! \% {
  55.   }
    ; r' B0 c2 s( i5 d) H
  56.   do{}while(millis() - StartTime < interval);
    % H- h* n9 O  g( j( k
  57. }% c0 V" W1 V$ k1 }/ F3 Z

  58. $ {9 ?. _, E4 d$ R  D
  59. void MotoL(int Power){
    / A- _1 k. ]% e( R4 U! Y
  60.   if (Power > MaxSpeed){
    % S: w$ X/ {7 x" }( J: t9 _7 Y
  61.      Power = MaxSpeed;
    # P7 W* k$ N2 ^8 \, \8 `$ T! [
  62.   } 1 I2 w9 u9 `5 B7 h  f) |. ~% E2 h
  63.   if (Power < -MaxSpeed){& W0 x7 _8 e3 }
  64.      Power = -MaxSpeed;
    ) i0 L1 N- j% J- A6 R3 g
  65.   } 3 W1 b) A% h, d- d0 C
  66.   motor_9.run(Power);
    ( z: m& h/ T  F. Y  d% t2 J
  67. }  
    " u# w8 f9 `6 q' f- y
  68. " I# t4 q1 F% Z) Z
  69. void MotoR(int Power){
    ) w0 \0 |/ a5 _
  70.   if (Power > MaxSpeed){+ J5 D* K+ @; x* M0 ?
  71.      Power = MaxSpeed;: M" x; |' ?% V2 M6 {; k
  72.   } 5 q- {; M1 h- F# z2 [
  73.   if (Power < -MaxSpeed){/ f: _/ ^: d3 l1 z/ w
  74.      Power = -MaxSpeed;! m5 \) Q. a9 q+ ~* u% y" [$ o3 B
  75.   } . n4 o: Z- F- D; I2 e( B
  76.   motor_10.run(Power);
    % d7 c3 t/ |: U
  77. }  
複製代碼
: d4 ]8 z$ o: `

8 A6 z! i1 K5 I
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯
9 \) [$ O, t5 m% r1 u2 r" }9 Q$ u
: q$ F- X% e+ ]- ~. |: z您好,不知是否能向您請教。
8 _& h, y% W8 r6 b7 h, G目前和宋修賢老師在處理Ardui Car) V2 k' b) a6 ^. h& X( L1 b
雖然已使用較繁雜的方式處理了跑出黑線外的狀況
% t: E; r3 [* E9 U; F3 k, ~% d) R  p6 u& v* @9 t4 j' {5 Y1 w
但基於想追求更精簡的程式所以還是想請問一下! o' V+ G5 h4 ~6 s- r
就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝
* z2 i+ y: ]: H) Z$ |: Y4 y/ q不知道您是否願意教我可以如何處理
' v% r/ R" B, Z9 h; V! d0 K
7 ^) P; r9 y/ p- D9 n+ v  m/ q+ d2 `+ d
以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;
    / A, p0 D& x% K% ?4 g, H
  2. double CNY70Val = 1000;8 X- t  H2 `) v2 M; Y
  3. int interror = 0;% [( n& x3 `' J7 z7 {
  4. int olderror = 0;  ?/ J, t: D& x; ]# Y5 t4 @
  5. double values;$ H9 o( s/ w" [

  6. ; x; P& `' U0 b8 q! k' L; F

  7. 2 L' q- s/ r& ~% d6 j( [7 L8 g
  8. void CNY70()+ q- G' c2 y6 G
  9. {
    1 F3 R* F( B: l
  10.   valuesRR = analogRead(RR)
    9 ?8 s2 c: \, ^1 t' c6 M, _
  11.   valuesMR = analogRead(MR);( t  P8 Q- Q* Q* e/ j) s: P! h
  12.   valuesMM = analogRead(MM);
    + |* u) D6 p  p; V6 y- v
  13.   valuesML = analogRead(ML);8 A4 L& S3 `# u% ?
  14.   valuesLL = analogRead(LL);
    / Y  e5 P/ a! V: E7 Y

  15. 1 t8 G. \1 y6 X" c
  16.   if (valuesRR > CNY70Val)1 C& y4 Z5 S4 f& M* ]+ O
  17.     valuesRR = CNY70Val;
    $ F, z, D& y: f2 `9 i" N$ \1 R' I
  18.   if (valuesMR > CNY70Val)+ `/ K: N  }; m6 v  n. T5 z1 d
  19.     valuesMR = CNY70Val;" W) F: |0 O: O. @% W! z' V3 h
  20.   if (valuesMM > CNY70Val)( e  B) @; {5 P  s% n" Y
  21.     valuesMM = CNY70Val;
    8 y" I* Y' n1 V3 A
  22.   if (valuesML > CNY70Val)3 W, l9 T8 d4 G6 @; G
  23.     valuesML = CNY70Val;! j+ o6 D% W: {/ N7 S3 O1 a  s* K
  24.   if (valuesLL > CNY70Val)
    . W: m. i- V- _9 a  L
  25.     valuesLL = CNY70Val;5 ^+ E( M; U# [: {( @) k

  26. . B" l; u0 ?- Z, a6 a
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    # E- H( T4 I3 p! y
  28. }6 l" C; w9 M3 {/ v  i2 K

  29. ( D3 J/ t) m3 P
  30. void Car()2 ^5 b8 C- ^& A. o) f
  31. {3 m3 [( r3 a4 o$ h- i
  32.   while (1) {
    4 M3 ], o) r- w) ~5 q/ ]/ S
  33.     CNY70();
    # H# {  c; R- X1 s0 n
  34. / S* l' v9 C9 Q# ]0 D4 ?8 z8 j1 u
  35.     int error = ((int)values);
    4 B, ~/ x1 l$ R  d
  36.     interror += error;) F6 v( y" V- c( ?: L) M
  37.     int lasterror = error - olderror;
    6 m& e: E* g  c& z* O
  38.     olderror = error;- s7 g5 |+ o6 G
  39.     int power = error / 5 + interror / 10000 + lasterror;
      e5 g: _$ H/ N2 g/ a% J% C
  40. 9 E% Z+ {5 Z/ g* g
  41.     if (power > MotoSpeed)$ P) A# Q3 [' G/ s7 g
  42.       power = MotoSpeed;
    ! d6 H% K' J! T( B: `7 f
  43.     if (power < -MotoSpeed)
    4 I! X+ q. m5 K9 r  w) S! K; L% @
  44.       power = -MotoSpeed;$ g' }" H! n. U6 y& {

  45. ) T8 V# ^; b9 T+ x  q* {0 J
  46.     if (power > 0)
    4 K4 o4 Q6 q# a$ t, O# z
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。( {5 X9 N9 [- J$ i9 z# x6 M$ ~
  48.     else- [9 B8 T6 q5 P. e/ _1 @1 L
  49.       Speed(MotoSpeed + power, MotoSpeed);7 `3 q9 Y% n0 l" l: E" o
  50.   }
    7 z/ k# w$ n  U+ U
  51. }
複製代碼

3 b, k5 g. `, T4 B# ~; @  g7 A# P+ g% b( S
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-12-2 07:09 , Processed in 0.025103 second(s), 17 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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