圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21828|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>; h! F" _! @( Y* F
  2. #include <Wire.h>
    7 h+ A' r; H5 Y7 j6 L( v9 }6 P
  3. #include <Servo.h>5 m/ I. c! u. |! S6 u" |

  4. - y: L2 u$ v4 O* G4 o& k4 q
  5. #include "MePort.h"
    ! e, ^' _" d: q9 H
  6. #include "MeUltrasonic.h"( ]5 Y( a0 o4 l- v( [! d. N
  7. #include "MeDCMotor.h"
    ( H0 L7 j" A  q2 A2 D6 q# ]7 l
  8. ( l9 P( X% U+ P8 {
  9. //double Input, Output ;
    : \& N6 ?. ~" K: \; z* k% j3 k, s- a
  10. float MaxSpeed = 255;
    , R' \+ Y' _2 a
  11. float MaxPower = 180;
    $ A7 ~& @+ N- N: O
  12. float MinPower = 120;) Z+ m6 B: [/ m
  13. float Error,ErrorAcc,ErrorDec;7 s5 j" I/ v1 S. U

  14. 1 h; f- L* R4 a  X0 R5 @6 m: N6 s+ O
  15. float Kp=0.14;6 C, A0 |. l7 p* n
  16. float Kd=0.00020;//23;7 N% Y, k6 V  A. r- _
  17. float Ki=0.000201;
    9 i/ f, Z6 @/ R: o7 ~

  18. 0 \2 _7 i; M7 q) D4 c" X
  19. float nPower;8 e9 s0 o  @, m1 h! U6 j
  20. MePort lightsensor_6(6);
    9 f" h+ y4 [4 y1 F
  21. MePort lightsensor_8(8);
    . I6 I6 A) l. Z2 V
  22. MeDCMotor motor_9(9);- f3 u3 H/ n/ M7 }
  23. MeDCMotor motor_10(10);9 A  U/ a  ]5 ]; r
  24. unsigned long previousMillis = 0;
    . w9 E# T1 W' h' Z0 d( U- f! v# u: I
  25. const long interval = 1;" ~/ d, F) F& b4 e* K7 J
  26. 5 H% B8 B4 C4 @9 @
  27. void setup(){9 s$ |- F9 e7 I
  28.     lightsensor_6.dWrite1(1);
    2 c4 p$ B& t1 Q. B
  29.     nPower = 160;& Z) n+ M  p, Q8 U& y8 j. E, v3 G
  30.     Error=0;
    0 F" q1 `2 Y' G+ B$ l' D
  31.     ErrorAcc=0;
    7 G/ X9 g4 ^6 `! ~
  32. }* L0 y) ?' Q! N: E; X
  33. ; s2 H1 Q% U$ L% P4 ~- f; d# w0 r
  34. void loop(){1 F" }7 D& [* {
  35.   unsigned long StartTime = millis();' q6 ?& _3 ]3 Z
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    7 e* W  V7 ?. G  {9 a5 P' n
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();9 s# s+ n3 W5 ?: e- V
  38.     ErrorAcc +=  nError*Kd ;
      a9 n  R5 k0 H( L, S. L
  39.     ErrorDec -=  nError*Ki ;) f" p9 R, ]% ~' W9 G+ J- W
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);9 a8 c& s; ^; E& D: G/ g) E) ]
  41.     if(nError < 80 && nError > -80){
    : Q7 `8 t5 {4 @7 }5 Q
  42.       if(nPower < MaxPower){( F1 z/ `( y7 Q5 ^' k
  43.         nPower += 3;
    & Z! T! B. p1 k
  44.       }
    - G+ k2 ]4 |, z7 j  J4 H$ X
  45.     } else{# \8 q/ T  B, u
  46.       if(nPower > MinPower){: {+ {$ O4 r! M# W$ C3 p
  47.         nPower -= 2;/ c) W) W# _! ?. l" {
  48.       }
    . n2 j/ ^, B9 l) ?
  49.     } ; ~/ f) Z0 U  n& p# o. Z1 Y% P
  50.     MotoL(nPower-Error);7 Z5 `' s  D$ }7 J. R7 n
  51.     MotoR(nPower+Error);    % Y( R6 y- T5 R) z
  52.   }else{
    2 H2 Y  {/ q% u. ~
  53.     motor_9.run(0);
    ) d2 u- h. }2 C/ G9 `
  54.     motor_10.run(0);
    - A8 d! O* N$ A$ [* _2 Y( {
  55.   }5 h4 r; W0 f, i
  56.   do{}while(millis() - StartTime < interval);( n* s- h8 a+ V# {% \
  57. }
      r, i& I- ]: k( A& d, H' i* O4 ]
  58. - P1 r4 K5 t7 b$ M- B
  59. void MotoL(int Power){: }& w3 D7 ?! A7 o! t5 l, s
  60.   if (Power > MaxSpeed){
    - q$ ?* }( h( C! D" \6 s$ K5 T
  61.      Power = MaxSpeed;
    # Z! O  |( b. J6 n( N- f
  62.   }
    1 N7 N7 p9 P7 X' v; T9 J
  63.   if (Power < -MaxSpeed){+ `# U" _& Z8 N' {9 [
  64.      Power = -MaxSpeed;
    : k$ ]$ j: c& [/ K  c
  65.   }
      H4 t5 ~+ D) i
  66.   motor_9.run(Power);3 n1 e9 h3 x4 i( v( `$ I
  67. }  
    0 u& L; c# x  p; Q" U7 w
  68. 4 h- V; R6 L# t' E; c2 q+ Z
  69. void MotoR(int Power){! W* W, \) q" c2 @  ?# P
  70.   if (Power > MaxSpeed){
    0 v  Z% w! ^  G2 ^4 \- F8 K* W' V
  71.      Power = MaxSpeed;; A+ y5 ~( @7 u8 s3 n
  72.   }
    4 u9 I5 N/ m; S& G; l7 N/ J! u
  73.   if (Power < -MaxSpeed){! N  O/ K/ a! }& {
  74.      Power = -MaxSpeed;/ k, B0 S" e* O! e7 a3 d9 e
  75.   } 4 y: x% }0 m4 s" p! B/ a4 s& f
  76.   motor_10.run(Power);
    9 n. n2 \0 m8 ]" W
  77. }  
複製代碼

) U( j7 u4 K$ ^3 L. ^, @- c. w% w. K! ^2 ~; e! y8 ]( m. R) i$ M8 \
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯 4 s4 C- x3 T8 F" B* W4 P: u! {
- s1 M# b5 t9 l9 C+ P  ]) ^5 X
您好,不知是否能向您請教。& A4 a% `! N) y" n4 a  V$ f
目前和宋修賢老師在處理Ardui Car
0 S6 B$ ]3 y$ P" b! }) I) y雖然已使用較繁雜的方式處理了跑出黑線外的狀況
: W+ F4 B$ r# H/ g# f5 O- r( m
4 Z3 t0 [$ Z" u0 \* N4 L但基於想追求更精簡的程式所以還是想請問一下
' F7 l5 K( y; j! N( u就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝
" G/ A0 B1 m0 f0 s& t5 E; l不知道您是否願意教我可以如何處理
: S4 E5 _% Y% h7 k& _: r8 Z4 F0 c
3 n6 V. c2 V% g9 j" X4 r; ~/ f( \$ |% v$ R9 `  `
以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;% f, F: T2 F4 X8 m) S) c8 V* b
  2. double CNY70Val = 1000;
    + Z, U, l( F# h4 N* B5 |
  3. int interror = 0;4 u0 F- J4 S$ u3 m. t4 z0 P
  4. int olderror = 0;
    4 s# R+ p- [6 q( M$ O4 w
  5. double values;
    * V/ @" v: ^6 e0 H
  6. 0 f! ~# D3 W3 [( w4 S2 g3 `+ `' h

  7. 3 R5 Q, I1 a2 U) t& z
  8. void CNY70()
    5 D$ I* i' v& S' [, y. d
  9. {
    5 l) A% W7 H5 `( T: z9 a7 f' G
  10.   valuesRR = analogRead(RR)
    : H- X6 T! H3 e5 E2 g
  11.   valuesMR = analogRead(MR);5 \( N, b% l, u2 C9 y  h* T) h
  12.   valuesMM = analogRead(MM);: ^# c; i" C: C7 D
  13.   valuesML = analogRead(ML);
    9 u1 d1 l8 D3 l9 P4 v- k
  14.   valuesLL = analogRead(LL);7 [- R! m7 K$ H

  15. ) A' r7 n# w; s/ p$ h4 _& Q4 v
  16.   if (valuesRR > CNY70Val)
    ' z0 ^+ n! p3 M
  17.     valuesRR = CNY70Val;
    ; ]4 o  |2 X/ W/ _: r7 B" O
  18.   if (valuesMR > CNY70Val)' n; O; D) p/ m# R' H5 [3 U7 v
  19.     valuesMR = CNY70Val;. \' M0 Q# ^( \
  20.   if (valuesMM > CNY70Val)/ ?+ @4 j- t5 X1 g( @* r+ ~
  21.     valuesMM = CNY70Val;8 y( ~" u: U% _# {8 V. y0 L
  22.   if (valuesML > CNY70Val)" _& i7 Q- Y& b& I
  23.     valuesML = CNY70Val;
    + q6 I" k/ A5 N$ i4 O# l  U) k8 z
  24.   if (valuesLL > CNY70Val)
    , u! h) @5 Y' Q9 {
  25.     valuesLL = CNY70Val;6 J. @. F8 F( _$ E0 S
  26. % R9 p5 D& ~( o7 R
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;- R, L  a. H$ K; i3 E1 [
  28. }/ c& ^; W. _* g0 i. ]4 V$ W

  29. 2 ]0 [9 E! _) U
  30. void Car()+ O$ w- E- g7 a
  31. {
    * a1 I# b! F: `
  32.   while (1) {
    5 [( V/ F/ o/ }9 v/ e* Y3 D
  33.     CNY70();6 W7 m9 d# i( L  a4 V4 e' a

  34. 2 l0 T1 i3 x" b5 l
  35.     int error = ((int)values);
    ' |1 J9 v5 H0 d* {: a/ @
  36.     interror += error;
    3 _& A0 R  t' b
  37.     int lasterror = error - olderror;
    ! u0 ?, Q9 v' Y+ b# u9 {
  38.     olderror = error;
    5 o, g, e! w: A" r) T' S
  39.     int power = error / 5 + interror / 10000 + lasterror;
    ) n' R9 C0 j( |9 u

  40. 4 m2 m6 j2 _, d' z
  41.     if (power > MotoSpeed)" Y0 G. G5 ?7 P. P2 v3 }  S3 W
  42.       power = MotoSpeed;3 E/ W; C: P2 r3 L# N5 J& [9 V5 K
  43.     if (power < -MotoSpeed)
    $ X- C! r) L; a, v; B* d
  44.       power = -MotoSpeed;( u7 k1 I+ e. N5 }
  45. + @$ j2 F; |4 Y9 A7 H
  46.     if (power > 0)
    8 H7 B1 Y; }( S# [. q; u. A1 E
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。
    4 b0 t: ^. A$ P! o
  48.     else
      O( c; M. @. H' Y# p2 {
  49.       Speed(MotoSpeed + power, MotoSpeed);. E# X6 w! ~  R( Q
  50.   }
    . q1 [7 ^& ^9 h3 e& w
  51. }
複製代碼
! k( E; i. K4 F- T/ f6 H
) S4 T4 C# K; ~# o7 U
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-12-4 08:05 , Processed in 0.026268 second(s), 17 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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