圓創力科技

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

查看: 11548|回復: 3

PID 循跡自走車範例

  [複製鏈接]
magiccar 發表於 2015-3-31 12:45 | 顯示全部樓層 |閱讀模式
  1. #include <Arduino.h>
      s8 `& W6 s/ V, B% Q2 y
  2. #include <Wire.h>5 a7 O# U& W; r( F" a2 W
  3. #include <Servo.h>, x* S& |0 q* z; i0 G

  4. ) q* \+ m7 R! ?$ ]
  5. #include "MePort.h"' g3 a& L& c  g4 ~
  6. #include "MeUltrasonic.h"4 X0 _6 A0 M' C; B+ E$ a  O$ |+ P
  7. #include "MeDCMotor.h"9 z7 ~  L1 q) S8 c, _, x
  8. 9 j( b, d( a) {  V; d; K) f
  9. //double Input, Output ;
    5 H2 g, K) j5 w$ x; Q" h
  10. float MaxSpeed = 255;
      G0 t$ y) m5 O8 P
  11. float MaxPower = 180;
    0 e; ^) a. M3 \: X6 Y1 q
  12. float MinPower = 120;/ e/ x# C3 h/ B4 R/ ?( k& r
  13. float Error,ErrorAcc,ErrorDec;8 c% B' R! ]* Y
  14. # e5 w! G% E7 W. e+ V* z
  15. float Kp=0.14;* R: z+ p& P1 @6 q$ y% ?8 M1 L
  16. float Kd=0.00020;//23;
    / ?- U1 h( X( z+ r
  17. float Ki=0.000201;! [, w) `2 i/ O. w0 y2 [

  18. 7 @% ~. }+ h0 J3 r. E
  19. float nPower;/ `: H. K, i: T+ o7 P0 n7 F/ ~( |2 K
  20. MePort lightsensor_6(6);
    5 x' k$ r/ I3 R8 [6 T) [
  21. MePort lightsensor_8(8);
    9 o6 E, D6 g1 G7 x$ l* q
  22. MeDCMotor motor_9(9);# {8 |! [, Y1 Z% ]! |
  23. MeDCMotor motor_10(10);9 f8 |# X* s/ ~5 b$ H8 c# x* U
  24. unsigned long previousMillis = 0;% O0 o+ |; p$ l4 m- N- z% v& g2 c% g
  25. const long interval = 1;
    * z  J) o0 q" I0 X* W7 i

  26. # C1 G% E" a' z3 m. A: V7 P
  27. void setup(){
    6 D: b3 L- e2 u0 }
  28.     lightsensor_6.dWrite1(1);6 r% S+ ^( H& X" |2 }/ L7 |( ]
  29.     nPower = 160;
    $ c( `+ H4 p/ ?9 I
  30.     Error=0;
    - M7 u: B4 U% R- M' ^5 i# L: ~8 Z
  31.     ErrorAcc=0;. W, ]. S5 d  E9 H6 G% [
  32. }7 P- K- @% r% |$ T  J5 _

  33. . S  F8 T. z( |7 [: _3 d7 J
  34. void loop(){
    2 Y0 [* P& m! h; _. g7 ]
  35.   unsigned long StartTime = millis();
    5 B0 S( C% @( D: D6 ?% W# @
  36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){* e+ W1 \! K- Y2 D9 o$ A+ |
  37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    & |, j& D- F7 y5 W# Q3 `+ s; D4 t
  38.     ErrorAcc +=  nError*Kd ;
    % ]: @6 c( ~: v
  39.     ErrorDec -=  nError*Ki ;) Y* ]  G- \; F* c9 w
  40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    " j5 C. ?  o4 i/ _% U
  41.     if(nError < 80 && nError > -80){5 o7 f4 H, t- z' ]
  42.       if(nPower < MaxPower){
    4 X5 Y1 `6 E, i4 a4 T- l( ]
  43.         nPower += 3;
    * x9 \5 G0 A3 |. W' I0 ?( l8 Y
  44.       }
    4 b# a( A; Z+ ~' d+ L
  45.     } else{5 Q2 r3 r+ V! i' b
  46.       if(nPower > MinPower){
    ) u4 B$ _; L$ M" T1 e  @
  47.         nPower -= 2;$ ^  [( F: l  X" H: z/ I: Q" A
  48.       }
    " w7 P7 i6 k# F3 Z
  49.     }
    5 @5 X& |" V  s: ]1 p/ N
  50.     MotoL(nPower-Error);+ }' j- U7 I. t( Q
  51.     MotoR(nPower+Error);   
    7 {* O: C6 f) T3 B
  52.   }else{" q7 K: B0 L9 l9 c0 g& b
  53.     motor_9.run(0);
    , v* n8 U6 A3 o3 L5 ~. @) v# t
  54.     motor_10.run(0);3 y# _' B$ S4 y
  55.   }* K& g4 Y9 ?( b/ @
  56.   do{}while(millis() - StartTime < interval);+ a6 o/ g) n, |" S: {5 K
  57. }/ p- q9 L: s# C" {5 d+ }9 L9 E
  58.   H( b( I  ]2 ?0 S! o4 a3 Z9 v
  59. void MotoL(int Power){
    7 |! Z- k& N3 a" g
  60.   if (Power > MaxSpeed){
    / v  y: F8 F3 g" _: W# m& u
  61.      Power = MaxSpeed;5 F# y7 G+ S' D7 L& b& e; I
  62.   } / e5 F" J  a) U# ?  o. O5 K" Q
  63.   if (Power < -MaxSpeed){+ |* @* ]; d1 E7 d. ?
  64.      Power = -MaxSpeed;3 F+ v% ^  m' s3 g
  65.   } ' A4 c; O3 ?6 c$ b1 w
  66.   motor_9.run(Power);0 Z# E1 v- N- o2 d: K' c
  67. }    ]; E8 H5 }+ D

  68. 9 z1 ^6 Y* o1 i9 t4 T6 {5 x
  69. void MotoR(int Power){1 |& [# M: U8 Q1 M" @+ o
  70.   if (Power > MaxSpeed){3 H% R  G, A; j
  71.      Power = MaxSpeed;; h5 Z6 B5 b) b' _
  72.   }
    " ]) z7 K. d1 _
  73.   if (Power < -MaxSpeed){0 }: h% c" M6 ~1 T' B
  74.      Power = -MaxSpeed;% f& c" a4 N0 T/ `1 {1 a9 U( R
  75.   }
    ( E+ A6 J& f1 ~
  76.   motor_10.run(Power);
    ! u! V+ ^) w8 ~
  77. }  
複製代碼
$ u% z$ @( u; V6 t

% X" [6 |* P1 L/ U4 L$ v
Shiichi 發表於 2016-3-3 13:47 | 顯示全部樓層
本帖最後由 Shiichi 於 2016-3-3 14:02 編輯
2 S$ h( b& \, `5 m- q3 t! ^$ j
" A9 z* G1 \* b  R您好,不知是否能向您請教。5 K, `# \' _7 ~
目前和宋修賢老師在處理Ardui Car/ f1 w+ j1 N- p/ c* V
雖然已使用較繁雜的方式處理了跑出黑線外的狀況
0 E6 U! d% C& I. t, [4 ^0 `  {, |: s+ Y, ]3 w+ w6 n
但基於想追求更精簡的程式所以還是想請問一下" P7 @. h. t( n# w6 [1 P9 \$ p$ P
就是如果我只以單純寫PID控制時,常遇到CNY70跑出黑線外後便往前衝
8 j1 F8 l: P( {" a9 b不知道您是否願意教我可以如何處理) C% b0 ]# I( b: M2 \6 j5 N+ V
8 w" z6 v9 e. s. B0 u/ e, O  }- v
3 r. r( X6 N0 r2 ~' v  @
以下是我挑出我一般寫純PID控制的Code:
  1. int MotoSpeed = 250;
    9 Q5 n: o. u- [, m, d* Q
  2. double CNY70Val = 1000;
    2 J- Z/ @) d; c! A
  3. int interror = 0;! `2 z6 D* j+ G( |! g, l" d0 `
  4. int olderror = 0;
    - S+ f4 R  ?  ]  j3 B. ~
  5. double values;
    0 n, w" L' [2 C. V1 g8 F4 c
  6. 8 p1 ]% r) G) ^, a
  7. ) m& G+ a! z2 Q/ ]0 G' C: A
  8. void CNY70()
    3 E5 L7 s/ t; a* S+ v' [
  9. {
    6 Q" w- \8 i9 a2 b
  10.   valuesRR = analogRead(RR), H7 J( I: B( W7 @. U1 u) D
  11.   valuesMR = analogRead(MR);9 f  D( A* l" T0 B: k( Q/ t- ^
  12.   valuesMM = analogRead(MM);% z2 R. k! Z5 t- U9 D: i
  13.   valuesML = analogRead(ML);0 v  c' k  K2 w% Q
  14.   valuesLL = analogRead(LL);
    * l0 d+ _2 R$ `4 U: N- o5 {
  15. ) w" b0 W0 d/ N2 w& S
  16.   if (valuesRR > CNY70Val)# F; h" P) |2 y7 p1 I0 k, s# Y
  17.     valuesRR = CNY70Val;
    * D- r( Y5 t9 B* x
  18.   if (valuesMR > CNY70Val)
      p, N2 G7 @) l
  19.     valuesMR = CNY70Val;/ Z: y4 H) u, \) T/ E
  20.   if (valuesMM > CNY70Val)3 n- F' u8 G. Z+ ^7 C9 E+ K; z
  21.     valuesMM = CNY70Val;; P2 u! P" T) M9 b4 a& q0 N  v
  22.   if (valuesML > CNY70Val)
    ) l" S/ C4 M1 A* R- ~7 y
  23.     valuesML = CNY70Val;
    1 h5 i; W) v, \5 ^0 s7 p
  24.   if (valuesLL > CNY70Val)
    ( q( R/ }/ V9 h, Z3 Y( n  W
  25.     valuesLL = CNY70Val;
    + p" C, \, e* {3 V7 H, ]7 ~9 ^

  26. 2 v: s" D- ~& c( p/ ~
  27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;% ]* F* S% i) N& c" E
  28. }+ @$ P. }+ }9 M

  29. 9 ~: T) D+ ]4 F# M" d
  30. void Car()8 C1 [, E' G# n  f3 j
  31. {# B8 Z( @( ]* Z. L4 h# ^! f% L
  32.   while (1) {
    0 p: d( {  t7 ^) e8 u) r! V
  33.     CNY70();
    7 r/ n; G/ f3 X9 L' e
  34. - e# ~& [; O. W7 z, \
  35.     int error = ((int)values);4 f: l6 _( z8 E% K8 n/ _7 c7 }
  36.     interror += error;' q# O5 R. _- Z3 P( N( `7 q
  37.     int lasterror = error - olderror;2 p; w$ @1 z, x$ Y4 z+ I. g5 K
  38.     olderror = error;% U3 H/ b: X9 V& G
  39.     int power = error / 5 + interror / 10000 + lasterror;7 A; b3 |5 F( o$ j) [

  40. 4 }/ v2 ?4 n3 X7 l
  41.     if (power > MotoSpeed)
    + B7 e% ?5 a$ l/ K# [6 x! q; i" S
  42.       power = MotoSpeed;1 L6 `8 a% J3 Q
  43.     if (power < -MotoSpeed)3 l/ [" z+ V/ ^7 f9 R+ ~
  44.       power = -MotoSpeed;, H) ^" P7 R! f" B- m
  45. " G6 R+ P& D1 a3 g; K9 U8 L( S
  46.     if (power > 0)' |' H( [. V1 ]; R8 f9 @- p
  47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作,正數正轉 負數反轉。* Q5 o1 {) Z! S  Z( F" f* t* N
  48.     else4 l/ y2 e4 e5 e9 _) d* r9 {
  49.       Speed(MotoSpeed + power, MotoSpeed);
    / m+ }" C9 i0 p4 r: C# r
  50.   }2 A, u5 C2 k2 w5 {9 ~& a' W
  51. }
複製代碼

9 R0 M* _: ]) o* o- m# F6 f
2 [1 \% Q0 G) A* r- [. s2 c
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2020-9-19 14:38 , Processed in 0.028911 second(s), 16 queries .

Powered by Discuz! X3.4

Copyright © 2001-2020, Tencent Cloud.

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