圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21830|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>
    0 v1 e" _/ U/ I- @0 l+ x2 w9 K
  2. #include <Wire.h>3 g+ r" W& o9 b8 E" P
  3. #include <Servo.h>
    ' s! }% f. x, m5 y1 m3 I3 R
  4. 7 J7 r5 B+ ~, W3 \; N1 Z  L' ]
  5. #include "MePort.h"
    % B: P. [6 k3 u5 j" h$ T$ V
  6. #include "MeUltrasonic.h"$ r  R) n( [: _/ ~  W0 N
  7. #include "MeDCMotor.h"- V2 N1 D" A1 ?* X* ?# Q
  8. 2 o* Q3 i8 V7 M* _: t5 y
  9. //double Input, Output ;9 P, q+ d& ~% Q& Y4 m. ~
  10. float MaxSpeed = 255;- k9 k; s+ I- P* ^
  11. float MaxPower = 180;% W; N- g7 ?) u0 I1 r, Z6 R
  12. float MinPower = 120;6 T  F2 C/ M& x# b5 Z' T
  13. float Error,ErrorAcc,ErrorDec;
    . ]0 O8 @8 t, t' ?! y

  14. 7 p: ~1 D5 Z. l
  15. float Kp=0.14;2 L$ @' ^$ `5 X8 n, J
  16. float Kd=0.00020;//23;9 z5 T# Q1 D- U, c& t* y6 U
  17. float Ki=0.000201;
    8 M) h3 \; M" j! c+ c9 S6 y) h

  18. 0 l7 r' U- V" w# X& Q4 ?/ G
  19. float nPower;
    8 G2 l/ T4 O% [8 M
  20. MePort lightsensor_6(6);6 T6 l3 N1 r$ A  D$ T3 }
  21. MePort lightsensor_8(8);
    ! r. k) d$ b: u- A
  22. MeDCMotor motor_9(9);
    * t7 ~( X, o' T: Y1 Y' v) N! Y
  23. MeDCMotor motor_10(10);+ Z+ A: D, g1 b+ D. X
  24. unsigned long previousMillis = 0;
    6 P% W0 x4 l7 ]- x7 w
  25. const long interval = 1;
    . G+ ^' P4 m4 {  q* Q' V! ]

  26. ! v3 H2 G9 l+ P8 y& T! ~9 I; _
  27. void setup(){
    / p8 {# E3 ]3 |2 ]% C7 a0 o9 h
  28.     lightsensor_6.dWrite1(1);
    5 q; M0 t/ }5 Y
  29.     nPower = 160;
    3 n6 D8 O; K5 ?# C
  30.     Error=0;
      {9 B! H& ~. z) d4 L
  31.     ErrorAcc=0;6 P1 n: x4 B  J" q
  32. }" Y5 J: D2 I  m" z
  33. : ^( P2 L7 \3 g" R
  34. void loop(){2 L5 s0 C6 k4 m. b8 w. f/ u- f6 d
  35.   unsigned long StartTime = millis();* S  `6 C" F) d* d7 V, I- a
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    ) v* m- a  B7 A2 x$ A8 L# }5 K4 U
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();; m$ B# F& ?5 X0 j4 {
  38.     ErrorAcc +=  nError*Kd ;
    4 ~. c. I1 @( v6 ^; x% u8 V
  39.     ErrorDec -=  nError*Ki ;: D2 i  T1 v7 x- c$ w, R
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    $ m8 D9 f% e' u; O0 I
  41.     if(nError < 80 && nError > -80){
    & v1 t( J2 f2 P+ D/ U. z
  42.       if(nPower < MaxPower){
    . Z# k, ~: v# n6 s
  43.         nPower += 3;/ p2 ~, ]  H0 L+ \
  44.       }
    3 m8 c. ^9 R& V$ V% G2 Z; U
  45.     } else{) n5 a$ b# O, A  m+ Q- ^# x- ~; [
  46.       if(nPower > MinPower){
    , e6 E4 v, V' [+ O+ K
  47.         nPower -= 2;6 Q$ P; f. d8 _5 ~
  48.       }' e& a  X  \, G8 f, m" L
  49.     }
    6 {) f8 n) ]  |" A  U5 T% |
  50.     MotoL(nPower-Error);
    0 }. F, N3 Z; v  P  r% R( ^
  51.     MotoR(nPower+Error);   
    % L- y) X) {, n! y/ O
  52.   }else{  C8 G4 m: W) }4 ^" i: K9 f
  53.     motor_9.run(0);
    ; y2 \- P. b  e( y' S
  54.     motor_10.run(0);* M/ c) c/ A/ u+ Z1 S  t
  55.   }
    / {2 W' B6 L" U2 S9 n8 Y2 p
  56.   do{}while(millis() - StartTime < interval);
    0 ~$ r. R, T9 o9 p" f( J" M$ n- T
  57. }* V. a1 M1 B. _6 k8 Q% E
  58. 8 p' ]! s1 E6 A( E) ]6 t
  59. void MotoL(int Power){
    # m' V7 s( A: i, S1 C
  60.   if (Power > MaxSpeed){4 ?: t' X2 A1 }, z
  61.      Power = MaxSpeed;$ r% Q. L! m7 X2 j; x  ?
  62.   } * A' L! m/ e5 t
  63.   if (Power < -MaxSpeed){
    % i& `, _9 M) r! D
  64.      Power = -MaxSpeed;
    ! C3 R! h! s' r4 L* ]2 [' P
  65.   }
      a0 y  {" R8 T
  66.   motor_9.run(Power);% ?0 t3 l  O5 q6 D* X. e2 `& ~' r
  67. }  , w% Q- ]. t% T: X' J1 l( J9 S
  68. ) v9 e0 J$ |* Q( o6 b9 r
  69. void MotoR(int Power){) Z$ c. l/ E8 B8 n3 M. R
  70.   if (Power > MaxSpeed){
    ; l/ y* t6 |9 Q3 M. O
  71.      Power = MaxSpeed;
    4 A7 l& X! m8 Z. V
  72.   }
    + V$ M! \' R: B7 V, {& D! [
  73.   if (Power < -MaxSpeed){. U5 `8 e: X4 M* _, @
  74.      Power = -MaxSpeed;# t' F0 c4 y) h1 Y( P
  75.   }
    , P/ y& ?% y8 x& u
  76.   motor_10.run(Power);
    & A  N6 B( `: x0 j# U
  77. }  
複製代碼
( d0 Y  g" d1 d* I5 \: Z$ Y

# q( p# N/ R- f5 {( B
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯
4 s! B/ o. Z( }9 p% S: ]2 a3 \' n- `  j; _; b; g% k6 e4 L
您好,不知是否能向您請教。$ x+ T# a5 ?+ E4 X  m0 q- |
目前和宋修賢老師在處理Ardui Car
& X1 `5 n/ {0 l5 c3 S. q雖然已使用較繁雜的方式處理了跑出黑線外的狀況8 [  i8 r6 R' y/ x* \% d/ n8 L

$ l3 Q) H5 J- E1 K, ?: Y但基於想追求更精簡的程式所以還是想請問一下$ }7 g+ F, h0 q- t
就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝
; I7 ?! m, \( w% ], ~  @2 H不知道您是否願意教我可以如何處理6 s% ?5 r; I* \& e+ a9 `8 C
/ p+ q' r6 }3 |( ?  w& R

+ O1 d* p- j% A+ A1 N; p以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;( M1 Q/ F) \6 I0 O/ l1 [0 U
  2. double CNY70Val = 1000;
    3 e: R/ o" H9 D0 j$ O' a1 P# G
  3. int interror = 0;
    + j9 _/ |8 {! Z3 p
  4. int olderror = 0;. H7 _$ B) {" I$ {
  5. double values;
    & G/ O0 B  m/ X* ~1 k3 @5 D

  6. 5 `+ \- Y! k, p$ ]5 Q4 K* s1 q; C
  7. 9 A4 r0 l! c! i) M. e6 ]
  8. void CNY70()- ?! A# U+ V% F: g6 W  ^% ^
  9. {
    / V: i3 D. J. p9 T' G$ X
  10.   valuesRR = analogRead(RR)
    % A. s) r9 d8 g3 l2 H% K! ~
  11.   valuesMR = analogRead(MR);
    ! `; Y9 K4 }! s$ [: T$ F6 N& g4 k
  12.   valuesMM = analogRead(MM);
    8 |' Y* Y3 G6 T$ @2 U
  13.   valuesML = analogRead(ML);0 m! w3 C# J' b8 ]; f9 X$ b6 T
  14.   valuesLL = analogRead(LL);  m: f# T. X$ t% d

  15. * Q5 `3 P( t) k3 T' T
  16.   if (valuesRR > CNY70Val)9 k  m( `3 E+ w2 U( L
  17.     valuesRR = CNY70Val;4 B9 |9 J7 n4 `* R; {
  18.   if (valuesMR > CNY70Val)
    ! g9 J- G4 ]  {7 O# D2 j1 h! C# z
  19.     valuesMR = CNY70Val;% |; `; D& Y! b; a6 ?# m$ L& |0 I4 u
  20.   if (valuesMM > CNY70Val)& J& v3 E; Y* F& p( u) a
  21.     valuesMM = CNY70Val;
    + W% r& C. B" n7 _/ a
  22.   if (valuesML > CNY70Val), c- B* n. o, z+ `, l  E
  23.     valuesML = CNY70Val;+ r' K  V/ F- Y! ^* E7 E
  24.   if (valuesLL > CNY70Val)& e$ N6 v) g8 U: K9 u7 h& u
  25.     valuesLL = CNY70Val;$ |1 g, T) V1 ~( W1 W6 h

  26. , |9 ~& Y3 V% a7 p3 M. q% K
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    / t6 w7 z2 Z& y# H4 p1 w
  28. }
    2 o4 X+ l; c- I; ?& \
  29. / c- Q6 ^: g) z9 ?- q3 @4 D' F
  30. void Car()4 }) n  l1 n/ D
  31. {
    . o4 V, k* i3 f2 H7 v: T
  32.   while (1) {
    % v) L/ E2 ?, m; @& t( ?
  33.     CNY70();" [4 F; ?; V5 b1 b5 S4 `8 U

  34.   z7 Z1 M1 X& ?, \8 ~
  35.     int error = ((int)values);
    : x, T; A; G. {
  36.     interror += error;% l! t4 `* \, Q% X5 T
  37.     int lasterror = error - olderror;
    % a9 f" Z# p) Y
  38.     olderror = error;
    8 m; X+ M  a5 F9 g, \9 k1 i
  39.     int power = error / 5 + interror / 10000 + lasterror;
    , T. L4 ]$ T5 }8 [

  40. , [+ q$ }. J5 y2 Z% I5 O
  41.     if (power > MotoSpeed)3 H" X& r, c  I- f5 U* P
  42.       power = MotoSpeed;
    3 |) W2 S2 O3 A- [3 `  y! s
  43.     if (power < -MotoSpeed)! I) r' [8 |6 B% p: Z% ^2 R. ~
  44.       power = -MotoSpeed;
    ! G. a/ }) k: h' Q
  45. 5 M4 ?" \2 i  m8 [3 M. Y/ _6 h( `; i
  46.     if (power > 0)
    ; K" D' C6 F: s+ Q: V& a+ h8 c1 ?
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。5 K, G0 ?+ m5 O" H1 J4 @3 \
  48.     else
    3 u: V1 p, Y" b& A& V3 q
  49.       Speed(MotoSpeed + power, MotoSpeed);
      Y1 r: z- }) v9 q# O* _& E5 f
  50.   }
    5 f8 I( O2 {" D5 B! c4 v. ]
  51. }
複製代碼

  ]' {1 _1 b; V6 c- ^3 t5 }
6 q( e2 e( d: N+ S5 q* O
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-12-4 10:15 , Processed in 0.025568 second(s), 18 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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