mBot Ranger 高速循線範例＿

1 ]8 n0 Q, g/ p, K

1. #include <Arduino.h>
   
2. #include <MeAuriga.h>
   9 \: m% X% F2 Q, U
3. #include "MeLineFollowerArray.h"
   1 O" \8 K3 p9 ~! Y& [" Y" {
4. 
   $ e. y- O% j- W
5. MeEncoderOnBoard Encoder_1(SLOT1);
   
6. MeEncoderOnBoard Encoder_2(SLOT2);
   3 q; l; T1 K# e& ?/ y  O
7. MeLightSensor lightsensor_1(12);
   " v: T1 C4 D; q5 h" G- K
8. MeLightSensor lightsensor_2(11);
   / E: h" v3 ^; O$ m9 p$ N
9. MeBuzzer buzzer;
   
10. MeLineFollowerArray linefollower(PORT_6);
    
11. 
    
12. #define Error1 1
    $ S1 z7 M1 U) K
13. #define Error2 2
    : o3 Q* A8 \  U& L
14. #define Error3 3
    7 _  F0 x% {* }1 @
15. 
    ) L& n0 M( J8 R# U
16. #define Kp 15
    
17. #define Ki 0.15
    
18. #define Kd 0.03
    
19. 
    
20. uint8_t sData;
    , y+ y' x/ m) W/ K6 Z2 ?
21. uint8_t D1;
    
22. uint8_t D2;
    * e" C) T; H# x) ]  _
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;
    
30. float derivative = 0;
    7 d) S: O3 d7 I. J3 u3 d& x
31. int  Speed  = 160;
    6 H% s% }5 x/ y5 v& ], Z6 }
32. float output;
    ; Q% G5 e" x( Z( g: b
33. 
    
