圓創力科技.MakeBlock TW

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

掃一掃,訪問微社區

查看: 67|回復: 0

mBot Ranger 高速循線範例_

[複製鏈接]
magiccar 發表於 2018-2-13 01:51 | 顯示全部樓層 |閱讀模式
Demo Video. L- j4 y) @8 s) m
  1. #include <Arduino.h>9 b: a8 _- y, h& {
  2. #include <MeAuriga.h>
    $ c5 E% X* S4 F
  3. #include "MeLineFollowerArray.h"8 X/ t( \" f& T7 f! M

  4. ( d' a! E6 u; c/ s- \  v( M7 c
  5. MeEncoderOnBoard Encoder_1(SLOT1);4 Z& ~" A' I, y9 P3 G
  6. MeEncoderOnBoard Encoder_2(SLOT2);
    1 s8 R7 P4 _+ X5 ^' T5 e
  7. MeLightSensor lightsensor_1(12);
    ) v# k/ {" k0 r9 k1 ]( j  F2 Q  j
  8. MeLightSensor lightsensor_2(11);$ D3 C) Z5 H! S3 V$ F, V, c
  9. MeBuzzer buzzer;/ e7 G) P$ c: b7 n3 R' x
  10. MeLineFollowerArray linefollower(PORT_6);" d! L$ A. m: _1 `( K

  11. 7 q1 t  z* N* h3 E4 t# w
  12. #define Error1 1
    7 B2 t5 I% I( a# `
  13. #define Error2 2
    ; O# D' n9 i. M
  14. #define Error3 3$ j! ~/ ?, N1 P  P
  15. : c5 V2 ^/ k, }9 v% y! f
  16. #define Kp 15
    4 ], ]$ t  G! |6 q
  17. #define Ki 0.15
    9 O2 }3 A' n2 \- D/ l# ^2 s* v
  18. #define Kd 0.03
    9 y/ G+ s) n" G: n
  19. ) ?1 V1 T5 N8 |1 }3 M- A* D/ y5 A
  20. uint8_t sData;
    2 J& _) M( ^! K" Z& q, M- b
  21. uint8_t D1;" ]* L+ f% s  M# m' {" B
  22. uint8_t D2;! n" J: x* T( A* Q& Y/ M& `2 [
  23. uint8_t D3;: J" B3 A3 k9 P7 T
  24. uint8_t D4;* z& M, b6 r, x+ O# j$ e( K
  25. uint8_t D5;
    ( I& G' X* t2 y0 R4 f) D2 }" D
  26. uint8_t D6;  {6 \) _4 C: v/ i. l
  27. # ^) Q- M* r) a8 u; g% D
  28. float previous_error = 0;8 H( ?0 _: u  E( A
  29. float integral = 0;) `+ q( ~: p/ p- ?* r5 |) \6 x' v
  30. float derivative = 0;
    ; J, t0 A* u3 r+ ?' G+ i3 t
  31. int  Speed  = 160;
    6 O% e+ u. R, J6 N7 r1 ?+ |
  32. float output;
    ) e& W* Y9 c& W* Z4 A% t5 n7 R
  33.   G! H" ~1 i- l+ a$ J; X
  34. byte Left;' p/ N1 Q: y2 h/ P
  35. $ S) V+ |1 ]( J6 ^2 Z, D5 ]! }1 \
  36. void setup()
    1 j  s: s& O; }* l8 v8 L
  37. {
    $ ^5 J4 S+ T7 H  b) I' S1 S
  38. //Set PWM 8KHz
    + @2 d* C$ H0 S. L' U
  39.   TCCR1A = _BV(WGM10);3 u: ~; X3 ]0 G; J
  40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    " W1 N4 F3 \, V# l- \
  41.   TCCR2A = _BV(WGM21) | _BV(WGM20);  j$ F7 ?8 p' s) |" [% M1 m$ k
  42.   TCCR2B = _BV(CS21);5 r' e$ P7 v. M, w! j0 m
  43.   Serial.begin(9600);' R9 P6 ~6 O; u* ^" s. H  o$ s
  44.   buzzer.setpin(45);" [* D* D6 W. Y2 m* X6 U. S
  45. }) U( ?+ u  T. t# y. l5 L! _9 z/ O
  46. , s! r/ f; J0 A/ ?, I" b$ h
  47. void loop()+ _" A7 W. H, K6 N$ [0 z
  48. {% F4 l" [" p1 N/ C& T
  49.   while(!((lightsensor_1.read()) < (10)));3 d; m# t# h, {5 F2 N1 m0 s# L
  50.   buzzer.tone(1047, 500);$ |$ g! ?4 x, d5 a
  51.   delay(500);1 J$ v& `0 ~# Q4 b( N( R
  52.   do
    2 |. G9 @% w9 H0 s
  53.   {- n7 W6 ]5 T* |$ v: j5 N
  54.    int Err = getErr();
    ! M" v  O1 v8 D/ z' t: \7 `" ~
  55.    if(D1 == 1)1 w) F0 r( T8 R! g9 U
  56.    {, r- u$ ^" q& i
  57.      Left = 1;) h1 P0 x" P. N
  58.    }5 G- [! V6 m5 d7 q6 F, S
  59.    if(D6 == 1)
    # I$ X: R$ m6 c" x
  60.    {4 I! r& j. z$ G% W, h
  61.      Left = 0;2 g5 {$ E! q. Q1 e' O
  62.    }  Q% ]2 D6 A" t" |! o
  63.    if(Err == 99): v) a2 K+ k* L$ ~! m6 [
  64.    {# ^8 h' S% H/ E' x' r1 U9 r
  65.      if(Left == 1)
    8 u; X; L5 J/ `
  66.      {
    . @( P! \  p0 f( ^
  67.        Speed -= 5;# D  G7 z( M! E- j
  68.        moto(0,Speed);
    ) q% h* X, `1 j# E& a! l2 j2 G
  69.        do! w6 e+ ~2 E" A4 O( r- g, z7 O, W
  70.        {+ K; J' h/ w; `# z1 m) S
  71.          Err = getErr();8 m* I7 a) J7 T, R% c
  72.        }while((D1+D6) == 0);& S) x* {0 R1 w  S  j
  73.      }, [) m  M% \: [5 l
  74.      else
    # a  [6 k' B4 O
  75.      {" [% ?! p9 Z1 F. _, e6 k
  76.        Speed -= 5;
    - p3 J- u: e3 E. @
  77.        moto(Speed,0); , A8 Z! {! m# Y; G
  78.        do3 p# x- g% a1 O" Z" J
  79.        {
    " O( G9 B: y4 q
  80.          Err = getErr();5 o- j% C! g2 Y6 T1 B4 a
  81.        }while((D1+D6) == 0);
    2 V# ^0 M0 ?8 ^
  82.      }
    5 O* q: [2 P0 d+ B* f- u; L
  83.    }) `5 ^" h& {, `4 c6 [' i8 H( K/ Z* V
  84.    else
    * j, V& W' m6 R. [
  85.    {
    ; g0 \" r+ N: c) @3 Y
  86.      if((Speed < 160) & (Err < 2)) Speed+=1; # U8 B9 r2 h* u% |$ Q* F$ E7 F2 ^
  87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    8 t! v, q. ?* @0 J3 \6 _
  88.      integral = integral + Err;( C  p  E1 ?( P4 o
  89.      derivative = Err - previous_error;; Y+ x1 J8 F) q/ l$ H3 k8 S6 |
  90.      output = Kp*Err + Ki*integral + Kd*derivative;
    ) z& \0 X' l5 U
  91.      moto(int(Speed-output),int(Speed+output));4 n# g7 Y& H4 I$ [' B4 a
  92.      previous_error = Err;
    ! S& T0 {4 s: T0 q" I) |8 Q  C
  93.    }) \2 ]* W, v) l! x( \9 e! }
  94.   }while(!((lightsensor_2.read()) < (10)));
    9 l1 {2 n. C7 D; u& M) o) [! x
  95.   moto(0,0);( Y7 s+ U2 }" ~
  96.   delay(500);
    6 d7 R8 C4 F: y* A  B
  97.   buzzer.tone(262, 500);
    / i8 z# ^3 m% M" ~( d. L4 n1 l, U
  98. }
    9 Q8 v+ ~% K, n/ @9 A

  99. ; Z0 K  j0 k" t9 J" J  M4 v
  100. int getErr()% Y+ N' ?: y& B9 {
  101. {  1 k# c. D$ @9 G4 Y8 x
  102.    sData = linefollower.getValue();
    , |+ T/ T$ V' }0 \1 L) k# l
  103.    D1 = ~(sData>>0)&1;
    - x. {6 j) n# L( I  Q! O
  104.    D2 = ~(sData>>1)&1;
    6 T# ]1 X3 \, M7 ~
  105.    D3 = ~(sData>>2)&1;3 d2 K. b+ ~* O3 x4 U
  106.    D4 = ~(sData>>3)&1;$ c7 D' U7 X7 m, M  ^, t
  107.    D5 = ~(sData>>4)&1;3 \. p9 u. G1 a% d
  108.    D6 = ~(sData>>5)&1;
    ( g, ], ?' {1 s/ F4 Z) L
  109.    int downD = D1+D2+D3+D4+D5+D6;6 ^$ X4 o& ]* w( P$ q" M6 V
  110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);% p9 q2 O- J4 w. }4 }4 _& f
  111.    if(downD == 0)$ w) z& M3 a- o( ~5 L# B3 ]
  112.    {
    3 Q, _4 \& V6 y
  113.      return 99;
    - D" H3 u8 V, f5 x8 A
  114.    }9 W( y8 u2 M* d: Q
  115.    else" S) A' c; e  ]' k
  116.    {
    . L' I. }- S$ N: t
  117.     return upD/downD;
    * R) d) B  n3 D1 A
  118.    }
    * q1 x0 g! K. Z& e4 J6 n
  119. }
    / j& D9 @. X  X8 f- }+ G' n
  120. 2 _$ ~& S- b3 r7 d0 ^; N- c. ?
  121. void moto(int Speed_L,int Speed_R)
    4 d1 b3 X) O! H! s
  122. {
    ' f+ }% c9 e" h& H3 d# d4 W
  123.   Encoder_2.setMotorPwm(Speed_L);
    / W3 J- h/ L/ N
  124.   Encoder_1.setMotorPwm(-Speed_R);  }$ F4 W8 {& {* D
  125. }
複製代碼
5 R: [) W& X8 G! K/ H
MeLineFollowerArray.cpp
9 ~, M$ \6 m1 ^+ J4 H, F
  1. #include "MeLineFollowerArray.h"
    9 m: \: [/ O; `9 d+ K5 k7 P- h0 ^
  2. 7 Y3 Y  r0 i* g+ `
  3. #ifdef ME_PORT_DEFINED2 n' e+ I1 B$ Z% R" ]" \$ n
  4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
    $ [1 z$ K- B/ U+ d
  5. {3 o. l5 ^/ A+ z' l$ T. {! p
  6. 2 ?8 t2 F% S" N1 W6 L
  7. }& Y+ Z9 g( h. ~- r
  8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
    4 Y; V6 s# |7 s
  9. {
    & T3 ^2 U3 v" P5 g2 _, H/ }: ~5 i
  10.     _DataPin = mePort[port].s2;; d# p& s+ b5 A: z# {, V. l
  11.     pinMode(_DataPin, OUTPUT);
    ' r6 |# k/ u) C  b0 ~
  12.     digitalWrite(_DataPin, HIGH);
    % ^9 k, H3 g1 T& H& v
  13. }( s, W& i1 e7 u  n) O* ~) t
  14. #else // ME_PORT_DEFINED
    - r6 F7 f% W# A% W) r# k, H3 X% T# G
  15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    # ~( t5 L* e" ]# F9 z2 e
  16. {& v, @8 K4 f, a. A: B3 b: V' a
  17.     _DataPin = pin;
    6 l* \% r  T% h& Z  u- i8 x
  18.     pinMode(_DataPin, OUTPUT);
    5 i0 E' M) R* E9 ^/ l7 K# D
  19.     digitalWrite(_DataPin, HIGH);
    0 E! h: u5 E( V9 w7 @
  20. }' |# w; e0 y$ _6 n
  21. #endif // ME_PORT_DEFINED
    $ q* v' a2 q# P4 i* k5 p

  22. 2 c, ~+ v: N& M, k1 A9 q" @

  23. 7 |( |8 c4 Q: }  l5 i8 f1 c8 ]
  24. void MeLineFollowerArray::setpin(uint8_t pin)  i5 B& h  g6 K0 ?
  25. {
    ( G% @! G" j9 x
  26.     _DataPin = pin;
    1 F: X( Y+ _, V7 N9 i) @4 S& b
  27.     pinMode(_DataPin, OUTPUT);
    " ]. b3 J" p3 |& A
  28.     digitalWrite(_DataPin, HIGH);" f0 r* h/ @6 y# y

  29. 1 [) M: W8 K4 m7 T
  30.     #ifdef ME_PORT_DEFINED
    8 u3 h) Q6 m# t+ T4 w# x: z4 e
  31.     s2 = pin;
    3 _% e8 R7 v9 u
  32.     #endif
    + X# |7 g: S9 `4 ~" A" {
  33. }/ A" a7 `) f9 }
  34. / E4 |3 J1 A" w- ^  [/ P
  35. uint8_t MeLineFollowerArray::getValue()
    1 P% r( L8 ~* \5 e8 g: k6 `# K
  36. {) y3 r" v; U0 @
  37.     uint32_t LOW_level_read_time;
    8 \+ G8 @- w- q' I1 j& |
  38.     uint32_t HIGH_level_read_time;
    2 |* M7 K* I. v% l- d# p
  39.     uint32_t time_out_flag;  N: {, g) h6 m8 z
  40.     uint8_t Sensor_Data[3];
    5 Q# r" w% T- d4 ]
  41.     static uint8_t old_data = 0xff;) Y6 a! b6 j% [$ B

  42. 0 [3 R: D% b' S8 E* b6 z  K# b, i
  43.     pinMode(_DataPin, OUTPUT);
    ( O: K: r6 Q- [5 x8 B! f% M  K
  44.     digitalWrite(_DataPin, LOW);% J) k4 Z7 X+ {; k) Q5 D$ K
  45.     delayMicroseconds(980);0 ]9 z; N9 A. t0 N
  46.     digitalWrite(_DataPin, HIGH);
    8 _9 d  Y  J/ l! A) A8 R9 g
  47. ' S. Q  k( I& T" k& N& X
  48.     pinMode(_DataPin, INPUT_PULLUP);
    % x; J9 o4 s3 `( E, Q3 t- ~
  49.     delayMicroseconds(10);% A7 S2 Z- B  D1 O$ [% D

  50. 8 W) U2 d* A* `3 n7 o$ f
  51.     time_out_flag = millis();
    # x" G# q) ?0 Y! u- F/ \
  52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );. I! G- T; z7 F+ r  e4 G/ @0 l
  53. " d3 ]9 B9 e  w! s4 _/ y/ E2 b- I* ]
  54.     LOW_level_read_time = micros();
    6 E+ s' S1 J2 O* x, N! X: m
  55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out4 G1 r+ t3 @* A* G! e
  56.     {- g% e3 B9 F8 F# s$ S# w- t
  57.         return 0xff;3 x& i2 a; ?$ w# D. D$ c8 Y
  58.     }
    ! ~8 Z/ K9 e$ m- O) L
  59. + v; ~7 W3 G9 P
  60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    6 @: h0 C2 L) u
  61. ' v; _0 ]: D7 ?  ~3 A1 M' m& b' |; A
  62.     HIGH_level_read_time = micros();
      Q9 u* m4 S! u% B( n, j
  63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level/ E& |3 m! G0 k! s, l4 B

  64. 7 @" t# T# J& k
  65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    # P' b# J1 e" [" j4 t' j
  66.     {& o0 u7 N7 P, s  z. ]$ T/ G! I. a/ P
  67.         return 0xff;8 n8 p. B# ^8 ?# ]
  68.     }7 N2 M+ p8 H: i

  69. 0 O  U6 @( K0 }) f* [
  70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))/ a: g/ }+ D1 c6 U6 g
  71.     {! s/ A+ L' V2 K) M( V9 y
  72.         return 0xff;5 M9 H) c. p0 o, ^! r6 u  ?! @
  73.     }3 q7 }. K+ Q4 q" r0 u
  74. 0 _3 h5 Z. ]2 J
  75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );& @. i/ c3 j3 E! |: j! H( @! j- w
  76.     LOW_level_read_time  = micros();
    1 R' y- f+ I6 k+ c+ M
  77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level3 I! g) m' ?/ N# u# c0 y' I- }
  78. 9 H8 F$ P( _5 e: j/ u- b+ }4 x3 X
  79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out4 D" q: s+ I7 V, U, g' F
  80.     {
    8 S) K& y8 p2 t' d5 a+ \
  81.         return 0xff;
    $ G3 A+ ~& A) V8 }
  82.     }
    / K( j4 x7 h* B% m
  83. + u$ s) `; P& C7 Y* d. Y
  84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    9 L. o  l  F1 l* H! }7 V
  85.     {
    3 [# B& W* ~, l- ~" s+ G
  86.         return 0xff;; c7 M/ a. f& A! ~7 y  K. v
  87.     }( [0 o$ L# U  K
  88. % C8 `" ~4 U, O$ _. l0 q) b1 w
  89.     for(uint8_t k=0; k<3; k++)9 G2 G3 d4 }5 X
  90.     {' H7 T4 _+ r; p" q( I
  91.         Sensor_Data[k] = 0x00;
    ) o# W# U9 P- y, U1 B9 U3 ~, O
  92. 8 c& X0 j( U8 B- j3 Y  y
  93.         for(uint8_t i=0;i<8;i++)
    ' \1 O- G" }# Y- @0 V3 H" I
  94.         {5 K  L; ^( \3 Z& \' [
  95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level; f8 g& P5 L" b& e
  96.             HIGH_level_read_time = micros();
    % s1 }3 t$ _6 x* q& V% K
  97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    - M2 V9 T1 f/ G

  98. & y  }, F5 M) {' Z6 }, M, I
  99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )% [: l7 d6 L9 O2 V
  100.             {
    1 k0 s) b8 k6 K
  101.                 return 0xff;
    - l2 m9 ]) _. S2 I
  102.             }3 X3 @* o* g0 M; c. @* g% @
  103. 0 n) Q- H- ?4 I0 @% H
  104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );  _4 O" M- M! O# H' t  z
  105.             LOW_level_read_time  = micros();
    5 h! {" p( R* b
  106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level% g8 h' ^% G6 ^' u
  107. ; l1 X* f8 _$ a' @$ W1 \
  108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 16 P5 C  Y) U8 i) V0 t0 l
  109.             {
    / @! x+ ]& f& `  D
  110.                 Sensor_Data[k] |= (0x80 >> i);4 u& l  W+ z& H; d$ P
  111.             }6 A) ^6 P) h. b. C
  112.             else if(HIGH_level_read_time >= 100)
    ! O8 L4 I) Q2 C3 I4 _
  113.             {
    & A1 I9 }' i) ]: T
  114.                 return 0xff;
    ( t6 @; S* z3 h& F
  115.             }
    * H7 m) j9 o" {+ y
  116. / `; G& B. c! S1 g7 j% N2 K
  117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
    5 b( }% ^- y! r( N. {% f  }
  118.             {4 Z  o& G5 x* n
  119.                 return 0xff;; Y7 p2 U  T! k! }0 c
  120.             }
    6 H) T6 T  d) b3 U( U+ C  n
  121.         }0 {" n4 }) c* E
  122.     }
    2 U( F4 d( C( X0 I/ G1 p1 f; o

  123. 0 N% l, Z2 @( x: q5 y* F& x9 c
  124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    3 I  v" ?8 s, N0 {9 ?  `
  125.     HIGH_level_read_time = micros();
    * o% l. i. W- G  ?
  126.     LOW_level_read_time  = micros() - LOW_level_read_time;
    $ ~+ c1 W" L% P% i# G2 J

  127. 2 F) A9 I, O5 W
  128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )6 G6 {' K0 x, ^: c7 G; B# W
  129.     {
    5 k" _' W' z& o8 H# Y) k
  130.         return 0xff;2 M( f7 {* J/ M' D
  131.     }) c- G, u* X4 [7 j: Q
  132. , ~, Y$ f% ~9 I, R# K/ o! l! i# d
  133.     pinMode(_DataPin, OUTPUT);
    0 M9 j- \3 z/ u, q2 D$ l  D' V
  134.     digitalWrite(_DataPin, HIGH);
    . U! U* h1 i" w

  135. # L4 D' X$ H" [
  136.     if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))3 K2 t4 l+ t2 T% t1 F! p* C
  137.     {, v, J) ?( Z/ n, p6 f1 W; ^" [
  138.         old_data = Sensor_Data[0];
    1 z8 X/ G8 k8 t$ o0 D8 H
  139.         return Sensor_Data[0];7 ^  u4 ^8 I$ [4 k; h4 `
  140.     }: F( g+ [( M( A
  141.     else
    $ G. V+ f9 V2 L9 h
  142.     {
    - J; I& r5 q7 `3 ^
  143.         return old_data;( t+ s$ n. j$ g
  144.     }3 x4 n" Q1 p$ @4 U
  145. }2 Z$ K/ \7 ^2 m! q$ Y/ \4 ?
複製代碼

. J6 m: P* u! @* {$ q, ?. Q  RMeLineFollowerArray.h   MeLineFollowerArray.h (731 Bytes, 下載次數: 0, 售價: 100 個銅錢)
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2018-2-24 02:26 , Processed in 0.057263 second(s), 20 queries .

Powered by Discuz! X3.4

© 2001-2017 Comsenz Inc.

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