mBot Ranger 高速循線範例＿

1. #include <Arduino.h>
     C% M6 k9 x$ k
2. #include <MeAuriga.h>
   $ s& d. K1 Y* l9 M9 {, H. {% R/ d
3. #include "MeLineFollowerArray.h"
   
4. 
   # V% n) V& J" G& O1 _! ?5 H8 n* Z
5. MeEncoderOnBoard Encoder_1(SLOT1);
   
6. MeEncoderOnBoard Encoder_2(SLOT2);
   
7. MeLightSensor lightsensor_1(12);
   
8. MeLightSensor lightsensor_2(11);
   
9. MeBuzzer buzzer;
   2 D4 b1 Q3 E# i4 h
10. MeLineFollowerArray linefollower(PORT_6);
    8 s. y2 W+ r6 g: T9 A
11. 
    
12. #define Error1 1
      z; Z  n% Z3 t* n) ^5 O9 G
13. #define Error2 2
    
14. #define Error3 3
    * y+ ^8 k$ G- v& O
15. 
    7 S# V9 F* ^, X
16. #define Kp 15
    
17. #define Ki 0.15
    
18. #define Kd 0.03
    % ?+ c8 v# i1 |( T* Z; r2 k
19. 
    : Q- u& y/ ?! G! a$ x+ @, M# y  p
20. uint8_t sData;
    5 W! U5 y3 E. l7 p* x
21. uint8_t D1;
    
22. uint8_t D2;
    
23. uint8_t D3;
    $ B7 q7 C! J: v: `! g: j& w1 M. \3 d
24. uint8_t D4;
    + e* l" |# d+ L+ n- J
25. uint8_t D5;
    
26. uint8_t D6;
    
27. 
    
28. float previous_error = 0;
    # ~) `- }( j) u' w: N5 ^7 V" g
29. float integral = 0;
    * A% c* I" t$ L( y& F0 m
30. float derivative = 0;
    ! E- c0 E4 J  g' s! z! F4 j
31. int  Speed  = 160;
    
32. float output;
    
33. 
    . q% Q, S/ t% Y* n9 o$ @" ]1 j
34. byte Left;
    
35. 
    
36. void setup()
    9 Q! h& \! h, Y. N
37. {
    + a; z! E1 b9 S* O& d9 `, M9 m
38. //Set PWM 8KHz
    
39.   TCCR1A = _BV(WGM10);
    
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    
42.   TCCR2B = _BV(CS21);
    
43.   Serial.begin(9600);
    8 w' K* E- L3 Y$ }0 }) ?
44.   buzzer.setpin(45);
    0 Q4 M. I7 e! V0 L6 ]
45. }
    
46. 
    - D# R5 }( F" @) u; L
47. void loop()
    
48. {
    . @6 `6 ]% i: Z5 U
49.   while(!((lightsensor_1.read()) < (10)));
    
50.   buzzer.tone(1047, 500);
    ' N' H" e; [* w* J
51.   delay(500);
    
52.   do
    
53.   {
    
54.    int Err = getErr();
    
55.    if(D1 == 1)
    
56.    {
    
57.      Left = 1;
    
58.    }
    
59.    if(D6 == 1)
    
60.    {
    0 K4 I) R7 Y; S$ r; L: v  h% ^
61.      Left = 0;
    
62.    }
    
63.    if(Err == 99)
    
64.    {
      }2 R: b6 |" [, e& V% Z2 N9 ~
65.      if(Left == 1)
    
66.      {
    
67.        Speed -= 5;
    % G1 ~2 m& T5 X" x
68.        moto(0,Speed);
    * a2 |* d  l. \) D% _
69.        do
    ' F7 o0 [1 c1 S3 |( b
70.        {
    
71.          Err = getErr();
    
72.        }while((D1+D6) == 0);
    
73.      }
      b' T5 E$ n) C. c4 E
74.      else
    
75.      {
    
76.        Speed -= 5;
    ) W) i; q  P# e% u$ `
