設為首頁收藏本站

圓創力科技.MakeBlock TW

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

掃一掃,訪問微社區

查看: 1834|回復: 0

mBot Ranger 高速循線範例_

  [複製鏈接]
magiccar 發表於 2018-2-13 01:51 | 顯示全部樓層 |閱讀模式
: Z9 T; X+ w  Z& x
  1. #include <Arduino.h>2 b* ?# }+ u3 w- N
  2. #include <MeAuriga.h># u2 v5 m, Q8 `0 t! [# E/ u" e
  3. #include "MeLineFollowerArray.h"
    ) E( z% I4 q5 L7 g7 N

  4. . l3 Z  Q3 h- s% F4 B# K
  5. MeEncoderOnBoard Encoder_1(SLOT1);
    / A# G0 O5 {0 w$ e$ }; @
  6. MeEncoderOnBoard Encoder_2(SLOT2);
    : T' S% w: O7 {: J% A6 Z
  7. MeLightSensor lightsensor_1(12);2 Z$ G, p) h! ]$ j
  8. MeLightSensor lightsensor_2(11);' V% c& E# _1 L; }* N
  9. MeBuzzer buzzer;
    6 q7 y# d) C8 l) E/ H- c3 ]0 F
  10. MeLineFollowerArray linefollower(PORT_6);' |, ?! l  ?; N% j& G

  11. " O# x9 R- w0 I9 x5 L
  12. #define Error1 1
    0 ]  _4 e. K. v) \( G
  13. #define Error2 27 m6 T! l8 M. g% x8 y( x
  14. #define Error3 35 ^& x! w; e. x- R) F# L

  15. 1 H! k7 u4 z; a! ^8 s
  16. #define Kp 15
    ) Y2 L- ]% O7 X
  17. #define Ki 0.15
    8 V7 P. m6 e& }5 p6 S1 Z
  18. #define Kd 0.03
    * O; [0 b0 \# A
  19. 4 Z5 J+ d: C% }# Z; Z8 g7 C* Z
  20. uint8_t sData;- Z) \. Q! s* p
  21. uint8_t D1;
    : v& n; k) g9 Z. T5 Q& S8 b+ v+ G. u
  22. uint8_t D2;
    " F5 N% V8 q& h! ?
  23. uint8_t D3;( d  m4 W- B3 E# ?
  24. uint8_t D4;
    / v- P, S" v) @& b
  25. uint8_t D5;
    7 M. }* x+ B& I: ?
  26. uint8_t D6;. }0 B' c' z! p6 m

  27. ' V* A* N& ~+ s$ q! v
  28. float previous_error = 0;1 V; [$ f) E3 B" ^) k! X( |" H" i
  29. float integral = 0;
    ! `& G: C* q7 g6 u) e9 i6 G7 H8 b
  30. float derivative = 0;, A/ [& ]0 U7 L
  31. int  Speed  = 160;
      j# M& e3 ^1 E+ K5 `. {0 u
  32. float output;  }; u8 x  M* L3 ^

  33. ) n$ k5 H( X$ e* ]% Q5 H: U  G
  34. byte Left;. o5 H1 t8 v* k; j! z* S% O
  35. # s" p- P" ~4 e% \9 }
  36. void setup() 0 n* Q" d7 W) J8 G: `
  37. {7 \' ?3 W2 ?3 a3 L. a' ^, S9 {7 P
  38. //Set PWM 8KHz- q/ F, m/ q1 x  S/ [/ v% n' s/ e
  39.   TCCR1A = _BV(WGM10);8 r- H6 V* J1 [% Z& h: s
  40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    , Q2 v* _% v5 t& \  X; ]5 W
  41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    ( m1 n) P( d4 L/ z
  42.   TCCR2B = _BV(CS21);
    4 `1 |! a: e2 B; w$ x
  43.   Serial.begin(9600);
      V3 k! b6 H5 b3 t
  44.   buzzer.setpin(45);
      w; Y$ e' }7 P$ l+ `
  45. }1 N8 b1 R" r$ E3 C, R+ B9 \7 d

  46. % n0 g5 O( m& L" z1 }
  47. void loop()! m: A4 ]! L2 F/ l
  48. {" q  o1 O5 u& U1 @8 l, l# w
  49.   while(!((lightsensor_1.read()) < (10)));
    3 ]% B  a" l& b/ D6 E! x
  50.   buzzer.tone(1047, 500);* U, N. n  E; n* N1 A0 C
  51.   delay(500);; @' g0 E; a0 T3 u- J  a, b
  52.   do6 Z. _' f* n8 u, u& o& s
  53.   {, x& j& F3 t+ v7 n
  54.    int Err = getErr();
    1 f3 d$ e5 E! v1 u
  55.    if(D1 == 1)5 R6 S2 B# W+ B( {; E7 L) h9 ]' `: t
  56.    {6 l6 T1 u4 Y/ t
  57.      Left = 1;$ i$ Z$ B7 R1 I  _3 x  @
  58.    }3 l& O( Q# \8 X# r6 b8 T
  59.    if(D6 == 1)' ~9 |7 m1 E( `0 b+ T" z
  60.    {0 ?4 n7 s& F4 z
  61.      Left = 0;
    0 g9 l4 f  |( r' n4 ^/ d* o
  62.    }
    7 ]' U& d" ~4 o
  63.    if(Err == 99)
    & h% d& \; m% X/ C, V$ E
  64.    {
    7 P; Q; H+ u0 Q% C- ^% V1 f  T
  65.      if(Left == 1): R! J8 _  p* D+ Q
  66.      {3 ^. d4 G& l9 v; ?
  67.        Speed -= 5;
    ! W  c  x: U7 L3 ?# ~2 X6 D
  68.        moto(0,Speed);) m, o5 d, d5 p5 F; R+ ]
  69.        do
    : |% \- u4 S* s% g" p" C0 E
  70.        {8 A; |' c0 }: Q+ [6 q/ m* n6 w
  71.          Err = getErr();
    0 _: Z1 @5 y/ h; _1 M* I- [( f
  72.        }while((D1+D6) == 0);4 g9 l7 J! E4 {' B
  73.      }
    5 k* y% O. P* G4 S( ^  D0 m
  74.      else
    : j& |3 g* k# {. A6 m: H
  75.      {
    6 A- E) Z+ K. b' H' }
  76.        Speed -= 5;7 S" {. _$ i0 h. `
  77.        moto(Speed,0);
    5 s' n- X. ~! }" J5 w2 s5 c
  78.        do% Q+ y2 ?$ Y* x2 ?- ^" C
  79.        {0 B9 N; `; ]8 d4 H
  80.          Err = getErr();6 g2 L. e5 u/ W+ ^
  81.        }while((D1+D6) == 0);
    ; T9 w1 X$ o% [1 R- E1 T
  82.      }
    + V+ z8 V2 q& t! A: c
  83.    }6 _" y) N& K# y) n2 S
  84.    else
    : b5 P8 `- M: f
  85.    {4 v2 l! r! e7 y1 m5 p4 n
  86.      if((Speed < 160) & (Err < 2)) Speed+=1; 8 e% H, I0 |/ I: q# I
  87.      if((Speed > 100) & (Err > 2)) Speed-=2; . |9 {; _' M+ k
  88.      integral = integral + Err;3 q2 b' A/ f& O4 [
  89.      derivative = Err - previous_error;/ O& M9 F) i' Y" c
  90.      output = Kp*Err + Ki*integral + Kd*derivative;
    9 L" J% _7 q- z5 d: ~* Z- \7 f
  91.      moto(int(Speed-output),int(Speed+output));
    5 n+ C7 W) R$ t" I$ ^3 ^8 q
  92.      previous_error = Err;
    * |! C7 A# D6 f6 G) o% Z
  93.    }
    8 }# g- F* s4 A; `: q. z
  94.   }while(!((lightsensor_2.read()) < (10)));
    ) o' K. D7 ^2 Q) X7 E* }
  95.   moto(0,0);
    : F- t% r0 r' E& E
  96.   delay(500);- E/ Q. g* ~" ~7 q. q
  97.   buzzer.tone(262, 500);
    # b$ ^9 D, p! p% U& I
  98. }
    - `5 E% Z% d7 z1 Q* W' Z

  99. ; r& z  P3 z# H
  100. int getErr()# H. O, ~& A2 r5 C
  101. {  
    , B: f& B) u& E4 f- f4 U1 @
  102.    sData = linefollower.getValue();1 P( U: ]( {1 `
  103.    D1 = ~(sData>>0)&1;7 K: ~2 w$ P5 |# W/ X& w, x
  104.    D2 = ~(sData>>1)&1;9 T- x2 n3 M+ c8 Z5 F0 O5 Z
  105.    D3 = ~(sData>>2)&1;
    / P* ~9 h- P$ Y2 a, F
  106.    D4 = ~(sData>>3)&1;2 _7 ^8 h6 t& R) O, x
  107.    D5 = ~(sData>>4)&1;
    / [( F  x" {1 x/ C* b5 K" U
  108.    D6 = ~(sData>>5)&1;& d) Q$ x) g0 S& }" b3 D
  109.    int downD = D1+D2+D3+D4+D5+D6;6 V4 T( V! D* @/ [
  110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);4 u" m8 g7 n: P, e
  111.    if(downD == 0)
    % r/ y6 V* Y, F3 C& Q- _% {
  112.    {' _+ v/ x; A; n( m) v* ~' K) o
  113.      return 99;
    " Z3 `3 h2 b+ \# A6 N: a( Q
  114.    }
    8 {) O: L3 {8 ?; v
  115.    else
    $ N/ x% _( r6 Q
  116.    {$ Y6 I0 \% e3 j& W3 l2 J" [( _
  117.     return upD/downD;3 c2 f7 X5 T8 J+ [  W
  118.    }' `+ r; I7 l  d! ]
  119. }
    ) M) [& T% E$ ^3 H2 u! P4 x
  120. 3 x+ T* }( R) g7 C
  121. void moto(int Speed_L,int Speed_R)
    1 S2 [+ o# ~! T; ?
  122. {
    # ?- d( U$ ?6 c$ _& L! O. s- y* o
  123.   Encoder_2.setMotorPwm(Speed_L);& q6 B6 D% ?; u* a! @
  124.   Encoder_1.setMotorPwm(-Speed_R);
    ' x4 z' a! @+ s# _
  125. }
複製代碼
. [$ }/ [$ {3 A8 R! ~2 _, |
MeLineFollowerArray.cpp
  f8 w4 Z3 e8 l
  1. #include "MeLineFollowerArray.h"
    * B; i. a: i) O

  2. 6 v* h' j" k, l/ f# T# p% N4 O# \
  3. #ifdef ME_PORT_DEFINED- u1 y1 v: I5 o9 Z
  4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)4 V% c2 k% V3 U3 C9 D3 D
  5. {
    / I" B- e8 k+ {) e1 b

  6. 0 t& I; c( `3 Y; e3 V/ w8 z
  7. }
    5 z4 Z! A" m' w3 U& {" i
  8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
    : s. R& H+ ]# e( r9 ?8 ?- L" r
  9. {
      e, K; x' ~4 b; T" o7 X
  10.     _DataPin = mePort[port].s2;
    6 r+ ?" S1 }! Y, r9 L0 H% P
  11.     pinMode(_DataPin, OUTPUT);
    2 R8 H1 k$ c. }& Q& b2 S/ l2 k
  12.     digitalWrite(_DataPin, HIGH);
    # _3 E! M9 }& B5 J7 G! \3 b
  13. }5 P8 s: d( a- k2 l9 K
  14. #else // ME_PORT_DEFINED
    + }- |7 C7 r0 K8 m
  15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)4 {  M9 |, B. B* h; N& |, [; W
  16. {
    : |( w% \& r. z
  17.     _DataPin = pin;- Z' `( I# @$ ~$ o
  18.     pinMode(_DataPin, OUTPUT);
    9 v# d0 x$ a/ _! K7 b7 \' R' H
  19.     digitalWrite(_DataPin, HIGH);2 z' ^2 E  d$ Z# m! ^/ @
  20. }
    : T; |- \+ V; G  P: V* i" ]0 J
  21. #endif // ME_PORT_DEFINED0 k0 A$ q* z' R$ g

  22. 9 Q: t" b; p3 A* Q: V) y
  23. ! X9 d5 ?7 C2 M) c+ T( U
  24. void MeLineFollowerArray::setpin(uint8_t pin)7 i+ r; H5 l9 a9 x8 q
  25. {9 j  @% G' z: s  L7 m+ x
  26.     _DataPin = pin;8 g8 ~$ H6 @% L
  27.     pinMode(_DataPin, OUTPUT); % a' `2 F( s7 V% R% ?7 f
  28.     digitalWrite(_DataPin, HIGH);
    * b: S: b9 U6 m) u( l# i

  29. 9 h' \: ?# l9 I- j( Z
  30.     #ifdef ME_PORT_DEFINED, }6 a" Q/ ]! U. j! m6 e
  31.     s2 = pin;4 l% s% ^  s/ d: }" q- r
  32.     #endif
    % K0 X6 ^& z* ?8 S1 j* p
  33. }
    & }' W3 E5 ?" f

  34. & q/ f, t6 y; ^& K9 t9 _
  35. uint8_t MeLineFollowerArray::getValue()
    * y( a4 U% T6 h
  36. {" u/ }3 d! d5 L2 }$ }
  37.     uint32_t LOW_level_read_time;# E& S- B4 W3 u0 Z3 Z$ C+ Y
  38.     uint32_t HIGH_level_read_time;, F6 a$ G+ Y3 I2 _
  39.     uint32_t time_out_flag;, |3 b7 y- J# A  Y& M; H8 J
  40.     uint8_t Sensor_Data[3];
    $ C. d5 n2 a- U$ d/ F
  41.     static uint8_t old_data = 0xff;
    " {6 {; R- D  F3 \1 m: a* d

  42. 4 M" M( C, ^3 C) c( q, P4 Z
  43.     pinMode(_DataPin, OUTPUT);- ^6 {5 y" I; s- A. Q
  44.     digitalWrite(_DataPin, LOW);
    6 j5 X1 }+ o6 _" Q& u6 C) u
  45.     delayMicroseconds(980);
    2 Y) Q8 n9 }. q0 a/ Q
  46.     digitalWrite(_DataPin, HIGH);% F" F7 E  l0 q5 H# N
  47. ! K/ A6 z0 `+ R! b  m2 A3 ^
  48.     pinMode(_DataPin, INPUT_PULLUP);
    . [: ]* B+ k) H
  49.     delayMicroseconds(10);
    8 W) r3 S- }$ x' q9 v/ Z
  50. + s4 }5 g; [1 k9 N. ]% f
  51.     time_out_flag = millis();
    : a9 n& J6 t9 Z' w, s
  52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    ! H3 d$ E) H$ Q* q3 v1 C

  53. 9 T9 ^, p, J9 _$ z
  54.     LOW_level_read_time = micros();$ u' r2 Q. ~" H) C: B
  55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out9 f+ L% I7 \+ |1 a6 ?  X
  56.     {! T6 [$ l8 T& m/ N7 C& ^! I5 l
  57.         return 0xff;6 v& ]0 C. ^% j$ k" Q( B
  58.     }1 o6 k3 A: p( I. {
  59. 3 q! P( Q. y1 X9 X' f6 g
  60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );0 O1 M( Q5 s! N& Q  E! \( B- D
  61. 2 T: v# O# |2 Z, K4 ]; y+ A
  62.     HIGH_level_read_time = micros();5 }0 W: H& Y2 |: E5 a9 ]
  63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    * X) H0 J' _* @2 `, Q" ~
  64. 1 j5 L, U* X. R& K/ J; b) F
  65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    ; n: p) v* b+ \# B6 j
  66.     {
    : \- @+ R; K% d7 D) n* Q
  67.         return 0xff;' ^- d! k7 O( ~; n& C* y
  68.     }3 c" h$ y$ I6 X1 T" E
  69. $ ?: b1 C" P/ ]6 y; Z" V
  70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    2 |) i3 @; N, c3 a
  71.     {; \9 c- j# i3 t) K
  72.         return 0xff;; W( C$ Y3 V  s# P$ M" y
  73.     }0 R) W' {$ e7 `, ^% n$ L, T

  74. : e! p" S' A3 B8 O# \$ L
  75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );! Q6 l' t2 a+ n' L% \
  76.     LOW_level_read_time  = micros();, h. k' r) g' f+ \: x' T
  77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level
    : E* v: i: e. [2 {! L) I
  78. : p3 {5 K: P5 C$ \
  79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out  x6 l- i; a* f9 @: j1 s
  80.     {
    3 B9 l" {4 h& }- y# p. W2 D( Q% K
  81.         return 0xff;
    + ~$ I; C9 T# x8 [; A
  82.     }
    : J8 C' c( T- S6 Q0 A; Z+ ~# r
  83. ; `: y5 U, m; g# Z
  84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    & y1 G# f* b5 c' U9 z
  85.     {
    , _- o3 c. B0 R( q. Y
  86.         return 0xff;
    ' R+ F2 l+ d7 ~9 c  v2 \/ @
  87.     }
    1 }1 Z: i/ X9 l( p9 x6 R- t
  88. 1 j6 z. t- G0 }" S' S/ v
  89.     for(uint8_t k=0; k<3; k++)2 g4 l. U! o) q( E" ]# c" G
  90.     {
    + o/ E5 k6 Q/ L( M+ T' g- E: k
  91.         Sensor_Data[k] = 0x00;
    ' ^, M2 G, E( Y( @$ l/ z2 K
  92. - n3 J" |- b  B! R: i
  93.         for(uint8_t i=0;i<8;i++)8 f  k; a' @- ]! |( I3 `
  94.         {
    3 d  n8 W. q1 r2 v% a- c" X# n
  95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    1 K. A9 e8 D6 n1 [6 C% J
  96.             HIGH_level_read_time = micros();
    ) F9 w+ ?6 i& _5 c5 d
  97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    ; |6 v) F0 Z. f" N
  98. ( f# Q! j/ K! M/ Y' V! n2 l+ I& D0 H
  99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    * b+ {( G- J$ L$ h$ l
  100.             {
    ! V$ U6 z3 D% S' x4 ]
  101.                 return 0xff;9 Y; u9 u1 A2 l6 o; t! B
  102.             }2 ~" ~! R+ E7 }' i5 ~% J4 z

  103. * I, u' t0 P9 `' _2 p$ q& Y! q
  104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );: I& T) F; e3 u# C) b; W: b
  105.             LOW_level_read_time  = micros();' c' b0 K5 Y+ B2 h/ T: b8 V  r
  106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
      P* x" \8 `4 j" M7 M
  107. 2 s: X# q; q/ n' }# l
  108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
    $ _' G/ S8 d* n) M# c4 i3 t
  109.             {2 f, I% p) H! ]2 y# G* p# |1 ^
  110.                 Sensor_Data[k] |= (0x80 >> i);. C1 Y; @$ e$ }" L6 `, |
  111.             }
    0 e( O- ?& I& ]8 h: S
  112.             else if(HIGH_level_read_time >= 100)
    * S+ F* h0 `  B
  113.             {
    & c+ a' N- M  x. k- z
  114.                 return 0xff;
    ) Y/ }( s4 `5 M- K
  115.             }, z/ I0 U+ B0 m# C6 T; T/ u

  116. 4 U  Y" F$ |* A" n; L; E
  117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
    " b4 W: {% d( Q
  118.             {: c8 x* Q) N* v
  119.                 return 0xff;! B% g; M1 n" `0 Y
  120.             }& l* W1 P0 S$ g1 r! C/ y
  121.         }8 t0 v0 g' ]0 j+ h( y6 K
  122.     }
    $ K) a3 a! E! s  [' n  k( S0 R) u4 e
  123. * ^/ n( y; B5 b/ T8 I7 ~: I
  124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level0 c- z) m9 I+ ?3 o  V) Q
  125.     HIGH_level_read_time = micros();9 e( Y% Z& z. R
  126.     LOW_level_read_time  = micros() - LOW_level_read_time;1 u' ]9 U) }5 W3 p
  127. 3 ]3 C% \. [! H: k
  128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )2 J( t' w" h9 s# X( C* ?
  129.     {( B3 J! q) D2 z9 N5 i6 G
  130.         return 0xff;$ j# w( l, |  p5 ^  [# ?
  131.     }
    3 R: l) _7 v7 G0 \" q$ w, B
  132. + k: E8 T/ T# B$ @0 u
  133.     pinMode(_DataPin, OUTPUT);
    ) G# E. a( @, X! S4 S% T0 r" f
  134.     digitalWrite(_DataPin, HIGH);
    + G" w/ [; x8 L2 J# @$ A, ]/ \0 V0 \

  135. ( S+ J+ c. ~& d" Z. D
  136.     if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))" e; s+ s  q1 b$ s7 h% J1 M
  137.     {; h' c. E. P1 \2 k6 R9 Y  s: }
  138.         old_data = Sensor_Data[0];, y8 s2 k8 |/ n* Z( X  ^
  139.         return Sensor_Data[0];3 C! r8 ~$ Z( V! B8 I* G) u( c
  140.     }) o" h7 @7 [2 G8 c) ]
  141.     else0 `! u9 D9 P9 {6 V& ?
  142.     {# p+ Q& ^$ J/ b# z) A/ g
  143.         return old_data;
    & ^. E4 J, g6 n- `+ n) q+ d6 D* ~4 v2 z+ F/ \
  144.     }
    " X. ]; Q6 W# f7 U
  145. }
    9 `) e4 m0 k- [- J% X
複製代碼
+ W5 y# G* M  g" c! e6 S) ^
MeLineFollowerArray.h   MeLineFollowerArray.h (731 Bytes, 下載次數: 3, 售價: 100 個銅錢)
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2019-2-17 07:19 , Processed in 0.051414 second(s), 20 queries .

Powered by Discuz! X3.4

© 2001-2017 Comsenz Inc.

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