圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21782|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>
    8 {) P6 w3 N* K8 I* {
  2. #include <Wire.h>
    4 N3 p% b& L, T; Y; S
  3. #include <Servo.h>
    . v' l! c2 S" a  h- b% B1 Z* @
  4. / p- ~- b  I! h4 Z/ M: }. r
  5. #include "MePort.h"4 ]4 O1 U: S) N7 k2 c; _! w
  6. #include "MeUltrasonic.h"( P) z: x* R4 h1 O# w" w& h1 }9 \
  7. #include "MeDCMotor.h"6 Z- t. A* y5 A7 M) w9 |
  8. ! J. f/ D% G3 P4 J1 F
  9. //double Input, Output ;
    ' }0 I# g9 K, s5 P8 G: H9 |
  10. float MaxSpeed = 255;
    * u" a% Z) u; d+ I& h) @4 @2 [4 U! L
  11. float MaxPower = 180;- `% l. X% f, V+ I0 x
  12. float MinPower = 120;/ g3 h- e1 Q+ h: Z% s' ]: g
  13. float Error,ErrorAcc,ErrorDec;
    6 z* U" P7 ^3 E4 }
  14. # o1 Z* W5 a! m2 G
  15. float Kp=0.14;
    & P' {/ d! q& D( W
  16. float Kd=0.00020;//23;& {1 n8 _; M8 b
  17. float Ki=0.000201;" M: m& @! c! U( M

  18. 0 J" C& d9 r; i3 U+ f" y4 l) @
  19. float nPower;" h8 T, m% E& {. v
  20. MePort lightsensor_6(6);( Z# s# l1 d2 C2 j5 i
  21. MePort lightsensor_8(8);
    ( @9 t: d: f- T' h, w
  22. MeDCMotor motor_9(9);6 [* J6 u) b; E1 Y# c
  23. MeDCMotor motor_10(10);8 l: {2 F# t, k, A1 c  \8 l# V
  24. unsigned long previousMillis = 0;
      Y0 f$ i; k' J. h6 b! w; u/ k8 V0 F
  25. const long interval = 1;# s6 t3 J4 b3 E. u( a! M% u6 j: q
  26. + n& w4 Q8 z3 x  s" ~% \
  27. void setup(){% N0 G0 n3 I/ Z9 e
  28.     lightsensor_6.dWrite1(1);
    ' b, ?% [! b: p+ F
  29.     nPower = 160;! f( {( O1 G$ O5 `; }$ l. \
  30.     Error=0;0 e, c$ W0 z( N
  31.     ErrorAcc=0;  Z8 b. j- E6 ]2 S3 o8 T& q$ c3 n( _
  32. }
    - e$ v! a' H2 Z+ t, e
  33. ; w# f# b, ?/ Z2 l; ], V
  34. void loop(){
    9 X  K' ]; B% {3 ?
  35.   unsigned long StartTime = millis();1 P: x4 P7 l2 S5 W7 K9 z
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){+ a. T/ ^! D1 z; N; X0 V* Z
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    ! Y( y; i( H; w
  38.     ErrorAcc +=  nError*Kd ;
    / U) L3 a( V( x) u
  39.     ErrorDec -=  nError*Ki ;7 e5 H' H* o' C1 D( H" V" F
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    # |" c7 E' d& k+ u4 v
  41.     if(nError < 80 && nError > -80){  `* X. n( }" _+ R; ?" J) s
  42.       if(nPower < MaxPower){
    & D- N6 l; L* Q% q1 k' D
  43.         nPower += 3;
    ! Z2 t+ h4 A) i
  44.       }
    7 x2 E. Y$ I% D1 C! p6 L- l8 c
  45.     } else{
    # N2 v. G' ~: p5 f
  46.       if(nPower > MinPower){' a1 a- r( z( l3 T4 e
  47.         nPower -= 2;7 p3 W! A% A  d# c* }; i
  48.       }! _) P4 A+ Y7 ~! C
  49.     } # o# U" r2 P9 m  U
  50.     MotoL(nPower-Error);0 R7 v, s: l' c) F% C: T
  51.     MotoR(nPower+Error);   
    - X+ P) b* C! c% C
  52.   }else{
    6 m* d/ ~% K: X3 t
  53.     motor_9.run(0);
    + s, V- u" d: H* ^, k1 l/ ^
  54.     motor_10.run(0);# S6 [' {8 s. S' M* o3 R0 n7 n
  55.   }
    3 \2 V" a- e0 P% a( z6 M
  56.   do{}while(millis() - StartTime < interval);% q% ~4 I, m+ S8 k4 o
  57. }
    3 q9 p! ~& K" m6 m& d! c
  58. % c5 k; y- [3 n& _9 @& i3 ?8 E) `
  59. void MotoL(int Power){
    ) }0 \, ?8 R9 `( l$ w3 q
  60.   if (Power > MaxSpeed){
    5 _" B) x" e# w  {2 G( f, @0 M
  61.      Power = MaxSpeed;
    " S  i1 ^) Z4 q( l& E" v4 }' v
  62.   }
    % {& W. P3 E/ e- |. W
  63.   if (Power < -MaxSpeed){
    9 X1 w  \, {" S' n; M, q( p
  64.      Power = -MaxSpeed;
    / {$ ^1 x5 V( u5 d9 j
  65.   }   @; e! B  v8 ?# Q* _0 [
  66.   motor_9.run(Power);
    5 Q, z5 X) T) p5 ?# d8 k
  67. }  
    2 B5 P; i+ {6 j+ g/ x; V

  68. 7 ]; o5 w$ j& g7 [$ h' L
  69. void MotoR(int Power){
    , J/ X' h% r; K' W# z
  70.   if (Power > MaxSpeed){/ G1 ]8 A; |$ U1 R3 H
  71.      Power = MaxSpeed;
    2 o9 U9 A2 J; t' |
  72.   }
    # M# c+ T! g2 O1 u* @$ C
  73.   if (Power < -MaxSpeed){) X: t& d4 r& \- f: s0 K
  74.      Power = -MaxSpeed;
    ' w6 }$ i0 {* k6 x
  75.   } 2 y5 x! q, }) ^+ S) F/ H# d4 d
  76.   motor_10.run(Power);  @' g! y. m6 H9 n3 L! a$ [
  77. }  
複製代碼

& H- e. Y7 s0 y6 P
1 W3 z( k2 E& @! J/ e
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯 ! d# x; m4 V4 L

% m' S4 a; G6 t3 ]您好,不知是否能向您請教。# |; o" I6 s, S/ `) y
目前和宋修賢老師在處理Ardui Car
0 A4 r# F$ _- O! @+ j雖然已使用較繁雜的方式處理了跑出黑線外的狀況  U( u( I( \3 q

& `7 d' g8 p2 [7 n但基於想追求更精簡的程式所以還是想請問一下
2 s' e1 Q1 y; \2 u+ n2 K就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝4 i4 y, m; d" v% e4 ^$ `, T
不知道您是否願意教我可以如何處理
1 \$ _* v( u3 `$ x" R) M: X
1 r/ I( T0 s+ x# D' m% @1 U9 J7 {3 |  W% f  [: w4 W: f3 o
以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;
    / A  g0 v# a  a7 f; x; Y' D
  2. double CNY70Val = 1000;6 A  U; x7 Z1 ^
  3. int interror = 0;& y4 T5 H' ^; A$ ^' x$ E4 e/ u( P
  4. int olderror = 0;
    5 p4 |5 M& g7 G) k4 D: C
  5. double values;+ R0 Z4 i1 V9 C5 {8 L- i
  6. 1 O! }3 a/ ~6 t% x# T2 Y
  7. 1 }( F3 {" `  j- g( {4 A
  8. void CNY70()3 P8 o0 O; G, I) S+ ^& V
  9. {
      j' L# X+ [# v( j  b: C
  10.   valuesRR = analogRead(RR)
    8 b2 l: K; D/ c; d
  11.   valuesMR = analogRead(MR);
    - F& Z1 n; g$ e/ [& w
  12.   valuesMM = analogRead(MM);
    0 G# Z2 J5 }9 n0 [' `7 c( `
  13.   valuesML = analogRead(ML);, a' {" X; r- h. u, |
  14.   valuesLL = analogRead(LL);
    4 a8 F7 _& `  D4 |" ^0 B0 E
  15. & U4 r: s) T3 |5 m. P; Y
  16.   if (valuesRR > CNY70Val)
    - K) I' N: o, e4 E/ c- E
  17.     valuesRR = CNY70Val;3 {( a/ g' d$ X7 i5 C9 v
  18.   if (valuesMR > CNY70Val)2 A* V. v& @8 t6 v3 \/ z' Q! p! d
  19.     valuesMR = CNY70Val;
      f# Q4 S' O- b8 s! q4 g
  20.   if (valuesMM > CNY70Val)
    ; U% K: j0 F; [! W
  21.     valuesMM = CNY70Val;; ]9 G. _+ z9 p4 t
  22.   if (valuesML > CNY70Val)
    ( F8 G! Q( M* h2 T
  23.     valuesML = CNY70Val;
    0 [  j' b3 }2 i
  24.   if (valuesLL > CNY70Val). \2 x2 _1 @5 ?$ g. G) F! R
  25.     valuesLL = CNY70Val;3 {: ~3 X6 a* W

  26. ; `3 O. c) s$ d3 i
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    9 y2 \7 `- _* r
  28. }/ S- p. E2 q& U

  29. 1 X( H# q' K* e- G# f
  30. void Car()
    1 I' X# A# l- W" e: S; _
  31. {* ]$ `6 k2 J( u( H2 Y& U% m
  32.   while (1) {
    / }* ?8 U) u6 [, U" N; P* j/ m, ~
  33.     CNY70();
    % f# n# m! I2 c. h" d& U4 ~

  34. ( n9 O' q2 g1 h+ k- z
  35.     int error = ((int)values);
    4 \& r5 i  ^9 W% P
  36.     interror += error;
      Q; ]; {7 ~4 ~
  37.     int lasterror = error - olderror;
    & K8 F) h, G* o2 u) [
  38.     olderror = error;+ A2 y3 s- f4 W
  39.     int power = error / 5 + interror / 10000 + lasterror;
    5 [. |0 w; k  c% S4 {
  40. 8 O2 c: v; K# `! p
  41.     if (power > MotoSpeed)4 d" s& A5 }, [4 y+ ~5 e% j0 ]
  42.       power = MotoSpeed;# S1 O% b' i3 b; s  e" \/ H1 |# h" a
  43.     if (power < -MotoSpeed)
    + r. f4 N: }; \% g* z
  44.       power = -MotoSpeed;
    4 S5 F: S3 j2 d; q! u$ }& \8 N
  45. 9 }' ~" k: p& v; _4 v- P; l
  46.     if (power > 0)
    1 ]$ H3 v  R$ N
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。8 t+ F* O7 U0 I* g$ P
  48.     else# X; @) L" ^0 }- U; V8 k
  49.       Speed(MotoSpeed + power, MotoSpeed);
    + r, Y2 q5 b) R2 ^4 ^8 r
  50.   }
    4 {& |! t* t) h% ~
  51. }
複製代碼

( Q0 g0 S9 J4 l6 |
5 A% s: v- B  ~. o  S- z! L
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-11-30 08:57 , Processed in 0.018780 second(s), 17 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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