圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21807|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>& o3 `4 u4 @% d3 A( K
  2. #include <Wire.h>
    9 M& q3 ?& S' C9 N: a4 Q$ h6 U( x
  3. #include <Servo.h>: o' Q* w3 t7 F6 e8 V& d/ a# ^# M
  4. 6 J% e  g% Q2 D. t# I3 B
  5. #include "MePort.h"! u) t- n3 F+ T. S% o0 c2 t9 h
  6. #include "MeUltrasonic.h"
    ; t  l0 D1 n- q
  7. #include "MeDCMotor.h"( [8 w% O; ?- S1 p" F( X) U6 P
  8. . Z4 L1 W- R" {) d) d7 v+ `# Q
  9. //double Input, Output ;
    3 J+ F/ W9 |* u& M
  10. float MaxSpeed = 255;0 }; k- d) Y) T0 J1 I* @3 ?
  11. float MaxPower = 180;
    8 Z0 l8 h- p9 D2 x" J
  12. float MinPower = 120;
    0 C1 o9 \0 n, X7 [" d9 J
  13. float Error,ErrorAcc,ErrorDec;
    6 O7 b7 i! `. y( }
  14. 5 _" T& c' f) Z8 s, [) F7 _) K" ~* U7 Q
  15. float Kp=0.14;, _0 ^5 K7 Z. L# n0 B7 e8 ~- I
  16. float Kd=0.00020;//23;1 `1 t* o6 Y7 Q/ ^/ r; ?% p5 R
  17. float Ki=0.000201;
    7 D- @/ a  {5 n& }8 `+ G

  18. 3 k0 Z8 x9 K& G- e
  19. float nPower;
    7 Q+ m6 F! H  r/ B
  20. MePort lightsensor_6(6);" A3 V. p6 b" v# B
  21. MePort lightsensor_8(8);% j& u0 c: W1 \8 R
  22. MeDCMotor motor_9(9);1 Q4 t/ ~1 I" F1 w( Z8 p3 E
  23. MeDCMotor motor_10(10);, J2 J2 Y+ o. r& D6 m( }: O  z3 {
  24. unsigned long previousMillis = 0;, c0 K- ^2 d( u, m/ S
  25. const long interval = 1;
    4 l. E+ }7 W8 l6 D5 p. q

  26. " M* I3 U: N4 P" z+ f! d- D, T. a
  27. void setup(){
    ; ^: l' T) x9 e+ Y! W2 j" p. t
  28.     lightsensor_6.dWrite1(1);
    + V$ C/ @9 \4 c+ G1 P5 U. N
  29.     nPower = 160;
    ; z4 X- k# ^% O0 Y6 {. |  j1 a
  30.     Error=0;# b" ]2 n# P' x' i" l5 r$ I
  31.     ErrorAcc=0;3 |1 k8 `) x: O5 }/ R+ @
  32. }
    7 @( O' X' u) _: a+ i
  33. ' X9 Q$ ~3 K% N; o0 y* ?9 g6 u
  34. void loop(){
    * S- {+ H$ w! H
  35.   unsigned long StartTime = millis();
    / J9 l2 q; z9 n$ J6 t  V6 K5 |& q9 }3 j
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    # H9 c' N5 R) S
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();7 ?; Q# Z9 \" l! w9 G" O: y
  38.     ErrorAcc +=  nError*Kd ;5 j1 s1 }& C+ \$ B1 J9 ?
  39.     ErrorDec -=  nError*Ki ;. o9 [2 {9 _3 W; L) ?
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    3 u' [0 j" }" s. O! ^* z. i  Z
  41.     if(nError < 80 && nError > -80){
    : i% v8 m- s( i/ U8 w4 @& {
  42.       if(nPower < MaxPower){
    ' p0 |. \: f% s
  43.         nPower += 3;
    ' x' }: m% G3 O9 z9 H) \2 k1 J
  44.       }
    / g. i; `. F# n3 O6 O
  45.     } else{. I& b% T6 g( o8 h# w
  46.       if(nPower > MinPower){1 t$ o- C' P' R6 A0 J. N
  47.         nPower -= 2;
    ; U0 i0 o8 k7 `8 l  M' r$ G0 _; h/ D
  48.       }
    3 H* K, G; M4 X! ?% r: m
  49.     } . w0 S/ O: v/ o! j9 g: l! c% b
  50.     MotoL(nPower-Error);3 a& t! u9 K8 b, j+ s
  51.     MotoR(nPower+Error);    0 P" a& k1 y0 y# Z. h9 V1 U
  52.   }else{
    " Q$ `" r/ V1 M
  53.     motor_9.run(0);
    - c* y; I8 x6 X# {7 N
  54.     motor_10.run(0);( k) e6 q. n# F2 R2 K* ]
  55.   }! j( F- y+ z) t7 j! h' t/ a
  56.   do{}while(millis() - StartTime < interval);
    % ^# `6 i: N  P$ m7 ?
  57. }
    ( C# r: Z5 W. p) Z2 ?

  58. $ ^2 W1 {9 \- ]' B
  59. void MotoL(int Power){
    : r0 |3 u( c# @6 ?/ r- D6 N6 \* L
  60.   if (Power > MaxSpeed){
    5 r- K8 R' I; N$ s
  61.      Power = MaxSpeed;
    ! {! A- C) S5 E0 v9 {8 ~6 S
  62.   }
    * @; p2 h) M2 ^( C7 a9 \9 B
  63.   if (Power < -MaxSpeed){$ l. E, g6 z+ t, k; d! _
  64.      Power = -MaxSpeed;+ v3 n3 L7 c; V3 k" c. \
  65.   }
      X  w! D- k4 m
  66.   motor_9.run(Power);
    2 A, M$ j, Z2 p# ]- H: U
  67. }  
    8 `. m) o, Y0 z3 t

  68.   A/ X/ s. G) ^1 ]
  69. void MotoR(int Power){: k; y5 j8 I; y) {  Q; i
  70.   if (Power > MaxSpeed){
    5 T# `( z  A. t- |6 b" e% L5 B
  71.      Power = MaxSpeed;
    + L5 H+ b1 h% X# u; }: k2 s  A
  72.   }
    ) {0 j+ i$ |6 X9 i9 k7 X
  73.   if (Power < -MaxSpeed){
    % W) F! L" X8 r3 l+ f. G
  74.      Power = -MaxSpeed;+ V  ^  @! P2 m" \9 j
  75.   } 6 {$ `6 E- w  `+ H( m% u
  76.   motor_10.run(Power);; k7 I! [; U% P/ e# |5 \
  77. }  
複製代碼
5 _  o6 [+ N: z. e; o% o
$ K; x8 V0 K% g4 w' j) W+ G1 ]
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯
& l) Y6 W/ i* }+ \: S8 |& @# |. J, n$ U. F0 h# _
您好,不知是否能向您請教。" P1 x. `' ~" X
目前和宋修賢老師在處理Ardui Car
* ~5 R- t. i; E雖然已使用較繁雜的方式處理了跑出黑線外的狀況* u/ |+ M5 B6 U0 d

1 u! m$ V1 s" d$ J$ F( V) k但基於想追求更精簡的程式所以還是想請問一下
* U8 K* p) f% c/ @4 }  X# o! m3 j9 i就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝7 P1 q) h. ~/ I2 h
不知道您是否願意教我可以如何處理- j4 _# Q# ^) ^; D9 h! N

2 t! R+ c2 C8 f7 I
3 o0 o. X4 r+ A( O* b以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;5 e, U1 @7 C! a* O2 X' M* x# X% e
  2. double CNY70Val = 1000;) y4 M3 p, N( i
  3. int interror = 0;! o0 D. z+ {2 G
  4. int olderror = 0;
    : W4 f' u1 ~6 {! W3 M0 T  G: m
  5. double values;
    " Z' n/ H) C4 P7 N" x% h

  6. 5 V' a1 D# `( n  Q3 d
  7. ; \7 p6 e( t* H% k/ T8 S, `
  8. void CNY70()
    " v! T. R3 f) i+ d
  9. {, R& Q' h3 c: D) i6 R8 k( z& f$ L
  10.   valuesRR = analogRead(RR)% d+ D3 ~, l' v( a$ V
  11.   valuesMR = analogRead(MR);
    ' L7 {6 n7 V( ~4 `, D- O$ X
  12.   valuesMM = analogRead(MM);
    , Y* _5 H) I' D- `5 @: Q, B6 f
  13.   valuesML = analogRead(ML);
    + n2 g+ p% o6 E
  14.   valuesLL = analogRead(LL);
    . _  }$ ?! E9 m

  15. ( ~/ U% x5 r% I5 T8 T% Y
  16.   if (valuesRR > CNY70Val)2 z: I  j, P& w7 U4 Y
  17.     valuesRR = CNY70Val;
    9 U5 S  i* \9 h5 U; d" `
  18.   if (valuesMR > CNY70Val). X2 D& J" ?7 S" E3 }
  19.     valuesMR = CNY70Val;
    # T* o9 {: ]/ H5 o5 i$ T
  20.   if (valuesMM > CNY70Val)
    8 [4 W) g0 v/ u& C3 i$ j
  21.     valuesMM = CNY70Val;: T6 o* Y5 Z( X! v/ d8 ?
  22.   if (valuesML > CNY70Val)
    ) a* {# Y6 h4 W
  23.     valuesML = CNY70Val;9 g. H0 H, E$ I; z
  24.   if (valuesLL > CNY70Val)( K$ w  W. u: k
  25.     valuesLL = CNY70Val;0 [5 f+ k/ {4 F" c3 |7 b
  26. ) v/ E: D8 W4 Q+ K
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;- B* B. p6 ^. q; e$ q
  28. }
    , _" g) B8 @7 J5 @! k* L3 B& Z
  29. $ l# V6 \9 y3 ^* Y
  30. void Car()
    * K$ _/ j4 {0 F  d! w8 |( x
  31. {
    ' r" `6 k* h, b. ~2 f+ t# i9 Z
  32.   while (1) {
    2 j  `* a7 B2 D5 I
  33.     CNY70();
    9 g+ [$ m  {0 X4 A1 O/ v4 ?6 \! x

  34. ! J- o/ |( x5 V
  35.     int error = ((int)values);' b/ q7 X/ D* s( a% U( w) R4 S( b
  36.     interror += error;6 B9 ^. L1 E8 ?3 ^/ P* i
  37.     int lasterror = error - olderror;6 T1 Z3 N  i+ @8 ~
  38.     olderror = error;
    - w' D3 p# V# P5 r* l( ]
  39.     int power = error / 5 + interror / 10000 + lasterror;. M* a8 ~2 ~; b0 A" E
  40. 2 @1 @1 F$ A/ i9 E7 Z8 r! H
  41.     if (power > MotoSpeed)' |3 t3 [& x( o2 g5 s' @0 K
  42.       power = MotoSpeed;, G) _; g, q7 Y2 {% x1 v+ [( M# G, f
  43.     if (power < -MotoSpeed). ~7 I; ^8 G8 C' Y+ s- ~
  44.       power = -MotoSpeed;* D# a& ?4 |! H! d( w

  45. $ R' a# |6 C2 ]1 a6 u
  46.     if (power > 0)
    ' U; T* d- E' V5 @2 p+ a
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。
    + k# X( l  ^. t  j; s! v
  48.     else
    & o2 i1 @4 P- A  h( D5 r) w5 D
  49.       Speed(MotoSpeed + power, MotoSpeed);
    . ?( Z4 U8 L0 G
  50.   }
    . e1 |) U+ |2 o: c/ o
  51. }
複製代碼

( g. G  d6 _! y3 f- j& Y  k( R: D, d" t0 J
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-12-2 08:48 , Processed in 0.023995 second(s), 17 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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