圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21707|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>
    2 s  C/ B( ^0 d) T3 x% H$ V
  2. #include <Wire.h>
    , G" u( @: P5 U! b8 B+ E
  3. #include <Servo.h>; O# v" W$ i1 Y. i: n

  4. ! Z' o; S/ x4 F/ M# ]+ K* p
  5. #include "MePort.h"
    ; W8 o8 v  D* y: @/ m5 N) F, R
  6. #include "MeUltrasonic.h"
    % ]3 u' q" _2 _0 C1 D, h  |/ G
  7. #include "MeDCMotor.h"
    8 T0 `- n  w2 S% Q( o  s6 `  I2 Y9 q

  8.   K1 `+ O) y- x  p- }7 I8 p
  9. //double Input, Output ;& N, J5 p% |6 k  M
  10. float MaxSpeed = 255;
    4 Z& P: o+ y$ }4 x
  11. float MaxPower = 180;
    ) [% c  p+ i, d5 c; E3 k7 L
  12. float MinPower = 120;
    , M1 D: v0 Q) I! q, e; r
  13. float Error,ErrorAcc,ErrorDec;8 M% }3 @# @8 z# }3 E  ^, q- G" Z
  14. . d, U! w! {1 [& T% T/ p+ c" [+ q
  15. float Kp=0.14;
    ! E; j3 t/ I/ m# D# P0 y& f1 J
  16. float Kd=0.00020;//23;# |3 R6 B, H2 ]# T, n# E8 z
  17. float Ki=0.000201;
    2 |; R) M2 _$ x# E9 [
  18. * N/ x5 m! I0 S4 C! U, r" V
  19. float nPower;$ _7 j1 E: k* }# S7 D- b) p
  20. MePort lightsensor_6(6);1 k5 _: @4 ^, F0 ]) t8 l: E: ^
  21. MePort lightsensor_8(8);7 {8 F" o6 R. l  L2 u% Y
  22. MeDCMotor motor_9(9);
    1 F) m* p9 }7 x! F1 `, i
  23. MeDCMotor motor_10(10);5 j5 N  P5 ?( K& A, v: ]* R
  24. unsigned long previousMillis = 0;
    8 t( v" \$ J9 F" B
  25. const long interval = 1;& r8 h! U2 |7 @7 ^3 `, X

  26. 4 r3 S2 ^+ M1 ?9 W- S9 Z
  27. void setup(){$ ]9 W# P3 E5 x' t9 @
  28.     lightsensor_6.dWrite1(1);- U" \* c# _+ Y9 [* w
  29.     nPower = 160;! B4 M3 O4 h) s+ _+ B; I# Y' `# z
  30.     Error=0;% O. Z! k- `6 ~0 R
  31.     ErrorAcc=0;) a! ^/ i& ~# r" K; j+ \8 r3 L
  32. }
    ' e. i0 k! H# G
  33. $ l4 V7 X# ^/ E- O) W4 \& @8 v! p
  34. void loop(){. ?2 E9 G" e3 f% q6 B
  35.   unsigned long StartTime = millis();
    . Q9 `% p; e$ }
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    5 O( y) Q& m) Z+ }. |/ K
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();4 `7 _, t2 u5 v, f
  38.     ErrorAcc +=  nError*Kd ;
    ; |6 I5 A) h; }; @) P
  39.     ErrorDec -=  nError*Ki ;
    - v/ }" x# E4 U+ x3 {$ ?7 L
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);& H2 A& o7 w) A: F9 q; M
  41.     if(nError < 80 && nError > -80){
    0 p8 P0 Y1 L! u+ |6 w
  42.       if(nPower < MaxPower){6 V7 j. Z8 v( i9 ?+ a0 n0 T
  43.         nPower += 3;
    1 v. N, ]% J5 r
  44.       }" T# q7 r8 ~+ ?8 @: T
  45.     } else{# _; A2 x' ?( F0 t. ]
  46.       if(nPower > MinPower){( A: @! n% w1 q* S( _! b
  47.         nPower -= 2;
    % W, e2 r" [: z5 C' C- e" `. n
  48.       }
    . }* l/ a* K3 @& d; c- e/ A8 c2 k+ t$ C
  49.     } # C& w* C# C4 _  P& k. C/ A
  50.     MotoL(nPower-Error);7 W6 [9 z# Q6 i# D6 O" ]& W+ c
  51.     MotoR(nPower+Error);   
    % ]* T; M% y# |
  52.   }else{
    8 u1 p* T6 G& m9 A* @4 k( S
  53.     motor_9.run(0);2 s9 T8 r! R* @
  54.     motor_10.run(0);8 F5 c! U8 ^2 k( O& z6 N$ S2 `
  55.   }
    6 \9 |2 D6 ]- R
  56.   do{}while(millis() - StartTime < interval);1 A& i; i& ]1 X) ^3 V
  57. }
    " Q' L, ]5 G# R3 N4 {+ {

  58.   `' H/ ^$ w/ a
  59. void MotoL(int Power){- q3 ]8 A: G7 R  o# |) u8 H
  60.   if (Power > MaxSpeed){, |4 B6 I' i( Z. T
  61.      Power = MaxSpeed;
    - i% x$ E) u( d' z3 a- D, Y+ Y
  62.   }
    4 Y/ g8 @+ w2 L3 P1 ^
  63.   if (Power < -MaxSpeed){( f5 W1 E  m7 `, b2 B( h; _. C
  64.      Power = -MaxSpeed;3 U- c2 s2 ^$ W" g0 C
  65.   } 8 {+ N, e7 o5 L) W- p! e  i4 o
  66.   motor_9.run(Power);1 ]1 h! S5 ^: ~8 A& G" e" g
  67. }  
    7 j6 S# f5 u) b: G6 b
  68. # U, p% E$ N4 L+ J( U6 D" z) j
  69. void MotoR(int Power){
    $ [5 ?: n+ N2 [7 a5 ^$ A7 L" x
  70.   if (Power > MaxSpeed){
    3 I( G3 n6 v4 j1 m- ^: S) X6 `
  71.      Power = MaxSpeed;4 m& e7 R/ q, h. ^* Z
  72.   }
    5 K, j; }8 {! e5 R: i
  73.   if (Power < -MaxSpeed){
    ; ^. I+ |  a5 z& E; e1 g
  74.      Power = -MaxSpeed;
    * N- v1 ^8 K3 C5 L7 u. R
  75.   }
    ' \6 O3 @; C# d& c/ E' u4 ~4 H
  76.   motor_10.run(Power);
    " `. {0 d& b; n2 D" z
  77. }  
複製代碼

6 i9 M8 A5 W+ t% h7 |: D. \  {5 p0 F- S+ R& E" c
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯 " S* n) I/ o& |2 l  y
* p# \+ j$ O- m- T
您好,不知是否能向您請教。
2 J7 h% z7 p5 T, b目前和宋修賢老師在處理Ardui Car
: r4 Y0 }! ]! Z) ^( c/ K雖然已使用較繁雜的方式處理了跑出黑線外的狀況. \  i: T1 H1 U9 R

( Z" e8 l3 e. H! ?' ]- t: t8 C但基於想追求更精簡的程式所以還是想請問一下
' r: R0 ]! ]" q: z/ u就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝( o8 M; o+ ?$ b
不知道您是否願意教我可以如何處理" G8 L- i8 \( F% @) g
1 }7 x3 T; g4 E/ ^3 B5 l% L
/ c, [5 t4 ^1 x3 N% r3 n" Q; ~3 P
以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;" a: T7 q2 a# z$ W2 E" d+ E4 M8 J7 X
  2. double CNY70Val = 1000;3 {" c5 _0 Q1 g# w/ @4 m& {% j
  3. int interror = 0;
    " `3 I) K, |0 {2 Z9 b( |; Q  o
  4. int olderror = 0;9 l; A+ H# N  F
  5. double values;$ C4 N" L) u7 w; U  s1 N

  6. 1 @) Y4 m# U. M9 T) \) w8 E: ^9 V

  7. ) |/ H5 x5 p3 e$ O2 U1 A
  8. void CNY70()  b; G# ^& m9 N" ^2 C' I7 B
  9. {! p. o$ p# X9 G' B: O
  10.   valuesRR = analogRead(RR)8 E& @7 L5 R. z  \
  11.   valuesMR = analogRead(MR);
    . P* Q4 }3 B  A' O3 e) Y! U
  12.   valuesMM = analogRead(MM);: u2 Y8 J. C) x* g# @
  13.   valuesML = analogRead(ML);# I  {2 [! j) i
  14.   valuesLL = analogRead(LL);9 E6 ]; ^- S2 u0 x6 r

  15. # G9 S) k: [% K$ R
  16.   if (valuesRR > CNY70Val)
      T* x1 v3 S3 ?, P
  17.     valuesRR = CNY70Val;+ V" ]$ e8 R4 N/ {( }% q
  18.   if (valuesMR > CNY70Val)9 ~2 O2 r2 Z3 F  `) u
  19.     valuesMR = CNY70Val;, Q' V2 C+ C" _6 D7 I
  20.   if (valuesMM > CNY70Val)1 p8 A9 n0 }5 q# \
  21.     valuesMM = CNY70Val;
    . H- X/ P4 W; M; t3 r+ s+ j- s7 B: y" T
  22.   if (valuesML > CNY70Val)
    ! X) Z* `) d9 Z9 K
  23.     valuesML = CNY70Val;
    ) j: u4 O* Z% z) L; k* w
  24.   if (valuesLL > CNY70Val)7 {, M5 K$ T6 Q; p
  25.     valuesLL = CNY70Val;
    : T% f( `+ U: ^" l# @3 O& s
  26. # o! K# S# h  ?6 R
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    6 b* X! Y" g: g. I+ H3 x4 b6 k
  28. }- Z0 J7 Z3 S+ L$ ~

  29. , W6 `, D6 i9 w, T0 @
  30. void Car()+ C. _& B$ q8 p
  31. {$ L. B3 v$ I5 ~; @! P/ v
  32.   while (1) {* W7 P0 R) M" J# J
  33.     CNY70();: L' w8 j& a5 n1 y5 R

  34. $ x" H# K8 J, a8 H. d7 C/ B, k- R( b
  35.     int error = ((int)values);! V9 }3 K/ q: r
  36.     interror += error;
    9 Z# K7 v2 y! t8 W8 [) ]8 a2 R
  37.     int lasterror = error - olderror;/ X/ ^+ W/ N1 q7 C  u5 {1 {# H6 G3 _5 y
  38.     olderror = error;0 O0 S: H- @& u' p
  39.     int power = error / 5 + interror / 10000 + lasterror;1 ?7 D5 \6 S$ R3 h
  40. 1 m9 e  [( O  Y$ j
  41.     if (power > MotoSpeed)
    - n: `; ?; A4 v* ~. R
  42.       power = MotoSpeed;9 }# `/ J3 J2 d
  43.     if (power < -MotoSpeed)  X' I) H1 C- {) c8 E
  44.       power = -MotoSpeed;
    ( N5 R# J( G- B# @' s6 c8 t
  45. / ~3 p/ a7 c; V: n9 W' o
  46.     if (power > 0)- F# i' ~7 _. z  _
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。
    ( N. R; D, A5 I
  48.     else  h. @, r) G$ n) `8 A3 b
  49.       Speed(MotoSpeed + power, MotoSpeed);7 {6 Y- F) h% ~5 c4 E% s; g8 w
  50.   }7 Q; @8 A5 y  M; V4 D/ I+ B2 q
  51. }
複製代碼

5 ~, t2 G5 c* x8 W$ _
( w# ^7 ^/ i0 k
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-11-25 01:34 , Processed in 0.026082 second(s), 17 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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