77.        moto(Speed,0);
    9 n* o$ I: c" \
78.        do
    ) E0 o5 T. o3 @# E4 [! m" C: m
79.        {
    
80.          Err = getErr();
    
81.        }while((D1+D6) == 0);
    ) m; ?, f- \) _
82.      }
    
83.    }
    
84.    else
    
85.    {
    8 \* N3 w9 I7 k0 M  ^
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    9 I* y& _/ J4 G+ M; b) `7 ~
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    # @+ d% D0 K5 [  L2 ~8 G
88.      integral = integral + Err;
    % i, x4 y6 j( H
89.      derivative = Err - previous_error;
    5 n. V. T. Q- i& g
90.      output = Kp*Err + Ki*integral + Kd*derivative;
      ?: P8 n3 s8 F4 `7 I
91.      moto(int(Speed-output),int(Speed+output));
    
92.      previous_error = Err;
    & K3 P# P" N. d8 k6 w3 |' h
93.    }
    
94.   }while(!((lightsensor_2.read()) < (10)));
    
95.   moto(0,0);
    ' j, I% i: r! c0 b! g* O$ W
96.   delay(500);
    8 w  z. Y2 N# J' @) S& ?: H
97.   buzzer.tone(262, 500);
    - d: L8 |- d' O: t- M
98. }
    7 Z* a0 D# P$ I4 t
99. 
    
100. int getErr()
     + B, |/ D, M' p: c% [$ y9 X1 K0 \2 R
