圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21727|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>) `; Z* W4 v% {( L9 D0 W
  2. #include <Wire.h>
    : C# T% u3 Z- P# f
  3. #include <Servo.h>5 u! D: H8 `" V* L8 L: U
  4. - R: {/ V5 m  T9 j" D
  5. #include "MePort.h", y' g2 T) U/ d% I) q
  6. #include "MeUltrasonic.h"
    0 W4 F2 b0 y* S6 z! n8 X+ k7 D
  7. #include "MeDCMotor.h"
    8 e  l: ?2 b& b' |! k4 L, C  q
  8. 2 \! |9 o# ]2 N* P$ Q
  9. //double Input, Output ;
    + w  L0 \5 i1 d% I& ?
  10. float MaxSpeed = 255;
    7 n- ?# R6 H( K% e' q
  11. float MaxPower = 180;
    3 t/ v5 y1 L+ Q  f9 S# m
  12. float MinPower = 120;  J* S9 V' O" p3 U# ?% h% T
  13. float Error,ErrorAcc,ErrorDec;
    , ^" d8 W! i, q( N, Y

  14. & K+ m& n' q1 o* s( Y0 [* f
  15. float Kp=0.14;, c4 y6 a  y( m# R+ P( e' D* C
  16. float Kd=0.00020;//23;" P$ t* _( h$ D! P
  17. float Ki=0.000201;9 }& u% P  u- P  C; i
  18. * y: r' \% t1 U1 ?
  19. float nPower;0 E) u! f# j8 ]2 ?: X+ h
  20. MePort lightsensor_6(6);' O9 R+ Z$ k. w- p" D4 [' C
  21. MePort lightsensor_8(8);# E  y2 X2 U4 G  @' \7 Z
  22. MeDCMotor motor_9(9);# }) [6 g* M0 \( F
  23. MeDCMotor motor_10(10);
    " W- b/ K5 h' }# N
  24. unsigned long previousMillis = 0;8 `1 e3 n- b0 H! J
  25. const long interval = 1;
    8 k1 T: D. p5 Y4 J8 B; U
  26. ' r- l/ g* {4 i
  27. void setup(){8 m: c% {7 s1 m+ U
  28.     lightsensor_6.dWrite1(1);
    ; g: F7 l1 X/ q2 F) R6 N
  29.     nPower = 160;# b% C1 E" @7 D3 x
  30.     Error=0;
    6 N) Y& G; a% V! Z
  31.     ErrorAcc=0;/ ^. K. ~6 Z1 X- G# S# \8 b' W
  32. }2 e8 h; u# ^! i7 @" D
  33. - ?* n2 A% d) `
  34. void loop(){
    - T: G- V4 _) e& R! y7 ?/ Z2 F% ]
  35.   unsigned long StartTime = millis();
    . a6 r9 N3 S4 k, [& j- @
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    . u% c8 ^5 I; W) M# ]
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();, p- W! {5 U' `0 K
  38.     ErrorAcc +=  nError*Kd ;
    $ y0 }- c$ [$ y
  39.     ErrorDec -=  nError*Ki ;' f1 p" t! g4 o) |1 i
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    $ X2 J0 _1 y3 U
  41.     if(nError < 80 && nError > -80){
    . q: ], p8 P. S6 N0 \1 U
  42.       if(nPower < MaxPower){
    ( E6 g# y% n, ]# K+ b
  43.         nPower += 3;
      @; X8 I0 P( R
  44.       }
    ! ^" j0 D" g1 g# C
  45.     } else{3 q+ e( {: l1 Q( B% q, {( A, ~
  46.       if(nPower > MinPower){
    : O3 @+ O- u0 k. @4 V
  47.         nPower -= 2;* y" }8 K+ [# M3 R
  48.       }
    , q6 `1 _8 _& f+ F4 x
  49.     }
    , h# @# c% s$ x2 P2 z; c5 x
  50.     MotoL(nPower-Error);1 H. e3 M" T" A# z. x) Z
  51.     MotoR(nPower+Error);    , ^. u0 T) M3 d+ ?0 h1 ^( S  E
  52.   }else{
    : Z7 A; J" N+ F9 s4 |
  53.     motor_9.run(0);
    6 o: o8 k; y5 s0 B2 K. ^( i1 l) F
  54.     motor_10.run(0);
    ' v) \1 e6 e( x$ a& @; Y! S( u
  55.   }- i! K# f! m, p3 G% v. H
  56.   do{}while(millis() - StartTime < interval);
    ( ~' _# w7 H9 B
  57. }
    5 J) c. W0 [& e

  58. " T' ^. l" R. y; X& l5 [1 t
  59. void MotoL(int Power){
    ! B. ]. `: H7 k+ |
  60.   if (Power > MaxSpeed){
    6 F  k/ N4 f; n8 C0 R7 k! X
  61.      Power = MaxSpeed;) H% f8 M8 ^1 Z) j8 N$ H
  62.   }
    # M9 J0 _8 L2 v5 w9 _
  63.   if (Power < -MaxSpeed){+ m, z0 \8 U( ?
  64.      Power = -MaxSpeed;
    ; T. O. `& ]1 w! ]
  65.   }
    " x$ R% W% q3 Y( E$ p# H9 _
  66.   motor_9.run(Power);
    + s4 k/ J1 R( z
  67. }  
    " _( J- l9 I: _* V* H! P

  68. $ i$ d* c- s" g) ^5 J7 B8 \# H& s
  69. void MotoR(int Power){
    7 a/ W" y$ b! z
  70.   if (Power > MaxSpeed){$ H5 y. z. ]$ H2 b8 c6 _+ J& e
  71.      Power = MaxSpeed;
      t  K# R. ?1 X# c% C" p2 Y
  72.   } , E3 ?7 X' N& h& S
  73.   if (Power < -MaxSpeed){
    8 Q' `: m, G. e& Y9 B* g
  74.      Power = -MaxSpeed;, ]1 t0 K, \1 N" z
  75.   }
    / {. j2 l3 |7 z  r
  76.   motor_10.run(Power);
    6 t  W8 J( [7 K# s% Q' g' g! H
  77. }  
複製代碼
0 j) ^0 {# P0 p( ~' {3 K
' J" a  p- h, x* E
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯
+ ~; ^9 q2 T/ s6 z* e9 o0 ?6 [2 U% f; i
# Y6 f+ r4 e1 m$ P您好,不知是否能向您請教。4 Z* R1 e3 ~2 A+ [1 k
目前和宋修賢老師在處理Ardui Car5 Z6 Y% }; h! L* T2 a" u6 b
雖然已使用較繁雜的方式處理了跑出黑線外的狀況; A- i6 u9 G+ m; q# l2 Q

0 ^2 F2 l- P, \% }但基於想追求更精簡的程式所以還是想請問一下
2 i; c( o6 j2 ]就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝
( O9 b5 a9 D1 @5 P不知道您是否願意教我可以如何處理
: G% `" \. s) |8 H- @8 R: F2 l$ R4 N

* Y6 [- |) a4 |以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;4 Z3 A0 C" |* P+ G/ c& t
  2. double CNY70Val = 1000;
    1 k  N) M6 Q# t- e
  3. int interror = 0;
    9 P9 R, ^% ?: J  R$ J' S
  4. int olderror = 0;
    6 k% v- L6 m; t3 T
  5. double values;# @+ X3 M. u* w- a! V

  6. ) `8 b9 U! M* `0 j  {" }
  7. ) l: P' h1 ]+ |* ~$ G1 _, h
  8. void CNY70()2 f2 N. p- Q4 {. a
  9. {
    . W! {6 f) J( [
  10.   valuesRR = analogRead(RR)
    ! G2 P' b" \2 Z
  11.   valuesMR = analogRead(MR);7 i/ S2 s+ D5 G0 ?! Y
  12.   valuesMM = analogRead(MM);8 E: w! E( ~- L& z: ]
  13.   valuesML = analogRead(ML);
    ' J, B" H, c. `  Q; q- A
  14.   valuesLL = analogRead(LL);: z" S) X  ~3 R0 C) ~- q5 a5 @) o
  15. 4 M* `2 P2 q/ O8 s* C3 J% G) L
  16.   if (valuesRR > CNY70Val)# ?+ x0 O  F2 N3 c: q4 L0 N
  17.     valuesRR = CNY70Val;  \1 |! y5 v' n6 r+ d. G8 \
  18.   if (valuesMR > CNY70Val)# s/ o9 U# A' ^# u2 z4 Z+ s) c
  19.     valuesMR = CNY70Val;
    ; l, S  o7 r) c1 J, L- a
  20.   if (valuesMM > CNY70Val): k/ Z; V; H8 f4 X6 `, _
  21.     valuesMM = CNY70Val;
    , `. B( k: \7 y4 ]2 @, U: D7 U
  22.   if (valuesML > CNY70Val)9 t7 P/ z$ g! W8 X, G5 R9 \5 ^7 ]
  23.     valuesML = CNY70Val;
    + }. M, q; }( @; W5 J8 _/ T! r6 H( n
  24.   if (valuesLL > CNY70Val); y3 z# x8 y" c" {6 a
  25.     valuesLL = CNY70Val;  q8 x2 O; I0 }) }' R
  26. 8 C6 R5 k  t6 o, G/ E  u
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    $ z, [6 }4 G  x
  28. }! m+ @+ E/ y1 _1 x) C, t! W
  29. ( ^9 i5 B  d, n$ h  L( U
  30. void Car()
    4 u( W/ f1 @+ c! `, M8 e
  31. {
    " ~) l2 f/ o. Q9 O
  32.   while (1) {
    4 U" T" \. X. C& i& b- `
  33.     CNY70();4 h8 L3 F2 R% d: U

  34. : f# c, q7 h( h
  35.     int error = ((int)values);
    , `) Q9 N+ e* H* u% i/ @
  36.     interror += error;
    " V4 S3 g8 n) f# t3 D
  37.     int lasterror = error - olderror;6 `1 }& @9 g7 t4 V& y5 y, G- e9 C. x
  38.     olderror = error;- f9 s& s- e- R  {$ Z
  39.     int power = error / 5 + interror / 10000 + lasterror;
    ; r. F: N4 u0 `+ F* W

  40. 9 [: h3 l1 m- G
  41.     if (power > MotoSpeed)7 Q' G; Q- J0 |/ P$ \* _( R7 u" H
  42.       power = MotoSpeed;4 a- P9 D5 V" l5 c! w
  43.     if (power < -MotoSpeed)3 b5 j5 A7 F5 h3 r9 L
  44.       power = -MotoSpeed;" J/ ~4 ~3 D, l* ~) f5 H7 B. |

  45. 2 k; o( x/ C! y5 c5 @5 ^1 W
  46.     if (power > 0)+ Z* {1 e7 j9 W$ P
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。  f* F- S7 f6 m( s9 P
  48.     else
    7 y" g6 i3 k" F) L4 g# _' K- L
  49.       Speed(MotoSpeed + power, MotoSpeed);
    * I( w/ Q' O9 }- ]) @" K3 p
  50.   }
    " o2 O' X0 e$ B4 }  c8 X
  51. }
複製代碼
) k( j2 g# A1 c2 N0 K
, Q" I' P/ g) j9 M2 h; l; n' ?" [
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-11-27 14:05 , Processed in 0.025750 second(s), 17 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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