圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21866|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>
    0 @; R; `7 G9 h
  2. #include <Wire.h># A# n# E! ?- `
  3. #include <Servo.h>3 E2 X" Y, N9 z% O3 [$ X
  4. . P# B1 O; u) Y* k
  5. #include "MePort.h"% G, i. |  v0 l
  6. #include "MeUltrasonic.h"
    4 R  D6 [4 V4 x/ s( Y8 i8 S+ w
  7. #include "MeDCMotor.h"
    . I; ?$ Y' L, [: J! a/ q
  8. # R9 c" {/ v" p  W6 V! T! s
  9. //double Input, Output ;
      Q2 t) E8 w1 K5 P4 j: K5 A
  10. float MaxSpeed = 255;
      i$ ^; ^; }8 n' O$ u
  11. float MaxPower = 180;
    5 A+ o! d. c7 R  z
  12. float MinPower = 120;+ a" o9 m% B/ w( A9 s
  13. float Error,ErrorAcc,ErrorDec;; f' c6 {( X% A, @- M5 W. O* x6 ~

  14. % B9 |( n& g9 o8 L3 f! F5 _
  15. float Kp=0.14;
    ' t+ B+ Q6 h8 X7 ~) E! P) z
  16. float Kd=0.00020;//23;
      L8 u! O. D; I
  17. float Ki=0.000201;2 j# o- j% @2 y" u( m: e2 ]/ ?: `
  18. % b& t5 \9 _  J4 X9 o! j
  19. float nPower;
    * k7 e/ ]/ h+ R: Q! Q" F
  20. MePort lightsensor_6(6);9 \  C3 S) r. g7 @. ~: z6 Z. ]# D
  21. MePort lightsensor_8(8);1 B; ~$ v* j* d0 |( l. C1 w
  22. MeDCMotor motor_9(9);2 i. n5 K* W2 P3 o9 p
  23. MeDCMotor motor_10(10);
    $ o) a* X( ^8 V- i7 V6 d
  24. unsigned long previousMillis = 0;3 m- C* F, Q# Y) Q4 \
  25. const long interval = 1;. h+ b0 n5 D/ E1 J. R
  26. $ U& i5 c+ i3 @% [
  27. void setup(){
    ; {6 I) K1 e* V2 F! i% w
  28.     lightsensor_6.dWrite1(1);
    ' m- s/ v5 v$ h1 z. H
  29.     nPower = 160;7 @# ~- B4 H) ^+ U, M5 R- A
  30.     Error=0;4 k9 e# S2 d7 s7 O( S
  31.     ErrorAcc=0;
    ' ~" Q2 v9 f2 \/ T0 w8 A7 ~) i
  32. }+ `! t: S0 r  v6 X2 i3 Y
  33. & c! q$ p5 U$ i# C' i
  34. void loop(){
    $ s- V- J, |; ~8 G3 c! x5 Y
  35.   unsigned long StartTime = millis();5 n) T& W& W8 z4 `
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){% |: F3 p  Z, Y0 s9 q$ B
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();) a0 R( e% N/ K; S: r
  38.     ErrorAcc +=  nError*Kd ;
    5 y- @" `( O& z0 ~$ H  e! H# K
  39.     ErrorDec -=  nError*Ki ;
    ; U' y! p* ~9 t% ]' s1 s5 n6 x( ]
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);- g& `7 x  w- v, Z
  41.     if(nError < 80 && nError > -80){' e" q3 }8 x: M1 h5 s# b
  42.       if(nPower < MaxPower){
    + {! n( }8 I/ B* J
  43.         nPower += 3;
    7 i, V4 P8 Y9 ]# m# \
  44.       }9 t& n# S0 j1 R9 \3 I( g1 q
  45.     } else{! X2 _' @  V- ]$ {  `7 x
  46.       if(nPower > MinPower){
    * L3 [/ e8 j4 w) T
  47.         nPower -= 2;
    & n4 Q3 @/ o* ^  {& V' w( P! t
  48.       }
    7 L6 K! E) u1 N, A
  49.     }
    3 P2 f# n2 P( y& t
  50.     MotoL(nPower-Error);3 k0 T% S9 @$ |8 z1 w9 U" ?3 y
  51.     MotoR(nPower+Error);    ( S: ^7 I! s1 k. S$ s$ g
  52.   }else{
    ( j; t2 ~, G5 J) j; \
  53.     motor_9.run(0);: F. W' `2 |# d" V( J+ {* E) d
  54.     motor_10.run(0);' F/ y0 q, U& R' w* J
  55.   }
    9 o. M6 H8 J6 t5 x' ^6 f: [
  56.   do{}while(millis() - StartTime < interval);
    7 e: x+ [; B, [4 i9 ?
  57. }* w: j% g& ]. b+ ~$ z' e8 O7 K* V- {
  58. 5 W6 P& [/ C: @% z) Z; e
  59. void MotoL(int Power){5 T+ u% _! a/ ^* B
  60.   if (Power > MaxSpeed){
    % i! N/ Z5 Y# g3 Y" W
  61.      Power = MaxSpeed;: ]- v  u2 N8 O& ^2 a/ e$ o
  62.   }
    4 d) R0 a4 O7 O
  63.   if (Power < -MaxSpeed){
    , y3 e, F( k* D' j8 P# O! E
  64.      Power = -MaxSpeed;
    3 n9 R$ @$ f" z' c/ W6 m
  65.   } 5 m: V$ \& s9 ?
  66.   motor_9.run(Power);
    & y4 U% c) l! e4 ~
  67. }  $ ?4 z- R6 T8 ~' B

  68. , F" s+ {' n' Y8 q, p# p
  69. void MotoR(int Power){, L2 \. p9 a, N! r, y. p% C, A
  70.   if (Power > MaxSpeed){
    3 r7 \! |3 I5 E
  71.      Power = MaxSpeed;/ c$ E" _2 f, z# t
  72.   } 1 r6 T, e1 U$ S3 t% r& b, t3 E
  73.   if (Power < -MaxSpeed){- s# ?. d0 _/ m8 `
  74.      Power = -MaxSpeed;
    & |' |1 T9 G( ^6 O
  75.   } $ |9 Y! }+ [7 x7 p1 n
  76.   motor_10.run(Power);8 d9 M( J5 j6 O, ^
  77. }  
複製代碼

: ]# E# P" k) F! m, M  F% I' A- ^6 l7 {
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯 : Y! Z1 _5 O# g+ }- x8 N

