圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21826|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h># P& Z5 X6 ^' `9 |. h" I. @, e
  2. #include <Wire.h>
    : C% e  G" i8 A
  3. #include <Servo.h>
    * E- ]: R) c9 L4 ~: R
  4. " c( S9 }# l1 y5 n5 V; q
  5. #include "MePort.h"
    3 f( {3 N- i" N( _& l( T! a- r
  6. #include "MeUltrasonic.h"+ g! H$ W+ b# d! G1 T" d- K
  7. #include "MeDCMotor.h"8 |9 a8 C' d$ Q4 q4 ~
  8. : U6 _3 k1 ]  X
  9. //double Input, Output ;
    - \( D; m9 s; F( p0 A! |) f
  10. float MaxSpeed = 255;  i$ L7 J( I+ b( I4 E/ S; N
  11. float MaxPower = 180;
    . r0 y5 u. p+ Z: G! W/ r; D
  12. float MinPower = 120;
    + \' F% Q* U3 U/ W; I1 m/ c
  13. float Error,ErrorAcc,ErrorDec;# T% L  G' N  j0 u* R
  14. ) u* V0 Y1 _. G. M3 ?1 q$ i0 a' b
  15. float Kp=0.14;
    % v9 l& Y6 _7 h* G& Y* t7 a) I+ H
  16. float Kd=0.00020;//23;
    4 s) v! ~, ~! v* h( b  h
  17. float Ki=0.000201;
    & c4 [) j# k. M$ y' i( ?, H, |( z" }

  18. 2 U/ s# g3 p; [* `7 m! A' |
  19. float nPower;" i9 v; q# J0 z$ s8 b9 w* j1 |( j
  20. MePort lightsensor_6(6);  E! u5 @9 C& |5 p; b
  21. MePort lightsensor_8(8);$ W, \+ b1 p2 p
  22. MeDCMotor motor_9(9);; }/ X+ K/ x3 m- e
  23. MeDCMotor motor_10(10);) X+ w. k; Z3 T$ _$ r$ X  ^
  24. unsigned long previousMillis = 0;
    6 @3 s" s, [9 b3 F. Z
  25. const long interval = 1;- g0 T% F" Q+ [7 B% A! P
  26. 5 o$ t; u2 m5 L. K9 q
  27. void setup(){2 K; |; `, Q: t& D3 D
  28.     lightsensor_6.dWrite1(1);
    ! b9 T& B; T+ b' E* k2 x1 L
  29.     nPower = 160;
    ; A5 x7 L; Y5 b3 \" E& N$ T7 r0 f  ~
  30.     Error=0;
    2 H! P% W2 Q7 n6 i0 v
  31.     ErrorAcc=0;
    " Z9 {$ }. `3 ]5 l' r0 J
  32. }
    " c) \2 `+ A# n& u& K! V

  33. 2 V+ ^: g0 @* V# \: ~0 N
  34. void loop(){
    + E# V0 N$ @- U1 {( n0 E
  35.   unsigned long StartTime = millis();
    5 ]. A- F: K7 {" |' H+ R! H
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    9 A+ r& O* K% r! V7 [/ y5 E3 O
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();) _2 D1 v/ V, u* E- n# O5 s6 ?* `
  38.     ErrorAcc +=  nError*Kd ;( Y6 n/ g, k/ f# ]  E0 \8 [
  39.     ErrorDec -=  nError*Ki ;( W8 M5 {2 x9 _3 P  k, W; r- E% m
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);$ I( ]. V* F3 T+ C, O/ Z0 |
  41.     if(nError < 80 && nError > -80){
    + e: Q" T$ a; e; t# h2 \
  42.       if(nPower < MaxPower){! l7 f3 ~; C( X+ h
  43.         nPower += 3;! N* u+ E" u% A8 }0 P8 h  Q
  44.       }
    6 ]2 C& e+ E0 e- t5 E. g
  45.     } else{
    & m# u5 B' X) k! u: p
  46.       if(nPower > MinPower){
    ) P9 E, v" B# z# f2 W: d" g6 N7 F
  47.         nPower -= 2;$ `5 ]2 a0 |8 J) C
  48.       }
    7 h0 ^' f! u! e, s7 a: }+ s
  49.     }
    + M+ l6 h9 \6 _* f
  50.     MotoL(nPower-Error);, C; j: q9 B8 V; z
  51.     MotoR(nPower+Error);   
    - y: l' V9 Z2 f: x# ?+ b
  52.   }else{9 b  o% D/ a4 Q% L: K. r
  53.     motor_9.run(0);
    ) x* f& v1 a) w% y" M; f+ Z# }6 ?. u5 t
  54.     motor_10.run(0);
    ' R0 [$ t/ ?3 X$ j0 c9 T
  55.   }
    7 i2 o, W, w% T- {" u6 A
  56.   do{}while(millis() - StartTime < interval);  P2 |5 G7 i7 L6 g
  57. }
    . p# ~; i  @  m
  58. - F0 y% I0 ]$ a$ M* i; G% U
  59. void MotoL(int Power){; J4 |+ @# j# M8 s
  60.   if (Power > MaxSpeed){
    * r, R+ R) G! u/ W
  61.      Power = MaxSpeed;
    ' z9 h3 ^* H) |
  62.   }
    5 @3 }# a& H# g2 R
  63.   if (Power < -MaxSpeed){
    ! B: T# `+ V) |, f* Y) @
  64.      Power = -MaxSpeed;* a6 ~! e2 t* Q( r1 O
  65.   }
    ( v( ]8 n( V$ K! G( z: V
  66.   motor_9.run(Power);+ p, O, s! D1 v$ ], y) Z
  67. }  ; I% j6 ]" u/ x* ^

  68. ; p+ x4 X$ C+ h
  69. void MotoR(int Power){
      i3 ]8 ~) I6 ]6 `$ V
  70.   if (Power > MaxSpeed){
    ( }. i9 `9 m/ ]( J5 n2 P4 ]$ ]4 s- T* q7 M
  71.      Power = MaxSpeed;
    1 F9 f5 Y( ~+ v- _
  72.   } : q0 F+ g' S) o# \% d( Q
  73.   if (Power < -MaxSpeed){- ?" _+ w2 ?" H+ C2 P9 k
  74.      Power = -MaxSpeed;
    # Y, M3 @9 F  s& `, G. Z
  75.   }
    : I; A" r4 J2 g
  76.   motor_10.run(Power);
    6 S5 g; s8 I: t
  77. }  
