圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21857|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>$ E8 H& K6 h% H, c6 t) ^
  2. #include <Wire.h>
    ! H# h& X' v/ j6 ~% ?' E
  3. #include <Servo.h>2 ^- V5 P" F1 e* z" x
  4. ' ~1 [) c$ U, v% X  o1 V
  5. #include "MePort.h", o6 I9 W+ d" _  M; {; C; z$ ~
  6. #include "MeUltrasonic.h"5 t( o& w  o7 T0 J4 ~+ z
  7. #include "MeDCMotor.h"( E' K; k3 y: L  v2 V; R

  8. % g8 y+ B# K# l0 F  F2 p
  9. //double Input, Output ;. N0 |2 H# V6 B+ a6 r6 p
  10. float MaxSpeed = 255;4 S, P& G% {4 g8 ]2 |  h9 W
  11. float MaxPower = 180;$ h7 g$ r5 L/ s. u; J! F
  12. float MinPower = 120;# M% X; t$ e/ Y3 X
  13. float Error,ErrorAcc,ErrorDec;
    % ]) _; }$ u; a8 U

  14. ) ~! v2 P' o2 `. `: f
  15. float Kp=0.14;9 b3 `( F, L9 X- ]$ [
  16. float Kd=0.00020;//23;
    4 V  ~) l+ t2 O5 J$ G) w8 }
  17. float Ki=0.000201;! O" l" b0 ?: X. b
  18. # j5 g7 R- s9 K
  19. float nPower;
    / A" }/ i1 a) P3 z  b1 g' o( ?+ G
  20. MePort lightsensor_6(6);
    2 ?! y: j4 ~7 _# A1 B' R- u
  21. MePort lightsensor_8(8);* q* Q3 N" i+ n7 w2 m
  22. MeDCMotor motor_9(9);
    5 y. N! f& b9 E# ~& L- \) w
  23. MeDCMotor motor_10(10);! x$ l; N7 ^% _# N( f& B. Y, p" c- s
  24. unsigned long previousMillis = 0;. e7 S- D5 ?5 A8 N5 |
  25. const long interval = 1;) G9 r3 s; C2 c0 w# y
  26. * x1 q4 B6 `. X" H) c- r! N
  27. void setup(){% s, C' [4 j. J
  28.     lightsensor_6.dWrite1(1);' Y  L2 e; R% g# C# L
  29.     nPower = 160;
    + E( a1 l7 `# F
  30.     Error=0;! _  z  G7 _( V% d4 g5 o3 e; y% \/ e
  31.     ErrorAcc=0;6 [7 z2 N. |0 m" [- O# u
  32. }8 X$ h! w' C& G( I
  33. % S& q8 C# w" `% f5 [( Q
  34. void loop(){
    + Q: y3 Z0 Q. U& W( w
  35.   unsigned long StartTime = millis();
    1 @: d$ }" C2 t8 t
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    & z. T& t7 O5 {1 l1 D" j, G
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    % f1 N- \4 v/ V7 x+ n
  38.     ErrorAcc +=  nError*Kd ;
    . [7 U5 o1 i8 u# k
  39.     ErrorDec -=  nError*Ki ;, K4 y& i% u# P$ Y8 J
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    7 S2 E0 r4 Z* B! P
  41.     if(nError < 80 && nError > -80){
    , c( A  G5 ~) @( J. d1 K) N1 a
  42.       if(nPower < MaxPower){
    $ N$ U6 n' u- T5 w$ h
  43.         nPower += 3;; F1 D( W6 S  B" \8 D6 z2 @
  44.       }: g* h/ N9 k; z$ _( K9 K/ ]
  45.     } else{* g7 _% E7 R- I& j. h
  46.       if(nPower > MinPower){9 N9 Y: c* _- m' i# W3 O% R
  47.         nPower -= 2;% r+ [* I1 R  H* m% q
  48.       }8 m! T0 y- a2 c  R+ ~6 a4 |
  49.     }
    5 J/ J( F. A# i! v; J2 a7 g& {
  50.     MotoL(nPower-Error);# U' w- C# w& H% G2 H
  51.     MotoR(nPower+Error);   
    " N& B0 T2 b0 Y& U, x2 Z; l
  52.   }else{5 f) U1 P+ D% `; u4 f6 P- `
  53.     motor_9.run(0);' }: d8 {  l5 Z8 ^1 i
  54.     motor_10.run(0);
    . b) _$ i* B$ L7 E! S; _6 J7 c
  55.   }
    1 q' S1 t. }4 I9 E  w' r
  56.   do{}while(millis() - StartTime < interval);  v8 H$ M' k( m9 B# l
  57. }# Z9 w3 j) u1 Y+ {( V* h0 B: E0 ?

  58. 3 |' n" i/ i+ Y2 F
  59. void MotoL(int Power){
    + _8 \) v0 I8 Y* \- Z
  60.   if (Power > MaxSpeed){% u; H3 [, F7 P6 v
  61.      Power = MaxSpeed;
    5 n5 w5 C+ M3 r7 s# p* N- O
  62.   }
    8 s$ Q: r  d0 h6 x2 A
  63.   if (Power < -MaxSpeed){* q1 i( c- q0 z7 B! g
  64.      Power = -MaxSpeed;
    # C( m" }  m+ ]6 V
  65.   } ( U7 ?0 {# H( G; @% K
  66.   motor_9.run(Power);
    $ N0 W' U) l  t' \3 k0 J
  67. }  9 f! s: ^+ [4 Y  J5 l2 u

  68. 2 r( Q  G7 W/ A4 J8 ~9 K; c
  69. void MotoR(int Power){! `) W* v# b+ a/ h9 P& _
  70.   if (Power > MaxSpeed){% a! H; x! U( a/ \# b
  71.      Power = MaxSpeed;
    , k* T4 E) |9 S1 X7 Z; {
  72.   }
    : k& U9 p* h" j) I# l. k
  73.   if (Power < -MaxSpeed){
    & G; U2 g9 D+ l) I! J1 M& f  p4 A) Z
  74.      Power = -MaxSpeed;/ S+ [) o1 t+ j9 w( T  d2 ]. ^6 P
  75.   }
    ; p3 ]3 K+ D( }" {
  76.   motor_10.run(Power);3 i5 F  U2 n0 L- `( S
  77. }  
複製代碼
( j2 [% P- s$ g, i
+ X3 A  J' k: A
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯
3 ]* N6 T' P0 x0 }+ E% `5 L- X" [. T; o
您好,不知是否能向您請教。: g+ J% y4 ?' T; U, O& ~
目前和宋修賢老師在處理Ardui Car3 @$ R6 q2 e& |
雖然已使用較繁雜的方式處理了跑出黑線外的狀況9 E0 e- N- g+ I" `! Q/ j% z$ `

* _( o$ a. D) J- a" X2 B9 U但基於想追求更精簡的程式所以還是想請問一下8 N' G$ A% t  q) c1 {4 v6 p1 F5 u
就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝- T9 E0 ?+ B: J$ y4 f
不知道您是否願意教我可以如何處理
% Z" n% R4 p3 X# u9 H4 v3 B3 [1 z3 f4 N( z

6 s; M  a& O( X0 ?/ s. M以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;
    ) b" I; a- T" c+ }# P% b
  2. double CNY70Val = 1000;4 G, h% S1 [  o
  3. int interror = 0;' M5 T9 `7 G7 @3 X
  4. int olderror = 0;
    : p. y1 K* q/ U$ D; o7 j- M
  5. double values;
    - v  z& G7 |, T) U
  6. " s& G% r$ Z  q- D- j. y

  7. 7 {' y: B* R5 s8 A8 b* ]
  8. void CNY70()
    % q9 {* B9 G! O9 l9 v. y$ ]% G  t! P  ~
  9. {" v) V; z' H8 t) _; g0 W  c3 R
  10.   valuesRR = analogRead(RR)
    0 R+ k- U; f9 v( j+ M1 b
  11.   valuesMR = analogRead(MR);
    ' }' @$ D6 O9 x# ~# D, E( ?5 X
  12.   valuesMM = analogRead(MM);
    + o* g" U2 F/ y' }5 p& p( ^
  13.   valuesML = analogRead(ML);
    , p% E5 Q8 Y; B" L0 f" J! W
  14.   valuesLL = analogRead(LL);
      H0 C: f  Y/ G

  15. 7 B, t* o. T! ~: x: Q
  16.   if (valuesRR > CNY70Val)# y. B6 C( |) Z% W" s
  17.     valuesRR = CNY70Val;" T4 Q9 _/ ?5 {& Q6 V  S, R$ U
  18.   if (valuesMR > CNY70Val)6 g4 j8 M, i1 d7 d+ |
  19.     valuesMR = CNY70Val;* h5 f! x/ w, M
  20.   if (valuesMM > CNY70Val); u% J# r2 {+ v
  21.     valuesMM = CNY70Val;
    ' C% n" O: I  f  d) I* i
  22.   if (valuesML > CNY70Val)2 N  ?" q# T1 s. U
  23.     valuesML = CNY70Val;  C: g2 _' k, M4 z4 p
  24.   if (valuesLL > CNY70Val)
    9 X0 w& P2 k+ [- I9 M
  25.     valuesLL = CNY70Val;
    7 `2 M7 e' r1 Y" x

  26. * c9 ~; M' K$ q* V9 V/ r
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;1 ^+ z+ z1 R' y  b
  28. }
    : z1 L" j% ]2 I; t5 V. H
  29. ) t( {9 @# K8 i5 f7 L
  30. void Car()
    - ~' @, V/ i0 c
  31. {9 p* X' R8 X/ G! q
  32.   while (1) {
    / \6 O+ |+ l8 M! ]( f9 E5 m
  33.     CNY70();6 O5 U9 p* d( F

  34. : G: p- Q& h' a7 Z" a, _# T+ ^
  35.     int error = ((int)values);
    ) n4 Q  A2 d" |2 s0 P- Y# \+ l
  36.     interror += error;
    1 v( t5 k" ^8 x4 t  b9 Z
  37.     int lasterror = error - olderror;, h2 D+ D+ P9 ~, {
  38.     olderror = error;; L# R8 T7 a) ?9 _) t, M2 S
  39.     int power = error / 5 + interror / 10000 + lasterror;9 v# Y6 F6 y9 `* J7 V6 S9 b8 ^

  40. 9 T/ J: o' d0 A; q6 \
  41.     if (power > MotoSpeed)  W+ F9 a" D9 \; ~/ g
  42.       power = MotoSpeed;9 h3 g# j+ @$ I" V" x+ p2 O, s
  43.     if (power < -MotoSpeed): @: z: V3 K# f! }
  44.       power = -MotoSpeed;5 b+ j+ ~- g. y( s; \5 m2 o7 k4 [' Q8 G

  45. + n" Q- Y" `+ F( w
  46.     if (power > 0)" }, U0 d, V$ T; _& d
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。: a; x6 y  k! Z) y; C
  48.     else
    8 ^8 \3 i6 Z, M6 e
  49.       Speed(MotoSpeed + power, MotoSpeed);1 x* `% p! ?# h5 B$ ~. d
  50.   }
    * V' V! j* E7 I6 v6 `  k2 i
  51. }
複製代碼

) w7 P4 i6 ~4 k: X6 U+ }4 |0 @, V, l7 Y. E/ c8 S
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-12-6 03:32 , Processed in 0.025117 second(s), 17 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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