34. byte Left;
    - Q$ l7 {+ t  ^) B: h0 {
35. 
    * K9 ?3 m3 X# z
36. void setup()
    
37. {
    
38. //Set PWM 8KHz
    
39.   TCCR1A = _BV(WGM10);
    
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    2 r; n. F, t# i6 x( g: l; E5 B
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    
42.   TCCR2B = _BV(CS21);
      B6 f8 ~$ I3 n: y! R5 H2 j
43.   Serial.begin(9600);
    
44.   buzzer.setpin(45);
    
45. }
    % t% |3 W3 _; p6 g8 G
46. 
    
47. void loop()
    7 x& g* t5 O$ N/ a4 p5 D
48. {
    
49.   while(!((lightsensor_1.read()) < (10)));
    - {3 O0 }6 E' ~
50.   buzzer.tone(1047, 500);
    
51.   delay(500);
    
52.   do
    
53.   {
    . J& |7 T" j0 ]
54.    int Err = getErr();
    
55.    if(D1 == 1)
    2 `4 P% @6 ]1 f5 d. L
56.    {
    
57.      Left = 1;
    
58.    }
    
59.    if(D6 == 1)
    
60.    {
    7 X, @! |3 e( h% M
61.      Left = 0;
    
62.    }
    * U0 d+ y# B) h4 c, B2 a* V& n' d
63.    if(Err == 99)
    / U* }5 b( q' H+ p* m4 d
64.    {
    " g+ ?, f5 b, Q, z
65.      if(Left == 1)
    & p( n* u% J8 C
66.      {
    
67.        Speed -= 5;
    
68.        moto(0,Speed);
    
69.        do
    1 x. O6 B9 ^1 M( G9 X  g
70.        {
    
71.          Err = getErr();
    6 ~, z# O# k% t* v: [$ ^* Q5 j/ j
72.        }while((D1+D6) == 0);
    : x% K: B! k1 \- o, D# W' G
73.      }
    7 }% q$ L  I( M! x* |2 o: A9 P2 B
74.      else
    , o7 F( _, Z5 ~9 `( t' O
75.      {
    
76.        Speed -= 5;
    . ?& f% u7 k3 M$ p' s' g: t
77.        moto(Speed,0);
    
78.        do
    ; w- x; H# I3 c2 N% C& E7 G
79.        {
    0 M9 e6 k! ~4 x% L; M  h/ r
80.          Err = getErr();
    
81.        }while((D1+D6) == 0);
    
82.      }
    ( ^; W, [# S3 v0 C, X: R( u! m
83.    }
    . l5 ^5 |8 K" r0 @$ o9 K
84.    else
    
85.    {
    
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    4 K! ^# i8 k5 v% @1 }4 x/ ]% Y+ g  G
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
      `5 M, h* ]' F9 N) |
88.      integral = integral + Err;
    , z: e) V4 g5 q5 q
89.      derivative = Err - previous_error;
    
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);
    
96.   delay(500);
    - _8 K& }  r. Q
97.   buzzer.tone(262, 500);
    # ]5 p5 @5 c3 p# L4 G6 t
98. }
    
99. 
    % ]+ A" V+ J- F, y
100. int getErr()
     2 l1 N. s3 @& _* S8 D
101. {  
     
102.    sData = linefollower.getValue();
     
103.    D1 = ~(sData>>0)&1;
     
104.    D2 = ~(sData>>1)&1;
     
105.    D3 = ~(sData>>2)&1;
     
106.    D4 = ~(sData>>3)&1;
     - _, o( t8 Q. N) d2 q" L
107.    D5 = ~(sData>>4)&1;
     
108.    D6 = ~(sData>>5)&1;
     1 ]: G; X: L6 O. r( R
109.    int downD = D1+D2+D3+D4+D5+D6;
     6 u3 Y4 Y0 ?5 N8 u( N2 b
110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
     . u& K; Z$ v" C8 F1 ?
111.    if(downD == 0)
     : q/ q, y! B" k0 a6 k! G% w
112.    {
     
113.      return 99;
     
114.    }
     8 h8 ?3 E6 }5 M. g
115.    else
     8 N$ ^: h8 _0 K" r
116.    {
     
117.     return upD/downD;
     - B6 C  |: I, i# C( c$ p
118.    }
     6 @! v( z+ G) m1 M
119. }
     
120. 
     * v6 I2 t! L6 [7 ]% G
121. void moto(int Speed_L,int Speed_R)
     
122. {
     
123.   Encoder_2.setMotorPwm(Speed_L);
     
124.   Encoder_1.setMotorPwm(-Speed_R);
     
125. }

複製代碼

1 B0 O) R% R" t3 @MeLineFollowerArray.cpp
8 o. Q1 [" l- c$ d8 A

1. #include "MeLineFollowerArray.h"
   
2. 
   4 j8 V! m! J7 j$ W
3. #ifdef ME_PORT_DEFINED
   
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   ( ^, H" ?/ x2 R
5. {
   
6. 
   
7. }
   
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   . X3 X9 {% W/ T- B; X9 I
9. {
   
10.     _DataPin = mePort[port].s2;
    
11.     pinMode(_DataPin, OUTPUT);
    
12.     digitalWrite(_DataPin, HIGH);
    2 K& n& p  G+ S6 D
13. }
    
14. #else // ME_PORT_DEFINED
    
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    
16. {
    . E& X# v; ^+ y+ Z& [6 T; J
17.     _DataPin = pin;
    6 D  N+ l" d; x" U( [$ K5 V0 y" D
18.     pinMode(_DataPin, OUTPUT);
    1 y# g; r' h# `8 c* _7 p
19.     digitalWrite(_DataPin, HIGH);
    3 }" u9 Q# ]2 Y4 w) t
20. }
    8 E: x0 g; J6 `
21. #endif // ME_PORT_DEFINED
    / k2 R2 k  h5 Z; r
22. 
    3 h" o" r4 N6 X* ^4 c3 ]) T; \, |2 D& k
23. 
    ! R2 X6 q6 Z  \5 f, X
24. void MeLineFollowerArray::setpin(uint8_t pin)
    
25. {
    & X& F2 X2 m; @
26.     _DataPin = pin;
    
27.     pinMode(_DataPin, OUTPUT);
    6 W' c# _/ q! [3 M( T1 Q0 l
28.     digitalWrite(_DataPin, HIGH);
    * S8 L! X9 e# h* r, V
29. 
    
30.     #ifdef ME_PORT_DEFINED
    
31.     s2 = pin;
    ' Q  M, R6 n: g0 S+ q% P/ Z0 b
32.     #endif
    
33. }
    
34. 
    - ?* J1 n  d- z$ x; o; K# ~
35. uint8_t MeLineFollowerArray::getValue()
    3 h, K6 ]+ N% R# o$ G
36. {
    
37.     uint32_t LOW_level_read_time;
    
38.     uint32_t HIGH_level_read_time;
    + c# F& D* \% H+ h& e
39.     uint32_t time_out_flag;
    
40.     uint8_t Sensor_Data[3];
    
41.     static uint8_t old_data = 0xff;
    : ]3 `/ x- E2 m% M/ ]  t3 q+ @
42. 
    ! A. t) h1 N& v, m1 ?4 v
43.     pinMode(_DataPin, OUTPUT);
    ' l$ @$ E, J& W1 f2 b9 x
44.     digitalWrite(_DataPin, LOW);
    
45.     delayMicroseconds(980);
    2 M5 l6 l( e/ O
46.     digitalWrite(_DataPin, HIGH);
    " @" J$ J7 r. r* S' K- D* h, w
47. 
    
48.     pinMode(_DataPin, INPUT_PULLUP);
    0 W: j% B# |' N. S3 e3 ?2 }" E$ D
49.     delayMicroseconds(10);
    - ^* i/ g2 j# v7 S7 Q. b
50. 
    
51.     time_out_flag = millis();
    / V' w& z) Q6 A4 Z, e
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
53. 
    
54.     LOW_level_read_time = micros();
    
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
56.     {
    , o8 W( W4 p, B- f! D5 q
57.         return 0xff;
    / P9 J) S  t6 R4 w. q% O
58.     }
    
59. 
    + n4 [8 A3 G$ K
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
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
    
66.     {
    
67.         return 0xff;
    6 S: o9 l( P" \9 X$ R
68.     }
    
69. 
    
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    ! @4 n  F0 w! x2 o$ d, p
71.     {
    8 J% k" Z' c* j1 X! @2 D) \/ Y
72.         return 0xff;
    . q. D8 P# k9 L0 o' j$ V, V4 J' X
73.     }
    
74. 
    # A  s( f% a* `$ I) z/ t& x) @
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
76.     LOW_level_read_time  = micros();
    0 L" D6 c% }. [4 R
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
    3 _' R! F/ \' [
80.     {
    
81.         return 0xff;
      H0 U9 n5 w% m3 V
82.     }
    * |- Z0 o. x2 r& M  k8 w1 k0 T
83. 
    + D! T/ I+ b9 N# b& {
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    9 v$ I1 E8 D  K  z8 J! F
85.     {
    
86.         return 0xff;
    
87.     }
      n+ g; O% y3 I% G6 E+ q1 c7 @
88. 
    ; _# c  K9 B& h
89.     for(uint8_t k=0; k<3; k++)
    
90.     {
    - w$ J2 S: g7 o! v( q# Z
91.         Sensor_Data[k] = 0x00;
    % j! I# @; B1 g
92. 
    
93.         for(uint8_t i=0;i<8;i++)
    
94.         {
    , E2 h" y  {4 c9 z  S: s& [; S
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;
    , A* @$ d  {0 \+ @3 T
98. 
    6 g5 |+ j6 `" I: T) ?
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    
100.             {
     
101.                 return 0xff;
     
102.             }
     0 D. I* k0 z$ I; l  a
103. 
     , O& i' s9 T4 x. R
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     , y' v' m' S. k: Q
105.             LOW_level_read_time  = micros();
     
106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
     7 X+ M4 ?) W2 B. Q. M
107. 
     9 c0 A  @! }& Z; p1 i
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     
109.             {
     
110.                 Sensor_Data[k] |= (0x80 >> i);
     ) v9 o% {4 S/ x( f/ P7 h0 c
111.             }
     - ]+ @" W" {7 @  ^  ~# u
112.             else if(HIGH_level_read_time >= 100)
     ) ?' _* d, r: O! a3 \. h
113.             {
     
114.                 return 0xff;
     3 r6 F9 `; r$ |! [
115.             }
     , r/ e; ?" s( K1 k
116. 
     . P) Q( R! t2 _; y2 W
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     
118.             {
     8 s' v) N8 w+ C( y
119.                 return 0xff;
     4 y6 a8 o/ O, x2 ^# U  A- f9 F, y
120.             }
     
121.         }
     
122.     }
     
123. 
     " ?& B- B  T& M* _3 J8 U+ {" S
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     " G0 G7 {/ L7 W7 `
125.     HIGH_level_read_time = micros();
     $ S% R" a. t6 g, ]$ h
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     7 _$ E, W+ N: a, O  \  l# ~1 ]
127. 
     
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     . z9 r( p, s1 B/ t+ c- x
129.     {
     ) @& |9 p' E+ u+ C$ D; ~
130.         return 0xff;
     , U2 b. q4 V9 V6 l* B" G
131.     }
     & f4 z9 ~3 U' ?6 S
132. 
     & _) D# `/ e. q* Q( o4 T
133.     pinMode(_DataPin, OUTPUT);
     
134.     digitalWrite(_DataPin, HIGH);
     
135. 
     / O' S' H1 \. {. ]6 h
136.     if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
     - N  j" K% u6 b, E7 u
137.     {
     % E0 H6 `0 B1 R, [' y
138.         old_data = Sensor_Data[0];
     : J7 |/ k( J8 I! i2 s1 X
139.         return Sensor_Data[0];
     
140.     }
     7 K( A, h! f, d* R' c2 A* ~8 o9 }  {9 h
141.     else
     
142.     {
     , Y, M9 k) m; u1 V
143.         return old_data;
     2 Q6 V: J, k$ W1 E! T2 y
144.     }
     0 X/ o" S" A$ Q) o* X) ^9 X5 v; s8 t
145. }
     : l# ?  u$ a8 w/ j

複製代碼

6 o  I/ z; a% F" Y9 K8 qMeLineFollowerArray.h   MeLineFollowerArray.h (731 Bytes, 下載次數: 10, 售價: 100 個銅錢)

2018-2-13 01:52 上傳

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

. b+ q, l( v8 q2 y
/ |2 C' h7 n" r9 W. d

強力推廣中

哇...要100 個銅錢

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

; ]* H5 m  H9 b# M認真的回饋本站一些內容, 很容易達成的!

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

感謝分享Thanks

能否用到mblock 5 上面呢？

roboger 發表於 2020-11-13 15:43
, @( k( ~/ q! C能否用到mblock 5 上面呢？

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

无法添加到扩展呀

高手就是高手