圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21841|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>. y5 ^8 N+ Q& p; r* |4 y! U
  2. #include <Wire.h>; {% O% ^+ F3 r: F; h) [
  3. #include <Servo.h>
    4 Z7 N4 T" g3 R4 ^! }1 x5 r
  4. , b0 r& m3 C) k  V3 q" [
  5. #include "MePort.h"7 b- j1 d, ]( Q3 |& ?
  6. #include "MeUltrasonic.h"$ b7 z' q& C+ T+ }) O# V
  7. #include "MeDCMotor.h") L8 x" I, T) f4 Y
  8. . s- W0 N7 j% [9 c* i
  9. //double Input, Output ;) G' M5 `1 R. |' q- Z3 l8 w/ i
  10. float MaxSpeed = 255;
      |: t8 H! ^2 Z/ J8 \$ e9 Y
  11. float MaxPower = 180;+ `4 p2 N: W0 a8 O$ `& N# J
  12. float MinPower = 120;7 A* r7 J7 P4 K, i+ `' _) V
  13. float Error,ErrorAcc,ErrorDec;
    ! b0 R  R8 W8 O

  14. : p7 m# \' [) r- G( D2 ^
  15. float Kp=0.14;
    $ J6 E$ n/ |& o/ Z/ S2 A
  16. float Kd=0.00020;//23;- u4 F6 L9 d: R0 V8 _5 t7 b0 g4 [- Q( S
  17. float Ki=0.000201;
    5 Z" I1 X6 I9 F; V
  18. + s& Y' `# H' c- _+ ?" V
  19. float nPower;4 O6 l+ S+ X9 P
  20. MePort lightsensor_6(6);
    0 w2 ~, n' [1 k/ n) O
  21. MePort lightsensor_8(8);( u, O4 J9 S; l& ~& b" L
  22. MeDCMotor motor_9(9);2 D6 w+ f5 \, m3 J  C# `2 V
  23. MeDCMotor motor_10(10);3 M( W, [* w, n) s0 B
  24. unsigned long previousMillis = 0;
    " g: \0 h( g4 b7 v, ?3 E
  25. const long interval = 1;
    5 l: f5 F5 a$ @( T: y9 P9 c: Q

  26. # ~1 i, {! d1 |+ O- i
  27. void setup(){
    / z, G% V$ @5 U( f8 O3 X
  28.     lightsensor_6.dWrite1(1);
    & R+ x4 n8 ~$ N- Q6 H- d, ]( V
  29.     nPower = 160;
    * [1 G0 I3 F# ^
  30.     Error=0;* s% o2 |+ f- T3 y; X0 D
  31.     ErrorAcc=0;
    , E0 |; i6 h# C/ l& [5 E* S
  32. }+ h4 ]! L6 i; ~. k% [

  33. - w, {) G2 w" R2 W+ N
  34. void loop(){  M+ w8 M. Y0 N- F4 L' W- ]
  35.   unsigned long StartTime = millis();
    # G" m+ K7 ^% N. v  {3 y; u0 S
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){8 ~" P( E7 s8 E" \1 G; w
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    8 ]5 r( E1 f6 g& ]/ A$ J
  38.     ErrorAcc +=  nError*Kd ;
    & P! Z# U( X* X  g
  39.     ErrorDec -=  nError*Ki ;
    / i. Z' y$ f0 l8 @  s2 Y/ C
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);& V, s+ t! O, J' P9 S
  41.     if(nError < 80 && nError > -80){
    - h* x1 h- r2 M+ p% W) ^
  42.       if(nPower < MaxPower){
    + w" G2 e" R7 O- g5 f
  43.         nPower += 3;' j& f. Q) ^! s3 N
  44.       }/ @8 T) X0 }% g% z: B5 I2 _* K
  45.     } else{
    ; J( W3 K# K+ h- ~
  46.       if(nPower > MinPower){
    6 U6 G, r: K+ p, P! D9 V
  47.         nPower -= 2;$ E4 a4 [  I4 V9 X) r* M' L1 ?
  48.       }
    . o1 l3 c5 Y, q: R8 |- [
  49.     }
    , v  o+ U, w9 h1 e
  50.     MotoL(nPower-Error);" `- D' A/ `$ s8 Z3 o
  51.     MotoR(nPower+Error);   
    % E) a( V0 x  g1 L/ T4 j
  52.   }else{
    5 `6 N- {; |' s: e+ e" [2 t
  53.     motor_9.run(0);
    / w, @# }9 h' w0 I% R
  54.     motor_10.run(0);
    $ i3 E3 G( t: K' I2 S" O' f# v& k& e
  55.   }
      Z8 ?8 P9 X2 R% b+ c; I
  56.   do{}while(millis() - StartTime < interval);
    4 B% x( E" Z- A
  57. }
    / Y& e0 {0 D. Z) O3 L; b
  58. ' c* ]# j7 T( J; C2 D2 E9 P% ?
  59. void MotoL(int Power){
    ( y4 [, W; f- x( O
  60.   if (Power > MaxSpeed){
    + z; E3 A8 H1 p) Q
  61.      Power = MaxSpeed;
      g0 N- t: {  E5 Z, l; t1 a% m
  62.   } ( ]- U" Y: L9 M: j- S+ r
  63.   if (Power < -MaxSpeed){
    . A' d, c; z/ u  R
  64.      Power = -MaxSpeed;9 {0 {: z# F: i1 G& U; r' i. V
  65.   } 2 h! S% ?* N1 Z) M' X, Z
  66.   motor_9.run(Power);
    8 m8 t6 j  e, [" ?8 q" F' S4 t. D
  67. }  ' |. s* c- j4 u: N6 J/ T- ?  V, P
  68. + z/ o+ K9 j. p- C9 ~! I/ O
  69. void MotoR(int Power){
    0 R/ N) U, h# L# ?' ]: `1 s  `
  70.   if (Power > MaxSpeed){/ t9 q! K- a' f2 r( f
  71.      Power = MaxSpeed;$ Q$ w: _9 c& w( s
  72.   } 7 i5 C1 ~* i% t* Y; l- \
  73.   if (Power < -MaxSpeed){; o- p$ k5 V" D5 [: C" B5 R
  74.      Power = -MaxSpeed;
    6 u& H) t1 C& y7 U
  75.   }
    " r( f' V0 p1 P
  76.   motor_10.run(Power);
    ( A1 T9 Q/ G  l" u9 l% v( h
  77. }  
複製代碼
7 d: P. Q: A( a) s

6 u  g/ u) u& t. n" w6 W5 A
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯 & J! M+ |+ H3 L/ q! J6 a

- X1 h3 ?1 u8 L# I. D您好,不知是否能向您請教。
7 {, P6 V, w: b: B目前和宋修賢老師在處理Ardui Car6 m2 K, M7 B, z6 q* K0 R" }. F
雖然已使用較繁雜的方式處理了跑出黑線外的狀況: \! ?( t: l) }" u5 E; V% T/ L
' s# K$ n" y3 C# `' H7 n
但基於想追求更精簡的程式所以還是想請問一下
% b& x* B$ U+ Z: n( V就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝/ M7 p: u& h) X
不知道您是否願意教我可以如何處理/ K# V1 _) Q' q: h, ^% t, }

+ ~/ e6 z5 D2 `
+ W" D7 _% e6 H( a; P4 r以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;  X1 r: u: T3 U% X0 C
  2. double CNY70Val = 1000;5 V0 F- T$ i5 S& c5 J
  3. int interror = 0;
    # a; F1 N% N2 t8 B) ?7 X
  4. int olderror = 0;$ D( F4 w9 i/ R) |: b: G" C, h) O4 d& _
  5. double values;
    * K( F$ ~% ], V% F
  6. * E7 `1 }! e5 s4 x; I" s& Q0 T

  7. 5 v. O$ Z( _  H* C1 e0 A
  8. void CNY70()* e1 a7 l  G" L9 ^$ m8 j8 `
  9. {
    & N4 f# L; n9 O3 t+ \5 @
  10.   valuesRR = analogRead(RR)
    9 o# {% Z( R/ q% n
  11.   valuesMR = analogRead(MR);4 p/ `  `5 ?( S8 e
  12.   valuesMM = analogRead(MM);
    " t8 w  Q" ]( |/ Z6 ~) q5 B
  13.   valuesML = analogRead(ML);* W3 N5 x% Q/ s4 _8 M
  14.   valuesLL = analogRead(LL);
    3 o7 V3 F% K: z$ g/ Y
  15. $ s$ y! M" P8 a
  16.   if (valuesRR > CNY70Val)
    1 v  W( w4 m: k% v
  17.     valuesRR = CNY70Val;5 T8 R( _# H; Z. p. ?. {
  18.   if (valuesMR > CNY70Val)
    7 u5 c& t0 \. |
  19.     valuesMR = CNY70Val;/ O8 x, k( j  e: ?4 Q, W' p" v6 I
  20.   if (valuesMM > CNY70Val)
    . S9 ?, m& Q3 |
  21.     valuesMM = CNY70Val;* ^: r6 U8 r5 {7 d. |: W
  22.   if (valuesML > CNY70Val). S/ \1 U8 X8 z, N
  23.     valuesML = CNY70Val;* }+ f: }# a, [$ r: H0 _& S3 [
  24.   if (valuesLL > CNY70Val): g' Y" L% Q& n7 J
  25.     valuesLL = CNY70Val;
    . d! l; s  u6 U. l% ]8 }

  26. ( A) d' e3 x' H8 H+ j
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    / z  ?4 d( \2 [' w. F; U
  28. }
    ) P/ b* j4 Y5 A1 C# S

  29. , L! @% D0 @1 j5 l% Q
  30. void Car()
    5 B- b+ W' @1 t& |0 y
  31. {+ d: W/ H4 }( x
  32.   while (1) {7 t9 Q; @. N/ K6 |9 X) @1 h
  33.     CNY70();. @4 N' r/ H7 R& b1 D
  34. 8 `  N6 A% m2 Y2 r3 [
  35.     int error = ((int)values);
    ) z+ v, G# ^3 E! {$ I, o. {' r
  36.     interror += error;6 J: Q) c# |8 y7 Z
  37.     int lasterror = error - olderror;. ?5 v* }. H& W# z
  38.     olderror = error;
    4 q% K9 a" f; H9 m4 ^
  39.     int power = error / 5 + interror / 10000 + lasterror;* c( X) o8 h" i* y
  40. : i4 ?3 }5 D& n; l
  41.     if (power > MotoSpeed)
    2 r9 R7 h0 {: `' G
  42.       power = MotoSpeed;( r; X* G" [" @. b2 |
  43.     if (power < -MotoSpeed)/ U* y, K2 S& s( I
  44.       power = -MotoSpeed;  }7 b/ }  V/ |
  45. # T; j; ]" B* H' K- u
  46.     if (power > 0)" M. ^9 E: i6 W9 o/ a7 q1 D( m7 ?
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。
    # j- M7 g; {8 o- b: X# a8 f/ M
  48.     else, ^5 M. g. z. P5 i) }
  49.       Speed(MotoSpeed + power, MotoSpeed);6 x7 q/ T# [9 L0 p6 P! I
  50.   }
    ( U6 s. G- v9 y0 M
  51. }
複製代碼

/ d1 `) ^8 I( ~. y" J
  L1 A4 D" a! A! \6 ?' M
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-12-5 03:04 , Processed in 0.025913 second(s), 17 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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