設為首頁收藏本站

圓創力科技.MakeBlock TW

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

掃一掃,訪問微社區

查看: 9570|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>
    - W/ G7 A% P( U  h
  2. #include <Wire.h>( x3 C2 g' _2 `$ u  L# O
  3. #include <Servo.h>
    3 M! y# R1 X7 F& M% m
  4.   k) @/ T! A" N
  5. #include "MePort.h"! |' P' T: `! A+ b
  6. #include "MeUltrasonic.h"
    ; ]$ V* u' Q  {$ d" Q) \$ @9 F
  7. #include "MeDCMotor.h"4 i1 c9 C# F, R/ i1 i- O
  8. 5 D: K. S; L% ]) Z6 I
  9. //double Input, Output ;, x8 k6 A1 m1 J' p
  10. float MaxSpeed = 255;1 t( \. j, C; L1 A3 `; d
  11. float MaxPower = 180;3 C, L2 b  ?' j" V
  12. float MinPower = 120;
    9 I' L, x# u. U% D5 Z
  13. float Error,ErrorAcc,ErrorDec;
    / t1 j2 E+ M" ?3 k- ^8 d
  14. ) G/ A6 q$ d7 o
  15. float Kp=0.14;+ G- |6 N$ L9 S; o
  16. float Kd=0.00020;//23;
    . S3 r# C* u  ^1 f0 u
  17. float Ki=0.000201;5 l! v. h' j) s& M/ s) r! X

  18. + Y; o3 o' L- e# ~, O( R- H2 m, H5 {
  19. float nPower;
    ( O0 R: |7 M  @2 J5 q
  20. MePort lightsensor_6(6);  ?  I8 [/ T5 C8 B$ S, v0 t
  21. MePort lightsensor_8(8);! C8 ^8 Q2 W, Q) i5 f. W/ a
  22. MeDCMotor motor_9(9);
    : s0 q0 |9 b* T7 o/ u. h0 x$ G' f
  23. MeDCMotor motor_10(10);" D% O# z+ o! V1 h# G( l3 Z
  24. unsigned long previousMillis = 0;
    ! ]+ k1 s* R6 E1 Z) ?# x
  25. const long interval = 1;8 ]0 z4 r8 q- t/ \+ E8 i/ D9 `

  26. 3 C: B& n/ k) V$ S, A
  27. void setup(){/ n; U( M9 X9 H
  28.     lightsensor_6.dWrite1(1);0 a% Y4 F9 K6 |- N! N0 c: i
  29.     nPower = 160;
    4 W/ K' v9 e+ `& o
  30.     Error=0;8 z2 X. @9 Y+ G) S7 V( a$ v: P
  31.     ErrorAcc=0;* v, s2 L- T" \6 {' s
  32. }
    9 D2 l# W# e8 g6 ^- |% n1 v+ f

  33. 7 ]2 k- x6 J- z- S* @9 ~% \
  34. void loop(){2 T* d" T* \2 p0 e
  35.   unsigned long StartTime = millis();2 L% }$ |4 v- H" |: L; h
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    6 |) e" F: {" Y& {- S# M
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();1 P/ @- X9 c+ f$ J. y; `
  38.     ErrorAcc +=  nError*Kd ;
    7 b& d& s3 z% ^' O- r
  39.     ErrorDec -=  nError*Ki ;! r9 i! x% c( y% y4 F- U
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);) `  X$ ]" g8 V! l' A: X& ~
  41.     if(nError < 80 && nError > -80){
    2 S9 V' P- ^# f' E3 Z1 t+ ~! z
  42.       if(nPower < MaxPower){
    ( ]' h- M0 X4 Z5 N8 o) h
  43.         nPower += 3;+ R* i6 y; q+ t- F0 [" a: ^
  44.       }+ T2 q7 A1 V9 W* q  D4 g
  45.     } else{8 [2 \8 D& k. Z1 }. C9 i: ?% `( D
  46.       if(nPower > MinPower){
    % g: a* x7 Q& C! V7 @4 u
  47.         nPower -= 2;$ F; A  X8 [1 p. i( I& B5 F
  48.       }
    ; R% n; M9 h- _3 u& m1 @
  49.     } ) Z3 e% q# c1 B4 n( l& F0 W
  50.     MotoL(nPower-Error);8 S; a! S  l  Z) F3 ^  v, c
  51.     MotoR(nPower+Error);   
      |( h3 G! f" Q$ l
  52.   }else{
    # x5 _* v; I7 B; }9 R  r/ h
  53.     motor_9.run(0);+ R1 R2 l. ^0 U! U
  54.     motor_10.run(0);0 k0 O+ u: w, J7 b4 N
  55.   }
    9 C6 F$ |3 ?  f, H5 z% Q2 a
  56.   do{}while(millis() - StartTime < interval);
    - ?; k# S: F3 X
  57. }
    ( l: P0 E; E4 \: n% @

  58. ' `* Y/ E; f# e# H/ H5 _' K1 K
  59. void MotoL(int Power){6 U( ^5 p& U# D" |0 i
  60.   if (Power > MaxSpeed){& _2 O$ C$ }/ O5 Z
  61.      Power = MaxSpeed;
    & h& _5 y. o; J2 P( H; }' @/ b
  62.   }
    , x8 e4 ~  K5 j3 W# [( }
  63.   if (Power < -MaxSpeed){: @0 O, Z9 {9 J" [
  64.      Power = -MaxSpeed;
    ; F# _# h; b) k9 L! |$ M& y
  65.   } + F! k% @, w% [  L& }, [
  66.   motor_9.run(Power);; u  F% _" {) B4 C% j& r+ U
  67. }  
    , @4 ~3 n1 Q5 v7 [9 H3 M% ~  {% f
  68. " j9 x7 @/ E  X1 \# v$ g) c
  69. void MotoR(int Power){
    0 A, u+ m7 [  O8 L! i+ R# v" d
  70.   if (Power > MaxSpeed){
    0 }$ s8 {2 B; z
  71.      Power = MaxSpeed;
    % [- |/ r# E2 P( W. G
  72.   }
    7 \/ X& I$ M# j3 F) y3 r8 z
  73.   if (Power < -MaxSpeed){2 |6 M  d: t$ ]/ g) i, v
  74.      Power = -MaxSpeed;
    3 E6 g' k/ J/ I+ F' O
  75.   }
    # N. G8 w0 i- z/ |; E  Z
  76.   motor_10.run(Power);
    * U- n# q. ?8 s0 t( A% M. K$ p
  77. }  
複製代碼
9 \. [8 J( f; A9 m3 a) S
0 ?4 P  n' R" D# q( S
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯   h3 X) @, }3 P8 ]! P

4 P/ `% Y* O6 ^7 Y" [, l7 m) G5 w您好,不知是否能向您請教。
  q* z- n: i6 F9 g& B8 [目前和宋修賢老師在處理Ardui Car5 V- D& a/ h0 _
雖然已使用較繁雜的方式處理了跑出黑線外的狀況
* A0 e9 @5 @. g" |) M
# x' r: n3 f1 n3 U但基於想追求更精簡的程式所以還是想請問一下
. q, J# w* o% q: Y  b就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝
# B. w6 p! J. ^) ?; J) z不知道您是否願意教我可以如何處理" e6 N2 H, f& Z- I5 n
* r5 f$ n2 u' c& |, X9 f, P$ D

+ O! d$ [; |$ }% a. I以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;' ?1 k1 }5 K; p9 [, o, m) k. j2 r
  2. double CNY70Val = 1000;+ H, {9 x0 T4 y$ a, B5 H* j
  3. int interror = 0;
    $ W" r; f2 R, X' P  t4 m( {, x
  4. int olderror = 0;( n( J' x+ e8 D& f
  5. double values;
    % o4 m% Z( @: V$ u3 [
  6. ) q# F3 o( W9 c8 ?. A

  7. 7 b7 |3 r9 Z8 a5 p' c
  8. void CNY70()
    3 m' r3 i4 R$ P% m; ~6 E
  9. {
    ( Y* E. `5 P; k* \$ ~; D! r
  10.   valuesRR = analogRead(RR)
    ' \7 Q5 f7 [5 X3 {4 A
  11.   valuesMR = analogRead(MR);
    ) G. t* r% |/ _; U8 |: G' J& f
  12.   valuesMM = analogRead(MM);
    7 ~  ]. y$ p1 P
  13.   valuesML = analogRead(ML);
    9 j# W8 H# P; X, S! m
  14.   valuesLL = analogRead(LL);
    , _0 h5 V1 u1 G( X2 ~2 X3 D6 _
  15. 9 x# o, s8 j- P" ?3 W
  16.   if (valuesRR > CNY70Val)- U1 G8 t0 C8 l7 K- d! C8 t
  17.     valuesRR = CNY70Val;
    & l9 Y& R7 g2 `; A
  18.   if (valuesMR > CNY70Val)' j5 d* H4 N3 q9 n0 I
  19.     valuesMR = CNY70Val;; L. W/ D4 I. S& Y
  20.   if (valuesMM > CNY70Val)6 W) d! @. r9 N2 i; p/ f, }
  21.     valuesMM = CNY70Val;
    ) t$ H' z' d  \3 n0 q3 e
  22.   if (valuesML > CNY70Val)% s' G: n1 W" B5 |$ H8 ^: X& m
  23.     valuesML = CNY70Val;3 a0 @2 U4 i) z( y$ D: e2 r
  24.   if (valuesLL > CNY70Val)( x: p+ S- V0 u9 ]& G7 j( A
  25.     valuesLL = CNY70Val;6 i0 w/ y% X- K+ J
  26. 5 O  k! H+ Y8 |; C* I3 ?4 T
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;! d6 }! i& j& y
  28. }  z" H6 K, p0 \

  29. 0 @# @3 Z/ z* u1 F, e8 t9 v# s" R/ \
  30. void Car()
    & s4 h$ O: V6 N7 b1 {4 e
  31. {/ k5 N; H' X' j2 z; ^
  32.   while (1) {2 `" h# s- J( u. o! w& r
  33.     CNY70();7 z( O- m& S5 U

  34. : I* r7 t4 ?/ G: O0 t
  35.     int error = ((int)values);
    - O/ }3 f6 F4 e
  36.     interror += error;# i6 L: V3 i2 m6 k
  37.     int lasterror = error - olderror;0 e! h; B; s1 X  X/ N0 _
  38.     olderror = error;
    ' k0 a0 i, S# z! g9 c/ i
  39.     int power = error / 5 + interror / 10000 + lasterror;
    & P0 m, ~: A) a) Z  {2 [* L

  40. & A, L( E0 M4 [7 z
  41.     if (power > MotoSpeed)
    # F6 o% j; H# _( R6 y
  42.       power = MotoSpeed;
    ' w) o  [- W9 @4 S7 G" p! C3 H3 G
  43.     if (power < -MotoSpeed)
    / b+ F$ J7 I& `7 [
  44.       power = -MotoSpeed;
    ' B5 @- Q/ P/ N; z4 g

  45. 2 V4 G' U" M' m
  46.     if (power > 0)3 w) i8 f6 d/ m4 }& c9 \+ B
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。% j; |9 b0 R  w- V( {
  48.     else: q( u! J+ {7 a0 S
  49.       Speed(MotoSpeed + power, MotoSpeed);
    1 t& U( F" C7 R+ Q9 Z
  50.   }2 e- k' C5 ~  h+ y4 w9 r
  51. }
複製代碼

7 J- ~" `4 o) \  c( W- W7 |
% [( B0 n, V/ q: E2 U8 T
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2019-10-17 14:15 , Processed in 0.045956 second(s), 17 queries .

Powered by Discuz! X3.4

© 2001-2017 Comsenz Inc.

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