圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21732|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>  j! |8 ~, s- p6 I* B$ K
  2. #include <Wire.h>: W# G0 ~: T! V/ E
  3. #include <Servo.h>
    # F. g# j7 v8 n$ d, C  d! e; a
  4. + y! L2 s( s; A5 \- T
  5. #include "MePort.h"& \2 v1 S4 F0 V7 p
  6. #include "MeUltrasonic.h"% L. [* r5 T2 J3 `. R
  7. #include "MeDCMotor.h"
    - O4 q$ z) g/ U1 g* j0 G  @

  8. 1 [/ q2 a% b; e
  9. //double Input, Output ;, @2 l) r1 z7 E2 L3 W  x
  10. float MaxSpeed = 255;
    . f) m% ]. L) V: q) x0 W: F1 R6 b
  11. float MaxPower = 180;
    1 h' T( ~( e, r+ B* N# q7 J% H3 _
  12. float MinPower = 120;8 v- g7 G0 D/ F8 F* g* }1 x: \) w
  13. float Error,ErrorAcc,ErrorDec;# ?6 f& F9 R7 G$ r( _& G

  14. 1 U. ~# |2 _+ {. {* u* P9 E1 {
  15. float Kp=0.14;" p1 m) P, M" k0 r1 {; R
  16. float Kd=0.00020;//23;9 l0 d$ `' C6 f% H1 M+ `7 F5 j9 l4 P2 e* _
  17. float Ki=0.000201;
    4 z% W; l) ?) L9 Y- H0 c
  18. , q* `+ w0 W! n& u6 z% K, L
  19. float nPower;4 S0 l( S) U# J# w
  20. MePort lightsensor_6(6);3 V" P6 J& K" y9 W1 h4 K+ v
  21. MePort lightsensor_8(8);
    $ x' W5 k' v# m1 Y9 S0 O
  22. MeDCMotor motor_9(9);( _6 B- `3 z& P- ~! t
  23. MeDCMotor motor_10(10);
    7 a5 U' d& O. N5 i/ f! {
  24. unsigned long previousMillis = 0;) V4 |( c' i; R! |  C
  25. const long interval = 1;
    8 z, U* E7 t5 M4 m
  26. 6 W9 X! ^* \( x( v
  27. void setup(){# O4 o) E; d' H
  28.     lightsensor_6.dWrite1(1);( }! E" y1 R% h# q! I2 E6 X% M; [* \
  29.     nPower = 160;# A1 y* \, p. [7 L0 [1 \. X
  30.     Error=0;. ]. Y' \) E# w% j: A  i
  31.     ErrorAcc=0;# K; p7 b9 f+ I. b' q1 J7 Q
  32. }
    6 w# v4 a0 I  {, `  y

  33. : x' N6 w2 y, {5 S" ~2 e- M! {
  34. void loop(){' O, i  o* i# x: ?8 `
  35.   unsigned long StartTime = millis();' }! [- i( L, ?( a7 K
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    . d6 {2 s* H3 z0 w
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    2 q. ]# W" m1 n+ j) t: }7 e) d, X
  38.     ErrorAcc +=  nError*Kd ;
    8 G1 @9 Y- o8 o: f8 K' M/ Y
  39.     ErrorDec -=  nError*Ki ;
    6 y3 G7 X, A, {* B3 Z  r7 R. e
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);* B7 R7 D; U5 n: o' {. u6 e& T5 w
  41.     if(nError < 80 && nError > -80){/ r2 h; b0 L: l6 o- V
  42.       if(nPower < MaxPower){
    & t' V% Q6 M7 r1 z
  43.         nPower += 3;" {7 |) q5 i  ]% ?. y
  44.       }
    ) T+ b. [9 Q7 u9 o4 t
  45.     } else{
    . _! `! o% K0 a; q. Z& i
  46.       if(nPower > MinPower){* W7 q0 s  Q) W6 X- |, Y) z4 E" e
  47.         nPower -= 2;$ h; ?8 H( |0 @2 z, m3 N' h$ E
  48.       }4 e; K7 G" _6 b0 {5 f
  49.     } + {7 s% J9 `0 v. B2 B  n
  50.     MotoL(nPower-Error);4 G) M9 G1 X: x4 e5 N5 w* d
  51.     MotoR(nPower+Error);    1 {# |, s6 C6 i: A5 v  V4 s
  52.   }else{4 I7 W0 C" x2 N6 `2 {2 r5 @
  53.     motor_9.run(0);
    , m+ d+ A& P+ F. c) @
  54.     motor_10.run(0);
    . p9 `2 T3 ~- H& `+ K7 g- ~
  55.   }. }+ p6 ~, }, ]( J( i( P# y0 H
  56.   do{}while(millis() - StartTime < interval);
    ) ]# {" K- a; c; n# N3 q
  57. }
    ) E0 k" R3 {, v- L( _" l! Z# z# N

  58. 7 {) P) I7 Q3 ~, ]( }* q" x" q
  59. void MotoL(int Power){5 v# I- |9 }* o" \
  60.   if (Power > MaxSpeed){. [+ }- c* O  \* m, b
  61.      Power = MaxSpeed;
    7 R9 g; O5 \, \) j/ e( n/ K( u
  62.   } . P& Q- K8 e$ {4 H" W' ~" o
  63.   if (Power < -MaxSpeed){# z4 x9 B/ e/ M) H+ w7 m
  64.      Power = -MaxSpeed;
    # P7 H7 k+ q) k) u& F/ U- D
  65.   } ' V' n. k0 Y7 M- n
  66.   motor_9.run(Power);2 L; J+ |8 v$ s. ]4 n6 |
  67. }  
    $ P- h7 S) {* Z7 |

  68. 0 Z( z3 P1 u. h
  69. void MotoR(int Power){
    ' w% }) p8 J9 M( h" j- \1 \, d
  70.   if (Power > MaxSpeed){
    + L. c# x& `4 P' T- w0 Q, V+ M2 ?
  71.      Power = MaxSpeed;  `% T% ^* d7 Y  A  m* F# f8 f0 l& z
  72.   } 7 f3 l. O8 }3 A# F0 H) Z$ x: {( B
  73.   if (Power < -MaxSpeed){1 @! O+ G, i# Z. {# c
  74.      Power = -MaxSpeed;
    + {4 G! g# I5 R) }' k
  75.   }
    % \4 I6 N8 l7 {' v2 Y" l0 i% ?2 L
  76.   motor_10.run(Power);
    ; }  m+ f9 g* }; G. E
  77. }  
複製代碼
8 r2 ]2 Y, v' C% }$ l: `# h9 J0 b

6 l8 p8 {8 {) A2 s4 U% f& K6 `) d
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯
  o" p/ s0 k  E
: M8 F# y# o/ b  K您好,不知是否能向您請教。6 H1 k1 O8 i& o  D9 _& A7 R
目前和宋修賢老師在處理Ardui Car0 T3 j8 S) T& p0 o4 f7 g7 b" \
雖然已使用較繁雜的方式處理了跑出黑線外的狀況
0 A3 P+ {( H, Y7 L4 r- z! }9 Y, D, }
但基於想追求更精簡的程式所以還是想請問一下
/ N: Q& d: P" `就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝0 d" ]) U2 S$ s+ T  y' |
不知道您是否願意教我可以如何處理4 I$ G4 q7 I, T9 a# y2 w

1 @5 @$ `( X- [0 T& C9 {
/ [7 f; h# w- I% y7 z2 m7 l- J6 H9 f以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;
    # A! y# ?& \7 x# v- s7 F
  2. double CNY70Val = 1000;6 W4 K% s) K+ d3 t$ o+ W
  3. int interror = 0;3 d2 C7 ]) g2 `+ ^
  4. int olderror = 0;  _% A* J+ F* v: d
  5. double values;* @' T2 @8 l3 z3 c  C
  6. % j, n/ r2 P6 g- m: J

  7. 3 [6 b6 m2 v# W! U
  8. void CNY70()9 e8 N4 G0 M% j9 h# }1 d
  9. {
    ; r  m, ]) A# |$ G- h. @: u; V
  10.   valuesRR = analogRead(RR)
    0 Q) h$ q( j) p" D7 e1 u- H9 \
  11.   valuesMR = analogRead(MR);! b+ X; j2 V/ s5 u" D
  12.   valuesMM = analogRead(MM);
    # t; Z) g% d( @4 G
  13.   valuesML = analogRead(ML);
    5 G( Y' Y, O% ?+ }
  14.   valuesLL = analogRead(LL);
    4 C- I9 B  r. U7 @
  15. $ y* ~7 l5 U9 O1 @* e; F+ C
  16.   if (valuesRR > CNY70Val)2 F- Q; D  l% M
  17.     valuesRR = CNY70Val;% u) j0 u3 k6 W7 w  C! \
  18.   if (valuesMR > CNY70Val)8 g1 A% K4 q5 l: T
  19.     valuesMR = CNY70Val;3 u, f2 \4 d" X4 ]4 f3 c, \( q7 ]" ^
  20.   if (valuesMM > CNY70Val)" s4 M$ ~! q+ H0 m9 h7 d
  21.     valuesMM = CNY70Val;
    $ l, x0 s: x3 n' x6 J8 H
  22.   if (valuesML > CNY70Val)1 I3 E7 |1 G  V) S
  23.     valuesML = CNY70Val;
    6 H$ _; D2 h6 H3 ^
  24.   if (valuesLL > CNY70Val)
    8 C% `3 z$ e. g# q8 B+ V
  25.     valuesLL = CNY70Val;% ^0 c9 L+ H& B8 H, T6 h" D9 f/ r
  26. * E# v7 d: M( L6 X
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;: V3 t5 D$ Y* ?& e. i% Y- f
  28. }( z) _+ I/ \* v! Q) W5 m# W: X& n

  29. % j  h) f2 M. |0 u+ n$ a. D
  30. void Car()
    ' x% r7 [7 p- D" \& a
  31. {! z1 w0 _+ u+ i! e+ e  a
  32.   while (1) {+ y( w: e3 }, F8 ^
  33.     CNY70();
    % x2 H9 k. b! o  g3 y: H7 j  R" @
  34. 6 \& X' h' N3 i$ y5 M- u/ k
  35.     int error = ((int)values);  f* j. z- v0 N
  36.     interror += error;: r6 J) E* c0 H( m% E2 O1 c
  37.     int lasterror = error - olderror;2 {" N2 b  ]7 B& S
  38.     olderror = error;
    . x, O" Y% K7 d
  39.     int power = error / 5 + interror / 10000 + lasterror;
    + ~. Q1 i% w2 @1 w3 @8 S
  40. % |1 A" G& g" M+ H3 R8 `. c
  41.     if (power > MotoSpeed)6 C1 `- @9 y. i8 J0 ^
  42.       power = MotoSpeed;  }( r5 f' \" O& C
  43.     if (power < -MotoSpeed)
    3 }& u4 J) v8 u- G6 j6 \: |( }
  44.       power = -MotoSpeed;/ ^; S4 `6 x2 @& B4 P  K
  45. . e+ V( ~4 N! T3 M- I0 T' y" Y
  46.     if (power > 0)
    4 e" v, r1 v; l
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。( }" y. t1 D# v4 r5 N4 n
  48.     else
    # c. ]# u9 f0 x7 u5 |2 G% P
  49.       Speed(MotoSpeed + power, MotoSpeed);
    ' V# A# H/ C% \* R1 ]
  50.   }; c7 Y# I* g6 A; {
  51. }
複製代碼

3 ~' b) L1 g2 ?9 f- x
$ {- V8 r- c7 [
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-11-28 03:04 , Processed in 0.025555 second(s), 17 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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