圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21027|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>
    ( Q  y) Q6 x0 r* s
  2. #include <Wire.h>$ K5 O. z4 c- Q3 _! x0 C: d% Q3 K8 B
  3. #include <Servo.h>
    2 p3 V9 C: f8 L$ T

  4. / K/ y7 X( [* a: Q6 k
  5. #include "MePort.h"! z8 k! t5 q4 Y8 _, a
  6. #include "MeUltrasonic.h"" s2 d+ e* k" ]% h: E
  7. #include "MeDCMotor.h"
    ' k+ p/ d6 g, E& J9 f; \& J- R# i" f' M) r

  8. & }% I) V+ p2 I4 V: o; I
  9. //double Input, Output ;
    % P; N5 V8 b1 S- X( U
  10. float MaxSpeed = 255;
      r3 {) C  x+ v2 H8 J" b
  11. float MaxPower = 180;
    0 M& y& d; |$ \$ ^
  12. float MinPower = 120;
    . X' ?( h2 Y4 x5 _; R, L
  13. float Error,ErrorAcc,ErrorDec;* G; n/ G+ K- \
  14. : v/ E" \3 n+ L' Z+ z
  15. float Kp=0.14;$ y8 Y7 i5 L. J
  16. float Kd=0.00020;//23;
    ' X) J8 W! c/ `$ O" K+ [0 Y
  17. float Ki=0.000201;
    / I8 u: I9 v+ ~8 z3 t0 `, p$ z4 Q
  18. ! m1 }. U: O, I9 \$ A; f& T
  19. float nPower;
    - t) L7 b, }% ?) ]2 s
  20. MePort lightsensor_6(6);; P, ?9 V4 y2 z- p. p' }; I. u/ O& f
  21. MePort lightsensor_8(8);
    1 b$ y) v$ ~  H
  22. MeDCMotor motor_9(9);
    $ f" p1 x5 p# T, u% S$ p
  23. MeDCMotor motor_10(10);
    , p* P+ K) n, f9 a5 O
  24. unsigned long previousMillis = 0;
    + u, ~7 Y  f4 v# _; g" X
  25. const long interval = 1;- h3 a* a7 V4 x- G, R5 ]* ~
  26. # N( V6 x( Z) F$ W# |' b. G
  27. void setup(){" e. ^/ F3 S0 h$ h# _
  28.     lightsensor_6.dWrite1(1);; `- d$ s. t# l3 n, n
  29.     nPower = 160;
    $ n8 P* n* o: x! x: O( G8 ?# w3 h
  30.     Error=0;
    $ Q0 S' E6 G) O  n: [- m+ s! h
  31.     ErrorAcc=0;+ Q" V. H* n' b
  32. }
    6 R! z/ j& G) M) Y5 R0 e
  33. + b$ s/ x$ F3 Z3 ]
  34. void loop(){
    . f) E4 d. v4 T3 f6 S
  35.   unsigned long StartTime = millis();
    0 T8 B) D5 f' |, i' h( q2 w4 g5 H
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    5 \8 @5 G3 J2 U6 ]4 g- {5 Y
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();$ {9 x$ [$ j9 {" m" W, |
  38.     ErrorAcc +=  nError*Kd ;) H+ G, Z; M9 O% u2 W
  39.     ErrorDec -=  nError*Ki ;
    3 S7 l" h+ `3 ^  b/ q
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);" d' D, R; P- J0 ]$ g& O+ @/ n
  41.     if(nError < 80 && nError > -80){8 W8 R. J' P  S/ y  @  `; z) V( F
  42.       if(nPower < MaxPower){
    0 s( N2 m3 W1 _" q  Y
  43.         nPower += 3;1 j, x9 [2 B5 A" K: ~
  44.       }9 d; B+ Q* M  O. K) ^+ D) l1 h. y. n
  45.     } else{
    ' l# F! D( Z' j4 I
  46.       if(nPower > MinPower){
    2 U: _0 z. {1 N" d
  47.         nPower -= 2;
    5 e; c+ O5 ]4 C& w3 ?5 `
  48.       }  |) [" _! w  X6 z
  49.     } & x0 n" n8 T3 Z& ~
  50.     MotoL(nPower-Error);
    4 [+ Z7 |) O0 ?3 F
  51.     MotoR(nPower+Error);    ! i, t7 `8 `+ N, s; c
  52.   }else{
    2 Q4 h0 k# u1 i1 y( O
  53.     motor_9.run(0);
    $ Z$ Z1 D9 d( ^) u* E. l
  54.     motor_10.run(0);
    5 n' b) n; t( s+ `" ?
  55.   }& \4 c. {: U5 _$ X% }
  56.   do{}while(millis() - StartTime < interval);
    6 r6 o% C/ d; J: _) Y% Z7 `
  57. }. f( N+ @# t- V7 C- A) E

  58. : L- ^5 G( ]  _" H3 `' T  Q9 {
  59. void MotoL(int Power){
    + I3 v: w0 x6 p! g
  60.   if (Power > MaxSpeed){
    9 F6 M9 M, ^' a$ Q  X' A
  61.      Power = MaxSpeed;- n% p: K9 m5 d- i+ W/ m+ I
  62.   }
    * Y' z- o4 s, h5 R/ n8 i
  63.   if (Power < -MaxSpeed){
    # Z3 S1 I. N) N. H% ~
  64.      Power = -MaxSpeed;8 ]; I2 F7 e3 J0 P1 j
  65.   }
    0 S& O; ]' L0 o) c+ q
  66.   motor_9.run(Power);
    ) A- b0 i( o# ^4 _, I
  67. }  
    # l7 Y7 f, t8 a" r) Y) l( M

  68. 7 y5 U1 l* s2 ]- {
  69. void MotoR(int Power){, |- M; Z+ N, V; Q
  70.   if (Power > MaxSpeed){% s; z' b9 d# ^( N9 x0 O! s
  71.      Power = MaxSpeed;$ c) I: S, X: H6 m, @" K5 q: d+ D% ?
  72.   }
      s8 ]* _, f7 D
  73.   if (Power < -MaxSpeed){
    * t) Z. O* W  a
  74.      Power = -MaxSpeed;
    8 [+ \( c8 D# h, [; j
  75.   }
    7 c: C  c! R* u9 }, Q+ ?( r( L' W
  76.   motor_10.run(Power);5 `$ C0 G4 \: n
  77. }  
複製代碼

$ R5 |9 T2 K+ V& G
- A1 U1 W" g/ y5 u' h( \% V0 V
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯
1 f. Y, e" E0 J- B3 f9 M9 S  i3 |) T; R
您好,不知是否能向您請教。& F$ g, M% {" X1 Q4 x
目前和宋修賢老師在處理Ardui Car
' T( G& N" D6 `. F2 Z# f雖然已使用較繁雜的方式處理了跑出黑線外的狀況7 A! h- S# A' ^. S& E6 {

, h/ C; b/ g" [* O, m9 r" a$ c但基於想追求更精簡的程式所以還是想請問一下
. u! b5 O' _8 I. \' g* g% W) X就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝
/ q" s/ J4 Q+ Y, ^! T9 P不知道您是否願意教我可以如何處理
5 w6 l7 |7 D) A) F
7 B2 w: W/ B1 x3 }6 ~0 s+ ^2 W2 _7 t8 w+ z
以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;/ ]( A4 [) J8 Q; b. w
  2. double CNY70Val = 1000;
    & m1 z/ j% C# H. b9 `
  3. int interror = 0;
    4 g2 }# t1 x* Y  s  l
  4. int olderror = 0;) a& S8 H: W3 \: M: u  b# b
  5. double values;
    2 ?% r! B9 G0 f# }

  6. # r3 C# p  Z: w( L2 L

  7. . S  I4 p- _% D5 D
  8. void CNY70()" V8 M! ~  \3 R% o2 `" J
  9. {
    6 J+ Z2 _0 z5 ?8 s% P
  10.   valuesRR = analogRead(RR)
    ( y3 j7 |4 e" k( N! b
  11.   valuesMR = analogRead(MR);
    * X; F" J7 t; O7 Y
  12.   valuesMM = analogRead(MM);* ~/ d+ l% q" a" w8 \0 n
  13.   valuesML = analogRead(ML);
    7 e; T! ]1 \  Y# t
  14.   valuesLL = analogRead(LL);
    0 _' t+ q2 e( T: ^

  15. ' w' O' z; e6 m) U, }  w2 h
  16.   if (valuesRR > CNY70Val)& ^; [  d8 @0 j* q. r  b
  17.     valuesRR = CNY70Val;
    ) i1 O$ t1 \) c2 P; V5 S# U
  18.   if (valuesMR > CNY70Val)
    6 l9 w5 x5 i. g& |7 W- L) N
  19.     valuesMR = CNY70Val;' ^( b$ r4 ?/ q  ~7 h
  20.   if (valuesMM > CNY70Val)
      \, [, v* G9 l1 E3 k) i; r* e
  21.     valuesMM = CNY70Val;. b, C: E. }9 R
  22.   if (valuesML > CNY70Val)$ {; Q6 B! P* ^1 U+ e' n$ ?
  23.     valuesML = CNY70Val;
    ) T# h# w5 U" c# p+ A- z# _" b9 C4 s
  24.   if (valuesLL > CNY70Val)+ k: c% B3 s9 f: m& {5 Q7 i" Q
  25.     valuesLL = CNY70Val;6 E. l6 K6 E: @0 \

  26. % Q* p% _# B9 F) v5 u! d& C- W
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;5 b9 e& {- d! g1 \3 P
  28. }( ?* _% _: |; D$ E0 }7 i

  29. ' Q. Y/ U7 q, h; b  n0 V8 K1 f- B
  30. void Car()+ n9 c) }% A+ Y0 s9 d
  31. {
    0 w( l! J4 _$ F" M
  32.   while (1) {
    , w8 `# v) N+ V
  33.     CNY70();
    3 U6 w' m, `6 `, Z* D
  34. * X- q! M- t( ^- ^4 Q
  35.     int error = ((int)values);
    7 H8 }; k2 q! x- Y: V
  36.     interror += error;
    6 @) V' E2 N4 l1 q
  37.     int lasterror = error - olderror;/ J" E! M' u' _" F3 @" d
  38.     olderror = error;" f( O" N) u* l8 x
  39.     int power = error / 5 + interror / 10000 + lasterror;! G+ \" |. Q" v

  40. 5 ?" n5 N! v2 E9 ~" s4 C8 C" j
  41.     if (power > MotoSpeed)
    2 B) P- c0 k2 |: I' ^' @
  42.       power = MotoSpeed;# \% M, B$ v  q: T+ L
  43.     if (power < -MotoSpeed)6 ^: a) J( w' h$ k1 M# B  p
  44.       power = -MotoSpeed;
    ! }% {) t. w5 L9 m! F
  45. ' [  v: J0 B) X: q9 w9 l
  46.     if (power > 0)
    ) a5 V" q! S, F) N0 o
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。) A* T' N1 P" s! T: Z# @
  48.     else
    2 J1 P) S' t. n& T) _
  49.       Speed(MotoSpeed + power, MotoSpeed);9 q, _( J$ G* {# S2 d: T  ]( r. T
  50.   }
    . Q4 ^% S0 F, o, |9 D$ v
  51. }
複製代碼
  j, w# E( \/ }" a/ q
1 m  T( d. P+ l
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-7-19 06:55 , Processed in 0.027570 second(s), 17 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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