設為首頁收藏本站

圓創力科技.MakeBlock TW

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

掃一掃,訪問微社區

查看: 8701|回復: 2

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>
    % o2 Y4 s  w( P7 B" H5 y( l
  2. #include <Wire.h>
    " V9 F$ R5 T) l4 R" g" l0 {
  3. #include <Servo.h>
    , D" h& I+ y2 c

  4. 8 O$ \, a) L, d2 j1 X: A7 b$ f' Z& y
  5. #include "MePort.h"
    # j* r! J& F2 v5 v& n& U
  6. #include "MeUltrasonic.h"
    5 T8 J3 a: M, q" ~" c0 q
  7. #include "MeDCMotor.h"
    5 @8 M+ b% z  d% C4 L3 |- [

  8.   ]" o  Q* Y3 \1 n! l% h
  9. //double Input, Output ;
    ' i% ?# m5 M# O) n/ h& ]& K. L% h
  10. float MaxSpeed = 255;6 @5 c. q' T/ E2 H5 l
  11. float MaxPower = 180;: O6 U% b6 \; x2 q( {
  12. float MinPower = 120;
    ; T$ Q9 {4 a' s! X
  13. float Error,ErrorAcc,ErrorDec;& m. ~- h: \$ q/ s" U
  14. 7 M0 q7 i9 ~$ a! H
  15. float Kp=0.14;: y+ Q! }% `0 f2 t4 E3 p& f' v
  16. float Kd=0.00020;//23;. m1 B6 F6 J8 _: ~/ B2 Z* j2 T
  17. float Ki=0.000201;
    $ ^7 I* v# z# a& {; ]( w. `
  18. 7 C1 p  C* f4 B5 q  {
  19. float nPower;' c/ @$ r, Q$ m- ]2 I# {
  20. MePort lightsensor_6(6);
    " n1 P: Z9 |( w' d$ G
  21. MePort lightsensor_8(8);! q4 ]  ^  x% `* e8 I
  22. MeDCMotor motor_9(9);0 J8 `5 F# b/ y! W
  23. MeDCMotor motor_10(10);/ m, n- }% \$ o
  24. unsigned long previousMillis = 0;8 J" E& F* y7 a& n2 z5 v4 w& Y
  25. const long interval = 1;: I) ~' z0 E2 L5 k" a( ^

  26. ' o& O  ?3 ~+ n* O, k3 l/ g" F
  27. void setup(){
    $ E7 J. {' U! H: W( _  @3 s8 b
  28.     lightsensor_6.dWrite1(1);! _5 H' }9 F! e4 f8 u/ i. j( e% \( u9 q
  29.     nPower = 160;
    % f0 C( ^' A5 n& T
  30.     Error=0;
    1 c  g# A! a; a1 A8 B1 z8 W( h
  31.     ErrorAcc=0;" G7 S" O0 j* Z) L7 g: h$ Z
  32. }4 n! o& f( m) m" \
  33. ; |* W- L4 d; Y) X( F0 h
  34. void loop(){0 m8 n, V0 h$ c, x6 J$ t3 K
  35.   unsigned long StartTime = millis();8 T5 v7 a. T) p) b
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){$ t- @& w2 H2 d* U; r
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();: ~( k' w6 \8 Z' ^6 P3 R  _
  38.     ErrorAcc +=  nError*Kd ;5 c" B! K% w9 v$ v9 l. T; x
  39.     ErrorDec -=  nError*Ki ;( }9 L  U# f/ d8 a
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);  ]+ ]- O4 A# Z. w4 q! K
  41.     if(nError < 80 && nError > -80){
    3 `9 M$ `* d4 m! A* b" n6 j: R) s
  42.       if(nPower < MaxPower){) E+ i2 u% |. e) Q$ R4 _
  43.         nPower += 3;
    ) K. w& V3 v( I. c( f" M
  44.       }
    & ~4 S; r: K4 |& ~$ H! M
  45.     } else{6 M# U/ O2 Y( N; n% o
  46.       if(nPower > MinPower){
    5 b# \) ]; a' b6 p
  47.         nPower -= 2;
    : s4 R: U9 w1 @" x
  48.       }
    6 P/ z( L4 ?" F& X& t
  49.     } $ k3 q1 r' q$ l' t, O" E6 i) r
  50.     MotoL(nPower-Error);" V" ~/ N: z: I1 z7 M
  51.     MotoR(nPower+Error);    ; I% F$ V5 o+ q# P8 b
  52.   }else{
    ' }3 U7 c. z2 x, J! {4 j
  53.     motor_9.run(0);8 `1 I3 U' I6 c- ]$ i
  54.     motor_10.run(0);
      i; R$ [5 w0 l; F
  55.   }
    4 G/ ?5 R/ [: j
  56.   do{}while(millis() - StartTime < interval);. G2 V: X* c! F3 [* h5 n: s
  57. }: c; A. u2 j9 T, T) }/ J

  58. ( B0 j) L- S! n  L2 {7 v& U
  59. void MotoL(int Power){% U9 z, W) ?. a& I1 S+ {; F; r$ C8 E+ Z
  60.   if (Power > MaxSpeed){& x4 ?* e8 d5 h7 u$ V
  61.      Power = MaxSpeed;
    8 Z$ `* V; M/ O* E8 i1 d; ]9 Y
  62.   }
    9 T/ E! T# `  c- m& @  L
  63.   if (Power < -MaxSpeed){
    3 ?& J* B0 |9 ?: G. l
  64.      Power = -MaxSpeed;
    2 }3 y, q, k8 ]5 G. Q1 o0 q9 R
  65.   } : I7 [1 P7 j1 N$ Q
  66.   motor_9.run(Power);
    : n% j4 K8 x( ^( _
  67. }  $ c: H" Y4 D  e. W& H+ }( A8 N
  68. & x1 ^* ^  M  `, A* \1 {: P+ R; x
  69. void MotoR(int Power){
    ; [8 F4 {$ s/ i3 i; s
  70.   if (Power > MaxSpeed){
    ! {/ ~0 p6 s1 c( [
  71.      Power = MaxSpeed;
    ( ]% \( j1 S* c% y3 }
  72.   } " b' |/ d! S  S; J4 I: ]
  73.   if (Power < -MaxSpeed){
    5 G" }6 _! x3 M. {4 i) r( V
  74.      Power = -MaxSpeed;
    7 j* R( A0 N- W3 i! ~/ q* X. A
  75.   } * S3 m6 B+ U# s9 j1 K" j$ K
  76.   motor_10.run(Power);
    * }$ ~; q  |4 O6 a$ u
  77. }  
複製代碼
* z: h# c% X6 H7 B7 M& ]( g+ d( s

) r; k, \8 W2 ?5 d5 Q
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯 # O  `# k; C, H( q9 O2 e: ~& k

, J( i% ?' A: W+ P# U8 j! r您好,不知是否能向您請教。
; Z# ?; J9 K2 j) \目前和宋修賢老師在處理Ardui Car# O9 {* \/ \4 l: u  l
雖然已使用較繁雜的方式處理了跑出黑線外的狀況# E/ P. i# U; X: C% B5 ~
! t! B, M+ l- h- j) v- q  Q; |' F) f
但基於想追求更精簡的程式所以還是想請問一下1 B% Y, x; w8 H# A! _
就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝. C4 {8 X: o, p
不知道您是否願意教我可以如何處理
% ]# o3 o" D$ B' R7 `: S% q9 d1 j9 t+ U; y5 C) [
1 y6 Y. v" R+ F. o- N/ z/ h
以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;$ u3 J& ^  S( M4 O
  2. double CNY70Val = 1000;
      H; t5 b# B; N3 ~; q3 u$ \% i& `
  3. int interror = 0;
    ! b- N& ~; y" @: Q7 ~" L
  4. int olderror = 0;2 o, r3 [' X/ z& |, P; \1 d
  5. double values;
    2 i. c# d0 `* B

  6. . h9 |6 T4 G: N+ r0 f. j

  7. - Q0 J. g" S9 W* U6 r
  8. void CNY70()2 ~7 o& f$ T9 B* ?; ~8 q9 W
  9. {) Q; F' o6 j( B: C
  10.   valuesRR = analogRead(RR)5 k6 ], K' G9 U: ]" f# R
  11.   valuesMR = analogRead(MR);; S1 z' L- p. F1 C
  12.   valuesMM = analogRead(MM);2 e( g$ m; t5 ?/ O! t" O, @3 u
  13.   valuesML = analogRead(ML);2 g* Q6 L3 y6 F& i+ @2 \
  14.   valuesLL = analogRead(LL);2 W& j1 w! `4 i& Q+ a

  15. & Y* E; y4 _) x! S4 T
  16.   if (valuesRR > CNY70Val)
    1 r# P6 b" A# H0 F; i6 D2 Q" m
  17.     valuesRR = CNY70Val;
    - q9 E1 F; F. s" @, d8 P1 U  q& K
  18.   if (valuesMR > CNY70Val)  Q1 |7 C2 e  Y" Y/ d
  19.     valuesMR = CNY70Val;
    + [7 J, H5 {# B- e$ R
  20.   if (valuesMM > CNY70Val)+ r& {* i& {- ]3 P7 H( m
  21.     valuesMM = CNY70Val;) p9 r9 F4 N% X5 D5 P& S3 E0 j; `9 i
  22.   if (valuesML > CNY70Val)
    6 c. C) j# A/ t% o
  23.     valuesML = CNY70Val;/ H: ]1 v- i4 \/ K
  24.   if (valuesLL > CNY70Val)
    : j0 a& X7 y% i4 _8 c% J: H
  25.     valuesLL = CNY70Val;  P9 x7 g7 i! Q

  26. % t; Y. V4 j7 S3 U2 d: A7 I0 I
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;! D3 i) `+ f( q0 C0 l
  28. }; M/ L9 G; [/ B2 O

  29. 1 G' }$ p/ E1 X  j: ^
  30. void Car()
    3 O& N) k' h: C/ ]5 B
  31. {
    $ W. D$ K0 B- ]; e+ D9 |- @) E$ Z
  32.   while (1) {/ P5 l) @4 y) w( F$ t( P
  33.     CNY70();* @; j# o/ L0 `* W' G
  34. ' o2 K: d' p2 \# _. P+ I. ]
  35.     int error = ((int)values);
    " ^7 m+ N0 E% K' S: M4 I% e5 _* F
  36.     interror += error;& @" |! Y2 |# c4 C8 ]
  37.     int lasterror = error - olderror;9 ~1 f/ D$ o; r! U/ N! u. |6 E8 l
  38.     olderror = error;/ L" q9 m' v: V; C
  39.     int power = error / 5 + interror / 10000 + lasterror;: `1 O' o( b- |# p  v$ K! [

  40. % v) o1 g( y* ]' M9 s5 L, y! q
  41.     if (power > MotoSpeed)
    " r; B5 F! R5 c
  42.       power = MotoSpeed;1 {: Z6 X" ]7 E/ S) I
  43.     if (power < -MotoSpeed)1 R; O7 _, j0 N0 `
  44.       power = -MotoSpeed;
    7 t# i/ _( J" |! ~

  45. ' f) }# F  O' T; ]5 l; W- i! @
  46.     if (power > 0)
    2 k" ~# J! F! R, o" T5 L
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。& k, R, Y  K3 [& n, |( l
  48.     else& }. v; v! ]+ a  A/ C. q" D
  49.       Speed(MotoSpeed + power, MotoSpeed);/ G0 k/ k# j+ i+ l6 Y  u$ c
  50.   }0 b, b# q6 \- w+ q# h
  51. }
複製代碼
0 y2 x) m8 P9 b
- ?4 b  N0 w3 L$ n$ L* j' A9 `& E
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2019-6-26 20:37 , Processed in 0.041158 second(s), 17 queries .

Powered by Discuz! X3.4

© 2001-2017 Comsenz Inc.

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