圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21777|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>
    - ~: i. L# }/ R! n. u/ g2 Y
  2. #include <Wire.h>! Q$ g" U1 c  ~; @
  3. #include <Servo.h>
    2 X3 H4 f* w, M# T8 I
  4. / p) s1 `# q$ [$ d
  5. #include "MePort.h"
    6 d$ r1 Q. J9 Q8 B& w0 D
  6. #include "MeUltrasonic.h") V/ J" M7 d  ~! b2 k: {
  7. #include "MeDCMotor.h"
      _" l% ~$ v  P$ B$ g8 i

  8. 5 `4 H4 m! \2 a
  9. //double Input, Output ;9 W) Q1 v( G+ N1 q
  10. float MaxSpeed = 255;, ~' t9 H+ y' F
  11. float MaxPower = 180;
    $ D) J& q, Q$ t2 g
  12. float MinPower = 120;
    # ?4 Q, }9 v; P6 ~7 w* p2 S# s
  13. float Error,ErrorAcc,ErrorDec;
    " [! ?, T8 N* d# g5 E3 S' c- t5 S

  14. ; ]4 R+ @9 d" Y6 i% P, S
  15. float Kp=0.14;
    - n6 r* Q  N8 Z: s- U9 _  u9 T
  16. float Kd=0.00020;//23;+ d7 E+ E4 J' O! Q! Y# |8 }& F
  17. float Ki=0.000201;) ^8 \% P7 g6 W$ c& q
  18. $ I0 [4 C. L5 H2 k
  19. float nPower;
    % u0 v$ `4 o6 ^9 h" G8 _
  20. MePort lightsensor_6(6);
      \8 x7 N8 u$ J$ z' G
  21. MePort lightsensor_8(8);6 I. z" F5 C" J& F' j
  22. MeDCMotor motor_9(9);; ~5 c: O' J) h2 r& I
  23. MeDCMotor motor_10(10);$ J* p, _* Z4 r# a
  24. unsigned long previousMillis = 0;: u4 X% H" d9 P* x. q8 Q4 p: D/ E0 r
  25. const long interval = 1;: i2 _+ a" N1 g; i! \' e
  26. ' R" [2 k- o& J
  27. void setup(){. q) c/ O! G0 h) @. N( O
  28.     lightsensor_6.dWrite1(1);( S/ X: n% |: `1 G' n" C3 O
  29.     nPower = 160;9 j7 @$ d3 ]' n2 D- s, T
  30.     Error=0;6 }  c" b; m# W4 E( G
  31.     ErrorAcc=0;+ V4 a5 T+ l/ w9 m2 A
  32. }7 f( S/ `( x8 v

  33. 9 P# w, Q, o+ n, C* ?; C  U9 p+ c& y
  34. void loop(){
    + T$ B3 u( B, W3 b3 }; O
  35.   unsigned long StartTime = millis();
    6 z; Q! I1 `6 \& z
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    6 B3 X4 s3 ^/ F7 A& a8 X) Y/ G
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    0 c8 B0 d8 I$ C$ a2 e: G
  38.     ErrorAcc +=  nError*Kd ;1 H4 c- m4 ]" A+ v- l+ |
  39.     ErrorDec -=  nError*Ki ;; A' d. p& J, \, Q, W# N
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    , n8 w$ Z# |0 i% o& j  P
  41.     if(nError < 80 && nError > -80){- L1 W% n% J0 t, [- F: \
  42.       if(nPower < MaxPower){
    $ ]* y1 W0 D, N6 W7 O
  43.         nPower += 3;+ M* e3 l8 V2 }/ F) m; m
  44.       }( C0 c9 `/ e- L
  45.     } else{
    & S5 X/ V6 S; W# e1 Q3 g
  46.       if(nPower > MinPower){
    , o4 E1 G7 H! `9 c  z
  47.         nPower -= 2;% P, b: @& D, C5 c2 v9 D
  48.       }4 F' H7 K5 Y5 y0 i
  49.     }
    2 _6 S( q% `4 S* d, K
  50.     MotoL(nPower-Error);
    0 }. u! ?/ R7 `7 h
  51.     MotoR(nPower+Error);   
    , x! X! E# R2 N9 Z! ?6 f
  52.   }else{
    2 Z$ U: c0 J" A. A$ [' y
  53.     motor_9.run(0);
    9 }2 P, R  T( V
  54.     motor_10.run(0);/ ]6 L$ V) ^/ ~# r
  55.   }
    * F3 [( ?# ]: U2 x# ~
  56.   do{}while(millis() - StartTime < interval);
    . W" s! T, X" ]1 d+ u) _
  57. }2 `5 b1 A" G. Q* S
  58. 4 O* J' O& T# D
  59. void MotoL(int Power){0 M5 U: V9 o6 z# n6 T) ~, k
  60.   if (Power > MaxSpeed){5 y( p9 e/ [7 F9 I7 W
  61.      Power = MaxSpeed;" b; b8 C$ Q1 h6 V  D2 Y; b6 e
  62.   } 4 y( l. l! R/ a- l7 d% b
  63.   if (Power < -MaxSpeed){
    2 O, ]: R0 V0 q6 d9 P7 y  A
  64.      Power = -MaxSpeed;! q5 a; X' v) o: ?1 Q: Z
  65.   }
    0 G4 m& V8 C, @6 v* X1 k
  66.   motor_9.run(Power);
    ) J. M2 m& p1 f' G' [/ T) [. `- J7 |
  67. }  2 V. g7 M. {. O( J
  68. ; F# p) p" f7 U" q- R, D7 X. O! d
  69. void MotoR(int Power){. O8 c& q2 H3 G
  70.   if (Power > MaxSpeed){
    3 E9 @9 g3 u# _* {" K
  71.      Power = MaxSpeed;' I4 E8 _  {  y2 V
  72.   } - Z( ^: P& @4 z! d5 X
  73.   if (Power < -MaxSpeed){& [* C8 a' D2 Q9 }. i; X
  74.      Power = -MaxSpeed;
    8 Y3 L7 e' R5 ^- v- m
  75.   } * G: V" c1 X1 c% r/ \$ o3 `3 ?
  76.   motor_10.run(Power);
    8 ^/ E4 W3 j% ^" y  V; K# b4 }% q
  77. }  
複製代碼

9 A' c7 [% L% |7 a5 X8 T$ @% t! d7 d. f1 X+ f6 h: u2 A/ r
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯 / p% v; v7 h4 R* h
  N" O9 O9 k( }6 [! ~
您好,不知是否能向您請教。! _. l/ O$ p$ \' ~" t4 W
目前和宋修賢老師在處理Ardui Car3 O8 N- r6 T; _! l" Q( `; I# o. Q
雖然已使用較繁雜的方式處理了跑出黑線外的狀況. L) e* z" U0 I- X: U) u
9 j) g2 X! j9 Z( n& x1 t0 D. O0 K
但基於想追求更精簡的程式所以還是想請問一下
- A; O# y; y' a; s  C就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝
+ `0 [9 s9 O6 [8 S% p, p  v不知道您是否願意教我可以如何處理3 ^( _% ?$ N* ^3 ^, t
0 P# ~, B' u1 p) f! ^" A. j9 _6 e$ Y

2 F* u0 M6 G: ~' v- w7 k以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;
    ' L0 c: B/ b; z
  2. double CNY70Val = 1000;) u9 n  V5 N+ o, h# H$ K
  3. int interror = 0;' f4 K5 A0 J+ G1 a7 J' y3 B
  4. int olderror = 0;
    2 P$ T5 d& w/ \6 r
  5. double values;
    ' B) j" |' N) e- f+ R& w

  6. , m) y9 f* l$ \9 |/ I, ^1 T1 o- ^" ^
  7. ) ~) _! v2 v) B$ X7 B, H$ `% b
  8. void CNY70()
    7 v' f# j$ \# p2 f( r! T% h
  9. {$ U8 z* j4 D7 x; N! H
  10.   valuesRR = analogRead(RR)) D5 E- Y/ s1 x  x- i& R
  11.   valuesMR = analogRead(MR);
    9 f* q9 d8 n' k6 I1 I) i
  12.   valuesMM = analogRead(MM);
    * h5 v3 R. p) w3 J# Z3 y! N
  13.   valuesML = analogRead(ML);
    7 g! N' @4 _! o8 Z! \5 N  m
  14.   valuesLL = analogRead(LL);- D" o% O' R1 H& V; f5 J5 P* \
  15. 9 B% R+ e& z5 |
  16.   if (valuesRR > CNY70Val)# M' S5 T) x7 ?' n  l3 F; w
  17.     valuesRR = CNY70Val;& z" e) O; A8 t9 o- s) d3 w7 |" I
  18.   if (valuesMR > CNY70Val)
    . b- U& x* Y% m1 L2 _6 E
  19.     valuesMR = CNY70Val;0 G/ a" ]* ^( W3 ?( Z8 F
  20.   if (valuesMM > CNY70Val)1 ]' p7 S8 d9 n
  21.     valuesMM = CNY70Val;! d9 K& @3 x6 P' _
  22.   if (valuesML > CNY70Val)
    % N( \7 e- j9 o  w9 Q# c; t
  23.     valuesML = CNY70Val;" K' i' r/ t: d' m/ h9 I
  24.   if (valuesLL > CNY70Val)9 P4 C. O$ E, s2 S% G; P; ^6 v
  25.     valuesLL = CNY70Val;
    4 u, ~) R- H  ^" M+ o

  26. 7 W' N: o" h7 ]7 h2 d* C% T: z
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;9 G! y" a4 J* e- L# z2 z# r/ F
  28. }1 q, }8 b. R: b

  29. " x/ i# k7 I3 e. m1 V7 Y' ]
  30. void Car()
    1 ?* w- R1 N" o1 k. N+ @- b
  31. {
    5 m; ~2 \) x2 O+ M7 O  h
  32.   while (1) {
    ! |1 P) R1 B; T( l
  33.     CNY70();
    1 Z1 e3 Q! K0 @% R8 X8 D, I  N' S
  34. ; p, q# |+ d! U8 Q' [) s
  35.     int error = ((int)values);
    3 S, q- T' d$ D# r) B1 [
  36.     interror += error;$ o9 u$ G- g( b7 B  g0 S5 S
  37.     int lasterror = error - olderror;3 t& }: k  B! o* b' J" K' K/ ]
  38.     olderror = error;5 {) I+ A- E' M8 M, Y/ @, ^. m# z
  39.     int power = error / 5 + interror / 10000 + lasterror;
    $ Q, G; P# K9 Q; `! b% K; u' a
  40. ) h  d( R# T3 x0 r. v* m
  41.     if (power > MotoSpeed)8 G  S" j7 w- y
  42.       power = MotoSpeed;  r& Z. {) P) A! Y
  43.     if (power < -MotoSpeed)3 _, B/ z5 w; E, V+ d
  44.       power = -MotoSpeed;( c+ M2 x9 B: |2 k9 k6 Z# y1 Z
  45. : R& a" P/ V3 T9 y
  46.     if (power > 0)
    2 D1 s" Y. n  r9 i# _
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。- `3 L9 c' ~3 L) c4 t" `
  48.     else; C9 E; _( w" _
  49.       Speed(MotoSpeed + power, MotoSpeed);
    7 Q: q4 X- N: n: T: r* s. s: w
  50.   }
    6 }2 U" Z0 k; W
  51. }
複製代碼
+ P6 d% [/ M" b% ]: u6 o
1 F( T& U* D3 [# M- o$ W
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-11-30 05:18 , Processed in 0.029347 second(s), 17 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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