圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 20350|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>
    ' X) q, U; u, e
  2. #include <Wire.h>
      C) O* C  `  c. p' m4 o4 n" S
  3. #include <Servo.h>- ^( d+ E! R8 i

  4. 4 C0 {* ]; u+ }0 A- n( F5 P) I4 r
  5. #include "MePort.h"7 K# x) p" t1 A2 q
  6. #include "MeUltrasonic.h"; ]# u  p  O( s+ R
  7. #include "MeDCMotor.h"% m& K0 z3 D( x, `; C2 S( ]. g
  8. 3 C5 M, l% I; ^9 y; h) G" R
  9. //double Input, Output ;
    / H- Z% ]. B* a3 `& p7 [
  10. float MaxSpeed = 255;
    / M5 i/ K) O( t! k9 t0 Q& v
  11. float MaxPower = 180;
    ( _+ Q, n1 K% B* `& P8 O, C7 f
  12. float MinPower = 120;+ e; B7 j  L0 d  ]2 }1 X
  13. float Error,ErrorAcc,ErrorDec;
    3 K. }" Y! e% d) E$ m
  14. . M2 ]+ Z# R- c
  15. float Kp=0.14;9 u8 y- Q# k( x: J
  16. float Kd=0.00020;//23;
    / v1 n/ f4 H3 I! \
  17. float Ki=0.000201;3 p' R* f: F& F6 Q: `) Z8 v& R! Z+ v3 B7 Y
  18. . p3 j* L+ |% d  w9 Y5 E
  19. float nPower;
    % C. t" v- `6 E, G
  20. MePort lightsensor_6(6);
    0 }4 W0 D0 g$ p2 _1 D. B
  21. MePort lightsensor_8(8);% Z1 C. k0 d( ^/ |# p& w6 v
  22. MeDCMotor motor_9(9);& K6 d1 b% u5 R; h8 ~* R$ C" E
  23. MeDCMotor motor_10(10);
    : ~6 u  q/ @* T3 K; J5 F
  24. unsigned long previousMillis = 0;. |- c. M- V' X' v; f, z0 k0 I
  25. const long interval = 1;
    # f% g" x9 e% }" j" f' C
  26. $ `6 X2 S5 J7 H- l0 o7 n" Q  y: K# |3 a
  27. void setup(){
      A7 N, x) W, H3 x; h) N6 j% s
  28.     lightsensor_6.dWrite1(1);$ C* c' u  V; G
  29.     nPower = 160;
    1 w5 P4 j0 u/ b5 C: X1 H! w* l1 o
  30.     Error=0;# N- ]6 P  b5 O  C$ o
  31.     ErrorAcc=0;8 d4 Y$ v' m: {) [8 x1 Y
  32. }
    # @, G, ?* \( j8 U2 J  B$ j

  33. ; p% ?/ d) R) u7 E
  34. void loop(){- w6 C; @6 v7 ]- y# d
  35.   unsigned long StartTime = millis();
    % Q) V9 X8 [" O% c& N8 Z
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){5 S3 F; l2 a1 x* o6 B
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();3 m/ _: v* R7 g3 B6 ]! ^
  38.     ErrorAcc +=  nError*Kd ;
    . w) r' f% {' R  }3 D
  39.     ErrorDec -=  nError*Ki ;
    5 c, {3 L& F8 g4 C0 I. C6 c: ^/ \
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    - O- u; K2 o/ E% R4 C) b% I. t5 J# j
  41.     if(nError < 80 && nError > -80){
    ; s/ Q$ O/ |( K! K7 f7 H, j: i
  42.       if(nPower < MaxPower){2 y: ~1 R: g) h" M9 P2 ?* v. {
  43.         nPower += 3;
    & |& Y3 {6 u& z" D: G2 `& Q; R, H
  44.       }
    6 W1 G- T/ F3 n
  45.     } else{3 i( {/ `" R! c/ {
  46.       if(nPower > MinPower){
    , _" t9 c3 j' @) k
  47.         nPower -= 2;: B9 ?% i/ O9 ~; ^/ Y
  48.       }( T. `5 ]6 o0 T! ~& m+ S
  49.     }
    0 x) l1 S: Y  M+ q" c3 R' y- E
  50.     MotoL(nPower-Error);
    - e1 `6 a+ S' S
  51.     MotoR(nPower+Error);   
    ) n6 P; b. ?6 C1 |  c
  52.   }else{  D! s& U2 n- a. p6 o3 y2 M4 M& G# X& d
  53.     motor_9.run(0);( X' {, W" A6 n, T8 `8 W- }) e
  54.     motor_10.run(0);
    * L+ m& ^. T! \+ ~( j- ]- M
  55.   }
    6 B; V4 E' S- g' U* I
  56.   do{}while(millis() - StartTime < interval);
    5 x) b' s7 m% F; S
  57. }/ i7 V$ P$ r4 v- c4 n, t5 U' I
  58. , i: S- x- X9 o4 r& t+ Z
  59. void MotoL(int Power){8 I/ O; Q' D6 A" J* h7 y+ Y
  60.   if (Power > MaxSpeed){9 o8 M9 l+ ~4 s9 H% b4 p
  61.      Power = MaxSpeed;
    2 U5 ?$ q! H0 u; W7 e
  62.   } 6 V0 ~* ^! A4 v2 \( h" }) V
  63.   if (Power < -MaxSpeed){
    * Q) ^/ v5 m* Z% |. s0 [
  64.      Power = -MaxSpeed;
    % ^* J5 E! x, c2 r
  65.   }   c6 X& e" r. \" n. g$ N/ n
  66.   motor_9.run(Power);
    9 k: G, W, u! A5 M7 [1 K
  67. }  
    ) u6 p7 n# ~/ p5 F1 ^- ?

  68. ; j1 K: w3 t5 {3 {
  69. void MotoR(int Power){
    , ?# R+ i) I$ w) p8 A8 x: f7 Q
  70.   if (Power > MaxSpeed){% k- U% @5 E' `" w: O& P
  71.      Power = MaxSpeed;
      h6 R) E1 S" g" V2 a, C
  72.   }
    . O6 _- ]" Q7 |* G/ c$ X
  73.   if (Power < -MaxSpeed){/ D; o/ w4 D, \2 \+ m# R' [
  74.      Power = -MaxSpeed;  S! b) ?+ E7 `8 U+ R
  75.   } 3 Q" S" l: s% N$ O7 ]! O+ p. T
  76.   motor_10.run(Power);* E% b. Q/ x) S# ^. F) Z
  77. }  
複製代碼
! w- D) z% f1 ]9 T
" M7 D% [/ w; }1 w
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯
) B/ y& R7 p& ]0 z$ _3 Q
6 P' ~: ?+ U- P3 Q( Y8 c$ A: n; v您好,不知是否能向您請教。
4 m" o! M$ n4 }8 I目前和宋修賢老師在處理Ardui Car
; V" g0 I  c# p, p雖然已使用較繁雜的方式處理了跑出黑線外的狀況; ]( N( K3 o% |) M8 v' F* b( B/ l

) E  k6 c" x3 b# z* S但基於想追求更精簡的程式所以還是想請問一下3 h& |$ `  C0 p8 L( w$ L2 `
就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝
8 [2 Y& V. k6 w  l不知道您是否願意教我可以如何處理, o" `# K! K2 {8 N5 a

; i/ x, B! o5 b8 \! |( v0 L8 b. L4 T* |2 }) A. O
以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;& o* W6 o4 D7 V: S, c- S. b4 f; L
  2. double CNY70Val = 1000;
    ) _0 O  m1 X* {! O6 m% ~- ?
  3. int interror = 0;; y2 ?6 T4 \/ N$ h9 ~, d" x$ `
  4. int olderror = 0;9 l3 v# C" T7 h( z- b; _+ L
  5. double values;
    + c' z  w, k+ C6 l/ u' [1 g5 r
  6. 1 i: R* M) ^& K
  7. : N( e1 B# F, z; o7 ?# w4 f5 Z
  8. void CNY70()
    0 q" P  q# k' C9 p* {% M# h
  9. {$ `$ n: F  ]! L  j: b. f+ K
  10.   valuesRR = analogRead(RR)
    5 _. |  U% n( U; W, F
  11.   valuesMR = analogRead(MR);3 Q( O/ E, W+ y' f
  12.   valuesMM = analogRead(MM);
    $ `* K7 [5 u- J# _% ]) a0 v8 j
  13.   valuesML = analogRead(ML);
    9 x( I( c+ V& D/ g, [/ P
  14.   valuesLL = analogRead(LL);
    # f7 U# Y8 J2 X! ^4 S* q

  15. & b/ Z. {7 E; U& r8 [% M& b3 w
  16.   if (valuesRR > CNY70Val)# g% i( [, E( Y, {' g4 }
  17.     valuesRR = CNY70Val;2 H4 L0 u* Z3 c3 |- m' A
  18.   if (valuesMR > CNY70Val)
    $ J9 e& [$ W  R: B
  19.     valuesMR = CNY70Val;
    * Q0 I, @; s: N7 p1 O5 {8 K) p7 g
  20.   if (valuesMM > CNY70Val)
    / z6 M  @: A8 X* |; c2 s; w; j
  21.     valuesMM = CNY70Val;3 x7 U# a' F- N9 y
  22.   if (valuesML > CNY70Val)2 \/ ~- Z6 Y/ _( [1 _+ e
  23.     valuesML = CNY70Val;" S' u3 ?# @2 B% h' `
  24.   if (valuesLL > CNY70Val)+ t2 O; d$ R+ v
  25.     valuesLL = CNY70Val;
    * b4 p! B7 d* |2 M: `

  26. 6 k3 e# J8 Y& T, X" c2 k. j
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;% d! y# J9 B/ Y+ [" L' i
  28. }3 L# F. ?& h( t) M- a
  29. ! E& E! y2 V# G# b# c
  30. void Car()
    & Q7 E) M) Z1 z8 h: t, p: [0 F
  31. {
    % e* O+ k. D+ }& M% H8 g) ~
  32.   while (1) {
    0 Q- {4 |/ u4 _
  33.     CNY70();4 N* a3 S/ |% {; P

  34. ! x8 c4 d. S8 ]+ `
  35.     int error = ((int)values);
    : C% m3 G9 N0 m
  36.     interror += error;" g) ~/ O- b3 l8 z( H$ \
  37.     int lasterror = error - olderror;
    3 z9 J: O5 j- Q
  38.     olderror = error;+ @; v: N# l% \
  39.     int power = error / 5 + interror / 10000 + lasterror;
    1 L5 F2 {  V! ^+ `1 h5 E

  40. , |' @* _4 Q* b* o, s; h3 O: E
  41.     if (power > MotoSpeed)
    & P, r! q. v& Y% @
  42.       power = MotoSpeed;
    : z' T# v6 q( |) Y" Y
  43.     if (power < -MotoSpeed)/ z( U' b" L5 a/ h; }
  44.       power = -MotoSpeed;+ W" f. T) @" l5 o+ |. [* z

  45.   Y4 O, x" X1 C! X/ f
  46.     if (power > 0)3 V+ f& T! w2 T# L
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。0 B. p6 Z) J9 h+ x
  48.     else  w4 }9 T8 d- n! s0 i/ `2 w
  49.       Speed(MotoSpeed + power, MotoSpeed);1 {* q8 y3 S$ L7 B( f0 Q  R# ]0 I
  50.   }
    ! S) x6 v$ p" D) c1 X
  51. }
複製代碼

2 n: a1 ], Q. H  p# a1 p5 G: `* b
- h; _# D* s, V4 W' _! \; k
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-2-19 07:03 , Processed in 0.027853 second(s), 18 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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