圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21721|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>
    3 X2 {* q# j# ]5 ~
  2. #include <Wire.h>3 J3 z: @( o5 J
  3. #include <Servo.h>
    ' l; ^9 [9 T: C# v% A7 [# `, @$ P

  4. 5 N  l$ _( U* ~3 r8 P' H- R" X
  5. #include "MePort.h"
    ; w% Y' i+ a* o( U$ y9 |
  6. #include "MeUltrasonic.h"# j7 D% V0 `) J* p$ _/ @- F; |; i
  7. #include "MeDCMotor.h"
    1 x* x9 X  j* J1 v* X5 P. l

  8. 3 A; I- T! p" M& A" a4 |' s
  9. //double Input, Output ;
    % ]# a4 M# Q9 V% L! d
  10. float MaxSpeed = 255;
    4 r6 k4 ~( I; k# T* R+ }
  11. float MaxPower = 180;" J  n4 O# l. u+ D
  12. float MinPower = 120;4 C0 f! f' L9 `. o
  13. float Error,ErrorAcc,ErrorDec;
    # d. b; [8 ]: E2 A
  14. . f, M8 p: s% N5 a7 o  S- T
  15. float Kp=0.14;, \6 T; W3 \  o1 a3 H
  16. float Kd=0.00020;//23;
    - M  _4 X% A1 k  m0 i1 L
  17. float Ki=0.000201;) K0 q* P- J* R* P

  18. 2 f7 L# ?# z* P- d1 D: Z* I/ b
  19. float nPower;5 u* P9 R* P( M$ |
  20. MePort lightsensor_6(6);9 z! K3 g* J$ h% N
  21. MePort lightsensor_8(8);* G4 t3 A1 O* t$ e& s! I  q! D
  22. MeDCMotor motor_9(9);* b  |$ t7 e* L2 ?- F$ P
  23. MeDCMotor motor_10(10);/ \0 H9 ~8 t2 e5 o8 z1 j& l
  24. unsigned long previousMillis = 0;5 W( ]* l6 _, V6 i
  25. const long interval = 1;
    3 y5 ?! \3 _6 u9 o+ B4 z9 `
  26. " O+ h0 |6 \5 o& U8 i+ U+ z2 t
  27. void setup(){7 S7 q# F9 J) T& K* f( |/ H6 ?
  28.     lightsensor_6.dWrite1(1);
    8 O" l! n7 T2 T) c
  29.     nPower = 160;& t5 s; ^7 |: Q0 y+ k
  30.     Error=0;* U5 l  |4 J8 @0 l+ N  c7 R
  31.     ErrorAcc=0;
    % K5 \2 O& p, l" H3 y* P0 o) J7 G$ {
  32. }1 @/ h! r, W% U; {5 z% @

  33. 8 Y8 ^' Q0 Y' c; p, ^
  34. void loop(){5 k7 O" d6 G0 Z5 _; j' o( h
  35.   unsigned long StartTime = millis();
    : p0 ], U; x1 B+ M: g
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){: ], H5 F$ `& e9 b7 r0 Q0 V& R; W
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    " E$ k" n2 D4 H8 f# d3 K) T
  38.     ErrorAcc +=  nError*Kd ;
    . N5 Y0 |( g" d
  39.     ErrorDec -=  nError*Ki ;1 R4 H# L6 w( w  k
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    - I: @5 m; H' p* Y, ~6 G8 }6 f
  41.     if(nError < 80 && nError > -80){6 r$ C# U4 r1 t
  42.       if(nPower < MaxPower){
    # D: n. c$ m+ n/ s/ ^
  43.         nPower += 3;
    6 t& n% i! E$ [& s
  44.       }  ~: _0 r7 e% B! O$ ~4 Z: T
  45.     } else{- X% s5 ?/ T8 Y
  46.       if(nPower > MinPower){
    ( H8 i6 y+ V, w" J
  47.         nPower -= 2;
    & B% }- \: b. C. o) m+ d
  48.       }  v' r) M/ R5 @9 L
  49.     }
    * I8 Z  E3 [  d+ q5 T! w$ y8 w
  50.     MotoL(nPower-Error);& g# \$ a/ F/ V6 ]6 `* ]
  51.     MotoR(nPower+Error);    ! ?) j- Y$ ^" ]% n* W/ @' z7 E
  52.   }else{- x$ V; E8 q+ V3 e/ b1 u% P( Z
  53.     motor_9.run(0);
    . O& E) a6 ^* n. D) ]: h
  54.     motor_10.run(0);$ A3 c' f. }* a) l* B8 A8 G3 Y/ D
  55.   }
    8 r' a8 R# Z  C
  56.   do{}while(millis() - StartTime < interval);/ c; U+ K4 s: {" ?5 d
  57. }
    : K. T: |! R5 a8 s. L
  58. ( h. E! |! x' _) ^% q( }  b
  59. void MotoL(int Power){
    8 I4 w! i8 U0 ]4 a
  60.   if (Power > MaxSpeed){
    5 b. {/ k% I; a
  61.      Power = MaxSpeed;; Z$ ~+ F% g- ?
  62.   } " c' H: T( I6 l+ s
  63.   if (Power < -MaxSpeed){: r+ L' Z  @+ K/ F3 j6 t1 g
  64.      Power = -MaxSpeed;
    % @6 J2 w5 A6 n* Y4 k4 o& r& G
  65.   }
      ^0 Y( y! u$ p. Z: g. G3 [
  66.   motor_9.run(Power);- H$ J0 P" c0 T
  67. }  
    * Z8 {/ w, B- U7 ]3 x6 f- F
  68. 8 W+ r/ w+ N. K+ c8 j5 G
  69. void MotoR(int Power){
    1 g# a9 e8 l3 H5 x& o, S( {+ ^% Q
  70.   if (Power > MaxSpeed){
    , a1 W; B1 ]# n/ q
  71.      Power = MaxSpeed;
    + o+ @7 o* M  ^  L  F5 X# j
  72.   }
    * w' g% h" r/ P% D
  73.   if (Power < -MaxSpeed){7 S/ l& M3 K6 b& M/ |/ p. H
  74.      Power = -MaxSpeed;
    & O! t0 |4 O' `9 P
  75.   } ' i! j8 e& T! L0 \! J: U
  76.   motor_10.run(Power);+ {, Q. d! o/ f. m
  77. }  
複製代碼
$ Z. y$ p- o& U- o
9 y* f1 U1 W; J/ `; g" r3 S
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯
: r! w& X6 H8 X, F/ T2 _) R& Q& w  K
您好,不知是否能向您請教。
' E, k; x+ y! f  Q9 v目前和宋修賢老師在處理Ardui Car
  w* K" I9 S0 t) P: Y8 Y6 \雖然已使用較繁雜的方式處理了跑出黑線外的狀況
: w( r7 ~  \8 {5 G5 V' g% m
. O& i& p% K5 D, w, x0 U/ i但基於想追求更精簡的程式所以還是想請問一下# Z; o- C* J2 p3 {3 y
就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝
' |! I2 p' r$ h4 }) o/ K1 @8 T不知道您是否願意教我可以如何處理
4 `% W6 c) W7 c* [& v3 b3 w8 n5 g! q4 Z) m- j9 W5 L/ C
4 n$ D. Z  L( m" s8 u
以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;
    8 N1 R( q$ x- |; J: a
  2. double CNY70Val = 1000;& \4 v% h4 K! E- D+ B
  3. int interror = 0;) f" e1 u# O  |4 `% D$ R
  4. int olderror = 0;1 f  r- ]* h& p/ S0 u
  5. double values;' F6 r: k+ ?2 h- k1 V$ [  m9 I

  6. : B' w3 X0 L$ T) t+ C
  7. 2 V; c$ \. N* f
  8. void CNY70()
    : x$ l: k3 v6 ]$ r
  9. {' m; W4 C; k' J$ C
  10.   valuesRR = analogRead(RR)$ g' u: d7 Y/ E4 B6 N" M! x
  11.   valuesMR = analogRead(MR);  I: ]+ z* X* C: v& d  m
  12.   valuesMM = analogRead(MM);
    - O% `2 t" ^6 a4 z2 }7 X" t, u
  13.   valuesML = analogRead(ML);
    " A) b" q% q' a; G$ Z4 ^, \0 M) E
  14.   valuesLL = analogRead(LL);9 u( Q. c+ l& J& u0 c. r) K
  15. % a% F( E6 \) R2 R
  16.   if (valuesRR > CNY70Val)3 h7 G( f$ a$ t/ L6 d
  17.     valuesRR = CNY70Val;
    5 V8 P* O. M$ Q; Y; d) `
  18.   if (valuesMR > CNY70Val)
    & y+ {: B0 m* {( g
  19.     valuesMR = CNY70Val;# `; O5 r1 {) c9 F7 S; X
  20.   if (valuesMM > CNY70Val)
    , t) U- N% H8 b9 u5 J
  21.     valuesMM = CNY70Val;
    1 {) N% I$ d' ]; n
  22.   if (valuesML > CNY70Val)
    6 L- q& r% b  T0 O
  23.     valuesML = CNY70Val;( s4 G. [( y' }: \# n$ T8 t
  24.   if (valuesLL > CNY70Val)
    . B" `) {+ g9 [9 R" D
  25.     valuesLL = CNY70Val;
    ( G, H1 Z$ [5 ~% X/ v. l

  26. 4 \8 z! ~7 p. B6 ?
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    1 w, t% ]. X: i# n3 f8 O0 W
  28. }
    . O! X8 F6 B  q1 x1 C# ?# e
  29. 3 `$ R1 E+ V' W  g0 K
  30. void Car()5 y/ h+ c/ m' _8 A% x6 w/ L
  31. {; I( y' a" X" F4 L6 {; R7 [% j% C
  32.   while (1) {
    7 l% N) C, V0 K" I5 _
  33.     CNY70();
    7 n, F0 j4 s# o! }) \* c, w" f5 Z

  34. % r7 s8 I* r" {, r# T  J- R: D) U
  35.     int error = ((int)values);
    4 t! c8 x/ Y; E( M: o2 M
  36.     interror += error;. |; o% j7 s8 \4 }$ z
  37.     int lasterror = error - olderror;+ |* H. K0 \4 ^5 r
  38.     olderror = error;1 v; s) V- ~8 c/ O. X
  39.     int power = error / 5 + interror / 10000 + lasterror;2 f) m% U, a) F7 P" Y" M

  40. 8 q; E, O1 s* [) |, _7 q8 m% ~; n2 x
  41.     if (power > MotoSpeed)
    . o- G: @9 h: e, @5 F2 N/ B. n  v
  42.       power = MotoSpeed;- g8 _- X# T! R; `: R. G
  43.     if (power < -MotoSpeed)7 I6 A6 ~; ]: I
  44.       power = -MotoSpeed;
    ' S* a7 U+ r9 l; A+ Q1 v0 i

  45. 1 i% A8 `2 X* j  S7 H  K& A* f
  46.     if (power > 0)  e! `% C8 y6 M  m% q
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。
    ' s8 F; o7 {# m6 `) c
  48.     else
    9 `! t- W0 t$ V
  49.       Speed(MotoSpeed + power, MotoSpeed);
    0 L' |% c0 p& e7 i
  50.   }  X. r; |% |* }, o6 x4 ]
  51. }
複製代碼
9 [, ?5 P# v  `: X- u$ \' [' m0 N  w
3 B* E- I) D0 T( r) \: f
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-11-26 20:42 , Processed in 0.019122 second(s), 17 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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