mBot Ranger 高速循線範例＿

% j2 _7 b5 J9 g  X% w

1. #include <Arduino.h>
   7 q. O, u' g+ P( H8 \" z
2. #include <MeAuriga.h>
   ) Y9 g2 ?3 o7 _) n- m0 [9 Z8 j
3. #include "MeLineFollowerArray.h"
   4 b- D4 N+ v2 l  t$ l
4. 
   
5. MeEncoderOnBoard Encoder_1(SLOT1);
   & Q0 A$ ~/ Z6 p  q' J, \9 E
6. MeEncoderOnBoard Encoder_2(SLOT2);
   ( V, _+ A4 F, b( ]. u- A. d9 Q* n
7. MeLightSensor lightsensor_1(12);
   # u4 {9 v/ Y7 O3 u7 w  E
8. MeLightSensor lightsensor_2(11);
   
9. MeBuzzer buzzer;
   
10. MeLineFollowerArray linefollower(PORT_6);
    
11. 
    8 d+ @+ |$ ]* v6 L
12. #define Error1 1
    
13. #define Error2 2
    4 b: g$ J  y  f4 X0 @) @: r1 ]
14. #define Error3 3
    
15. 
    " R; R  P1 N! [: K& [
16. #define Kp 15
    0 ~6 u) O, |' f5 D
17. #define Ki 0.15
    : t4 `. J  P1 _" n! j% _" i
18. #define Kd 0.03
    4 w, s+ A/ e& F
19. 
    8 }+ J, `+ I7 K0 F# }
20. uint8_t sData;
    
21. uint8_t D1;
    ' w  O! D' |3 ^' I0 h
22. uint8_t D2;
    
23. uint8_t D3;
    
24. uint8_t D4;
    
25. uint8_t D5;
    
26. uint8_t D6;
    
27. 
    
28. float previous_error = 0;
    
29. float integral = 0;
    $ d9 i8 o" q* x; c
30. float derivative = 0;
    
31. int  Speed  = 160;
    
32. float output;
    ) N3 Z6 v0 u% a
33. 
    
