圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21790|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>& O9 [1 z. R) K6 Y- `% L6 n
  2. #include <Wire.h>
    & ^% z. K$ i( Q: w$ S
  3. #include <Servo.h>7 g8 _2 D3 d3 D2 V" T

  4.   }' n7 ?$ J/ G0 e
  5. #include "MePort.h"
    0 @( @  a* [* |! h  |1 D# P  ~( }
  6. #include "MeUltrasonic.h"
    0 y6 t- ~7 A4 X2 T: }% Z2 ?
  7. #include "MeDCMotor.h"
    8 c5 s9 c% x9 Z5 ?! y

  8. 9 _/ P+ W6 x6 W* y
  9. //double Input, Output ;
    6 l. V$ `% Q4 t2 Q8 b
  10. float MaxSpeed = 255;; W0 G7 F( _( L8 G3 Q6 Z; G
  11. float MaxPower = 180;4 h9 {8 j8 ~* D7 w2 z2 l: V
  12. float MinPower = 120;4 Q: T: j4 |$ f4 }0 }. u& N' c; s
  13. float Error,ErrorAcc,ErrorDec;3 }8 q. z5 g7 I' E& Z* f; K

  14. : g+ z: m# ]* n% c! K: I
  15. float Kp=0.14;
    1 f- O% x4 D0 W0 ]. ?( A
  16. float Kd=0.00020;//23;+ B- J# X+ t! |) `: y5 o
  17. float Ki=0.000201;) u% s1 k/ j$ |: i8 n# M# H* Y

  18. / }6 c  F- D& p* ~, j* w& }. L
  19. float nPower;  ^3 E6 J, g' S* P2 }: k2 J
  20. MePort lightsensor_6(6);
    , y3 n% S/ Q9 l$ ]% N
  21. MePort lightsensor_8(8);2 j/ G/ s6 s5 Z
  22. MeDCMotor motor_9(9);- K0 H- c. @7 ]5 y
  23. MeDCMotor motor_10(10);7 n1 ~+ ~4 h- T! k. A0 d
  24. unsigned long previousMillis = 0;
    # v  F  D* l" o6 k9 z6 F2 g
  25. const long interval = 1;
    - p7 ]2 j1 R2 y( y9 {

  26. ( ]& P3 D$ b8 _- N
  27. void setup(){% e" B. t( j( {5 ~0 P1 t6 P
  28.     lightsensor_6.dWrite1(1);! z1 ^+ C2 e# X
  29.     nPower = 160;0 E# P/ [$ ^; `, j
  30.     Error=0;: F& k" U/ C% K8 ^& t% P! J
  31.     ErrorAcc=0;
    6 A* }' _" y. @8 }3 m$ g  k
  32. }
    8 X* G! V# ]5 O0 c, i
  33. 4 m* G' r  {) J; R
  34. void loop(){+ @1 S* D. m3 d5 Q! a! Q# P- ?
  35.   unsigned long StartTime = millis();* d: a/ s& y& U- ?, i: Y0 h7 t# g! i9 G3 e
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    3 g. e/ F( z- p' T# b1 Q% p5 M, x
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();/ r: E9 l  S0 W! s/ V7 N
  38.     ErrorAcc +=  nError*Kd ;* \! f! Y) H9 I7 m# _+ R4 }
  39.     ErrorDec -=  nError*Ki ;
    - d: @! G. B$ i
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);& X4 w; T1 A: r( C& Q
  41.     if(nError < 80 && nError > -80){4 T! c: ]9 l, k* i) |1 i$ X# Q
  42.       if(nPower < MaxPower){
    5 E# E6 u& h. a5 @1 ?
  43.         nPower += 3;
    8 m6 w2 l8 j$ y5 ]5 \: N9 K
  44.       }
    + R) k5 Q0 W- K: E
  45.     } else{
    1 g/ w. x) z: @$ l, y7 a5 R
  46.       if(nPower > MinPower){
    4 A4 A7 v* O& @; @4 O/ Z2 k  l
  47.         nPower -= 2;
    / ^+ ?# O  I7 r8 D" n! ?+ t
  48.       }
    ; P5 z) a4 Z& K2 u6 y/ D
  49.     }
    % f& h1 h4 l* z! ]( u7 S1 {
  50.     MotoL(nPower-Error);
    ! {2 E5 B' {( L# B* h0 m
  51.     MotoR(nPower+Error);   
    3 G$ M6 _* u- Z4 q$ f. U
  52.   }else{
    : Q6 R6 W3 y* q! J( u9 l
  53.     motor_9.run(0);
    5 x1 Q4 q1 l% w' i
  54.     motor_10.run(0);. T7 K& z* O0 e+ h% {% j2 Q
  55.   }
    9 n+ Q: ~# g5 X1 V/ n+ g3 K7 c: |
  56.   do{}while(millis() - StartTime < interval);
    9 R4 [3 i" K* Y. Q" W3 ?
  57. }
    " t3 z3 Q! s/ @9 j+ D( U, Z

  58. & s0 o: c1 o; x
  59. void MotoL(int Power){
    * ^& F4 q' T$ u( M# @
  60.   if (Power > MaxSpeed){' j" T* e; }+ {# T* p4 A) }" k7 A& M
  61.      Power = MaxSpeed;3 t& b1 Q0 F+ q! e8 _# A) U
  62.   }
    6 t. A- ?  ]) G0 C
  63.   if (Power < -MaxSpeed){: [( U' [% `* E" u5 ~* e7 z
  64.      Power = -MaxSpeed;& g# v. ~1 R/ ?" n' ~% V0 z
  65.   } 3 G6 ?' S/ w- j) G' _
  66.   motor_9.run(Power);% [4 D; P! |* I' y$ v
  67. }  
    + Z) p7 p/ t5 G7 {2 R6 d7 B

  68. 3 h0 A% H' x3 q' N! F
  69. void MotoR(int Power){- R4 ?  y: I$ D2 r, a2 W# M
  70.   if (Power > MaxSpeed){# H# J/ z0 n  G; _$ O0 q
  71.      Power = MaxSpeed;
    ! }7 Q* V$ M( x6 P: @
  72.   } : h! C0 [) r6 C7 H2 b9 N
  73.   if (Power < -MaxSpeed){
    4 [; y3 f7 d5 ?! k- I
  74.      Power = -MaxSpeed;; _+ R$ ~  O, B; r# v5 t; V4 O
  75.   }
      u$ ^. z+ F: X2 l  z
  76.   motor_10.run(Power);
    % T  V# ]+ L: V( i" V) R3 B2 H
  77. }  
複製代碼

( ~- |  o0 T9 ?7 L* Y7 V: r: P  r# n9 {
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯
% h  o! j" U: w1 X  Q$ e0 {* Q* {3 k* W& t8 G8 T6 ~
您好,不知是否能向您請教。
8 Y$ o5 j! V5 p, I9 j目前和宋修賢老師在處理Ardui Car6 C; n+ ~1 v+ p) c
雖然已使用較繁雜的方式處理了跑出黑線外的狀況+ I4 P7 t+ [9 f' w/ G% n

7 Y9 u% Y8 c) u! _' A; d% T2 u但基於想追求更精簡的程式所以還是想請問一下. g; V7 ^$ K5 }! B1 L: M* d
就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝' h% U4 n6 D. N$ f
不知道您是否願意教我可以如何處理
! G* w( Z) u" b% a# ?3 R' m
) K; Z) X6 H# i# G1 i) m9 X1 K
以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;0 |/ C, A4 S! P/ N. D+ R. `
  2. double CNY70Val = 1000;
    5 X  p9 L' h7 ?8 O; W8 m4 C
  3. int interror = 0;) u+ c5 l9 x7 G
  4. int olderror = 0;7 m  b: t( c0 d
  5. double values;% `/ I8 t; x; @0 ]

  6. 8 B6 r; W% d. r$ S7 d

  7. ' Z: c4 g! c4 c3 }- r7 x
  8. void CNY70()
    / ?! |/ g& s1 [9 v. {
  9. {1 G0 p4 v' R) o2 \4 y* A
  10.   valuesRR = analogRead(RR)7 l/ D2 F! b1 D! Z+ }1 C! Y
  11.   valuesMR = analogRead(MR);! v; ~9 C, s8 G; ?$ ]2 W
  12.   valuesMM = analogRead(MM);; A6 H8 F% p- b: v( |
  13.   valuesML = analogRead(ML);( p& H9 O! j+ ^7 |" N
  14.   valuesLL = analogRead(LL);, B1 P- P" i  k% d
  15. 0 ^- h, t- u( N* v# Q* |; d) a& j
  16.   if (valuesRR > CNY70Val)7 U( B4 M$ |1 b  j
  17.     valuesRR = CNY70Val;
    7 p$ p: t9 T2 r% f# q
  18.   if (valuesMR > CNY70Val)1 g, B" q9 f" d) Z( Q
  19.     valuesMR = CNY70Val;
    ! m, X' U# t" `5 D: W
  20.   if (valuesMM > CNY70Val), }) c' B7 Z1 l$ N) J
  21.     valuesMM = CNY70Val;
    . m+ T/ p/ E7 j0 o7 |# O4 R
  22.   if (valuesML > CNY70Val)
    7 v( S2 Z2 i. F. i* f
  23.     valuesML = CNY70Val;1 ?: T+ D0 S0 k+ e1 A- p( A4 R/ h/ |0 h
  24.   if (valuesLL > CNY70Val)
    ; |0 v  o- y; y& d. ~; T
  25.     valuesLL = CNY70Val;( `8 V5 s5 N/ X. j1 I& o
  26. ( w5 F0 b  \! y  U: Y6 ^
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;5 o- V. L$ k' }1 Q4 ?9 ~' G
  28. }4 h+ r" x& p& D) H

  29. 3 X" c9 n; z+ f& G* v! m7 ~
  30. void Car()" w3 {4 j; |4 I: F8 h% a1 m  \
  31. {4 G$ s* W' b* d* e6 }
  32.   while (1) {5 r) J1 a' s0 S* S8 O: V: g, R; \% J
  33.     CNY70();: A/ \" I/ a4 n0 t
  34. ' @% f0 O  [9 d1 x
  35.     int error = ((int)values);
    0 Q, s9 V( T' X6 I
  36.     interror += error;
    % J! Q* m8 c. O7 p! x
  37.     int lasterror = error - olderror;; H0 S) G( ]; z( ~7 S
  38.     olderror = error;, _" r$ K2 s# c* I! ]. o) s3 a
  39.     int power = error / 5 + interror / 10000 + lasterror;
    5 p+ {# {$ {& k# u% |# U% q
  40. ' b( e6 O! V% K2 n: ]5 ]/ l$ y
  41.     if (power > MotoSpeed)
    4 l2 I( \! D# P9 z/ s: c9 g# Y( r. e
  42.       power = MotoSpeed;
    8 r: L: {' `2 s# l* T! n/ L9 s  V
  43.     if (power < -MotoSpeed)
    0 d. _2 `% C* {3 K0 V; E, V" ]
  44.       power = -MotoSpeed;
    $ j6 G+ Y# ]0 u: u4 e0 @! m8 N
  45. ' O" O: b2 s& S) \$ I5 |
  46.     if (power > 0)
    ' }, ^# U7 s7 W9 j* I
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。! g3 g1 d  A. [$ E9 I4 O' O
  48.     else
    . |% x- |/ U. \
  49.       Speed(MotoSpeed + power, MotoSpeed);
    ) G& y5 Y) A! ~+ m
  50.   }
    4 H7 _- I0 b, W2 z. H9 I5 i
  51. }
複製代碼
+ s$ o. ?" v( d( ]# e
7 j( w. Q; R, K; H' F4 v) i% A
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-12-1 03:48 , Processed in 0.027590 second(s), 17 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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