圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21713|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>
      A0 p7 J) W2 a5 D' ^  [
  2. #include <Wire.h>
    5 m2 G( C4 c: W: y- s
  3. #include <Servo.h>
    . f4 X9 D0 U/ G5 s6 X* ~

  4. " Z* u1 a! D- v
  5. #include "MePort.h"7 L' x  ^+ w6 x0 ]. K; o
  6. #include "MeUltrasonic.h"
    : G( V; I: w) G; O8 }  p5 F& D! J
  7. #include "MeDCMotor.h"
    # w& F5 C( b! v$ R& X

  8. 4 v6 k7 z& ?6 J7 J2 x: {
  9. //double Input, Output ;  @5 q& {5 a) D9 _- F7 L7 O
  10. float MaxSpeed = 255;
    0 v! F; F( g- w4 Z  O- O
  11. float MaxPower = 180;1 i# k* I$ c- h! X& G
  12. float MinPower = 120;& `/ V1 ^# J2 b
  13. float Error,ErrorAcc,ErrorDec;
    8 q! M8 |5 K7 }6 n

  14. ' w) k  X+ H# O# J7 b/ Q: K
  15. float Kp=0.14;
    & A0 p/ T! B- L$ X0 f# Z1 D
  16. float Kd=0.00020;//23;3 V0 t$ ]. }9 ?* l  n
  17. float Ki=0.000201;
    8 ]' P* |2 L5 E& X# q* x7 n5 m$ u
  18. ; X" `% F$ ]7 q. R4 A1 I
  19. float nPower;) D& l$ s& u* M) g1 ^
  20. MePort lightsensor_6(6);4 y+ Q5 x  ^# U4 m+ l
  21. MePort lightsensor_8(8);
    ) B. h7 P2 k- U5 P! p) U. ~
  22. MeDCMotor motor_9(9);
    ( o. b" P( J7 v1 [; k
  23. MeDCMotor motor_10(10);& `( _* A8 J# d: t  o/ \
  24. unsigned long previousMillis = 0;
    7 E4 U9 \( T" V  ?& _
  25. const long interval = 1;
    5 I1 `) G, S% {
  26.   s) [+ M; I: Z3 u( }" g
  27. void setup(){6 L/ D, z: [0 J3 M! X$ n
  28.     lightsensor_6.dWrite1(1);
    . G# t* Q$ @$ g2 |5 w2 O4 ^
  29.     nPower = 160;/ w; Y, o+ o" L* l
  30.     Error=0;
    ; Z) P+ y& l% L1 z1 F* g, S
  31.     ErrorAcc=0;" O$ V8 C1 `6 J' E5 }. U0 ]( v
  32. }. R! ^  k2 n1 K; K, E  D; ?. l8 y

  33. 2 c% D/ F, B9 C' z
  34. void loop(){/ d5 @* A2 x0 O9 A5 Y) |4 C! S
  35.   unsigned long StartTime = millis();5 K8 y4 }/ y* v6 T
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    & |. N0 Y7 J" ?+ B
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();3 A, }0 X% I5 a- Q+ c$ s
  38.     ErrorAcc +=  nError*Kd ;$ K6 K2 ^; E8 K* d% `9 \6 t6 i
  39.     ErrorDec -=  nError*Ki ;
      p' X6 I% }9 g- }" M& f. }' w6 O/ G* o" P
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    0 x. l' z5 @9 J; G$ T6 [& M2 Y
  41.     if(nError < 80 && nError > -80){
    ! b# Q! x$ F6 k1 Z# P
  42.       if(nPower < MaxPower){
    & `0 a9 f7 a, {3 U3 ^" l# t
  43.         nPower += 3;
    ) N9 c* o' S. _7 u! w4 d2 _
  44.       }+ G; M# m; j- f& [( B
  45.     } else{
    , e! N1 U( @; z) h
  46.       if(nPower > MinPower){
    / M7 |/ L& N( b' O
  47.         nPower -= 2;
    # V$ ~* Z1 `( h# P
  48.       }- Z1 s- `: R8 @/ [6 H
  49.     }
    6 o* R8 p8 N) X9 c6 `4 T7 g
  50.     MotoL(nPower-Error);
    , ~3 i" d) R7 L' h( V/ N
  51.     MotoR(nPower+Error);   
    % Q- n3 G4 w, X' ~
  52.   }else{
    + M8 K6 Y: M# u! u+ {
  53.     motor_9.run(0);
    : l2 U' |6 W. h
  54.     motor_10.run(0);- S0 {3 S0 v  r6 f  S
  55.   }
    ( N9 s3 ~. _" {0 N+ D$ M( K
  56.   do{}while(millis() - StartTime < interval);
    % g- u1 Z% K7 \5 R& R9 i( {- b+ d& M; h
  57. }
    1 I; U* v2 S- i" b; I4 i

  58. 3 ?( ^$ V  b, j5 r: N
  59. void MotoL(int Power){
    + n$ s' q, m' n: D$ \2 O+ z
  60.   if (Power > MaxSpeed){8 h* y' e8 Y& n& N1 g
  61.      Power = MaxSpeed;+ h. c6 M/ I+ h+ ?6 N* U
  62.   } - e$ Y/ |% o$ x# U+ }! d! }
  63.   if (Power < -MaxSpeed){( F" |2 M/ V$ v
  64.      Power = -MaxSpeed;; Z& N$ ?$ _& z
  65.   }
    + o+ R  z  d/ T0 v) d8 ?
  66.   motor_9.run(Power);) B& N8 V5 v! t/ ]& g5 ~
  67. }  ' J  |, Y: s2 ^1 k) S9 e0 Y
  68. ( z8 x" W: U: V9 P+ v, N; m
  69. void MotoR(int Power){# }, H/ N5 a% F, M
  70.   if (Power > MaxSpeed){
    1 p/ \- F% o1 T" Z5 `$ u+ }4 p
  71.      Power = MaxSpeed;
    - I: t% i$ z" F& [
  72.   } 4 h9 s& V( e( D) K) {6 M+ i8 _; {
  73.   if (Power < -MaxSpeed){2 d: ?, n& w  C
  74.      Power = -MaxSpeed;
    " T7 j3 ^2 C: K% b
  75.   } . O  ~+ S( `/ J$ G
  76.   motor_10.run(Power);
    9 ?5 Y) U! u+ W; p& U$ u
  77. }  
複製代碼
2 ?+ Z, r1 @1 G6 }! I, H9 `0 C

5 H, f5 G" m; m( B
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯
, y' y  S0 ^, I4 e
3 ?0 D, v" `' X* Z3 P您好,不知是否能向您請教。
8 h( S# H* z% m+ W4 v7 s目前和宋修賢老師在處理Ardui Car1 {5 O' Q; {/ v2 z7 F+ ]
雖然已使用較繁雜的方式處理了跑出黑線外的狀況
4 A) n/ `. U1 T+ U# W$ Z; s4 V+ i( |2 M, F2 t( w5 n6 W
但基於想追求更精簡的程式所以還是想請問一下
8 W4 x! T% }$ [9 t就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝' o' a, o7 A/ |/ m9 b/ j" j9 k
不知道您是否願意教我可以如何處理2 q' h  y, j) c  D5 J/ l

; c" V, z1 L% }2 }2 K3 V8 h6 o2 D  ]% L; i4 S
以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;
    ; o, q2 O% I+ M% v' U7 q% o& l
  2. double CNY70Val = 1000;
    1 k' E, |: D: C' ^: d9 T( @& O! _
  3. int interror = 0;' R* X  h' O6 G# ]( Z1 u
  4. int olderror = 0;
    4 [+ i8 D& {: ]1 |; v
  5. double values;
    7 W, y# J  F, ^! r# p' j! _  M. `

  6. 9 S7 R7 E( z9 l9 B2 ]

  7. $ I6 ]2 W+ O9 k
  8. void CNY70()
      M2 g6 O% g3 p9 ]$ W4 {
  9. {5 y$ u6 K: o+ o* O# `4 w
  10.   valuesRR = analogRead(RR), l3 v  G: \% W8 b% z7 W4 ?- T
  11.   valuesMR = analogRead(MR);
    0 x5 N+ z( W2 e6 f, j( ]6 V
  12.   valuesMM = analogRead(MM);
    9 K$ l$ Y/ S% z& {. T! V
  13.   valuesML = analogRead(ML);
    & N5 V  ?6 r5 L1 q9 q
  14.   valuesLL = analogRead(LL);) i( z8 h* K0 \

  15. - i8 n* A0 O- p6 Y4 W* l- W
  16.   if (valuesRR > CNY70Val)5 P* C; m1 x  z8 n' @
  17.     valuesRR = CNY70Val;
    8 }5 U/ y+ a$ {7 b
  18.   if (valuesMR > CNY70Val)
    0 I# I9 Y" F9 u
  19.     valuesMR = CNY70Val;  c/ ~+ m, b0 v  `
  20.   if (valuesMM > CNY70Val)- N) D1 S1 ^* O
  21.     valuesMM = CNY70Val;
    ' y' h( N/ |, k' h! y6 ?% B" E
  22.   if (valuesML > CNY70Val)+ J; ^- D2 O1 k
  23.     valuesML = CNY70Val;0 `* A, l  i( U( a9 ?
  24.   if (valuesLL > CNY70Val)
    ; {  J2 _  R* n+ W6 _
  25.     valuesLL = CNY70Val;4 M8 r' Q+ D& y

  26. 8 H. i/ a, `0 {1 W, U
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    ; J* Q# w/ s- T2 `! R4 R7 s, d
  28. }2 d4 j7 S1 n) Y
  29. ; q: `$ W5 ?& P9 J' c# r
  30. void Car()
    $ a0 m" \; ]+ z2 t5 _( z9 O2 v8 h
  31. {
    * z! L4 W! a) n. ]
  32.   while (1) {
    6 K. S& [% Y/ L- v# R7 W
  33.     CNY70();2 f/ K4 S2 N' }/ s) m% n& @
  34. 0 m; j2 _8 f( l3 s% e7 i3 |# W
  35.     int error = ((int)values);( f) M; j! M& L, Z" w% O. e8 y
  36.     interror += error;5 \1 u0 z1 \8 F  B1 |
  37.     int lasterror = error - olderror;% X# K+ L* H; M' D1 I" y
  38.     olderror = error;  t& O8 e& V: S$ J3 u+ u3 E
  39.     int power = error / 5 + interror / 10000 + lasterror;! X* o' p1 P5 o) [+ h, W  O9 l

  40. ( \. W5 h8 q; T- j4 C5 @
  41.     if (power > MotoSpeed)/ K1 M/ }9 m/ I9 j# g5 I& f/ A
  42.       power = MotoSpeed;# p5 n3 M( l9 y( t0 U# W8 x6 g
  43.     if (power < -MotoSpeed)
    ( f! s1 n% d2 C2 W- Z  [; z4 n( y* ]
  44.       power = -MotoSpeed;
    2 ?6 r5 `" `* v+ D( x/ Z

  45. " \3 k. x, |: [  _+ [( y
  46.     if (power > 0)* J. F! X: p* V+ y3 Y# K# E9 c. g
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。( M7 H1 f: V- w' Y8 i& F
  48.     else
    - M# R- @0 R4 O  O5 S* t3 ]
  49.       Speed(MotoSpeed + power, MotoSpeed);4 a( t# J+ n; @  k: q& H8 Z  W6 Z
  50.   }
    5 N/ t- y* r+ h. ^" }' H
  51. }
複製代碼

! H7 t7 @7 m3 I8 L) Y
" t! f0 Q- d7 y
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-11-26 05:30 , Processed in 0.020932 second(s), 17 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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