圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21820|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h># ~; y* M/ C' b
  2. #include <Wire.h>
    , N/ N3 p% R1 |- x: v/ ~
  3. #include <Servo.h>
    / v( P7 K6 \  d) ~" K- B$ P

  4. 7 r3 r# H" i" z+ j  |! T% e5 [0 n
  5. #include "MePort.h"
    : F4 d, v* j, Q$ e1 J4 O! ~7 U
  6. #include "MeUltrasonic.h"/ J. i+ m2 r- j. P+ ~! w" T
  7. #include "MeDCMotor.h"
    , z" s# i( E- |+ e  P* E  m
  8. % q8 H' F( u( `+ Z3 Z) h
  9. //double Input, Output ;
    / P5 M0 S) O: p( u( O$ e
  10. float MaxSpeed = 255;
    0 X  B# ?1 L0 U3 |: [
  11. float MaxPower = 180;
    / H# }  e1 z3 e: ~
  12. float MinPower = 120;5 k! d3 ?7 p6 ]" [% z
  13. float Error,ErrorAcc,ErrorDec;
    4 a6 M* w; i  C* A/ u+ a

  14. 1 u: C9 |& c3 v
  15. float Kp=0.14;- N8 [2 k: |# m8 s% V7 k
  16. float Kd=0.00020;//23;
    # g: ~, z; M2 y/ G
  17. float Ki=0.000201;! ?% o. W& Y+ K0 J
  18. 2 D7 H; ?' A! I6 w- X  s
  19. float nPower;
    2 e* K' A. a+ f9 }+ j% E2 y
  20. MePort lightsensor_6(6);
      T* M6 {( y4 T' l( Z- x
  21. MePort lightsensor_8(8);7 x; ?, x4 P; y& g6 A& ^
  22. MeDCMotor motor_9(9);& G: R+ ]# |& H. R& E* p+ A
  23. MeDCMotor motor_10(10);
    ) F( A$ ^' }* F1 P& a# M7 B
  24. unsigned long previousMillis = 0;
    3 Q: ~* `" ?% l# s% E1 t5 {
  25. const long interval = 1;
    * a9 k  U9 E4 L7 s. n% \: }
  26. + a/ Y8 x+ x+ ]: Z0 E8 M( v
  27. void setup(){" y7 r$ A+ V& {& }" H# K3 |- |0 t( L8 F
  28.     lightsensor_6.dWrite1(1);
    " Q) e1 j7 B# L( G8 g) t1 W. I/ }
  29.     nPower = 160;
    4 y: \  C( K9 V0 e
  30.     Error=0;
    , ~8 E9 \9 ]1 Z$ L9 m: x' q5 Z
  31.     ErrorAcc=0;5 q, J' n% ]; k- h1 s4 C. q. v
  32. }
    1 g! ?  r6 \6 V% Y
  33. / L$ ~" \# g5 R: C
  34. void loop(){
    & I$ T- \8 l/ m/ G( J. g# M
  35.   unsigned long StartTime = millis();
    5 {+ v' d; B6 i: Z2 i$ R
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    ' `! a2 m* o, `: s% D; G
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();, Z+ ^2 a( V$ s4 V+ J0 F5 T, K
  38.     ErrorAcc +=  nError*Kd ;5 N; |: X& _7 k; R
  39.     ErrorDec -=  nError*Ki ;
    ' x2 d9 B: h* \2 S2 [
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    / H- V. ?& \' w$ U
  41.     if(nError < 80 && nError > -80){
    ; Q2 I6 q" P" z% Q0 Y( @
  42.       if(nPower < MaxPower){
    6 z$ L. B7 D7 l0 A3 S8 x- O2 T
  43.         nPower += 3;/ r* D8 C3 O- ]5 W9 W4 Z8 ~& B
  44.       }( @+ q* V" t* z- r% x( `
  45.     } else{
    & \, N) b* H0 \% C; r, B+ o& Y
  46.       if(nPower > MinPower){
    & u- `4 N% H) H$ k  g
  47.         nPower -= 2;! V) |3 s! Q  R
  48.       }
    7 U! c2 @  @; u( ?6 l
  49.     }
    1 s& r5 j% s3 l7 m  b* N
  50.     MotoL(nPower-Error);
    & M( p1 M4 N1 j" n1 X
  51.     MotoR(nPower+Error);   
    - [" h' w, }/ ~% \, }0 y3 X
  52.   }else{; R5 ?5 |# T* P, G( X" n# |4 m
  53.     motor_9.run(0);2 o* W2 s- [4 e7 N
  54.     motor_10.run(0);
    7 k! x8 [' k9 `
  55.   }) I! \# b7 x8 ^3 g
  56.   do{}while(millis() - StartTime < interval);7 N5 k7 l! K; G' i! k& C1 B
  57. }
    & v: p) C5 ~; L

  58. ' t! ?, g  s% [+ N8 s
  59. void MotoL(int Power){6 \5 K( ]4 o, U) k
  60.   if (Power > MaxSpeed){! q+ O( o: I( B- B8 J2 z
  61.      Power = MaxSpeed;
    . x$ a9 _9 n# i6 P9 @
  62.   }
    9 [1 o. k, e9 g7 a/ [7 C# G
  63.   if (Power < -MaxSpeed){
      [/ E, x0 M4 s: I( ]$ a
  64.      Power = -MaxSpeed;; g2 W' |- r; R* q! ^
  65.   }
      x/ W% ~& X4 X+ H. g
  66.   motor_9.run(Power);7 I& c1 D7 c; f+ L$ K0 r
  67. }  / Z8 V8 e! ?- q" `: o6 Z

  68. " P+ d7 d- P& ?: I4 B+ j" d" ~
  69. void MotoR(int Power){
    % i4 n! n; [7 o' {4 V* E; N
  70.   if (Power > MaxSpeed){$ k/ t* B/ `  {% q1 |
  71.      Power = MaxSpeed;; B" s& S, i2 D( ^0 V& W
  72.   } 6 r- ]- m/ ^- U& U
  73.   if (Power < -MaxSpeed){
    ; S/ i4 @. D; ?9 Z5 \; F5 {
  74.      Power = -MaxSpeed;% K0 }3 {5 Y- ^  B' Y
  75.   } 0 J" T  R" P3 ~% T
  76.   motor_10.run(Power);; B- A" `  H6 r' P
  77. }  
複製代碼

) ?4 s3 p# w  J2 y# }6 Y
+ F  N& ^, _3 X% D! |6 I' \+ y. |2 v# O
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯
5 X+ q! h5 d! p, ]8 z( W. j8 V7 e) m2 t/ X5 M
您好,不知是否能向您請教。2 v2 Z; k2 I5 Q, i; @+ k
目前和宋修賢老師在處理Ardui Car
* _; j9 H5 ^# \; B$ I8 `雖然已使用較繁雜的方式處理了跑出黑線外的狀況
/ i0 ?. {- ]+ N; p9 b
. ^2 F7 F; {" I5 g: L$ [. I但基於想追求更精簡的程式所以還是想請問一下. j% C3 ?, _0 f3 W
就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝
$ j: r/ j, W  w7 P不知道您是否願意教我可以如何處理9 ~+ i4 Q2 C8 H% U
# D; @) j- ~  }$ w5 S, h

4 U3 V3 p( b) t# q5 }7 N% Y以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;: e+ [8 H% ]  z
  2. double CNY70Val = 1000;  P- ~" \$ M. U/ t7 H. s( {
  3. int interror = 0;
    3 F; ?- e% h$ Y, a, g5 I
  4. int olderror = 0;: i6 W( h4 k$ h8 }8 o$ m  F, B$ s
  5. double values;
    " [. f2 _8 S1 o5 B% J
  6. 2 K7 ^( A2 {9 v
  7. : K5 X/ ]* I- \2 G4 T
  8. void CNY70()& ?5 a/ n2 t4 D& |8 |  S/ Q
  9. {! ~) P' X, }6 o5 t) M- s" m, H- \
  10.   valuesRR = analogRead(RR)7 e! j6 H6 c- [$ p' p2 \* |8 H6 d1 q
  11.   valuesMR = analogRead(MR);) g( E0 d' y, z1 A" @9 ]
  12.   valuesMM = analogRead(MM);6 b  T: X/ w& ]
  13.   valuesML = analogRead(ML);
      h& s9 T6 o$ }. P1 L7 N$ J, {' z
  14.   valuesLL = analogRead(LL);8 J7 @0 l$ M$ e  X
  15. 4 m* A4 k: ]  n  p  v0 X. O
  16.   if (valuesRR > CNY70Val)
    , p1 o7 Q- n! x. W# R
  17.     valuesRR = CNY70Val;( j* R1 |; k+ b: H$ ^9 Q
  18.   if (valuesMR > CNY70Val)
    ' z% }7 n2 d3 I5 O0 `& `2 Q
  19.     valuesMR = CNY70Val;+ @) |4 p2 K4 v' C1 D" a8 G
  20.   if (valuesMM > CNY70Val)
    % x/ v( W8 O3 x5 _' k) c
  21.     valuesMM = CNY70Val;' t( k! K/ D- X# g
  22.   if (valuesML > CNY70Val)/ S6 e% H$ d; f$ m6 e- b& i3 y; T4 J
  23.     valuesML = CNY70Val;
    7 g$ W  g) K2 F; U5 Z0 Q3 T
  24.   if (valuesLL > CNY70Val)
    . D; |  B9 h2 v: S" j0 G" b
  25.     valuesLL = CNY70Val;
    ! C2 B' b! K0 a) w. N

  26. 4 o3 Y& O) P9 t) X2 Y4 Q$ Z
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;; x1 Y& R) p' [* @: J
  28. }* m$ q3 M3 `9 ^/ h

  29. / S3 I/ f- J6 p4 A- R
  30. void Car()
    7 U2 j: B; ?* I2 @. B: n
  31. {
    0 P9 R* m  B. d% y0 }
  32.   while (1) {& i) B, W+ o! x6 ?7 c! w$ g7 N5 Q0 f
  33.     CNY70();# n# l6 E! `, V  U) r# X! c

  34. 9 Y1 [2 j1 }+ M7 g6 J1 y
  35.     int error = ((int)values);
    8 @7 v) v, A9 l/ c0 }. G
  36.     interror += error;+ h& X1 b# v2 K
  37.     int lasterror = error - olderror;. v# v% T9 a% P; Y0 a
  38.     olderror = error;, l* ]8 L  q; X% @" [2 `3 J; G
  39.     int power = error / 5 + interror / 10000 + lasterror;, b! v2 m' M: s
  40. ! x0 L" b  d% q4 v6 [; G# J0 w' T
  41.     if (power > MotoSpeed)
    ; Z$ L- l5 h7 }1 v8 }
  42.       power = MotoSpeed;4 D* n  b2 a! i9 e
  43.     if (power < -MotoSpeed)) ^+ f9 |( Z/ T5 E* q+ a& [
  44.       power = -MotoSpeed;
    2 ~* ?. M' P) y# t# q6 A7 j' n
  45. * h+ X3 R6 j* i2 j
  46.     if (power > 0)
    . ]3 o; [- ?( }  T
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。
    1 Z0 L! w9 D* c' M+ R' m* X
  48.     else" I# ~, m' @. l/ q
  49.       Speed(MotoSpeed + power, MotoSpeed);
    . S1 S# }3 H% m/ o' l
  50.   }( J: U- i: o- ^; K' G7 l6 k
  51. }
複製代碼
5 X/ o( ~, X) @8 r
7 W! s+ h, p7 w  j( M" O* e; @) E$ b
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-12-3 11:30 , Processed in 0.026594 second(s), 18 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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