+ V8 A+ l9 T% i  |9 k您好,不知是否能向您請教。
0 p4 j6 l6 a! M  k/ i9 }- _$ K目前和宋修賢老師在處理Ardui Car
. Z% E  [5 n, z. L雖然已使用較繁雜的方式處理了跑出黑線外的狀況& N( w5 t  T1 X1 L3 j$ v2 f
8 ^5 K. L4 |. }! s
但基於想追求更精簡的程式所以還是想請問一下
9 G- r& `: [" p7 s: ~就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝  O: ^# }) U1 ]7 r% f
不知道您是否願意教我可以如何處理
: \: N* _" I5 p! E) l
6 z* c7 @# W& w4 i. ~- Y; j5 n
以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;* [& ]1 x$ K0 I. h+ M
  2. double CNY70Val = 1000;
    + M$ ?6 z1 n2 [4 H9 `3 _1 b7 m  A
  3. int interror = 0;1 _) R/ g! o% Z9 P1 E2 e
  4. int olderror = 0;
    9 f1 ?% O4 a8 _& `0 O( [4 N
  5. double values;% G1 w( j* \* {. H0 Y( x
  6. 8 A" z, g8 B: t( K

  7. ; a9 |, @0 `, F' A8 k
  8. void CNY70()' e+ }+ P( `0 C" B! s6 I
  9. {' v; h; \1 n9 X# _' d7 K1 t
  10.   valuesRR = analogRead(RR)  S& f  S2 m8 M- r' K  z  O9 P
  11.   valuesMR = analogRead(MR);
    + U% S. C5 T- B* E5 N* j
  12.   valuesMM = analogRead(MM);' x; m& ^) I$ {( W- G; c2 R, [
  13.   valuesML = analogRead(ML);
    & n. ?5 m* Y% p
  14.   valuesLL = analogRead(LL);
    & @4 q2 [7 B* E% }6 {" X

  15. & ?2 O- b* X3 w5 J& x7 _
  16.   if (valuesRR > CNY70Val)& t, ?' d1 M" g! b0 \
  17.     valuesRR = CNY70Val;
    : d) A+ J% T. B9 h) ~; s/ ?: ^
  18.   if (valuesMR > CNY70Val)
    9 ~7 W& F( g1 H- T. a
  19.     valuesMR = CNY70Val;
    9 j' v, ?* H: L: Z- V* A# g4 p, f
  20.   if (valuesMM > CNY70Val)
    5 U3 z; W/ I( H1 N/ D4 j
  21.     valuesMM = CNY70Val;
    0 O! {. J- |6 l
  22.   if (valuesML > CNY70Val)
    4 y1 d, C4 D; w/ o
  23.     valuesML = CNY70Val;1 R. r0 B) V# P  s6 E" F
  24.   if (valuesLL > CNY70Val)
    % [% ?* z5 U$ d. x# e6 O6 _
  25.     valuesLL = CNY70Val;! U" B. E" h1 V5 e

  26. , {1 E  k( _! y5 o" i
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;# \; D/ a: _: h
  28. }
    * m" k7 C; W5 c# c! @  ^$ s
  29. 2 s, t  [% `- U  Q& L& E2 R
  30. void Car()
    / J0 k; y, O1 z! |
  31. {3 ^2 l* `2 F( b7 n+ H: V; E$ P
  32.   while (1) {
    2 Q! ^4 q) a6 j! s* g0 _! U
  33.     CNY70();
      |) M- b4 j, J
  34. 9 ?6 f! J# v# v: D% ]
  35.     int error = ((int)values);: e' y# o8 q* A  N9 ^
  36.     interror += error;- d1 O) e# k3 O) j7 `
  37.     int lasterror = error - olderror;
    4 @' t' F+ A. n: d
  38.     olderror = error;
    3 \% O# X8 L" C5 o
  39.     int power = error / 5 + interror / 10000 + lasterror;7 g" o5 [4 ?& {5 ~" }

  40. 7 r9 w& H; T7 ?2 B5 {. a
  41.     if (power > MotoSpeed)
    ' D) e3 w8 z9 g& V0 x
  42.       power = MotoSpeed;( i" k. {# r4 t3 K' w5 K; B( Y
  43.     if (power < -MotoSpeed)
    ! Z4 b8 L5 Q$ w& ?
  44.       power = -MotoSpeed;/ J/ v  \# z* W

  45. 1 Z( A% V, ]( s! R; |/ w6 d8 l
  46.     if (power > 0)8 C/ W# o" f0 t$ q
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。
    3 V5 O) v0 L& D
  48.     else
    / f4 C. Y/ L* E5 Q
  49.       Speed(MotoSpeed + power, MotoSpeed);
    - f8 [- c3 t1 Y( ]! r2 T# c( V
  50.   }1 M3 V( g; m0 S! I
  51. }
複製代碼

+ O, P% S3 e1 {8 [! h; {
) Y  k/ z4 m& B5 I- l7 w) B
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-12-6 20:37 , Processed in 0.024167 second(s), 18 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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