34. byte Left;
    ( c  _+ A2 ?4 z  h# n
35. 
    
36. void setup()
    & [% @$ T& i# A" \
37. {
    7 y8 j4 H2 i2 p- B+ }1 |! \# T
38. //Set PWM 8KHz
    
39.   TCCR1A = _BV(WGM10);
    
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    + H- N3 y4 p4 _; ^( z
42.   TCCR2B = _BV(CS21);
      ~' Z0 P5 q! }2 S% v
43.   Serial.begin(9600);
    
44.   buzzer.setpin(45);
    & F' E" ]5 L1 s; Z3 T; d1 N
45. }
    
46. 
    9 B" v: c1 G6 q! a( G1 m) `2 _/ |
47. void loop()
    8 x% E3 p' p* u0 J
48. {
    
49.   while(!((lightsensor_1.read()) < (10)));
    : _! ~- [) Y& t* L, ~, [8 Z
50.   buzzer.tone(1047, 500);
    
51.   delay(500);
    ) Q9 I3 k6 a3 L. f, c+ w8 `, p
52.   do
    
53.   {
    
54.    int Err = getErr();
    
55.    if(D1 == 1)
    
56.    {
    9 f) Q1 |$ k# O0 T( D$ [7 ~
57.      Left = 1;
    ( V. F$ ~7 {; g" t! j
58.    }
    # C3 U/ N- B  w, m! U" s
59.    if(D6 == 1)
    
60.    {
    
61.      Left = 0;
    ' Z7 u1 y; w6 ^( z1 C
62.    }
    ! f$ A2 ~$ W# p. Y" ]
63.    if(Err == 99)
    
64.    {
    
65.      if(Left == 1)
    ( G5 z7 G, ~1 q
66.      {
    # ~, S8 E) c, b* C+ v7 u  D
67.        Speed -= 5;
    
68.        moto(0,Speed);
    
69.        do
    
70.        {
    % z) y! E  C- j2 v6 z
71.          Err = getErr();
    & v7 [1 }# ]2 C
72.        }while((D1+D6) == 0);
    
73.      }
    " j' T; [* l! B
74.      else
    1 R; u+ W* P3 l1 B9 y. i. x
75.      {
    
76.        Speed -= 5;
    1 |- @: U0 X, f9 ?
77.        moto(Speed,0);
    
78.        do
    & B- l: T+ i; S- s* w: {
79.        {
    7 E% c" ?- ~4 C: L- F
80.          Err = getErr();
    6 k! e" ?5 c- X1 p8 N& }% i) C9 o
81.        }while((D1+D6) == 0);
    9 z5 x* s: k7 E% L0 w2 P
82.      }
    . N6 v, _  p" p; x) ]' m
83.    }
    
84.    else
    # Q, B& i% g7 A* T; W$ l
85.    {
    3 E1 y9 i- q9 y( C/ s) E4 H0 i
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    6 e+ {0 J7 z8 z# C8 ^; ]1 W
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    
88.      integral = integral + Err;
      U/ c# J$ i& P7 V7 x2 e
89.      derivative = Err - previous_error;
    # H  K: O, U, |# q3 Y% B
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    6 t/ Z8 K: e7 ^6 r
91.      moto(int(Speed-output),int(Speed+output));
    0 [9 W1 j2 @# k1 n
92.      previous_error = Err;
    
93.    }
    
94.   }while(!((lightsensor_2.read()) < (10)));
    
95.   moto(0,0);
    8 v& ]6 t0 \3 @6 Y3 i* |
96.   delay(500);
    # D" E" p3 N9 }. R7 {
97.   buzzer.tone(262, 500);
    
98. }
    
99. 
    
100. int getErr()
     7 h  B+ G! C+ F0 F9 Z% B
101. {  
     ' Y9 {2 s) R: c. ^
102.    sData = linefollower.getValue();
     + A' i- b3 W! Z& Q
103.    D1 = ~(sData>>0)&1;
     9 f! Y$ L2 s2 `  x1 t5 I
104.    D2 = ~(sData>>1)&1;
     
105.    D3 = ~(sData>>2)&1;
     
106.    D4 = ~(sData>>3)&1;
     ) v$ E# e$ s! H9 P
107.    D5 = ~(sData>>4)&1;
     % p! u! d  C7 {" |6 p
108.    D6 = ~(sData>>5)&1;
     $ U7 T& h/ u0 u
109.    int downD = D1+D2+D3+D4+D5+D6;
     
110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
       A$ t" M& _8 J  \+ P8 X7 u
111.    if(downD == 0)
     : r6 _7 s# G$ A2 R' c3 ]% b
112.    {
     
113.      return 99;
     5 x' i" J" s( k' U! h
114.    }
     , v+ n) Z0 N/ B$ ?0 ], d: [9 W
115.    else
     , T8 O5 X% t/ D. J, O. h9 e
116.    {
     # z6 D' e; w& G7 d3 _
117.     return upD/downD;
       v. c$ S: I2 J% q* e
118.    }
     # y1 U! ?/ f+ U6 w  }6 q
119. }
     
120. 
     
121. void moto(int Speed_L,int Speed_R)
     
122. {
     1 ^& G' E5 j4 B* K* X8 W5 B9 l5 \
123.   Encoder_2.setMotorPwm(Speed_L);
     - _" m% e/ `% P+ b; ^, Y
124.   Encoder_1.setMotorPwm(-Speed_R);
     
125. }

複製代碼

MeLineFollowerArray.cpp
( U  L& c5 U! \' q

1. #include "MeLineFollowerArray.h"
   4 k) [' r+ M6 y) b$ N' k+ F5 n% g
2. 
   
3. #ifdef ME_PORT_DEFINED
   
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   ! C# g% `! d, S& k% J' M  f' u& G
5. {
   1 N+ Z: I1 [0 c6 I7 H* U
6. 
   $ z& M! s" O6 Y7 `0 {: |& S
7. }
   
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   1 \3 d0 v' J3 u) f/ F0 w  A
9. {
   ; `! N' B/ q5 {; s. a
10.     _DataPin = mePort[port].s2;
    
11.     pinMode(_DataPin, OUTPUT);
    
12.     digitalWrite(_DataPin, HIGH);
    / Y  i4 m- o4 f: ]3 y: [% S8 w
13. }
    
14. #else // ME_PORT_DEFINED
    " c/ H1 W( R/ r5 n7 g- F" O6 c& \
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    * |- U; o# A- H: H+ r
16. {
    
17.     _DataPin = pin;
    
18.     pinMode(_DataPin, OUTPUT);
      q' J: ~$ `$ l5 J, s8 ?
19.     digitalWrite(_DataPin, HIGH);
    2 ^8 ~+ h" n8 \* N8 e
20. }
    
21. #endif // ME_PORT_DEFINED
    2 h: F- m0 x3 }: z
22. 
    $ U# a+ f3 b, ^/ g: v1 p
23. 
    , M# J* i7 p3 j$ r7 z1 x+ J
24. void MeLineFollowerArray::setpin(uint8_t pin)
    . \7 I5 n( h+ s+ s8 Y
25. {
    0 Y; F0 v# s: z: A
26.     _DataPin = pin;
    " |* Z, R$ x* G# I
27.     pinMode(_DataPin, OUTPUT);
    0 Y1 f, P- r- a+ m$ u
28.     digitalWrite(_DataPin, HIGH);
    
29. 
    
30.     #ifdef ME_PORT_DEFINED
    & d+ }# _1 |& j* T  c! j- ~+ Y: J
31.     s2 = pin;
    
32.     #endif
    
33. }
    & B- G" d8 z8 a& R( g& e4 b& e
34. 
    , v4 l2 t2 c2 a% A3 l. z2 d
35. uint8_t MeLineFollowerArray::getValue()
    
36. {
    
37.     uint32_t LOW_level_read_time;
    
38.     uint32_t HIGH_level_read_time;
    
39.     uint32_t time_out_flag;
    9 S# |( h. @, z3 ?4 S2 e, f
40.     uint8_t Sensor_Data[3];
    # {4 {  Y0 h9 E
41.     static uint8_t old_data = 0xff;
    
42. 
    
43.     pinMode(_DataPin, OUTPUT);
    & P# F0 I+ u# T! U' Q! r2 d  {
44.     digitalWrite(_DataPin, LOW);
    * D  w, q; j/ x( r7 S
45.     delayMicroseconds(980);
    
46.     digitalWrite(_DataPin, HIGH);
    
47. 
    - U4 j7 \) L  N
48.     pinMode(_DataPin, INPUT_PULLUP);
    & z6 f8 i! W8 J: n' B5 w: A
49.     delayMicroseconds(10);
    
50. 
    
51.     time_out_flag = millis();
    
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    4 A6 }, O# K$ u; ^
53. 
    * L/ z* {0 r  p! a# f9 E" _9 [2 h
54.     LOW_level_read_time = micros();
    
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
56.     {
    
57.         return 0xff;
    
58.     }
    
59. 
    
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    0 m6 N/ C! }( y/ @& D3 ^
61. 
    
62.     HIGH_level_read_time = micros();
    
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    
64. 
    
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    . v' I" c) K/ n* j& t" U! e. Q
66.     {
    & I; }0 s& E  {. u
67.         return 0xff;
    
68.     }
    
69. 
    ) |. M: q+ [! [+ G
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    
71.     {
    ; t& r# d8 Q3 _& z! {- G% R" e+ `
72.         return 0xff;
    
73.     }
    
74. 
    
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
76.     LOW_level_read_time  = micros();
    
77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level
    
78. 
    " R& ~$ S4 V; B- s( ^: p
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
80.     {
    
81.         return 0xff;
    
82.     }
    
83. 
      P( Q. i+ q, S2 c6 D/ `! _
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
      [* |: q" K& U/ X, ]4 K
85.     {
    . P, Z/ a7 E: T; I7 @0 @: Z& Y" B
86.         return 0xff;
    ; J! D- ~1 K- g
87.     }
    9 `1 _! C- s2 U. X5 I3 o, N1 {0 K& ]
88. 
    
89.     for(uint8_t k=0; k<3; k++)
    2 @4 S" F0 B  V! F% p4 F& R
90.     {
    
91.         Sensor_Data[k] = 0x00;
    
92. 
    " |- }' l( o' `& l1 Z; N
93.         for(uint8_t i=0;i<8;i++)
    / e3 q( y& M) r# `8 p8 j$ Q
94.         {
    & _' q- `8 u5 v
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    
96.             HIGH_level_read_time = micros();
    % U- H1 M2 ^2 D4 u; Z$ c) Y" i# \- D* t
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    
98. 
    
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    ( x2 W. O2 @0 v
100.             {
     1 ~5 Y# }) ~  r& I$ o, i
101.                 return 0xff;
     
102.             }
     3 g, d5 ~  S- P( n& U
103. 
     
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     * b9 u+ a9 r& u' B0 L& u" i
105.             LOW_level_read_time  = micros();
     1 M) w1 I9 j; L* O
106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
     
107. 
     $ I# x! e: D: o; F. _# P) @
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     3 H5 a6 D( R$ L. p+ R0 w+ D
109.             {
     : x6 g; F$ Q: R8 z* m5 ~
110.                 Sensor_Data[k] |= (0x80 >> i);
     
111.             }
     ' d% O" `! m' D0 l" B7 ~8 f8 V
112.             else if(HIGH_level_read_time >= 100)
     
113.             {
     ; l  [7 z4 e( S+ U, B/ l+ D* u
114.                 return 0xff;
     7 m: ?: [, N/ h  Q" a0 F
115.             }
     % p8 }; U9 u$ x- ]
116. 
     
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     
118.             {
     
119.                 return 0xff;
     + h; N" ?/ F$ |; ~# ^
120.             }
       _4 I: P2 p) `4 h8 ~8 U. p
121.         }
     8 c4 u" g" ]$ ~6 Y) G
122.     }
     5 i8 s8 B. K4 W# N; e7 C, C
123. 
     ( m% k, z: J+ K8 _
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     4 l9 a/ I8 O& q1 `2 t  Q
125.     HIGH_level_read_time = micros();
     
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     
127. 
     " }3 o8 t: b! x& N0 F
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     6 o( [$ U% y. Z; n
129.     {
     
130.         return 0xff;
     
131.     }
     " Y6 c& D4 M- u4 ^4 G! ]
132. 
     
133.     pinMode(_DataPin, OUTPUT);
     % W! Y- d' h+ b/ o8 m9 C
134.     digitalWrite(_DataPin, HIGH);
     % e5 V" c" g3 v$ u/ N! @
135. 
     
136.     if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
     
137.     {
     0 \4 y% [; R5 @: z" g
138.         old_data = Sensor_Data[0];
     
139.         return Sensor_Data[0];
     
140.     }
     ! m1 `; s+ }5 l( g
141.     else
     $ K! g' h$ Z! n! }( w
142.     {
     
143.         return old_data;
     
144.     }
     3 P" ~; ~8 ]6 Y3 F% d
145. }
     

複製代碼

MeLineFollowerArray.h   MeLineFollowerArray.h (731 Bytes, 下載次數: 10, 售價: 100 個銅錢)

2018-2-13 01:52 上傳

點擊文件名下載附件
閱讀權限: 20

. m/ t1 d6 K& a1 n! D4 V

強力推廣中

哇...要100 個銅錢

daven 發表於 2019-3-6 00:09
哇...要100 個銅錢

2 H" k8 Z1 I% f! H  A. q8 w; u認真的回饋本站一些內容, 很容易達成的!

感謝分享，還不夠權限下載

感謝分享Thanks

能否用到mblock 5 上面呢？

roboger 發表於 2020-11-13 15:43
能否用到mblock 5 上面呢？

+ T- S; c4 K4 J+ L. {# H* bMeLineFollowerArray 在 mBlock5 中, 只有支援 megaPi 控制板, Ranger 無法使用, 如有需求, 我再寫一個擴展上架吧!

无法添加到扩展呀

高手就是高手