設為首頁收藏本站

圓創力科技.MakeBlock TW

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

掃一掃,訪問微社區

查看: 9243|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>; a' F0 [) [; N) }5 d
  2. #include <Wire.h>
    ) @8 @- m# {" S  ^5 V
  3. #include <Servo.h>/ @* o; f2 `  K: h

  4. 5 L) a" ~9 l* X: P: q$ E
  5. #include "MePort.h"
    % T5 G7 F, H8 `/ H% e# K9 w  o
  6. #include "MeUltrasonic.h"
    7 ^% R. y5 l3 C5 b" R  u
  7. #include "MeDCMotor.h"
    5 F  n# Y! L! e. t5 p
  8. 9 f# |5 ?% G4 w9 Z: _
  9. //double Input, Output ;+ N3 Z* B$ V* R8 T5 a( m
  10. float MaxSpeed = 255;& h1 B3 z7 o% v
  11. float MaxPower = 180;$ V& C4 @! t( [- n5 Z# h
  12. float MinPower = 120;8 E4 E# {3 u8 a: e
  13. float Error,ErrorAcc,ErrorDec;
    - }; i, O8 U5 D6 B& r6 W
  14. - L  d. K6 M! x
  15. float Kp=0.14;7 @6 c3 ^2 n  q! H* y. }5 s* D
  16. float Kd=0.00020;//23;% |3 \8 z9 \1 |( k% o" _
  17. float Ki=0.000201;) {3 P# W! c! M2 E" s* n3 Z- i  P2 x" k
  18. 5 d; o* s7 ^7 I/ m7 N- w& \
  19. float nPower;1 s+ X3 S, ~! p) X; U0 \  I/ u
  20. MePort lightsensor_6(6);) h% E9 `; Z+ g7 G
  21. MePort lightsensor_8(8);: ~) M- h* s% ^
  22. MeDCMotor motor_9(9);
    2 C7 m3 k+ x" E3 Q1 N: S1 U. G; a. H
  23. MeDCMotor motor_10(10);: P4 J4 c$ s" m" }. S
  24. unsigned long previousMillis = 0;
    " X; t$ Y5 N7 p8 f
  25. const long interval = 1;& k4 ~4 d6 Y5 A, ]: Z& ]& E+ v

  26. 1 h$ u3 G  D# w0 m
  27. void setup(){' {: @0 G; p4 P1 K+ O0 J
  28.     lightsensor_6.dWrite1(1);
    + M1 E' d! S" n" N' E& z( X
  29.     nPower = 160;
    ( J2 Y% ]8 B5 s; W
  30.     Error=0;
    # C! i4 R* S0 \; U: y: X% w
  31.     ErrorAcc=0;: ^, n4 M# t$ T6 h% X
  32. }
    9 n6 @- u3 Q9 W( s, v+ ~4 R  Y5 l6 j8 T1 ]

  33.   P) L* u* ~# z0 j9 ?( U6 D
  34. void loop(){
    4 Z7 w: |! ~) R3 c8 _+ \
  35.   unsigned long StartTime = millis();
    . j! V/ w4 R+ f, |  `" f( Y: c6 v
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){+ s9 Y- O  Y/ J- ]- S  E$ {
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();5 A& V/ D# {- k
  38.     ErrorAcc +=  nError*Kd ;
    4 B2 V6 N- C" K5 T) g
  39.     ErrorDec -=  nError*Ki ;
    / O- s8 `$ k% K& P
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);% M9 [4 S0 Y8 [0 I( H6 l, b: D
  41.     if(nError < 80 && nError > -80){8 s. V% r/ M. J1 A* q7 u3 z
  42.       if(nPower < MaxPower){
    ( I$ R3 a, n7 r8 Q; V  x; {% F
  43.         nPower += 3;2 s. N* ?! H' w* z9 u/ V3 B
  44.       }+ ?9 w: t( u- t1 I) u
  45.     } else{
    . @+ B  t  }4 C; b$ w! Z& s2 j
  46.       if(nPower > MinPower){/ }. m0 w3 ~% U. P
  47.         nPower -= 2;
    - m* n- l- V/ ?" \7 ~8 @% ^0 g% w
  48.       }
    . R6 F; r* ]0 X0 u# K
  49.     } / @, J! S, n0 N% b3 x' e! @& x
  50.     MotoL(nPower-Error);! w: C+ h. i- Q( n8 \
  51.     MotoR(nPower+Error);    + F( T$ {. \! [: c$ Q
  52.   }else{
    3 S& i0 e7 ~0 a8 G& U; q
  53.     motor_9.run(0);, m1 N; ]% L! V$ u9 R9 E/ }. Z
  54.     motor_10.run(0);
    & I. V9 G8 {/ L% x6 j
  55.   }
    " u# j$ P- _6 l1 J9 s' d
  56.   do{}while(millis() - StartTime < interval);
    & j/ W- o$ |8 v6 F
  57. }8 J  R- H2 e0 Q
  58. + ^: n- e% ~& q. h" P9 h
  59. void MotoL(int Power){
    ) ^) N( R# t4 H0 M2 m
  60.   if (Power > MaxSpeed){
    % j2 {; J4 i% [& {
  61.      Power = MaxSpeed;7 u  `1 ~$ K- a- \- T
  62.   } & |/ z1 i. r  h0 }
  63.   if (Power < -MaxSpeed){
    : C, {4 H+ R. s$ p* W- R  k
  64.      Power = -MaxSpeed;  C( g# P; ~% ?# `; i; ~- l+ W8 [
  65.   } ) t0 Z7 y" ?9 k2 e! K
  66.   motor_9.run(Power);
    3 _5 D7 w0 ^3 L! f" A
  67. }  9 q# g, Y% m; B- J$ r& M6 T8 r

  68. + l2 E/ X* ~1 s% g
  69. void MotoR(int Power){
    & _2 i# G) I& y1 _" C
  70.   if (Power > MaxSpeed){
    + F& i: |2 ?3 c1 ?
  71.      Power = MaxSpeed;
    # v# b+ Q1 E! `; Q( m( q
  72.   } 9 y( ^, o9 v3 G% n) V  n7 f
  73.   if (Power < -MaxSpeed){& h! q) b  [5 e  I  k
  74.      Power = -MaxSpeed;( k( r6 M5 q. P# a% h0 x% c* U
  75.   } 0 S* V" L; Y) ]1 c) |. b
  76.   motor_10.run(Power);
    - F- B5 S8 f- h) @
  77. }  
複製代碼

- q. T( L2 V) q2 r
  w: z% O' }1 t, d* y% h, F6 s
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯
; h. h# @) b$ i4 x: Y; a0 E% }, n& P2 \* |! n
您好,不知是否能向您請教。8 Q3 O0 E+ q9 F7 |
目前和宋修賢老師在處理Ardui Car
3 b$ a7 {, U) d8 X雖然已使用較繁雜的方式處理了跑出黑線外的狀況
+ P( o; R% Z) O
* I1 g. k  y: z/ Q7 q但基於想追求更精簡的程式所以還是想請問一下
5 E+ w! O2 f& V5 _2 q: ^就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝* T" R( L. w, [* }# f
不知道您是否願意教我可以如何處理
3 r6 \' a! H- x: g6 T' Y# Y. z2 \. ?7 T& V3 @
9 I- j0 d# n1 X6 k/ q/ a
以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;
    ' J# j7 C& f. Y4 ?( X6 f
  2. double CNY70Val = 1000;
    - ?4 N. G- F& Q1 ?0 ?9 o
  3. int interror = 0;* ~& [, P# ]) ?
  4. int olderror = 0;+ _5 q. W3 @3 n) V0 }& w
  5. double values;$ \- n3 L5 e2 x2 q: g( r
  6. & O( H6 S* i/ ^9 i* j

  7. " h& q/ I- K, M. d
  8. void CNY70()) c5 d* k1 I% w' N$ g! i
  9. {3 k: z& d  t, K+ ]
  10.   valuesRR = analogRead(RR)1 b, b) N% }) E% \4 t% [0 ?0 ]
  11.   valuesMR = analogRead(MR);9 Z1 I) C! V, A- a
  12.   valuesMM = analogRead(MM);* I+ ~2 A7 m$ Q) z, Q% V8 k
  13.   valuesML = analogRead(ML);
    % B) [* y+ k  v2 g" ^7 R/ l0 H
  14.   valuesLL = analogRead(LL);
    " {4 W4 }$ G$ P) U
  15. $ _( D8 i( X" n
  16.   if (valuesRR > CNY70Val)
    % Y) y; f1 `8 \7 T9 c; R+ T& _
  17.     valuesRR = CNY70Val;- p* A, p) Q5 s7 [2 \. o5 r
  18.   if (valuesMR > CNY70Val)& Y4 C& K- K6 W7 t1 C$ ]8 q0 B" c0 F
  19.     valuesMR = CNY70Val;1 I, h, ~0 Y& E+ P
  20.   if (valuesMM > CNY70Val)
    , j0 k' r) r& J! L' T
  21.     valuesMM = CNY70Val;
    7 G9 v1 v& i. O
  22.   if (valuesML > CNY70Val)% ~! _0 Q& N# M, K
  23.     valuesML = CNY70Val;, b! y+ p. B  C( Y' L
  24.   if (valuesLL > CNY70Val)
    & x9 ~# K% e7 h( Q, X
  25.     valuesLL = CNY70Val;
    5 Q+ t: L  o% b
  26. ; t* G3 P9 N6 I6 O! ~
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    . {" m% x4 j" c) O% `
  28. }
    * L( e: r7 L2 ]" L7 R
  29. 8 W! d+ Z  D6 H! D4 m1 K
  30. void Car()
    ( V3 c  l. A7 @2 v. ^! Z. {3 g" w! \
  31. {: d9 E8 Z7 u  p& ~0 \
  32.   while (1) {
    ( A/ p. H* Y# ]
  33.     CNY70();
    # L3 r. M' m5 g  [% |

  34. " F/ t" h1 s& s* ^9 A' z2 t
  35.     int error = ((int)values);5 G0 [4 e- @5 x9 L
  36.     interror += error;
    9 P+ I$ C+ |; n9 m/ {6 G; U
  37.     int lasterror = error - olderror;
    8 y1 D% ~' T! I* x% T0 e. x- t
  38.     olderror = error;
    2 Q! h, ~2 p( y: Q! N
  39.     int power = error / 5 + interror / 10000 + lasterror;0 e+ D0 c; b! k7 w' }

  40. 2 r# _8 ~/ c% r) r4 o
  41.     if (power > MotoSpeed)
    ) [/ e- ^2 c9 D& m5 ]
  42.       power = MotoSpeed;  Y; U; m' r8 _: j+ I1 m
  43.     if (power < -MotoSpeed)
    7 d! }# G8 g; y
  44.       power = -MotoSpeed;
    3 [! L& A; A5 c* c9 d

  45. ! Z5 k0 w; `! q' j5 ]% Z/ p, E; }# n
  46.     if (power > 0)1 P' T# d% z9 y  z& e5 t
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。
    , s- J2 u' p) Y' ?. R6 s
  48.     else- ?5 O$ a" q- L
  49.       Speed(MotoSpeed + power, MotoSpeed);! L) S; F* M: \/ l
  50.   }
    ) Q# w, G7 v9 L$ F! N
  51. }
複製代碼
# q4 u- ]. M3 B' m$ y/ J6 v; ~

$ a8 }& M( s9 B
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2019-8-25 16:29 , Processed in 0.051897 second(s), 17 queries .

Powered by Discuz! X3.4

© 2001-2017 Comsenz Inc.

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