圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 20953|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>
    % H% y4 K. {* ~- b& w$ q
  2. #include <Wire.h>
    6 l4 b/ L6 ]/ Q, a
  3. #include <Servo.h>
    . @1 F0 F+ r4 u% M0 \1 \& @

  4. 7 L8 g0 E  a$ L0 ?" g2 V
  5. #include "MePort.h"# A. _) t$ F( f. X1 ]' O
  6. #include "MeUltrasonic.h", p) p3 }* P* |  J. L
  7. #include "MeDCMotor.h"
    & U$ i6 V2 [8 O, h$ A% z$ H) V
  8. , ~  V& m  P$ C2 x( t. c
  9. //double Input, Output ;
    2 o1 J# n' x2 d+ z* h& [: [/ T3 X
  10. float MaxSpeed = 255;/ p$ ^" Z  x: }( b- u
  11. float MaxPower = 180;
    7 H3 W8 }0 g. m
  12. float MinPower = 120;- n. [$ a7 f' z" B9 r6 Z3 t% q4 p
  13. float Error,ErrorAcc,ErrorDec;
    ' G! X: G2 U9 P, ^' h; n8 t! N
  14. / D3 Z. ]" g9 G3 E* T) b2 U
  15. float Kp=0.14;
    4 r% a: }7 m2 W8 {# K( K( \
  16. float Kd=0.00020;//23;* E1 W+ C. U3 I: \0 H$ A! r
  17. float Ki=0.000201;) g( Q, b8 l5 V. e4 G# y, X, p
  18. ; n6 B8 H: V3 {
  19. float nPower;* g3 k, V. B0 Q2 x- ~7 S
  20. MePort lightsensor_6(6);
    " P4 D0 B5 C+ W0 e0 p* S$ i
  21. MePort lightsensor_8(8);: U' |4 Z) W7 c: H+ n  x
  22. MeDCMotor motor_9(9);
    . G3 W; ~# }) M( |/ z5 z2 P
  23. MeDCMotor motor_10(10);
    & X  D1 F  L2 D: t3 t
  24. unsigned long previousMillis = 0;# h0 G6 t6 ?) }" `/ w; x
  25. const long interval = 1;
    ' A0 O0 ?9 I- t

  26. ; F7 X& j+ E4 z, y1 ?. C6 C# J
  27. void setup(){& E  d; h' }& {; @6 U$ \; T) t) f
  28.     lightsensor_6.dWrite1(1);8 N2 H9 k/ f# x& U$ c
  29.     nPower = 160;" h# T" |9 W, i
  30.     Error=0;, ?/ @$ n5 `4 Y+ A, f0 n0 }
  31.     ErrorAcc=0;
    ' y) s" S$ W, |5 U5 ?
  32. }
    7 C/ K6 t$ F9 ]* a& u

  33. 1 o1 q7 d) `0 L: d
  34. void loop(){
    8 i$ U, a$ j* @2 P& F% \. {& D
  35.   unsigned long StartTime = millis();
    5 P9 L6 o: n% h' R, C
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){2 \& x+ I( }% g0 ?  P, I$ _1 ^: v) c
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();, u2 l% n5 U/ v. K
  38.     ErrorAcc +=  nError*Kd ;
    : }& C, f4 b* r( \% h0 n
  39.     ErrorDec -=  nError*Ki ;, e& Q, @7 g% Y4 O, B7 D) z9 ?* l" k
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);3 n/ x% u7 q2 I  B5 p6 v/ p% \
  41.     if(nError < 80 && nError > -80){; k0 _4 @( k9 V7 y  A& y
  42.       if(nPower < MaxPower){
    3 U$ @% v6 H* u' ?2 K$ N
  43.         nPower += 3;3 ?( [9 H* Y/ v6 q7 |! Z  U) Q
  44.       }( j" x2 y+ F' ~3 ]8 N" ^9 U
  45.     } else{4 |0 W; [& |3 _7 {
  46.       if(nPower > MinPower){' w$ ]" K' s5 W, Y/ e  n% \
  47.         nPower -= 2;
    1 J0 S( d. D+ K6 C9 i& [
  48.       }' U. [+ \5 Q5 a+ W
  49.     }
    7 T5 K7 f3 x2 R$ G  r- {5 k
  50.     MotoL(nPower-Error);
    ; G: F- S, P9 L) O" z
  51.     MotoR(nPower+Error);      h( H+ X( n$ D! ?
  52.   }else{
    2 ^% p. V  q1 Z' n. x
  53.     motor_9.run(0);
    ! W" W" A. o3 g2 E! \* ~
  54.     motor_10.run(0);
    9 B$ Y6 [# w9 [7 v6 c
  55.   }$ _7 L. q& e  v3 E: a! }) ~1 L
  56.   do{}while(millis() - StartTime < interval);
    ) V) r0 a0 a; o- o, r& {
  57. }; R8 Z* b2 R6 X" [0 ?( Z4 X1 N8 y
  58. - k; j5 E+ N  E
  59. void MotoL(int Power){
      c6 ?, ], z+ q9 L8 J
  60.   if (Power > MaxSpeed){! u1 k1 M7 ^  s. l3 |9 d4 c! {
  61.      Power = MaxSpeed;4 O- `8 V8 ]/ ]5 E& U8 X/ n) ~
  62.   } : J' G3 @+ d1 J
  63.   if (Power < -MaxSpeed){
    $ A# _7 T$ G7 L# ]
  64.      Power = -MaxSpeed;
    $ y6 z) H+ n; ?3 C" R- g
  65.   }
    , d, U$ m5 z. ]  W( J3 n" ^
  66.   motor_9.run(Power);# `- V' d8 Y+ q; ~* v
  67. }  
    . O7 Y8 t" i4 c4 t; S
  68.   N5 F% f5 n( C
  69. void MotoR(int Power){/ A$ B2 T, e9 A& U2 Z4 O/ q& Y2 l
  70.   if (Power > MaxSpeed){% j7 i& v1 ^8 R9 w, [
  71.      Power = MaxSpeed;7 A/ t% F" u0 X* Q# T
  72.   } 9 `! w/ e$ P1 b! `: `
  73.   if (Power < -MaxSpeed){  Z' V; |& {  X  n8 n0 U; U( D8 y
  74.      Power = -MaxSpeed;
    2 _1 z; V  ~. b* N9 F* D# p% N
  75.   } ! I) x# w6 c8 J
  76.   motor_10.run(Power);6 H) R; h" P3 f% k
  77. }  
複製代碼
5 ?" F* ?" _; N4 R/ t

( J0 a+ I* r7 [2 ^, ~' N* y/ b
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯 % B% R, x- g! r/ ^! W
" F* u7 F# v8 s% C) j: N
您好,不知是否能向您請教。
9 g8 ?  @5 z: ]! _0 J目前和宋修賢老師在處理Ardui Car
6 K; p( D1 |2 T6 s; E雖然已使用較繁雜的方式處理了跑出黑線外的狀況
6 ^5 h( B9 l7 H6 m
7 y2 Y" G5 t( v* @但基於想追求更精簡的程式所以還是想請問一下0 ^7 p  M! J$ g  d3 A
就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝
$ O3 J1 x; f3 I6 {不知道您是否願意教我可以如何處理8 J- k1 `! K, u; v$ K

% s% n4 Y& @# v
9 |' w8 y" m6 h) z5 C# y以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;
    1 L( O7 O+ R1 X- @9 r9 q
  2. double CNY70Val = 1000;9 }! ^9 A- R. p7 Y* |
  3. int interror = 0;" f" g* V3 X) f; A
  4. int olderror = 0;2 J* ?: ~$ T( o2 C! B$ P& o
  5. double values;
    ( k1 Y" A6 y) d& L- ]7 b

  6. / Z* P: d6 B1 t8 C; w! j
  7. - u, ~; T& g% [
  8. void CNY70()
    & m& Y& X3 P) n3 G9 D
  9. {
    ' N, u! g% o* ]) ?0 H
  10.   valuesRR = analogRead(RR)# [8 Q4 \' _  t7 x# V( W+ ?
  11.   valuesMR = analogRead(MR);7 N0 U; [& B9 _/ x( ]% l" e( x1 _
  12.   valuesMM = analogRead(MM);
    - w! `) M- q8 X6 C
  13.   valuesML = analogRead(ML);
    : `2 j. K, _$ Y8 c/ t0 t2 i' g  N3 B
  14.   valuesLL = analogRead(LL);7 e6 @. a. R" W- ?# Z+ s
  15. % p  a2 D# B: B) w8 m9 R
  16.   if (valuesRR > CNY70Val)
    - |5 {4 G5 H4 `" {; D
  17.     valuesRR = CNY70Val;  x( g$ ?9 Y" M+ h0 Z) g3 A6 Q
  18.   if (valuesMR > CNY70Val)' d- u$ j: c" V" |& ~0 V3 A
  19.     valuesMR = CNY70Val;/ n; v5 z" O$ R
  20.   if (valuesMM > CNY70Val)
    * c( q1 O+ k: N. W% Y" e7 f8 \- V
  21.     valuesMM = CNY70Val;0 @- H1 U8 L, s& G$ v+ B
  22.   if (valuesML > CNY70Val)
    3 W$ Q) r( T/ F, L. }) f
  23.     valuesML = CNY70Val;+ |3 B; B5 g3 d9 s$ C+ i: K$ H
  24.   if (valuesLL > CNY70Val)
    " ~0 A4 V: f" F2 ~7 l% X
  25.     valuesLL = CNY70Val;+ ?1 C9 f! J8 a+ e& k: F& P: p
  26. 1 B! c6 d2 Z+ x3 `# P+ @. a
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    6 B9 Y% n# |7 {5 c' Z  @0 L
  28. }
    7 O3 ]- x) {3 ^+ ?3 E
  29.   c( Y% S! u3 u) [' b1 j
  30. void Car()5 _. e6 _9 ?7 Z0 O! i
  31. {  J: o$ h1 X8 t0 k' f) T( z( F
  32.   while (1) {
    8 R/ p1 v, v) E4 {
  33.     CNY70();4 L& `- c9 y' M4 K( T; L

  34. 3 J- ^: D. c+ R1 V2 ~4 @; G8 i
  35.     int error = ((int)values);4 H& |, A0 b) J4 f6 X
  36.     interror += error;
    ) \( j, d! W' k7 K
  37.     int lasterror = error - olderror;
    * e( g" t$ N; E! S8 y! r: X0 u
  38.     olderror = error;
    ! n. ]7 _1 U/ G# B# r. _  L/ Q% \0 x
  39.     int power = error / 5 + interror / 10000 + lasterror;
    5 [3 U4 g  K: s, N5 o: ^

  40. : F8 Q3 T( b- R) L  f( M
  41.     if (power > MotoSpeed)
    + |; L5 K" y/ ~4 o: |6 }  {' B3 ^7 `
  42.       power = MotoSpeed;9 O$ V# p2 _0 \( a, q
  43.     if (power < -MotoSpeed)
    / j4 v" z, Z! x
  44.       power = -MotoSpeed;/ t) W+ M1 Y4 |7 `$ M# N+ X
  45. # @, h8 y+ {4 @2 p  d
  46.     if (power > 0)5 F2 H+ Y8 m' u9 e
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。
    2 M3 q# }! ?" q+ `
  48.     else- A+ V8 N! A( g& f
  49.       Speed(MotoSpeed + power, MotoSpeed);7 v  \& C& V9 S1 e0 `
  50.   }
    , K/ q. D) ~; k# x% W! b( H
  51. }
複製代碼

2 D* L% R+ L$ q, f, L) F) w6 E( c+ C" P8 A3 ~' G- |
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-7-9 17:04 , Processed in 0.022448 second(s), 17 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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