圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21692|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>
    2 t$ _/ F2 _3 f
  2. #include <Wire.h>* k  |( o8 t9 _
  3. #include <Servo.h>+ i( ~- @$ l' P3 m! }
  4. ; j0 }( }& v% t# P' C  i  Q
  5. #include "MePort.h"- S: V2 p. d4 R! c( o
  6. #include "MeUltrasonic.h"
    7 ~5 I0 I0 ^/ ~% D; T
  7. #include "MeDCMotor.h"
    . v3 A. q% c# t; r; G

  8. ' V$ m7 g; F+ ]% |
  9. //double Input, Output ;/ C1 n. Q# o+ p+ t' I' r% U, C
  10. float MaxSpeed = 255;' z" I/ P  W2 R& G+ s
  11. float MaxPower = 180;4 X( E4 r1 I1 z) [1 @2 t
  12. float MinPower = 120;) o7 y* P# ^% R/ W# |
  13. float Error,ErrorAcc,ErrorDec;+ ^  P$ x$ x; l$ {3 N

  14. : ~2 H8 P/ X& ]& S% f' v0 Z. U
  15. float Kp=0.14;
    * P; u" r) K  Q" N4 _& c2 w% h
  16. float Kd=0.00020;//23;: D6 X( W& i1 i
  17. float Ki=0.000201;
    ' y* [0 _# U0 d# w  T
  18. 6 L0 v; t. d! ^" m) K, N" z  v
  19. float nPower;9 S9 n& u- f/ R# E
  20. MePort lightsensor_6(6);) ?1 U" r1 f8 |, B' S
  21. MePort lightsensor_8(8);
    9 S8 N6 P9 @% U# v0 U
  22. MeDCMotor motor_9(9);+ j! `+ U+ U/ a3 R% Y
  23. MeDCMotor motor_10(10);) w1 C$ v0 E# q) j
  24. unsigned long previousMillis = 0;9 w9 M; D' L) n$ k) Y# D% T
  25. const long interval = 1;* R; s# E& p3 B1 n3 I

  26. 1 y; d1 {! d4 |+ H' t
  27. void setup(){
    9 `' C( T3 _0 J6 c  g3 f. H) T8 Z
  28.     lightsensor_6.dWrite1(1);) S3 N: [( m5 l) F5 f6 k
  29.     nPower = 160;
    1 j* Z% A7 v2 i# H* f) O2 _+ t$ g
  30.     Error=0;( ^- y5 ~9 r) l/ O8 z" O6 Z
  31.     ErrorAcc=0;8 e9 n! O; z( B( F; Q( T$ P
  32. }# l1 `- q* H) h8 X- g
  33. 9 ^. W9 v, q+ v, i$ w& Q
  34. void loop(){
    5 C' u5 T* S- ]7 l- R
  35.   unsigned long StartTime = millis();% ]8 P$ M: N7 G1 }
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){, c2 S% ]" m; B$ q+ H, q- f
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();2 f) T/ z& h# L) ^4 J% ?
  38.     ErrorAcc +=  nError*Kd ;
    4 O3 C& j9 R- t
  39.     ErrorDec -=  nError*Ki ;
    9 V: R1 i9 `& F  p. `' i, I
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    3 r$ G7 I+ |" l0 R
  41.     if(nError < 80 && nError > -80){: F9 X' ~4 \  o  u( l1 W
  42.       if(nPower < MaxPower){9 s% ]' y1 z8 z8 q' W$ o' k- W
  43.         nPower += 3;# q& v8 {, x' `% @, N
  44.       }
    ( t6 D; k3 e1 G6 s' |. S* H
  45.     } else{& h' L: {' b3 f$ J# A) ]  l
  46.       if(nPower > MinPower){9 m: `& a' I0 a# a' f' r+ f5 Z& h
  47.         nPower -= 2;+ S9 X- i) [  w% e) d/ @( ]
  48.       }% q+ k# h* ?; M+ d" Q5 \3 K
  49.     }
    8 B" U9 s1 }, t* {0 c# d
  50.     MotoL(nPower-Error);' ?% c4 h: D4 r9 n! M
  51.     MotoR(nPower+Error);   
    & }. c: k) g& g+ i& u
  52.   }else{
    & t/ u: y9 i+ t& a
  53.     motor_9.run(0);
    / U, L  O) c: O1 h1 H) U
  54.     motor_10.run(0);1 H0 `+ [- g& z4 j
  55.   }' b# N, y. l  f0 n0 u" N, s4 {
  56.   do{}while(millis() - StartTime < interval);1 A0 S0 B6 f' x% L* c
  57. }
    0 J5 a8 m5 M8 u( |: A- j

  58. : d; {: S: |- _7 S6 A$ U
  59. void MotoL(int Power){6 c! ?# ~) u- U$ b* q) H
  60.   if (Power > MaxSpeed){% B, A6 E$ s: v8 M
  61.      Power = MaxSpeed;; w% Z' x( U* C+ Y! `7 ]2 y8 \% ~
  62.   }
    + c) X+ d: }! u2 Y
  63.   if (Power < -MaxSpeed){. N+ L7 E1 B" X1 b
  64.      Power = -MaxSpeed;
    9 M5 `& H  C( R. }, ?; s' s! K
  65.   } , R$ i; q9 a0 X9 F! H
  66.   motor_9.run(Power);
    ' y2 ]7 B3 P3 L7 i+ o
  67. }  
    8 n+ U6 z9 @! ]6 B& M; T
  68. 8 e! p7 k. f. w+ B5 y
  69. void MotoR(int Power){
    2 |" g1 X. w. [6 m
  70.   if (Power > MaxSpeed){
    . I( b# l4 g8 ]3 d' i+ D$ H
  71.      Power = MaxSpeed;4 x& z; |8 ?/ P* ]6 q
  72.   } / Q6 l( {( z* Y" s0 e- f# i
  73.   if (Power < -MaxSpeed){
    6 H' i+ i" I  M7 e" L
  74.      Power = -MaxSpeed;
    * E, u& Y5 p; z- Q' J3 O+ z2 _; ~$ P
  75.   } % F  a8 [/ w2 Z# h
  76.   motor_10.run(Power);9 G. I" t1 Y+ x+ v0 u
  77. }  
複製代碼
: e* e9 z: w& B8 U  t( T6 G0 p

, c5 ]5 I9 j! [7 E; r/ e7 P* c% c
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯 1 k) l2 c/ t% \; D6 b
6 ~5 Z2 M, s5 M; v0 D
您好,不知是否能向您請教。
% {3 I7 P8 u( K, ^2 Z( _4 Q目前和宋修賢老師在處理Ardui Car
4 i/ [0 I2 z. z雖然已使用較繁雜的方式處理了跑出黑線外的狀況
# U5 d2 j; Z( o1 a- v7 d
! \' b  l. r' v但基於想追求更精簡的程式所以還是想請問一下5 p" M# A2 e& G: ~
就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝% @5 M$ n0 P2 |
不知道您是否願意教我可以如何處理
+ U' E2 d1 a: K1 T+ H7 O; a6 _* z: A' D  j& t2 X
' S" Q/ B9 p; `' i6 W* l
以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;
    4 _% R; d* J2 `+ h# X" d0 B% }
  2. double CNY70Val = 1000;
    . Y! N2 y, B: y5 r8 U! a9 q
  3. int interror = 0;
    $ s7 f& D9 t, H- H7 n4 A
  4. int olderror = 0;/ V4 L3 x/ ]/ d( A
  5. double values;6 _5 g0 f9 S, I, q! ~# t
  6. ( \" u8 M' r/ Z9 G+ l$ u

  7. ! w; R1 |5 U# I* ]
  8. void CNY70()8 q0 S1 N1 H- F
  9. {
    $ v4 ?5 e$ N* J0 r7 R  r/ g4 W6 x
  10.   valuesRR = analogRead(RR)
    & {2 x/ F$ ^  l. M' h5 G+ K/ `
  11.   valuesMR = analogRead(MR);) Q( W$ t! F1 Q! D" P  Y
  12.   valuesMM = analogRead(MM);
    6 g3 v. p0 N, y6 z. I  V' \# n- b
  13.   valuesML = analogRead(ML);
    1 j! {% ^- H+ U+ c
  14.   valuesLL = analogRead(LL);8 V- k, E) s# r% P; e: D7 ^/ g% ?
  15. 8 F6 Z: l6 ?1 r
  16.   if (valuesRR > CNY70Val)! x+ ]( ]5 B. C1 R9 ]! Q4 o
  17.     valuesRR = CNY70Val;- C: l  K4 v1 _1 ?1 F: x' d; i. j  {' z
  18.   if (valuesMR > CNY70Val)
    4 P' N' T  U, i& l# b/ c) O! u' w
  19.     valuesMR = CNY70Val;
    ; |# p7 H! \2 A% p6 J* M9 F7 [5 X
  20.   if (valuesMM > CNY70Val)
    ; {8 d5 ?( H  s2 y* p0 u
  21.     valuesMM = CNY70Val;# x+ G7 Z- \3 Y$ j# |, _( Q
  22.   if (valuesML > CNY70Val)' l0 N# c4 O3 `9 {
  23.     valuesML = CNY70Val;
    , }* n" Q# `) O! S  q  ~; y
  24.   if (valuesLL > CNY70Val)! Y0 J6 t8 ?7 I
  25.     valuesLL = CNY70Val;
    ( ?( N  v9 K7 y
  26. ! @% x+ F; Q; F* `5 G
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    : e" o9 f/ F. ~$ ^, E# e( a
  28. }1 L8 E( Y3 s1 _
  29. , j, I" Y2 o$ K- A' r
  30. void Car()
    ; |+ j' j* o' H3 f) J
  31. {5 }# T% l9 D# }( G3 q0 r
  32.   while (1) {! I5 u* P+ M# M4 `) m& n+ \
  33.     CNY70();
    - [: }8 ]' Y3 ?" ~
  34. 3 b( R& P* n/ M& |# d9 O/ F/ H
  35.     int error = ((int)values);
    , N) V' Z$ W. H
  36.     interror += error;3 Z$ H. m) ]  s0 c. J( }
  37.     int lasterror = error - olderror;
    - i* E- _" W/ A5 B/ C  W
  38.     olderror = error;5 [: P( \* d3 F/ Q; Y6 D" B
  39.     int power = error / 5 + interror / 10000 + lasterror;6 z, @- E& U+ n0 s

  40. , |$ r9 c* t6 p: }2 r. d* M
  41.     if (power > MotoSpeed)
    + B, Z1 k$ l  G6 ^* x8 o% K  j% e
  42.       power = MotoSpeed;
    - H1 Q/ V1 f$ V' B
  43.     if (power < -MotoSpeed)
    4 Y/ l+ f& f4 Z9 j! S# |
  44.       power = -MotoSpeed;/ z# Z- ?; G# A1 _! x6 G

  45. 7 Z- `8 n; h! |5 o% |
  46.     if (power > 0)
    ) n# @0 u# G1 U: R" c
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。
    3 M; F' W* f6 Q7 ~
  48.     else
    % Q* X0 E  D( D- r; R
  49.       Speed(MotoSpeed + power, MotoSpeed);( B! h7 i1 G$ z
  50.   }
    ' c+ ^2 l7 W. I2 c9 L: o
  51. }
複製代碼

' m4 }7 {9 D6 j2 z9 C' j* A) n. q, n
; n0 @7 ^* t$ j# Y
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-11-22 11:22 , Processed in 0.021194 second(s), 17 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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