圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21815|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>
    - X5 R! y3 z" w, J
  2. #include <Wire.h>  V0 p8 t2 M( \( p2 x
  3. #include <Servo.h>; x) X4 w, F% t7 e

  4. % a- ^1 I6 O2 f% V% Z) U2 u
  5. #include "MePort.h"0 n$ P, U, O  J- ]& j: F' z# _4 }# S
  6. #include "MeUltrasonic.h"
    6 `! z9 W+ G' G  g- T8 ?
  7. #include "MeDCMotor.h"* a  `% C1 {, v: s& M+ o# v

  8. 1 k  V+ p2 T( ]3 R3 t3 h
  9. //double Input, Output ;9 t& m( u2 }- z
  10. float MaxSpeed = 255;  I- [- K! J# {4 v
  11. float MaxPower = 180;
    ( _; t3 j% h& n& O9 b
  12. float MinPower = 120;
    4 u/ z3 q, u3 K, E6 F
  13. float Error,ErrorAcc,ErrorDec;
    0 N4 ]5 f" v9 N" ~1 I! N

  14. 7 S% C: a3 D% p. B+ e0 X! {7 K5 K
  15. float Kp=0.14;
    ) ^6 @/ s$ B  W7 l- m2 r( H' y
  16. float Kd=0.00020;//23;8 {: K- v$ K+ X
  17. float Ki=0.000201;$ _: F+ n6 j! q, I7 f
  18. " \! Q0 W/ V7 Q- h5 Z2 M
  19. float nPower;, d7 {% D  ~% x; K! H, n
  20. MePort lightsensor_6(6);6 P! x- b6 ~: M' q. b7 `4 j! o( ]
  21. MePort lightsensor_8(8);
    * W+ \: f. g/ D) p& k
  22. MeDCMotor motor_9(9);% a2 |& D9 y6 L4 o# ?
  23. MeDCMotor motor_10(10);2 W- [* G3 q, _, D
  24. unsigned long previousMillis = 0;7 E4 |: n% g( n( y. [+ X, n
  25. const long interval = 1;
    5 A1 k4 x2 _( O% r/ u  }4 M5 O
  26. 7 K/ J3 S2 [7 A, a4 r1 ]9 Z9 N3 U
  27. void setup(){! S7 |4 F! J2 g: r( `3 z) l
  28.     lightsensor_6.dWrite1(1);/ T# @% B! r% q" y. E" b3 k
  29.     nPower = 160;
    ! s# G* M* J) j, x% e: m
  30.     Error=0;# y+ y  W% C2 G9 w; [* |, h
  31.     ErrorAcc=0;
    2 j8 D* i  A; c+ B# @. M
  32. }
    1 H- u! ^* d* i

  33. . S; a+ g6 O' j# C+ M% \
  34. void loop(){# K5 w) C( h: P- j  p4 P! ?/ ~
  35.   unsigned long StartTime = millis();1 n8 u  U; m! m: b
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    , V7 _$ p% k  C
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    & I: [. ]* D% m  E0 `
  38.     ErrorAcc +=  nError*Kd ;
    ' n4 n- w/ W- c/ X5 y
  39.     ErrorDec -=  nError*Ki ;
    " J) ^; E$ |1 G& G2 [
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);$ Z7 \2 ]  Z1 H9 B. P8 n
  41.     if(nError < 80 && nError > -80){
    5 s, v5 m  F8 e: p4 c* P* ^* }
  42.       if(nPower < MaxPower){
    5 E5 ^! `. a8 {( J& h0 Z
  43.         nPower += 3;7 Y" u2 G4 @% q. v% V3 o
  44.       }
    ( I) n1 r- _% c  a% i2 e
  45.     } else{0 w- {9 q1 A( l) }
  46.       if(nPower > MinPower){
    $ o8 Q7 f' P$ \# q
  47.         nPower -= 2;
    % j+ b& H2 q6 X7 D) ?4 k. v
  48.       }
    ' d- {9 v, R+ `  ]) v9 ^
  49.     } ) u! n% B3 b9 e' I1 m9 }
  50.     MotoL(nPower-Error);
    5 V; z; n( R2 G6 M; a+ j$ K! a
  51.     MotoR(nPower+Error);   
    ; V2 s  K3 e9 J3 K
  52.   }else{" a8 x) @, d1 \4 D
  53.     motor_9.run(0);
    " G. G! F+ a# `- k$ c4 M4 b  M
  54.     motor_10.run(0);
    2 _2 Z  F: ], f% [
  55.   }
    7 q. u/ d. o3 y4 W
  56.   do{}while(millis() - StartTime < interval);
    4 K9 h! M9 I+ f
  57. }3 E6 Y# o# r1 |& R- ~1 M6 e/ k
  58. 6 Z3 V+ L( F7 C2 y7 `, u
  59. void MotoL(int Power){# d, t/ t6 B7 _, o5 g" F
  60.   if (Power > MaxSpeed){
    : q4 m* @2 \( y0 Q1 q+ A& Q& D
  61.      Power = MaxSpeed;7 |0 V: R2 m4 N4 k3 O
  62.   }
    4 @# x# P% ^& k) K9 l& b
  63.   if (Power < -MaxSpeed){
    ! S4 O- C2 g. E; Q( T5 V
  64.      Power = -MaxSpeed;) F3 Q" {- ]$ H3 y% X
  65.   } 4 P$ R( b- E, _
  66.   motor_9.run(Power);5 w3 r' m. q) X; x5 x
  67. }  + q, d1 R2 T4 a7 g5 T

  68. # z6 {, O5 J) W  T" x& k0 J6 |
  69. void MotoR(int Power){8 p0 ]/ M8 B" \1 U6 U
  70.   if (Power > MaxSpeed){; r5 N" O7 A6 ~# F3 A
  71.      Power = MaxSpeed;# _0 c9 B& G0 N$ B! q; n
  72.   }
    ! B/ S- U7 W. p& [
  73.   if (Power < -MaxSpeed){8 n( W5 b' x' T3 S! ]- e
  74.      Power = -MaxSpeed;
    ) Z) N6 [5 g3 ~  o+ u; U1 H
  75.   } 8 _( K# [$ I1 H, y# a
  76.   motor_10.run(Power);+ N& y5 P1 C% Y
  77. }  
複製代碼
% N+ t0 [4 @- X- ]) w6 Y4 Y

) {0 ?! v: {/ q: K: K8 V' W
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯
" F$ |) }% |6 h$ e
; k" t4 d& C2 t3 C, A您好,不知是否能向您請教。
) J! f; t" @9 N7 o/ q4 a目前和宋修賢老師在處理Ardui Car. l( K8 }3 B6 r) J
雖然已使用較繁雜的方式處理了跑出黑線外的狀況4 t' j: ~  b0 e- d
$ }9 _2 O- K5 g
但基於想追求更精簡的程式所以還是想請問一下4 ^, q( ?& i% j
就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝
2 j2 q( f: Z+ @4 J, e不知道您是否願意教我可以如何處理
7 z; @" k: B3 N8 o- y
4 y$ W1 S: t7 l8 v. g( n# G2 F' Z" F( d4 w
以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;
    " G# S5 g+ J' U4 p8 l% C# i; O4 Y
  2. double CNY70Val = 1000;5 i) s0 I1 r0 B/ O% F: Y
  3. int interror = 0;
    ) y$ H8 h& t* o& H$ n
  4. int olderror = 0;
    * v' K! n8 S7 o, b; ?
  5. double values;) k0 Q. w3 ^( \3 M! V0 I

  6. 0 g/ n5 T/ g, z% f

  7. + o* i3 N7 V% K9 E, [, D: L% r; x" ^
  8. void CNY70()0 L/ X  [" m" ^4 ^5 |
  9. {0 M/ n& b7 o3 S
  10.   valuesRR = analogRead(RR)
    8 w. ?& w. X% D
  11.   valuesMR = analogRead(MR);+ u5 z3 C+ L; Y( ]$ V
  12.   valuesMM = analogRead(MM);- l8 F" m& j4 H- E& |( S1 }- n. p/ e; L
  13.   valuesML = analogRead(ML);  ?- ?9 I: n7 o% K
  14.   valuesLL = analogRead(LL);1 [1 n$ y' @. _

  15. * w; O9 Y  P5 S4 `4 Q8 B
  16.   if (valuesRR > CNY70Val)$ \7 A( t0 v" d+ I/ f: R
  17.     valuesRR = CNY70Val;
    & M) a# \5 {8 s2 L% n
  18.   if (valuesMR > CNY70Val)
    5 _, z& j5 y/ W0 `' n/ R
  19.     valuesMR = CNY70Val;" ]0 b6 ?+ T2 q4 T& o; L
  20.   if (valuesMM > CNY70Val)
    7 G8 k# Q4 N* N6 K8 t
  21.     valuesMM = CNY70Val;
    ( T0 l5 F: |7 E
  22.   if (valuesML > CNY70Val)
    1 C& D" A+ b0 J* F; d! M: ?& ]/ V! J0 J
  23.     valuesML = CNY70Val;; m! O0 V  x+ s
  24.   if (valuesLL > CNY70Val)* E/ G& H) V9 L4 t9 n6 x
  25.     valuesLL = CNY70Val;' }* x* M' W% p# N4 q- z  U
  26. 2 e2 L/ N- }0 v7 |
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    6 m  x8 k* D2 d+ h6 V! q* y
  28. }9 }8 n( w/ F6 G

  29. . W$ K3 ^7 J) Q, u4 b
  30. void Car()
    7 i" y4 }# N: v1 x0 ^5 ?" [
  31. {: A! N. W" G# x" m0 K0 `
  32.   while (1) {
    ( N. K" X7 e, c% q: [
  33.     CNY70();. i. t% k0 Q' w3 T, J& A

  34. ! c" R& H5 g- H# ~
  35.     int error = ((int)values);
    5 b9 V4 _5 g7 Z% W* R( T' t+ j+ g# ?: g
  36.     interror += error;9 ~8 I; S( H: O3 \& O
  37.     int lasterror = error - olderror;
    $ N3 E1 c2 O* n" a  L0 l" q
  38.     olderror = error;
    / F2 s8 _' T! f( E
  39.     int power = error / 5 + interror / 10000 + lasterror;
    ; t% w8 T4 e' s4 R' [) c# L
  40. 4 H: y- |  K  }; I. t
  41.     if (power > MotoSpeed)
    - S7 i8 f/ F/ z
  42.       power = MotoSpeed;
    9 b4 P' k. V1 ~0 W
  43.     if (power < -MotoSpeed)
    * T" U- v0 H$ b6 c* y+ [6 h
  44.       power = -MotoSpeed;
    9 h7 J" n9 N+ R5 t- w

  45. * U1 `3 H: |3 z8 |1 G
  46.     if (power > 0)" L* d# z2 |- A- }- f4 g
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。8 x0 t8 X# |% L% c6 X
  48.     else: i$ `' c8 ]. q7 W* Y$ T
  49.       Speed(MotoSpeed + power, MotoSpeed);* F# x9 k& {) x: |$ x" Q9 W  E  t
  50.   }
    ! R! i( D3 V4 y. @& R
  51. }
複製代碼
" `( D) g% G, [; v; l

" V, A! H: D' O- h8 a+ ^
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-12-3 03:25 , Processed in 0.024994 second(s), 17 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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