圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 20349|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>% l3 P( x" A4 J$ l; m/ A
  2. #include <Wire.h>+ X1 R9 ~) \) p, O
  3. #include <Servo.h>
    - ~! P1 ^* j! `7 M+ B! g. V
  4. + q& i" c; L. i$ v4 D- h
  5. #include "MePort.h"- Q& X$ w2 A. ~) }
  6. #include "MeUltrasonic.h"
    6 I- N1 p% H8 ^  r+ E' [% K. w3 D
  7. #include "MeDCMotor.h"+ g$ A8 U3 C! N3 \" r

  8.   Y$ ?7 K0 Z# l% k( U
  9. //double Input, Output ;
    - P, }3 h) {+ \
  10. float MaxSpeed = 255;
    + }8 z4 x* R5 b; z, P
  11. float MaxPower = 180;
    2 t+ j" r! w" @+ U$ U8 I8 K# W# k3 L
  12. float MinPower = 120;' }1 y1 f  e) A
  13. float Error,ErrorAcc,ErrorDec;
    + L0 H  ^& V# O- q  \/ |
  14. 6 h* |! z; L2 t3 Y" _
  15. float Kp=0.14;
    # r/ H7 J6 i+ x2 B+ ^
  16. float Kd=0.00020;//23;
    6 c5 E) e$ Z6 p9 H; k+ ^8 Q
  17. float Ki=0.000201;: ]  R1 j+ K6 @- i% J

  18. " _7 Z4 ?6 e2 ]- l$ t1 v- C6 v
  19. float nPower;
    # {1 C/ c% ]/ N+ [. Q3 o* ^
  20. MePort lightsensor_6(6);
    * U7 G  G$ o! e) x9 J
  21. MePort lightsensor_8(8);: v+ L; @) W0 R
  22. MeDCMotor motor_9(9);
    1 z3 P- q- O5 p: @) @
  23. MeDCMotor motor_10(10);
    3 p" ^1 X3 k4 L1 a# f
  24. unsigned long previousMillis = 0;1 {; ]  h# ]1 G7 E, B& n; }2 n2 r
  25. const long interval = 1;" y1 s& |, \- R2 i) T% z- C. L
  26. 7 ~# u5 J* q2 ~
  27. void setup(){# U" @6 e) i9 `6 G
  28.     lightsensor_6.dWrite1(1);
    3 @: J: k& G  ?0 {- w1 c) ^
  29.     nPower = 160;
    ( x$ ]  r9 R" R9 G* m
  30.     Error=0;+ m! }+ k; }$ e& C4 T/ i
  31.     ErrorAcc=0;
    ( q3 Y3 X  o, [' `
  32. }
    + g! I3 A" {$ U9 \  m0 M/ J# D
  33. 5 a! E7 `& ]% _) o7 L
  34. void loop(){0 W+ t$ J# b0 s) {' l
  35.   unsigned long StartTime = millis();
    " l4 E2 ^' j" V( c0 w
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){2 D, ?$ r6 N6 L0 p4 u
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    ; u# d  k' X& J6 u. b
  38.     ErrorAcc +=  nError*Kd ;9 A* @# k" b0 @
  39.     ErrorDec -=  nError*Ki ;
    4 K; q0 `, [" S( s
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
      b$ y9 Q! u% a; j2 s5 [
  41.     if(nError < 80 && nError > -80){
    , r' s, f; U* c/ m% g' T# e* Y
  42.       if(nPower < MaxPower){. j0 y# s% h* n: {: u" j: [
  43.         nPower += 3;
    ' R7 m7 N3 E- `- |  y
  44.       }
    - q; ~1 B- |4 m1 x4 L, M
  45.     } else{
    6 v0 I" ^9 g- E& N  T
  46.       if(nPower > MinPower){
    , A: X% R) B+ b3 T/ @
  47.         nPower -= 2;6 c" l+ N% a. [2 C  P6 p
  48.       }
    : C  b: d+ J% X. D5 R2 E
  49.     } ! u3 h! D6 j; z; M& A- v3 A
  50.     MotoL(nPower-Error);
    % @9 B( V0 l4 Q' [  v; ]! h
  51.     MotoR(nPower+Error);    # v4 A$ z: l! E9 Y! Z; T6 {
  52.   }else{5 Z" T; m3 B- y7 `2 Q; U
  53.     motor_9.run(0);7 M+ f! x6 \4 n# T! z# E9 @
  54.     motor_10.run(0);
    0 u! O7 f1 ]+ c; Y4 v2 x
  55.   }
    - a  B+ B( D) i& S- R
  56.   do{}while(millis() - StartTime < interval);
    & V! w/ q& N9 l! j
  57. }+ Y' r4 S% T9 Z4 @' G: M7 D% ]

  58. 1 g) m4 _6 y. F' F$ g& y
  59. void MotoL(int Power){
    7 ]! R( j8 H& e
  60.   if (Power > MaxSpeed){
    3 p. f! `' S# l6 _- O  X
  61.      Power = MaxSpeed;
    . _" _* d' g) M$ D
  62.   }
    ! M: v$ R6 e' g+ u( M6 g
  63.   if (Power < -MaxSpeed){
    ( r: S$ A- {) ]) @4 n: @2 f
  64.      Power = -MaxSpeed;/ N/ e' Z0 O: N% c, Q3 o
  65.   } 0 H3 D! f7 }7 C
  66.   motor_9.run(Power);/ W0 S4 Q0 w0 [( l7 J7 V7 y) U
  67. }  
    0 P. c% |1 M3 m0 o3 U( J9 K
  68. + V# R& ]( K; z! U$ S1 o  D# p
  69. void MotoR(int Power){
    $ B6 R- k8 B* g* a( W
  70.   if (Power > MaxSpeed){
    * ~+ }8 Z# T5 j! |9 |6 w4 y9 O8 U
  71.      Power = MaxSpeed;) t! j5 _  q: v2 C& O9 F  d' B& L. Z
  72.   } 6 B% m4 L5 Q/ o* j
  73.   if (Power < -MaxSpeed){- x* q) C% \, o" R- N
  74.      Power = -MaxSpeed;
    % g2 w$ Q, [1 f1 z
  75.   }
    " v4 P% g2 k" P" S3 M
  76.   motor_10.run(Power);
    2 \+ q5 @8 I' ?  N+ a5 e' F6 F: i
  77. }  
複製代碼
( s- L' U& u( y* {
6 v2 i4 Z3 S6 H# r
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯 5 C- ~! ?* ~2 j# Z3 Z7 X# }

0 f! G! F) e6 X, Y5 t" A" ?您好,不知是否能向您請教。
6 m+ Q6 l) Q) ]" ?! ]目前和宋修賢老師在處理Ardui Car1 N+ y9 X( I' [( l+ o
雖然已使用較繁雜的方式處理了跑出黑線外的狀況' N; ?3 H; |5 p
' ?4 }# L4 {" O; D' I) s1 y
但基於想追求更精簡的程式所以還是想請問一下
6 N" L3 d: _2 D就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝* u0 v. \, r" }; j: Y9 ]) }. I
不知道您是否願意教我可以如何處理
( z' U2 Q# N  {4 `; Q; _+ _. I3 q9 j7 J
, {% L% K+ z( t. S/ N) E
以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;# O0 L+ A5 R% B8 ]5 B+ H
  2. double CNY70Val = 1000;
    ( I5 P1 |# Y, k4 z
  3. int interror = 0;/ F! ]& H( n9 Y( z
  4. int olderror = 0;: i; l; \! b5 l: w* y; l
  5. double values;; @3 `9 D/ {- y- y& Q( p
  6. 8 R7 ?0 ~3 E7 {' k. ?! ~- C
  7. ) q) l- {1 p6 q0 V  _+ d
  8. void CNY70()3 w& R/ b. U. y( e, `5 B- a
  9. {
    , e* \2 m8 Q% F, m7 l
  10.   valuesRR = analogRead(RR)7 E, y$ r" R! d; O* _/ F
  11.   valuesMR = analogRead(MR);
    5 Z2 A1 H8 z6 ~/ x2 N8 c9 Y) n
  12.   valuesMM = analogRead(MM);
    ( L% |% `7 @" j5 f) R: }
  13.   valuesML = analogRead(ML);; E7 G$ y) K# B3 i0 D6 H" P# f' A
  14.   valuesLL = analogRead(LL);* ?9 F0 {! Y. c" T
  15. 9 D# ?. u1 W$ q$ W
  16.   if (valuesRR > CNY70Val); }1 v# H& ?( o! v' E
  17.     valuesRR = CNY70Val;
    ( u3 k. c/ j9 X$ O7 v9 f# p9 o
  18.   if (valuesMR > CNY70Val)
      R5 u4 ~+ \; j4 L, p+ s1 R3 b* g
  19.     valuesMR = CNY70Val;
    5 [- Y1 U% L1 G1 L
  20.   if (valuesMM > CNY70Val)$ x9 }/ ]7 \0 T' {' m
  21.     valuesMM = CNY70Val;, }  ?) n/ \6 y; X5 i, L
  22.   if (valuesML > CNY70Val)
    ( m1 s! N0 K+ @1 i3 C: e, }
  23.     valuesML = CNY70Val;7 M2 ~5 }7 ?8 J; c8 i3 n+ z$ r- h
  24.   if (valuesLL > CNY70Val); t* h. j- Q. h. H7 i
  25.     valuesLL = CNY70Val;
    " @% t( i1 J4 V' r9 m1 L$ C- h! V5 d

  26. . }! F+ K9 h8 [& @" i
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;2 h+ D' v5 ]7 |: w1 `
  28. }
      F% Y0 |5 z* c3 x: d; Q; x

  29. - }* p" D4 G' B: I& Z
  30. void Car(), E8 y% T. Z9 x6 ]* ^7 [# q3 j
  31. {1 S/ Y; O7 r; _, S' _' k# v
  32.   while (1) {
    " R' G. B. J& F$ f3 f- f, Z1 t
  33.     CNY70();* i2 G6 y/ h4 [# j* A

  34. 3 R* f3 [8 v* x: a4 X( Q
  35.     int error = ((int)values);
    9 b& X' y8 A7 ]8 R( c9 b
  36.     interror += error;+ }9 r; f% S3 L( b0 P
  37.     int lasterror = error - olderror;
    1 {" M9 R/ [3 G" X+ W" v
  38.     olderror = error;% _) B5 U5 s9 i) K
  39.     int power = error / 5 + interror / 10000 + lasterror;
    / V+ j' n  {3 ^2 i$ W; Z0 g- Z9 w
  40. ; j2 ^. L4 ]6 O0 S
  41.     if (power > MotoSpeed)
    8 M4 ]1 O" T7 m2 x
  42.       power = MotoSpeed;) r$ K/ u# z' b% |# c8 p
  43.     if (power < -MotoSpeed)0 q% ^$ h! v; l- g) \
  44.       power = -MotoSpeed;4 \6 E/ f/ X. K) x3 I7 A, ]+ d
  45. . Q  b& u" E' k5 {% t
  46.     if (power > 0)5 p0 z2 o9 C: {; [: ]1 O
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。% v6 H7 B& u6 F& w; i" R( d/ J
  48.     else: }* F1 X& L9 I
  49.       Speed(MotoSpeed + power, MotoSpeed);# u2 U8 f: z/ a& B3 f2 T
  50.   }! ~; z& |; D6 ^8 Y) V- x% ?2 t
  51. }
複製代碼
3 U9 P0 F3 \8 b, Y

8 e3 @7 h- f  t
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-2-19 07:01 , Processed in 0.025972 second(s), 18 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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