圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21787|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>
    $ ?& X0 R7 M( ?9 ~6 w7 w
  2. #include <Wire.h>
    * @. e5 O; _2 A0 }5 [$ \. R" G6 E* ]
  3. #include <Servo.h>; P$ u/ a6 u% K

  4. 0 D% ^, o9 W& z1 M1 P9 @; R
  5. #include "MePort.h"* T, e; k* `2 Q( z
  6. #include "MeUltrasonic.h"- N7 X; D' q+ c, L3 z7 _
  7. #include "MeDCMotor.h", F) o! }: ~1 f4 |. a+ p
  8. 9 U! |+ i2 Z4 t  c1 b
  9. //double Input, Output ;- k3 l$ M1 F, A3 i# Q
  10. float MaxSpeed = 255;
    + L6 y& R' I) \: e, Z4 ?2 ~
  11. float MaxPower = 180;
    7 ]; v$ E! Y- X0 Q
  12. float MinPower = 120;; X/ G& L- F; B. \
  13. float Error,ErrorAcc,ErrorDec;. G8 k% B8 w" J! @7 ^

  14. 3 h" r3 N0 `6 G! ^
  15. float Kp=0.14;
    & v, d2 Q5 ~. O5 V  z
  16. float Kd=0.00020;//23;: m: Z  G+ P* g# J
  17. float Ki=0.000201;6 v- M  o+ {3 O6 k$ L9 v

  18. 3 B5 F  u8 o5 v( e3 Z
  19. float nPower;7 s$ Z% s3 i. q) ~7 _+ {8 ~
  20. MePort lightsensor_6(6);
    2 m3 ?3 [: }6 _& N
  21. MePort lightsensor_8(8);% j& m" a& q. a7 h
  22. MeDCMotor motor_9(9);* c. y1 y  N$ z8 b/ I+ }6 ~
  23. MeDCMotor motor_10(10);0 @( y/ o9 @# T6 L% y7 I
  24. unsigned long previousMillis = 0;
    . y; a/ l% x8 x' U, L
  25. const long interval = 1;8 r2 B! S! n! u) H& a! @
  26. ( N# J0 j  q" r7 a2 z
  27. void setup(){
    ' e  n/ `% \( s! W
  28.     lightsensor_6.dWrite1(1);
    ! D3 u' t+ ]$ J
  29.     nPower = 160;
    2 D$ [1 I! n; D0 P/ f
  30.     Error=0;  B4 y8 H; m5 e0 t% I  M# V
  31.     ErrorAcc=0;
    ( Q) g: b6 X- l$ M9 S4 f' g
  32. }1 I% ^# B" \6 s5 g+ u# V' x/ N

  33. - H6 f% ]; u9 i2 P! U
  34. void loop(){' L! {  k' `! y- l; _7 ]! |: r
  35.   unsigned long StartTime = millis();( @* }4 `- d# u
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    " h% w! B1 [* h& j; W
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();% T5 y1 h7 _0 j1 J, x- D# F! z/ G
  38.     ErrorAcc +=  nError*Kd ;/ j, c$ N% P8 Z2 h1 {
  39.     ErrorDec -=  nError*Ki ;" m$ G+ N2 D0 I/ U$ {
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);, I) o+ j5 j$ b* e* P$ @8 i; W
  41.     if(nError < 80 && nError > -80){
    0 u8 A, f, p8 t6 v  t5 y6 E6 w# Y
  42.       if(nPower < MaxPower){% K8 ~6 Z* T* e' r1 \" U* a
  43.         nPower += 3;6 k" g+ o( u' K* H+ I
  44.       }- e* L3 b. ^+ S5 C+ j
  45.     } else{
    7 u( N$ l5 d! @+ C" v7 x! [
  46.       if(nPower > MinPower){
    2 `. a* \& y3 w. I
  47.         nPower -= 2;
    / e) ]  c8 g: N4 Y/ j) L
  48.       }2 w% E/ q% X, b4 ^' {; F" ]/ }& l
  49.     }
    9 E) @, e! M2 Q. h9 ^4 Y" Q& I
  50.     MotoL(nPower-Error);4 K  Z0 c' h) Q
  51.     MotoR(nPower+Error);   
    # w5 d; s& u* V3 f( b( z7 |/ X
  52.   }else{
    - c: T/ ], S% L; g
  53.     motor_9.run(0);# c( ^  G& j+ n! }2 J
  54.     motor_10.run(0);( B' W1 C+ M9 K9 i% {9 y
  55.   }5 _* u2 N8 q  U9 R& u. q! u
  56.   do{}while(millis() - StartTime < interval);
    5 ~+ z+ A, P& C8 U$ I- ?
  57. }6 q& P* ~) v  I# H1 ?/ J

  58. ) C! X0 c! ?, C' F. T: z
  59. void MotoL(int Power){4 W- E3 Z2 }2 {! Q2 K8 ?5 [2 Z3 [
  60.   if (Power > MaxSpeed){# Q  `1 q% ~, f7 n( Q; s& c+ L
  61.      Power = MaxSpeed;
    ( P. }. I) V. c
  62.   } ; {) \) E+ n+ p( V3 ]
  63.   if (Power < -MaxSpeed){
    2 z7 s/ B" X+ K) s+ E4 K
  64.      Power = -MaxSpeed;: I9 E; w% R; y3 s' `
  65.   } ( l  s1 N* W) k3 F. @$ O. B( G1 e9 t
  66.   motor_9.run(Power);) h) A) N+ w0 w/ b9 d# r2 U- m% Q
  67. }  
    - s5 a8 f- b8 D
  68. : w! W- n& ?1 f& ~% G
  69. void MotoR(int Power){
    + @. `- \- {8 Z" x0 B  K) Z
  70.   if (Power > MaxSpeed){6 I$ I4 H4 P$ p1 c. p; p2 b8 C) b; e
  71.      Power = MaxSpeed;
    8 \: F6 p! S& \2 K* T  q- p
  72.   } " A+ ^  \# a( a  s5 L, }4 c
  73.   if (Power < -MaxSpeed){# \- h: e4 w5 k: T' f& t: R" ?9 e
  74.      Power = -MaxSpeed;( j: ^) P, n+ k* k1 t5 d( v
  75.   } 9 W0 E7 p* h; |" a* }6 Y8 ?
  76.   motor_10.run(Power);2 c4 Y) n2 O$ x3 F0 R! F. d
  77. }  
複製代碼
: t0 D( R" ]. H) N5 w; D# o: [

( s6 C& q7 q# f* ^
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯
" o$ i, A9 G+ f0 w
0 w' {6 o' E( Y' s9 L+ e% c您好,不知是否能向您請教。
5 h9 e, _9 w" }目前和宋修賢老師在處理Ardui Car
. o5 n* A. U7 @! e雖然已使用較繁雜的方式處理了跑出黑線外的狀況
0 I( f9 `. U" I9 J: r. S8 ?6 M2 W; j5 y; |0 u4 D+ k' n
但基於想追求更精簡的程式所以還是想請問一下
( ^8 j$ F+ ]2 ]( U- e5 H3 ]4 S3 @就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝
  G: e+ v) ]6 P  N不知道您是否願意教我可以如何處理6 ]% x% j* P2 ^1 k# R
