設為首頁收藏本站

圓創力科技.MakeBlock TW

 找回密碼
 立即註冊

QQ登錄

只需一步,快速開始

掃一掃,訪問微社區

查看: 1509|回復: 0

mBot Ranger 高速循線範例_

  [複製鏈接]
magiccar 發表於 2018-2-13 01:51 | 顯示全部樓層 |閱讀模式

4 _0 g6 z' S" K1 u
  1. #include <Arduino.h>9 W8 M+ ^, |. [+ G  ^
  2. #include <MeAuriga.h>
    7 q. C5 V, c5 A) F
  3. #include "MeLineFollowerArray.h"3 _/ z5 V' b3 A
  4. 2 O7 P4 y# I1 n9 B. w
  5. MeEncoderOnBoard Encoder_1(SLOT1);. i9 r6 l- F. j
  6. MeEncoderOnBoard Encoder_2(SLOT2);7 f4 w  t9 i; V9 \
  7. MeLightSensor lightsensor_1(12);; G# O% ?& s5 U( K, @" T
  8. MeLightSensor lightsensor_2(11);4 k0 x- t. N6 P' w
  9. MeBuzzer buzzer;% R% _: l/ y" K3 G. O
  10. MeLineFollowerArray linefollower(PORT_6);' r$ {5 r. K1 @2 |6 k5 Z2 s5 q0 X
  11. 6 H' w( R0 B+ L
  12. #define Error1 1
    # {' P0 b/ P* D3 |6 w4 F
  13. #define Error2 2/ c. b# S; E! }$ s
  14. #define Error3 30 \! A5 Z8 Q9 G6 q
  15. ( h: w1 T3 e5 S/ M- P
  16. #define Kp 15
    ! r3 G' h( p( p- h) D4 Z* C
  17. #define Ki 0.15
    ) h# {! |/ k& j8 @
  18. #define Kd 0.03
    ' {0 C$ _3 I; a

  19. % K, U1 e1 A( ?
  20. uint8_t sData;
    , @% {5 m9 e, O7 B, U- \
  21. uint8_t D1;
    4 `0 y+ K1 L2 N
  22. uint8_t D2;, u  k& S3 g- J/ G6 |. u
  23. uint8_t D3;0 P* J1 F( d) i
  24. uint8_t D4;8 O. Y3 i: b$ s3 {
  25. uint8_t D5;
    $ D* v: _/ V& N3 ]
  26. uint8_t D6;
    6 i+ J" e. i( n9 C, G1 j2 w! P

  27. % a$ V# f5 x2 N5 k8 y7 e/ y
  28. float previous_error = 0;
    8 \" u  ?' ]- ]7 Y
  29. float integral = 0;
    0 f2 L8 n9 ^7 }
  30. float derivative = 0;
    ; V. \1 `* o8 m; P! A
  31. int  Speed  = 160;
      o' }5 _" d. l6 @' h& O
  32. float output;
    % G7 w3 Y# q  V" h- l/ X/ j) _

  33. ' ^) q1 P" `9 |
  34. byte Left;
    , }$ B# |( v/ J9 S

  35. 3 O, d, a! J6 P) N7 b1 E
  36. void setup()
    ( ^8 Q0 `! U& B7 w
  37. {
    " j& `; \3 V; M& l& i
  38. //Set PWM 8KHz
    # o1 a/ S0 z( [# V: A4 w
  39.   TCCR1A = _BV(WGM10);2 ~/ \; W1 h1 ]2 a+ D9 g) m
  40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    1 J* O4 Z, c+ y: B/ F  [% N
  41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    : m) L. Z% x' ]! [+ i4 {4 Q0 P
  42.   TCCR2B = _BV(CS21);
    . P; o  W: t' z7 H5 k& w
  43.   Serial.begin(9600);
    8 u) @4 \) u, w( m: o' K5 C, ~5 r/ _
  44.   buzzer.setpin(45);2 z1 H! Q% j" w2 }: _: @
  45. }
    ' y& w& N, k+ f$ \' _: k
  46. ) L# B; s  K7 g5 H* H
  47. void loop()
    9 n* h4 b! |7 Y% O
  48. {& A, L1 G& ?0 j+ y& ~( R; b& z
  49.   while(!((lightsensor_1.read()) < (10)));
    " w1 X4 D8 q1 x; L: G+ y
  50.   buzzer.tone(1047, 500);* ]- _. I% t; _: w' e. q6 E# U& ]
  51.   delay(500);
    " z5 R! h8 `, F. I' ?( T
  52.   do1 C. M, x- q* x! s; B
  53.   {
    " x8 N6 |, D( u& q, q' s& `  S
  54.    int Err = getErr();
    + s8 T9 r1 L5 X6 {7 t4 O
  55.    if(D1 == 1)! J! N0 x- m; `% G
  56.    {3 A# m. h& |# n. p7 I% c0 @
  57.      Left = 1;
    8 }' o0 z- c1 T5 h9 e
  58.    }  M/ A) E  e, e- j
  59.    if(D6 == 1)( o8 i9 R+ e6 [! J+ n7 B* G. r
  60.    {
    ( C7 }  J, s$ x& C+ O
  61.      Left = 0;7 J/ E+ C) r- H- q9 q
  62.    }5 N, S' ]2 Z0 }: C
  63.    if(Err == 99)$ \1 ]2 \9 j# B( W* ^- P; u
  64.    {- K1 [8 i- ~7 ~. B) h
  65.      if(Left == 1)
    ) G5 S9 N! W7 [
  66.      {
    1 \( R0 g) o0 m; S
  67.        Speed -= 5;; _5 x. b! P: j+ |2 m5 C
  68.        moto(0,Speed);
    # ?/ n+ K7 \/ d' y8 v) {7 {
  69.        do
    ! b! i0 Z% Y5 S4 Y0 R) Q1 R5 N
  70.        {
    # ^" X! E1 S) Z8 ~0 E
  71.          Err = getErr();
    # ~" T/ D( |3 K0 S  I+ V# q
  72.        }while((D1+D6) == 0);
    " B) N8 h! q% Q+ q& ~% N- y6 C
  73.      }# W( W( _+ L0 z- m8 R
  74.      else
    . j" [) ]! e* S" i7 `
  75.      {# P' t$ d. S* X) P
  76.        Speed -= 5;/ I) C8 H$ x" n2 `( T& f. u1 C! N
  77.        moto(Speed,0);
    ( N4 @+ }5 }7 h, \, @1 P
  78.        do
    " b8 X' C$ h2 J9 x$ S! Z
  79.        {7 @& T9 B: G6 a( n! n# C0 p3 I$ E% r
  80.          Err = getErr();
    $ l2 I$ y. \7 U7 V( e$ L5 F' v
  81.        }while((D1+D6) == 0);4 X9 t: \. [1 ]  ]  K5 K/ m
  82.      }
    ( w  e8 R+ G% c
  83.    }
    7 b7 ]! H. y0 h8 M) F( C- J, l3 Z
  84.    else' k5 {+ f2 E% @+ O4 x' @
  85.    {
    4 k  m0 \( N, i& Q. P, i% N' q
  86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    $ D- y4 L. m0 d
  87.      if((Speed > 100) & (Err > 2)) Speed-=2; 2 A& h; o4 b$ R/ P
  88.      integral = integral + Err;
    0 b( |& E: k8 x: a
  89.      derivative = Err - previous_error;& y# a# Z* m0 n3 H" V6 k
  90.      output = Kp*Err + Ki*integral + Kd*derivative;
    , s5 _1 n0 S: P+ _, _" p: l
  91.      moto(int(Speed-output),int(Speed+output));  M% `1 V' R( R  }; I( z+ a" M
  92.      previous_error = Err;
    # u2 \# ~2 n4 U2 {0 Y9 i  k
  93.    }
    9 L3 p" l5 H# k, [4 a, `& a6 ~
  94.   }while(!((lightsensor_2.read()) < (10)));) z9 f1 p' e; `- L/ G5 C
  95.   moto(0,0);
    , K3 d3 S( W# n0 T% l+ f6 ^
  96.   delay(500);
      s" W# e9 O" g
  97.   buzzer.tone(262, 500);/ m4 V$ g% x8 R7 [
  98. }
    6 ?+ q2 N: P* E' W
  99. 7 d" w- [1 d. T7 ?5 m7 }
  100. int getErr(), Q+ g3 m! q2 T: ^" v3 v
  101. {  
    : @5 i. S9 @( q. o  t, l  n2 b
  102.    sData = linefollower.getValue();0 b: T7 `# l% O; I
  103.    D1 = ~(sData>>0)&1;
    8 |) l7 ^! A, `
  104.    D2 = ~(sData>>1)&1;5 f/ [0 i; r, B4 @* I2 Q
  105.    D3 = ~(sData>>2)&1;  k2 W1 U! u1 d$ v2 f
  106.    D4 = ~(sData>>3)&1;1 p( _1 R4 }+ ^# k  F& J
  107.    D5 = ~(sData>>4)&1;
    * \# }6 k2 V$ K. y
  108.    D6 = ~(sData>>5)&1;" F- f) M4 s; w6 b  B
  109.    int downD = D1+D2+D3+D4+D5+D6;
    - H2 `$ l1 N7 ~
  110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);& d8 W: v8 P0 x1 x7 J2 g6 D! W4 ^
  111.    if(downD == 0)
    ) I6 ]% \' H2 X' }1 T" r( G4 ]6 {- L* r
  112.    {' F' ~0 o4 z3 R- m5 [* {
  113.      return 99;
    , _: I8 p- h8 E* p% G0 M; `
  114.    }- |4 h0 p6 d: Z% V/ K. c1 l% V  D
  115.    else
    9 @- C* l. M! c# s6 x* B1 P5 K
  116.    {
    " }" \# X1 K* U# J; w
  117.     return upD/downD;  T% B% ~, ~  Z1 r+ q% H) d) [+ h
  118.    }
    & G+ C  K( l) V- R% J& M$ Y
  119. }
    1 s! p/ u$ H, c

  120. 0 a) B3 r0 e0 l# Q& O
  121. void moto(int Speed_L,int Speed_R)4 A4 x+ g9 S, ?
  122. {: V: T7 a1 I2 @1 ]5 Y5 v! X0 O' e
  123.   Encoder_2.setMotorPwm(Speed_L);
    4 `9 N/ y# d/ w& M3 j
  124.   Encoder_1.setMotorPwm(-Speed_R);
    + ^" y  Q5 h4 ]6 p
  125. }
複製代碼

- B; q* C: o; h+ pMeLineFollowerArray.cpp
& `' J9 S' X  Z# a* m1 ^, P0 t
  1. #include "MeLineFollowerArray.h"/ E$ ]6 I% I- P8 _
  2.   _( E$ q  ?- Q' q
  3. #ifdef ME_PORT_DEFINED7 _4 @7 r- g0 {$ _
  4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
    1 a; W! z2 V  D7 n1 u- N' p
  5. {9 x; E" R$ R% R8 L: l4 }1 D& z, d
  6. : w1 i1 R1 G+ e; e% ^+ A& `
  7. }4 l. r$ v9 J( L& F# N
  8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)! J0 B- A+ a4 F
  9. {, w9 ~2 }+ x, e: i( R( D% I  @
  10.     _DataPin = mePort[port].s2;
    5 N% h8 H; F5 r6 |7 }; [3 y
  11.     pinMode(_DataPin, OUTPUT); ! @) Z% I' Z, ?( N9 n: z
  12.     digitalWrite(_DataPin, HIGH);8 }: u) u2 Z! P: F7 V! g$ z1 C
  13. }  Q1 C1 ]& K6 b
  14. #else // ME_PORT_DEFINED
    7 ^5 r9 t7 ]' N/ m
  15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)/ W4 e6 r* c9 [$ B$ n
  16. {
    + s& T. v4 D8 v
  17.     _DataPin = pin;
    : N2 S1 }- A0 x. x8 D  }9 C6 o' ^
  18.     pinMode(_DataPin, OUTPUT); / p# H% @( O' e8 t! i6 }
  19.     digitalWrite(_DataPin, HIGH);1 }/ e) U* m# \  s8 R/ c
  20. }
    ; w4 ~1 ?3 Z2 g& I# S/ r
  21. #endif // ME_PORT_DEFINED
      M& h9 r) ^2 r8 g9 u8 i

  22. . O1 s3 G, H1 N& ^. `7 o/ e. V, h

  23. . U( f2 F, @( E7 F
  24. void MeLineFollowerArray::setpin(uint8_t pin)
    ) r% w+ ^/ V  _
  25. {
    . ]1 q) z# Y% z1 x2 Q1 A
  26.     _DataPin = pin;: Z" M, G) m6 D& J2 }
  27.     pinMode(_DataPin, OUTPUT);
    3 O+ }  x3 y, G" f3 h+ i; N' B
  28.     digitalWrite(_DataPin, HIGH);3 i% l5 P- y9 z; e2 n5 e$ O# I

  29. & Y& T- E5 T, [
  30.     #ifdef ME_PORT_DEFINED
    4 ~& v+ l8 k! }; G& Z5 H
  31.     s2 = pin;
    0 b! X7 _# v! Y, x
  32.     #endif& m# c; K8 ?+ }" N1 B) i
  33. }1 d7 V: v. k, D5 x7 @! x' P

  34. 9 R9 O$ A' w- z2 L& ^. [9 H! X" m& ]
  35. uint8_t MeLineFollowerArray::getValue()2 Y, o! b. j/ F% ]6 m
  36. {
    . s' N- j/ D4 Y, X
  37.     uint32_t LOW_level_read_time;0 L4 s1 s" E$ {5 _; d* ]% ^6 t" z. F
  38.     uint32_t HIGH_level_read_time;
    . ^9 M& I, }$ D+ y/ y
  39.     uint32_t time_out_flag;. L: z, _# q8 i( a- j, u
  40.     uint8_t Sensor_Data[3];7 y8 Q5 q& T4 w' o, T' m1 F
  41.     static uint8_t old_data = 0xff;
    5 c( n2 I2 j; n) r: m7 @- f% l+ \4 a

  42. ! l( `  ]7 K3 l7 K/ L1 `2 d+ h
  43.     pinMode(_DataPin, OUTPUT);6 f# l6 Y' g/ Q0 i
  44.     digitalWrite(_DataPin, LOW);
    4 a7 J' k" |6 M0 o9 H3 y9 `5 b
  45.     delayMicroseconds(980);
    & R( F, H$ w4 X- W! q- _
  46.     digitalWrite(_DataPin, HIGH);$ J- u" Q" C% X5 x. y
  47. / p' ~8 R3 x+ o* |6 R$ a
  48.     pinMode(_DataPin, INPUT_PULLUP);( P4 U2 x& |: a" [* p6 v
  49.     delayMicroseconds(10);9 N* w/ [- i/ Z4 Y2 q
  50. # {" _- n# I0 O- R, U
  51.     time_out_flag = millis();' L2 \3 y6 Z6 F4 f5 E
  52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    3 g- z, t1 W# R- v6 m0 `" v
  53. # j5 {2 t: e" _$ K- a
  54.     LOW_level_read_time = micros();
    $ M% f% ^9 y9 t: |" S
  55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    ) B, P/ P/ H. [
  56.     {
    ! ?% S0 l) [. k3 t0 C
  57.         return 0xff;
    5 V2 }2 W9 j: W* R3 z2 z& _
  58.     }) L) m$ ]. C& @! s/ b! o
  59. ; O) X- A, H! f) N. ]& m' y- C
  60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );/ s, Q. p1 O; r& h( A$ s

  61. 5 \" D- u6 B) s* K/ v. a  I* u
  62.     HIGH_level_read_time = micros();
    $ V; o' o3 q  p
  63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level3 d: E5 C4 P1 P) @

  64. 5 o+ I$ t5 @6 y  D2 B/ J
  65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    ! w: c" T- |9 V' d
  66.     {
    - |' R* w. C9 I- q9 _0 H  R
  67.         return 0xff;4 Z. f# p0 K" Q) u& R
  68.     }( m2 L8 R0 A; F5 |+ \

  69. 8 E" `+ O8 }$ J
  70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110)): k" W8 Y0 }1 X  `5 J
  71.     {: i2 Q/ I" u( [. P; g& q
  72.         return 0xff;" l1 n4 x" w3 ^3 f% D9 G7 T4 ~7 B
  73.     }
      P$ l2 S- W# V  [9 y
  74. 0 B9 p4 h: a3 Y# q9 ?
  75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );0 u# U5 D/ w' _0 o' p
  76.     LOW_level_read_time  = micros();$ B1 O: u' l* Z! p  m
  77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level
    : C6 h& a  }: H, Q6 Q/ _2 ?9 v

  78. 2 k; c; w# r$ v9 a0 ]4 c  o4 A
  79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    + T9 Q+ N5 T% ]- d' g
  80.     {; L1 e* _+ I$ l# u5 x. Q
  81.         return 0xff;
    * A5 O; _1 Y# D! a- a; `: s
  82.     }# r& w6 S; f4 H8 |  {
  83. ; z) F2 w+ i% ]( k
  84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    - `$ m5 l9 O9 T- a3 @. E1 S* F
  85.     {1 `/ Y9 G0 g# X
  86.         return 0xff;
    & R- D4 M/ h1 p% L" R) p
  87.     }) H& k7 M/ j. r6 l: d

  88. 7 h5 r, C9 Q+ u# }% A! W' ?
  89.     for(uint8_t k=0; k<3; k++)! z% d' J" c. }1 j5 G* W
  90.     {& {/ e3 m, J, v  h2 a- r
  91.         Sensor_Data[k] = 0x00;
    ( E! L' B4 X9 Q6 t- f

  92. ) N) T) O/ k+ Z; r4 F# k; I
  93.         for(uint8_t i=0;i<8;i++)- s* Q2 ~% F# d" k. [* g% l' f
  94.         {- W' u9 R% F; Z8 n5 V6 ?* _! r
  95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level! c3 d6 s' S+ q6 |( ^; `
  96.             HIGH_level_read_time = micros();2 @8 }8 l3 ?$ z6 \6 a
  97.             LOW_level_read_time  = micros() - LOW_level_read_time;6 W0 q1 ]1 h7 |' ~* W
  98. % B- ~8 C1 {4 M8 N1 _
  99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
      ~/ ^) A- J* R# E" _' J
  100.             {! }" x( k( r+ D  ^4 A- z+ Z) C
  101.                 return 0xff;0 [; G; {4 W" e' O9 G
  102.             }
    0 T! B& W" I% K% m& Z" x/ z2 o
  103. ! E; ^& f5 |& E/ U9 V) W7 @( h
  104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );( X: A, o+ F9 M# }
  105.             LOW_level_read_time  = micros();' y" u% t! d+ j: H  ]
  106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
    + T; c# D' s1 E" g
  107. 5 [# W+ ?  d* h' j& Y; R
  108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 13 ~  g9 n; p8 q+ x
  109.             {
    & ?* c+ o4 g% ~8 t% ^: O
  110.                 Sensor_Data[k] |= (0x80 >> i);
    3 t+ |( d" ?8 }* ^
  111.             }
    - z% G5 _  x0 a
  112.             else if(HIGH_level_read_time >= 100)
    . e. p! i; r9 {+ \% t1 U6 G
  113.             {  V$ S7 P! w; G  A6 h( ?* ~
  114.                 return 0xff;
    & Q$ s* b2 v& R3 g3 `# m: O
  115.             }7 g1 F- J8 C8 U: N# |' q# Q6 c

  116. 5 i9 k: F/ D  p1 J) d
  117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)8 R5 s  `) {  \) H6 h. B
  118.             {
    2 `! g" e' D5 T& N# f
  119.                 return 0xff;
    ( F, ^# a1 g1 Q( T- |, B8 n
  120.             }+ P$ K/ r" x5 ?) Q2 Q
  121.         }
    + H  u4 Z% T) W0 z
  122.     }/ J& U2 {8 `& t+ E
  123. ' R  x8 s* J/ c$ f  ?/ I6 S
  124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level% ]+ J# X# f+ Q3 B  T9 J: `! v
  125.     HIGH_level_read_time = micros();! X; e+ m) {1 N' Y' S
  126.     LOW_level_read_time  = micros() - LOW_level_read_time;! h5 J  E  X0 A$ W
  127. . F- ^# d5 w& Z; d: l* z
  128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )0 R0 @: t7 }/ D- L  K, R
  129.     {0 K: i7 O# E  ?; f7 V2 z
  130.         return 0xff;
    ; [* r, p$ N/ t( [5 x( [" @( W
  131.     }& d( X+ h' W) J+ a/ b

  132.   g* h& {1 ?( C- t& C) A7 h% T
  133.     pinMode(_DataPin, OUTPUT);+ @; D. ^! A; _9 C* @
  134.     digitalWrite(_DataPin, HIGH);
    4 w! P& `( z, `' d2 j% j
  135. % i* K; K2 u( Y! i( {9 A+ e; b
  136.     if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0]))), k& s5 [3 h- l: Y# I. a
  137.     {/ g8 o. J; O3 {' R2 s
  138.         old_data = Sensor_Data[0];2 s1 `" u  u, M! h/ }7 m
  139.         return Sensor_Data[0];1 o$ W+ X# j* _, I: ~
  140.     }: H1 ~7 F% j5 g  @2 L
  141.     else+ j, o+ S9 y& |: X
  142.     {/ N9 H( ~. J- `- q8 c0 L
  143.         return old_data;
    ! j9 a4 K/ j; _. \/ c
  144.     }
    - M, h: I! @' x* U2 N" w. W1 p
  145. }- O7 Y: b( U" z' O. a* X
複製代碼

8 {% b. F  g0 A" [/ IMeLineFollowerArray.h   MeLineFollowerArray.h (731 Bytes, 下載次數: 0, 售價: 100 個銅錢)
您需要登錄後才可以回帖 登錄 | 立即註冊

本版積分規則

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

GMT+8, 2018-12-13 20:51 , Processed in 0.059186 second(s), 20 queries .

Powered by Discuz! X3.4

© 2001-2017 Comsenz Inc.

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