圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21791|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>
    ; F' x- B" F3 @
  2. #include <Wire.h>8 L) z; e% F0 V
  3. #include <Servo.h>" L4 A, J# B) B7 W# B
  4. : o# W3 [  C; L* ?
  5. #include "MePort.h"! t$ `; V5 {& z2 B- L
  6. #include "MeUltrasonic.h") G) `& `) [' b' E
  7. #include "MeDCMotor.h"
    0 Y2 O, K$ i3 M1 q2 J) J/ S/ B9 L% p

  8.   i; [- l! ^. i9 S$ O
  9. //double Input, Output ;4 ]7 k% r( Z# Z/ |
  10. float MaxSpeed = 255;5 {" f$ M, d7 h$ L
  11. float MaxPower = 180;
    , W9 `. f9 M7 L# w
  12. float MinPower = 120;6 Z2 e) [  g& b: B3 e  J
  13. float Error,ErrorAcc,ErrorDec;
    9 D# ^( |) ^& M' b
  14. ; B+ L& z  I# K
  15. float Kp=0.14;1 Z. Q' e7 @+ v0 f; v, d9 M* X
  16. float Kd=0.00020;//23;
    5 y3 |: C8 u: e0 [/ t
  17. float Ki=0.000201;
    2 b% ?5 O& M( `
  18. 5 Y5 |- _. @/ Z( o4 }
  19. float nPower;
    1 m" A% g; j2 }, w  z
  20. MePort lightsensor_6(6);  U! I9 k6 h7 B' g% B8 W
  21. MePort lightsensor_8(8);% E; T; C2 q9 T& z! z5 T
  22. MeDCMotor motor_9(9);
      r9 [/ \0 f/ e4 W+ n: N
  23. MeDCMotor motor_10(10);
    : c7 J' w. F- h- d& }) q
  24. unsigned long previousMillis = 0;
    ! i' D7 R8 H2 t- t# d$ z
  25. const long interval = 1;
    ! P/ W# l; }- d  V8 \
  26. ) X, g+ C( k( H& P; n/ i
  27. void setup(){
    $ |! r; T2 @8 a4 Q* z+ W
  28.     lightsensor_6.dWrite1(1);
    ; ^- a, \5 g3 C: s- a9 S
  29.     nPower = 160;9 k8 P9 q. J$ i3 t, Y
  30.     Error=0;- d. Y0 [! I. O9 p$ U
  31.     ErrorAcc=0;
    $ [! r& }' [0 s' h6 m( s
  32. }2 R2 L. P" \' v
  33. 1 H9 w8 |, W! |( a$ E( v
  34. void loop(){8 P/ F0 {* H% m, |
  35.   unsigned long StartTime = millis();
    " \, l4 [, p- O5 X; J1 ^
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    ; e$ B! G2 t7 ]) m
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    - p$ [& _: T5 O) g
  38.     ErrorAcc +=  nError*Kd ;, E) N6 y6 ^, T' `$ e3 h
  39.     ErrorDec -=  nError*Ki ;
    5 l! n& U9 S( T8 H) W. w; K1 B
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);+ o& o$ h* _- I* f, x+ k7 R
  41.     if(nError < 80 && nError > -80){, [: \, k! M+ z, w
  42.       if(nPower < MaxPower){
    0 o) h; ?, A- B8 X) j
  43.         nPower += 3;
    ; @, y. R- k5 N9 a+ s3 Y+ M
  44.       }
    / b9 b- D5 f) y, {8 C% b5 G6 O
  45.     } else{* b5 \; _7 b8 e, b0 c( F
  46.       if(nPower > MinPower){+ F4 h* L0 i% I! c8 `1 w
  47.         nPower -= 2;; K' ^6 M* F/ A5 _7 l8 E
  48.       }, F; D$ }8 P; E' R/ h& r5 k
  49.     }
    2 u9 c8 e! \" l
  50.     MotoL(nPower-Error);
    ; V" b  }2 N9 c( w
  51.     MotoR(nPower+Error);   
    7 Y9 _. i# {; V4 z* l1 w0 i+ F/ P# }
  52.   }else{
    ' s6 b( R% J4 g/ Q
  53.     motor_9.run(0);
    , u: }& r# h# O/ S. }9 I, p
  54.     motor_10.run(0);
    . E/ r+ [6 o( g6 R& p/ f5 G/ F
  55.   }6 h& _/ ~- L# R) {
  56.   do{}while(millis() - StartTime < interval);9 A7 q1 K' T6 H) Z7 W* t
  57. }( B6 J8 r7 A- L& c5 X4 f
  58. ; J5 i' c! {$ L" c! @; V
  59. void MotoL(int Power){1 ?$ N- J8 R. w7 _! V' ?" i3 z
  60.   if (Power > MaxSpeed){
    ; J4 l, ~( E- ?4 s9 j' {
  61.      Power = MaxSpeed;, w- \& E$ A# @
  62.   } / `9 O/ g* X  Q( r& b) ]3 f
  63.   if (Power < -MaxSpeed){% N  i$ [! A, ]' \' i1 @; S5 g  s' U
  64.      Power = -MaxSpeed;
    ) [8 @( F7 C- N. ~$ p1 ~
  65.   }
    0 V- x: r$ v4 ^4 z% _/ y0 ?! W
  66.   motor_9.run(Power);+ G" u8 s; K' i4 q5 L; C3 Z3 p
  67. }  
    9 P* a- g# S* a, Z; T* E6 J6 n
  68. 0 K/ J8 R/ j9 M5 r3 t: b9 W% r" Z
  69. void MotoR(int Power){
    - t5 t$ D/ h' {/ Z$ Q$ h
  70.   if (Power > MaxSpeed){
    1 h0 @6 Q) `3 b' n7 I
  71.      Power = MaxSpeed;
    6 P& F# f- f6 w) b3 l" y  I
  72.   } 5 ~7 p/ O+ g7 U4 P$ R9 }* w2 G
  73.   if (Power < -MaxSpeed){! B" m8 a$ [3 \( N: s1 u$ B
  74.      Power = -MaxSpeed;
    . k% h3 y1 h, W
  75.   }
    3 t0 R) Q9 j* o+ w8 _3 G
  76.   motor_10.run(Power);
    5 h3 Y% r* i/ Z6 m+ Q0 F0 M( o& W
  77. }  
複製代碼
+ ^' v8 j! R% m) a% V

+ V. P$ p# z$ S! z. J8 ?5 K0 G. c
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯
. V; m, R' ^, L3 b+ P
8 I+ E6 }9 M  U: \. g% ?" i您好,不知是否能向您請教。* U! W4 f3 T  v& m8 Z8 u0 X0 B
目前和宋修賢老師在處理Ardui Car
* J5 J2 H7 z7 Y' l7 f  K1 E雖然已使用較繁雜的方式處理了跑出黑線外的狀況! M/ [2 a* M, y% |; o
# ?/ k& x, ~% c+ x3 G$ C4 Q
但基於想追求更精簡的程式所以還是想請問一下) ]" P8 |# e: _& t$ D- d8 T
就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝8 o/ u* Z5 ^2 T2 ]! Z2 ~
不知道您是否願意教我可以如何處理' Q  o/ z4 ?& ]* `( [+ O

+ ]7 }5 T! p  g$ c& R/ ?  J* `4 q3 J3 i8 L" ]* k2 [
以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;
    ! M+ Q. M" ~9 Z! @
  2. double CNY70Val = 1000;
    0 |+ ]% s4 ~& |1 }! V
  3. int interror = 0;) }& O: b+ x3 ]' p9 ?
  4. int olderror = 0;* H- L6 q1 G# h9 r0 S6 S
  5. double values;
    " O# B$ _$ r3 a
  6. 0 [' `2 s. ~, d- I# z

  7. + b4 N* r$ H2 v& T0 b1 W$ x
  8. void CNY70()
    # ]1 a, J8 O1 b( M7 L
  9. {: B, @2 B0 A  L6 ]% _' Y3 V
  10.   valuesRR = analogRead(RR)
    . U9 V- L* N& O4 `4 y8 B- k1 B/ o8 w
  11.   valuesMR = analogRead(MR);
    6 b9 r/ e* U5 R$ Z
  12.   valuesMM = analogRead(MM);
    2 z; j5 F- Q; M! \* R; m6 \0 N
  13.   valuesML = analogRead(ML);
    0 H8 Q/ H% h2 x- [) b
  14.   valuesLL = analogRead(LL);+ W' t* Z/ w! z6 V

  15. % W" B9 Y7 J% {; i! Q9 D( w
  16.   if (valuesRR > CNY70Val)
    ! ^7 b) [# t! k6 \9 y2 x4 N% B
  17.     valuesRR = CNY70Val;, K( d' Z* B3 i' t0 @+ w. L2 h
  18.   if (valuesMR > CNY70Val)" ~! R1 q; K+ J
  19.     valuesMR = CNY70Val;
    ' R6 j: H! i6 @# V( S$ c; n* ]3 N- n, v, h
  20.   if (valuesMM > CNY70Val)
    ! P/ C- k5 d# Z4 _+ V
  21.     valuesMM = CNY70Val;
    3 U& H% ]  v% M) Z+ D7 ^2 S
  22.   if (valuesML > CNY70Val)3 m" w/ M6 b6 n: j& I
  23.     valuesML = CNY70Val;
    4 `/ m, S- s- w0 _8 L' Q
  24.   if (valuesLL > CNY70Val)
    ) |2 @3 B* R' I
  25.     valuesLL = CNY70Val;
    5 j& Y( d) H! _+ C' G  |
  26. # s; J$ v* s% E# }, h
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    7 k" U. {. `. S! b) W/ p
  28. }
    3 u3 Z- }! K7 C" Q. l6 o

  29. 4 ~5 d2 ~+ G' ^9 i. y
  30. void Car()
    6 G8 R6 b6 A. Y" @# ~$ ~6 ~0 E! L! F
  31. {
    3 F! @- N6 P1 ]9 e
  32.   while (1) {3 w! X6 q# i3 R4 u+ w& m0 C1 p
  33.     CNY70();
    ) ]0 t6 w- ?1 ^+ c* t
  34. & p9 c# H5 E3 U* ]1 ?
  35.     int error = ((int)values);
    ' J! A3 n) F, T' x8 @  r7 t/ [: x
  36.     interror += error;, n# q5 B+ c, X5 s4 F
  37.     int lasterror = error - olderror;
    8 `5 c2 H+ Z; P* E2 |* `( j
  38.     olderror = error;) [6 q% D! R! P9 @: r5 f
  39.     int power = error / 5 + interror / 10000 + lasterror;. l% |$ y2 n5 P7 f, N) e2 n7 K
  40. 6 g( h/ o6 `/ C' I* a2 t; u
  41.     if (power > MotoSpeed)7 V# g, d6 K- U( N, V7 ~5 X9 m, Y
  42.       power = MotoSpeed;1 @6 c1 M5 n2 V0 \  _
  43.     if (power < -MotoSpeed)
    : E8 P% @6 O) a
  44.       power = -MotoSpeed;
    $ A& X' `' r7 e

  45. ) T9 G% h' w( u+ M5 a* L" M
  46.     if (power > 0)
    8 A/ s: f- m! Z" l. n7 ~
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。
    ( Q/ P2 E7 U$ J( M" s/ [- T2 r
  48.     else
    ! R3 A# _8 e, f7 V
  49.       Speed(MotoSpeed + power, MotoSpeed);5 d, G4 w5 H4 m' R' U/ c
  50.   }
    ! y, k( i( g4 K" D0 H- ~1 d7 J
  51. }
複製代碼
3 U( [. N$ F& N2 m+ g# ~: R% L

6 ?+ s2 y4 l3 o6 f7 S  L
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-12-1 06:02 , Processed in 0.020766 second(s), 17 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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