圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21808|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>/ D6 Z! e( r. O* ^. F9 J6 O9 u
  2. #include <Wire.h>
    + [0 D4 \$ _, X
  3. #include <Servo.h>
    + Y: R2 k4 l, T' ]* Z) V1 A

  4. ' ?; Z" v# k* g2 ~1 M
  5. #include "MePort.h"
    / T1 g/ {& |: a( p. W4 ~6 z
  6. #include "MeUltrasonic.h"
    4 K, w4 M/ C& e/ {
  7. #include "MeDCMotor.h"! [9 I) h! R1 E' u# ~* P

  8. : V' ~4 F+ d! [  u4 i/ J
  9. //double Input, Output ;( J! [3 \1 z+ \6 y* T( y
  10. float MaxSpeed = 255;+ e1 _2 [1 a/ Z) }* a, B
  11. float MaxPower = 180;- V; o0 B: p; i  \
  12. float MinPower = 120;8 ~4 U# T7 r  f7 K0 ?
  13. float Error,ErrorAcc,ErrorDec;
    , ?! ~# c( Q. ~8 q  |0 e9 L# }
  14. 2 P7 {% S6 f3 f- V
  15. float Kp=0.14;2 @8 ~+ q' o7 @8 G$ U$ K
  16. float Kd=0.00020;//23;
    5 M0 f, s" U! w* {
  17. float Ki=0.000201;' t8 O2 u7 _* y! Q

  18. 8 N& h8 V9 R5 C
  19. float nPower;) C1 R. U" j' [4 P
  20. MePort lightsensor_6(6);' l  p- q/ D; f. y; ^
  21. MePort lightsensor_8(8);; K4 Y* P+ F0 P% D* R# L
  22. MeDCMotor motor_9(9);, A9 m6 n7 E" ~+ x/ h" B1 q
  23. MeDCMotor motor_10(10);8 ]# E/ t4 y- C- B3 A7 N9 x
  24. unsigned long previousMillis = 0;
    # e$ M$ i2 z" ^8 I. g/ D8 X
  25. const long interval = 1;8 c: U3 k3 s6 }7 ^0 i6 `; l0 k

  26. 0 t. l/ E; l+ O# W* t/ ~
  27. void setup(){
    ) ?; z6 E1 H+ `0 I2 `, ]
  28.     lightsensor_6.dWrite1(1);
    ; r6 u/ e2 V6 X# y
  29.     nPower = 160;
    4 v. L3 ~4 S# W. v
  30.     Error=0;
    3 H& m: i" p* h0 i
  31.     ErrorAcc=0;4 A9 J# n3 C% i" d' W  C) p
  32. }7 z, p& Y$ h/ T
  33. - i; c1 `( z# r& B
  34. void loop(){
    3 t* g8 b8 @- j2 [- e) n6 A
  35.   unsigned long StartTime = millis();
    ' Q5 i1 @" b4 e$ U! c6 G5 F' J& c
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    2 g- _6 I! q# f/ M/ I/ }2 {: l
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    # F' @6 P0 Y! N2 x
  38.     ErrorAcc +=  nError*Kd ;/ }- Q+ [/ t2 ^
  39.     ErrorDec -=  nError*Ki ;1 i8 v" u5 |& Q9 @# d
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);3 S" s9 r/ C4 e, Y- ]
  41.     if(nError < 80 && nError > -80){. k0 ~3 K( x3 V* I: [
  42.       if(nPower < MaxPower){
    ' a; Y$ L! I; h7 t# v) S* \5 g
  43.         nPower += 3;
    , o% g" N8 `1 b8 G; c
  44.       }0 k/ P& |+ z, Y
  45.     } else{( Q+ r6 ~: u7 a; v4 ]
  46.       if(nPower > MinPower){
    ) K) e7 w; @" f* \+ A8 f4 S
  47.         nPower -= 2;
    & l- L3 j' M) c! A& I6 l: F7 O
  48.       }
    & d5 k/ ]$ ^' S  l$ V. m
  49.     }
    7 j3 J$ m- N7 m$ w$ C2 ^0 P6 I
  50.     MotoL(nPower-Error);
    - Z; q6 o5 `. p/ e- H( q
  51.     MotoR(nPower+Error);    , \, Y/ a; v9 S& D
  52.   }else{
    ; m; X" X; ~! H& c# o
  53.     motor_9.run(0);
    ! C4 ]* D, o! W- r% ?$ v% G/ ^0 m1 q+ L
  54.     motor_10.run(0);8 a/ Y. u. P, B
  55.   }
    6 M! B0 E, G) B+ L/ s: V
  56.   do{}while(millis() - StartTime < interval);
    . G' t) w% T  d- q
  57. }
    : Z+ w1 y$ M# c8 `% X

  58. ! t! Z) Z, G' W- E
  59. void MotoL(int Power){
      D  @) @+ z2 Q
  60.   if (Power > MaxSpeed){) I" g: a; m' I  V. V8 b
  61.      Power = MaxSpeed;2 O! u  _& r0 |. `
  62.   } ; P" z5 W9 d+ h( M: C. [3 u# h
  63.   if (Power < -MaxSpeed){
    3 G3 Q: ^' t2 u# [0 R. [! T
  64.      Power = -MaxSpeed;# w. h7 [8 O) B- l5 k
  65.   } 0 N2 A# H6 J5 s7 y) p0 K" ?
  66.   motor_9.run(Power);
    # A  X3 n3 e4 t6 d6 L. u- p/ v
  67. }  
    ( E2 l9 S' k1 `$ f

  68. 6 b2 R$ I" [! v4 Z. e  P% ]$ X
  69. void MotoR(int Power){$ e9 N5 R7 o. ^0 z
  70.   if (Power > MaxSpeed){
    7 i8 Q- V" @( g2 U8 N
  71.      Power = MaxSpeed;7 Y% ?6 r( X( h2 G8 M0 @, R
  72.   }
    + t* o9 }4 U' l$ d& A) r2 x, f% y
  73.   if (Power < -MaxSpeed){
    7 R2 `" y5 E9 ]+ E
  74.      Power = -MaxSpeed;
    " H- [2 k- g! P; g- @6 ^
  75.   }
    ; T- a0 A8 d+ i( f% f
  76.   motor_10.run(Power);
    7 S2 M2 ^  A& {% f
  77. }  
複製代碼

5 I' n* `' _7 m7 s( z6 e# S
" k' z  K  Z& Z% J& u2 Q- X
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯
1 D2 F. o( l0 E' Z$ J, U% W2 I1 z, {
您好,不知是否能向您請教。. B1 l; V5 v+ s" m& e
目前和宋修賢老師在處理Ardui Car, \- w/ }# `. K5 a
雖然已使用較繁雜的方式處理了跑出黑線外的狀況
+ g! G) O; ^: J7 z3 J! T
. W# g6 @0 M7 f但基於想追求更精簡的程式所以還是想請問一下
+ Y3 h# }3 z* p: V6 Y就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝! Q) u, @+ f6 h
不知道您是否願意教我可以如何處理$ q+ X" V6 Q  U, \

* Z  _+ m: W& B$ O0 h9 n. v* L& X! z8 j2 D- n  a* _
以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;+ b2 ]! y- N" g
  2. double CNY70Val = 1000;
    - k8 @# \2 H" u# A8 M! `; _
  3. int interror = 0;7 [6 H' x0 u+ d. w# h
  4. int olderror = 0;8 v1 k% E1 U3 C" F$ K& ?9 \# n
  5. double values;
    5 \/ u( [: T3 p; h& c: Y

  6. ) q# x% {% }. O/ k! q

  7. * f/ P" z- q8 H2 e
  8. void CNY70()6 d2 H3 E- l- P" u. X' ]7 b
  9. {8 u6 K3 Y0 E$ o6 O- p
  10.   valuesRR = analogRead(RR)  D) `. a9 G1 x0 Q
  11.   valuesMR = analogRead(MR);4 m2 j. }3 J5 e7 u. V: W" `
  12.   valuesMM = analogRead(MM);: }2 c2 O+ A# U' w5 v) D, b  r7 Q
  13.   valuesML = analogRead(ML);
    - J+ B7 K" k9 q+ y/ s6 ]
  14.   valuesLL = analogRead(LL);% y. ?* n1 ]+ o6 d6 u! a6 z

  15. 0 P8 _& w6 X. O% f
  16.   if (valuesRR > CNY70Val)
    4 G  z5 c' e- o3 x8 K  A/ m0 S( E
  17.     valuesRR = CNY70Val;
    8 c  l' @6 h+ E2 t; C! S) Q9 M
  18.   if (valuesMR > CNY70Val)
    . e8 P7 B3 i+ Y
  19.     valuesMR = CNY70Val;
    " ?! `7 I' e; F
  20.   if (valuesMM > CNY70Val)" C8 D8 X$ n$ o; M
  21.     valuesMM = CNY70Val;
    3 a& l9 D/ [) g
  22.   if (valuesML > CNY70Val)7 F+ M: d- i$ M: K7 i- l! K
  23.     valuesML = CNY70Val;
    , E- c$ x" b! U5 R  i
  24.   if (valuesLL > CNY70Val)( z+ j. v: V, |2 y9 m0 \8 [
  25.     valuesLL = CNY70Val;5 ^% \$ `" X" m0 ~' I

  26. ; d% q- C3 E+ C4 {" |4 a: P2 X0 q
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;7 r/ O! _9 x3 V, `7 X0 q; A
  28. }  z5 N/ c# Q; M5 o3 B- y

  29. ; s. R) l. k4 b" u- R
  30. void Car()
    ) ~# m/ b2 _6 x, T  I
  31. {! K3 W* @1 [  G8 ]) @
  32.   while (1) {! V  c& g* C9 w! i' u. k0 R; g) Q
  33.     CNY70();8 \8 B) `, Y  |1 s1 ]4 E3 M

  34. : j- x$ w6 Q+ P9 R8 r
  35.     int error = ((int)values);
    * R- u. I% O, r: y8 |# E2 B6 u
  36.     interror += error;8 {$ Y* \& U7 T. B' S1 Y
  37.     int lasterror = error - olderror;
    " Q! v2 J7 V- `) D$ x
  38.     olderror = error;9 f- I6 s* j: V" I! e! Y- r- m- k
  39.     int power = error / 5 + interror / 10000 + lasterror;8 ?8 b4 r$ u' k8 m  j

  40. ) w/ o$ Y3 H0 c9 O) O( N
  41.     if (power > MotoSpeed)/ s4 o! m5 D- l9 }
  42.       power = MotoSpeed;5 {) L' v6 ~# G0 F3 g' ^2 E
  43.     if (power < -MotoSpeed)
    , y: N& \& }3 [" y" O
  44.       power = -MotoSpeed;) K' r) V7 N  \( K, N, Q
  45. ' \+ }3 k& ~0 r8 b( t
  46.     if (power > 0)
    5 g4 {& H# Q4 z( o$ P) ~, q8 P. u
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。0 a, M( E: Q& W0 p9 {& {
  48.     else8 _2 W/ f( U  I8 S* i
  49.       Speed(MotoSpeed + power, MotoSpeed);
    ) y; S, n% T0 `
  50.   }- L* j% ]7 n: p# n- |) S
  51. }
複製代碼

& F% w& L3 R& X0 k) \- U" b7 v. ^' b; p/ s5 ^2 H
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-12-2 13:50 , Processed in 0.022909 second(s), 17 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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