圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21688|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>, W/ w* y, h1 I5 a/ Q; d
  2. #include <Wire.h>
    ) p2 G5 Y/ T* Q
  3. #include <Servo.h>& q( p: ^4 |  K. E" y* N" O5 p1 W0 k
  4. ; I3 P  }: ?4 }6 L4 ~7 v& S& |
  5. #include "MePort.h") J! M5 l. P3 K8 R/ I' D& o+ D+ r
  6. #include "MeUltrasonic.h"9 A1 w) t3 M4 [$ y' N
  7. #include "MeDCMotor.h"
    " e4 B$ I% S3 o7 j

  8. & S, Y& M5 |0 b
  9. //double Input, Output ;/ z9 H# z9 s4 W7 n8 ^
  10. float MaxSpeed = 255;
      Z9 X  H2 e. D+ h/ m
  11. float MaxPower = 180;# q% w2 e) a  M2 i9 o/ h
  12. float MinPower = 120;! ^% n% d0 X* _  j  T; x" b
  13. float Error,ErrorAcc,ErrorDec;4 q# w/ I3 i7 M$ @* J& L- E# |

  14. ' ~+ d; G# v1 Q) r
  15. float Kp=0.14;
    3 @3 k" w/ V5 F1 ?8 @4 B
  16. float Kd=0.00020;//23;# u, a  m& t, F% Z6 [
  17. float Ki=0.000201;+ n& L4 {" t5 S2 [* X8 {
  18. 0 Z: X. s4 a+ f  y' C; h
  19. float nPower;4 V+ A% F) U7 P* H, X3 W5 e
  20. MePort lightsensor_6(6);
    2 f( S, r% C4 M6 L1 d5 y+ Q
  21. MePort lightsensor_8(8);0 b$ |- G9 O7 R/ d3 ?! m9 ~# g
  22. MeDCMotor motor_9(9);. Q" t  |+ s' f5 K: R8 a7 K  C
  23. MeDCMotor motor_10(10);
    5 V2 j1 x# n/ D7 _' V3 C  c( w+ M
  24. unsigned long previousMillis = 0;2 Q: G! h* p6 j: j: }: Y3 |
  25. const long interval = 1;. p2 D$ b" D+ L7 _1 R2 T
  26. ' c5 w! k! u5 |) t) Y/ R0 M
  27. void setup(){
    $ f) Q0 n* Z' N/ H0 G: {3 m# a
  28.     lightsensor_6.dWrite1(1);& G' o! `  \- z9 ~) P2 d, q/ n6 [" C
  29.     nPower = 160;
    & a2 V+ H( @' L# u+ r
  30.     Error=0;. f8 {" W0 i# B2 q0 T; z& n; s
  31.     ErrorAcc=0;7 j* Z+ [, R7 z1 ]8 @
  32. }
    6 w- D1 s3 M% E& Q0 ~+ f2 s( Y$ z3 P

  33. & F& ~1 @1 Y9 n
  34. void loop(){
    5 C4 G0 J# h4 ]. m/ E" t/ {4 s
  35.   unsigned long StartTime = millis();
    " R. n' I+ W! H% R' E$ \
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){) T* A: o% I- D9 a
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    ) ~/ B/ r3 N  R9 s1 C
  38.     ErrorAcc +=  nError*Kd ;
    ; ~1 J0 y$ y  p+ l; o
  39.     ErrorDec -=  nError*Ki ;2 V7 E* e* P" n- @
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    - g. h' ]1 t6 b: P
  41.     if(nError < 80 && nError > -80){
    + r1 [1 s1 a5 ?1 p4 a1 c
  42.       if(nPower < MaxPower){9 B: R) {+ F# S
  43.         nPower += 3;( f+ k2 @4 K$ `$ L5 {
  44.       }
    6 ^6 D- b' c1 N
  45.     } else{
    - Q4 T! o2 l0 \; _4 H' \. P2 |
  46.       if(nPower > MinPower){
    : S2 t' s1 c! B# i7 a
  47.         nPower -= 2;
    ! }$ C. e2 f! w4 A% S6 K' t) {  Q# {
  48.       }2 N- D0 p9 I! F% C
  49.     } + }& f$ P: f' Z3 X  n: U
  50.     MotoL(nPower-Error);- D$ I3 i8 C$ ^) p9 W1 u5 l
  51.     MotoR(nPower+Error);   
    ' `1 e6 i& }# X; s
  52.   }else{
    ; p  W, O$ o& B3 z' U
  53.     motor_9.run(0);
    & g& q8 C5 N2 M* [  g7 p1 u! t
  54.     motor_10.run(0);
    2 r& X+ A0 N! y: o0 j2 [( e+ Y* t- U
  55.   }( a7 S$ D4 K1 e7 p3 i1 O( A; P. d
  56.   do{}while(millis() - StartTime < interval);
      F: z) g: T4 L, B. F& Z* g
  57. }
    3 T- i% v, u' n1 g, |, N6 P
  58. ' ]  Z3 t( N8 \+ ~9 t  P/ O! P8 A
  59. void MotoL(int Power){
    ! t" `; `1 t" R. v3 c  {4 V
  60.   if (Power > MaxSpeed){! X: i, [. ~2 C  e# c
  61.      Power = MaxSpeed;1 Z+ J' L& B/ t+ q! o3 p0 S
  62.   }
    6 c7 r& Q, C, ?* M5 X( w# @- n/ Y  K+ e
  63.   if (Power < -MaxSpeed){( `9 i* N# z  q
  64.      Power = -MaxSpeed;9 x2 W; ~+ X& z; T0 G( u% Y
  65.   }
    % ]4 j  u: O/ @6 J! g
  66.   motor_9.run(Power);
      Q: Z) U: \5 C* W5 C) T( R
  67. }  ' }  e# R1 c; B4 Q
  68. 5 w, \+ f3 {  `& A
  69. void MotoR(int Power){
    0 f0 r  x6 J3 M& j1 T
  70.   if (Power > MaxSpeed){( g( y# {6 q* d2 n8 f/ l8 A" y
  71.      Power = MaxSpeed;; }5 `6 j3 Q# z8 p$ F7 i3 v
  72.   }
    / Y; N" n3 j* G3 @3 ^( f4 r
  73.   if (Power < -MaxSpeed){
    ; A- U' ?6 H% I1 H
  74.      Power = -MaxSpeed;
    $ X: y, A# w0 j1 F! m6 B6 C
  75.   } % B; G+ J/ w1 J" p6 Q# R8 Y
  76.   motor_10.run(Power);* q9 p0 L" D4 z5 M
  77. }  
複製代碼

+ {+ x# l" z5 O* p
6 |5 ^$ Q1 c, e# i; G/ }
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯
4 o: D- O8 a2 F+ [
5 v- X' d( I/ e) g, ^您好,不知是否能向您請教。0 ]+ n' O# K+ a: r* K: w3 I+ T, P
目前和宋修賢老師在處理Ardui Car
0 P  Z# x; T. j. b8 y- [雖然已使用較繁雜的方式處理了跑出黑線外的狀況" K) n6 l7 ^. ^6 E# L# O2 @
3 ~6 p' ^& [7 \8 T/ M
但基於想追求更精簡的程式所以還是想請問一下
) b& E$ ]7 o9 @' ]) w# B( G就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝  R) t  T. _3 _; ~9 C# D
不知道您是否願意教我可以如何處理' k. d( J) K3 u

/ s0 D0 _8 ]/ {# [% n$ ^
; c( ], v2 \+ r; L& p9 J# \0 l4 b9 i以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;4 Z, B4 b/ O" A& Z7 d) a
  2. double CNY70Val = 1000;, u% b! y6 [! j% t& l
  3. int interror = 0;- u1 ~5 i% ?& D
  4. int olderror = 0;1 G9 n0 \& n) g; N
  5. double values;
    3 D, m! ~: F* T& j
  6. 3 Z& Q5 L! A  ~" {
  7. + E+ a, m3 {' u
  8. void CNY70()2 x: y! W% G. E2 e( ^
  9. {  o/ W, k9 w, b1 ^. m: s
  10.   valuesRR = analogRead(RR)
    4 n. D& z* N3 l* I
  11.   valuesMR = analogRead(MR);
    , V1 y5 ~  o) g
  12.   valuesMM = analogRead(MM);
    + G6 {! E' L, i) p; L" N: S! ^
  13.   valuesML = analogRead(ML);
    # A4 a% J, ^. }7 M" M9 C  i
  14.   valuesLL = analogRead(LL);
    $ Y2 ^1 {" P: z

  15. : K. \1 i7 i) B: m. j2 `. x: K# L8 o" H. U
  16.   if (valuesRR > CNY70Val); _% n, z+ C+ e3 M8 d) T0 r; A
  17.     valuesRR = CNY70Val;
    ( L; q: f/ y8 n; j1 F1 P
  18.   if (valuesMR > CNY70Val)* R, |3 l: I' U, a
  19.     valuesMR = CNY70Val;
    , @; x! I$ L: @5 d5 b
  20.   if (valuesMM > CNY70Val)
    9 W. Q, g2 M2 f) p5 N  V' N5 b
  21.     valuesMM = CNY70Val;8 o$ c# s- b" w0 i$ P
  22.   if (valuesML > CNY70Val)
    . J) r! s1 ^# Q
  23.     valuesML = CNY70Val;7 h) U0 w- o. k. H3 i  O
  24.   if (valuesLL > CNY70Val): M( c8 j; Y6 v8 H' V
  25.     valuesLL = CNY70Val;0 @$ n4 t. O6 I
  26. ; D" a+ J3 c: V" A
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    ) A- q- b/ J: S: r! ]. _9 h
  28. }  o' |' e  g1 X

  29. ( _8 h7 u! r! s- d4 X) H" R: U
  30. void Car()
    , m& ~4 G1 C! _$ }7 ?
  31. {! Z& j  I9 U* o) p1 |2 R
  32.   while (1) {
    4 g) \0 |: L3 Q' S* H
  33.     CNY70();
    7 h5 [- ^; l8 P0 ^7 \, s

  34. 2 T' M1 H+ S6 Z. S( t4 X
  35.     int error = ((int)values);1 l1 Y% q& E. p9 |! Q+ _- p, A
  36.     interror += error;
    - f$ _) K! y$ W. l$ T. v# K
  37.     int lasterror = error - olderror;
    2 t/ e$ w$ H; a; A
  38.     olderror = error;
    5 B, M. o6 m2 A+ Y, r; V
  39.     int power = error / 5 + interror / 10000 + lasterror;
    ; E$ _2 T, r! v& [

  40. % }, O/ U2 f  }* \
  41.     if (power > MotoSpeed)0 Q! N' X! d) m
  42.       power = MotoSpeed;: ?) I2 B  q1 {! ]$ C$ r- I
  43.     if (power < -MotoSpeed)
    " D! h) B  _. p& X
  44.       power = -MotoSpeed;
    * u/ q8 m7 |! t/ A% O
  45. 9 S4 p; s5 g, X4 G" K
  46.     if (power > 0)3 V4 n! F" }9 \; h& ]  s0 j( I$ Q
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。8 U6 `! O2 ]* I) l
  48.     else
    4 X( i: R* v. _$ H( Z! }
  49.       Speed(MotoSpeed + power, MotoSpeed);8 X# u/ i) n, h" b9 D
  50.   }+ \) B+ f8 I3 v3 P8 ?
  51. }
複製代碼
2 t2 c2 I) C: T+ f8 i) Y' b- t
) b. S9 ?7 A, V1 @) k
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-11-21 19:13 , Processed in 0.025459 second(s), 17 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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