圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21805|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>
    . O6 d. z6 o" T! E" a
  2. #include <Wire.h>' F$ t6 d0 P. M' l
  3. #include <Servo.h>& ^& ?$ Q( c2 B

  4. 6 {: @# e* ~) e1 y1 H$ b' O
  5. #include "MePort.h"
    & z3 B! |, ]. V3 r# U' s. _$ F
  6. #include "MeUltrasonic.h"2 ^  {5 R3 k1 e# F7 Z
  7. #include "MeDCMotor.h"& P- L! Z/ ~7 ]5 f

  8. ' C* F9 A  u! p7 X( q
  9. //double Input, Output ;
    / d# Y+ k1 d; a- j
  10. float MaxSpeed = 255;9 y# z9 f9 b/ a) E
  11. float MaxPower = 180;
    , r" [4 m, V/ ]8 ?2 k
  12. float MinPower = 120;
    5 V) x: ~6 U. b7 U
  13. float Error,ErrorAcc,ErrorDec;, P+ I) x' e8 Q0 }- T! s' ]

  14. ; B* M; m, y) y2 V' [- B6 J  C% s
  15. float Kp=0.14;
    ; D5 P$ N, l, A
  16. float Kd=0.00020;//23;: t& u9 C1 q1 M* f' m
  17. float Ki=0.000201;
    8 O" m3 C  }) A! W  l- @
  18. . M: [& W3 A: d6 X% Z! o* u4 ]4 s
  19. float nPower;$ [7 P; e8 C) u9 m' F
  20. MePort lightsensor_6(6);* I5 y3 `7 ]# O) _+ A6 H7 F
  21. MePort lightsensor_8(8);
    9 |2 F6 o0 |) F' L, M( X# C
  22. MeDCMotor motor_9(9);
    5 N2 A+ A3 w4 d' ~! m# c
  23. MeDCMotor motor_10(10);1 b/ b0 |  z$ B
  24. unsigned long previousMillis = 0;
    ! I/ d0 k/ v* _: s  k1 l9 `
  25. const long interval = 1;4 ~% J" U+ c/ ^' c

  26. 2 }6 v( ~/ X, E, Q
  27. void setup(){2 w$ {6 O. t; W+ M3 l5 |
  28.     lightsensor_6.dWrite1(1);
    , b3 P' n  R7 b* o; J9 n, M9 \5 c
  29.     nPower = 160;; s2 f" s- D6 _1 O& R
  30.     Error=0;
    : R) t+ k) ]8 P6 \
  31.     ErrorAcc=0;
    + m) @" Z2 i9 t5 X
  32. }& m  L9 O" F' N: O: t1 b$ p

  33. 4 v0 h  U. l' f. x! u
  34. void loop(){& |, n5 ~- a$ U. w* E
  35.   unsigned long StartTime = millis();  R' l, h8 B1 a5 R
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){8 [; Y( Q( {; B8 S
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();# Q) b( c$ k7 P- S
  38.     ErrorAcc +=  nError*Kd ;* o( Z" t& P& \6 P! D
  39.     ErrorDec -=  nError*Ki ;
    1 I9 a9 x2 O+ R6 Q- r6 {
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    3 u4 `- W4 z7 I, d2 K& d
  41.     if(nError < 80 && nError > -80){
    ! G* U3 ?9 \" o8 X) d
  42.       if(nPower < MaxPower){
    . v+ L: b4 P' M; p* M) \7 V
  43.         nPower += 3;5 b) ]2 a5 d) b# `* k8 M. b" M& a& q
  44.       }" C5 p9 R9 D8 I( Y, j. Q
  45.     } else{
    , J8 b- F& K- Z) a& z3 S
  46.       if(nPower > MinPower){8 w# q+ p/ {) y* u/ c( T
  47.         nPower -= 2;2 R1 T' U4 B) G: I) k- W
  48.       }
    9 p, W* T7 u# a' `1 h/ G0 g
  49.     } * P+ }' Y6 A: V% @& G+ u2 Z+ f
  50.     MotoL(nPower-Error);: d( W& f- Z- {) B
  51.     MotoR(nPower+Error);    6 v0 r( M2 }" q4 f3 X
  52.   }else{) k# L/ `; W  |+ i
  53.     motor_9.run(0);
    9 E2 }1 N7 Q1 u% O
  54.     motor_10.run(0);
    1 b- [! L0 A" H8 q
  55.   }3 B$ a7 U+ g2 h' r/ n* w
  56.   do{}while(millis() - StartTime < interval);# L$ p7 C. N) Y3 U0 I% H& Z
  57. }
    5 ?$ }6 L& ]7 O) H* D

  58. % }3 N: S# n, P, L% X0 R4 ^
  59. void MotoL(int Power){
    8 Z; m, c& e0 D2 a
  60.   if (Power > MaxSpeed){6 g( Y4 u, l. s# b' {/ y% _9 w8 R
  61.      Power = MaxSpeed;
    " I% v! _0 l- g1 @
  62.   }
    , l# ?  {& B5 B$ i7 G4 J# _. P6 `& X
  63.   if (Power < -MaxSpeed){
    , N) k1 G7 q( h1 \: g8 n
  64.      Power = -MaxSpeed;, V: }0 P% g, z( Y+ `
  65.   } . ]) S( L+ c' [, ?& o3 F7 d# _
  66.   motor_9.run(Power);* U5 T7 f3 T# s$ }) v# N
  67. }  7 U! d* {  X0 g2 a+ M
  68. 7 ~" f) a$ |. y, M6 s
  69. void MotoR(int Power){! q6 p+ ~" H1 l; E8 x5 b7 K
  70.   if (Power > MaxSpeed){9 J( f2 U% E: _6 J2 V6 B* [
  71.      Power = MaxSpeed;
    ; e4 }& V+ H! \, k8 Y
  72.   } 1 V; r4 S& i% ?
  73.   if (Power < -MaxSpeed){
    2 x& T: l" ^6 d& K
  74.      Power = -MaxSpeed;" e/ ]: s$ d8 d# g: e
  75.   }
    7 p* g; C( Q6 t. v
  76.   motor_10.run(Power);
    $ A% A7 s" ]4 w6 n) _
  77. }  
複製代碼

3 T2 X+ i3 S2 X5 p
, R% J4 E+ @; k
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯
0 w% i" A# P2 ^% s# B; i. O" g5 j# j5 m5 K  `" @5 K
您好,不知是否能向您請教。+ ~7 N* Z  a6 S7 ]
目前和宋修賢老師在處理Ardui Car! K3 k8 w# {0 }6 ]& P( @
雖然已使用較繁雜的方式處理了跑出黑線外的狀況
1 r$ ^; j0 ~6 V9 \1 Z$ H* s, [7 Z( m/ X* M
但基於想追求更精簡的程式所以還是想請問一下
- ~6 ~, ]( D/ A就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝9 W: E* j: N: F7 t8 T" `5 f' I
不知道您是否願意教我可以如何處理
6 H4 f/ H' L, i! z) v6 F# U7 ]
, X2 l6 y$ ?$ S& }# |
  `" j1 c* R4 C( Y; F  X! y以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;+ P# m8 E4 X' s3 I% V$ J
  2. double CNY70Val = 1000;
    ; j+ S4 Z7 j4 M8 r8 l: C! L/ ^
  3. int interror = 0;+ g4 h( b+ i/ w" ]7 H
  4. int olderror = 0;
    + |9 V+ X. N) o7 H8 S: ?
  5. double values;: T6 p; |5 g& }8 M! I
  6. # o( }% H- ?- G
  7. # G3 [5 @- h* C
  8. void CNY70()9 ]4 R/ A8 H- X
  9. {% K( r/ \' d) ^' T5 A) }, C( B
  10.   valuesRR = analogRead(RR)4 w8 @9 Y/ w8 j( B7 T
  11.   valuesMR = analogRead(MR);
    / K; z4 ]9 V) y
  12.   valuesMM = analogRead(MM);2 S. X* T* ?* D1 d5 g7 ?; A
  13.   valuesML = analogRead(ML);1 [; z/ k9 i% G
  14.   valuesLL = analogRead(LL);5 w, o9 r7 H: n6 M- M& U' ]
  15. % G$ u0 a0 e0 _. {, _
  16.   if (valuesRR > CNY70Val)5 }3 x2 D! `0 C
  17.     valuesRR = CNY70Val;# w& C( i1 p: w
  18.   if (valuesMR > CNY70Val)9 G2 z% \  ]( U$ d/ b$ w& T4 y
  19.     valuesMR = CNY70Val;
    : [0 D1 f  K8 H# \7 J1 H, M
  20.   if (valuesMM > CNY70Val)
    * s3 o2 X6 \3 d  a* Q
  21.     valuesMM = CNY70Val;9 O4 k0 [. O, X4 [% w/ }, y! Z
  22.   if (valuesML > CNY70Val)
    + b. h# h$ d% x, y6 o1 |, v- G; T
  23.     valuesML = CNY70Val;- S6 P) s- h9 s% S% R
  24.   if (valuesLL > CNY70Val)
    ; Q  M- K7 ?6 r! }, x2 A: j: Z
  25.     valuesLL = CNY70Val;% b, F; d) c, q
  26. / \( {) L" F! y8 ^' d
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    7 p* l9 C% G7 T* @6 z; {
  28. }% g1 |( ?3 k( S/ y
  29. 8 ^  ?8 ?- r  J/ J" w% |0 H' n
  30. void Car()# q$ ^- J7 G- P" l9 l- ~/ x
  31. {. z4 N: e7 L, b: }8 i6 [
  32.   while (1) {. l7 @, g' H" ~5 a5 j  a
  33.     CNY70();
    / r+ y$ s! ~9 `* ~2 y
  34. 0 R" t; W: n3 F- l: h# T( Z8 {
  35.     int error = ((int)values);
    ) @4 D# A7 X( ^7 j1 }
  36.     interror += error;* Q* S2 u2 Z; s2 q4 E
  37.     int lasterror = error - olderror;
    - W) I, _6 [3 k5 b; j9 k- q
  38.     olderror = error;6 K  s$ N& C" @% v- w1 @3 r
  39.     int power = error / 5 + interror / 10000 + lasterror;
    4 Z( e* ~& _3 `9 \1 C; u6 ]5 m

  40. 3 \+ `1 S* m6 J: d
  41.     if (power > MotoSpeed)+ O, B7 B; L# [
  42.       power = MotoSpeed;
    2 Q/ k* j& I8 A& l' b* m
  43.     if (power < -MotoSpeed)
    5 |# j7 f, w! e2 c; n
  44.       power = -MotoSpeed;
    : W: [5 E9 d/ \5 h/ s! @6 D) _

  45. : x% B1 b0 t4 G: \( u. J
  46.     if (power > 0)
    : ?. Z. h: D+ P7 Q. N$ R3 G6 d
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。
    ; H1 u0 \: G; {( }/ J; K) N* E
  48.     else
    / E6 K7 g' c5 `$ \! v- f% J4 U
  49.       Speed(MotoSpeed + power, MotoSpeed);
    0 Z( O* F2 z! k# w$ x8 Z
  50.   }' W) D( \* W* z2 k
  51. }
複製代碼

- {, C' a; v8 S! o6 g, m5 F4 d0 y; V) B( @, R. \
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-12-2 06:16 , Processed in 0.025127 second(s), 18 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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