圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21737|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>
    6 l+ g" O. H/ K* i3 |
  2. #include <Wire.h>
    * ~. a2 K8 ~; |/ L% w
  3. #include <Servo.h>/ |& f, C$ W- \1 S" ~. K: P" \7 x

  4.   ?$ z$ K0 `( V7 ^% u. @8 A
  5. #include "MePort.h"9 T3 z; R- U. t1 x
  6. #include "MeUltrasonic.h"% {. H  ^; G2 C" l" z, x2 b% ]
  7. #include "MeDCMotor.h"! R! v! \6 e  s% [
  8. 0 w4 N3 d: q  C3 v  O
  9. //double Input, Output ;2 E2 P) p, [0 }5 l5 L) T
  10. float MaxSpeed = 255;7 T* w# h0 ^8 e! w! ]
  11. float MaxPower = 180;
    / J. i# Q1 |: l* G8 Y
  12. float MinPower = 120;6 d! C/ r4 `0 N! s( l  l
  13. float Error,ErrorAcc,ErrorDec;; P4 p; l8 q0 ]  |' z$ ]
  14. 8 N0 L, g3 j: T2 L$ a
  15. float Kp=0.14;
    * W0 H* @) X6 N* H
  16. float Kd=0.00020;//23;" n7 F! s1 B5 }; x4 m: U5 C
  17. float Ki=0.000201;- K1 U+ _; R+ g: m, g+ l) ^# q
  18. 3 `" J- G8 z- \3 x& u- e& t. I
  19. float nPower;
    ( ]$ g! I3 H' K8 L; a
  20. MePort lightsensor_6(6);
    ; q4 o, ?1 j  _/ j8 Q- l8 e
  21. MePort lightsensor_8(8);" m- u7 z/ l! A# c3 N
  22. MeDCMotor motor_9(9);
    : N0 c; |( J- J8 U# D- R* z
  23. MeDCMotor motor_10(10);
    - A; O4 ^& V3 l7 f- m7 J6 r
  24. unsigned long previousMillis = 0;
    7 c+ C6 k* q7 b8 k% y3 X
  25. const long interval = 1;
    ' p- Q( Z; R- U

  26. 4 u. m6 d  C, n: t, |
  27. void setup(){3 S7 y2 T$ O, @  f1 M. f# _  E
  28.     lightsensor_6.dWrite1(1);
    0 K0 O5 H$ e7 Y
  29.     nPower = 160;" _1 u. k7 \+ N4 Z# ~
  30.     Error=0;
    " }3 |, o8 k& o8 P5 m- |1 ?) P
  31.     ErrorAcc=0;
    3 \$ ?4 k/ U: ~, h' @
  32. }
    9 u& g, D) y' G% t
  33. 3 h* E7 }" M6 T
  34. void loop(){2 o- l+ N+ E7 m. `+ Q( K* v& ~
  35.   unsigned long StartTime = millis();0 S% l2 j' O! a* w' o3 y9 T  q
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){6 V& f1 S0 p' ^
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();* f! W8 M5 z0 {6 |/ i
  38.     ErrorAcc +=  nError*Kd ;* k: m* E+ ?/ k9 D% U) N. N* Y
  39.     ErrorDec -=  nError*Ki ;
    : W& ~- K& U# S' W
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    9 @* E7 m, ~! q9 k8 `$ ~
  41.     if(nError < 80 && nError > -80){7 ^0 _+ R5 w! O% I1 A$ k$ j6 x
  42.       if(nPower < MaxPower){
    0 f3 a& L6 {  A1 m
  43.         nPower += 3;  Q& P3 R) D( Q, b2 @: s
  44.       }
    " K4 `8 K0 o! Q1 d4 f
  45.     } else{
    8 \8 C  ?: A% H4 J4 j0 o$ x
  46.       if(nPower > MinPower){) @7 ~+ s% A, m1 a/ {
  47.         nPower -= 2;: w# ^! N' ]9 @! t! }3 q
  48.       }! G* \. e$ l% X! d9 \, e# c
  49.     }   C: z; _( O5 k- h' [# R$ D
  50.     MotoL(nPower-Error);
    ) C& i, H$ V/ x* ^/ u8 }" J
  51.     MotoR(nPower+Error);   
    . u2 }0 @; f: J, X1 _) m+ D
  52.   }else{
    % ], `. v# a( u7 K, r' q; c
  53.     motor_9.run(0);+ y0 l6 u( Q& R0 _6 L+ z1 J6 ~7 j" i
  54.     motor_10.run(0);
    0 {4 Q5 W2 h3 r- G: w. W4 O2 p* |$ E
  55.   }
    : p8 r+ e6 F0 Y0 I1 v3 P
  56.   do{}while(millis() - StartTime < interval);
    ! W' f0 @  o0 s
  57. }
    0 A$ Y: D% m, {6 k9 h1 y
  58. 7 }3 t/ @+ Q8 x' |
  59. void MotoL(int Power){
    3 G- x2 n& ?) y1 Z( a; Q
  60.   if (Power > MaxSpeed){
    ) C; J' A8 M, ?; C3 T9 Q
  61.      Power = MaxSpeed;- K/ y/ d' Q0 F
  62.   }
    & |7 V4 a9 Y8 Z- b
  63.   if (Power < -MaxSpeed){- j; O- x2 y. S2 n, A0 E! q7 |7 {8 o
  64.      Power = -MaxSpeed;
    ) F% j/ ?2 e6 e, I
  65.   }
    + o& N' N9 j8 ]* t4 ^0 B
  66.   motor_9.run(Power);
    / x% j4 }+ L% h' `/ g6 L- H
  67. }  ' R' o5 G# B: }* l/ A
  68. * t: z; b) R- V' A3 h
  69. void MotoR(int Power){
    3 J" Z/ D# E7 \6 l6 b3 y* |" R
  70.   if (Power > MaxSpeed){: x; o& g; G; t( _# t, t
  71.      Power = MaxSpeed;# \5 }: K! ~* ?( V; \
  72.   }
    + a1 C7 P" j. e' ^# ~
  73.   if (Power < -MaxSpeed){6 N5 ?; e' l+ m6 ?# L" u
  74.      Power = -MaxSpeed;! {6 `- u& p9 N0 i7 {- g( u' t: g! N
  75.   } + t! s4 K, F* W0 l; u
  76.   motor_10.run(Power);4 ~" b; M0 p: M* k$ Y% `
  77. }  
複製代碼

: O4 v& v8 U! b) B& T- D8 u! n  r# c4 A) q* \& ~# C
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯 9 a1 s# _& R; C2 Q( m' i
7 q; C) z9 Z7 q# u/ [$ X
您好,不知是否能向您請教。
4 r  T+ P! \& u9 I5 A6 C目前和宋修賢老師在處理Ardui Car) x) }  X3 j% l# h3 y
雖然已使用較繁雜的方式處理了跑出黑線外的狀況
+ ]$ g9 O) W2 {" i
% S& A. w: u4 M7 C但基於想追求更精簡的程式所以還是想請問一下
& l+ L1 U- f  Y0 u6 @$ J就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝
3 f/ ^+ `* d, d# I$ h) R( O不知道您是否願意教我可以如何處理
- Y0 y; R- j6 h* u1 q4 W( v+ r( S7 }7 K! X6 {. f) Y
. h; l' e7 Y6 z# V  t# o+ I
以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;0 A' t- u) U$ x0 P1 {) @
  2. double CNY70Val = 1000;
    7 m% e6 t5 j" _$ q1 ~
  3. int interror = 0;. |! |7 b6 E; p7 Q* s3 J$ D
  4. int olderror = 0;( ]# T$ E' t: W0 h( F
  5. double values;; V7 t  K* ]1 H' A. C

  6. 6 Q% K* c. o: w
  7. 9 @7 }& `2 M3 L2 w; ~$ p1 B
  8. void CNY70()
    % k; A3 W+ `+ |9 Z, `
  9. {
    5 j2 I9 z+ Q# _0 ?3 b9 ]
  10.   valuesRR = analogRead(RR)
    ) a% x# X& l" l9 x/ x1 c( O
  11.   valuesMR = analogRead(MR);! A% e% b$ C' K5 l0 L; x2 J7 K
  12.   valuesMM = analogRead(MM);4 n/ Q1 a5 y/ W  X5 x! e. C  L
  13.   valuesML = analogRead(ML);
    7 E( e( G  ]% b- p4 w1 [. x: R
  14.   valuesLL = analogRead(LL);& K- X1 Q+ V% T, U

  15.   n7 N7 e4 s! Q1 I8 [, ^  V
  16.   if (valuesRR > CNY70Val)
    / z" ~$ y- l$ k+ `
  17.     valuesRR = CNY70Val;& O# a: w; t9 Z- F- l
  18.   if (valuesMR > CNY70Val)
    ; |- L* ?* J# D) o  Y8 i( u
  19.     valuesMR = CNY70Val;
    * d" i$ w+ k( ^
  20.   if (valuesMM > CNY70Val)
    2 D" Y8 v; e/ d, p( n+ o
  21.     valuesMM = CNY70Val;
    ! h( a* o, R+ R; @  c5 L
  22.   if (valuesML > CNY70Val)# n" D0 c( ^, C/ o& h$ |
  23.     valuesML = CNY70Val;' ]; R2 |9 \, G# b* n1 |2 k
  24.   if (valuesLL > CNY70Val)
    2 {1 V5 O; L" `4 [+ Y, R
  25.     valuesLL = CNY70Val;4 O; r5 a; a+ q9 H7 F. d) H
  26. , ^0 \# o6 x0 J, `2 g! v, f  |! T
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;* ~. U" M7 ?4 I7 m+ Q
  28. }, A; T  V9 c3 w
  29. 8 x* N" z' j+ f$ u) g( K. @
  30. void Car(): {  q6 R, D/ s6 T! s
  31. {
    . U6 e: s! W, j  j& j7 s: d
  32.   while (1) {
    . X) h! b. K$ o1 g( v- W
  33.     CNY70();
    " [8 K' s: O0 ~- k& Z. L  [

  34. : s6 N8 {9 [& i7 H+ v! J
  35.     int error = ((int)values);
    ' |1 [5 E5 i/ W8 ?
  36.     interror += error;
    2 d( _% K$ I( `$ T9 x" P# g
  37.     int lasterror = error - olderror;
    $ N- n! P$ _; g; t
  38.     olderror = error;2 `' w7 D  y8 W- E5 K$ z
  39.     int power = error / 5 + interror / 10000 + lasterror;
    ) [/ H4 \4 `% Y- L# q- Y

  40. / }# t$ ~' H* S' |+ X
  41.     if (power > MotoSpeed)
    + g8 d) P; |( z: S" x0 @5 C
  42.       power = MotoSpeed;& [, B0 Q( K# K
  43.     if (power < -MotoSpeed)6 v, f- J' s6 n3 N  M
  44.       power = -MotoSpeed;  [! i! r/ r+ s$ P
  45. % R6 M4 G  Z1 c# R
  46.     if (power > 0)
    . W7 D: ^# X9 j/ D6 V1 j
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。- f0 T, g  E1 h$ ~: i# ^
  48.     else) @" x) Y! J. Y% c
  49.       Speed(MotoSpeed + power, MotoSpeed);
    3 B; [& P  Q+ @8 T! ^* P
  50.   }
    . ?1 [8 H3 N8 t8 L  |- }* p
  51. }
複製代碼

2 o6 p7 F: r$ d! O
$ u; j5 q8 e( {% V& q
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-11-28 12:58 , Processed in 0.027213 second(s), 17 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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