設為首頁收藏本站

圓創力科技.MakeBlock TW

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

掃一掃,訪問微社區

查看: 544|回復: 0

mBot Ranger 高速循線範例_

  [複製鏈接]
magiccar 發表於 2018-2-13 01:51 | 顯示全部樓層 |閱讀模式
8 J- _% A8 h1 U% c$ O+ Z* R
  1. #include <Arduino.h>
    7 x6 w, Q" A& l. X
  2. #include <MeAuriga.h>' [+ o6 n* H. b" |& S
  3. #include "MeLineFollowerArray.h") T  l4 \6 G8 k- J1 S

  4. 9 a3 {6 ^+ [  F2 w7 l7 l( h
  5. MeEncoderOnBoard Encoder_1(SLOT1);
    , k' }' a# }4 [
  6. MeEncoderOnBoard Encoder_2(SLOT2);: J9 U* G, N7 j+ q
  7. MeLightSensor lightsensor_1(12);
    . d- ]) O0 }& O. y1 d# z$ G% M' U
  8. MeLightSensor lightsensor_2(11);+ p) h# j# O: n
  9. MeBuzzer buzzer;2 T% S9 ?+ M4 s
  10. MeLineFollowerArray linefollower(PORT_6);1 l1 S$ O$ `( k$ m! e( }) x# H

  11. 2 a, O" h5 X2 S. }3 t2 |& c
  12. #define Error1 1& z0 v, P0 ^9 t: r4 n; u
  13. #define Error2 2
      E( j- Q6 C) X: }- t0 V; o
  14. #define Error3 3! q3 V% h6 N( ?8 P+ c. y8 Y
  15. 9 p0 {4 @. F" C- u$ L( `
  16. #define Kp 15
    4 u6 B( v/ S$ F' Z
  17. #define Ki 0.15
    - Z! c* }8 `# H6 P1 F& e" s6 T
  18. #define Kd 0.03
    ( _% R4 r" Z! R6 Y2 w

  19. 3 r/ l5 z" B8 |" g
  20. uint8_t sData;7 V. Y. x1 m7 O( r
  21. uint8_t D1;
    + A; ^6 S; O% z+ Q
  22. uint8_t D2;1 f8 O3 J$ v9 I# T$ j4 b/ Z
  23. uint8_t D3;, q- ?9 \& A9 ]1 T, l+ l9 B" t- G
  24. uint8_t D4;5 ]3 _3 T( g4 x$ _9 z& U4 I
  25. uint8_t D5;
    6 t9 h2 f! u; D4 b8 T
  26. uint8_t D6;7 T, K: D0 O9 H" X  g. e

  27. 7 |. G1 z# w; g- y! E* P
  28. float previous_error = 0;- u; y$ E3 b: N) Z$ F
  29. float integral = 0;
    ( v9 J3 O6 s& A
  30. float derivative = 0;/ }, k/ Z& b) W# m
  31. int  Speed  = 160;
      D6 b7 `1 }. q+ R# S
  32. float output;8 c& ~: D( y, }6 p0 F6 F

  33. & `3 L. g1 o5 I4 n5 i$ P
  34. byte Left;
    # [0 \. z" V6 Y

  35. , T9 \. g% i, E
  36. void setup()
    , C  ~5 C6 s& U2 O) q3 K
  37. {
    - N9 j! M# z8 T
  38. //Set PWM 8KHz  ?' p; B4 s' z: u" {2 J2 H0 s
  39.   TCCR1A = _BV(WGM10);
    7 S( r! @; X& H9 V4 c1 ~/ y
  40.   TCCR1B = _BV(CS11) | _BV(WGM12);* |, m  {# D  ~1 ^# A! V
  41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    1 b. e. C+ K  i
  42.   TCCR2B = _BV(CS21);
    ) A* ?# v- G. B$ ^& `2 d- H
  43.   Serial.begin(9600);) O; s8 f$ \4 v# J4 z
  44.   buzzer.setpin(45);
    " g  u+ [7 q( l" Q, I
  45. }* Z! V" }$ {2 ~; W/ l
  46. ) L" |# S+ |3 N' a0 S
  47. void loop()* Z& M( r% H% D# i
  48. {7 a" c7 ^3 G6 T  E6 ~4 L
  49.   while(!((lightsensor_1.read()) < (10)));
    9 _& X0 {( z# Z! v
  50.   buzzer.tone(1047, 500);+ g5 }$ L: E0 s/ T2 ?. ?8 ?) j1 f
  51.   delay(500);
    * T$ k, _3 z' y
  52.   do8 _! V- H; v8 a$ `( I/ z3 u
  53.   {) O/ R) _: u1 P: Y! w$ o& Z5 y
  54.    int Err = getErr();
    . l* x' p* W; K8 O
  55.    if(D1 == 1)% k: h% |1 Q$ ]& w: ?
  56.    {# k. j) u, S. J& f8 y% U( S+ Q
  57.      Left = 1;
    7 _, U8 b5 ~2 f' `7 i
  58.    }1 t7 j  Y6 C3 h
  59.    if(D6 == 1)
    + x" s+ ?6 g" j
  60.    {! Q7 ?5 Y2 n  C+ b( H
  61.      Left = 0;
    ( I  s: h# m0 j) m* E
  62.    }
    ! O4 j1 q& R2 u; |4 H9 L
  63.    if(Err == 99)
    7 W2 X" e7 R9 @4 j' e
  64.    {2 c( V4 a. B$ b* D
  65.      if(Left == 1)
    ; I+ k% y& ~1 D
  66.      {# j9 \( N# X/ F0 B2 }* H
  67.        Speed -= 5;
    ; A% `0 f6 ]$ c  D- |, D6 }
  68.        moto(0,Speed);. G' c6 p3 i! h3 p5 O* T
  69.        do
      ]' {6 X. d# r" {. I
  70.        {. T+ \& l7 E* e: |! b, j/ _( M
  71.          Err = getErr();
    & A5 i6 `& d2 b& f; |& n) B
  72.        }while((D1+D6) == 0);! \: k+ d3 N3 ]0 E6 b8 d
  73.      }
    $ ?+ M+ V6 h8 y2 a, |1 G
  74.      else& V  w; s  u: G# I, S' D0 y  L9 c
  75.      {
    * ]* M7 J) f; c8 x" D% o
  76.        Speed -= 5;' o2 t  J) Z! e
  77.        moto(Speed,0); : }7 }6 X! g2 r0 C6 n
  78.        do! B; l) K! B7 h- x
  79.        {
    4 t. F& I+ D4 N$ ]0 S$ @4 ~4 r# @
  80.          Err = getErr();
    : i( o# y$ T- l
  81.        }while((D1+D6) == 0);5 H0 B3 b' Q/ c0 s5 h2 X8 N
  82.      }
    . s- l2 \' E  k2 i/ Q, J& p5 f
  83.    }& Z+ H' e2 W3 A
  84.    else8 P! s# Q2 b  v. j$ n
  85.    {  A5 H# E- [# d: G( F( z) i
  86.      if((Speed < 160) & (Err < 2)) Speed+=1; * _+ Y" n- W% z# C1 C- @& y
  87.      if((Speed > 100) & (Err > 2)) Speed-=2; 3 h+ ?, j$ Q& k8 G
  88.      integral = integral + Err;! d& @  }- e9 \$ |
  89.      derivative = Err - previous_error;. R5 c. a8 w: R
  90.      output = Kp*Err + Ki*integral + Kd*derivative;* E7 C4 @) B" ]7 l" m( S0 F  n0 Q
  91.      moto(int(Speed-output),int(Speed+output));
    ) h8 G( _. K/ L% q) Z2 p9 d+ \* N+ u$ k
  92.      previous_error = Err;- k+ W/ k5 g5 N7 e, t
  93.    }- _- n% z- c6 n+ `  N
  94.   }while(!((lightsensor_2.read()) < (10)));
    ) H! H1 o3 a. k
  95.   moto(0,0);
    0 L3 r/ o& W; _. S
  96.   delay(500);
    - i& I1 H# q0 G. i
  97.   buzzer.tone(262, 500);
    . N8 R: z$ l  D1 v
  98. }
    $ O) S0 F  h6 [0 _8 w8 E$ I
  99. / ?" x8 \$ t* X7 ^# c
  100. int getErr()- h5 q: ~6 S8 Y
  101. {  6 A! W* H: a, t9 @. @  t# s, h" J
  102.    sData = linefollower.getValue();
    1 ?% K; h9 S! G" c
  103.    D1 = ~(sData>>0)&1;
    7 Y6 V- J$ Y! U
  104.    D2 = ~(sData>>1)&1;- X. O7 t+ R( y* F: [" S. R
  105.    D3 = ~(sData>>2)&1;& n) }9 ^. q! K: v, `' W
  106.    D4 = ~(sData>>3)&1;
    ' t& |0 \3 H3 t  R6 a9 j
  107.    D5 = ~(sData>>4)&1;; e% \( _% L7 o5 D3 ?' I
  108.    D6 = ~(sData>>5)&1;/ i8 g4 u6 \$ w% F. P" A
  109.    int downD = D1+D2+D3+D4+D5+D6;2 t  ~/ H8 ^; ~! d/ ]
  110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
    : U4 r' X  l# p2 [2 k3 w8 |
  111.    if(downD == 0). }: E* G. y& B* m/ V$ M; d: Y
  112.    {) B; S7 ?% {2 Y" D. M; t
  113.      return 99;
    4 d6 t- A; ^. z5 ^) j$ d
  114.    }
    / N' X. o( Q3 P
  115.    else! c! X# `2 d6 N1 F
  116.    {
    * [7 f: d' |9 I2 m" {* G
  117.     return upD/downD;6 F; O  l! W6 w, Y' X
  118.    }
    , {. F0 v7 X- Q: v! u* k! c5 ?
  119. }
      j8 b$ V% d  P9 T
  120. $ U( b+ {* x9 O
  121. void moto(int Speed_L,int Speed_R)
    0 w3 B( _- |2 d" {4 k
  122. {
    3 c: I  V0 ?- K0 ^* Z/ _/ G  E5 Y
  123.   Encoder_2.setMotorPwm(Speed_L);  ]& \- Z9 w  c" H5 e  ^" S9 z
  124.   Encoder_1.setMotorPwm(-Speed_R);
    / p9 ]9 F- ^: p2 q! Q
  125. }
複製代碼

. v4 L! ^4 f* T$ E6 wMeLineFollowerArray.cpp( V8 i$ c4 `2 }8 g5 T) |5 ~; y
  1. #include "MeLineFollowerArray.h"% B- |! k5 n" c# R/ B/ O6 v
  2. # u0 O0 W- u- w
  3. #ifdef ME_PORT_DEFINED- L$ m3 D* ^7 O
  4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
    7 U& F0 d1 K/ r1 S
  5. {/ m: W9 c( c  u/ q- `* N4 ]
  6. 3 V% {% p" ?1 v  ~
  7. }9 o' q9 D4 M! |4 k
  8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)' M6 e6 |+ ^1 b
  9. {: J( g) P/ B& T6 b* ~' L
  10.     _DataPin = mePort[port].s2;* {. E8 ~2 u& R# A8 @) a0 S
  11.     pinMode(_DataPin, OUTPUT);
    ) r# |  T, }9 D5 c. e  \( |
  12.     digitalWrite(_DataPin, HIGH);
    " c% A) s9 s) S2 E4 J
  13. }/ T- t% Y+ l, H+ T2 {
  14. #else // ME_PORT_DEFINED+ L" w, q7 B! O- X3 [
  15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin), t! w; F% D6 `" _3 h
  16. {
    $ b2 m- b1 \9 m) O' F
  17.     _DataPin = pin;2 K; x; w' M! H4 T
  18.     pinMode(_DataPin, OUTPUT);
    8 M. {0 f4 F# t8 }/ P6 k1 f
  19.     digitalWrite(_DataPin, HIGH);
    ; K2 D$ e) Y; U) I, `5 j/ ]
  20. }
    " w! k7 Z6 I- q) q4 |
  21. #endif // ME_PORT_DEFINED
    8 m, \, O9 X; W  \
  22.   r9 Y5 I# }) c) L! k7 Q$ f5 e
  23. 7 \9 p( \' l! O5 b. ?
  24. void MeLineFollowerArray::setpin(uint8_t pin)
    - a% K' ^- j5 Y2 d1 v5 j
  25. {# ]7 N- ]& s  Z0 r: G5 x& p7 J; S
  26.     _DataPin = pin;
    ) a( W3 d$ h; {: E- I  a
  27.     pinMode(_DataPin, OUTPUT); . t" h: q1 C; b9 _6 W
  28.     digitalWrite(_DataPin, HIGH);
    & I  {( d+ I( J1 c% R; p2 q$ ~

  29. 1 `- o: X5 a& x- F% b7 ~9 @
  30.     #ifdef ME_PORT_DEFINED% k- J+ r8 X' K6 A, y
  31.     s2 = pin;
    . q+ Y4 ?. c6 T+ I1 x
  32.     #endif/ J+ h0 ]  H( ^& p' N! X
  33. }
    5 R, c1 q( h; j( B

  34. 3 E, x1 i* n' ~7 @) I* [3 w# T. E6 k
  35. uint8_t MeLineFollowerArray::getValue()- i0 f3 ^+ c- k0 H" Z* p
  36. {
    5 x8 U! y/ A6 @; \8 M3 F( |* x
  37.     uint32_t LOW_level_read_time;* B# l# k/ t0 H/ I, m
  38.     uint32_t HIGH_level_read_time;
    8 ]5 X) u; n( j: q8 j5 H
  39.     uint32_t time_out_flag;
    8 F# F( W. P% ^( v
  40.     uint8_t Sensor_Data[3];( s/ C5 U, _6 t; m4 E& N3 ^% _: J
  41.     static uint8_t old_data = 0xff;. W; h' r; w. y

  42. % A" \7 u+ w, r, i( A% w8 k$ C
  43.     pinMode(_DataPin, OUTPUT);
    ( M* t- O& I" ^, S( y% C3 H
  44.     digitalWrite(_DataPin, LOW);& x  Q0 E3 v' m& w0 Q% G, a- ?+ O4 I
  45.     delayMicroseconds(980);1 J- e7 [& n6 C5 I/ B( F* u
  46.     digitalWrite(_DataPin, HIGH);
    5 K/ }9 k: q( W: I) |! V9 ^4 N
  47. 7 C$ E( \8 Z( U* Q
  48.     pinMode(_DataPin, INPUT_PULLUP);7 P+ `, h. f' ~* S3 ?
  49.     delayMicroseconds(10);$ N7 E! Y7 M* p  m1 v
  50. 6 U8 P% l8 n% ]+ i9 M+ {+ {
  51.     time_out_flag = millis();3 j2 N  p, K: y' `  p
  52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );: ]8 ?  x8 d$ f& `* _! _
  53. 7 n, O- _" S8 s3 _7 C) N
  54.     LOW_level_read_time = micros();
      b! _- d+ r4 R: d
  55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out* E; o. D& W8 v( ^8 q
  56.     {; `! R6 z) ^# H9 r  U! t
  57.         return 0xff;. ~, p4 U! r3 ~- ^9 y8 |
  58.     }
    8 F- f( N' |9 }( p1 H) c$ A) \+ H
  59. 7 y1 E# ?, V! T$ f8 j
  60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    . B; T( _9 Q/ r
  61. 7 l& Y- b* d0 Y1 Z* G, F2 j
  62.     HIGH_level_read_time = micros();, f' Z5 E! [1 A/ C! u* l$ L
  63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    $ X/ n- n/ [% p& p5 B0 [0 W
  64. 4 ~) i( t& x9 F4 q% Y
  65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    . g: n& O0 Q9 J6 N! ~
  66.     {
    , L( s6 L+ t/ L' x- ]1 ^  {
  67.         return 0xff;
    + @* r/ }& ?  p7 p1 T
  68.     }
    9 J) n" m. Q4 G( E5 r7 L

  69. 5 W8 q) X6 s2 `" x
  70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    8 E, P: N) l9 O+ m: K& G' @
  71.     {
      ?( w3 r5 }- Z+ W7 G5 ?
  72.         return 0xff;
    0 l/ l4 i* m8 Y9 {: x/ U0 `
  73.     }
    ) C$ [. r9 z  _2 p: v

  74. " `3 M9 O7 q% e5 O8 t/ X) b* C
  75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );" Z  O. t3 X- {# |2 ?& q
  76.     LOW_level_read_time  = micros();
    % R6 ^8 f( ]2 F# C$ C7 z1 n1 q
  77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level
    , }. k8 c" S( A

  78. 8 G+ V. D! b+ V$ J6 k2 \# }
  79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out: V1 C: f# _+ ]+ B7 m: l+ L: @% J3 i
  80.     {2 ?7 M( z, ?; m' g3 f, m) J$ H
  81.         return 0xff;8 ?7 H1 l4 Q# r
  82.     }6 b  H5 }+ x6 S5 J+ m6 ^" q6 d8 A

  83. . K1 V1 P9 Y* h/ ?7 A% k
  84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    , @: T: F/ E7 c! i5 h$ E* [' U" o
  85.     {. C/ L1 D" A' J6 N+ B2 a
  86.         return 0xff;
    8 K1 j% G7 c6 X0 k8 E+ Y# b
  87.     }& P3 v2 k& `. k' U- K
  88. : k- i; r1 A0 k8 f" O; V, R
  89.     for(uint8_t k=0; k<3; k++)" c! ^$ V% a9 E* i$ |
  90.     {6 C1 ^  l: R1 |/ M; K6 h
  91.         Sensor_Data[k] = 0x00;9 w# Y0 L* S" i* f& z  X6 T
  92. $ }2 S7 B2 j4 f: v% c
  93.         for(uint8_t i=0;i<8;i++)  Y% l7 u% U( Y; K8 A0 H
  94.         {
    % y! C- E, s9 f
  95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    " Z# l) ]* s- f8 j0 [9 Q# l
  96.             HIGH_level_read_time = micros();
    + I1 q& i7 a) U* V, M4 j+ f7 \
  97.             LOW_level_read_time  = micros() - LOW_level_read_time;& P. I$ U, n, i
  98. + X4 f: b2 \2 h6 P& ]
  99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    1 b! Z/ C' ^' X; ]# j
  100.             {4 R; G1 e( ~7 I% B2 X
  101.                 return 0xff;
    3 B2 l7 ^; y$ `- B$ q
  102.             }0 R! Z) z- ?7 ^! \- B5 K# h; z
  103. 1 b, ^6 z* v; P4 p5 b
  104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    " w; |8 C# ~3 O, E' i
  105.             LOW_level_read_time  = micros();
    ) j  c+ @" n# N3 m5 e
  106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
    + a; Y7 g% P& x7 l9 U

  107. 4 {; {/ `& i- ~( B
  108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
    % C/ E# |) R4 H* j9 b5 g
  109.             {0 _& g4 W% B' ]8 s% L% p
  110.                 Sensor_Data[k] |= (0x80 >> i);
    + K8 |7 A! t% o; p9 \) Y
  111.             }' ^3 w7 e8 Y3 e; [/ p
  112.             else if(HIGH_level_read_time >= 100)
    ' |7 L. T- N- r) ]$ M
  113.             {& |- Q: A+ P" I. m, h
  114.                 return 0xff;6 p! F! V0 \  i5 p" V% N
  115.             }5 a" ]- R7 {) ~, N! k2 n0 m2 P

  116. 2 q# H( z# H* J+ `3 r# M
  117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out); j; w$ s- t- c" m" W7 O) }
  118.             {
    ! R: M) D' N* ]' j- |0 y, N8 _1 y
  119.                 return 0xff;9 o0 c* D( L! q: g2 O0 \2 l
  120.             }( L9 q! {3 Z$ H2 K& [" B" O
  121.         }
    + \4 h& d2 V" `$ \' }. b, w
  122.     }/ h( Z7 g/ ?3 {7 ~! J$ _, p; d

  123. ; h+ X6 E4 ~6 D% n  ^" s/ P6 s7 h
  124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    7 L5 D( S( a; ?# ]& X; E! V: z
  125.     HIGH_level_read_time = micros();
    6 q; ^$ {! h5 f7 J7 M
  126.     LOW_level_read_time  = micros() - LOW_level_read_time;7 ^0 [3 L! q* j

  127. . |( [' _* F4 }
  128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    ; X0 k6 M, s9 R+ t
  129.     {+ G5 g0 }: s* S& j  L9 U& X
  130.         return 0xff;3 w# p# [. U! [4 ?$ E1 \3 ^2 s
  131.     }% R" a$ |  ]# N' ~  y
  132. 1 C$ V1 ]. m7 D. x8 w2 l! a1 I
  133.     pinMode(_DataPin, OUTPUT);
    4 _; K' X- z0 B
  134.     digitalWrite(_DataPin, HIGH);2 {, G- {7 C# D" F* o; K% {
  135. 0 V: t6 v! j2 L7 `& D
  136.     if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
    2 r, u' h9 o7 r
  137.     {: e/ [# u/ ^. A' v9 t
  138.         old_data = Sensor_Data[0];7 o$ M3 d6 H9 j& M* e, n. k% v
  139.         return Sensor_Data[0];. m' C$ {* h, m6 |( {
  140.     }- Q/ e) y7 C7 ^7 Q  j0 x2 [$ h: u
  141.     else% A! L) T! F2 n8 C
  142.     {2 v, c7 b4 N% f
  143.         return old_data;$ @  O- n& g( ~  s, \
  144.     }, b: J( j0 i% p( D
  145. }/ q4 \. ?, }4 T0 q, c, E
複製代碼
1 {3 @* w: c( ^4 l
MeLineFollowerArray.h   MeLineFollowerArray.h (731 Bytes, 下載次數: 0, 售價: 100 個銅錢)
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2018-6-23 04:28 , Processed in 0.045181 second(s), 20 queries .

Powered by Discuz! X3.4

© 2001-2017 Comsenz Inc.

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