圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21811|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>
    2 |- _8 c7 w% [/ l* q# H
  2. #include <Wire.h>, @8 O, k0 k* s0 l8 O9 y
  3. #include <Servo.h>
    0 a5 ^9 x/ z3 h

  4. 6 h2 a/ S2 `% m) f' k4 Q; N$ z
  5. #include "MePort.h"$ N- V- l! l2 D
  6. #include "MeUltrasonic.h"
    , W+ ?1 y1 w, N  R6 Z
  7. #include "MeDCMotor.h"0 {+ y7 Q: t6 n7 y3 |; x7 ]  r  F

  8. % z3 u8 ~/ c+ [3 d
  9. //double Input, Output ;4 k! F" A: |0 @2 A8 G+ p
  10. float MaxSpeed = 255;
    # @& B: l; B& \6 c' X
  11. float MaxPower = 180;. E$ }7 U4 Y. Y% X. Z& }5 D( E3 O
  12. float MinPower = 120;
    - `5 g1 [, j5 o+ f' \  Q. i; E2 J" A6 r
  13. float Error,ErrorAcc,ErrorDec;6 K9 n/ a5 Q5 E9 K  U8 V
  14. / Y/ q; W9 [3 r8 N( ?0 ?
  15. float Kp=0.14;( w4 h& F9 `$ ?: W0 x: r
  16. float Kd=0.00020;//23;+ ]# ]* c! P9 Q2 A; E6 q
  17. float Ki=0.000201;
    - E8 F' ~, K7 e; b# V% r& U

  18. 8 B6 M3 e/ W5 ?: |3 N# x" C
  19. float nPower;4 s0 c! O* s' @2 [  a- c& u
  20. MePort lightsensor_6(6);
    - A1 A) D+ ]0 u. ~
  21. MePort lightsensor_8(8);
    & @; W' m  U, w, E# t$ m
  22. MeDCMotor motor_9(9);3 j$ [7 Y" W  s* u
  23. MeDCMotor motor_10(10);
    3 J" I& a- j5 K  Q4 |
  24. unsigned long previousMillis = 0;2 ]7 y9 e( t% o2 U3 V; v
  25. const long interval = 1;
    # I$ l0 e4 b; d: `

  26. 7 A, ~. }9 W8 M# U3 [
  27. void setup(){
    - e2 p% l8 `) x; ~
  28.     lightsensor_6.dWrite1(1);
    : T" x7 L: c/ u0 G6 m
  29.     nPower = 160;
    $ Y& {- ]1 D) w; d1 T8 q
  30.     Error=0;
    + m% q0 `" ]- A+ {
  31.     ErrorAcc=0;, M4 V& }2 E4 a1 u- l" V- k
  32. }# d5 e, D# B8 c( ?/ s6 f
  33. 3 ^- R6 h- n3 R- c/ L, X0 U
  34. void loop(){# _$ s" `  v! G* H9 `& V
  35.   unsigned long StartTime = millis();
      J$ f1 a  n! k* o8 n  ?% h
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){6 V% H% b! b; X0 m
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    + h+ j2 d( K5 ~( n& o
  38.     ErrorAcc +=  nError*Kd ;
    / a. Z- Q- B( {$ @+ `8 ?5 x
  39.     ErrorDec -=  nError*Ki ;6 A. f3 E2 r) N
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);* |: |5 {' i; `+ S% g: k
  41.     if(nError < 80 && nError > -80){
    " q* y8 \* C/ r: @+ }
  42.       if(nPower < MaxPower){
    : o) u  H6 y3 u4 |# y5 O
  43.         nPower += 3;
    7 J- ~8 o2 P# x
  44.       }+ Q- y/ J4 |8 G; A$ v+ {
  45.     } else{. w2 w, p. n( _
  46.       if(nPower > MinPower){& _  b9 x8 l* W! O# C* K
  47.         nPower -= 2;
    : E8 v! k! Y4 E2 i9 r" J& r; ]* e
  48.       }* ]4 b2 S% i7 c( j6 R8 S
  49.     } . S* |; D7 N: P' Z
  50.     MotoL(nPower-Error);# x3 a& |3 i7 N( i  R1 d, \
  51.     MotoR(nPower+Error);    4 d3 ~: ]2 N4 i% A! y6 C$ k3 D+ I
  52.   }else{/ ^9 {* o1 c5 g
  53.     motor_9.run(0);" n! y" O9 N( S! n8 T! O% Q* ^
  54.     motor_10.run(0);! K# f0 i" Q( J' ~/ h" T3 [( ^
  55.   }/ \) I- O2 J$ I1 k% p0 o
  56.   do{}while(millis() - StartTime < interval);; {  y, L: M4 u6 o
  57. }
    8 v, k% I; {/ d
  58. % o6 @0 T1 C: a3 ~
  59. void MotoL(int Power){# e. d+ q( t8 [, S0 k& O% c
  60.   if (Power > MaxSpeed){
    5 z/ N  }6 i/ a
  61.      Power = MaxSpeed;4 G5 h  G  `8 Q7 g  f& i8 [
  62.   }
      f, L8 A$ h/ M/ p
  63.   if (Power < -MaxSpeed){
    + w: F: u5 ?* j9 U& }( W& e
  64.      Power = -MaxSpeed;
    9 @$ V6 w* x0 P
  65.   }
    & j: b  Q( u2 r- C# G: A9 N; s
  66.   motor_9.run(Power);
    9 q0 Z- z; [) T9 G% C- G% Q' k* Z
  67. }  
    % ?* g1 M) L9 c$ {' ^( _
  68. 0 f+ y' k* W0 @( q3 \% f
  69. void MotoR(int Power){. ^: t4 @8 a9 d. v: u; y, K
  70.   if (Power > MaxSpeed){2 H; h) X2 Y8 V: w' ~+ G
  71.      Power = MaxSpeed;
      K( q: A% M8 n) x! ^* h, h0 @
  72.   }
    ! B) x5 B( Q8 e( k
  73.   if (Power < -MaxSpeed){
    2 B7 @! x4 O$ W( }+ a2 E( f, J9 T
  74.      Power = -MaxSpeed;
    % ]6 @5 x( c# s8 W. y& i/ l
  75.   }
      L$ E4 R0 M  {7 r! i2 q
  76.   motor_10.run(Power);* ?3 a7 g8 g3 z: v, z/ N& M6 s
  77. }  
複製代碼

4 v) `6 ]2 K4 Q. X9 T: g
6 y" Q$ `$ h  r0 p/ G  C% J) C
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯
- i& ^7 D: d. v( h$ Z. x( L2 e5 U( p- Z/ w# F3 d, t& Q
您好,不知是否能向您請教。( @- X5 n% g* j  v' n3 T; S* Q# K
目前和宋修賢老師在處理Ardui Car
" X. j* S( M: B  e+ [& A雖然已使用較繁雜的方式處理了跑出黑線外的狀況
) P: q/ w) M: |; B$ d- S' e' I3 V9 m" Y, q* p5 ^' _
但基於想追求更精簡的程式所以還是想請問一下
$ k+ o2 Q, a$ U8 C* E* e就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝
" Y( }+ o8 C0 g. Q; |3 i# m不知道您是否願意教我可以如何處理
- o1 s) D3 f0 A) F" G. n0 W# Q* T5 o- v# B/ |% `1 ]' T% [
  U# @& o, ^: `# ~0 E
以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;
    7 S! X5 o3 l" ~( ]8 \  E
  2. double CNY70Val = 1000;$ {7 l' i. T2 A% c, O
  3. int interror = 0;
    6 r# I$ x6 K! V5 `  D; d# _  R0 L
  4. int olderror = 0;. P( Y1 x, P5 z) c3 D! V
  5. double values;
    $ Q" o& P! R- g9 j! s9 \
  6. 5 N1 X' v! G8 F' P
  7. 8 c) J* i8 A1 }/ J
  8. void CNY70()6 H, F& {1 \2 @+ v
  9. {
    # [; \% Y! j( J0 i9 [2 R8 O0 u- m
  10.   valuesRR = analogRead(RR)
    ; A2 I0 \  o7 F  \: f
  11.   valuesMR = analogRead(MR);
    7 m# x2 T& I6 `% h9 E% R/ P
  12.   valuesMM = analogRead(MM);3 e- `6 u  s/ k6 u  K8 L& s
  13.   valuesML = analogRead(ML);; A+ ]. a2 [: R9 j5 l
  14.   valuesLL = analogRead(LL);
    $ P; F1 ]7 o& v& b: L

  15. & o& p! A0 ^' w% E
  16.   if (valuesRR > CNY70Val)
    . ^* B5 V4 X7 g& h, U. K
  17.     valuesRR = CNY70Val;6 ~; L# p% d8 X" K- B& b
  18.   if (valuesMR > CNY70Val)' n* B: [7 Z; X% n
  19.     valuesMR = CNY70Val;3 M/ d% n! A% K) j8 u
  20.   if (valuesMM > CNY70Val)4 }( B, s) i/ G: X: W3 ?
  21.     valuesMM = CNY70Val;( D1 S% v- `" u- I0 b) u- d" N
  22.   if (valuesML > CNY70Val)
    # G4 w! V1 g& u: Z" |/ u' D
  23.     valuesML = CNY70Val;
      K  U% \5 u8 G
  24.   if (valuesLL > CNY70Val)
    6 k0 o! f1 P7 z+ W) B: J
  25.     valuesLL = CNY70Val;3 c3 O, C6 l3 W; Q/ W
  26. 6 K7 B( ?8 N1 P1 T; b
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;$ D0 C: @' R0 F* v9 l; l
  28. }
    $ f0 Q8 \, _" ~6 F) s6 K' r

  29.   V. [' E  U) w" E  w5 s
  30. void Car()
    - i( ?, d5 }/ L) R$ y
  31. {% K* ^# m% d# H) H1 ?
  32.   while (1) {
    0 }$ b; }' V5 @9 I6 K, a
  33.     CNY70();$ Z3 p, t' ~, P& h  {8 b

  34.   U$ k$ @/ l  S: I" O: W
  35.     int error = ((int)values);
    1 i( C: v( [% G8 W# Q
  36.     interror += error;
    ! K% i7 R1 D5 _' N. m- w7 X3 C
  37.     int lasterror = error - olderror;4 r% A1 x7 ^) _& o# p
  38.     olderror = error;" f2 v9 t/ F! s( Q0 u7 G4 j8 ?
  39.     int power = error / 5 + interror / 10000 + lasterror;
    ' l0 d! B  t( @* H* i
  40. , Q' s4 ?  A' m: v8 z% G2 R& g& U+ U
  41.     if (power > MotoSpeed)
    % |4 v; r% X9 c& z% a
  42.       power = MotoSpeed;
    ; d7 J( X+ X0 l! a  x9 W2 d
  43.     if (power < -MotoSpeed)
    % g9 K3 J' u' B* g
  44.       power = -MotoSpeed;
    1 R. L0 m0 o) W5 v2 o% r
  45. 6 Q( P9 C- e: Z1 I. d1 f3 z+ o
  46.     if (power > 0)
    , B+ z3 \% O5 s' n
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。
    0 e4 v9 }3 F8 |
  48.     else9 ]" I4 J& B8 @  C0 I$ d2 M
  49.       Speed(MotoSpeed + power, MotoSpeed);5 I5 X. M! [9 M+ y
  50.   }
    8 X# `" A& |  ]$ B
  51. }
複製代碼
- S+ O+ W8 V0 U7 V
3 Z  c5 T& }' k, v1 R5 s
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-12-2 20:18 , Processed in 0.025905 second(s), 18 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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