設為首頁收藏本站

圓創力科技.MakeBlock TW

 找回密碼
 立即註冊

掃一掃,訪問微社區

QQ登錄

只需一步,快速開始

查看: 10365|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>
    ' {7 ?* `. ~" l$ M/ @
  2. #include <Wire.h>
    ! s- f3 R! L. p! B& p
  3. #include <Servo.h>
    - Q) D8 d1 ]; C7 `$ z7 d+ K. k3 s
  4. - U! p& c1 y9 d* x
  5. #include "MePort.h"7 }2 ]$ @) o: P
  6. #include "MeUltrasonic.h"
    8 o1 y, a: k) f1 i$ m
  7. #include "MeDCMotor.h"
    6 v8 T. T6 Q* k) I, u

  8. # }1 c* T5 z' ^. J/ u; L3 L
  9. //double Input, Output ;! h1 v# S- X* B: y& b( d1 f2 ^
  10. float MaxSpeed = 255;
    4 ]$ v& b) D' Z
  11. float MaxPower = 180;3 V( q4 T! w6 [; R/ Z  E
  12. float MinPower = 120;4 G  y: `& e( m7 T+ D$ @: h
  13. float Error,ErrorAcc,ErrorDec;
    3 F0 B$ A% [( Z7 l, K5 w
  14. & u4 P: g$ v2 r3 u
  15. float Kp=0.14;/ L3 b. N- t$ \% p  B$ u
  16. float Kd=0.00020;//23;/ I* Y  B: K- E
  17. float Ki=0.000201;
    3 ~; i7 E% _& b6 Y$ q7 L. E
  18. ! I* L2 M% y) k2 H
  19. float nPower;% i; q/ v, Z" i3 A& t6 ]- I
  20. MePort lightsensor_6(6);+ J3 U; E) M: L% ~: u2 d8 O
  21. MePort lightsensor_8(8);
      u5 \' f5 e  N# I" c3 ^# {" I
  22. MeDCMotor motor_9(9);
    ( @/ q- v! K% f1 @5 O" Q2 x1 M
  23. MeDCMotor motor_10(10);+ m; c4 W* S2 c  Z; o6 S
  24. unsigned long previousMillis = 0;
    6 y! i7 V- L; N/ Y4 V, D- l7 h
  25. const long interval = 1;
    * t2 c1 w: @7 n1 u
  26. ! C; i# g& O/ H, P- ?. K4 S' h3 s
  27. void setup(){
    8 O3 w. ~( k# A
  28.     lightsensor_6.dWrite1(1);  A  L  ]/ {; r8 J5 h8 }
  29.     nPower = 160;
    6 ^' h7 @# `# m' G( A! j) r
  30.     Error=0;8 |4 |7 h2 g. [# Y0 T9 v+ c
  31.     ErrorAcc=0;
    * H3 C8 V, e! N9 z5 Q
  32. }
    2 e2 q& C. k% _

  33. ) |* D4 |, e4 X' U/ F# X
  34. void loop(){% z' G) w" }* M. G
  35.   unsigned long StartTime = millis();
    " {. n6 l% D" B" U
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    3 B4 ]8 Q6 |  o  `' `1 ~
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();+ g. c1 t  p/ P9 Y9 i2 Y
  38.     ErrorAcc +=  nError*Kd ;
    & f& u, Q! P+ V, M. Q3 T7 b! i
  39.     ErrorDec -=  nError*Ki ;
    . J6 Z0 ]3 m- i# \$ f& ?
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    ! f8 \0 A" V( D; e7 f4 _1 u) a& W
  41.     if(nError < 80 && nError > -80){
    ! [( J" f$ R: o3 r
  42.       if(nPower < MaxPower){
    9 g9 x& f* x) h* J' [
  43.         nPower += 3;
    - L$ N+ B" g  K8 B7 C& B
  44.       }2 p& m$ N6 H! [+ u
  45.     } else{; t! _" l: @; P7 E
  46.       if(nPower > MinPower){
    $ G0 J0 P; u# c; @- L
  47.         nPower -= 2;
    6 _: y' I5 ?5 y1 h
  48.       }
    ! ?! ^% ?- f/ b& _" h( V5 w9 }( b# ~
  49.     } % P+ J  T+ g- z, l; K. D
  50.     MotoL(nPower-Error);
    # X) w( |. l2 Q' b0 z, S' W) ^
  51.     MotoR(nPower+Error);    8 ~* S4 D& N2 S9 L0 {% P) s) }& F3 p0 C/ M
  52.   }else{
    5 {7 X1 Y# ~3 r* S: Y8 q
  53.     motor_9.run(0);
    0 ^. d% r5 T6 ~( C7 f" M3 a
  54.     motor_10.run(0);! V* P/ D( T8 E3 ]: ~
  55.   }
    : @7 C$ Q2 z9 D, j- k- S* Y
  56.   do{}while(millis() - StartTime < interval);
    ; }( V% }1 j& s# i- d
  57. }
    2 m9 Y( b6 o7 b4 m! A. k6 y

  58. ' [6 }# H" g2 T' N
  59. void MotoL(int Power){
    - h9 L+ X$ [3 r2 b; n0 l' R
  60.   if (Power > MaxSpeed){
    + q+ g( Z, `; H8 @, @9 @% y: J
  61.      Power = MaxSpeed;
    & |. b3 O" \' `& u8 @4 b: A( J0 H
  62.   } $ U6 F: I. I  o. J( R
  63.   if (Power < -MaxSpeed){! l6 T: z4 E# I/ o# [
  64.      Power = -MaxSpeed;
    8 m# u1 @# M* ^
  65.   } . R- Y! o' O3 i. `  l& R
  66.   motor_9.run(Power);
    4 C$ G, c# q" u0 \( G3 o4 I
  67. }  
    # t" D3 E) a% n% S7 f# h6 j5 ?0 i+ S
  68. - Z2 \2 G1 ^2 I; }
  69. void MotoR(int Power){
    4 @: T; k4 B$ k' w! i1 i
  70.   if (Power > MaxSpeed){
    & l% Y& ^7 d( ~
  71.      Power = MaxSpeed;; Y9 }' |, u1 Z; E& t( y6 @
  72.   } 4 y  J2 T# r( \" _+ q
  73.   if (Power < -MaxSpeed){
    , `. K- H$ j5 q; z& o
  74.      Power = -MaxSpeed;
    7 k- q; m* _, G
  75.   } 8 c- g  m; A' `7 @
  76.   motor_10.run(Power);, d7 x  I% i9 L" P2 P
  77. }  
複製代碼
9 u0 w" O9 c+ j1 M+ N8 ]- h$ Q
  t: B6 h" I3 h8 V! g
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯
% V, L  ]/ H8 L
# a- r9 d6 a: R- j您好,不知是否能向您請教。
# u5 {! _# f$ `5 d+ P目前和宋修賢老師在處理Ardui Car
2 d4 V) @6 [4 F7 h; Y( B( B雖然已使用較繁雜的方式處理了跑出黑線外的狀況" q+ h: {( S1 E" o7 w: ~  t8 Y
0 W/ \6 Y) ?( P, j. K% S% m
但基於想追求更精簡的程式所以還是想請問一下
; `1 v: G  g* Z2 |* J就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝+ e$ \. \9 {8 z# b9 H$ E7 N$ V
不知道您是否願意教我可以如何處理3 u( t; R$ ~" M2 @! e2 ^# [) P

  n$ t9 m& f; K2 v; j/ R5 n5 |9 [2 @, H' n2 w
以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;
    5 `1 R, O: x+ ^: c
  2. double CNY70Val = 1000;
    / `- K. _- z6 z0 U
  3. int interror = 0;+ r# Y& L3 [" m0 l
  4. int olderror = 0;
    6 q! W# l0 X* `- r) j" O' u( F3 I
  5. double values;% f+ G1 N) J5 N6 Z( E4 C) H

  6. ' y) k1 A  M  o0 b2 C: u4 j1 t% O% ]

  7. ( R( |/ U! V' X5 |" t  [
  8. void CNY70()4 @' R# r$ s0 K, a6 ?3 I- s7 H
  9. {  `1 y* `: u' s& j
  10.   valuesRR = analogRead(RR)
    + G4 O4 n; Z! e1 j- ~5 R2 O) @
  11.   valuesMR = analogRead(MR);8 z) H) T9 ]- Y  Y( V4 `$ g) i" N
  12.   valuesMM = analogRead(MM);
    5 g  _' S" N+ @- w
  13.   valuesML = analogRead(ML);+ \2 }+ h, t, I( h
  14.   valuesLL = analogRead(LL);
    8 H5 }8 \% x0 e1 ~
  15. 3 Z# E, \  i! L" G
  16.   if (valuesRR > CNY70Val)6 q/ P( {( X2 o9 i
  17.     valuesRR = CNY70Val;* C4 I& \$ ?' j7 l9 S" q! q
  18.   if (valuesMR > CNY70Val), v4 U3 {& {  Z/ d
  19.     valuesMR = CNY70Val;6 L2 }( D4 o2 ]
  20.   if (valuesMM > CNY70Val)
    8 s) o$ A# w5 x/ w5 F
  21.     valuesMM = CNY70Val;( R. J" W2 E% Q" C( m
  22.   if (valuesML > CNY70Val)# f$ Q1 S$ M6 }
  23.     valuesML = CNY70Val;* \2 O& Q' O+ v9 Z4 x) q1 v
  24.   if (valuesLL > CNY70Val)  y2 b: a! m; \) c
  25.     valuesLL = CNY70Val;
    6 M* L% I& h- |# Y4 a
  26. 3 M, ^7 ^+ w7 x' m
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    8 z7 ]+ X+ O" m- I
  28. }
    ) l$ b1 g. f4 Y/ R3 ?5 }; b. N. P7 ~
  29. ! z( p/ h' x: {/ E
  30. void Car()
    ( |6 w7 W, _  B5 s/ q, p
  31. {
    ! S' |8 c. `* W  b( y9 o
  32.   while (1) {* }' E/ c7 r, u( J  h
  33.     CNY70();, ~: l( L" h; M( L0 X( H

  34. 7 |( E/ _* p* m( e
  35.     int error = ((int)values);
    + K3 i- O" y$ \  R. G3 z1 T
  36.     interror += error;
    9 ^) S; j" \6 L$ A9 D2 Q, Q
  37.     int lasterror = error - olderror;
    . w- F+ J; F% i% \* g' K
  38.     olderror = error;
    / e' k+ h" t' N/ O
  39.     int power = error / 5 + interror / 10000 + lasterror;8 S9 }* J1 k8 P
  40. / d9 N& H% Q% b" P4 m
  41.     if (power > MotoSpeed)
    ' I& O% X/ q: V" J9 i" K
  42.       power = MotoSpeed;
      N) J$ m4 P/ A5 t6 P
  43.     if (power < -MotoSpeed)5 c4 @+ g* t5 D7 `) d
  44.       power = -MotoSpeed;
    : k( E3 \( w; W* B) F9 L( @' N0 d, H

  45. * V5 V. i) {7 E4 p
  46.     if (power > 0)
    : ^' N7 o5 D" ~5 q5 w6 X
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。
    . \2 |& a, f$ h+ S/ U
  48.     else
    - _# I% N5 V/ q# E
  49.       Speed(MotoSpeed + power, MotoSpeed);/ F& a9 _) T+ z' y6 M  L  @' ^
  50.   }
    1 V2 e" s/ }5 k9 Z1 k5 }5 q; {
  51. }
複製代碼

# W# m# u- g+ t3 U0 `/ l
2 Q2 \" z. l/ F- d
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2020-2-27 01:18 , Processed in 0.036210 second(s), 17 queries .

Powered by Discuz! X3.4

© 2001-2017 Comsenz Inc.

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