圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 21836|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>
    * p+ ?8 [! i  W4 z$ d
  2. #include <Wire.h>: }3 `* X1 O* S5 }. A) m6 O
  3. #include <Servo.h>
    ! \6 z, C2 a) H) J/ x# @

  4. ( C: c$ s& T$ B- h
  5. #include "MePort.h"5 R$ O1 a& j' n: @% ]7 c
  6. #include "MeUltrasonic.h"( ^1 Y1 ?" t1 Y) A( b
  7. #include "MeDCMotor.h"# f; B( E  M( |' i- G9 ?" ~) ?3 d

  8. 4 z. h) E  N8 Q7 S3 [" p
  9. //double Input, Output ;
    0 \9 G0 R1 p2 m2 p. V0 T
  10. float MaxSpeed = 255;6 j- h5 y7 H0 y( X/ N2 P
  11. float MaxPower = 180;4 d0 h) l9 Z4 O* D$ q
  12. float MinPower = 120;# ?. x- ]6 g8 E
  13. float Error,ErrorAcc,ErrorDec;8 m, m1 _. E8 h+ J9 I" G
  14. 7 k5 w5 L# g. f
  15. float Kp=0.14;
    6 S1 ]+ z5 ?. r  g! d
  16. float Kd=0.00020;//23;5 O: q; }; o# Y" {( V& d  g
  17. float Ki=0.000201;4 s+ g# [) \. V# l

  18. : a. H* i) [6 E& N6 m0 ~9 \$ j- ]
  19. float nPower;
    ; h5 @5 |( Z* g. y$ _4 `" n
  20. MePort lightsensor_6(6);
    : z# m' g: \: ~" N' d2 i
  21. MePort lightsensor_8(8);
    ; O3 A1 R, b0 p1 c0 I, o$ p
  22. MeDCMotor motor_9(9);
    . z) R% n" x% w, Y, C* {5 V
  23. MeDCMotor motor_10(10);
    9 w# J: i$ F8 E! Z  k; d5 R% E
  24. unsigned long previousMillis = 0;
    2 A9 v3 m9 t7 g: x$ G: \
  25. const long interval = 1;
    ' v/ `2 }# R  k/ ~5 ?
  26.   G2 L* _" V7 o& _( l3 w
  27. void setup(){8 ]& n8 |7 l% ~+ J
  28.     lightsensor_6.dWrite1(1);4 l8 h" |% n4 ~' z
  29.     nPower = 160;& O' ^  K& a3 i% S4 l
  30.     Error=0;$ z" V! q: a" g* q  q
  31.     ErrorAcc=0;
    # l* u% Y. S" W- ^' w& K
  32. }! P' z/ |! g, r0 W! R

  33. " F; ?! h7 d8 i
  34. void loop(){% U5 E1 j' `! W8 P* }% `
  35.   unsigned long StartTime = millis();
    + B; _% E2 y' E
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){& a$ j0 ]0 M, r5 B
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();' p: v- ^  u) D' Y; j6 l
  38.     ErrorAcc +=  nError*Kd ;: `/ `, k- r3 R/ }2 Y; X: |
  39.     ErrorDec -=  nError*Ki ;, W1 x( E7 R  ?) k8 h# I
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    9 A0 p# z6 Y5 y, H8 J9 e
  41.     if(nError < 80 && nError > -80){
    ; o2 H5 q3 g+ ^* ?! o  Z
  42.       if(nPower < MaxPower){
    1 J# @( p  D- s3 h* Q, p
  43.         nPower += 3;# K5 w8 K( t: M: V$ E% ?0 O* p' O
  44.       }
    : w4 K. k! B& }  p2 @$ g: m
  45.     } else{5 B+ |% Q% l- b. R' q+ k+ ]% K6 b
  46.       if(nPower > MinPower){/ p* T- a4 n5 S5 C* x
  47.         nPower -= 2;$ e- K, }4 A" N$ Y
  48.       }( c- R4 h: X7 X; s: c
  49.     }
    0 M! Z" l3 i! u) V9 P/ v0 H
  50.     MotoL(nPower-Error);
    & e- r( h( m/ O$ t
  51.     MotoR(nPower+Error);   
    1 D6 R. i8 [$ G5 J$ L4 }/ p
  52.   }else{
    ' E$ W% N) f( x0 b5 x5 S
  53.     motor_9.run(0);
    % A. ^5 C/ A( m( C8 G
  54.     motor_10.run(0);4 n7 k9 d* X. x: ?1 x8 r2 t8 O
  55.   }
    - B* [+ J; C7 U2 v
  56.   do{}while(millis() - StartTime < interval);: D! q6 [$ [2 S2 B$ m
  57. }3 q$ W* @+ M& u5 K7 M9 x

  58. 9 }. P; A& R9 Y, M& @
  59. void MotoL(int Power){0 {6 t. j% j& Y* @
  60.   if (Power > MaxSpeed){
    " m+ e, x0 t, H' O% u
  61.      Power = MaxSpeed;' L1 ?( z7 E# l5 m* c
  62.   }
    3 o0 _3 Z. ~8 Y- Q2 w( j+ h
  63.   if (Power < -MaxSpeed){* f- T! L5 Z4 S
  64.      Power = -MaxSpeed;/ N4 W: p/ a7 |3 a$ o( N: I. m, L' s
  65.   }
    7 m0 l2 Z' w$ _8 P* K; U% I; u
  66.   motor_9.run(Power);
    / u. t) T6 T& ]; t0 u, i
  67. }  
    7 Q3 |6 ~: O: ~$ M$ v7 u

  68. % C& L* i" M4 c8 N0 @6 M
  69. void MotoR(int Power){
    0 ~( V$ O; m# Q, H
  70.   if (Power > MaxSpeed){$ H- r) w8 \( I8 x, m* Z# Y) z0 i5 c
  71.      Power = MaxSpeed;
    8 h& m: f+ Y3 b0 L: o. O, p
  72.   } ( m  ]* ~5 K  ~3 }
  73.   if (Power < -MaxSpeed){7 c4 D7 c8 S+ i$ v. E
  74.      Power = -MaxSpeed;. e, ~3 S! I% h8 r; F+ x8 N7 a
  75.   }
    9 s2 m9 N1 j9 j# Q- [  e
  76.   motor_10.run(Power);
    + T& v  ~+ k  o* k1 [
  77. }  
複製代碼

- W3 O/ Y$ @& v7 O
: w& r+ n2 K" L$ b$ D0 A9 r
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯 ! Y9 G- [* ]$ o0 p. ]3 P
$ l# c9 V% G& {2 b3 C
您好,不知是否能向您請教。0 S6 L8 y, Q6 \0 {4 Q
目前和宋修賢老師在處理Ardui Car
) M0 P2 E' X  _# A4 U  k  ^雖然已使用較繁雜的方式處理了跑出黑線外的狀況7 R5 _" V9 ~0 `9 k$ I
4 K' F% _0 {* `! N- {, c7 t
但基於想追求更精簡的程式所以還是想請問一下
  C8 {  i( o% s! C3 Z! ^就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝0 l8 K, b2 y1 D& ^
不知道您是否願意教我可以如何處理
% T# {1 W) j" G* Z7 W' ]/ U- ^& |
' ?8 R0 C& l; U; @* q/ D
  z6 O9 n. T% V. Z8 r2 L; F1 v以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;- g5 N2 `  Y. x* a
  2. double CNY70Val = 1000;7 P& T: j: _8 r/ ~5 w  T
  3. int interror = 0;% N- N" B' B' l4 ~1 o5 u# @
  4. int olderror = 0;
    # a( C9 D$ {( g) S, W+ S9 q( \
  5. double values;' N3 z" ]: z) K; h# {' k
  6. * S& X! n% W/ j1 Z' D% S
  7. " Z! S+ U) a2 S$ l% ~6 x
  8. void CNY70()
    4 }4 u2 M! o  g" [
  9. {4 i( W' B+ {3 {0 C. J, t
  10.   valuesRR = analogRead(RR)! V3 {- L. ^2 _$ _, X
  11.   valuesMR = analogRead(MR);& K8 T+ g  B# b' Y4 l. ^4 t6 ?
  12.   valuesMM = analogRead(MM);
    4 P! y, U$ V' E6 f
  13.   valuesML = analogRead(ML);8 _. W1 s" o1 J4 F7 ?4 c
  14.   valuesLL = analogRead(LL);# P8 f6 {. n/ i) r
  15. : F8 C9 a5 D" U( C" u: h
  16.   if (valuesRR > CNY70Val)
    9 I* r! K  X" a) B/ _8 T/ G
  17.     valuesRR = CNY70Val;2 h  ~6 v( D' m3 h
  18.   if (valuesMR > CNY70Val). A1 _2 i0 ^  k5 W$ ^, E/ f) {. e
  19.     valuesMR = CNY70Val;
    0 U  p/ s8 t! s0 K! S
  20.   if (valuesMM > CNY70Val). Q$ v9 Y8 D/ ]* w* s. i
  21.     valuesMM = CNY70Val;6 o+ n, B: j$ f5 g
  22.   if (valuesML > CNY70Val)
    $ X9 H- m5 k! y1 h) Y
  23.     valuesML = CNY70Val;
    8 U+ k% u4 ]% M1 f4 H4 M: d8 v7 I
  24.   if (valuesLL > CNY70Val)
    6 i8 [# @8 @5 K' W. I* m) F8 s7 i
  25.     valuesLL = CNY70Val;9 l3 g: X$ G8 D
  26. 1 E: r1 K: e. ~9 V' b* e5 y, K
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    " F( Z+ u/ f& Z% G3 L& e4 |
  28. }; _' r% o1 Q7 `- T+ U) |

  29. ( L# o& Y4 o+ _, x, Q- k4 k3 J' n
  30. void Car()  y$ T9 E5 }& g( u( p5 i- C
  31. {
    - I( ^3 S7 q: \+ j2 D) v! g! N
  32.   while (1) {
    ! Z9 W) _8 F  y# i3 u
  33.     CNY70();
    ( n$ F7 ^" H* V( T: }2 T8 O
  34. " ^5 c  ^+ w5 X; P/ z, m% C
  35.     int error = ((int)values);
    . D9 F$ `! ^% n+ Y- ~
  36.     interror += error;- i4 K# r5 p" @7 b" ]& M/ s
  37.     int lasterror = error - olderror;/ U& z6 T  H; ?+ ?
  38.     olderror = error;  ^, K0 W5 R8 o5 B
  39.     int power = error / 5 + interror / 10000 + lasterror;
    $ ?2 i- ]* |! Q9 y

  40. + d7 |, d0 t  H& |# J$ s
  41.     if (power > MotoSpeed)) Y& [  p* A: r0 B3 S+ `5 i) u
  42.       power = MotoSpeed;' Z; v) Y) L" z# T
  43.     if (power < -MotoSpeed)3 r) x9 \% I0 I. t7 o
  44.       power = -MotoSpeed;
    3 u, i  W+ f" s/ `: K' X

  45. # A; h  ^5 ]5 d) _
  46.     if (power > 0)
    2 r' Y# W0 W( U8 n9 i' J( o  m' h+ e
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。# ~* d* t6 ]2 n- o% b+ n
  48.     else# d! ^0 Q4 O0 K+ F! X5 C
  49.       Speed(MotoSpeed + power, MotoSpeed);- R+ ?" o% P4 A% y9 q
  50.   }
    + \' ^* L2 M' T! l3 B/ G% b
  51. }
複製代碼
% T7 ?6 i) b' K0 R  l# V; s  U
. Y% t5 G. s& Y- |& L( z+ n8 g
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2025-12-4 20:10 , Processed in 0.026136 second(s), 17 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

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