查看: 23329|回覆: 3

PID 循跡自走車範例

 薦 [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>, }/ [$ `: ]9 ?. V) t
  2. #include <Wire.h>
    % y& }; {% n4 X/ h
  3. #include <Servo.h>
    ! m/ H* Q7 {5 l$ h
  4. 1 i) b( W) r" k) b- W6 N
  5. #include "MePort.h"! N0 G( e5 m) q# ]' C+ L
  6. #include "MeUltrasonic.h"
    / y$ N; Y1 i4 M% R& Q
  7. #include "MeDCMotor.h"1 H. L& @" i$ W+ i+ H2 L
  8. , S& X5 A) T, [8 \# A' W
  9. //double Input, Output ;7 \6 h% O7 t9 m; {
  10. float MaxSpeed = 255;
    ) q, R4 T! Z1 I) {3 |3 ?
  11. float MaxPower = 180;8 O/ B8 R  M" O7 p5 X
  12. float MinPower = 120;, o% f" I9 w3 v4 P; h7 g1 D
  13. float Error,ErrorAcc,ErrorDec;7 Y4 b+ O9 z: \" H6 N; ]/ Q. x

  14. 6 a) S' j6 F0 m" z. ^! a
  15. float Kp=0.14;
    * V# U6 i4 |2 `
  16. float Kd=0.00020;//23;
    : y% r4 r, r- @7 F
  17. float Ki=0.000201;
      z/ b$ H* v; y% d5 {' N9 m
  18. * j9 H9 Y- b: A
  19. float nPower;
    ( h0 \' }- {0 s2 l, P( G* S
  20. MePort lightsensor_6(6);
    ( L. q5 r3 t1 V0 G4 q7 \' [4 V
  21. MePort lightsensor_8(8);) w% y/ N4 U' o% d. Z8 D
  22. MeDCMotor motor_9(9);
    5 i( q: I, _- }3 S( [1 f* z
  23. MeDCMotor motor_10(10);
    : p/ p$ Y: Q+ k2 ]+ B
  24. unsigned long previousMillis = 0;
    7 f3 B, g# T" g# S1 a2 d, x
  25. const long interval = 1;2 v% b0 [1 U9 l" g

  26. 3 {8 @5 v, X$ y( l; ]4 F
  27. void setup(){; Z1 Q' v" t% O8 ]$ `2 q
  28.     lightsensor_6.dWrite1(1);7 s. @5 z, j3 U# Q0 b, F* i
  29.     nPower = 160;) @% m3 [  k1 R3 B
  30.     Error=0;- a- Z! e/ X- s" S% o$ V
  31.     ErrorAcc=0;' R- b3 P1 O+ r
  32. }
    7 h2 \% A5 u2 ?1 q: {6 D# H

  33. * e3 a4 X7 ?2 _- C0 D6 ]9 v$ k" D
  34. void loop(){
    , J5 C+ D  E) O7 S9 |( J6 t
  35.   unsigned long StartTime = millis();
      ^* ]4 W( G. Q1 Y, h% E$ n
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
      U6 H: T1 Y* B8 R/ Q8 Q$ C- z0 Y( |7 T
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();. w" R9 e) W# M6 V- `4 v
  38.     ErrorAcc +=  nError*Kd ;
    % \5 G2 |1 J( |* g: J! d& ^
  39.     ErrorDec -=  nError*Ki ;1 e0 t8 p/ J8 V. y' w) ^
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);3 J- q9 I" g* S) i; A: q& r* z
  41.     if(nError < 80 && nError > -80){
    3 O3 i" ?$ L  n7 r
  42.       if(nPower < MaxPower){
    " K0 N9 C% N8 C, r
  43.         nPower += 3;
    " N* S+ v* J% l4 @/ Y
  44.       }: l6 f0 l* w4 d  F% b9 X1 F% K- X
  45.     } else{+ b# p, h. a" R/ h8 d5 w6 E; E
  46.       if(nPower > MinPower){& _3 z* t5 N' [% @, [
  47.         nPower -= 2;: {. L$ H" J' s' j7 R
  48.       }
    & N3 j$ d6 |+ |
  49.     } ) I/ v+ Y0 n# }, [/ f. `% D
  50.     MotoL(nPower-Error);6 l& L- x3 I% M
  51.     MotoR(nPower+Error);   
    3 N+ ]* ~2 X2 E
  52.   }else{$ o, b, t6 E) Y$ i
  53.     motor_9.run(0);
    9 w: D+ h1 r* O7 W' @* Z" C
  54.     motor_10.run(0);
    / T7 S1 R) E0 p& c
  55.   }
    & i" R- [- U/ x8 U* |5 w
  56.   do{}while(millis() - StartTime < interval);
    5 l3 t! V1 O: N% Y
  57. }
    4 K7 q4 c1 C- v# R
  58. . J5 z' R3 b7 q' q( X" U3 Q) ]
  59. void MotoL(int Power){4 m4 g% E5 L: q+ A% r
  60.   if (Power > MaxSpeed){
    : k% t2 R( g+ N3 {# l2 `! [
  61.      Power = MaxSpeed;
    1 p" j2 N5 Q* s6 h+ _4 `' R3 i) h
  62.   } & M# L/ s# p. z- @# s0 F, B
  63.   if (Power < -MaxSpeed){' ~0 \' p2 X0 |+ R/ p& u) W
  64.      Power = -MaxSpeed;
    , P' l4 H1 r5 l0 |* R. Y" s
  65.   } ! A. W2 a6 s* E
  66.   motor_9.run(Power);
    1 p/ z, F& ]3 U) O
  67. }  
    # o. i( t  p, A% y1 j

  68. 6 V# C- B& B3 Y, B. L9 W0 H
  69. void MotoR(int Power){6 I$ I- T- A4 p% s$ n+ K
  70.   if (Power > MaxSpeed){
    / D6 K3 o2 N7 j* {; r
  71.      Power = MaxSpeed;
      ]7 v: `3 j* q
  72.   }
    ' q8 l6 D5 k* z- h9 B. h
  73.   if (Power < -MaxSpeed){
    2 L* M  e/ s$ ~# f% _4 D: h
  74.      Power = -MaxSpeed;$ N! ]- A% v1 w
  75.   } 8 |( z( m: z- a) S
  76.   motor_10.run(Power);
    6 m8 k" f9 U7 m  p# F- t
  77. }  
複製代碼
2 ~% _- P; B5 W6 r
  O, d! S9 K1 E' M1 X/ d
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯 # {, `$ `2 N* ~  r/ @8 p0 ~6 q

. K- z2 `+ f7 ?! o0 Y. [; }您好,不知是否能向您請教。
" o8 e0 Q3 @" C( U+ n) B$ Z目前和宋修賢老師在處理Ardui Car$ w5 Y5 |. e6 u, F2 G8 ^) B# ]
雖然已使用較繁雜的方式處理了跑出黑線外的狀況  W# Q8 N& Q! a4 z% p, ?6 u

1 D* G' Q( t2 Q8 ^" x! f但基於想追求更精簡的程式所以還是想請問一下* x' Z% [* ]7 V- x7 I
就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝
0 n' j* P' }' V* N& b, T不知道您是否願意教我可以如何處理- c* h. T$ f; w( a& c9 J" c7 S2 N$ s

7 i! m; D6 J; @# l  g
- S" G* z- {* C以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;
    . O1 \' o1 {9 k9 |7 a- y4 g2 s! }
  2. double CNY70Val = 1000;
    " M) H. l* Y- F
  3. int interror = 0;/ x5 X+ W! [+ o+ u8 [8 b9 H, M; U
  4. int olderror = 0;3 s' w7 |3 d" H# E! u$ ~+ }
  5. double values;
    4 i+ d! u* o6 A

  6. & n$ A" z, O) x7 \- m6 r
  7. * b% \7 q9 [0 ]+ q: F
  8. void CNY70()
    1 |3 y* x1 s8 r# i  J9 C
  9. {1 B  q8 k0 ?$ @  [; D
  10.   valuesRR = analogRead(RR)3 l3 N( g9 i% [" h8 j/ `0 ^
  11.   valuesMR = analogRead(MR);' a- `% Q$ Y8 q
  12.   valuesMM = analogRead(MM);' V9 o7 b6 B" s0 r/ E8 g
  13.   valuesML = analogRead(ML);
    / u. S- D) E5 T2 S+ c6 F
  14.   valuesLL = analogRead(LL);( i& C; I* ^# ^8 X3 z

  15. & a) x1 {- h5 T% C% [: ^
  16.   if (valuesRR > CNY70Val)) J9 P! u" o0 U6 S! @
  17.     valuesRR = CNY70Val;% u& b! p; J) b7 r2 h+ s; z
  18.   if (valuesMR > CNY70Val)+ Z2 B6 V- N3 k% n6 \/ G  s" @
  19.     valuesMR = CNY70Val;
      Y% V1 A2 H$ `6 g) ?
  20.   if (valuesMM > CNY70Val), ]$ N, {2 U4 I4 G- W! P2 F
  21.     valuesMM = CNY70Val;7 N$ [7 F% ^6 C, Q2 d5 h+ A
  22.   if (valuesML > CNY70Val), q- T! c! Z- _3 ?( N( V8 A. i/ W
  23.     valuesML = CNY70Val;' r6 k* m5 K3 Z7 l- V8 T
  24.   if (valuesLL > CNY70Val)& O3 @% k! F8 ~4 t" t# T# T9 n
  25.     valuesLL = CNY70Val;' f7 F7 [$ q! f

  26. 0 C) B$ Q* c% s/ o4 j
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    4 _5 ]1 @. R7 H
  28. }5 M9 ?: b5 b& G* N; V0 Y
  29. 5 W" h* D* R  e9 c& H6 {/ U1 ]
  30. void Car()0 B+ W. Q" F6 ]! P4 W0 W+ r, t' J
  31. {+ y* B" F! w- @' n! |
  32.   while (1) {: t/ s5 S: d7 k. A# |
  33.     CNY70();: ~% M) }& }3 ^3 M6 G
  34. 7 ^! L& h, J- k" ~, v
  35.     int error = ((int)values);; H* M) [  X4 v  l
  36.     interror += error;
    4 B* s% P0 U- q/ a
  37.     int lasterror = error - olderror;
    ' w4 v4 ?4 ~7 U0 i
  38.     olderror = error;
    3 `( O9 e9 w' ]0 A1 S5 W. o
  39.     int power = error / 5 + interror / 10000 + lasterror;
    ( K" X7 \4 b& z" e+ T+ K+ {
  40. 9 J8 \+ N, E' _* B
  41.     if (power > MotoSpeed)( z/ t% H1 r* @: a1 s
  42.       power = MotoSpeed;
    . u6 M" t) @' m1 }1 N, y' S+ i
  43.     if (power < -MotoSpeed). ^) a+ q8 V* ?9 {2 Z/ Y
  44.       power = -MotoSpeed;7 A1 F. j3 A- \* i) ?5 z3 z0 ^
  45. 8 @% b& F' Y1 F! T/ z3 u" o7 m6 v. q
  46.     if (power > 0)! b/ Y( b. e7 R$ c7 n! j
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。- p  y) l. M' }8 O4 K
  48.     else7 C/ b) H. S% r, `
  49.       Speed(MotoSpeed + power, MotoSpeed);: x' e% ?7 y9 b5 s  U
  50.   }
    4 C3 r$ j! X: X$ B2 Y9 a
  51. }
複製代碼

" E. a7 g# [' J5 N
" X1 B' S2 f, l$ J, M
您需要登入後纔可以回帖 登入 | 立即註冊

本版積分規則

Powered by Discuz! X5.0 © 2001-2026 Discuz! Team.

在本版發帖返回頂部