查看: 23330|回覆: 3

PID 循跡自走車範例

 薦 [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>3 J8 a  G5 N5 l% E7 w, o" D
  2. #include <Wire.h>4 T0 s7 M$ k4 E# l
  3. #include <Servo.h>
    , Q# i. m% v$ E8 p
  4. # ~1 _" T/ R" S
  5. #include "MePort.h"
    1 t9 C; }6 M- K
  6. #include "MeUltrasonic.h"
    & m; M: u7 L) i7 f& p% u0 t. ?
  7. #include "MeDCMotor.h"
    ' A+ d8 e+ `1 z, c% O

  8. 5 Q: N7 e8 `) ]8 s" N
  9. //double Input, Output ;5 b3 @/ s( `8 J- R/ h5 j
  10. float MaxSpeed = 255;5 D* ?2 A. h/ d) x3 ?4 i
  11. float MaxPower = 180;
    4 p5 w! a7 T7 v: s  M7 y
  12. float MinPower = 120;& y4 P( f7 i1 Z0 D
  13. float Error,ErrorAcc,ErrorDec;) b- S4 E% m* ^1 z2 g& M

  14. ' R- ^2 h% f: b& P7 A
  15. float Kp=0.14;
    0 ?, `2 i9 N* r- C
  16. float Kd=0.00020;//23;
    ! p( ^4 s* S/ r1 i
  17. float Ki=0.000201;, U2 p3 D# x; H4 h

  18. ( \, g- p7 }# I. U
  19. float nPower;2 W: K0 ^. Z: c7 @
  20. MePort lightsensor_6(6);# ?2 [2 T4 C# J* P* a3 a: J
  21. MePort lightsensor_8(8);
    0 _+ n0 {" n  T) a% M
  22. MeDCMotor motor_9(9);
    3 e7 D" C- x& t+ V. N: l+ V5 B- r) H
  23. MeDCMotor motor_10(10);6 P5 V/ }7 m2 E3 m' w/ h
  24. unsigned long previousMillis = 0;. E' z) B0 \; s$ _3 l
  25. const long interval = 1;
    " m" S/ U# K8 O1 B1 x; c  T
  26. + D8 t) r9 u7 O3 R
  27. void setup(){
    , `$ ^. p: g, b+ S
  28.     lightsensor_6.dWrite1(1);! e4 v. n& }5 E3 g: M
  29.     nPower = 160;5 n: k/ t- V5 i  }; c
  30.     Error=0;$ q; E% ^8 e, u) _1 R, ~: [! r
  31.     ErrorAcc=0;
    / [4 I4 K7 f- P" l* x1 `
  32. }3 |4 M* T$ b% A, ^

  33. / e& }) v  B& t" v3 r
  34. void loop(){" L9 A  E8 a( l# }" \. o% x+ I
  35.   unsigned long StartTime = millis();
    ' g$ p2 s2 T5 t+ y2 ]
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    ) Z& W) z0 }% s# Y9 J  J
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    5 Z5 T" D1 Y: v" \
  38.     ErrorAcc +=  nError*Kd ;9 j4 ]- n8 }0 l
  39.     ErrorDec -=  nError*Ki ;
    1 D, i' m8 Z- C# U" v
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    9 z+ h* ^7 e1 R; E5 f0 S( J
  41.     if(nError < 80 && nError > -80){
    ; F$ z5 C2 ?9 K2 d" W5 T
  42.       if(nPower < MaxPower){# M& I3 ^& Y' Q$ O
  43.         nPower += 3;! ]0 B% P9 M% q: C2 s3 J; ?) T
  44.       }
    ' ]# ?3 t) s6 F9 n4 g* s" ^
  45.     } else{
    6 e9 Q( F8 z4 |+ T# j
  46.       if(nPower > MinPower){
    * {6 h9 s0 {8 w' R' ?
  47.         nPower -= 2;
    5 U# Z5 ?* ]* `5 s+ E) [
  48.       }
    * s( j# q0 A, c- M4 O* Y( P3 O
  49.     } 7 k. }! d& ]1 _% o+ I
  50.     MotoL(nPower-Error);0 Q$ ^" Q+ x6 Q6 p# e' U
  51.     MotoR(nPower+Error);    4 V* x6 V! ]1 o+ G5 g$ ?
  52.   }else{
    / H+ U! @$ \9 F4 t0 N
  53.     motor_9.run(0);
    ' Z2 d# o7 f2 j& ]2 F# r* {
  54.     motor_10.run(0);, _1 A# Y; [/ u' J: ^0 h- q  j
  55.   }) E& F6 c3 y' d$ b/ h6 w2 q
  56.   do{}while(millis() - StartTime < interval);- P4 l9 S; t1 G. ^  B
  57. }" D2 Q5 f3 P9 d+ k% U
  58. 8 P, D7 [/ v* b& _) {/ ~
  59. void MotoL(int Power){; ]$ ]7 d: c/ W2 I2 o
  60.   if (Power > MaxSpeed){; y) G/ p3 n, ]$ O) L
  61.      Power = MaxSpeed;
    $ ?( c/ a5 W4 p: s
  62.   }
    . f. F( d' x# ?/ }! _3 a4 L
  63.   if (Power < -MaxSpeed){: C- }4 B5 f! \/ ?. l/ E
  64.      Power = -MaxSpeed;
    4 _1 O% l7 j6 @$ H7 O
  65.   } " I: L7 D8 R' e" k
  66.   motor_9.run(Power);6 i5 k% I# y; a' X7 K1 Y
  67. }  * A* d, W& }- G. O/ Z; |; R
  68.   ]1 L3 ~7 Y8 ^. x/ A
  69. void MotoR(int Power){
    ) y! w8 G* q: I! A8 ~
  70.   if (Power > MaxSpeed){# W  r0 D! R8 s8 M$ n
  71.      Power = MaxSpeed;
    " ^$ m* B' a3 r# J0 O/ X2 A( e* `
  72.   }
    # h6 e. i; J# Y0 i6 {
  73.   if (Power < -MaxSpeed){
    3 U* l0 |# x) y5 T7 X
  74.      Power = -MaxSpeed;
    , X: |+ @7 F! ?
  75.   }
    0 l! a9 }& O3 n* N
  76.   motor_10.run(Power);' t& y. U7 Z9 r) {* t8 z' ^+ q: Z
  77. }  
