圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21695|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>
    6 T9 H1 l* f# D
  2. #include <Wire.h>
    * Q6 z3 t$ P6 K; V/ l2 ^
  3. #include <Servo.h>
    # m' Q$ }" Y! q6 r$ ?

  4. ! _* o9 |" n! y6 g( e/ C
  5. #include "MePort.h"1 s- V% X* H: Q. U1 b6 T
  6. #include "MeUltrasonic.h"
    : Q$ Q- p1 ]$ k: i
  7. #include "MeDCMotor.h"
    $ M2 C& a$ o4 [- c6 Z9 L! B  Y
  8. 0 h% ?' {* D# w7 t5 g( d$ p  y
  9. //double Input, Output ;4 q; e- v0 A4 a( O$ ^+ z6 C. s$ L7 D
  10. float MaxSpeed = 255;% B7 X0 w. n4 M  ]7 H2 {
  11. float MaxPower = 180;/ V8 V+ G; L/ A! O
  12. float MinPower = 120;, @! a: R9 E0 D( L& o7 V- f
  13. float Error,ErrorAcc,ErrorDec;8 A7 E+ a6 I( [' C- e
  14. 4 n$ P% g5 A3 g( V( ^: ~2 g
  15. float Kp=0.14;
    " T! c3 m% f% Y/ H' o2 m6 ?
  16. float Kd=0.00020;//23;
    , A9 m+ o0 T; B* K+ }
  17. float Ki=0.000201;
    # L8 [3 m% l. g

  18. " n1 k+ v) D1 g* t
  19. float nPower;  L( b' J2 E8 }0 S3 y
  20. MePort lightsensor_6(6);
    ' c" L7 g9 V. j2 m5 }8 s
  21. MePort lightsensor_8(8);* M0 ?  m' x5 _  Z
  22. MeDCMotor motor_9(9);
    * x0 v, f& \' ~: U* K, G
  23. MeDCMotor motor_10(10);
    5 q0 _3 j+ i( Q, k+ {
  24. unsigned long previousMillis = 0;$ L, _) N; u' l7 q+ d; [' x
  25. const long interval = 1;, |0 F; N' W8 s5 q9 c- I

  26. / [+ M; d( H$ ^( b
  27. void setup(){
    - [; e+ f1 Y, e
  28.     lightsensor_6.dWrite1(1);; o3 W) G: Q7 W: r) A
  29.     nPower = 160;
    2 f! p% x5 ~$ ]5 n/ s4 l; i. H
  30.     Error=0;" q( M; n: I3 ~3 t; b6 k- v6 A, C: \
  31.     ErrorAcc=0;  I& }. Z7 o. O3 b- k; Y, f. n* _
  32. }- k( K; I  p' `9 C8 Z3 A
  33. " s8 O$ s5 w6 v" x! P
  34. void loop(){
    ; @. t* v+ U: k% S
  35.   unsigned long StartTime = millis();# o# b6 w' M$ w4 N$ j1 a2 d
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){% t3 T( j) |3 O: P" t
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    % ]( g- [$ V5 e4 R. l, u/ r9 \
  38.     ErrorAcc +=  nError*Kd ;
    6 y* m  K3 |2 F8 ?* n# m
  39.     ErrorDec -=  nError*Ki ;6 ]9 C7 E/ L$ O
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);+ w, K; }) j( d- v/ d& J- I
  41.     if(nError < 80 && nError > -80){# T* b, z" \$ m+ s9 ?  I2 y) n
  42.       if(nPower < MaxPower){& h+ {& W" u7 T7 q
  43.         nPower += 3;& p; {" o0 j1 i4 j! O% g8 S' x6 U
  44.       }
    . m3 t' `; x4 i; b8 `+ E: p& r
  45.     } else{- k, L- D5 G( ~& ~% O- g; U" w
  46.       if(nPower > MinPower){
    % X4 b5 a6 w! p8 v, I" l  r' W$ I
  47.         nPower -= 2;
    , Q& t. h& x! [) ~8 n7 G# @! p
  48.       }. O- `8 l4 ~7 k! ^9 F/ j0 @
  49.     }
    0 ?6 O$ w; m6 F4 C5 D- z: v7 z, R4 h$ ^. H
  50.     MotoL(nPower-Error);5 L, U9 P! v- `; K2 f
  51.     MotoR(nPower+Error);   
    4 e0 J- C/ T1 n
  52.   }else{
    1 r: |; k6 Z2 _& }! l( V1 e' z
  53.     motor_9.run(0);* u  `& B9 t0 \# v& W; K
  54.     motor_10.run(0);
    ) M' b$ G/ K: @  h
  55.   }" l4 w) w, k; _5 ?1 q6 r( {
  56.   do{}while(millis() - StartTime < interval);
    # e) ]% ~) T1 r3 i, l* |6 W
  57. }. c! W) m; p6 W+ U: }

  58. ( l: j, @& g6 ^5 m* C
  59. void MotoL(int Power){
    + b* C, G3 J" w- h5 e- c
  60.   if (Power > MaxSpeed){; Y+ i, r( n. u7 j' x3 p
  61.      Power = MaxSpeed;
    % m: ~) ^% q6 ~0 E4 n
  62.   } 7 o* e$ u5 N9 w
  63.   if (Power < -MaxSpeed){! R0 e' L6 \) l4 W) c
  64.      Power = -MaxSpeed;
    ! ~6 D/ n6 Q8 L
  65.   }
    9 g0 L1 E, X4 }7 ~
  66.   motor_9.run(Power);
    . q1 U  ]0 a: ~6 Y2 M& P
  67. }  
    1 n2 J8 u: l* ]

  68. # T# i6 H: d& G9 t
  69. void MotoR(int Power){% n7 L) B3 B2 t3 p9 ]" U
  70.   if (Power > MaxSpeed){9 o8 G! F0 O4 Q# M0 m/ s& Z
  71.      Power = MaxSpeed;
    " v* K; E+ t) c) V( x! `0 K' h8 o# Q
  72.   }
    7 V2 {4 b! k2 I3 q' Q, a
  73.   if (Power < -MaxSpeed){
    7 u7 U/ ^3 q8 a: |
  74.      Power = -MaxSpeed;
    5 N* O2 @9 w  b3 [% {6 n. E
  75.   }
    # v0 Z& K2 [; c; q
  76.   motor_10.run(Power);
    7 S7 L: g1 N) `7 w2 v# V& J
  77. }  
複製代碼
1 k) L  `0 a; @, D) \

* b3 V7 Q' e3 h, a/ |; Z+ m0 V
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯
/ ~& l; W% F: O9 _
6 [" T& O+ M' T' p您好,不知是否能向您請教。
  x' O; O5 H3 m% U  C; l目前和宋修賢老師在處理Ardui Car
3 z" c) n1 I' n! L9 u" E雖然已使用較繁雜的方式處理了跑出黑線外的狀況! W5 R. ^7 [7 Y7 {! f

4 X( s$ J2 q: n但基於想追求更精簡的程式所以還是想請問一下
5 v& Y6 z6 k8 w) O) l  M就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝. W& |( f- }8 x$ Y6 ]& q. {0 D! z  z
不知道您是否願意教我可以如何處理: B% H9 G5 R7 T0 P$ m, K: X3 g  [

( g) b. ]4 d$ D8 R3 E3 P, u$ I9 a/ j9 D& w) K/ b" ^% q0 Y
以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;
    - J" r* b6 e6 _3 f1 }
  2. double CNY70Val = 1000;9 ?% ]8 r4 h9 b2 y
  3. int interror = 0;
    7 a$ W# W% P% ?* v6 P! s( _
  4. int olderror = 0;
      {' U8 P3 ^% W" e. T% U2 _5 O
  5. double values;6 S! l: T8 q, _( F/ q
  6. ; E0 }! P9 s7 q. n: C

  7. 6 I# _, _  e  e! A
  8. void CNY70()
    & t3 y6 r; j; M2 b4 a
  9. {
    7 ]. \5 Q# z# \" X3 m, x/ T9 P
  10.   valuesRR = analogRead(RR)' \% y: {( z4 y- `& @% D! C
  11.   valuesMR = analogRead(MR);; J2 y) i) p8 L* |5 X
  12.   valuesMM = analogRead(MM);
    ( O9 C+ \1 l% _1 B
  13.   valuesML = analogRead(ML);
    , X( Z6 ?; n5 B5 \) O  @
  14.   valuesLL = analogRead(LL);6 s  h  x4 \# @- b6 j7 L4 w% _& M

  15. ; v3 P- v; J) l9 F! N% O5 v$ p
  16.   if (valuesRR > CNY70Val)
    ) Z' }/ }% Y% k2 P5 e
  17.     valuesRR = CNY70Val;5 P8 }) P2 n& P
  18.   if (valuesMR > CNY70Val)0 ?. o1 M3 Y: ?& m$ @& r
  19.     valuesMR = CNY70Val;
    5 @/ Q  D) l, U- o. A
  20.   if (valuesMM > CNY70Val)  L  K9 K' N! |3 L
  21.     valuesMM = CNY70Val;
      ^8 y- M! `# X4 y% `0 }- K
  22.   if (valuesML > CNY70Val)5 a, H. N$ i( o
  23.     valuesML = CNY70Val;- A, S% Z. q' f0 G
  24.   if (valuesLL > CNY70Val)5 y: C0 c2 y5 l& A7 j& Q# ~2 @7 R
  25.     valuesLL = CNY70Val;
    ; M% n# r: y, W
  26. ; D' B( r& \; `" p4 ?6 o1 v
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;. B, {' U  W1 k2 F6 g4 ]+ ?2 I
  28. }" Y8 _2 q. Z; e. S* q
  29. % X3 v, q9 w8 E7 _; e  k
  30. void Car()$ ]# i( j; Z5 g& [
  31. {1 N; l0 A5 c9 E8 V
  32.   while (1) {( L0 e6 }& Q+ X) l! m" K' C" L
  33.     CNY70();8 L) @# j& h, V, G* j- \0 b" v& Z6 s5 E

  34. # V9 @# R, Y4 ?2 A* |
  35.     int error = ((int)values);- ^6 J7 p7 A5 [- p% z2 Q
  36.     interror += error;
    ! z6 c4 e" Q  n0 _% K3 h  c8 w
  37.     int lasterror = error - olderror;
    8 J7 [5 E7 a5 T3 K% I* p, _2 F
  38.     olderror = error;
    ; o& w# c$ }3 h2 \; P
  39.     int power = error / 5 + interror / 10000 + lasterror;$ R# s( i  Z4 Y2 E* W! P

  40. 6 q; W" B- v# G
  41.     if (power > MotoSpeed)
    * {" A; c  T& Z2 ]: ]3 q1 W% t1 v, f
  42.       power = MotoSpeed;$ E! H. R3 Z: O9 E$ t; _8 v% H+ a9 I1 g
  43.     if (power < -MotoSpeed), }# y, E4 V2 V  s6 h5 h
  44.       power = -MotoSpeed;, |. E( @) q& C) B+ F: |

  45. * s( N6 }6 ]) X' l2 z9 ^
  46.     if (power > 0)
    0 w# h+ H, Y' z
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。
    ' l) k4 K3 V3 @
  48.     else- W4 }, X+ m3 E, g
  49.       Speed(MotoSpeed + power, MotoSpeed);
    ; O2 ^) J- k" N
  50.   }
    ! z( L% R' c$ ^8 F$ M7 ?, `! L
  51. }
複製代碼
2 \+ Z& p. [9 h) ?9 M

1 R0 r) f2 `4 }7 [
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-11-23 01:25 , Processed in 0.024600 second(s), 17 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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