mBot Ranger 高速循線範例＿

1. #include <Arduino.h>
   # `  }. ?# y  k: T
2. #include <MeAuriga.h>
   . a' Z: E6 H) a5 U6 [6 {! D
3. #include "MeLineFollowerArray.h"
   . |9 _' y- |, V% M) G
4. 
   
5. MeEncoderOnBoard Encoder_1(SLOT1);
   
6. MeEncoderOnBoard Encoder_2(SLOT2);
   7 D  h1 v- w; _
7. MeLightSensor lightsensor_1(12);
   
8. MeLightSensor lightsensor_2(11);
   . v8 T' f% Z4 a9 Q: w
9. MeBuzzer buzzer;
   2 N/ K% S, {$ j/ i& [
10. MeLineFollowerArray linefollower(PORT_6);
    : `9 l5 y8 i' `9 v8 R6 U( U
11. 
    
12. #define Error1 1
    - f# L* v% {8 k
13. #define Error2 2
    
14. #define Error3 3
    % t) }0 Q4 b. l+ M9 m6 Q2 L1 }# q+ M
15. 
    
16. #define Kp 15
    
17. #define Ki 0.15
    . w+ H- t% f. G5 h' s5 {
18. #define Kd 0.03
    
19. 
    3 ^# m" M* V5 E7 A; j- [2 y0 c% a
20. uint8_t sData;
    
21. uint8_t D1;
    
22. uint8_t D2;
    * D: C! v1 `- g
23. uint8_t D3;
    ! N* E8 {9 _' q' d8 x
24. uint8_t D4;
    . |: E) z! a5 G/ X7 T
25. uint8_t D5;
    2 p. o2 m1 _2 O7 F( ^
26. uint8_t D6;
    6 F) N" [3 x  ]8 [
27. 
    
28. float previous_error = 0;
    
29. float integral = 0;
    ' D" E0 Z% K4 w9 F! p1 x) o
30. float derivative = 0;
    
31. int  Speed  = 160;
    
32. float output;
    * N0 E9 v, c& P
33. 
    # M9 a* f8 m( `5 ^" h$ ]
34. byte Left;
    8 c. P  ^/ r. e& q
35. 
    
36. void setup()
    ) R7 }5 N6 I5 d' @: ?
37. {
    # x" Z7 X6 L* j& E
38. //Set PWM 8KHz
      S( {, h; q) f  N) o0 Q2 c
39.   TCCR1A = _BV(WGM10);
    4 o1 e+ X/ Y" D; j/ k
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    
42.   TCCR2B = _BV(CS21);
    7 ]2 y: ^! Y& ]6 y% v
43.   Serial.begin(9600);
    / X' `- D8 X/ \. l3 \% d* H' Y- x+ y
44.   buzzer.setpin(45);
    * ^3 u& S2 f8 n" M# v
