圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21843|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>, |1 o6 i1 o" U! }' z
  2. #include <Wire.h>
    $ P& _; n" T, A" D1 f! M% K0 ?
  3. #include <Servo.h>
    5 j4 e  W) O( K# s

  4. , p) U* _7 i. k0 ^2 _7 V; r, M
  5. #include "MePort.h"
    % [" g3 ^' s6 T( Z* ^# g, @( c
  6. #include "MeUltrasonic.h"- E  z% ~; X8 ~, r" V$ Y6 W
  7. #include "MeDCMotor.h"
    4 D! X/ A7 @( t, c# F- s& t& g
  8. 3 g2 }# [) H8 C( z
  9. //double Input, Output ;
    ' R5 B- z- q* s3 p
  10. float MaxSpeed = 255;+ t" R5 t. p, _8 l2 l  q' V- v
  11. float MaxPower = 180;
    6 B1 U  U% s' L6 A. d6 e8 y) Z
  12. float MinPower = 120;
    2 U" V7 k$ p: [- D
  13. float Error,ErrorAcc,ErrorDec;  @5 n; T% q( p

  14. " i* i- t% u2 n$ ]. c, y
  15. float Kp=0.14;  m. m$ p( z% H& w  ~) U' A$ L
  16. float Kd=0.00020;//23;$ X/ v1 P# G6 R( s$ \
  17. float Ki=0.000201;" |7 f5 P9 s  i2 y
  18. ( z7 f& D. g4 h* p, a0 f  e
  19. float nPower;
    ( w8 p" J* z6 p2 N- U. V8 B
  20. MePort lightsensor_6(6);6 u* T& K% [: y' S- h+ P. E0 f; g9 ?
  21. MePort lightsensor_8(8);
    0 D  E& q' \0 Q/ U2 o6 S
  22. MeDCMotor motor_9(9);$ n( `5 ~: G( f( P
  23. MeDCMotor motor_10(10);
    " X5 p* B! U: b  p
  24. unsigned long previousMillis = 0;
    3 K3 k1 k( H% i# P" w; q. t
  25. const long interval = 1;2 O- G7 B# h$ j, ~" {
  26. 8 y, \: M( ]3 @" t* I& g- R' o. B
  27. void setup(){% B. e! H  z% h" y7 ^# }6 G9 S
  28.     lightsensor_6.dWrite1(1);
    / l/ m; d% q4 \- a
  29.     nPower = 160;
    6 X( Y" a+ D# ]' P
  30.     Error=0;( U" A6 V) \0 C$ J4 b
  31.     ErrorAcc=0;
    : H: G% |8 N& V% |! R
  32. }# h+ t2 m8 ~$ k7 i- l
  33. ( a% c/ Y& h& k" u; v
  34. void loop(){6 G9 G/ g3 N9 l/ h1 w% W" ?
  35.   unsigned long StartTime = millis();
    1 h: M+ Z) F, A' O* D: I
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    . B1 {7 b, F4 K" ^9 c, Y0 d
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    8 {& i2 F7 l% E: m1 T
  38.     ErrorAcc +=  nError*Kd ;4 l; C: \/ U& [8 K0 |
  39.     ErrorDec -=  nError*Ki ;  |- e+ ~6 [- E8 n
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    % \% O8 u) ~: W5 x: A. e
  41.     if(nError < 80 && nError > -80){" T+ _9 l2 I9 k
  42.       if(nPower < MaxPower){
    / |/ w: g5 R* p4 d+ F
  43.         nPower += 3;% g- ^0 C( U6 R& ?: ?. J
  44.       }
    " s. r) @; @" y# v2 Q
  45.     } else{& q4 A+ v* ]# z- \9 O& s+ e+ R% n
  46.       if(nPower > MinPower){0 M7 l1 b& i( A5 Y. T( v
  47.         nPower -= 2;6 a4 u( ^! z  B: D% b" Z
  48.       }
    ' R9 D) x6 }  X2 [7 ]
  49.     } ' I- `* D" P& F8 t2 ?* z% C& F- }1 E
  50.     MotoL(nPower-Error);" R1 U) `' b3 {8 A
  51.     MotoR(nPower+Error);   
    * ~$ M" k: _$ G% S' i+ D. @% }: \
  52.   }else{5 H4 V. \1 w4 h) B
  53.     motor_9.run(0);
    ( n! o" ]$ R9 D' v* H# f) b( E
  54.     motor_10.run(0);
    & J& F3 R9 r* g" ]" T( @
  55.   }
    . m# y4 J- Q! s/ ^# |& a7 r! E
  56.   do{}while(millis() - StartTime < interval);
    3 p4 q# b& Z4 ?/ }4 O$ R; J) M/ M
  57. }9 O+ I) y5 m5 `! s0 `! x
  58. 2 j4 [% R3 L9 v
  59. void MotoL(int Power){
    ( k. ~$ Q, G: K% c% Z
  60.   if (Power > MaxSpeed){% B+ a. u$ {; Q! W- |) K6 C
  61.      Power = MaxSpeed;% X7 n1 b# b$ Z# }, C
  62.   }
    ) Y# P4 W+ o2 C; k$ q4 M
  63.   if (Power < -MaxSpeed){( u( d5 Z1 l  U, X
  64.      Power = -MaxSpeed;# g0 X: D; X" S
  65.   } , a& I' u. P- N' j6 |( [3 ^
  66.   motor_9.run(Power);. r2 Y& \6 h* H, w; q/ Y
  67. }  
    % d" A  B  M, Q0 |2 Q7 z/ X, o

  68. 0 l" i+ h* b3 F9 h) k) O+ N' ]8 ^
  69. void MotoR(int Power){
    , X& @+ H+ y4 b1 B/ X4 X
  70.   if (Power > MaxSpeed){
    3 D; M" G3 P6 p! a# ^
  71.      Power = MaxSpeed;3 u2 W' d+ w0 A$ ~1 P4 A9 W+ n
  72.   }
    - R- f$ X3 N& b& w7 e6 B
  73.   if (Power < -MaxSpeed){( G( a; k( D- ~; V
  74.      Power = -MaxSpeed;
    & c- o/ X5 g5 ~/ c' y( _5 c" c9 T
  75.   } ; ?/ j; {$ v4 E, k; g
  76.   motor_10.run(Power);/ j8 e8 b" E# x. R0 Z
  77. }  
複製代碼
5 X& R5 j" \6 Z1 P' q$ T

$ H  [- I3 N) E( Q
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯
% C7 q" P  ]) M, T% N7 h# z+ ~8 E) Y
您好,不知是否能向您請教。
4 M: |8 e# m( r8 k目前和宋修賢老師在處理Ardui Car
6 z" x9 g+ y! h8 T$ _4 T雖然已使用較繁雜的方式處理了跑出黑線外的狀況: p/ o( L8 J# Y# [9 }

5 Z( p1 l+ Q6 V# _但基於想追求更精簡的程式所以還是想請問一下
9 {1 K# X7 j1 r3 x# A" h- W' a就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝4 R2 X7 U# r. y5 E/ T$ U
不知道您是否願意教我可以如何處理& L/ x; {: c5 ~1 K, q) W& y
4 |8 W3 {2 Y, o+ ]' C

% {" g7 |" A4 }  G以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;4 j6 n8 X; [; w8 s6 Z% X8 i
  2. double CNY70Val = 1000;3 X9 z* U) v+ ]9 V& S
  3. int interror = 0;  s2 r6 b( C; D7 x. ^
  4. int olderror = 0;
    $ l- e4 Q! y5 H( H( F
  5. double values;
    1 U" a& Q. U" [0 ~/ D/ I

  6. 8 f4 Z8 w5 M6 ~, g( Q
  7. 4 R% [$ I/ X* P1 [; ^! B; f
  8. void CNY70()
    # F4 j: E$ v- r; y
  9. {8 s' m; P; V6 M
  10.   valuesRR = analogRead(RR)
    7 }0 S1 M) G. b! k5 t
  11.   valuesMR = analogRead(MR);2 G! x) |, ?# O% A" g( W* k
  12.   valuesMM = analogRead(MM);. e1 Y8 h$ c) N6 |" S, M5 \9 j0 ]
  13.   valuesML = analogRead(ML);6 T- q6 `$ \0 H& O
  14.   valuesLL = analogRead(LL);
    * Q- I' Z7 K. K8 x, F; w
  15. 1 n+ S4 E  ^- A) y/ K" l
  16.   if (valuesRR > CNY70Val)3 y/ N, I; J- v! i- v$ A
  17.     valuesRR = CNY70Val;# j9 T: n4 V& v7 ?1 |
  18.   if (valuesMR > CNY70Val)# Y. f+ Z5 t9 y% R
  19.     valuesMR = CNY70Val;
    ! I8 x; s2 Q1 `
  20.   if (valuesMM > CNY70Val)/ r; @5 g  d) j% ?( W+ \" f
  21.     valuesMM = CNY70Val;: x6 v# D- l- e0 l5 e
  22.   if (valuesML > CNY70Val)
    ; _$ b. q5 _" l. Y/ F( k+ X' s
  23.     valuesML = CNY70Val;
    . G' ~7 f2 M8 |3 F/ v) _# V  H7 c$ J9 s
  24.   if (valuesLL > CNY70Val)
    3 i8 x+ S. J9 }0 Y
  25.     valuesLL = CNY70Val;
    8 Q; k9 z% D! V$ p$ \

  26. + m) {5 N- I7 `+ K1 I
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    ( B% n3 M0 c5 @2 q+ n9 i9 z
  28. }
    / m9 j- ?  P) w) h/ f$ X4 F' B# i
  29.   t, `2 k: Y" M
  30. void Car()3 d* c' R1 M8 C) j
  31. {
    3 m. h# x; o9 q0 R2 `) e; M
  32.   while (1) {. K9 B7 z, C! E1 m: M. ~
  33.     CNY70();
    # k5 u4 X; w- W9 u; l: B" L2 _
  34. . H' R& l& L% i0 J* C
  35.     int error = ((int)values);
    : U  Q7 ?, F0 z5 s( ]% |1 {3 ^
  36.     interror += error;% b6 m5 s/ m. U& r. `3 Z
  37.     int lasterror = error - olderror;. w! t  E2 ^2 ]& D4 R1 P1 x
  38.     olderror = error;
    5 ~  t4 E6 ?, @4 `  v4 p+ ^
  39.     int power = error / 5 + interror / 10000 + lasterror;
    $ @& C: M$ R* H8 H
  40. 3 H( |3 }; f. a  p2 _
  41.     if (power > MotoSpeed)3 I% s  y2 V; m# k6 b- g
  42.       power = MotoSpeed;( w6 l  Y4 v2 y; j
  43.     if (power < -MotoSpeed)
    * H% w; U' i' q4 M0 S+ Y+ p
  44.       power = -MotoSpeed;: u+ j( p* c8 A

  45. $ U* j1 ?8 |1 [- s8 V1 H% ~$ ]
  46.     if (power > 0)
    - w: {1 l- B) q6 W
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。- B6 O7 U( C# k3 l' R0 u
  48.     else
    3 x  J- T' j2 i% \( ]; H
  49.       Speed(MotoSpeed + power, MotoSpeed);
    8 V6 e3 x9 F0 }2 V) u7 B; g5 N2 v
  50.   }
    , b# X3 _6 u5 z3 A" \6 L
  51. }
複製代碼

1 Y1 |( k4 V: D4 t9 a0 c2 _. D1 M1 X% }, d7 U* x
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-12-5 05:50 , Processed in 0.028038 second(s), 18 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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