圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21690|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>6 c8 d- J4 ]+ `+ e8 T( H2 q
  2. #include <Wire.h>
    5 I; ^0 r' r, a2 a& `( c" p
  3. #include <Servo.h>, g' J  A: J+ c* T) }' t' ^
  4. 5 _  b* d* r& e& K
  5. #include "MePort.h"
    . V# m8 F) r) K# z6 j/ n
  6. #include "MeUltrasonic.h"
    1 O% h. h' P9 y
  7. #include "MeDCMotor.h"
    : }, G' r, M1 _+ \* p9 x& [, E
  8. ; N; E5 }9 ?% l0 P5 I
  9. //double Input, Output ;
    + n$ g1 S: y. Q$ {% n$ L
  10. float MaxSpeed = 255;
    , p) W! c$ z+ v; I/ j  q
  11. float MaxPower = 180;* r( r& D9 b8 Y; a+ g
  12. float MinPower = 120;8 b- g( {* R$ Z' B7 K/ x7 q
  13. float Error,ErrorAcc,ErrorDec;
    ' p! P% y( K7 c' U

  14. . l, d7 g, [& w8 s2 f. w( m8 v
  15. float Kp=0.14;
    3 ]1 F: c7 H) D6 I( E
  16. float Kd=0.00020;//23;
    / ^$ [- F% ^+ y' ?: R
  17. float Ki=0.000201;
    7 q5 Z* z; }& S8 h7 y+ S
  18. / E; ~& D8 c4 @! A" {; z& C$ i
  19. float nPower;( }8 ]/ ~9 E* _# E" J
  20. MePort lightsensor_6(6);
    + E% b' ^8 ?- a* i+ j. U  N
  21. MePort lightsensor_8(8);. n6 k% u8 {4 u, X$ e3 @
  22. MeDCMotor motor_9(9);
    $ ?$ S) V8 p4 K/ `% w
  23. MeDCMotor motor_10(10);
    0 `& v2 C4 J' o! G/ L
  24. unsigned long previousMillis = 0;
    * }. a1 O6 s9 ~2 \
  25. const long interval = 1;; I( |+ b9 z; p. v" m

  26. - v, X6 y; x7 F% \9 R
  27. void setup(){
    1 N! C3 \  ], T$ F# S% t7 `
  28.     lightsensor_6.dWrite1(1);
    * K0 W1 c/ f, T$ i: Y0 l
  29.     nPower = 160;
    , w1 o$ z1 Z5 }2 Z
  30.     Error=0;
    1 j% y+ Z% e% K6 H8 x" k+ ^
  31.     ErrorAcc=0;, C/ q+ r. f1 h4 ^
  32. }
    ( q" F- \! I- {: P! K

  33. , ^- g5 I! N! n! H
  34. void loop(){: j. T6 G: N8 z- y/ o/ v- \7 P' s
  35.   unsigned long StartTime = millis();/ T# s' K) p8 ?$ \2 E
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){, h5 J2 W5 N! {, ?0 P
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();( x  P8 B/ r4 w; q' L
  38.     ErrorAcc +=  nError*Kd ;
    . \5 P6 g* a% `8 j: n
  39.     ErrorDec -=  nError*Ki ;
    + X; y: C7 x3 R1 \0 {4 l4 s9 \
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);: Q  k9 [% M8 K2 h, |( ]
  41.     if(nError < 80 && nError > -80){3 X0 x8 Q. r/ Z/ U( |7 S
  42.       if(nPower < MaxPower){
    + q; }2 p9 K5 }  J  |' A
  43.         nPower += 3;
    $ @, p9 M0 I  x) _0 y3 J3 Y
  44.       }( w: X/ f: V* c/ K2 u+ L
  45.     } else{" l/ f3 b9 g; G: @: V
  46.       if(nPower > MinPower){
    : b  |5 G. q- p, g
  47.         nPower -= 2;- A( N- K: k, t) |1 g! Q, a( V8 t$ M$ |. d
  48.       }& t# r* I- u$ @
  49.     }
    8 O& ]4 U; z% f, _# W; i6 |
  50.     MotoL(nPower-Error);
    / d2 p6 t+ o4 L' e  H/ j8 e
  51.     MotoR(nPower+Error);   
    $ {$ p" p: i% A% |
  52.   }else{
    6 ]* \. ?. u& t( [7 e, G
  53.     motor_9.run(0);
    1 M& |! T* Z5 q# y" p
  54.     motor_10.run(0);9 H" S. i5 k; e7 Q4 z
  55.   }6 K# ]# W5 K: o% Q. J' s
  56.   do{}while(millis() - StartTime < interval);
    ( P) A6 T: K. [4 P
  57. }! ^. Q1 ~9 \% P2 o! I1 I5 p% \

  58. 6 M+ J9 Z! J. U7 S; c
  59. void MotoL(int Power){) \: h1 Q5 [: \" L6 N, G
  60.   if (Power > MaxSpeed){1 B; |7 D8 T' a  `
  61.      Power = MaxSpeed;2 I. _1 z- ^" E5 m" a+ k8 Z1 b
  62.   }
    9 s, _. M$ K2 s  w3 x4 T
  63.   if (Power < -MaxSpeed){5 `* a: \) s0 |6 D3 b* }/ D+ n* [3 @
  64.      Power = -MaxSpeed;+ h; r5 [/ n2 v) L0 Y0 G
  65.   } & a- a: x9 k6 r% @" {# U6 ]5 s
  66.   motor_9.run(Power);5 V% L# |. ?. K) H
  67. }  & H1 p7 C, w. J, ^: ~) u
  68. ! y3 B6 B# c, I
  69. void MotoR(int Power){" \, T5 [# W' }) q' d4 M
  70.   if (Power > MaxSpeed){
    5 F5 t# z0 [- P) ]
  71.      Power = MaxSpeed;
    ( `- ?( {5 I: N; H1 w
  72.   } 5 w0 k% M3 z1 r8 H' S' ~" J/ H6 H
  73.   if (Power < -MaxSpeed){: z- G7 d$ I& Y8 m/ R1 I
  74.      Power = -MaxSpeed;) |% L2 v1 Q5 s- S; J
  75.   }
    / X! l8 ^6 s4 J! ~# H
  76.   motor_10.run(Power);
    ) ?0 K3 O2 l. K; X8 j8 ?4 V& R
  77. }  
複製代碼
( }: ]! N4 X0 j4 V7 ~/ B1 v1 m

0 ?$ G4 T# ?/ t
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯 8 q$ r0 n9 l; \
# `' p6 `: ]. q) f% \
您好,不知是否能向您請教。1 F* K" a8 K4 k! s. E1 y& b. s$ \
目前和宋修賢老師在處理Ardui Car
2 {8 p! K" M* i* h9 S雖然已使用較繁雜的方式處理了跑出黑線外的狀況& |" i9 Z6 k# |
: f5 R8 D  d0 n: C$ {! A6 `
但基於想追求更精簡的程式所以還是想請問一下
% d! y2 ]% I3 E; N$ B就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝
( d) k) V! ]6 l' V0 _$ M不知道您是否願意教我可以如何處理8 b7 [& `  T/ M8 k

, @  Z) u5 k2 a( v% ^1 g
# y5 l7 w( `% B! p, U1 {以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;' o; D9 l7 _) S4 P
  2. double CNY70Val = 1000;8 U: E. P, I0 u
  3. int interror = 0;
    1 q4 s+ C4 R8 h/ ]4 ?
  4. int olderror = 0;
    ; M8 e; o+ \: ~/ w
  5. double values;
    6 X4 [& m1 e# ~6 m4 ?3 a

  6. & U1 x4 b; @  f4 }) [% U# J" L
  7. 2 J5 X) |7 S- j* ?! w
  8. void CNY70()% U' E* `6 ~9 H( u
  9. {
    : o& J2 R% P3 X; C
  10.   valuesRR = analogRead(RR)' G; ~6 H8 i) `- N
  11.   valuesMR = analogRead(MR);9 Y; P: e6 v1 P
  12.   valuesMM = analogRead(MM);0 \& X9 O  L+ i
  13.   valuesML = analogRead(ML);
    . d0 ~5 k1 s! h% J- e) s* m
  14.   valuesLL = analogRead(LL);( F) Z3 Z  N  E' b# s) T) h# a
  15. 0 J- Q# |  N! ^/ U) X2 g
  16.   if (valuesRR > CNY70Val)
    1 o) W3 ?1 P" `1 P
  17.     valuesRR = CNY70Val;6 K4 o7 ~/ E! w, i' y
  18.   if (valuesMR > CNY70Val)
    6 \4 P* x" N; Z0 f
  19.     valuesMR = CNY70Val;! s; f% Z' J0 N% k) m! M) a; n  p
  20.   if (valuesMM > CNY70Val)
    $ X6 {1 n1 e# h' O0 p- D+ j$ g
  21.     valuesMM = CNY70Val;
    1 G2 V: m7 s* k, Y- u# z6 H* I
  22.   if (valuesML > CNY70Val)
    ' E  P, u3 N5 E* u# |% b# D
  23.     valuesML = CNY70Val;. y: E) f! W5 W
  24.   if (valuesLL > CNY70Val)
    ; T  |. Z7 C! q# b
  25.     valuesLL = CNY70Val;
    7 O: E5 h  {/ @: l+ B

  26. ' K7 j4 @5 M: ^) @+ b
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    5 K2 B5 y1 @! @6 w- ~9 b
  28. }, T! F# B( @* C; @. }

  29. $ l. t4 }; D; j4 H+ Y0 m
  30. void Car()
    2 A, J0 I& L, k* _1 `. X: n
  31. {
    * }# l  ~1 ^" X0 G. U1 W
  32.   while (1) {, w- ~6 L/ g7 \- v' X9 F
  33.     CNY70();9 m$ a7 ~6 Y/ T; Q: _
  34. 5 f2 `: g6 `2 ?9 h. `; O4 A2 L" w+ \/ X
  35.     int error = ((int)values);4 _* ]  Y, D! y
  36.     interror += error;
    . W# F$ p, v3 a5 ?& E) v
  37.     int lasterror = error - olderror;: n6 @, z9 f: Z) G; F4 p
  38.     olderror = error;4 ^0 r' K' p; [, Y. j  L+ Q
  39.     int power = error / 5 + interror / 10000 + lasterror;% v- x7 i/ \- c: i
  40. ; u; z* _: e) n* ?8 M
  41.     if (power > MotoSpeed)3 R5 |8 N& e: d% T. Q
  42.       power = MotoSpeed;
    / K1 g4 E2 R* _  T
  43.     if (power < -MotoSpeed)
    : C" w6 n, y) {! a3 s4 E2 F
  44.       power = -MotoSpeed;
    1 A# f: y7 a7 @6 d7 w
  45. & l6 W- g. I" n4 v& Q
  46.     if (power > 0)
    ( e/ q$ F: c2 n
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。4 z1 C1 T3 b7 O7 S$ l
  48.     else  ?& O2 D0 s6 a1 s$ K
  49.       Speed(MotoSpeed + power, MotoSpeed);8 a0 Z, Y* F* ]; t4 U
  50.   }5 J: J, T( X) n% Q  l6 o
  51. }
複製代碼
; W0 \: ?7 W, \. ~1 N( p- ]

; A9 K  D4 C9 u  e/ I: I" Y
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-11-22 03:54 , Processed in 0.025157 second(s), 17 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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