圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 19934|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>
    $ N, O+ C  Z& }# m7 ?  O8 G" S' L
  2. #include <Wire.h>
    $ s: ]7 Q) A- u& ^% P( F% F+ k
  3. #include <Servo.h>
    % ]3 R' i  v2 M6 Y
  4. 9 [8 }, T: r% r5 |
  5. #include "MePort.h"2 z2 P* ?7 Z+ H% d3 r" X
  6. #include "MeUltrasonic.h"
    # T8 u2 y7 r5 |: k
  7. #include "MeDCMotor.h"
    3 s' C0 d( H) p5 ]( A+ @) h3 b: z

  8. . l* ^( |3 X# T
  9. //double Input, Output ;* i/ ~+ S4 }' G6 t" Y
  10. float MaxSpeed = 255;
    * Z; V4 ~& G; V" Q9 m! `7 P
  11. float MaxPower = 180;
    , ], P  K8 g5 `$ y
  12. float MinPower = 120;
    . s5 P2 [- n9 u& a, l- ^
  13. float Error,ErrorAcc,ErrorDec;) {* W/ m& m* J0 I/ I5 c

  14. ! f7 r1 w' }" d, H. ]7 \" l+ i
  15. float Kp=0.14;) @% S0 j+ x3 V' |/ X4 d% K
  16. float Kd=0.00020;//23;: N5 Q8 k; P3 @. n7 @
  17. float Ki=0.000201;
    ) o2 ]5 R. P/ {5 m' G' R
  18. ) O8 t' }3 y, F; B4 I6 F
  19. float nPower;3 T7 s+ t8 W% z! e0 o
  20. MePort lightsensor_6(6);! I$ v: X# J% X+ C% S- C
  21. MePort lightsensor_8(8);
    5 H! Q8 e. ^& I4 `) B6 |+ m& R
  22. MeDCMotor motor_9(9);
    ' A0 C! H9 M. K2 \3 c/ ^& O' ?
  23. MeDCMotor motor_10(10);
    $ Y8 h$ S0 I: t& c
  24. unsigned long previousMillis = 0;
    , g; d% _+ f" d
  25. const long interval = 1;) Q& H2 D) H8 g; X! J
  26. + T& n7 q% I9 y5 e3 F2 H5 {
  27. void setup(){
    - H  y. K! \# E7 z6 q6 ?; R/ a# |
  28.     lightsensor_6.dWrite1(1);
    1 h6 P8 C- s5 |/ M
  29.     nPower = 160;
    ) D( O' ]4 u4 d# k8 d& F
  30.     Error=0;
    ' D# ?' D' }, o0 N( t" ^! `
  31.     ErrorAcc=0;/ g1 d+ j$ U' a1 `! k* Q
  32. }
    ! s9 w8 s6 z3 E! W
  33. . _8 ~! n2 V, _9 d' H0 B6 i: {- j
  34. void loop(){
    ) f+ n0 P- w0 j+ \5 [" s1 a" G
  35.   unsigned long StartTime = millis();$ J2 Y6 o- ~% \8 E4 [: W' _4 C
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){- T4 m* J( X6 j. B9 i- w
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();8 A6 L3 c' ~3 W( F
  38.     ErrorAcc +=  nError*Kd ;" O: a/ _- n/ o, u# b' m( t
  39.     ErrorDec -=  nError*Ki ;
    3 x1 d( ?  i, ?# V1 e) }2 p
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    1 e- F. y8 g- q) k$ l+ m
  41.     if(nError < 80 && nError > -80){
    , T) Q9 D# e# g9 F- {8 z
  42.       if(nPower < MaxPower){4 k# X( q9 S3 s7 |' l2 s
  43.         nPower += 3;
    . g2 A) ^9 V% O' ?
  44.       }
    4 M' g9 y. {, f* w; y
  45.     } else{  Y2 C. i1 x+ q$ p+ \0 i
  46.       if(nPower > MinPower){9 c0 J- x- _" R% A) d$ q
  47.         nPower -= 2;" q! Y/ d# A1 i1 x3 E
  48.       }4 g3 Y' R0 {6 I& K2 e- G. R
  49.     } 6 }+ e+ J/ g7 T* u0 E8 `
  50.     MotoL(nPower-Error);3 \: J' ^7 M9 c+ Q$ D( \# F
  51.     MotoR(nPower+Error);    ( D/ I) b4 \4 k' U- o
  52.   }else{8 A: j; A- ?9 d( ]5 d
  53.     motor_9.run(0);
    ! N5 ?! h6 X5 c  D0 T
  54.     motor_10.run(0);
    & @# v. G" F* w! v  o# T
  55.   }
    / S1 `4 _1 k* p" X' a5 A0 R
  56.   do{}while(millis() - StartTime < interval);
    - h: M! p/ i: w  `- y5 Q
  57. }
    " C% [: _0 `2 E8 D" D$ S' S) e

  58. , _% B0 j# x( [4 f
  59. void MotoL(int Power){
    % T/ T! m" |4 Z  x
  60.   if (Power > MaxSpeed){
    . [" ~: r6 [2 d: a
  61.      Power = MaxSpeed;  T: S8 _  O4 q
  62.   } * i9 c/ R. b# X# Q
  63.   if (Power < -MaxSpeed){0 z& n8 Y: B# s* v% u' i* s2 Z
  64.      Power = -MaxSpeed;; ^, o1 G# O; O' z
  65.   }
    ( R$ Y# q4 Z$ i
  66.   motor_9.run(Power);
    ) a& @% ^& X! R% V
  67. }  0 f) h+ X  Q; K" K6 w
  68. 6 A. d8 O; A! {) f! m
  69. void MotoR(int Power){% |; j: l4 K  w  [( c  o9 r2 g
  70.   if (Power > MaxSpeed){
    $ F2 Y" F# L# m( L0 c
  71.      Power = MaxSpeed;
    ) h2 Z+ P/ A" }# J; d. }/ c
  72.   } 5 z0 d, K1 Q5 X+ n
  73.   if (Power < -MaxSpeed){! f0 l. k3 Z% f
  74.      Power = -MaxSpeed;
    4 m. F- A+ z! }; E$ X3 G
  75.   }
    ) t3 q( Z8 X0 [2 ?
  76.   motor_10.run(Power);- }$ L+ Y6 l1 e8 A+ A. J
  77. }  
複製代碼

/ G& y* X0 @; p8 x: |& p0 H0 [5 {, `2 J7 R
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯
5 x! N3 f8 c0 b( N5 E6 w  ^+ K; V- M! L8 f
您好,不知是否能向您請教。0 @- M- h; m' S' _. e0 A1 E: G2 ]
目前和宋修賢老師在處理Ardui Car
* g4 T0 u8 i2 Z5 h' n0 m( h雖然已使用較繁雜的方式處理了跑出黑線外的狀況  ?; R4 }6 g8 t& }( Q
- _  H7 t$ }3 T6 Q" M4 x
但基於想追求更精簡的程式所以還是想請問一下
) D2 u1 d! e9 B$ J' h. v* ?就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝. b! n( q% \% l9 R
不知道您是否願意教我可以如何處理- R; W' `/ H/ n0 _* q4 y6 w
( H8 h2 Q2 v: c- s, ~
; S$ K0 }) [+ W' U: p5 u& T0 }9 I$ S
以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;8 B! b& s% f, s" V+ L3 S# Q
  2. double CNY70Val = 1000;
    8 p: }; d$ \0 |6 E& D5 Y5 _
  3. int interror = 0;4 |) ?* V- U- }( C9 Y& }) [6 d* L) z
  4. int olderror = 0;
    / Q8 {7 q- L: C
  5. double values;0 ?3 {3 g, C; J9 G

  6. 5 r9 Z; p! u. j$ k" q& \4 V' F
  7. $ N3 Z0 `# ^6 U6 a# M
  8. void CNY70()- K/ A- S: ]7 ?. Y9 M
  9. {
      y5 l/ |" W8 X# a$ T3 V- e
  10.   valuesRR = analogRead(RR); x7 r% R& f8 g
  11.   valuesMR = analogRead(MR);
    # J* K% C6 ~: {
  12.   valuesMM = analogRead(MM);3 ~9 ~# T' ^1 H; F1 ]: c) P& |4 l! q
  13.   valuesML = analogRead(ML);
    2 `1 V" W  z7 }; h4 C+ H& R
  14.   valuesLL = analogRead(LL);  S' ~% @7 L& L, V8 M: k
  15. 4 Z  Q4 p7 X+ F4 T! E6 o7 c1 B
  16.   if (valuesRR > CNY70Val)
    1 @# T4 ?$ a! ?6 R- K" k0 z
  17.     valuesRR = CNY70Val;$ H  v' n5 A  t% y% U
  18.   if (valuesMR > CNY70Val)) H' ^$ b% ^7 S& n: `8 G
  19.     valuesMR = CNY70Val;3 f, @' w) Q+ f( P
  20.   if (valuesMM > CNY70Val)
    ( @: U3 b9 K/ O  w4 P( O* X4 i; B1 e
  21.     valuesMM = CNY70Val;2 Q1 i" Z8 y3 s. T, b% p0 w% w0 b
  22.   if (valuesML > CNY70Val)
    - T- ]$ I, H+ ~- G' y# V4 f
  23.     valuesML = CNY70Val;
    ' C* F. I6 N$ Q- ?! g
  24.   if (valuesLL > CNY70Val)5 P! T4 B# K: z/ |
  25.     valuesLL = CNY70Val;
    8 w6 {; J3 K3 ~- T6 Y) ?" ]5 H0 T- J- O

  26. 2 M' h' [1 ]( l3 N; Q$ y
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;3 M+ _! }2 S6 c( Z" T
  28. }
    / t( q3 H2 `  x0 E" f8 u

  29. 3 w9 \& `! `  L) Z+ N
  30. void Car()
    ) j8 k6 S) i, b
  31. {
    ' r2 h( @# a' U
  32.   while (1) {9 H1 S+ E" J( v2 J
  33.     CNY70();5 o6 O1 o: }6 \0 {. F' \7 C; u. `
  34. % V; P5 i+ D# w& u4 e: t
  35.     int error = ((int)values);5 N- p7 {  T8 z9 \
  36.     interror += error;
    5 b) i! [# t- d5 j8 B6 F, M
  37.     int lasterror = error - olderror;, T$ a, N4 C# `/ g+ n
  38.     olderror = error;
    8 R* F) `! Q& m  ~' p) _
  39.     int power = error / 5 + interror / 10000 + lasterror;
    $ j: D/ i- i+ S3 _
  40. . J7 g  C* n0 k$ I9 [
  41.     if (power > MotoSpeed)
    ( b, O6 y1 z2 j( s
  42.       power = MotoSpeed;
    3 \# s: f0 }+ b3 S* E" b3 {7 n
  43.     if (power < -MotoSpeed)& |0 c6 e+ a3 f) `& {, p# h9 i
  44.       power = -MotoSpeed;+ @+ |" `  V6 G% X6 H

  45. & S$ M! U# W$ k$ v2 c) ^! {
  46.     if (power > 0)7 H! ~4 e+ }6 Q0 G/ c8 p4 }. E8 o
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。
    7 ?5 O0 D. h7 U- W4 q* ^8 m- w6 T' O- b
  48.     else
    0 N9 U& ~* q5 \" r4 G' y
  49.       Speed(MotoSpeed + power, MotoSpeed);
    * Q, l5 @& C' U4 n. Q+ A6 a) a
  50.   }
    ' D1 q& l) V5 I
  51. }
複製代碼
. s: H) ?$ T4 J0 _6 y0 q( @
1 w: ?  m# ]0 o$ l
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2024-11-23 19:29 , Processed in 0.025336 second(s), 17 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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