圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21799|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>
    ! H2 Q/ k, K$ P! u% \/ b1 S
  2. #include <Wire.h>
    4 d( ~& v# R# T. D; U. i, d
  3. #include <Servo.h>& ~  |- c$ U# P& _

  4. ! Z9 P& L2 N! K& e8 |, `: D
  5. #include "MePort.h"& M" P) H1 C6 V* J) N; H" W6 |' F
  6. #include "MeUltrasonic.h"
    ! G7 |. V3 r. ?
  7. #include "MeDCMotor.h"/ y# b3 e( x: D  h/ Q8 w

  8. , [6 P4 E; o4 c
  9. //double Input, Output ;2 Y5 V$ z" _" L2 U+ W  S5 r2 O
  10. float MaxSpeed = 255;% ~( O& k. K: Z$ m) y/ b
  11. float MaxPower = 180;+ g4 _/ Q) y  d
  12. float MinPower = 120;' d- }) F0 d* |7 h( r
  13. float Error,ErrorAcc,ErrorDec;
    % @  r  U- s( A
  14. , S: o. @3 L4 |' F/ e1 [( F  S
  15. float Kp=0.14;9 `- k) _; a# ^
  16. float Kd=0.00020;//23;1 {% p( W5 }3 V
  17. float Ki=0.000201;' }1 G9 f  |% e3 ]5 q

  18. ( e7 g' R$ f* k: ]
  19. float nPower;  i5 j, T* x, H# k5 R' ~
  20. MePort lightsensor_6(6);
    + M1 `/ ^# L  C2 G( _) z  h
  21. MePort lightsensor_8(8);! t& u: r- ?6 s7 k
  22. MeDCMotor motor_9(9);
    7 O3 B0 G7 {. y& B% O& k
  23. MeDCMotor motor_10(10);4 p. B* G' Q1 x- n2 k
  24. unsigned long previousMillis = 0;; S- b6 i& H7 I" _8 R% w1 g4 f/ F
  25. const long interval = 1;
    8 {. A0 m% E" f5 m% P8 w

  26. 1 h) h6 I: \4 K) z& ~% h7 X# @
  27. void setup(){" C1 X+ d7 ?3 m+ [% F
  28.     lightsensor_6.dWrite1(1);
    6 r: ^) i8 S5 z$ n! u! E/ y+ m
  29.     nPower = 160;. x6 |, j' ?; C* @
  30.     Error=0;8 d* g3 r; K+ G8 y* F
  31.     ErrorAcc=0;
    1 |% u1 E* O1 {5 O
  32. }
    # L9 N$ u8 S' `: y" L9 z0 ^
  33. ; `: f7 @) O; B6 r& w8 m
  34. void loop(){
    : v: z, d, f% F5 e3 ?6 Z" n4 P
  35.   unsigned long StartTime = millis();
    ! c0 }, i; x: M$ ~- w$ X, c
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    # b$ B4 {" G9 d) a; R7 L4 M9 S
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    2 u8 K& I5 K  _4 J7 j- t6 I; P
  38.     ErrorAcc +=  nError*Kd ;
    2 j4 j* p) h& P. n* M
  39.     ErrorDec -=  nError*Ki ;
    6 f  e9 |7 {' n
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);) K# i6 ~5 C/ ~4 z( i
  41.     if(nError < 80 && nError > -80){
    * q8 {/ y( {; R' R4 ^
  42.       if(nPower < MaxPower){
    ; f4 c2 k0 Y0 \* T. p$ _/ C
  43.         nPower += 3;
    6 e5 s. T1 p4 t& c' ?8 k
  44.       }
    + b! Y9 T/ y9 G+ j' u( J
  45.     } else{
    0 M) |1 Z" j0 e+ p, Q( }2 H
  46.       if(nPower > MinPower){- `& J8 D% w/ t( d3 i
  47.         nPower -= 2;- f! V# t; F2 h- B) c9 n* ~3 {8 J0 s
  48.       }
    ) o8 h8 y' r: H' h% P
  49.     } + w; U, O, Q$ t0 t# d
  50.     MotoL(nPower-Error);
    7 M+ T, Q9 e+ L% p* Z
  51.     MotoR(nPower+Error);    2 }: v/ f" S% D
  52.   }else{
    , h1 z+ ?( {, R  J; U, |" E& G
  53.     motor_9.run(0);; q0 S/ b& n6 m8 J8 z
  54.     motor_10.run(0);
    0 c) Y2 T! S$ ~8 G
  55.   }
    ! Y) b9 E6 f4 U; x1 i0 R
  56.   do{}while(millis() - StartTime < interval);/ G" T6 w! z# i% Y
  57. }
    ' V( k& S# {. m# K, B# w% Y

  58. ; F0 D  j1 j3 o+ u0 r
  59. void MotoL(int Power){2 ]# `, h. y. h5 \1 r# s7 H
  60.   if (Power > MaxSpeed){
    / G* a1 d. j, U: V; V
  61.      Power = MaxSpeed;
    0 I# }0 J- W+ E. R/ [2 [+ R. l* F
  62.   } 5 O# Y; [* E1 w! i6 U) N. q9 [4 a
  63.   if (Power < -MaxSpeed){
    7 Z" R3 D" F! u1 V( [
  64.      Power = -MaxSpeed;
    " m& l: }' F  c( u8 E
  65.   }
    & H; y" J3 r% O9 r6 q7 x* `; P
  66.   motor_9.run(Power);
    + U3 u# h' ^: w* o% P
  67. }  
    ! ?- j* ?2 c' ~: X( P
  68. 4 I. {, M9 t) [- e2 W
  69. void MotoR(int Power){( v2 d" @# M; m3 A6 u
  70.   if (Power > MaxSpeed){
    ; a) z- F8 m/ I, x/ `2 D6 h
  71.      Power = MaxSpeed;
    : u$ U- L" m( A! d; W( Q) p. ~
  72.   } 5 E" i! k3 [. u# X' L$ ^. h6 y
  73.   if (Power < -MaxSpeed){6 {8 P, B0 G4 g, c! Y# Q8 X. y
  74.      Power = -MaxSpeed;
    ) l. B1 _! [/ O' Z) h
  75.   }
    ' Z: e) `3 Z1 P& c
  76.   motor_10.run(Power);* N6 H% t6 |& A9 B1 c% O- n
  77. }  
複製代碼

, x3 w3 R% r: ^2 B8 W5 k  W
  ?5 B' T  u; d1 B6 p2 v6 S# z1 ?
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯 : @# S. G+ B  n$ d7 m1 q

: B7 z6 B, V: B3 T您好,不知是否能向您請教。
0 |2 s* e  S" B4 ~% Y" Y% H/ y$ V目前和宋修賢老師在處理Ardui Car, c5 j- t/ H* B! x: e# S! n
雖然已使用較繁雜的方式處理了跑出黑線外的狀況  _2 O! g) V2 j. g4 z* Y

- a% B5 f% N! z5 c% T但基於想追求更精簡的程式所以還是想請問一下
; d+ C- q: C+ T就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝
1 t" J: I" B$ {9 Q- H0 z不知道您是否願意教我可以如何處理* @' ^* A, h. z* h4 g# g
: A7 s: X- o! g8 F

. q( P6 E5 {" x0 z/ p% i, C/ F以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;
    # ]% C- E! D3 u9 E  J
  2. double CNY70Val = 1000;
    $ ^& o# V$ A- g% F! N/ i$ {3 [/ D. Q
  3. int interror = 0;" A' Y2 Q  c5 b- _/ ?2 Y
  4. int olderror = 0;# l/ l* D& K) n) R( G# K" e# J, j
  5. double values;% T7 w# v; w6 A( E* p! ]8 R% w
  6. ! E6 s* W: _/ t5 U; q# s4 i( T

  7. 3 A+ ~8 u) B  U, f6 N
  8. void CNY70()
    % J7 F" `/ a  U' i. c
  9. {
    7 X9 v  X; Z6 x' f% u, o
  10.   valuesRR = analogRead(RR)5 J6 W' T' P4 S; E( {* ^4 V
  11.   valuesMR = analogRead(MR);" I- V* c" J& J% ~
  12.   valuesMM = analogRead(MM);
    - b+ Q9 k  [. P- Y% Y( a* ~
  13.   valuesML = analogRead(ML);
    - L4 q) m) f3 Z- w3 h. a
  14.   valuesLL = analogRead(LL);* g, U8 F9 E/ }

  15. * y/ @) Q$ C' s
  16.   if (valuesRR > CNY70Val)
    2 C5 _( c" P6 e
  17.     valuesRR = CNY70Val;
    3 B6 X+ Z( T5 ~1 @/ @7 w  F# e
  18.   if (valuesMR > CNY70Val)
    $ t1 J% G" T1 [& o) a
  19.     valuesMR = CNY70Val;& e3 B& O2 k" v2 ]
  20.   if (valuesMM > CNY70Val)
    $ ^$ V- X: k0 ^0 L8 [7 Z( ~2 V
  21.     valuesMM = CNY70Val;0 A, ]  v4 B  Q/ e/ e" P
  22.   if (valuesML > CNY70Val)
    7 A# R9 U1 x' t5 q
  23.     valuesML = CNY70Val;
    0 T; Z  k( J5 S5 `4 K! b
  24.   if (valuesLL > CNY70Val)3 V% Y' d6 a1 t2 \$ M5 c: @4 }+ j
  25.     valuesLL = CNY70Val;% B. d( r- G: o5 }3 h  G

  26. " I, ?4 I" K8 [. V! d6 Y: s
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;( H% y0 t* }0 r  o; v$ \, B: `) x6 X
  28. }
    " d7 e5 g) A0 z6 D

  29. 2 c# F4 V4 I* i2 {2 G1 n
  30. void Car()
    0 S! p) b1 D) \7 A+ f8 R1 g& c
  31. {4 s2 [! D1 A0 w2 f. i
  32.   while (1) {
    5 I/ d9 n& q7 B0 l/ r' B
  33.     CNY70();& Q9 T: y. @/ o8 P4 ?0 c; D

  34. ! m5 Y- r; h2 p: I3 c$ q! l
  35.     int error = ((int)values);1 u3 d1 t4 V6 x& {- _& S
  36.     interror += error;
    : [& f( g6 W9 l3 U/ ^& r* B
  37.     int lasterror = error - olderror;! p3 q: K! {( d! ^& F; u9 U
  38.     olderror = error;7 S; b: w% W7 ?
  39.     int power = error / 5 + interror / 10000 + lasterror;
    7 R9 L0 e; |7 F
  40. 5 L& A5 p8 B$ r. b) h' E( t
  41.     if (power > MotoSpeed)
    0 x& V( B9 `( Y/ ~5 [: J
  42.       power = MotoSpeed;
    ( j5 [# P/ b# ?! }2 [7 p. Q# T
  43.     if (power < -MotoSpeed)
    , n; j4 F3 [+ X
  44.       power = -MotoSpeed;/ F! j% w6 A9 |; G7 b0 `4 R( g, z

  45. 1 t, L; N3 K6 T) l3 N3 `: e. R$ g
  46.     if (power > 0)" L! k7 j+ v& W3 p. a
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。
    ! v. C3 ]3 a! ^) k% l5 b
  48.     else- ~4 X( _; y# [$ g3 T
  49.       Speed(MotoSpeed + power, MotoSpeed);
    " W( e7 F, Q/ j
  50.   }
    * L" X" }; N/ h" t% r+ R
  51. }
複製代碼
0 j* E8 m. [8 G* ]  j5 L: ]

  y( N9 r  \9 A
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-12-1 17:48 , Processed in 0.023723 second(s), 17 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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