圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21809|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>* R+ {) ^2 ^/ _& B/ j! y
  2. #include <Wire.h>$ S" N$ L5 C  c2 [+ ]
  3. #include <Servo.h>
    # b2 b% c6 ?- I; g0 V

  4. 7 V# R) o5 E$ O1 v
  5. #include "MePort.h"+ Z2 a5 I/ \! T( E
  6. #include "MeUltrasonic.h"4 v3 P4 ]* W" M  {0 S( w
  7. #include "MeDCMotor.h"
    ( m* O' A+ Q" Q( C$ B
  8. , Q' L) ?1 v+ _
  9. //double Input, Output ;* A/ j; ]" }* z2 t5 q
  10. float MaxSpeed = 255;+ p! f, P0 T, D8 b; ?
  11. float MaxPower = 180;
      P- Z1 s# j6 x6 q8 y  g
  12. float MinPower = 120;
    ' J$ H  R; h( d
  13. float Error,ErrorAcc,ErrorDec;
    * @4 e& c. M5 u" J2 `
  14. 3 ]5 ]0 b8 f, n) R! V
  15. float Kp=0.14;% _" D' Z& s+ a1 d! F: T# e$ B
  16. float Kd=0.00020;//23;# K% D9 S* T+ [
  17. float Ki=0.000201;
    * G1 g# l3 j! }

  18. % t+ u; q, G0 q3 B/ k
  19. float nPower;
    / n, A/ k& a* k4 M* ~
  20. MePort lightsensor_6(6);6 k4 S% R5 t2 m
  21. MePort lightsensor_8(8);
    " K, N$ _6 B# R0 H
  22. MeDCMotor motor_9(9);
    - _0 C( W2 S8 R/ z) u$ L
  23. MeDCMotor motor_10(10);
    3 ?7 ]( k# g+ Z1 f( c
  24. unsigned long previousMillis = 0;5 c( h- c/ R. ~- u% e4 z, S
  25. const long interval = 1;
    ; H, X1 `: y, H7 h5 W5 i: R
  26. 8 U2 q) |7 K. X, J' s( [
  27. void setup(){
    & \$ n$ |5 p% w
  28.     lightsensor_6.dWrite1(1);
    2 O4 n& X4 n/ Q! B3 {) K* n
  29.     nPower = 160;
    ! T% V5 j2 w: @0 S# L& r
  30.     Error=0;
    % Z' d$ G: N; h; c" r, V
  31.     ErrorAcc=0;( l3 `( m3 t* d8 p7 O1 p
  32. }. ^0 _  _7 t7 i* T! ?; W' q

  33. 5 ^5 T3 E  u7 p; o- S& ?& r
  34. void loop(){$ k% J9 X7 J$ P3 b/ B( s
  35.   unsigned long StartTime = millis();
    , k& l1 o/ W. F+ P. t9 w
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){3 _$ A' U$ L5 T
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();/ i* s' y$ S1 h/ o
  38.     ErrorAcc +=  nError*Kd ;" n, B  Q+ C5 ^; B% Q3 E7 U( K* q
  39.     ErrorDec -=  nError*Ki ;- v& m2 k( B8 G2 h7 b; D- ~
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    * n# s! U2 P  n2 F; Y
  41.     if(nError < 80 && nError > -80){; T& b  F( v+ {, }; l- y( y
  42.       if(nPower < MaxPower){
    ( X# K1 {0 {8 C7 g# d
  43.         nPower += 3;
    ; z9 F4 U+ ], B6 S0 b. W: ?- u7 \( F: b
  44.       }) \" o' ^4 `5 Y) ]" A
  45.     } else{
    # }) [  Y2 b" i9 E
  46.       if(nPower > MinPower){$ z! J! B( V+ u. @) j
  47.         nPower -= 2;
    ; U" V5 o) l" m9 m
  48.       }3 D# Y1 Z; D" I; I4 x7 m
  49.     } " X* N' V9 B3 p% H+ w. W9 a
  50.     MotoL(nPower-Error);
    ( A1 w+ ]# O: ^0 B8 b  ]
  51.     MotoR(nPower+Error);    + g1 Q, ]# t: |* I+ C5 r2 B
  52.   }else{" q- h7 J6 |; g/ u. X  G. ]( N; M
  53.     motor_9.run(0);
    % ^# W9 y3 G+ c3 k4 Z
  54.     motor_10.run(0);5 z4 E. [$ X9 R" w0 X% P/ q6 f
  55.   }
    3 F/ d3 g- m: w# N! [' P5 w
  56.   do{}while(millis() - StartTime < interval);$ L6 d3 O; K6 p% b% w
  57. }0 L( c# E* a. i/ ?

  58. ! k: h* f4 C" ?6 `# B9 B8 V
  59. void MotoL(int Power){
    $ h3 u, ]2 T$ l0 [# h, v( Y
  60.   if (Power > MaxSpeed){& W- ~" ?  j+ `2 }1 e* B
  61.      Power = MaxSpeed;
    % D" V* T0 ]/ {3 a
  62.   } - J  t8 n( B2 u- e% l
  63.   if (Power < -MaxSpeed){
    ; v6 Y. G, K3 d8 c1 U% N
  64.      Power = -MaxSpeed;9 }' Y7 O, h) |  M* E
  65.   }
    " A  r. f# D6 s; f# @& R
  66.   motor_9.run(Power);$ l" u; l9 A$ |0 ]8 p# c; ~
  67. }  
    $ x1 u$ m7 O8 m# [
  68. ( D) B& _  c$ z* ~& z1 S
  69. void MotoR(int Power){
    % P8 M7 B4 N3 M- m7 k5 k: r
  70.   if (Power > MaxSpeed){: R: P8 B$ l" O6 r- X; V7 F
  71.      Power = MaxSpeed;, e/ {& l$ X6 |! m7 F
  72.   } 8 X( i' g! D3 e- X: s- h* Q# }2 x! P# h
  73.   if (Power < -MaxSpeed){
    , [( a3 ]2 v( x, y! O# X: k1 ^  G( d
  74.      Power = -MaxSpeed;2 o- E" Q% ^5 _9 H7 p1 @( R
  75.   }
    2 M/ I5 R( d  T: z) t  M: }
  76.   motor_10.run(Power);
    2 d0 |- u2 }) S: R# F) E
  77. }  
複製代碼

9 z$ \  L6 T9 u2 `! x
  v/ `, v% q  W0 z  s" W; a$ T
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯 ; t: B; a, N+ v% T

  M: T. P% Z9 r$ }, W: E& V& }您好,不知是否能向您請教。
0 z6 w; U" Z+ I$ P3 e% @- U目前和宋修賢老師在處理Ardui Car  c  }2 z" x9 C8 B% S! t
雖然已使用較繁雜的方式處理了跑出黑線外的狀況1 _: F& B0 B# C4 P% E' {, B
5 [# @: y; r: G
但基於想追求更精簡的程式所以還是想請問一下- R# H# N7 K1 L( l9 F( W  f
就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝' R0 P3 F: ]4 ?5 G
不知道您是否願意教我可以如何處理1 t, x% I# [" T3 i* }+ ]- t
) `  f8 Q! T5 I' v) C; S

) V. J3 n; E$ F$ D# `+ P. r以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;
    , E* b; K6 H( C3 ?0 U8 e" K$ ]
  2. double CNY70Val = 1000;3 V' O* ?( d# Y; ]: o7 ~. W
  3. int interror = 0;5 M3 L% _! d- s& [* k
  4. int olderror = 0;6 Q8 q2 t5 C% w6 V8 G
  5. double values;) p1 K! X8 F7 D& V7 D$ O

  6. ) Y  i* @) r$ V$ Z( j) ]' @3 e/ p

  7. 5 C' B/ b$ X* u. K
  8. void CNY70()( x1 l* g! z* \  }' U7 k4 q
  9. {
    : h0 i+ _' x3 y9 V& o; X& J+ e
  10.   valuesRR = analogRead(RR)5 B/ @' C1 s6 {& q
  11.   valuesMR = analogRead(MR);, y, _' m3 \3 d: J
  12.   valuesMM = analogRead(MM);
    5 m5 `3 P7 y; C5 V2 ]% ?: |
  13.   valuesML = analogRead(ML);
    7 j  a8 ^& d; {4 ?, |' X" S
  14.   valuesLL = analogRead(LL);
    2 j6 r5 H- K2 X; s, O( O7 J
  15. 9 n+ I5 a4 Z7 x
  16.   if (valuesRR > CNY70Val)/ I# p" u7 H4 I& ~. G% ~
  17.     valuesRR = CNY70Val;7 C! J6 q3 L6 ]& T9 W9 D) z% @7 R
  18.   if (valuesMR > CNY70Val)& S; `- Y( [6 j2 a3 {, F8 M8 F
  19.     valuesMR = CNY70Val;$ M5 p, _; |; N0 l8 e
  20.   if (valuesMM > CNY70Val)
    4 I& A. R( F* h5 }  w+ z+ @
  21.     valuesMM = CNY70Val;
    / p* c1 r% ?0 Q( q. {$ C
  22.   if (valuesML > CNY70Val)" x3 t% T" }2 M4 X+ J  {
  23.     valuesML = CNY70Val;
    2 \# P4 D1 K1 q% ^/ D
  24.   if (valuesLL > CNY70Val)0 i0 ^1 Q% t% K! S. n- U6 ]
  25.     valuesLL = CNY70Val;% R" i) ]& f, {: x4 {
  26. ) j4 I9 b9 {( C6 n
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    ( S5 `4 H4 n3 i5 T( }3 K  i1 W
  28. }! F6 o& Q* x0 h" |' \5 S

  29.   O8 A) j9 E8 E. t8 d/ T' @! Y7 Q
  30. void Car()
    8 X# \4 U1 [/ X, f2 T. o. A) Q
  31. {
    ; R' @/ ]3 |5 O
  32.   while (1) {
    6 [9 M1 M4 u& i/ J3 I& C
  33.     CNY70();6 [4 k" O, f% `$ I# a$ D- t
  34. 8 Z+ [# N5 _& w% K
  35.     int error = ((int)values);
    - Y. T( g1 w0 O4 Y* s0 {7 N* M' w
  36.     interror += error;, N0 C9 G$ @, o' ]9 U  _+ G" R# O; u
  37.     int lasterror = error - olderror;
    & L. w  Q/ f! i" Q
  38.     olderror = error;
    7 A4 H  E1 e/ v7 r- m( w7 D
  39.     int power = error / 5 + interror / 10000 + lasterror;. U6 T+ {" g6 N$ B# W
  40. 9 s' k8 |6 Z( ~# H
  41.     if (power > MotoSpeed)8 r( j, O4 z# |7 b7 g& Q6 o, K
  42.       power = MotoSpeed;
    * V. }5 {8 b! _' Z  U
  43.     if (power < -MotoSpeed)5 c4 C/ w# k9 H7 B* k& u
  44.       power = -MotoSpeed;
    , A6 H, m  _6 u& {9 j/ l

  45. 1 a8 D1 C- Z4 }. Y5 ^* t
  46.     if (power > 0)
    * K5 f3 U& B6 v% W. u& }
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。
    # N9 L& b. o. H( m$ W/ K) E
  48.     else
    3 ]. B9 V/ @: B. n
  49.       Speed(MotoSpeed + power, MotoSpeed);
    0 d% m% v# ~6 w' W2 |$ `
  50.   }# p) Z3 B# D' D1 w- x$ @
  51. }
複製代碼

' d& s' M! d, V- T  H" a/ X' B8 d2 f$ f, W# O, e. i  p1 f! ]
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-12-2 14:53 , Processed in 0.026166 second(s), 17 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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