圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21851|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>
    ! i6 B: k3 O8 @
  2. #include <Wire.h>
    / K- G1 d3 g4 r4 T  U
  3. #include <Servo.h>/ y1 {& s" @, d+ B& L2 }5 y7 s, O, S

  4. 3 S$ l' Q' P# b, L3 F' l; S
  5. #include "MePort.h"
    * G* H7 Y- B- L+ @& _
  6. #include "MeUltrasonic.h"
    - T' d! {9 R6 r0 d% E
  7. #include "MeDCMotor.h"0 j6 A, g1 {, }  S6 s
  8. / {9 o( y, S  _" H2 V
  9. //double Input, Output ;
    + h  V3 g+ R$ `
  10. float MaxSpeed = 255;6 t' I, ^5 r, z6 C0 s8 n6 @1 ~
  11. float MaxPower = 180;/ I: E$ _* j& ~5 b9 ^
  12. float MinPower = 120;
    , g& A; ~2 V1 V. ~6 U; L3 D
  13. float Error,ErrorAcc,ErrorDec;
    ( _; o7 h7 |/ p9 u% p8 `

  14. ) @, ?& k5 M6 P7 |9 ]' u' G
  15. float Kp=0.14;* x, n/ H' m8 Z  ?8 V
  16. float Kd=0.00020;//23;0 j) n+ I/ l- |: J3 \* a* I8 I
  17. float Ki=0.000201;
    / J) c$ C1 w$ Z' i7 G9 d; f3 \

  18. 5 `. N3 X, t! B
  19. float nPower;' h7 j: B7 |$ O5 K0 j6 ]& r7 n8 n
  20. MePort lightsensor_6(6);
    6 `: R3 r: f9 b6 V/ |. Q
  21. MePort lightsensor_8(8);+ b3 z2 e: u6 M
  22. MeDCMotor motor_9(9);- m6 k3 p- V" ^
  23. MeDCMotor motor_10(10);# F9 N( B) G/ Y- n
  24. unsigned long previousMillis = 0;2 I; g% ]' h0 ?. k
  25. const long interval = 1;) u2 D/ s2 {& x; O
  26.   p- v0 _* ~" q) |! |
  27. void setup(){9 n+ q* |$ I* X$ T) G3 n5 e
  28.     lightsensor_6.dWrite1(1);
    : t) }! f. p* _% t
  29.     nPower = 160;8 h9 c( d  l% x1 X5 p* d' c
  30.     Error=0;$ g5 \( [# U/ g) N4 K) U0 K" d
  31.     ErrorAcc=0;
    & _6 L# U5 ~0 P. [/ r
  32. }
    ( Z  G& P" S4 d) q
  33. ! Q6 _3 [2 _$ R: Q
  34. void loop(){7 P, z" E4 q! |$ S
  35.   unsigned long StartTime = millis();
    $ m8 f; E3 a5 u( ^! Z
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    * D- u( u7 e/ {& n4 v/ C  z
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    9 u0 l9 z$ Y1 o( B. e0 o  b1 U
  38.     ErrorAcc +=  nError*Kd ;# \: g+ M/ w: {1 \* `4 S/ e. k
  39.     ErrorDec -=  nError*Ki ;2 B: P0 L9 P  Z3 u# i# }0 D$ z
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);' T* y3 m, h. o1 z6 U! R
  41.     if(nError < 80 && nError > -80){: N, ^' d$ s/ C0 j: j; Y
  42.       if(nPower < MaxPower){% o) e" t2 c; q, h) H) X
  43.         nPower += 3;' F3 W( u7 c3 S# J5 m5 E& K
  44.       }% k2 Z2 n- y, n9 x8 p
  45.     } else{
    ( E, e8 D" J  Q6 \' Q  J, {: `
  46.       if(nPower > MinPower){# o0 q7 ]. U& O' L( r2 u
  47.         nPower -= 2;$ r, Q: X# ^$ k! O" F
  48.       }
    , }2 Z) m9 `# r" z
  49.     }
    ; v0 {/ E" ~% `7 L! T
  50.     MotoL(nPower-Error);4 C- c( m7 h2 N8 F. P1 c/ r2 L$ D1 j
  51.     MotoR(nPower+Error);   
    3 [; K" k3 N/ X' O9 {  w
  52.   }else{3 q% B/ x& c# l. X
  53.     motor_9.run(0);3 R5 m* I6 N4 {2 V; X: O2 K
  54.     motor_10.run(0);
    8 Q" C. x: L  s; g. y6 o- N3 a
  55.   }
    / h( ^' x2 h; f. `+ l2 }
  56.   do{}while(millis() - StartTime < interval);
    1 F- g  q  C* O* [
  57. }# P+ i8 V8 j2 r' a
  58. 1 Q2 m$ z! I! s- r/ g
  59. void MotoL(int Power){0 `4 C8 g7 G' z# ]
  60.   if (Power > MaxSpeed){
    " Z- y& }6 w( p' O" T7 U
  61.      Power = MaxSpeed;
    ) o" V3 X5 h$ \( k8 ?8 G+ ~
  62.   } $ k2 x3 _+ R( R0 T  C% ?: v+ G
  63.   if (Power < -MaxSpeed){
    ) f& f% A6 }$ u- T, f+ M3 ?
  64.      Power = -MaxSpeed;$ [9 ?( |: }0 r1 E+ S
  65.   }
    % S+ B9 N# J5 C: l9 y! }) u
  66.   motor_9.run(Power);, {- ]& [2 m3 ^, T" y, u
  67. }  
    * D( C1 b6 s( k; I

  68. ( C) W2 @9 h1 p$ Z: [/ I- l" e6 H9 c
  69. void MotoR(int Power){
    6 }- g" l+ d9 p" a3 N9 @5 R( t
  70.   if (Power > MaxSpeed){' e" p" ?* Q* |' b/ V* |
  71.      Power = MaxSpeed;
    0 q; |3 r1 v5 I0 M* Z
  72.   }
    5 k4 P2 J7 P: ~2 ?1 d
  73.   if (Power < -MaxSpeed){
    8 v! V; y1 c, F2 E
  74.      Power = -MaxSpeed;
    # p( _" k. Q* h. k. z- D8 e* B
  75.   }
    / e% }# v' L1 S% A% M6 M
  76.   motor_10.run(Power);4 j, J7 \3 c. S3 S# ^  S
  77. }  
複製代碼
! o' ]1 V3 l: I2 k

/ |% {* t3 S' _& i5 o
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯
+ O/ z5 E4 M' k1 d& Y* c
. t3 H8 j+ C" W! @0 Y  G您好,不知是否能向您請教。
: W+ G0 i# {. y/ h1 P- ^* d目前和宋修賢老師在處理Ardui Car8 K+ h: N) ^6 o/ [9 b, ]5 ]
雖然已使用較繁雜的方式處理了跑出黑線外的狀況! v1 ^* g: n2 ]

$ u, x& q9 w* h/ X  X1 }& X# }  }' q但基於想追求更精簡的程式所以還是想請問一下
$ C) @. P7 z5 x' B4 C% g% j就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝/ L9 J7 M% k. O8 `- J) B
不知道您是否願意教我可以如何處理
8 d$ L. u- T3 d- X# _1 C5 [/ y
- @1 O6 D% U& _, a( N2 h( s& r2 {
5 p( M! a8 g; ^. M( ]5 ^7 {以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;' G* G( G7 u9 Q8 h1 m4 V. f$ ?
  2. double CNY70Val = 1000;
    # h( S. V. r8 Y! w4 h' ]
  3. int interror = 0;6 g  S' v$ t+ T/ v2 F9 r; S
  4. int olderror = 0;
    ; v, W8 Y5 Y: t, v# ~2 Y
  5. double values;
    / {3 P9 ?+ m" E% R  ]

  6. " B9 N% e" r+ @: G+ s6 ^" _

  7. ) ~) U$ U8 P# v
  8. void CNY70()
    7 Y& t( o" R: l/ u* u" m  g
  9. {8 C0 p* m; F" l5 Q  e+ U' z
  10.   valuesRR = analogRead(RR)
    $ m& X! e# b0 G! W- O
  11.   valuesMR = analogRead(MR);' q9 O/ t5 G" @
  12.   valuesMM = analogRead(MM);( y+ s  s0 n6 D! s
  13.   valuesML = analogRead(ML);
    + \: l) n+ i. C. s' ~; a
  14.   valuesLL = analogRead(LL);" u) s2 l7 p  R

  15. 5 t7 u9 \7 g1 U% s2 J
  16.   if (valuesRR > CNY70Val)9 s4 v# v! D/ X' ^- p
  17.     valuesRR = CNY70Val;
    & x$ y6 h% Q2 K  S# z2 W% A1 Z
  18.   if (valuesMR > CNY70Val)
    $ z# {6 f( d. T8 b  G' p2 N0 c
  19.     valuesMR = CNY70Val;- D+ W' K) F0 A( h& `1 ?: k+ B
  20.   if (valuesMM > CNY70Val)
    7 U( L- e5 ^" E7 P3 j5 U1 G
  21.     valuesMM = CNY70Val;
    ' Q- {4 x7 C% J  Y5 _. d" N9 _
  22.   if (valuesML > CNY70Val)( m2 [# J& q3 }: ?1 m+ o
  23.     valuesML = CNY70Val;
    . e/ u6 ?5 ^# B% j/ p3 ]/ {& b
  24.   if (valuesLL > CNY70Val)# R/ M0 j9 q' O. J
  25.     valuesLL = CNY70Val;6 f' J' D( N$ w
  26. - Y$ |9 l# s! q% l/ q
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    + i' f/ Y# k, q7 C8 s3 `8 }
  28. }
    6 ]* \9 x/ b# Z- d- K

  29. 9 ?3 r' v. U, T( i# p
  30. void Car()
      U& p% ]' a  G8 Q. l6 C
  31. {9 p( U  i6 G* C
  32.   while (1) {2 [* G. R3 t0 ^- v
  33.     CNY70();
    . O$ ^- e5 v$ N7 B, T9 F* x

  34. ( N7 X' S+ u4 I: {; X# B! V, l
  35.     int error = ((int)values);, g# \3 F- T0 s" O
  36.     interror += error;/ _* s( |+ Y: W" [4 a& Z) w
  37.     int lasterror = error - olderror;
    . {2 {( B# v( e
  38.     olderror = error;. u, v' ]4 L) N% k; v
  39.     int power = error / 5 + interror / 10000 + lasterror;) k! N; `2 I& e5 R# M7 A; L! x0 S4 H
  40. " ~0 G7 Z; X0 p  Z  n
  41.     if (power > MotoSpeed)4 y2 e/ Z  q) b8 ^& F
  42.       power = MotoSpeed;2 c" N& `' M* I7 r# q
  43.     if (power < -MotoSpeed)
    # {& g# a, |9 q) B- z" u
  44.       power = -MotoSpeed;, ~3 O* I6 ^# `" z& ^. {
  45. * r; M) i. J4 q3 s
  46.     if (power > 0). r6 l$ R. s  C
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。
    * ?, N# T6 m  f5 ]/ W. M( N
  48.     else' u: Y' k9 c! [) z( M
  49.       Speed(MotoSpeed + power, MotoSpeed);7 y0 l% ^2 E  b
  50.   }
    . t5 v+ q* R0 N7 F
  51. }
複製代碼
- V8 |* H, f1 R( X
' L. `4 z: p/ }! Y% d/ I# `
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-12-5 15:55 , Processed in 0.022864 second(s), 17 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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