圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21705|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>4 I" g1 L& M8 T8 d! Q; K% E
  2. #include <Wire.h>) V0 ~5 ?- I. S3 y
  3. #include <Servo.h>
    . l# Q+ t6 ^0 m& g) \

  4. 4 U* A- y4 L" ~" M. a, n3 i3 N! a
  5. #include "MePort.h"3 R" q7 n6 s3 f: ^& t* T# O6 ~
  6. #include "MeUltrasonic.h"
    3 w$ R/ s) W& V5 D" U! [7 v
  7. #include "MeDCMotor.h"3 S- O0 x% P7 G6 t  h

  8. ; {' r2 W% G/ ?- K2 u
  9. //double Input, Output ;, b( v, A7 e) W! V# \
  10. float MaxSpeed = 255;
    5 s/ g* ?9 a- i. ?9 R
  11. float MaxPower = 180;
    * M& S1 ~" z# N; F
  12. float MinPower = 120;
    ( E0 u8 u, k2 Q& e
  13. float Error,ErrorAcc,ErrorDec;" m, X3 k9 V# J7 R

  14. " X5 W) }" I9 _
  15. float Kp=0.14;: U9 A7 n! L) g
  16. float Kd=0.00020;//23;
    ) g8 ]0 b# Y0 C2 g" G3 u
  17. float Ki=0.000201;
    % k+ f, Z; N: G

  18. 0 l+ C& M% u" L5 N" o, {
  19. float nPower;; T, ?, s1 @; N  M& C
  20. MePort lightsensor_6(6);
    9 }: {. L) B: a6 `/ t
  21. MePort lightsensor_8(8);
    : z2 r' \- X: k0 @6 v7 z' |
  22. MeDCMotor motor_9(9);
    " [+ X2 b/ v  M+ e
  23. MeDCMotor motor_10(10);# i1 r" [3 c  P1 u9 l' E
  24. unsigned long previousMillis = 0;0 a" u5 P2 f% W5 {( s
  25. const long interval = 1;
    $ X& I5 H& c6 H* V" `% K

  26. ( F9 l! m4 N* U! n+ ?
  27. void setup(){
    5 J8 _& G# _( N5 V# r5 k8 K
  28.     lightsensor_6.dWrite1(1);
    4 q" |! I3 ^3 l
  29.     nPower = 160;
    1 g" E+ Z" A$ T* y) E
  30.     Error=0;
    ! ^0 o6 i9 N3 _
  31.     ErrorAcc=0;; y# o/ B1 E7 b( a( H
  32. }% G: w; b$ P$ o% Z

  33. ! S5 \+ E  R( [. U+ V* H' g$ Y
  34. void loop(){
    # k' q+ s. }* ~/ u  {) @
  35.   unsigned long StartTime = millis();
    8 E" ~0 f  w) Q* t0 x5 c7 [4 q
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){  C& l2 e$ A# \2 t
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    , X3 L# e( b$ K; E4 {
  38.     ErrorAcc +=  nError*Kd ;0 O! w$ D3 L% h1 U* V8 H( k
  39.     ErrorDec -=  nError*Ki ;. P+ G* y2 M2 a$ ]1 m0 F
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);2 V- `& H! m4 `1 S# @
  41.     if(nError < 80 && nError > -80){& N5 Q9 q" H' s2 o4 ~
  42.       if(nPower < MaxPower){
    / w" m  H. c/ h9 e+ K) u
  43.         nPower += 3;+ \% q3 A5 M. L. S
  44.       }
    ' [5 N% q  m. K2 D9 D6 K
  45.     } else{) a5 |/ W  ~+ T4 ?, N1 z$ k1 E# ^5 S
  46.       if(nPower > MinPower){& Q2 C) W1 k" ?+ J2 O
  47.         nPower -= 2;6 A% y/ f0 A6 I8 q0 U
  48.       }
    # }5 |2 q. d5 b4 Y
  49.     }
    . l. V' i  E, o2 J1 g
  50.     MotoL(nPower-Error);0 C0 {) V$ T* K- T7 {6 u0 Y# X3 o
  51.     MotoR(nPower+Error);    - {  K; q5 n6 b4 H
  52.   }else{
    " }: |0 O: y! g! s  ]2 `4 x; s
  53.     motor_9.run(0);
    % i3 X9 z; ^, r$ j2 M
  54.     motor_10.run(0);9 u1 Y8 \! H9 @1 ^) N
  55.   }9 g. f! g+ W, u
  56.   do{}while(millis() - StartTime < interval);3 |. W6 _, ?3 n! U* ~2 N, R
  57. }+ O  r& `- N. P# B0 f/ a( J0 X

  58. . ^5 S+ g/ ]  q( M3 O8 N
  59. void MotoL(int Power){
    " \. @; G" x* F4 j( T) g8 }
  60.   if (Power > MaxSpeed){; U" [! Q/ k$ _; Z4 k" K" q7 e! Z
  61.      Power = MaxSpeed;8 K% j% K( U2 o
  62.   }
    9 e6 \( J: E8 d- D) e  n4 k' A/ Z* R: h
  63.   if (Power < -MaxSpeed){
    ; ^* u: M- W. @# B
  64.      Power = -MaxSpeed;
    ' I  a9 f1 ]% K5 v/ U- M$ d
  65.   } ! U$ \! z' Z: C* ]
  66.   motor_9.run(Power);
    6 U. N" u, E6 b( E3 E
  67. }  
    - a- u" f! ^! \* |2 K5 K7 @2 `
  68. & u4 G0 a3 l& E8 u* t+ X. T
  69. void MotoR(int Power){0 f# Z' A$ _! M- [
  70.   if (Power > MaxSpeed){
    ! Q4 k) Q9 c2 i" y5 i5 o& ~
  71.      Power = MaxSpeed;1 j- \, k; r8 ^
  72.   }
    + l  O. s* w2 `+ N
  73.   if (Power < -MaxSpeed){& w7 ^! ]1 n6 W6 B
  74.      Power = -MaxSpeed;
    : Z$ V$ D9 x# w, _% ]0 y& A
  75.   } 5 W! I0 E* Y" I" g. Z5 w0 S
  76.   motor_10.run(Power);
    4 u4 K, E' c1 X/ a
  77. }  
複製代碼
1 s  M6 C  B' [6 v0 |1 R2 F
1 s' Z& @2 ~. [$ X; S( W. \, t1 w
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯
5 v& W3 Z4 W' b) }9 \: q& O2 L% m0 r8 Q- \
您好,不知是否能向您請教。7 d. [8 g3 e, x
目前和宋修賢老師在處理Ardui Car
# d" Q) k1 X& y! Y6 F雖然已使用較繁雜的方式處理了跑出黑線外的狀況
  r( V3 y7 v9 {+ P% w
! Q4 n9 r( t' j但基於想追求更精簡的程式所以還是想請問一下
+ [5 i4 f  Z; D" W( o) T就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝
& [8 L0 \4 K( o" m! e$ |不知道您是否願意教我可以如何處理3 s9 _* f$ T6 \. p$ _& B

3 Q) ?6 `) z1 ^  F8 V. }4 j
! u1 n4 s, r3 G% f/ T/ ]7 c以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;
    6 y3 `) Z$ Y( N/ M" Y
  2. double CNY70Val = 1000;/ P4 f3 ?4 Y* Q6 W7 l
  3. int interror = 0;9 Q. ~. F' N& @
  4. int olderror = 0;) K1 r( E$ e1 l; Z+ b: e. S
  5. double values;
    ' ~" x* q# W$ I# x
  6. 5 n. k2 S& ^5 j
  7. 7 L* X: @- j3 V- Y: P- I& d5 l
  8. void CNY70()5 v4 I" d3 v6 C4 H* e* x
  9. {3 c" W  Z4 u! g5 _5 r( H
  10.   valuesRR = analogRead(RR)* ]1 i7 ^3 v+ f; W& U1 n
  11.   valuesMR = analogRead(MR);" e5 h' a9 o: p
  12.   valuesMM = analogRead(MM);
    1 X+ F' T3 S+ B" C
  13.   valuesML = analogRead(ML);
    0 k8 Y, ]$ a2 T+ G2 h
  14.   valuesLL = analogRead(LL);
    / Y* F: ?* O; J+ C7 W- z# D# g: i
  15. 0 B, w8 ~( _7 y  D/ o: n
  16.   if (valuesRR > CNY70Val)
    : o1 G. I1 i% a/ ]! d+ T: o
  17.     valuesRR = CNY70Val;1 l- c1 }7 i6 t3 K7 W
  18.   if (valuesMR > CNY70Val)
    # h/ |. ^9 Q/ K9 s, p% A
  19.     valuesMR = CNY70Val;4 B/ Z& l. E$ N5 g. B; @$ w
  20.   if (valuesMM > CNY70Val)& x' g* h4 p. m  X6 `# Z4 f" Q  [
  21.     valuesMM = CNY70Val;! l9 w$ z7 {% b  b) H
  22.   if (valuesML > CNY70Val)% H8 Z1 o# q* W! q4 f& U
  23.     valuesML = CNY70Val;
    8 G& Q7 C' ?: h4 B$ k
  24.   if (valuesLL > CNY70Val)
    - t( W) z# q0 x- b
  25.     valuesLL = CNY70Val;
    $ {3 X+ c8 t* w- u. p; |7 R

  26. 9 O4 e: G# a  H5 c$ K! o4 O$ `6 e
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;% H$ r; o( T. {5 H3 c
  28. }
    . ~- j) m* }# P$ U5 p, C3 ~
  29. ) d9 T: y1 k+ o3 y  y
  30. void Car()
    ) G: V# A- M$ @% a0 H: j$ R
  31. {
    ; c5 d9 _% P0 A# L3 H! t  B1 @. r
  32.   while (1) {
    1 c+ l$ W3 o* y* \& A: p: a7 v
  33.     CNY70();9 n6 q9 Q) Q/ X! ?/ y3 L5 |  |

  34. ' o; a! u0 k' F
  35.     int error = ((int)values);
    . n& ~8 c1 ]1 m! b. t8 j
  36.     interror += error;7 \4 a- P9 ^, t( M
  37.     int lasterror = error - olderror;0 N2 ?% ^9 j& d2 A7 {! H
  38.     olderror = error;
      o9 r( H: s/ E
  39.     int power = error / 5 + interror / 10000 + lasterror;
    $ a* ~2 [: q- C8 h

  40. ) I, `, W3 I# m5 b2 U9 _( N9 y4 Q
  41.     if (power > MotoSpeed)
    % t( f3 ~! k# o$ C$ Z' @' y
  42.       power = MotoSpeed;
    ( d) i0 q0 I  f* W
  43.     if (power < -MotoSpeed)
    " b$ W* x% \" u6 c
  44.       power = -MotoSpeed;
    % A' j: q7 ?+ C, F( n4 {
  45. ' E% v' ]* [6 n4 b
  46.     if (power > 0)) Q+ f$ w3 ~0 \
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。
    8 M* L% O$ T  a" x8 X2 x- h1 g
  48.     else
    8 z# @2 ~) e4 s2 i  O- ]% |
  49.       Speed(MotoSpeed + power, MotoSpeed);
    5 g  Z% S% ~5 B8 K% y
  50.   }
    3 S. |4 r$ W; A5 c
  51. }
複製代碼

- C  r( n8 p$ G  Q1 Y5 ?, A: b
, M# N- r  `0 B0 o3 ]4 w( E8 x* a
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-11-24 19:26 , Processed in 0.024392 second(s), 17 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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