圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21698|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>
    - b1 z% u2 K7 f: p$ J
  2. #include <Wire.h>
    / S1 w2 b2 G. m- p) |1 A/ G
  3. #include <Servo.h>8 t9 ^: g6 {! x/ d, o' P

  4. 7 H/ x$ E* Q+ ~+ N
  5. #include "MePort.h"" f2 _( i) G; g! n$ [- b! h1 @
  6. #include "MeUltrasonic.h": {+ n1 r* o( @
  7. #include "MeDCMotor.h"
    $ X9 Q( l, }& O3 {  j0 J; W
  8. - [. _. g) t$ e& X0 T
  9. //double Input, Output ;* R# w% k/ X$ n5 H  q
  10. float MaxSpeed = 255;3 A) P5 u8 I) s
  11. float MaxPower = 180;6 O; l) K% h" p2 C- N
  12. float MinPower = 120;+ C/ P4 ?% \1 q# t0 U8 o5 R
  13. float Error,ErrorAcc,ErrorDec;/ G7 A, x" G* u, H
  14. + I8 e. _% T( l6 Y* M  H9 n) C+ Q9 o
  15. float Kp=0.14;  o( f. u6 V: E2 [. I
  16. float Kd=0.00020;//23;6 v" e( F! S5 u) x
  17. float Ki=0.000201;
    3 Y- V" _9 z  n, w* H7 ?5 h! \3 M
  18. ; z9 A7 r% J3 F. }$ ]
  19. float nPower;5 N3 ]4 r- D& \
  20. MePort lightsensor_6(6);
    2 }4 |$ e! Z  f+ s( \
  21. MePort lightsensor_8(8);6 |3 y  s8 U3 y
  22. MeDCMotor motor_9(9);* a1 w5 }, b# B  x( h
  23. MeDCMotor motor_10(10);0 O" u# A- H7 M, i! B2 H
  24. unsigned long previousMillis = 0;
    " N- g$ n- g, y
  25. const long interval = 1;7 @0 z' t: p/ Y: y' l6 e
  26. . c$ A2 [1 |9 x% Z6 i. ~5 L2 ~
  27. void setup(){) G) d% V" a5 v$ o
  28.     lightsensor_6.dWrite1(1);2 O3 c1 `5 J9 ]& D' }
  29.     nPower = 160;
    / }' ]# p/ K/ a2 _# y6 X
  30.     Error=0;
    ' g' y  F; e$ _. g, r+ N
  31.     ErrorAcc=0;
    1 i) g8 [& Z- u: _
  32. }
    + c. M& O- e4 @
  33. 5 w" C& p# x, A9 d2 D
  34. void loop(){
    9 a+ m% q* U+ z; N: @7 p
  35.   unsigned long StartTime = millis();/ H" H1 r3 T, H2 f7 a8 @) f
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    * S( Q. ?+ ^. B% b% ]
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();9 T" O* E  W6 G7 R1 [
  38.     ErrorAcc +=  nError*Kd ;
    1 l; e6 W6 p! m1 a# H
  39.     ErrorDec -=  nError*Ki ;
    & {* f! \1 l  r1 C
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    2 @0 ^) E# b4 ]* J+ z' e
  41.     if(nError < 80 && nError > -80){+ Z7 @( z  x3 X$ O
  42.       if(nPower < MaxPower){5 `& X& R5 ~% d% V, x0 G% r# [
  43.         nPower += 3;
    3 v& N  v( z/ R: O
  44.       }
    * e$ [* A! U: U/ p
  45.     } else{, U$ E0 B1 f! Z* L2 \  d) B6 [
  46.       if(nPower > MinPower){
    & j# J# u, |8 s; w) s' e; c$ W! Z& i
  47.         nPower -= 2;- p  {2 B* R% d9 |, ]; x* |2 \8 s7 F
  48.       }
    . d. W' @( E: Y) F, o
  49.     } - S* H, x5 W# P7 S2 N6 s1 c% |
  50.     MotoL(nPower-Error);
    7 n  J" b2 \  j+ U0 K
  51.     MotoR(nPower+Error);   
    5 U6 E0 ^! ^# |% i7 ]8 O" }5 V
  52.   }else{
    3 C' t- S$ U6 v* d
  53.     motor_9.run(0);: A7 L  ^" V" ^9 J) ~. g
  54.     motor_10.run(0);
    0 ]2 W! }* I, j5 A9 o
  55.   }# m7 W0 o; D5 j3 k7 ?/ N
  56.   do{}while(millis() - StartTime < interval);
    3 }3 _4 I. K; L1 T' y$ ]) p
  57. }
    $ v& E) L! Z' Y
  58. : ~7 r% C/ [- U
  59. void MotoL(int Power){
    9 y  k/ o5 o3 `3 y& t: T' M
  60.   if (Power > MaxSpeed){3 d( y. A) i) d! g
  61.      Power = MaxSpeed;& k/ M" w0 j: r; }; i
  62.   } # |& k* n% E: b5 _5 ^7 k4 b
  63.   if (Power < -MaxSpeed){6 k: D% Q1 V8 t- S: _+ R# {9 t( x
  64.      Power = -MaxSpeed;
      `; Q0 C) T& V, ?
  65.   }
    ; `& g7 |+ G1 W4 |; R" m
  66.   motor_9.run(Power);
    ! g* h. J% a% ?6 T& n; X
  67. }  ( N' @6 s- M4 C* Q$ K- k7 [, x

  68.   x  ^' o4 f' T* f* r8 s
  69. void MotoR(int Power){7 r8 @- q5 K$ l- O0 {" T5 V, y
  70.   if (Power > MaxSpeed){, G* u! h7 z( f* f6 y
  71.      Power = MaxSpeed;; A6 J0 _$ ^$ |8 H: z
  72.   } 9 M$ a" B' O8 d  P" p: a' x
  73.   if (Power < -MaxSpeed){8 }! h' T+ d0 M8 o1 Q: }9 C7 e/ P
  74.      Power = -MaxSpeed;% w9 ~7 H3 q" c. L5 I3 \& F. ?0 {
  75.   }
    / i+ b, D4 v. l/ Q; t
  76.   motor_10.run(Power);' t4 J9 m7 v3 N, r
  77. }  
複製代碼
. T$ {5 M5 d- P' K4 D& R

# U/ r+ N3 \. B
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯
1 ^5 z" z1 D. A/ g& O. ?% D6 k! I! X# {
您好,不知是否能向您請教。7 i: R+ z/ h6 @( A6 Y( ?# X
目前和宋修賢老師在處理Ardui Car
. `4 s" A' O* w: B; H& r雖然已使用較繁雜的方式處理了跑出黑線外的狀況
, {1 B. P2 F. N! g  t! E8 W6 ]  B7 f3 U8 O: l+ S
但基於想追求更精簡的程式所以還是想請問一下5 @. b; ~" N" O4 p# t
就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝
9 N$ r4 i- [8 i* W9 j3 z4 K不知道您是否願意教我可以如何處理! Z( C( X  a' T% e3 ]9 s5 N

0 w( s1 |" k4 \5 N' O( N+ l  }5 a" I
以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;! \; t2 f4 j1 e9 k- a' `
  2. double CNY70Val = 1000;
    # W/ U! T$ ]/ G: T8 G) |7 s
  3. int interror = 0;
    / J; J4 p/ s3 @$ g7 @' c
  4. int olderror = 0;
    # I" q3 j7 p+ Y# B- K3 L
  5. double values;  P  e! D1 P9 r) N8 H

  6. ' ?4 x$ g" E$ }

  7. ! {& S. z: ^; k$ Y
  8. void CNY70()
    - r# s9 g& v  A
  9. {
    5 _! r( g$ n6 X" z1 s+ U  G) q/ `
  10.   valuesRR = analogRead(RR)$ Y2 h5 ?& T# ^8 ]9 r
  11.   valuesMR = analogRead(MR);
    % e- V: O, z( V/ c# t  `5 ^
  12.   valuesMM = analogRead(MM);7 i3 i$ h9 H8 I. C& a! }
  13.   valuesML = analogRead(ML);
    ) |7 D0 C! z$ w( T% K7 c
  14.   valuesLL = analogRead(LL);& k" M" [) E, j4 u8 U  ~
  15. 9 n" H! ]4 p6 y# W9 d8 Y
  16.   if (valuesRR > CNY70Val)
    ; K# \' U# L% n
  17.     valuesRR = CNY70Val;+ J. \% c2 g% d( T
  18.   if (valuesMR > CNY70Val)
      i1 E6 @. I+ w0 B
  19.     valuesMR = CNY70Val;# ^# z$ l0 m) u- X
  20.   if (valuesMM > CNY70Val)
    9 u: l, {' @* f8 v' |
  21.     valuesMM = CNY70Val;
    2 H% J! V- U/ i" [) t3 N, L
  22.   if (valuesML > CNY70Val)
    ( \& ?( s0 j$ ?+ a
  23.     valuesML = CNY70Val;. U4 w7 A. K3 E$ Q5 R5 Q
  24.   if (valuesLL > CNY70Val)* G: I0 u, n* Y1 g" M. e
  25.     valuesLL = CNY70Val;/ ~+ H4 l4 r+ t& O

  26. . e; ]6 h; @* ]2 ^; R# `1 v
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;: C) Z4 A: P( A6 [8 V- `
  28. }9 z3 k' S) M% i* p3 a( }

  29. ) ^/ t  n$ ]5 }/ M
  30. void Car()
    ) D: N7 O3 i, d' B! y4 v, u9 v1 ^, I
  31. {
    - n8 T) ?! p& ^) d
  32.   while (1) {
    ' v" M. B  ^( O- e" l; P% n
  33.     CNY70();6 n% Q- p$ u4 ]: w. w& V
  34. % `9 D- c2 U/ W0 h3 u
  35.     int error = ((int)values);0 _" K% P( Q5 U0 P9 C, q+ H6 M9 x: `0 X3 x
  36.     interror += error;
    + w& v8 t" a6 c) [
  37.     int lasterror = error - olderror;5 y% V, k, ^% c$ ^* {; ^
  38.     olderror = error;6 R$ ^* C% z: d4 [/ s5 b8 `
  39.     int power = error / 5 + interror / 10000 + lasterror;- Y# S+ Y( z/ L2 O5 k0 n
  40. : A, _; Z! c( y  _9 _: j: x
  41.     if (power > MotoSpeed): @( v+ ?8 f3 P! Y" w$ ?+ U
  42.       power = MotoSpeed;% b% w% F" d+ X3 h
  43.     if (power < -MotoSpeed)& e. [) P0 i2 O' J. _" u! H
  44.       power = -MotoSpeed;
      O' G" ?0 J  F  S% b

  45.   i1 z4 B7 e9 e+ l& M/ R
  46.     if (power > 0)) v# B2 J0 L8 [. U/ i  ]5 `
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。( Z+ h* n  @  K0 a6 r5 n
  48.     else0 l7 {9 f) ?; \- M. H
  49.       Speed(MotoSpeed + power, MotoSpeed);
    0 k5 W4 X- W: @$ z' C  B
  50.   }
      i- e& Y. o  S& Q9 A5 I4 T9 p6 D& x
  51. }
複製代碼
/ b5 {) Y5 I# E1 [
% H9 q; [# k) g- J6 ?6 v) D
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-11-23 16:11 , Processed in 0.026121 second(s), 17 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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