. _3 `% }4 @+ [2 Y0 D/ _

: C+ Y$ a" L0 H& Y3 r! L& H以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;' {) R0 `3 ^& G4 w( }
  2. double CNY70Val = 1000;$ m& u& I, |# y5 q4 u
  3. int interror = 0;
    ! z8 C* S7 r" e, K7 t4 D
  4. int olderror = 0;; L  P8 e# o* i4 ~. i. k
  5. double values;4 D* o6 H& d% z! y2 {! ]8 [, s

  6. ) n! m# g8 o8 n3 o0 U
  7. " a( L" ?# t8 X+ i7 y
  8. void CNY70()2 Y3 X9 r, t! F! S: k" u
  9. {
    ; A' R1 y& J- U; P
  10.   valuesRR = analogRead(RR)
    / p. T0 [5 R3 V2 q. F4 }1 J0 Z
  11.   valuesMR = analogRead(MR);
    % Q) u8 h1 g+ X
  12.   valuesMM = analogRead(MM);
    ; V( g9 Y5 w" J5 O9 E9 c
  13.   valuesML = analogRead(ML);/ Q6 m# `4 I5 V( }6 `5 [/ `5 }" H
  14.   valuesLL = analogRead(LL);
    3 u% X: N9 W1 F, }- m

  15. 5 c& |% G+ R7 f3 B5 G1 X' ^' c4 k( \7 M
  16.   if (valuesRR > CNY70Val)  b' @: M% r9 U+ t7 t$ o; w( g5 C
  17.     valuesRR = CNY70Val;9 D' B( W4 j; \7 {, d) Q
  18.   if (valuesMR > CNY70Val)% r0 H* i- a! _+ v- d# L( P
  19.     valuesMR = CNY70Val;6 n, F4 O, N) @5 |
  20.   if (valuesMM > CNY70Val)
    : l, P; M* Z7 W) e$ u! F' {
  21.     valuesMM = CNY70Val;" W" z' s$ f1 E5 D0 f4 Q1 g. M. |
  22.   if (valuesML > CNY70Val)
    7 \9 d. z" M9 j8 x
  23.     valuesML = CNY70Val;/ c& |6 U; ?8 L2 k
  24.   if (valuesLL > CNY70Val)) B. A9 a, J! z) {0 U
  25.     valuesLL = CNY70Val;
    9 g: A$ u. j( x$ H4 v

  26. ' c: Z% P  c+ s+ p3 L2 @
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    ( L. q" _9 \5 b- B) O6 R
  28. }9 N; m3 R  L" Q  T
  29. ' b6 O$ l2 G$ o
  30. void Car(): d2 {& ?- l/ t( v1 K( }  u' L
  31. {
    4 N7 t0 a6 d+ t* F: B
  32.   while (1) {. `9 S+ |# J4 V9 T: L6 z) K* Y% U
  33.     CNY70();
    9 k* R. s) C; r/ Z* c% k% Z
  34. # [" a3 S, E5 \0 r8 h7 b1 {
  35.     int error = ((int)values);( S5 D0 E5 t/ S3 R; g
  36.     interror += error;
    ! Z3 n5 v& A3 b9 S) Y! _6 c
  37.     int lasterror = error - olderror;0 t) d# M/ [  h" ^+ n2 J
  38.     olderror = error;1 V' ?! v9 `% j  F, F% C& ?7 b* \
  39.     int power = error / 5 + interror / 10000 + lasterror;
    - T* S. Z" g: P6 e+ q- ?. _1 e* g, A: e
  40. % _$ E/ P, I- u$ B1 Y
  41.     if (power > MotoSpeed)6 @( F: o. D  `/ j2 x! E3 h' }$ |
  42.       power = MotoSpeed;5 F; H5 h+ _2 z* n$ b/ h
  43.     if (power < -MotoSpeed)8 b& Y# y1 B  x% Q
  44.       power = -MotoSpeed;
    : d7 N+ z& H9 q6 p( p

  45. ) M  N$ R- N4 @# c
  46.     if (power > 0)+ B7 I$ p+ |% s8 A9 Q! Z
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。' T5 H1 w- s$ ^2 f6 t
  48.     else
    $ j' O7 C, F  H; V7 s* l+ C  A
  49.       Speed(MotoSpeed + power, MotoSpeed);8 m" U9 X' w0 j8 |- k& s$ ?( |" `
  50.   }) ^3 u. Z/ n) y  A9 c9 {
  51. }
複製代碼
$ v* u( `& z. C6 _5 _

# D% u, P  y5 q& D
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-11-30 21:40 , Processed in 0.022747 second(s), 17 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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