圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 11744|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>
      C! f. J$ J4 j: n! P) L* W
  2. #include <Wire.h>% S) F$ L0 K5 {8 I# B' Q
  3. #include <Servo.h>9 U3 s3 w$ E% z8 C# _  @

  4. $ W& S  Q' Y) R
  5. #include "MePort.h"
    * U) y4 [! D' `1 y( w5 @1 S$ \
  6. #include "MeUltrasonic.h"
    . N, C  l( J/ N4 b4 u
  7. #include "MeDCMotor.h"
    7 p: Y- Y7 p' @( T- a4 a
  8. ! l) i% S! J  j9 `; z  T. T
  9. //double Input, Output ;+ ~4 W8 f1 X/ s
  10. float MaxSpeed = 255;
    # E. R! ]5 Y5 G* o, }/ i& ^
  11. float MaxPower = 180;
    ' F- X  z+ }4 z
  12. float MinPower = 120;) n3 @. w1 n+ h( d7 B7 ~
  13. float Error,ErrorAcc,ErrorDec;6 y5 ^: w7 _8 K4 y, v! D- ]  D4 C

  14. 6 F: f: D9 ~& ~# E/ R5 M
  15. float Kp=0.14;
    * N, s1 c4 d- c9 X" H% a* I
  16. float Kd=0.00020;//23;8 u2 A# B) ]' _9 p. }9 N2 I
  17. float Ki=0.000201;
    & e4 l! @! e4 {2 ]) t
  18. / D/ ^1 X* Y; S0 E7 `
  19. float nPower;
      h3 S7 \8 F  G$ V( m$ P
  20. MePort lightsensor_6(6);0 U; [2 I- |) V7 O5 c( X+ o: a  B
  21. MePort lightsensor_8(8);
    1 q6 z* d$ C+ \. x6 y5 w* d
  22. MeDCMotor motor_9(9);
    ' v% U' B5 p$ E" F/ j
  23. MeDCMotor motor_10(10);3 t8 x+ B6 q' W8 z: ~
  24. unsigned long previousMillis = 0;
    * l$ S. S2 |( ]/ N. F9 c- B( A
  25. const long interval = 1;1 T7 |. |9 n# D) n, A! P/ t

  26. 2 h! _8 ~  [) @  ~2 ^" P
  27. void setup(){
    0 o$ _) z( i, k: O. f/ p  ^, x4 B
  28.     lightsensor_6.dWrite1(1);
    - A3 y7 A' m# }8 F  X: o
  29.     nPower = 160;
    " t6 D! i* w5 u9 Z" ?1 Q( K
  30.     Error=0;
    : H7 p) p6 C. Z( E( G9 l& `
  31.     ErrorAcc=0;
    % b6 \) M+ N6 F
  32. }
    3 a) a& ]) R- F) h$ I

  33. ( R+ B3 T' R& C( P
  34. void loop(){0 ^& l2 A6 N  c4 [7 p8 T
  35.   unsigned long StartTime = millis();% ^% ?, a3 _/ H1 P6 X  ^
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){5 Y2 C/ R* G; Z8 }/ \3 x2 V
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();! T: s: q( S5 q/ k1 P
  38.     ErrorAcc +=  nError*Kd ;$ X/ W1 h' f3 O- C
  39.     ErrorDec -=  nError*Ki ;7 l1 A1 m- \! a; U4 f
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    & g* z% Q, b' ?
  41.     if(nError < 80 && nError > -80){
    " v6 C- @* h( u" m+ K
  42.       if(nPower < MaxPower){' ]) R8 g7 a( c7 V1 p8 O3 o' j- p
  43.         nPower += 3;
      O1 i. R( N& Q5 m9 S8 \
  44.       }0 C$ a0 b! ]: J% L) K) g  C
  45.     } else{
    * t. ~- n$ [- V% W
  46.       if(nPower > MinPower){: l* t$ f% k  \1 l% S' O% d
  47.         nPower -= 2;
    & d: C( o% U# {" t2 l1 V
  48.       }
    6 j# _9 d" }: R- H
  49.     }
    $ z8 b# _+ Y$ X
  50.     MotoL(nPower-Error);
    & v/ ]2 w3 O. z
  51.     MotoR(nPower+Error);   
    1 G) K7 u# I8 o$ e4 r! `6 i
  52.   }else{
    & v' z1 G; o0 I5 u
  53.     motor_9.run(0);7 L* Z, b% U) k' J+ p9 k2 p  f. W
  54.     motor_10.run(0);
    " [3 M; }! i; i
  55.   }+ l1 l, P& K8 ~* z" F
  56.   do{}while(millis() - StartTime < interval);
    1 x( F/ \: g( N+ r3 ^- q/ s
  57. }
    1 U# r+ v7 r- s, x

  58. 3 A: a( k# r9 d& ?/ a  f
  59. void MotoL(int Power){
    + v) X( {* M: d: Y2 F* s: \' S+ \
  60.   if (Power > MaxSpeed){
    3 ?+ q, A5 E& Z& X( T$ W
  61.      Power = MaxSpeed;
    6 g* o9 g  K9 ^, l
  62.   } ) }( T: m! _- g
  63.   if (Power < -MaxSpeed){: T' ?7 W8 h5 e" g/ S% g' Z
  64.      Power = -MaxSpeed;
    2 h: g. e, y. I  A% R1 L
  65.   } 7 V, l2 G4 h1 A- N
  66.   motor_9.run(Power);8 E2 q3 h8 z: N/ r3 o0 i8 d0 _
  67. }  
    * k: I# u0 R/ y9 \; {

  68. ' s" H# T, f9 l3 ~0 e  e
  69. void MotoR(int Power){$ R" M$ W9 ]; S
  70.   if (Power > MaxSpeed){4 p9 u; H: j$ u
  71.      Power = MaxSpeed;
    8 N  i1 H) q; t) e5 s
  72.   }
    3 r5 r2 j" a5 ~( ]
  73.   if (Power < -MaxSpeed){8 E1 F% h! m# U7 s( p7 r
  74.      Power = -MaxSpeed;
    & ~' d/ y. G+ d' a  {) V( Z
  75.   } ) t: [' j+ Z7 \0 V
  76.   motor_10.run(Power);
    7 y. L6 t) H2 o% _- j6 R
  77. }  
複製代碼
9 ~5 r5 _3 v' P8 q* K

$ v) w+ u, j' \: v0 h( Y
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯 $ l) \8 ^! `; O  x6 f1 {
, Q: q/ s, T. G  Q. i% D" F( T" @
您好,不知是否能向您請教。. n1 n  g, z$ H
目前和宋修賢老師在處理Ardui Car
5 [5 u9 U6 g9 {* U# y9 _3 Y雖然已使用較繁雜的方式處理了跑出黑線外的狀況3 K2 u" k* Z! L' Q! R& N. j
- [' u1 k* ^- P- A" H1 V
但基於想追求更精簡的程式所以還是想請問一下
6 t- `- ?" g. J9 L3 x; D就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝
) P0 w) q9 n; b' g$ P- C不知道您是否願意教我可以如何處理0 h  o0 @# m3 Z" v* s  `5 \
/ G/ ^+ T1 P" B( d
& Z( D  A% w3 f# g
以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;% u, L: Z. k6 d  q: N* W
  2. double CNY70Val = 1000;
    3 c' C/ P0 e" p' }1 K3 O! e
  3. int interror = 0;
      C; ~% \8 G3 N$ k5 s
  4. int olderror = 0;  Y3 D5 J2 [* e% L9 w/ g' e* g# \
  5. double values;. K2 |% N) ?9 k7 y/ X

  6. ) E- r( T; S5 j, y: n
  7. 0 q7 h2 S# ^4 g1 _* c' I! w4 h" o
  8. void CNY70()% _) ~* }5 b0 P( r5 J" H
  9. {
    , j- |; x2 z1 I& I* L
  10.   valuesRR = analogRead(RR)) k9 L; j, m7 i$ v! V+ N' c
  11.   valuesMR = analogRead(MR);( W" A5 ~6 l2 W* S  k$ H
  12.   valuesMM = analogRead(MM);- N, V- s' i7 T5 K# i5 V
  13.   valuesML = analogRead(ML);0 x$ U3 f  {. `2 a
  14.   valuesLL = analogRead(LL);/ R5 m+ z3 Z& F, {7 G# D7 z" F

  15. 5 z4 i, a0 a7 j8 B# o2 ]* b
  16.   if (valuesRR > CNY70Val)
    . ]7 D( b& F. n  }4 S. R, T  S
  17.     valuesRR = CNY70Val;
    6 ?5 L5 [/ i+ L2 X  b
  18.   if (valuesMR > CNY70Val)! c9 E9 n  `4 A7 S) }. M1 u
  19.     valuesMR = CNY70Val;
    6 C$ o% p5 c$ Z5 ]; I
  20.   if (valuesMM > CNY70Val)/ O( L3 K+ ]9 j! Z6 g
  21.     valuesMM = CNY70Val;& q% \& v" Z/ v* l- @) a
  22.   if (valuesML > CNY70Val)% X# n( a% s- h: c: @, e4 u3 m& @7 ?
  23.     valuesML = CNY70Val;7 v. ]: h2 [, @7 a8 V; F" e
  24.   if (valuesLL > CNY70Val)$ S5 P0 U2 h" R6 V4 s
  25.     valuesLL = CNY70Val;6 v2 t( k  b5 T8 i
  26. ) _' f: v. o; O9 m( A
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;$ r# m8 |# A8 G% C
  28. }
    $ Y/ y8 P5 ]9 s3 ]

  29. 6 ]# E. q( p! A5 Z5 |
  30. void Car()7 C4 x2 D3 q& T% h' \. C1 p
  31. {+ ]0 ~  W  S' e; E  \6 r
  32.   while (1) {3 q- D, f2 j' b# E6 T2 K, o
  33.     CNY70();
    ; L7 ^) i' N3 B) ^, `/ }

  34. " U) q: H1 V3 G' u5 w, {5 Y3 h
  35.     int error = ((int)values);
    1 p9 S( g% B7 ]6 F: y9 ~  e
  36.     interror += error;: a$ G: e) j: h& D! p
  37.     int lasterror = error - olderror;
    ; d6 {9 B' }- r8 a
  38.     olderror = error;  i! X3 C2 {: e
  39.     int power = error / 5 + interror / 10000 + lasterror;
    + i  G  g4 x% w% S
  40. 9 F/ d2 L5 u9 \
  41.     if (power > MotoSpeed)) `+ r" `/ ]) G1 z% d4 Q
  42.       power = MotoSpeed;
    " X+ a6 @. |& O5 Q6 g
  43.     if (power < -MotoSpeed)
    ) t6 q& K. u, I% I
  44.       power = -MotoSpeed;
    * N+ n! Y. n" O$ H6 V1 X6 r

  45. : }# t7 i- x" G+ B9 U8 O
  46.     if (power > 0)) {0 w* j1 {  f# o' k$ n
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。% h  {7 L4 k' @- s
  48.     else
    4 l4 i6 Q* h" b2 F! H8 z
  49.       Speed(MotoSpeed + power, MotoSpeed);
    - N4 |) F: [. @  g6 n6 ]
  50.   }7 ?5 z, g5 s# S9 B7 q
  51. }
複製代碼
! Y1 S( u1 \; P" h7 R9 w6 T
$ b  d, b% _6 a. S6 g* G( K2 }3 v- x
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2020-10-26 13:01 , Processed in 0.027384 second(s), 16 queries .

Powered by Discuz! X3.4

Copyright © 2001-2020, Tencent Cloud.

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