圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21832|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>- W8 n$ c5 _9 \) H5 r, e; s1 s
  2. #include <Wire.h>/ @4 q+ z2 V- K" w
  3. #include <Servo.h>
    ! c+ y- e$ T" _, P

  4. / d- n; H8 C0 f# a( K
  5. #include "MePort.h"0 F1 k& E2 ~  b0 r3 l( R! _) M& t1 C
  6. #include "MeUltrasonic.h"
    ( g& |( ~0 q8 X6 [5 m
  7. #include "MeDCMotor.h"% y1 ~5 c% U6 \4 K' H5 ?; |, Y! k
  8. $ P8 u! c! n  U5 o: }
  9. //double Input, Output ;* n5 m- x2 D3 j8 A! {+ t
  10. float MaxSpeed = 255;7 R( P% A: N9 ^9 y' J5 f
  11. float MaxPower = 180;' D3 K6 @+ _2 s  O, q( e! z- z
  12. float MinPower = 120;
    ) c0 |: Q. D" k! P! X3 U1 g
  13. float Error,ErrorAcc,ErrorDec;# l$ [& N; @3 G% Z) f6 A- l( b0 ^

  14. 8 P4 T9 r4 D( X! S" N7 Z8 Y
  15. float Kp=0.14;
    ( R! G3 D. O# f+ F& |% a2 g/ S. \
  16. float Kd=0.00020;//23;3 A3 q6 Y3 c+ q! I
  17. float Ki=0.000201;. ]' v9 @/ {1 J6 Q

  18. 2 B9 ~) G& U! \) o
  19. float nPower;2 M, M* M- T3 z
  20. MePort lightsensor_6(6);
    : o8 B) f2 {& [" K
  21. MePort lightsensor_8(8);" H: y8 `; @; N, K( Y3 T
  22. MeDCMotor motor_9(9);
    * s) Y7 ?& D1 P( b* R
  23. MeDCMotor motor_10(10);
    - I' R0 i5 \5 H7 L, Q$ B+ Y
  24. unsigned long previousMillis = 0;
    ; ?- a  t; h) b& u7 d  k7 v# \
  25. const long interval = 1;+ Y1 ~8 n! f/ C7 ~* [! r& M
  26. 0 K1 A! n  W) j2 o( A+ v8 m
  27. void setup(){, ~5 i. F# h' K' }% L% U( t
  28.     lightsensor_6.dWrite1(1);. z, s& r7 S( Y4 X% @
  29.     nPower = 160;
    " U8 b' f* G% J5 z. C1 t8 m' w, j
  30.     Error=0;
    / B: D% C4 U( K& l
  31.     ErrorAcc=0;+ w1 h2 T8 |2 ^# U
  32. }
    5 G/ k4 ?. q( i- L

  33. ; e- i/ F! ?( u, s6 _( T9 W+ N0 M
  34. void loop(){! z& h3 D+ a$ ?" L3 t, D5 D
  35.   unsigned long StartTime = millis();2 v" B. f5 Y# [
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){$ c- E3 _& Y9 W. O
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();2 G5 }- n# h% O0 [/ G5 U
  38.     ErrorAcc +=  nError*Kd ;
    % m2 f# O) p* u7 X
  39.     ErrorDec -=  nError*Ki ;1 H+ O! H4 r0 o( b! r  E5 l
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    6 ~& {- k2 \: ?* w2 k) ]& t
  41.     if(nError < 80 && nError > -80){- \" T. s- u& m/ |; h+ t
  42.       if(nPower < MaxPower){; T* I. B0 t, B- g1 ~1 F
  43.         nPower += 3;$ Z+ B* ?" H4 C' M3 Z
  44.       }8 U7 E% e8 t8 X. @6 Z: {6 Y8 d
  45.     } else{7 b! K) M, `! l; T6 k+ L
  46.       if(nPower > MinPower){# E7 N) {, q9 V6 N8 o
  47.         nPower -= 2;
    6 ?# o) y' `$ }5 Y5 h0 K& `* e8 C
  48.       }/ l' ?, j/ o# ^( N# o3 S1 M. W  E
  49.     }
    : R7 ^; U; m/ E* W
  50.     MotoL(nPower-Error);
    7 [+ g, w/ _% Q
  51.     MotoR(nPower+Error);    ( e: T, V) }; a& s9 t* Z) A
  52.   }else{& q$ e- p! Y% t( k% z
  53.     motor_9.run(0);2 N, H# ?/ F0 S' u- J
  54.     motor_10.run(0);: s, h8 q' B2 T! e4 k
  55.   }2 w; m: {, L" q
  56.   do{}while(millis() - StartTime < interval);
    . r. c0 X3 ~! f; B9 q, ]+ {
  57. }% B2 {2 p. Y! G2 O6 ~# R( _# G

  58. ' [* N! T/ X7 l
  59. void MotoL(int Power){/ v, ^. v" D; o2 z% P: Y0 v) J
  60.   if (Power > MaxSpeed){) e0 p  L" S* S  D3 C, u$ O
  61.      Power = MaxSpeed;6 P/ c' u* m% Z, h6 P( Y' ^6 Y, r
  62.   }
    . u, @+ ^" E; C' H2 c
  63.   if (Power < -MaxSpeed){
    & R7 f' G& x* q; G$ F
  64.      Power = -MaxSpeed;/ X& X: n. y% X) Z/ @1 D% c
  65.   }
    ( [" ]+ k1 I. X# S+ z" T* U' I3 ~" d
  66.   motor_9.run(Power);* p) @" I- T6 Y# X/ b
  67. }  % W- Y3 ^6 J' j0 b8 w8 z7 s" {* q
  68. 7 U  A  a/ t5 i5 L
  69. void MotoR(int Power){! b3 m8 e* P7 W9 Y
  70.   if (Power > MaxSpeed){' ]- F7 a' p: Z3 c0 J# p) g6 D
  71.      Power = MaxSpeed;
    1 m' t/ `( L$ L4 W0 b1 O3 H
  72.   } 1 e$ W8 ~- I' O) S7 [6 p- Y5 G
  73.   if (Power < -MaxSpeed){* K9 ]' F. p7 R. `  S7 N: ?" {
  74.      Power = -MaxSpeed;3 V! {/ b' V. m! t" q5 [
  75.   } ' O: h, b, n/ a
  76.   motor_10.run(Power);2 g- P$ {* y" ^
  77. }  
複製代碼
8 }6 q' F! ^/ x1 x
, b: ~& v& K! E: m2 T$ j: V8 G) W
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯
2 y' C7 v1 P' Y/ ]- I& J( y
& ~1 C5 F1 Q& Q9 V! `您好,不知是否能向您請教。
  I$ T/ Z( J; T6 N' g& N目前和宋修賢老師在處理Ardui Car0 T. M0 m6 E- f8 a
雖然已使用較繁雜的方式處理了跑出黑線外的狀況% p) Z" b+ i* y

5 G+ j8 Y5 f/ T5 c+ a; X! Q5 M但基於想追求更精簡的程式所以還是想請問一下
) X1 I0 M4 J8 q% w, n* v就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝
9 X* B) a1 g! b% a不知道您是否願意教我可以如何處理  j( B' z$ q& a9 L0 Q- ~) l

8 W" r! X8 }  G+ `! J  F9 R$ W5 ^. a. m
以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;
    7 y5 B! D+ y5 A$ R/ C) e
  2. double CNY70Val = 1000;0 G. C* ?6 s2 E# q, b8 ]
  3. int interror = 0;+ l5 [% K$ S2 G) D: c
  4. int olderror = 0;
    ! y' T6 n# V3 n& i+ v$ m
  5. double values;
    , e0 n3 R$ |, ^' W5 Y+ Y( O
  6.   v% z8 p: t" r0 ~8 L
  7. - @; g  Z& g& {: _( \' e
  8. void CNY70(). M8 y- d% s- ]* V
  9. {
    * \  M" y& _# K. {
  10.   valuesRR = analogRead(RR)6 q* a) ?# M+ ?
  11.   valuesMR = analogRead(MR);& G; L* `! b3 l; ]
  12.   valuesMM = analogRead(MM);
    2 i  h# h! l" u3 y. @  o4 p
  13.   valuesML = analogRead(ML);6 s. m8 l) H3 u
  14.   valuesLL = analogRead(LL);
      T$ T7 m) v+ g* f
  15. $ W) y1 {1 Y/ ]* T: `1 ]
  16.   if (valuesRR > CNY70Val)
    ) s0 K8 a' Z' o7 \  K9 q* s7 j8 T2 ?
  17.     valuesRR = CNY70Val;
    . o3 _- c$ e. F" |8 Z+ @& y" m
  18.   if (valuesMR > CNY70Val)
    ) a& f; C+ @. F! C8 Q' O$ n9 h; _
  19.     valuesMR = CNY70Val;7 k3 t6 \3 z, w6 U7 L9 o  }
  20.   if (valuesMM > CNY70Val)/ Y$ B! L' ~) M* n4 f: o
  21.     valuesMM = CNY70Val;
    2 g" d$ k5 T7 e9 d- Z  ?6 U& v& a
  22.   if (valuesML > CNY70Val)4 s: K7 E% V# k" ^: T
  23.     valuesML = CNY70Val;- O, ~( D, ~* _. f0 y$ m( j
  24.   if (valuesLL > CNY70Val)3 W7 G* g2 P; n5 v8 H
  25.     valuesLL = CNY70Val;
    + n6 s1 b) a4 Y  s5 A7 K3 _- H) V! I

  26. 4 }  g3 _7 A4 D5 a1 F+ l. T
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;& _6 N3 N! k4 V* \, j
  28. }2 V) N5 C2 |9 U2 ^/ t  {

  29. 9 t& M$ i; d! j. S7 L/ G
  30. void Car()) \  l: E+ }  k4 M
  31. {& Y+ S4 S3 Q. c+ s
  32.   while (1) {# G) b% R/ v7 T( e% T
  33.     CNY70();# a& P- ?; x, J

  34. 4 C) @4 H, d* _/ q& K; @% z" `
  35.     int error = ((int)values);
    ; D5 i$ }  X9 B
  36.     interror += error;
    3 p$ a2 c, E5 S+ q1 q
  37.     int lasterror = error - olderror;. k& y: H: b, B! ~
  38.     olderror = error;1 c3 i: I; u, d1 m$ l
  39.     int power = error / 5 + interror / 10000 + lasterror;. V3 v% |3 P: ^5 E% ~+ P4 o

  40. / |6 J& i, j( R+ B- z7 }- y. ?
  41.     if (power > MotoSpeed)
    ( m& {2 a' R( ]3 I! Y0 s
  42.       power = MotoSpeed;
    $ P, I# y6 f+ N9 G
  43.     if (power < -MotoSpeed)
    5 `5 A. H3 D" y, X. b* X1 d5 N
  44.       power = -MotoSpeed;2 @: f0 X- f9 l% B
  45. ( n5 c0 g4 v# M- b  ^9 Z
  46.     if (power > 0)2 K7 Y- _) Q: S! N8 }8 F
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。! H% e. o$ w; ^" s
  48.     else2 O! }8 X' }9 \* y/ W& ~) M9 b( Z
  49.       Speed(MotoSpeed + power, MotoSpeed);% f! y) R8 t! M
  50.   }
    & [" H! O) u7 o7 w
  51. }
複製代碼

9 S8 q( N5 W- r
$ R, z- `& v+ g5 u6 G
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-12-4 12:01 , Processed in 0.039600 second(s), 17 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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