圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21734|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>/ S0 u1 }; j7 ~0 D" T
  2. #include <Wire.h>
    . v; y8 t! ]3 o" [# A# x' ]8 H- |" E
  3. #include <Servo.h># F( A: K3 l( w6 E2 Q0 x7 J4 J3 m# r
  4. $ g$ B- F/ T2 B* |4 m
  5. #include "MePort.h"" k% _) X! c" y2 y
  6. #include "MeUltrasonic.h"
    / J1 R( m0 x" w+ ^5 i6 _# V% B; o
  7. #include "MeDCMotor.h"! D( D9 |# N5 g7 g

  8. $ w3 x6 Y& m2 R5 a. f) v: E
  9. //double Input, Output ;% E- I9 A* f; G* g" d
  10. float MaxSpeed = 255;
    & ^0 _- W, ^! f! k  R& b
  11. float MaxPower = 180;9 {7 o1 k& Z2 Z$ Q8 q1 W' f( d
  12. float MinPower = 120;7 o7 G0 B, Z- q5 q" ]
  13. float Error,ErrorAcc,ErrorDec;
    + Y! \* Q. w* k& c
  14. / ^8 \/ w2 H  {
  15. float Kp=0.14;
    " l, t) t2 ]0 ^+ ?# p
  16. float Kd=0.00020;//23;0 K' Z; H5 t2 `9 E# `
  17. float Ki=0.000201;
    . v2 v% h  v' ?! B) H/ i; n! A

  18. 7 E9 X% S5 P# d! M* Z- I
  19. float nPower;
    % i8 ~* J8 O* U2 R' p0 b
  20. MePort lightsensor_6(6);5 H. r6 Z8 d# k% T8 Z) v
  21. MePort lightsensor_8(8);
    ' |$ l9 h* l( k0 ^+ ^0 _: `
  22. MeDCMotor motor_9(9);
    6 y; i, Y, g( A8 \, d  u
  23. MeDCMotor motor_10(10);
    & z) V- E- N# B* s9 p7 x
  24. unsigned long previousMillis = 0;) \: V  ^6 k0 }% F
  25. const long interval = 1;
    ! |/ s; O7 h) F9 e) S- j

  26. 4 P: \; N3 f* t
  27. void setup(){9 q9 f  G" o7 Y
  28.     lightsensor_6.dWrite1(1);7 w( X7 a1 F* @/ U% Y
  29.     nPower = 160;, A2 P% ^" D7 P3 g$ p/ R
  30.     Error=0;* O* e9 d+ O% W2 ~- B6 i
  31.     ErrorAcc=0;
    1 V, S3 H, y5 ~5 E4 P
  32. }) d* o  V* u: l

  33. / u, H* [, q) {3 u! Y1 E3 E3 ^
  34. void loop(){$ G- V9 b# w4 }+ H
  35.   unsigned long StartTime = millis();+ V7 e1 k: D+ h& J
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){4 A; \1 v0 c  m' }7 m
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    6 F, l6 ^) \  P
  38.     ErrorAcc +=  nError*Kd ;
      \) @$ }% p! `0 M$ q# d
  39.     ErrorDec -=  nError*Ki ;, G/ }( r" C! M1 `
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);$ b7 H: ^. V  J" a) W" n4 T$ `& I
  41.     if(nError < 80 && nError > -80){
    " c; v: |% P1 p7 H5 O, G1 S1 Q
  42.       if(nPower < MaxPower){! _" n; `. I! F2 D) D* E/ Y
  43.         nPower += 3;
    " B' O' j2 p4 [
  44.       }" d4 V5 M+ o. @" A4 W
  45.     } else{
    : d+ U7 u" R4 ]
  46.       if(nPower > MinPower){
    3 q$ T- c. o; I  k, K! L7 i
  47.         nPower -= 2;/ z+ S6 e$ v/ W8 f
  48.       }
    * _4 _6 l( I* y. A4 `0 a7 L
  49.     } 2 n9 K* P& M- J  d9 q! m  \
  50.     MotoL(nPower-Error);
    3 z0 b/ ~7 o0 e
  51.     MotoR(nPower+Error);    5 p/ L/ P, v; F- r0 b
  52.   }else{0 f& M3 S0 a! |4 ?
  53.     motor_9.run(0);+ P/ F5 r( }( H" P4 V. u! ~, z
  54.     motor_10.run(0);3 \+ J  T0 Z3 y  D9 u
  55.   }* I! {( C5 g) I% a4 t2 j; ~
  56.   do{}while(millis() - StartTime < interval);1 {+ r3 R# K4 S6 N" t9 r5 {
  57. }
    . o/ A) p2 C3 F0 ^7 d

  58. % g. s: \4 v. E7 F2 d. b0 L
  59. void MotoL(int Power){: K- n6 c- p# F* b1 X+ R+ e3 S
  60.   if (Power > MaxSpeed){
    & L! b2 G( }7 }& u* b
  61.      Power = MaxSpeed;
    ; ?* R& M+ U! d# k! e+ v& @
  62.   }
    - q" E4 s: R+ d8 V, b$ [
  63.   if (Power < -MaxSpeed){
    0 K  j, T( C6 h6 S8 z2 b. O) ^) q
  64.      Power = -MaxSpeed;7 a2 g7 J2 l4 _% i% V
  65.   }
    0 G/ X4 `" u# V
  66.   motor_9.run(Power);
    + [$ x7 f& n7 O6 L. @! M
  67. }  
    9 c0 Q4 a9 y- C; U# D

  68. 8 q% K# z/ o4 ?( E7 u+ w9 r
  69. void MotoR(int Power){& H- j. W4 z8 i4 U# y
  70.   if (Power > MaxSpeed){
    , g4 k, P" R7 f6 F0 H: v0 [
  71.      Power = MaxSpeed;9 t/ z' M4 t% r: E/ R# c
  72.   } : v9 b& T( c. k( s$ c% ], f1 w
  73.   if (Power < -MaxSpeed){" V: F- z2 W0 {6 v
  74.      Power = -MaxSpeed;; N0 [) s. P) w* ?
  75.   }
    : C) n# j) M/ ]5 d' G) Y
  76.   motor_10.run(Power);/ O; r. \7 U1 B  N' U- b: `- ?: S: D
  77. }  
複製代碼

# a3 o  B. \5 q6 O" R0 N8 y) S0 j+ `$ _
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯 $ z' n6 Y/ H! x, d& t) ^2 ?

8 x' y+ p* \" n! h9 R您好,不知是否能向您請教。
. Z. Z- I( N/ x4 [' c4 R, ]目前和宋修賢老師在處理Ardui Car
' t/ P- p1 c$ e; N* K' ^1 P7 g雖然已使用較繁雜的方式處理了跑出黑線外的狀況6 t1 |$ D, k, F* H! n( G! `2 m' P
4 Y) Q4 k0 M0 w/ r% j7 e
但基於想追求更精簡的程式所以還是想請問一下$ D* p  T' Y! j8 R+ T
就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝
( R$ M  H: L" i不知道您是否願意教我可以如何處理7 q' e* [6 s) @! H& l

" p+ c# g' c9 R4 n# S
6 a( A# [* a! Q0 [; r, z- v以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;% y% Q# Q! y1 P
  2. double CNY70Val = 1000;
    ( |2 j/ R$ c& A' ]* L- Q
  3. int interror = 0;
    . u7 J, o2 v& M  }& a9 p+ m
  4. int olderror = 0;) {2 ]/ w' s4 i' F- Z. \
  5. double values;3 l0 e& D- u$ i8 _
  6. : t3 ]  G$ y& p0 {- L2 U

  7. + g2 ]3 m9 A& ?9 R7 L1 j* f
  8. void CNY70()8 G" A4 Z6 o4 j# r
  9. {+ s7 i5 p* o/ ~6 |
  10.   valuesRR = analogRead(RR)* ~0 ^: S/ Z4 z0 w  {' l
  11.   valuesMR = analogRead(MR);
    & d9 q* I2 k' T1 }- @. F  R4 t5 F, N
  12.   valuesMM = analogRead(MM);5 v' H3 s' ~+ H
  13.   valuesML = analogRead(ML);3 D7 @. g" I. T9 f/ ~1 E& r4 g4 t
  14.   valuesLL = analogRead(LL);7 [: q; ~5 X& i, x

  15. 4 t5 ~8 l0 M$ i
  16.   if (valuesRR > CNY70Val)
    9 g: X1 F& h7 E& a$ o8 Q
  17.     valuesRR = CNY70Val;
    % U  T7 W/ O& q" B$ p" B& l6 J
  18.   if (valuesMR > CNY70Val); e- p2 G0 ~' a% Q! P1 U; f
  19.     valuesMR = CNY70Val;
    ! W, f% D4 B) |$ y& d# F/ d7 [
  20.   if (valuesMM > CNY70Val)8 C1 A$ j5 @$ [$ y$ Y/ c
  21.     valuesMM = CNY70Val;
    7 ^- a: {" o6 x7 ^2 a9 R( s
  22.   if (valuesML > CNY70Val): `% F% f7 f! ?
  23.     valuesML = CNY70Val;; r" y  g' o$ w0 M3 \- V: x
  24.   if (valuesLL > CNY70Val)& c  a! m# n: f: ]/ t
  25.     valuesLL = CNY70Val;$ o5 _% C9 B& v& S
  26. ' H" j/ I1 B" ~2 e2 O8 U7 f
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;- A# @7 d( Z! v  v# Q' E
  28. }" g- U5 }# L+ v% x# @$ y5 [$ |$ j
  29. & ]% I  L. ?8 z1 n: h# I3 v( C
  30. void Car()
    . j/ m: Y- n- i5 @0 E  I6 e7 Q
  31. {& S; c8 T9 R- n3 ?% j2 p0 R' P- I
  32.   while (1) {$ V* ^2 s* R* i( d, _: h4 B
  33.     CNY70();
    / U. f+ G+ I3 g3 c( Z
  34. 7 {3 Q, K/ O! q8 k. x" q4 O
  35.     int error = ((int)values);! }: Q1 S' a6 J8 v
  36.     interror += error;
    8 r7 j8 R/ Q8 S9 h# f& h+ }
  37.     int lasterror = error - olderror;
    - ~" z, h4 w0 V: D& N
  38.     olderror = error;7 V  E2 {" Z: S* c
  39.     int power = error / 5 + interror / 10000 + lasterror;
    + f! u2 q1 ]4 u) B) N
  40. & |5 m* Q0 T1 n% E7 |" J. y
  41.     if (power > MotoSpeed)& t8 l. t6 F5 h# m
  42.       power = MotoSpeed;
    " G5 z' ]* q% V
  43.     if (power < -MotoSpeed)/ u! Y( {" `6 W6 h4 Z2 g
  44.       power = -MotoSpeed;( L: |( T1 I8 D2 O9 E
  45. : }8 Z* m$ `4 M- X( c1 s
  46.     if (power > 0)( K- q1 `* i; f( @
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。
    ' ?( U# G- @1 |
  48.     else/ {1 U. d! K& A2 s& ^9 y  \
  49.       Speed(MotoSpeed + power, MotoSpeed);
    / p, c8 l5 m; G' ^2 C  l
  50.   }
    . Y$ X# [+ R3 L% Q4 ~
  51. }
複製代碼
% X, z9 T* n2 g' E, w
" [/ B3 c1 w, ^1 l5 I. f
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-11-28 06:37 , Processed in 0.019250 second(s), 18 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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