圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 20951|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>9 ]6 d! O. R2 J6 F& ~
  2. #include <Wire.h>  y& t, D' o2 p6 b3 I  c: o+ z
  3. #include <Servo.h>+ x6 h/ f- L2 R: K4 C' I# l  ^
  4. - }+ M" X/ E$ Q& V
  5. #include "MePort.h"" L! E0 r% W: r% t
  6. #include "MeUltrasonic.h"
    + F8 E- h  e/ G* ?6 @
  7. #include "MeDCMotor.h"+ L( l. }% l1 q% ?  e

  8. 8 c) B0 O9 ?' }+ S. ^
  9. //double Input, Output ;
    ( o$ ~$ V; S) T7 x, ?! h! K
  10. float MaxSpeed = 255;
    # F7 M3 x& m" u. s5 _
  11. float MaxPower = 180;
    - |$ {# O- E( q5 A+ o! X% s
  12. float MinPower = 120;( `. `3 K# d9 i( g: C! |1 M# l
  13. float Error,ErrorAcc,ErrorDec;
    ' R1 c9 o' g* c* r% f# ^4 r8 H
  14. , l9 T( s: C0 d" x4 r/ p: r
  15. float Kp=0.14;  r8 H" d# I0 R
  16. float Kd=0.00020;//23;' d2 G2 m6 p5 r/ t( x# d! Q
  17. float Ki=0.000201;9 q$ O- E- A4 A9 A1 v

  18. 0 H6 u+ G/ K+ S0 T" ?; }" A, S
  19. float nPower;6 q6 N; _" d3 p2 \( i+ ?; ~; V9 Y" L
  20. MePort lightsensor_6(6);
    * Q! |( R4 b$ Q1 T: |6 I8 T0 r
  21. MePort lightsensor_8(8);
    4 W4 a0 b. U/ e
  22. MeDCMotor motor_9(9);
    , p5 p9 P7 g4 `0 p% @
  23. MeDCMotor motor_10(10);
    " T6 ^/ ]) C% M% V
  24. unsigned long previousMillis = 0;/ Y2 n8 n6 Q7 z( m$ Y
  25. const long interval = 1;1 G8 M6 q$ o7 O$ s/ |5 ^

  26. 4 r7 \+ D+ o4 X9 O1 i: s. Y8 U
  27. void setup(){
    & B- d8 k& H9 c. {3 F% E
  28.     lightsensor_6.dWrite1(1);8 ~5 R. A& \& p9 _  ^; N; U
  29.     nPower = 160;, s0 G5 z8 j0 T
  30.     Error=0;* L. d' R* p! g/ r  ], d
  31.     ErrorAcc=0;
    - j9 X/ S2 P. R2 C. y+ D
  32. }
    . P1 l- ]) v- D9 `" L9 v

  33. 5 u. m6 z, v/ L- }, }
  34. void loop(){0 g# z1 `9 Y5 g* E3 h1 d! l  p
  35.   unsigned long StartTime = millis();
    & [' i7 [( F$ c" @6 \. n
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){' ?$ e! w8 f! e
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();3 Z% @! y" B1 I2 i
  38.     ErrorAcc +=  nError*Kd ;6 ~% n% }8 J) m5 h2 \7 ?" l
  39.     ErrorDec -=  nError*Ki ;6 A2 g+ Z( `( B4 ?- R
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);; g9 o) I- r: w2 D( J# @
  41.     if(nError < 80 && nError > -80){% b' l. e9 Z6 K
  42.       if(nPower < MaxPower){
    9 p# }+ e) D& d. p/ m
  43.         nPower += 3;
    ! y5 m; z( @4 N
  44.       }
    / @% G- J$ S8 z% I7 o9 {
  45.     } else{
    5 a1 ]0 S- C* \2 l
  46.       if(nPower > MinPower){1 v8 V! F8 W) T6 a; m9 a: w* H
  47.         nPower -= 2;
    % s) @, ]) p& P9 s8 q6 H6 L
  48.       }# g+ `5 V7 a, _7 I$ o, r& `
  49.     }
    * @5 F; w- G# }, u- \8 D0 H
  50.     MotoL(nPower-Error);6 ]5 L/ n4 I+ |* v' k
  51.     MotoR(nPower+Error);    5 x1 Y  S/ y* H
  52.   }else{5 W- t% O1 l% {' W9 n6 z$ t7 v% M! `* x
  53.     motor_9.run(0);$ R6 o% k+ {- T
  54.     motor_10.run(0);
    / s6 A; c1 r4 J2 i* f8 z
  55.   }
    9 E- d7 X  _% y$ {
  56.   do{}while(millis() - StartTime < interval);6 m: s3 M' j& l7 f
  57. }
    " Q$ `, N8 r2 B* q: J
  58. & u" T3 w$ ]! t/ M. B( R
  59. void MotoL(int Power){
    . V- E1 m- F( g9 L  T- s( G. Q5 n3 m
  60.   if (Power > MaxSpeed){
    4 j3 k: ?* d( s6 I
  61.      Power = MaxSpeed;
    3 L) B5 e) q) |  V; ]
  62.   } ( J  @+ j4 V6 {, {& l% U( U
  63.   if (Power < -MaxSpeed){
    5 B4 w  J) N  h3 L" L* n
  64.      Power = -MaxSpeed;
    $ Z5 f/ J, V7 r+ l
  65.   } / t* L& E  m% _; U( g# h5 ]
  66.   motor_9.run(Power);
      f8 Y5 A. X. G5 |
  67. }  3 t" f7 r  A2 L" Y7 ], Q0 g6 ^

  68. 9 V: A2 @" _5 b
  69. void MotoR(int Power){
    ( w" q) K) F4 }( V
  70.   if (Power > MaxSpeed){6 v* p9 D# V' Y
  71.      Power = MaxSpeed;
    + D4 c% S& `  t1 b
  72.   }
    5 W# T: Q0 d7 U
  73.   if (Power < -MaxSpeed){
    $ N( z* J; t# ]% `- \6 U/ s; X
  74.      Power = -MaxSpeed;; ^5 a% r, k' Q$ _! k9 k
  75.   }
    . D: [3 Q3 s/ _. W
  76.   motor_10.run(Power);( W$ T# O/ h1 w# \( s( l( ^
  77. }  
複製代碼
. G0 ?, Y! z) w7 T: g/ ?

1 j( ?3 [( F, `7 n
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯
3 n% l% n  [" O% h5 N5 u- W* b9 X3 ]9 j% v, F/ }5 E; N% `
您好,不知是否能向您請教。
/ X. d0 z* M3 A目前和宋修賢老師在處理Ardui Car
) L2 F$ ?/ w3 E( [( @( d雖然已使用較繁雜的方式處理了跑出黑線外的狀況3 ^* G( _/ M. l
  j, d$ I; i% X0 h! G
但基於想追求更精簡的程式所以還是想請問一下
, ^% O) d/ c0 o就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝
3 n3 K5 g* s; k* c; A0 z6 C- b! s不知道您是否願意教我可以如何處理6 H  Q& E! s0 U- H+ b. C
; k7 u& d- X. k$ m

+ W$ g1 d  h0 f# V以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;
    ! [- z" j* f1 E6 l
  2. double CNY70Val = 1000;
    # m# E/ S) s% A2 t. |1 H# j
  3. int interror = 0;7 l, O( M: G. D% U
  4. int olderror = 0;
    % p. U2 D. Y! [) x  y
  5. double values;7 S8 @2 M- S1 F* h5 M5 h6 q

  6. ; J+ \: T! c* {
  7. ) v9 Y8 M2 ~: z& Q3 |' k3 u
  8. void CNY70()
    1 q1 k# h( J8 K; f+ [8 u
  9. {
    % V) ]. e9 D& a6 i& T7 E
  10.   valuesRR = analogRead(RR)8 J& N: E0 p1 ]5 @
  11.   valuesMR = analogRead(MR);3 U+ u' u* L; |- Y7 v
  12.   valuesMM = analogRead(MM);
    1 G& w4 R% T! Q/ g! o5 ~; N+ B
  13.   valuesML = analogRead(ML);: c/ f( K& Q! E% k5 k
  14.   valuesLL = analogRead(LL);
    $ z) n9 n3 G) N! F
  15. $ y) N  k6 [+ E6 r; Q. y& G, |
  16.   if (valuesRR > CNY70Val)
    ) ]2 X% G1 k+ R+ `2 s) h8 L- T
  17.     valuesRR = CNY70Val;3 T: k; h9 u+ m. l! r
  18.   if (valuesMR > CNY70Val)2 r/ l* }( z1 {- a3 ?5 I
  19.     valuesMR = CNY70Val;9 N) T$ G, w; c/ ?
  20.   if (valuesMM > CNY70Val)' m8 S; z* Z% V5 \) W2 Q; r
  21.     valuesMM = CNY70Val;
    ( G$ C9 W0 {% Q5 }6 i3 ]# v) w# b
  22.   if (valuesML > CNY70Val)
    - A% r0 v$ k0 ~; q3 v5 ^. R
  23.     valuesML = CNY70Val;# [* _- {( c+ ~# Z0 N8 i7 D
  24.   if (valuesLL > CNY70Val)
      I1 O" A0 O: T5 m
  25.     valuesLL = CNY70Val;
    . v; ?3 ?0 e4 I$ G
  26. % f! x) l% V7 ?) {" L: c7 F- n
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;( [; Q5 A. _# }  H' V4 ~
  28. }8 l7 O2 |9 X" L) ^. ?" O. C3 z

  29. ! y- X; x0 d: D" a9 n, l+ u
  30. void Car()+ D& t; `5 I' H& {% Y) a1 ?
  31. {+ g* ^) M0 G! O$ P- }! p- C% n
  32.   while (1) {7 r% c' J: i3 B6 v4 I
  33.     CNY70();2 }: z0 n$ f1 K  q3 f0 X
  34. 8 b# V, @: ?3 `
  35.     int error = ((int)values);+ H2 G+ s9 t! O$ `: Y
  36.     interror += error;
    ( K8 c! [5 U) ~0 K% S5 U$ ]
  37.     int lasterror = error - olderror;, b- ], A4 l" b- r* P8 i& z" l/ ?7 c
  38.     olderror = error;7 ~# l2 k* ^7 M  H) P
  39.     int power = error / 5 + interror / 10000 + lasterror;
    # c1 q3 B8 M& }7 K' `! `( M0 R

  40. & y' d2 U# o& o% _; G$ f& @
  41.     if (power > MotoSpeed)+ I; Q/ ~! Q0 D9 @
  42.       power = MotoSpeed;9 @9 @  c( ^+ u# X5 X
  43.     if (power < -MotoSpeed)7 O5 z2 F$ q4 r* Z' o* Q
  44.       power = -MotoSpeed;+ P! P6 f! {' |3 W

  45. % z, E: J1 o' B7 C' L( P! r
  46.     if (power > 0)
    ! Q8 W( C2 H7 R$ `+ A  }; T) Y
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。9 s( D& e$ F5 Z3 y1 J2 Q+ b3 ^9 `2 F
  48.     else
    0 b5 o+ i" N4 y7 s
  49.       Speed(MotoSpeed + power, MotoSpeed);
    # U% S, I& ?4 K/ M, o, m
  50.   }
    , j6 z  Q$ z5 n$ S3 I' R- X
  51. }
複製代碼
0 I; @' ~7 N# M  @- g
* Y' G' ]1 K( m; j: K& M
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-7-9 14:02 , Processed in 0.025223 second(s), 17 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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