101. {  
     * h5 C& \, O+ X4 l2 T: v6 t% C
102.    sData = linefollower.getValue();
     
103.    D1 = ~(sData>>0)&1;
     
104.    D2 = ~(sData>>1)&1;
     , g1 m% j0 B* D+ w+ m8 P( S
105.    D3 = ~(sData>>2)&1;
     
106.    D4 = ~(sData>>3)&1;
     7 l. d5 ~% m0 v4 L/ F8 ?( r( W* B
107.    D5 = ~(sData>>4)&1;
     . X: K& R( w$ f
108.    D6 = ~(sData>>5)&1;
     $ _- {! H; ^: `  L+ V0 F! s) O
109.    int downD = D1+D2+D3+D4+D5+D6;
       p. L0 Z. u. i# r# T0 K
110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
     & b4 E. L+ ?4 x! ~" i: s0 q( i
111.    if(downD == 0)
     9 V' @" {" s1 k  J$ x: V0 I; A, f
112.    {
     * R7 [8 {# E, V; w# Z
113.      return 99;
     
114.    }
     & A+ j% z0 T2 }; W
115.    else
     . z- _8 {7 Z) v1 ]9 X* O4 r; \
116.    {
     : H2 S, j9 Q4 s4 d- Q  x7 h, L
117.     return upD/downD;
     
118.    }
     / I! ]! _( a4 I9 `2 U) S
119. }
     
120. 
     % B& b. P5 |: J+ J) T& H
121. void moto(int Speed_L,int Speed_R)
     
122. {
     
123.   Encoder_2.setMotorPwm(Speed_L);
     + E) j; H( T& W! h1 @6 H
124.   Encoder_1.setMotorPwm(-Speed_R);
     4 o( Y! D' n# W' [
125. }

複製代碼

MeLineFollowerArray.cpp
5 ?& P  v4 e5 b. J) V$ [  l

1. #include "MeLineFollowerArray.h"
   - y1 S! t6 ~. o) F1 m% Z
2. 
   
3. #ifdef ME_PORT_DEFINED
   2 G/ }/ ^# C2 M4 l) Z9 H; j: A1 |
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   
5. {
   
6. 
   ; M  Q8 K7 c0 i# y% R
7. }
   ( Y6 @! H! A7 Y
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   : ?$ U, B8 W! |; P# U/ @2 h: T
9. {
   8 @+ P$ Q# E5 M6 A
10.     _DataPin = mePort[port].s2;
    
11.     pinMode(_DataPin, OUTPUT);
    1 f, s7 G$ |0 n2 U$ M% k0 X* U
12.     digitalWrite(_DataPin, HIGH);
    
13. }
    
14. #else // ME_PORT_DEFINED
    ' I4 K5 W7 f. v
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    # j4 N9 M& v( U; T- z9 C; F
16. {
    & }' ^' ^9 L0 S% s
17.     _DataPin = pin;
    % N, Y4 u2 Z# [# T$ t% N; @
18.     pinMode(_DataPin, OUTPUT);
    - Y' ~$ ]/ ?3 s5 O7 e
19.     digitalWrite(_DataPin, HIGH);
    
20. }
    / A" j# i. O, L7 N, r
21. #endif // ME_PORT_DEFINED
    # e, h# ^% ^& O% h/ L" o
22. 
    0 \! W" Y9 Z2 a& U' y
23. 
    ; @7 B1 M" \; Q/ f, H
24. void MeLineFollowerArray::setpin(uint8_t pin)
    
25. {
    / H8 q7 E8 D: _% G& [5 A9 R3 H
26.     _DataPin = pin;
    " `( Y! a4 b8 u4 X
27.     pinMode(_DataPin, OUTPUT);
    
28.     digitalWrite(_DataPin, HIGH);
    4 o, [/ }* y% F/ f6 G
29. 
    
30.     #ifdef ME_PORT_DEFINED
    
31.     s2 = pin;
    . n" o$ ^4 g. e+ S, y; l( b+ c% Z4 v/ E
32.     #endif
    3 f, C- C+ g% a* I& }4 i) ?/ j
33. }
    $ Q( f9 Y* t! R5 f& {# S# C2 f
34. 
    
35. uint8_t MeLineFollowerArray::getValue()
    
36. {
    
37.     uint32_t LOW_level_read_time;
    6 B% k- H& }$ _* N2 k
38.     uint32_t HIGH_level_read_time;
      j" Z' z/ X% S' P
39.     uint32_t time_out_flag;
    
40.     uint8_t Sensor_Data[3];
    4 M1 ~/ q9 f$ }0 {$ y
41.     static uint8_t old_data = 0xff;
    
42. 
    ! ^4 q4 b) L  q9 l0 L" e
43.     pinMode(_DataPin, OUTPUT);
    
44.     digitalWrite(_DataPin, LOW);
    
45.     delayMicroseconds(980);
    
46.     digitalWrite(_DataPin, HIGH);
    
47. 
    ( X3 g+ B/ Z* O
48.     pinMode(_DataPin, INPUT_PULLUP);
    7 G/ y0 a4 n1 u3 Y
49.     delayMicroseconds(10);
    ( H; y9 X5 s; ^
50. 
    
51.     time_out_flag = millis();
    
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    * Q# l! ~0 R1 R5 z9 }
53. 
    ' F, w5 V3 n& R) U( |( k7 _
54.     LOW_level_read_time = micros();
    , P  X- b& z( d
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
      Y5 P, x1 y# @4 ]8 Q, f& |
56.     {
      }8 K1 O% b" C
57.         return 0xff;
    
58.     }
    $ s. v& e  b: o) p4 Z& f9 _, H
59. 
    5 [+ m9 C3 _' p6 m3 `' g
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
61. 
    
62.     HIGH_level_read_time = micros();
    3 Y5 t" y% e' B6 K' }7 J
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    3 ~' ], c: r3 @$ q* p2 O5 w
64. 
    
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
66.     {
    
67.         return 0xff;
    ; C/ C! m+ T) q: ^" g
68.     }
    2 E. l5 B( [/ r$ I0 h4 u
69. 
    + i' s+ F2 ^; y) z3 h5 x; u
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    
71.     {
    
72.         return 0xff;
    
73.     }
    # L5 X0 O) b4 e: z% t( O' g2 A
74. 
    1 i( v% F$ J- n* X0 d; T, A
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    - F1 E7 s( F$ o* [0 U4 W* j
76.     LOW_level_read_time  = micros();
    
77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level
    
78. 
    0 c# o  |; E9 b7 x$ E* d3 `# [
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
80.     {
    9 v4 W' t) [2 v6 ~& m0 E% l' i
81.         return 0xff;
    " N2 `. Q$ z( P: \0 F* V- V) R0 T
82.     }
    8 Z& |+ _# C, z8 ]+ p7 T, z
83. 
    / A* V0 t. E) Q
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    
85.     {
    
86.         return 0xff;
    * o8 S7 E) M; w1 K2 ~- \
87.     }
    + V% q. ^7 z5 k' O7 H' k3 L8 v4 |3 }
88. 
    " T" o, h0 U$ Y* v2 A2 \
89.     for(uint8_t k=0; k<3; k++)
    - H) l, z! i5 D( r$ X0 g9 o2 o
90.     {
    1 M' B, I/ C- b% e+ B
91.         Sensor_Data[k] = 0x00;
    
92. 
    " [/ c& v* O0 p& _' N8 A
93.         for(uint8_t i=0;i<8;i++)
      X( Y: A% e2 ~  q, V9 X& ~2 X
94.         {
    2 P1 Y; g% ~6 n, B
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    
96.             HIGH_level_read_time = micros();
    
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    
98. 
    
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    
100.             {
     : Y& l) v1 E. c* L& f7 z! i
101.                 return 0xff;
     
102.             }
     $ M2 y; {4 Z8 T/ C; c1 ]
103. 
       c  I6 N! S- d
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     0 c) ?7 e" W. @( C  X& h0 j8 Z# l
105.             LOW_level_read_time  = micros();
     5 U& M. W! u' a
106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
     
107. 
     2 k) S2 B& g$ [) h' h6 ~& A
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     5 N* i* s. {4 [$ \. E
109.             {
     ; M$ k2 b# Y0 k2 u" I5 P
110.                 Sensor_Data[k] |= (0x80 >> i);
     5 g2 E2 d8 X/ d6 Q9 {
111.             }
     
112.             else if(HIGH_level_read_time >= 100)
       s, H+ h; t. ?5 H3 F
113.             {
     7 {) @8 ]0 K: u% ^
114.                 return 0xff;
     
115.             }
     
116. 
     3 \3 J9 d- k+ j5 V% {+ p
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     
118.             {
     
119.                 return 0xff;
     
120.             }
     1 L0 M( f- a# c& \: M8 H
121.         }
     
122.     }
     
123. 
     
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     
125.     HIGH_level_read_time = micros();
     
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     3 f2 g# f5 y  E8 X, c3 \
127. 
     3 X- R0 ~7 Y! M" s
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     
129.     {
     - i; c# X! R; d8 G. t. f
130.         return 0xff;
     9 P* ^# Y+ }& B
131.     }
     
132. 
     
133.     pinMode(_DataPin, OUTPUT);
     / X0 M) n0 J* l3 U* \
134.     digitalWrite(_DataPin, HIGH);
     
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.     {
     ' H) J) R' h  x" g# q5 J, E% J
138.         old_data = Sensor_Data[0];
     
139.         return Sensor_Data[0];
     
140.     }
     
141.     else
     
142.     {
     
143.         return old_data;
     # P# c4 s  z& l1 q) h$ o
144.     }
     
145. }
     6 k$ h5 `! U# `! j

複製代碼

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

2018-2-13 01:52 上傳

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

強力推廣中

哇...要100 個銅錢

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

認真的回饋本站一些內容, 很容易達成的!

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

感謝分享Thanks

能否用到mblock 5 上面呢？

roboger 發表於 2020-11-13 15:43
1 }( F8 W/ C# Y" M, o9 s+ @能否用到mblock 5 上面呢？

MeLineFollowerArray 在 mBlock5 中, 只有支援 megaPi 控制板, Ranger 無法使用, 如有需求, 我再寫一個擴展上架吧!

无法添加到扩展呀

高手就是高手