複製代碼
$ P: M, c1 g7 I, X: `
( ~) r1 ?, p$ H& j: q  c
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯
& b" k7 T' ~- d) O8 g0 d' S) h; X8 I; b5 o4 [
您好,不知是否能向您請教。7 X8 l: H, P0 Y" x/ w1 M8 N. X
目前和宋修賢老師在處理Ardui Car% ^7 y" P+ U2 \0 `- C9 B7 |: @4 t0 f
雖然已使用較繁雜的方式處理了跑出黑線外的狀況  Y: M9 Y+ ?7 J. P# L
3 a+ i- V; ~* s- I" w$ `) \
但基於想追求更精簡的程式所以還是想請問一下
2 Z5 t& N( e7 s, ~  E/ v. y8 J3 \就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝
5 w2 t4 }- Q4 U& w% i5 a7 {不知道您是否願意教我可以如何處理
& V! {( I. @5 c) v! `8 b+ _# m9 @& ^( s0 L3 V" m: p, o
- `+ b0 Q) P: z7 W( W
以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;. n5 n$ w5 e: a( U" s
  2. double CNY70Val = 1000;! p! r# r8 S$ y- C$ W. f
  3. int interror = 0;* {; h$ p8 n3 ~. n1 b) O
  4. int olderror = 0;
    $ m! s; m# G5 O$ T9 B/ j
  5. double values;
    ; x( ^( i2 C& b: _" m

  6. 0 Q( m, g& P+ S- R7 t

  7. # m, @, e" T* g3 A. a
  8. void CNY70()6 J' r( ]0 |% U$ [
  9. {
    % h5 P  v4 N7 o: {; Y
  10.   valuesRR = analogRead(RR): m) i5 q3 Y8 R
  11.   valuesMR = analogRead(MR);( D: [  n4 e6 P, n! s/ R! r0 X
  12.   valuesMM = analogRead(MM);4 x7 }+ J* U5 }- h
  13.   valuesML = analogRead(ML);" |) t9 \6 w1 f. r; J
  14.   valuesLL = analogRead(LL);2 u- l; Y7 q: q. b8 A8 ], [

  15. 0 @( x( Z) l* l5 W4 _1 F) q
  16.   if (valuesRR > CNY70Val)3 Z2 e2 t4 i6 H; A" ~3 k  V
  17.     valuesRR = CNY70Val;, q8 j# P; y. n
  18.   if (valuesMR > CNY70Val)
    4 {4 U/ I' N$ K1 y; {: ?0 W
  19.     valuesMR = CNY70Val;
    0 d/ e9 N7 T! n2 k4 ~. v0 J) D$ H& n
  20.   if (valuesMM > CNY70Val)* d( J8 m. w( s7 A4 F6 b* x; U
  21.     valuesMM = CNY70Val;: K5 z$ ~+ U6 h- _: g$ h1 C8 v; R
  22.   if (valuesML > CNY70Val)9 u  V! R, t6 V, o! L: O3 j2 d' _
  23.     valuesML = CNY70Val;- v. t" c# K0 v$ D
  24.   if (valuesLL > CNY70Val)4 F- s5 t+ u4 ~2 c' {$ O
  25.     valuesLL = CNY70Val;! g; V9 d) F0 B5 S( Y  Y3 p

  26. 1 b) W5 ^: Z" n9 `8 o! P$ I
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;+ K8 Y" [1 r+ T1 a
  28. }
    * B3 U9 t9 }# w! N
  29. + r* i. U) U6 l: r' e8 o
  30. void Car()
    ! \9 H2 }7 ?2 d+ l/ I4 j$ I. G' t
  31. {
    3 P3 _8 {4 k- q! U+ Y. H7 k# z
  32.   while (1) {: M/ e# b. X( a/ l4 q) W8 {) ~
  33.     CNY70();# ?# c& P1 c3 f) M/ a
  34. ( b5 R% n# @0 v. q
  35.     int error = ((int)values);+ v$ p9 r; b" X9 x9 L8 x0 Z* l# |+ \
  36.     interror += error;& P' _4 Q# _) N- I
  37.     int lasterror = error - olderror;
    0 ^1 G; q6 T& ^% B5 R* v
  38.     olderror = error;
    : a; L4 ^. {5 Y8 Z$ |2 |+ P
  39.     int power = error / 5 + interror / 10000 + lasterror;4 P8 M7 i: h  L- g$ g- Y

  40. 7 H  P2 P, T6 L& l9 n
  41.     if (power > MotoSpeed); ?! b6 n0 ^4 E2 {1 Q2 m3 \
  42.       power = MotoSpeed;
    ; ^& G, b7 f- \$ s; l' U
  43.     if (power < -MotoSpeed)
    ; l# Y* N5 K7 @: r8 N# q
  44.       power = -MotoSpeed;! U& j5 r* p0 I- T3 O) l

  45. + j5 M+ l; G% k8 A/ |
  46.     if (power > 0)
    . k9 N  S2 \  q2 o
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。
    ! I, o( N4 h# p( @5 I& M& o' ~
  48.     else9 g# c: n. f9 `9 E% d0 G" x
  49.       Speed(MotoSpeed + power, MotoSpeed);
    . y, O, N* c% L1 R  K2 J5 _4 q# V
  50.   }
    ( ~5 p! z# r7 p( O, P) w0 L3 R
  51. }
複製代碼

) Z8 c# `1 r% N7 @  F* Q7 _# l. Y. ]; f
您需要登入後纔可以回帖 登入 | 立即註冊

本版積分規則

Powered by Discuz! X5.0 © 2001-2026 Discuz! Team.

在本版發帖返回頂部