圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21858|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>$ y9 V5 w7 ]" W
  2. #include <Wire.h>
    ' S! J, t: `# I$ y. u1 c4 x
  3. #include <Servo.h>0 n% A& q3 Z5 ?; g* }

  4. / f3 E6 R- l: O# a- o
  5. #include "MePort.h"; M' u  |+ h+ h0 t% s  Q9 N
  6. #include "MeUltrasonic.h"* O, S9 \1 o  u$ I
  7. #include "MeDCMotor.h"' K7 T  f& |# I% W; f

  8. 4 z$ n  Y; `3 ?6 C
  9. //double Input, Output ;
    $ j7 x, t( E3 f% e
  10. float MaxSpeed = 255;
    6 J3 k6 U$ r; C/ V
  11. float MaxPower = 180;
    ) b  p+ X: f- J; k. c
  12. float MinPower = 120;/ k% f$ D. D5 W. K; @
  13. float Error,ErrorAcc,ErrorDec;* ~5 O0 A( x0 m  U

  14. ; ^1 o$ {0 M1 W' q( I
  15. float Kp=0.14;! F, M0 f0 @2 t' \  ~, z8 K
  16. float Kd=0.00020;//23;
    ( D& c% z5 b( I' ^
  17. float Ki=0.000201;# ?; E4 A( p8 ~2 Q8 b* H
  18. ( G1 v4 n+ |7 O, P; D' @# m! U
  19. float nPower;/ X0 D- H$ \5 S% q; ?# u2 T
  20. MePort lightsensor_6(6);
    . m  N1 X' f: n  D2 G- b
  21. MePort lightsensor_8(8);8 t" {5 s4 w" ^3 Y& q9 E& y
  22. MeDCMotor motor_9(9);0 i: r. h( U# u" u& k$ q" r
  23. MeDCMotor motor_10(10);8 `3 _- H: j6 Y. c; B; \2 Y$ E
  24. unsigned long previousMillis = 0;' N, Y$ N6 {1 i+ |
  25. const long interval = 1;2 a0 e- n; N2 g5 y8 p0 o

  26. 6 _9 }$ y1 e& K
  27. void setup(){
    / N9 N4 _0 a9 v# L- q
  28.     lightsensor_6.dWrite1(1);
    5 t3 S1 X( U, @7 E- j6 B/ r8 e4 R
  29.     nPower = 160;5 F  C. f' S* W; _
  30.     Error=0;
    " B, x& K4 D) S3 P, S0 W
  31.     ErrorAcc=0;  Y  V, J$ J! Q7 ~# t& L
  32. }9 F; b7 A& \& S% l5 V" r. v  H

  33. $ |- ^9 L6 c) |' D% @! |
  34. void loop(){
    ) f( p# U+ p* _  j" a: E
  35.   unsigned long StartTime = millis();# L" A$ v& t- Z4 ]2 l7 \
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    & q9 ]7 x8 w4 [' n: V2 {
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    ) y/ k' V/ t3 a' u
  38.     ErrorAcc +=  nError*Kd ;" j1 v  [7 {7 e  @$ e$ s) l* n
  39.     ErrorDec -=  nError*Ki ;) F+ c, r: l- Y. }: S
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    ; t, B" m6 R) ^4 @* A
  41.     if(nError < 80 && nError > -80){
    ! [7 R- L1 v/ a9 ^
  42.       if(nPower < MaxPower){
    9 U  m) R, g0 G0 a1 ^4 Q' {
  43.         nPower += 3;6 i/ V$ Q# y$ Q  I
  44.       }
    * V0 k/ X, V( x( W% i2 k4 \2 |" b
  45.     } else{
    - A- P1 W$ B* s" A
  46.       if(nPower > MinPower){
    " C1 A) V2 h# l, [) Z' D- p& n
  47.         nPower -= 2;
    ! J) D) a6 E8 y6 c4 X
  48.       }. j) B7 ^9 \* p4 {3 h* o0 b; [+ t
  49.     } 2 W- u! Z# i* E0 g  H
  50.     MotoL(nPower-Error);
    7 [* U1 n' R; l2 `( v
  51.     MotoR(nPower+Error);    $ Q+ W. |; c- p! _5 S& _
  52.   }else{7 r0 y& ], ^2 O. u. n% T
  53.     motor_9.run(0);
    % c/ |4 n+ ]8 v; e$ ~
  54.     motor_10.run(0);
    8 N: Y! y8 H  \. I( {, P/ i
  55.   }
    9 _+ y- F5 M# V3 J
  56.   do{}while(millis() - StartTime < interval);  b( ^$ o& u/ [( L6 g4 l- F
  57. }
    & c& t" l$ P& }5 k  P9 G

  58. & f- e$ y4 Q6 _* E( F- P/ R
  59. void MotoL(int Power){
    3 M1 O' E8 _# X! h- g7 p
  60.   if (Power > MaxSpeed){
    ) f* S. o# ?+ ^: Q2 G6 d! V+ f' P
  61.      Power = MaxSpeed;
    / z6 ?* N- A4 D9 n( f4 o3 E: t% f
  62.   }
    ) _9 Q! I0 n9 A
  63.   if (Power < -MaxSpeed){
    & n  i8 k; d+ o$ p' o/ y
  64.      Power = -MaxSpeed;
    3 z$ a8 O9 `1 y
  65.   }
    5 m" |5 z; g5 }0 E
  66.   motor_9.run(Power);
    ! b1 I4 q$ T# L5 m
  67. }  
    * P* l" w  V* h7 J

  68. % c( X/ |/ g9 i' D/ E$ B6 l; t
  69. void MotoR(int Power){3 A5 X% Q" V1 N* H& }
  70.   if (Power > MaxSpeed){
    0 H8 j2 K9 S! R0 j4 T! V0 |
  71.      Power = MaxSpeed;0 U. e7 E% H: {
  72.   }
    7 r- y4 i/ Q3 O: H( X* w$ q% G
  73.   if (Power < -MaxSpeed){+ |  [, ^, s. F0 @8 _
  74.      Power = -MaxSpeed;# f6 E- |& s4 D
  75.   } + D1 v$ ]& q9 X9 H( |: F# G+ R0 |
  76.   motor_10.run(Power);
    & q# [9 A2 t) X$ ?7 a2 I
  77. }  
複製代碼
& l* v8 Y) O  ]& ~" W
; F1 B1 Z" ]  I* s4 T! G2 H! R# v
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯 * u8 }' n; A! P5 S

( U# p4 l, C4 M, Z您好,不知是否能向您請教。
! d2 c! J1 {9 ~, J8 k( Q目前和宋修賢老師在處理Ardui Car
) [) p. \3 W2 J% I雖然已使用較繁雜的方式處理了跑出黑線外的狀況
* S: l9 r$ v5 Z; _: ^8 u, I/ c7 e4 O' a" T% n3 Q% I, i
但基於想追求更精簡的程式所以還是想請問一下: w  N* ]2 Q' g- ?" G) d
就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝, W; y2 N% f6 p8 l$ D; F1 I
不知道您是否願意教我可以如何處理
  F+ i# G4 a- r5 J/ @4 o
) H* _; J& H' q
1 _) R) i: \& ]) m4 i以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;
    5 L) s0 g7 t" K2 Q6 y$ |, e; c+ \; F
  2. double CNY70Val = 1000;
    + n# i9 w. C3 q5 F) ~
  3. int interror = 0;3 c% H" r/ [( l" }* a! m
  4. int olderror = 0;
    ! \# Q3 C. ~4 ?" I- `
  5. double values;& O1 Q5 }9 S' B' B7 P* m( x* s) t

  6. . P+ u' h, O( t; r4 P( z- R
  7. ) d- i1 i. [# E- R' ~
  8. void CNY70()
    ! a- Y0 S3 d; m
  9. {
    + Y8 m% ]9 Z# ~/ e* w4 y; ~8 G3 X3 e! j
  10.   valuesRR = analogRead(RR)
    9 j* ^% _8 w" d1 n! F% e
  11.   valuesMR = analogRead(MR);
    . j6 r" b- u$ R, m. k
  12.   valuesMM = analogRead(MM);
    / r' d3 ?8 Q% I2 m! F; l6 \3 ~, m
  13.   valuesML = analogRead(ML);
    5 T2 Q0 m0 o( N, J
  14.   valuesLL = analogRead(LL);
    1 G. v! b# [3 S  D: V# t2 u) R
  15. & ^- p# N, @4 M4 G) `" z
  16.   if (valuesRR > CNY70Val)2 Q/ B; W2 {# ~2 s4 l' V
  17.     valuesRR = CNY70Val;& G# n' F. |6 V6 }
  18.   if (valuesMR > CNY70Val)) i! e: ^- r+ Q, {6 j2 q. z; `
  19.     valuesMR = CNY70Val;
    * N. ^- k9 I) q& m
  20.   if (valuesMM > CNY70Val)" A: Y; u# a0 ?" d6 c: t* w
  21.     valuesMM = CNY70Val;
      [( P( e* b9 N: x
  22.   if (valuesML > CNY70Val)' D3 \5 R9 N* I8 O+ X; y& p' h) x
  23.     valuesML = CNY70Val;7 M. K  }) I+ o, w
  24.   if (valuesLL > CNY70Val)
    . h% ?- s# o9 o9 Q! N: G
  25.     valuesLL = CNY70Val;
    4 P2 S5 t: D! d9 Y
  26. - Y+ o# P& r9 @# P5 s
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    * Q" W" b2 Y8 R9 @! I
  28. }
    7 p/ @& ^7 H' X/ m; E9 I& o
  29. $ A3 J2 n' H* x7 u5 N. F
  30. void Car()3 T3 C; u8 c' m* w
  31. {
    5 J  S$ Z$ l+ j4 n, T4 z# R
  32.   while (1) {
    1 \) e6 ?7 i  `
  33.     CNY70();4 j: P& G# ]# C/ o1 t- D  z
  34. + B" C! J7 p8 r2 J- T  y: J
  35.     int error = ((int)values);3 c8 `& C  }& K5 X" h6 |
  36.     interror += error;
    4 E! d& w1 M1 f( A5 m% r% g8 r
  37.     int lasterror = error - olderror;
    , j5 r) W% a- \1 Y! p
  38.     olderror = error;
    ( q" K* P6 D- ~- c6 U
  39.     int power = error / 5 + interror / 10000 + lasterror;
    ( i* }* V7 N; |) d+ {+ o' }2 M% s
  40. $ l7 c5 E3 g2 q/ o. X, v$ R6 k8 ]
  41.     if (power > MotoSpeed)' n( r2 C4 b4 U2 ]4 C( n7 ^" A7 B8 \
  42.       power = MotoSpeed;! v& q0 h6 r% X8 M3 v, U3 L* f
  43.     if (power < -MotoSpeed)
    5 Y3 B1 \) {5 u. b- m
  44.       power = -MotoSpeed;9 v: Z& Q( `. U* v
  45. : x& u0 Q7 A: W: t
  46.     if (power > 0)
    2 j2 e' j* ^/ S- L3 G
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。2 H* s% A) G( t$ N1 D1 S
  48.     else1 F" U5 l8 D' R3 \" `9 `
  49.       Speed(MotoSpeed + power, MotoSpeed);$ i4 p7 I# q/ H- }, q6 l
  50.   }- |, K2 x) L3 \. m+ p" q5 g- X9 P  H
  51. }
複製代碼
) I( Z  R. V. y
# B: L. G; @+ Y5 E5 c
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-12-6 04:56 , Processed in 0.025571 second(s), 17 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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