複製代碼
: t1 ^: q# T4 P1 [  B" F
+ @- r1 e: B, l* J
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯 $ B. N% ?/ Z' w+ m8 j
% w0 J- Y( w8 x$ z7 I3 K: n
您好,不知是否能向您請教。
" B0 `- @* N7 H$ n, n目前和宋修賢老師在處理Ardui Car
' q3 t  m( w2 F雖然已使用較繁雜的方式處理了跑出黑線外的狀況: i3 s0 Y8 _) H/ x! m; ^

! y- H+ ]$ k4 o+ i- d但基於想追求更精簡的程式所以還是想請問一下+ Q9 o" e1 J5 W1 h- H+ T0 b- j" N
就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝3 K2 s" s, l2 K: U# o
不知道您是否願意教我可以如何處理6 z* `, G% k# j! t& H! H

4 T! I+ m& K8 }
, @: m; ?% G7 O8 I以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;$ C6 |9 j3 U* U- Q5 F
  2. double CNY70Val = 1000;( o& }! [& Y" p3 k( |
  3. int interror = 0;/ _; D- _% T! Q7 H) {; U
  4. int olderror = 0;0 P+ y. X9 n# z9 L# K7 v
  5. double values;# d' M/ q& u1 c5 \8 e, p& v$ Y

  6. ' |2 n, l' {8 C' G( Y2 D
  7. ( O0 O: i+ y$ u* s
  8. void CNY70()
    7 L& x6 x, A# P- v- n" ^! ~' k0 f
  9. {
    8 K* `  A# a& O) u6 u9 T3 I* K+ ?
  10.   valuesRR = analogRead(RR)
    / Y1 F- y, f- r, f
  11.   valuesMR = analogRead(MR);
    ( y5 e) r( L3 o9 K: ?
  12.   valuesMM = analogRead(MM);
    1 m8 [$ S8 l. R7 }
  13.   valuesML = analogRead(ML);# k$ c4 j8 W1 X* \: J2 H
  14.   valuesLL = analogRead(LL);$ A" a- j5 \$ V7 m

  15. ' X( G9 z5 _- z5 x& W' d5 h
  16.   if (valuesRR > CNY70Val)# g' @" f- V5 F9 F9 {' u+ A
  17.     valuesRR = CNY70Val;4 Q/ G: ^9 R! C1 @( B9 L
  18.   if (valuesMR > CNY70Val)8 ~2 A) X% P6 p/ U" l* D+ e
  19.     valuesMR = CNY70Val;
    6 X% t2 g$ G2 Z; R  B
  20.   if (valuesMM > CNY70Val)
    . ^2 S% P2 r& m1 p$ I) [1 L" }
  21.     valuesMM = CNY70Val;. I$ y% x1 d& f
  22.   if (valuesML > CNY70Val)
    ; w! S+ w4 O& J. t, O
  23.     valuesML = CNY70Val;! m- X3 t* e# D+ G% _+ E
  24.   if (valuesLL > CNY70Val)
    8 R  g( M" z, o( y: o% I
  25.     valuesLL = CNY70Val;  P# _  N  \6 s% Y6 n
  26. - m9 {. V  @# B: O
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    * ]8 f$ h' o5 `, Q9 M1 ]8 P
  28. }
    $ _( E6 R# ^# {5 E9 {: p3 w
  29. 5 H! t# ~; V* p4 K$ k" [4 D) `
  30. void Car()9 b9 a4 c: ?+ }8 D1 Z
  31. {: d, q" D: P" R( I- j- E: P1 t
  32.   while (1) {0 y/ s. E# X4 K* V
  33.     CNY70();( M/ I2 T7 T0 _  E9 X: F
  34. $ a) Q1 g' p# [6 M" I$ g
  35.     int error = ((int)values);
    2 Q+ f: B! H6 e# v+ A& a" a
  36.     interror += error;
    ; h) f7 l8 }7 n) X9 W$ _9 L
  37.     int lasterror = error - olderror;
    , A* x2 E6 ~0 N0 a% L& T0 e; g
  38.     olderror = error;
    2 ?9 q0 ]0 r3 @( f! K) ~/ g- S9 U7 R
  39.     int power = error / 5 + interror / 10000 + lasterror;
    ! j8 e+ D  D) m+ z' ?6 \# e5 P

  40. 3 C' _1 L3 A2 z, h& J" A8 x  o
  41.     if (power > MotoSpeed)
    5 @1 H& ~1 e5 R+ w4 h# Y% s0 r/ D
  42.       power = MotoSpeed;: Y; b0 h, v7 Y9 O. _' j
  43.     if (power < -MotoSpeed)% X* e9 }8 o+ G7 T# z
  44.       power = -MotoSpeed;
    # x8 z  A- D5 M% U7 N& c/ |% K

  45.   s1 s0 X0 p, A/ G6 X& t) t
  46.     if (power > 0)
    7 i. `5 }, _7 c* ^9 C5 c6 V& H, C
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。
      i% h/ s& C& \9 n
  48.     else
    7 [2 X) d" J0 j" s4 \. y
  49.       Speed(MotoSpeed + power, MotoSpeed);
    3 L* d8 U* c: D: V. W7 {$ Y
  50.   }
    4 C0 z5 w+ \0 n1 h' K
  51. }
複製代碼

% Z4 Y3 B! L2 g& H  I6 Z% a  r0 L# D  ?/ x5 ?8 W( V8 g
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-12-4 05:33 , Processed in 0.020659 second(s), 17 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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