45. }
    2 Q% P8 b9 K( [  G% J* w7 `
46. 
    
47. void loop()
    
48. {
    - D" ?1 D: J- L, _
49.   while(!((lightsensor_1.read()) < (10)));
    # B, m' T  ?' d; |: h0 i- k
50.   buzzer.tone(1047, 500);
    6 [5 I/ {. m$ k2 P7 E  G
51.   delay(500);
    , d$ l9 O& {$ f, V$ w/ ?! @
52.   do
    6 ?( W6 R8 P( A4 S
53.   {
    
54.    int Err = getErr();
    
55.    if(D1 == 1)
    ) n- f8 c1 Q# D% I3 q1 _
56.    {
    , {: `1 E+ F8 n' b$ w' j$ |; C
57.      Left = 1;
    
58.    }
    
59.    if(D6 == 1)
    * O% c" b! T* p3 J# I7 p- U: n! |
60.    {
    
61.      Left = 0;
    ) b1 N1 C) b6 [+ [
62.    }
    
63.    if(Err == 99)
    
64.    {
    7 [" J+ Y3 v0 [# ]( u
65.      if(Left == 1)
    9 j7 C+ O! |; K  t
66.      {
    * x% `- w" t0 }, M5 t! I) ]5 s
67.        Speed -= 5;
    , e+ `: n8 D5 R* i$ M
68.        moto(0,Speed);
    
69.        do
    7 U+ B7 k! c$ x' g) |7 z0 N9 {7 h
70.        {
    
71.          Err = getErr();
    . O2 ?/ o9 O* ]' H# x" Y
72.        }while((D1+D6) == 0);
    + T4 G" I; O$ L2 Z' k' l
73.      }
    
74.      else
    9 F! J4 |3 X" f
75.      {
    
76.        Speed -= 5;
    
77.        moto(Speed,0);
    + \$ h& r3 `  _
78.        do
    # n4 _1 g+ ]* q7 c7 {: t
79.        {
    
80.          Err = getErr();
    ' U# S7 h5 G- U
81.        }while((D1+D6) == 0);
    # T# u2 u) s( |& d% f! e- M
82.      }
    
83.    }
    
84.    else
    
85.    {
    * l3 L  A: }3 v) D; D) }0 @: r1 Z9 g
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    ( S; B% ]  o9 F( v! W# s
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    , G6 p) z; _* D" x  i$ r
88.      integral = integral + Err;
    ! y! P: V2 y; }# p: J$ j' f# V
89.      derivative = Err - previous_error;
    / j6 ~( J! A3 w2 H
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    
91.      moto(int(Speed-output),int(Speed+output));
    
92.      previous_error = Err;
    
93.    }
    
94.   }while(!((lightsensor_2.read()) < (10)));
    
95.   moto(0,0);
    . K7 r6 `+ Z' U) V% B5 B4 ~
96.   delay(500);
    
97.   buzzer.tone(262, 500);
    
98. }
    9 ~1 f1 g9 _7 a3 n. |
99. 
    
100. int getErr()
     
101. {  
     
102.    sData = linefollower.getValue();
     
103.    D1 = ~(sData>>0)&1;
     7 y. ^& z$ }- }; W% C' n
104.    D2 = ~(sData>>1)&1;
     
105.    D3 = ~(sData>>2)&1;
     
106.    D4 = ~(sData>>3)&1;
     
107.    D5 = ~(sData>>4)&1;
     4 [% g4 [2 S/ S9 d3 I, p
108.    D6 = ~(sData>>5)&1;
     
109.    int downD = D1+D2+D3+D4+D5+D6;
     $ U/ H1 T; X7 L6 j8 x
110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
     
111.    if(downD == 0)
     
112.    {
     6 i. H. y- |. X
113.      return 99;
     
114.    }
     ! Y; i  g2 @- Z" U3 h
115.    else
     2 J0 b# _* R# o
116.    {
     
117.     return upD/downD;
     
118.    }
     3 H$ d3 n. p' v: F) q0 d
119. }
     
120. 
     5 V4 {5 ~! ~3 d5 m% h8 h
121. void moto(int Speed_L,int Speed_R)
       U; ~9 ]  R7 s7 }. l* ^: V. q
122. {
     
123.   Encoder_2.setMotorPwm(Speed_L);
     ; p5 y4 ^& n. y
124.   Encoder_1.setMotorPwm(-Speed_R);
     
125. }

複製代碼

MeLineFollowerArray.cpp
- D: x4 L) m+ E9 {- C

1. #include "MeLineFollowerArray.h"
   
2. 
   
3. #ifdef ME_PORT_DEFINED
   
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   $ \6 Q  A: _* n6 W% E) O
5. {
   
6. 
   5 Y  B+ L+ [% d. R( O7 d
7. }
   7 k1 b$ \/ p2 {$ [3 S, w" B
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   ; Y, j2 p2 R6 V, N
9. {
   
10.     _DataPin = mePort[port].s2;
    
11.     pinMode(_DataPin, OUTPUT);
    
12.     digitalWrite(_DataPin, HIGH);
    
13. }
    
14. #else // ME_PORT_DEFINED
    
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    7 K9 g, C6 u& r. k" _- J
16. {
    
17.     _DataPin = pin;
    / V! i0 L6 |# u5 [
18.     pinMode(_DataPin, OUTPUT);
    3 }0 q1 X' T4 J  q
19.     digitalWrite(_DataPin, HIGH);
    
20. }
    
21. #endif // ME_PORT_DEFINED
    5 H8 a% w( D6 c: B
22. 
    - v9 c+ Y/ _. n, I
23. 
    
24. void MeLineFollowerArray::setpin(uint8_t pin)
    8 z  z0 N: h% r( l' N
25. {
    
26.     _DataPin = pin;
    
27.     pinMode(_DataPin, OUTPUT);
    
28.     digitalWrite(_DataPin, HIGH);
    8 A$ q6 g- |& n* K, e% L0 Z
29. 
    / u' d: k$ D- |* Q9 b/ S! x
30.     #ifdef ME_PORT_DEFINED
    ( c6 q( Z% C5 G& G& f
31.     s2 = pin;
    
32.     #endif
    7 ~& E3 \: T1 L+ l( O) ~
33. }
    0 n: v) |$ }$ x2 V
34. 
    6 s/ D) F8 Q- y9 I. A% H$ S' j
35. uint8_t MeLineFollowerArray::getValue()
    * j& ^  l+ p: R0 @- H5 a
36. {
    - |$ z( r' f9 q, o; F4 D
37.     uint32_t LOW_level_read_time;
    
38.     uint32_t HIGH_level_read_time;
    % n4 }, u: D/ W* e
39.     uint32_t time_out_flag;
    6 y" Q! E; E% @9 U  j" B
40.     uint8_t Sensor_Data[3];
    
41.     static uint8_t old_data = 0xff;
    ! U9 D8 f, F7 Y( t+ v; H- a! K
42. 
    
43.     pinMode(_DataPin, OUTPUT);
    
44.     digitalWrite(_DataPin, LOW);
    2 N( E5 y- j* T  h* e& k9 F6 \5 Q+ g
45.     delayMicroseconds(980);
    7 P2 v" f7 y/ z( T9 w
46.     digitalWrite(_DataPin, HIGH);
    
47. 
    
48.     pinMode(_DataPin, INPUT_PULLUP);
    ) ~  R8 p6 o) f5 |! T
49.     delayMicroseconds(10);
    * B% D/ K6 }, c3 C+ O
50. 
    8 b6 p; g' k8 ?5 o1 P. C- C
51.     time_out_flag = millis();
    & L' l: l0 k3 t+ l, ?/ r3 v3 X
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    ( s. _: r' C2 \+ c- k
53. 
    
54.     LOW_level_read_time = micros();
    
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
56.     {
    
57.         return 0xff;
    4 o" Y) Q6 N; @! o/ d
58.     }
    
59. 
    3 Q4 B* k! @0 c& Z
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    , m+ v' ?3 ]& h8 T
61. 
      N0 l) Y; V9 P2 M' e2 ?- N  g
62.     HIGH_level_read_time = micros();
    
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    3 m0 y8 z& b3 C; x
64. 
    & V5 {! m5 M5 T% O; m( F' a  g
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
66.     {
    0 H# }0 }( A+ H
67.         return 0xff;
    . V8 L) Y( Y- j6 @$ C" I- @: K! I
68.     }
    
69. 
    9 q1 Z$ y$ b- [! r0 `' Q, e6 R
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    4 P* {6 f4 K3 r8 A
71.     {
    ( v- S+ o" ^. b0 T) w& q, _
72.         return 0xff;
    
73.     }
    7 P2 p1 N. k0 F$ Z* L
74. 
    
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    # X+ q9 F0 u" B) Z
76.     LOW_level_read_time  = micros();
    # C1 r8 n* J9 M  `0 M7 e
77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level
    
78. 
    
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
80.     {
    ) r* O* A9 e* D" Y
81.         return 0xff;
    4 v! J5 X" \" W1 \0 Z9 |; h- x
82.     }
    
83. 
    
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    8 Z5 E1 `5 w& w$ b
85.     {
    
86.         return 0xff;
    ( b7 C: F. g: H0 ]# s
87.     }
    
88. 
    % Q) V9 A* l8 x0 ]7 P4 Q( Z
89.     for(uint8_t k=0; k<3; k++)
    
90.     {
    ' E" }8 V+ ~$ b  V6 E6 K3 E) {
91.         Sensor_Data[k] = 0x00;
    
92. 
    
93.         for(uint8_t i=0;i<8;i++)
    . h5 G% u0 D9 i* c7 \4 K. v
94.         {
    / N2 h0 Q# k; p3 F, A
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    7 k& d  X1 i. m
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) )
    ; k) f+ ^/ _5 M3 S' ~& O
100.             {
     " ?% c4 L/ J7 B+ C/ [! P7 A: f
101.                 return 0xff;
     
102.             }
     
103. 
     " q1 J" X5 |: S( D' T  |6 h  U
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     ; M# z; Y2 u5 ]% W0 r# _* Z
105.             LOW_level_read_time  = micros();
     
106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
       m: e# _3 U$ u* F" z# E
107. 
     # R! `  k$ @) b
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     
109.             {
     
110.                 Sensor_Data[k] |= (0x80 >> i);
     & d( _8 o% Z) r! y+ t6 k! o
111.             }
     4 R% o) m7 Z$ g3 q& F
112.             else if(HIGH_level_read_time >= 100)
     9 f+ [. O7 o% v6 t0 ?
113.             {
     
114.                 return 0xff;
     : y0 H: M! _5 f( j) C; R
115.             }
     ; y4 G6 V5 t3 F5 Y
116. 
     
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     , A( ?0 F7 z" Z* b! P; n
118.             {
     . n' m7 R9 ^) A: P* o3 q& U4 }
119.                 return 0xff;
     7 k3 n- P' _7 t5 S2 m
120.             }
     
121.         }
     5 w3 ]: ?. q( b9 @% f
122.     }
     
123. 
     9 j9 ^- N! h% _; O
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     ; ?" u( S4 x) o: U$ B
125.     HIGH_level_read_time = micros();
     
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     9 j9 l% {5 R4 \1 J$ C. n
127. 
     
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     
129.     {
     + m  C) B% p- \$ G" r
130.         return 0xff;
     % p# H3 V3 o" f' L6 T
131.     }
     2 L/ f. I. V0 M" s4 r9 q9 G; p
132. 
     
133.     pinMode(_DataPin, OUTPUT);
       n0 u8 x9 H. l! M5 I( j, I) `
134.     digitalWrite(_DataPin, HIGH);
     
135. 
     ! S" ^: ^) o) K, d: K
136.     if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
     0 @- U/ Q# w4 F" b8 c' R& T4 ~
137.     {
     
138.         old_data = Sensor_Data[0];
     8 F  T: v8 |3 h0 m# B
139.         return Sensor_Data[0];
     
140.     }
     
141.     else
     ; o3 R4 g/ G  A) r, A& M
142.     {
     
143.         return old_data;
     
144.     }
     
145. }
     0 _) S- W* N/ g! \( Y

複製代碼

" w& J0 w' z3 K" q; P7 NMeLineFollowerArray.h   MeLineFollowerArray.h (731 Bytes, 下載次數: 9, 售價: 100 個銅錢)

2018-2-13 01:52 上傳

點擊文件名下載附件
閱讀權限: 20
" ?7 r7 d; j. t% p) ]: r! |
( d1 {0 Z, ?. G6 O; J, ~# ]

強力推廣中

哇...要100 個銅錢

daven 發表於 2019-3-6 00:09
- i/ G; R  N; u哇...要100 個銅錢

6 T9 r! a& U: r3 Q% n; e7 p7 F  V認真的回饋本站一些內容, 很容易達成的!

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

感謝分享Thanks