圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21809|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>
    7 w0 q2 s1 s5 F+ ~; `
  2. #include <Wire.h>
    4 Z4 ~6 [# \( t1 f
  3. #include <Servo.h>
    . u" ^) U2 E  @- b  u/ Z2 g  f

  4. $ o. H6 r; i( H- Q0 M% r
  5. #include "MePort.h"2 |# _5 q" p; l0 N
  6. #include "MeUltrasonic.h"
    $ a2 z% W+ U5 G* a3 F& g
  7. #include "MeDCMotor.h"
    3 U; k0 c" w* C' d5 i4 v
  8. ) E0 x2 f: j. D8 t* A
  9. //double Input, Output ;5 s& }. r  ^9 }; \! k3 b
  10. float MaxSpeed = 255;; i! g/ L- L/ j3 m5 D2 P$ p
  11. float MaxPower = 180;
    ; ]/ x/ z- `3 T
  12. float MinPower = 120;
    6 D1 J- s% e& e+ ?  [7 E
  13. float Error,ErrorAcc,ErrorDec;# x$ f9 k7 j% P/ m' m7 Y0 C
  14. " y  i+ a5 e% F7 p2 i
  15. float Kp=0.14;3 H2 [) A- F/ K  ^
  16. float Kd=0.00020;//23;" F" u6 w! n& O( M
  17. float Ki=0.000201;
    # C& l4 Y& }, C3 d. F

  18. # W$ V: q9 i- @. s6 W
  19. float nPower;
    # x. O! j' `7 P4 \
  20. MePort lightsensor_6(6);9 u, W1 }, O/ s
  21. MePort lightsensor_8(8);7 }' U- X9 p& d2 E# D
  22. MeDCMotor motor_9(9);
    % u+ t9 L  _* b* ^
  23. MeDCMotor motor_10(10);
    2 ~# K% t& o' E8 s5 p. H' x+ d
  24. unsigned long previousMillis = 0;% m# P3 j/ G9 @% Y# L# Q
  25. const long interval = 1;! S& A) b  y# n, n, V$ [% X

  26.   @& \2 @( k6 ]; u
  27. void setup(){# P& P$ y5 l7 c& k2 k
  28.     lightsensor_6.dWrite1(1);- ^: n6 c/ X) s# O+ C  `
  29.     nPower = 160;
    0 {. X  S8 M0 m( y: X" }( n
  30.     Error=0;' p+ e( |  v: o
  31.     ErrorAcc=0;9 u3 H6 G( @8 ?) |# z" U& d
  32. }
    . S/ T7 C6 o/ W3 Y# s5 p) U

  33. 3 M6 M6 U8 [! y. u8 w+ o: v/ b0 P* _9 b
  34. void loop(){2 ~2 t" z7 M' x! B
  35.   unsigned long StartTime = millis();3 O# X: y6 e' T6 ?( t- E
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    5 q3 h& m2 Z  L9 Q- Q, P2 p- u
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    & `' f7 u5 y1 x3 M, {9 R
  38.     ErrorAcc +=  nError*Kd ;4 S% _6 N3 }+ A# L/ S
  39.     ErrorDec -=  nError*Ki ;& T6 ]- e; P7 w! f
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    ( ^+ ]6 P! A( S  R+ ]
  41.     if(nError < 80 && nError > -80){$ ~; F4 d- e8 u
  42.       if(nPower < MaxPower){0 u2 C2 M. e& S  b
  43.         nPower += 3;0 D! O# p6 c4 S4 U7 G, P
  44.       }) q7 a! o4 K0 m3 _+ C
  45.     } else{
    " `  V' q3 K) q( I# ?
  46.       if(nPower > MinPower){- i: u: [* V  d' r/ w- Q6 n/ a
  47.         nPower -= 2;" }1 Y6 a" P1 `; I8 w
  48.       }
    9 m9 g) \9 U4 i8 `& {
  49.     }
    0 U" ~2 U/ a' s$ s2 g2 Q
  50.     MotoL(nPower-Error);3 N' i7 D6 I6 d& N4 \) ~2 I
  51.     MotoR(nPower+Error);   
    # v0 R; |3 W- o( l- h
  52.   }else{
    ! [5 A6 F7 X/ ]
  53.     motor_9.run(0);
    / z/ [2 q7 Z3 S4 {  O( v$ Q
  54.     motor_10.run(0);
    2 y  ~* N7 d" ]5 o
  55.   }3 d, c1 \$ V. t
  56.   do{}while(millis() - StartTime < interval);
    1 X5 R$ w& @  i8 W1 n* w4 w
  57. }) ?  ^- R; G$ W: `. h1 g: S

  58. # G/ |! @5 h! L; I
  59. void MotoL(int Power){+ s7 i3 L- o0 o: V7 R
  60.   if (Power > MaxSpeed){
    . [: M* O# i% j1 k5 B  s7 |, S( _4 W
  61.      Power = MaxSpeed;. s( T9 C8 x$ i7 j( N
  62.   } $ _9 {* A9 _9 L) F: d
  63.   if (Power < -MaxSpeed){
    - {4 a2 p6 K% t0 B5 x
  64.      Power = -MaxSpeed;! V* m& ?7 \) M# [$ |
  65.   }
    $ ]) H& p+ W. \
  66.   motor_9.run(Power);. Z3 M/ U& @* g2 O1 X
  67. }  , d4 R2 E# m. a+ e, G) \& N5 m
  68. 6 g& \& E) G3 i
  69. void MotoR(int Power){
    7 \6 N8 ]- W+ t9 D6 S6 c+ d
  70.   if (Power > MaxSpeed){: w3 e% Z% h+ H8 }4 _' s
  71.      Power = MaxSpeed;
    7 g( v  k" U$ e5 P
  72.   }   o( y6 I2 \& A+ P' O$ Q+ c
  73.   if (Power < -MaxSpeed){% {3 u; D* S# s0 `9 n0 a- K
  74.      Power = -MaxSpeed;: a; C5 w# ^, }: I
  75.   } 9 ?$ S- Q% I. f- }6 ^% |% a7 T5 t; w
  76.   motor_10.run(Power);- [" k( i% ?, k1 B9 v) ]
  77. }  
複製代碼
3 m1 G- g! Y! ?

/ ]/ M3 B$ K8 N, G" i
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯
( T) t7 ~: C9 U& `  e/ h
& J, o  \9 a# `" q% i' T9 y您好,不知是否能向您請教。! y- M; u4 d! f% p$ y  b% _
目前和宋修賢老師在處理Ardui Car
% Z6 z7 {1 y5 k. W9 F% Q( H雖然已使用較繁雜的方式處理了跑出黑線外的狀況: Q0 o: S1 w/ A* W  J: t% ~2 I1 \

5 |5 S4 B6 V+ ^) L  d但基於想追求更精簡的程式所以還是想請問一下
; p& @; s+ v! b+ x3 F就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝
  u) M6 P) ~( `3 d不知道您是否願意教我可以如何處理
! \  r; b% C- k9 r3 n
0 Y, I% T! B& H8 A; m# z- k) E3 r0 M# P$ Y
以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;6 |; H- n9 _! A8 w+ T
  2. double CNY70Val = 1000;
    % o  x- v  X+ g* I5 C) i
  3. int interror = 0;
      c8 i% D$ K/ U! k+ o
  4. int olderror = 0;9 H: o8 f& |/ h: u; Y, e6 @
  5. double values;
    : a6 V3 V) n+ B. H+ P8 c( R" \
  6. 3 w9 H% R" I9 @4 C; R% s, e
  7. . }6 P+ m6 q2 g1 W6 j" X7 R
  8. void CNY70()) w3 h, v, z- T, ?
  9. {
    4 i2 D1 l5 y5 _# ?; T
  10.   valuesRR = analogRead(RR)
    3 g# y* E2 P7 X3 v' K" F
  11.   valuesMR = analogRead(MR);( d* s) H& k& e) c, i4 G
  12.   valuesMM = analogRead(MM);
    9 M0 D. i2 t  y& a' `* H0 X
  13.   valuesML = analogRead(ML);
    6 S$ B& |2 b/ E4 A- `! M" y3 F
  14.   valuesLL = analogRead(LL);2 g* e4 b: J, [% x5 Z7 Z* R0 I% e

  15. $ c1 w8 ?, S+ K! d; H! B: p  m
  16.   if (valuesRR > CNY70Val)
    / v% H6 @- M. h+ Z2 |
  17.     valuesRR = CNY70Val;2 h# T/ @6 F( {) r; Q3 ?8 q5 ]
  18.   if (valuesMR > CNY70Val)
    6 m, d5 e, O* E
  19.     valuesMR = CNY70Val;, R; q, n$ \1 u+ H, W/ X
  20.   if (valuesMM > CNY70Val)2 `3 G9 E9 Z. E& G* e+ f
  21.     valuesMM = CNY70Val;
    2 A3 _$ w0 W; x* s8 J$ o/ f0 B3 l
  22.   if (valuesML > CNY70Val)
    . ~* n& p0 V' ]$ O0 y) D
  23.     valuesML = CNY70Val;' f6 ^* K$ g& {
  24.   if (valuesLL > CNY70Val)
    5 x4 J& ?; b4 i  R0 n+ k  @) X3 W
  25.     valuesLL = CNY70Val;; u6 l3 H" k% ^  L- b+ a

  26. 0 C3 p  e+ s+ d* u9 T1 }4 ?5 ?
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;" s" _) @( l* }( d( e8 U: H
  28. }
    ; T5 a5 M, Z3 P

  29. : n$ X6 ?' ^/ }* r7 `1 i" @
  30. void Car()
      W( i" V* ]% ?! f) d
  31. {6 c" `4 [" e3 {# v
  32.   while (1) {) @- F; l" ]% [
  33.     CNY70();. l  C( Q- [1 t( F- s

  34. . v# F  p, m; w4 E, }
  35.     int error = ((int)values);5 _6 T9 ^- t5 f% j
  36.     interror += error;
    2 ^; P; N/ T) j6 n" p/ \
  37.     int lasterror = error - olderror;
    : M7 P: C/ E5 \0 A" S0 \  n5 g
  38.     olderror = error;
    3 \% D" [# ]- e) {0 e) K6 U3 R
  39.     int power = error / 5 + interror / 10000 + lasterror;# H8 ~7 S% G) E/ I9 B; Z
  40. 1 q4 x: d, l  x7 i8 {
  41.     if (power > MotoSpeed)
    ( u! ^) X! S: t1 m$ e& u% Y4 U) `# d
  42.       power = MotoSpeed;8 J& ^, u6 k! J3 n( D
  43.     if (power < -MotoSpeed)# E5 L6 a0 ]( ]# ?
  44.       power = -MotoSpeed;
    - h% z) I9 V. ~6 i% e9 }4 `: v' C

  45. 4 K0 @: d- j/ }' n% N/ G
  46.     if (power > 0)
    2 F$ b% r( T1 e7 [7 z# l* k3 k' F' F
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。
    ! q# e( K9 D% O/ r; v
  48.     else
    ; `; _3 R9 I5 y/ O. X9 D+ v0 j
  49.       Speed(MotoSpeed + power, MotoSpeed);5 _: u2 `, j, s$ a
  50.   }9 b1 |$ L: ]1 B; \, e
  51. }
複製代碼
/ T; ~0 y7 I% s* J6 D6 F( e
) l) N- [# Q! L; a5 }% W2 b
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-12-2 13:50 , Processed in 0.021061 second(s), 16 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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