圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21798|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>) t) x+ }2 a/ }0 b
  2. #include <Wire.h>* W7 r3 n  K! w- h% T
  3. #include <Servo.h>
    + ~8 ^# M/ ~: r0 `5 W  N; j
  4. 1 p5 P  O. ]( |( Q5 r5 o
  5. #include "MePort.h"& \2 Q, q* n# x( P0 A: h
  6. #include "MeUltrasonic.h"8 N8 w( W+ k5 _0 x
  7. #include "MeDCMotor.h"  q! n9 k) V+ o6 `

  8. 5 [9 l$ E9 I1 o! a# Y& o# S, H
  9. //double Input, Output ;
    " G& }4 u' t! m+ f
  10. float MaxSpeed = 255;
    0 `4 q0 M2 R3 O5 @- Z
  11. float MaxPower = 180;' v/ k+ b7 l6 Z# J
  12. float MinPower = 120;
    ; j# I! A. e! j
  13. float Error,ErrorAcc,ErrorDec;
    8 s  h7 E  j, K9 B- M3 m. t
  14. , ]: [* D! V2 i3 A" Z, |/ v
  15. float Kp=0.14;  H5 H; o+ w6 F, p1 z
  16. float Kd=0.00020;//23;
    9 A1 x$ ?2 T$ t, |9 v
  17. float Ki=0.000201;
    ' T" V3 @3 p1 `! Q

  18. ) K$ h! W9 C1 A: T
  19. float nPower;
    2 E4 q1 W* `' R; Q3 L
  20. MePort lightsensor_6(6);
    5 d0 x% f6 C2 C4 b' u/ ^0 O# Z/ U
  21. MePort lightsensor_8(8);
    / k' e: e3 i- y6 y% g
  22. MeDCMotor motor_9(9);, m3 W* E$ N# P
  23. MeDCMotor motor_10(10);7 d$ ~# M1 K7 R6 V9 N
  24. unsigned long previousMillis = 0;
    ' L- N: p0 |6 {$ G
  25. const long interval = 1;( q- H' Y8 s' k, t5 [5 L  c( x
  26. 6 I4 Q# ]. L# ~" [; z. h. P; x  M
  27. void setup(){- {, Q; J& r" l3 ~
  28.     lightsensor_6.dWrite1(1);
    ; \7 d, [0 D+ N$ h- J- k
  29.     nPower = 160;( y/ Z: x9 I1 j2 B& A5 z4 x4 D
  30.     Error=0;- m* F- q  O+ c3 C! {) O5 J  O
  31.     ErrorAcc=0;& s# B; e1 U" U( v) k, x
  32. }
    ' ^" }0 w& Q2 N) H
  33. ' M! F; _. L" e8 M
  34. void loop(){3 d* h% T- n' c( E
  35.   unsigned long StartTime = millis();
    9 G8 b3 y- e7 O  Y, x& U! P
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){( g& a8 V1 Z% Q9 ]. N
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    / y2 f+ L! U6 e9 ~, h
  38.     ErrorAcc +=  nError*Kd ;
    : t5 m" U. q0 f3 n4 h( L
  39.     ErrorDec -=  nError*Ki ;: O5 A6 B7 F* `1 T9 ^, k$ Y
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);0 e% ~0 `5 O% u% ]* O6 F- ]: C* K: S2 v
  41.     if(nError < 80 && nError > -80){( b' z* ?; i/ z, @5 q8 e
  42.       if(nPower < MaxPower){: m3 K, P* t+ ?. M* W+ n; |
  43.         nPower += 3;
    - K5 m: T1 p& [. ]
  44.       }
    & T% G2 q" x; B, @7 C9 m
  45.     } else{
    5 O) Z3 |; u$ V. D* [8 H
  46.       if(nPower > MinPower){% O) }- F3 V* L' {' D1 r: r  K( {
  47.         nPower -= 2;
    . z* A: z5 R( v) I4 n
  48.       }/ C9 l4 c; l+ P0 _0 ^5 m8 X" v' v
  49.     } 6 h# J8 U/ Q9 U: h; z2 k
  50.     MotoL(nPower-Error);+ K* w. A9 [1 G& f. Y' z  A. o
  51.     MotoR(nPower+Error);    & [4 x  N% l4 V5 v$ ~5 y  B+ x
  52.   }else{0 J2 f$ Z7 ^- b5 y; c0 B; S1 f6 e& z" y
  53.     motor_9.run(0);, ?% v4 }* F, x8 p+ v4 N4 @% c- i
  54.     motor_10.run(0);& ?( w$ H8 Y  }$ M% X5 V7 @
  55.   }
    2 c8 i& y% u% @6 I# E- `
  56.   do{}while(millis() - StartTime < interval);/ o/ l# o7 y! _" M4 e8 b* z
  57. }" {; A7 D+ ?' `+ K4 G6 W
  58. * n2 z5 ~7 Z) W
  59. void MotoL(int Power){/ \; B, B( S  b/ k: T- `
  60.   if (Power > MaxSpeed){
    1 y: z# }! Q- ]5 N/ K' t- I
  61.      Power = MaxSpeed;4 ?+ b/ z( A& V5 |9 b& p
  62.   } 6 U  y: I$ R! @6 v0 W) m8 \
  63.   if (Power < -MaxSpeed){' w6 C+ q; F$ T' b' N/ A
  64.      Power = -MaxSpeed;
    . E$ \- P' J3 G. j/ |' `, q
  65.   }
    5 t7 u4 h4 x2 \+ ?7 O
  66.   motor_9.run(Power);
    5 W/ h$ f" m* }+ q
  67. }  + G& o& X) c# P5 p

  68. 8 x# f2 ], ]( ]& c3 M/ d2 K: C
  69. void MotoR(int Power){  ]$ K7 o% X7 |& \, r
  70.   if (Power > MaxSpeed){
    ) t8 C5 P1 U  h4 z8 v
  71.      Power = MaxSpeed;; n+ S7 X, p3 m; _
  72.   }
    1 `2 V; a0 H2 w3 [$ \  T" V. ]  E
  73.   if (Power < -MaxSpeed){
    4 R1 [: f" r4 L% x' K  e+ H
  74.      Power = -MaxSpeed;
    3 D4 R; d, M% Y, i2 {+ i
  75.   } 6 i1 r, r1 v6 j% Q
  76.   motor_10.run(Power);7 _* S9 P. v# R* t- w
  77. }  
複製代碼
, n! a, t" r6 u; Y, j# |
+ G/ C( ?$ q$ s, p4 F: B
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯 9 B: z9 s. Z. I* m. W" F

: H* L1 w  ^' A* G您好,不知是否能向您請教。8 R8 D, @% \4 i% D
目前和宋修賢老師在處理Ardui Car! p3 s4 ]1 e! l& a
雖然已使用較繁雜的方式處理了跑出黑線外的狀況
/ B9 R  V3 B& |" p2 M& E
) t  d- a& Z9 S$ X但基於想追求更精簡的程式所以還是想請問一下
  w/ F8 H8 W. E, k( b! e就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝% D" a2 v' T/ W# r3 D. K* p; }5 F
不知道您是否願意教我可以如何處理- e& k7 t2 _' M- E
3 A* u5 n$ L2 V# p% M6 _
3 B" V+ F+ x/ ]2 z
以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;
    & D! C! {9 Q7 m$ M
  2. double CNY70Val = 1000;
    + X! @% {9 k, H
  3. int interror = 0;
    * o# F: P( |/ F$ e1 a
  4. int olderror = 0;
    ) o7 \0 h  b& P7 A
  5. double values;
    % m- z& b; ?3 b+ [( _8 }. a1 I
  6. $ O5 [7 i1 c0 E' ^+ S

  7. ! I* X) ~( e) Q. g" v
  8. void CNY70()
    ; b) a1 O( P8 @1 K
  9. {0 S2 U: v1 e8 B; I, L# T7 |3 D
  10.   valuesRR = analogRead(RR)& F/ @( f. Q2 \" X+ M7 u( r" D$ o8 _' u
  11.   valuesMR = analogRead(MR);8 j4 x1 M( Z" M! g6 Q  f
  12.   valuesMM = analogRead(MM);# b5 a: V- }* x! \
  13.   valuesML = analogRead(ML);4 r' B/ t  h4 |: W5 T
  14.   valuesLL = analogRead(LL);
    + p$ B( s: s  N$ H1 V
  15. - Q$ f+ q* d, {1 y& s- _+ L
  16.   if (valuesRR > CNY70Val)6 Y- a9 I2 b. v& T
  17.     valuesRR = CNY70Val;
    $ E8 z" _& l% o# c
  18.   if (valuesMR > CNY70Val)
    : b* }5 p; F! Z& e) l6 H+ h
  19.     valuesMR = CNY70Val;
    ' g$ L. i" c' }: C( ]  ~
  20.   if (valuesMM > CNY70Val)" J2 x+ r( R, ~: s8 K: w3 f: }
  21.     valuesMM = CNY70Val;
    ! N9 g: v: y' A2 o8 h1 O! v
  22.   if (valuesML > CNY70Val)
    3 x( S4 {9 a* K; ^
  23.     valuesML = CNY70Val;
      G8 x; ~7 w: ]- P& q6 p" E8 G
  24.   if (valuesLL > CNY70Val)
    6 d  P& P1 O: M8 x. L( S; x* q
  25.     valuesLL = CNY70Val;2 [+ S1 ]: `% O6 K: _6 j- \8 t
  26. / c  P  [& j+ l! t3 o8 G+ c
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    4 I4 a: E- m) q) `# u) `) |
  28. }) p: O& y- J) k. o9 K
  29. / j$ Z$ X" w& G- {, e* J4 n
  30. void Car()
    * h5 r  e: S* c/ g$ l0 W
  31. {
    * v) ^, G8 D+ V8 E. Q6 J, O) U
  32.   while (1) {
    ) W1 N3 s- M$ v: T( Z
  33.     CNY70();
    , f+ r. U/ l- B4 z/ D+ ~
  34. ! c7 Z+ j5 V! p& m7 X9 A
  35.     int error = ((int)values);) L1 Q" V, @0 R9 Y
  36.     interror += error;9 O9 s/ X# `0 e* \
  37.     int lasterror = error - olderror;
    ( w3 m7 c- O/ m6 k9 l
  38.     olderror = error;
    & S: G# }" q5 G' ~! n$ C
  39.     int power = error / 5 + interror / 10000 + lasterror;8 A) ?2 K( D' p6 \" M+ o! J; B

  40. 0 K% C/ _9 y( E$ b
  41.     if (power > MotoSpeed)
    ' }3 x' b1 O9 M8 Z( {" D
  42.       power = MotoSpeed;$ K: K1 L9 g# p7 h0 Q
  43.     if (power < -MotoSpeed)3 D+ J  C' U6 T: O* X
  44.       power = -MotoSpeed;8 w# v. p+ r) R$ c5 w3 u
  45. & ?  D+ k7 S0 Q, \$ X/ p
  46.     if (power > 0)
    8 j/ J# u; ^; M$ Y0 q
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。. [" f7 P; i) {# ?
  48.     else
    6 C. o0 {/ H8 i& E5 H
  49.       Speed(MotoSpeed + power, MotoSpeed);+ ]2 o5 N4 }# n& \/ e
  50.   }  r* k8 k( y/ [. w( E/ h, p
  51. }
複製代碼

/ Z+ O$ E1 s8 P
+ O! P- y2 S+ a. x/ G3 W8 n) t
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-12-1 16:19 , Processed in 0.026946 second(s), 18 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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