mBot Ranger 高速循線範例＿

9 M( ]  u/ `1 E9 m* u1 k

1. #include <Arduino.h>
   
2. #include <MeAuriga.h>
     S5 z$ e7 ^0 ^0 K2 |1 f
3. #include "MeLineFollowerArray.h"
   
4. 
   + `; Q: v/ W% y& l9 @6 t
5. MeEncoderOnBoard Encoder_1(SLOT1);
   
6. MeEncoderOnBoard Encoder_2(SLOT2);
   ' C% w0 q# ]) t4 w2 i
7. MeLightSensor lightsensor_1(12);
   5 y# O8 \$ ^0 m+ m
8. MeLightSensor lightsensor_2(11);
   
9. MeBuzzer buzzer;
   ) ^+ N& e' t; s! N- x  ?
10. MeLineFollowerArray linefollower(PORT_6);
    . G' `+ \: k: u; [/ S
11. 
    7 G/ O4 n* p; r: F# M1 s
12. #define Error1 1
    $ f/ a$ h  _$ _" y, N& v! m' r1 y
13. #define Error2 2
    - [5 g, C0 ]/ B  j1 H0 p
14. #define Error3 3
      N2 n/ t9 p8 F3 h: Q1 B
15. 
    ; t) c; p6 C" ?* d( U# S3 M
16. #define Kp 15
    ) P& t  b, X! M$ S
17. #define Ki 0.15
    & F6 N% W0 C% E9 o4 B. ?- M3 t
18. #define Kd 0.03
    
19. 
    
20. uint8_t sData;
    
21. uint8_t D1;
    
22. uint8_t D2;
    
23. uint8_t D3;
    + O3 |2 ]) R* O' I
24. uint8_t D4;
    : }+ O: t4 _6 F: _1 n! d6 e
25. uint8_t D5;
    
26. uint8_t D6;
    
27. 
    ' o4 y5 [2 A8 C5 m# O
28. float previous_error = 0;
    
29. float integral = 0;
    1 f  e/ D8 S, ?3 J) G' d
30. float derivative = 0;
    3 z: s+ Y) b& y% M9 K! _. C
31. int  Speed  = 160;
    
32. float output;
    
33. 
    6 d9 y' L6 B7 y0 ~7 Q
34. byte Left;
    
35. 
    
36. void setup()
    " G, c/ {  V5 f* r" H, I! ~
37. {
    
38. //Set PWM 8KHz
    
39.   TCCR1A = _BV(WGM10);
    - D! _# p2 x0 U+ n' A2 o  w' e
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    
42.   TCCR2B = _BV(CS21);
    
43.   Serial.begin(9600);
    
44.   buzzer.setpin(45);
    2 i' c3 s0 b) v- f; F
45. }
    
46. 
    
47. void loop()
    3 R; d; A. k) L" G5 ], A% v2 z
48. {
    
49.   while(!((lightsensor_1.read()) < (10)));
    
50.   buzzer.tone(1047, 500);
    
51.   delay(500);
    
52.   do
    
53.   {
    9 o6 I2 G. d. i$ H
54.    int Err = getErr();
    
55.    if(D1 == 1)
    
56.    {
    3 X9 E. y$ I8 N0 P* P/ `% Y
57.      Left = 1;
    : b" [8 K. P8 t6 O# ]  S9 Y
58.    }
    2 ~4 p* y3 f  h5 ]4 Y
59.    if(D6 == 1)
    4 o) @. l  A3 I: }. k
60.    {
    2 s: r* r8 [2 n" q! ~
61.      Left = 0;
    & p6 s# h! x# o. f$ U$ m
62.    }
    % O0 J# ^. P0 _4 a# K
63.    if(Err == 99)
    0 E1 E, n1 C# @7 n
64.    {
    
65.      if(Left == 1)
    $ r1 o5 [' k: {
66.      {
    
67.        Speed -= 5;
    3 \0 v. G9 r% x1 i( W
68.        moto(0,Speed);
    
69.        do
    . _6 V1 n& |7 S1 @. [8 a
70.        {
    
71.          Err = getErr();
    
72.        }while((D1+D6) == 0);
    
73.      }
    ' Q, n8 l2 j- ?' b; @9 ^
74.      else
    
75.      {
    
76.        Speed -= 5;
    
77.        moto(Speed,0);
    
78.        do
    % B9 _2 A! L/ Y( y3 i: t: C& f
79.        {
    , z( ~) n  c- l- a  X
80.          Err = getErr();
    
81.        }while((D1+D6) == 0);
    2 W" X- ^, B$ `
82.      }
    + H' R. e2 w( ?
83.    }
    : b/ h. k6 V/ M! W, L* r
84.    else
    # T2 I# y# Y8 T' O7 @
85.    {
    3 O1 s  i& F% E+ n' X3 ~% ^; r
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    ' U- r5 p2 w" A7 Y! l- L% G
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    " l- _0 K3 I* T
88.      integral = integral + Err;
    
89.      derivative = Err - previous_error;
    % [. `' A8 ]9 c% p* U  J* _
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    " H  d7 X! i. p
91.      moto(int(Speed-output),int(Speed+output));
    3 Z5 |5 `: b$ y0 J8 ^0 ?' S% ]
92.      previous_error = Err;
    
93.    }
    
94.   }while(!((lightsensor_2.read()) < (10)));
    # Y4 L( H1 i  L. ~% V1 a1 ?3 b
95.   moto(0,0);
    0 x7 C1 i: R4 E9 K. a
96.   delay(500);
    
97.   buzzer.tone(262, 500);
    
98. }
    0 Q$ N: P4 |2 c2 E
99. 
    
100. int getErr()
     
101. {  
     + V6 X; U" w) D* _+ L5 w
102.    sData = linefollower.getValue();
     4 q4 n4 a3 c! A! r  ^% G% z% i: Q
103.    D1 = ~(sData>>0)&1;
     
104.    D2 = ~(sData>>1)&1;
     
105.    D3 = ~(sData>>2)&1;
     
106.    D4 = ~(sData>>3)&1;
     
107.    D5 = ~(sData>>4)&1;
     
108.    D6 = ~(sData>>5)&1;
     
109.    int downD = D1+D2+D3+D4+D5+D6;
     # y4 \/ e. H6 H0 L5 L! i
110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
     
111.    if(downD == 0)
     ! M0 ?2 X1 k; b  R& x9 d" k* ~
112.    {
     1 X& L! m9 y+ j4 Y; G% e, e
113.      return 99;
     4 c7 O  E3 S$ [- I0 g
114.    }
     7 u6 T2 k: N7 w
115.    else
     
116.    {
     
117.     return upD/downD;
     , U" c% w( h* z  ]0 V
118.    }
     & y: B! L7 n  ~9 A0 J
119. }
     
120. 
     
121. void moto(int Speed_L,int Speed_R)
     . {0 o- ]; f+ m  ~% D- b- ^
122. {
     
123.   Encoder_2.setMotorPwm(Speed_L);
     
124.   Encoder_1.setMotorPwm(-Speed_R);
     - \# z* ~/ ?' Z1 i& h: Z  q4 d: ?
125. }

複製代碼

. R: Q) `  p' x, wMeLineFollowerArray.cpp

1. #include "MeLineFollowerArray.h"
   9 l8 i: j( C, S. i
2. 
   
3. #ifdef ME_PORT_DEFINED
   
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   
5. {
   
6. 
   
7. }
   
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   1 e6 r! I( Y" I  N9 b. H) Q
9. {
   
10.     _DataPin = mePort[port].s2;
    - Z' B1 O: ~* S% ^6 x9 Y" E
11.     pinMode(_DataPin, OUTPUT);
    0 ^9 \/ U% z1 w7 f+ P# y! A7 e
12.     digitalWrite(_DataPin, HIGH);
    
13. }
    2 _3 x, F8 Q, n6 ^) X
14. #else // ME_PORT_DEFINED
    ! d+ ^( Z. |  f( `' m: m; u& m
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    
16. {
    6 }* p' n. U& M# Q5 q: P1 u3 C1 C
17.     _DataPin = pin;
    6 Q! ~6 b1 K' z  g
18.     pinMode(_DataPin, OUTPUT);
    $ w1 G; h6 W( j$ g7 |! l
19.     digitalWrite(_DataPin, HIGH);
    
20. }
    : E' B$ S% \. C. J
21. #endif // ME_PORT_DEFINED
    ; J9 v' j+ s% I2 o. m; h! Y
22. 
    ) U) @. C" Q6 B; i- G% x$ L/ ^
23. 
    1 @8 N' H4 P4 ~; Y% n7 b
24. void MeLineFollowerArray::setpin(uint8_t pin)
    ! Q# O  t7 W, y) W, X3 k
25. {
    
26.     _DataPin = pin;
    
27.     pinMode(_DataPin, OUTPUT);
    , l* c" b2 E4 q! r, ~0 g" i
28.     digitalWrite(_DataPin, HIGH);
    
29. 
    & J8 Y; l2 Y4 C% K( X; N( g4 j7 _
30.     #ifdef ME_PORT_DEFINED
      O, n- p2 \# m8 h  c/ B
31.     s2 = pin;
    
32.     #endif
    
33. }
    
34. 
    / |+ E1 L" U; P4 a+ i( B
35. uint8_t MeLineFollowerArray::getValue()
    
36. {
    
37.     uint32_t LOW_level_read_time;
    . B9 C) M/ U! V/ `
38.     uint32_t HIGH_level_read_time;
      z5 {7 C/ [3 h
39.     uint32_t time_out_flag;
    
40.     uint8_t Sensor_Data[3];
    / o) @$ D( U! L- ]  K
41.     static uint8_t old_data = 0xff;
    
42. 
    ( g) ~/ k# I, e0 m' S
43.     pinMode(_DataPin, OUTPUT);
    
44.     digitalWrite(_DataPin, LOW);
    
45.     delayMicroseconds(980);
    
46.     digitalWrite(_DataPin, HIGH);
    9 }0 ]0 F( v* r! o' b
47. 
    9 k  Y! {7 X" Z+ _% G
48.     pinMode(_DataPin, INPUT_PULLUP);
    
49.     delayMicroseconds(10);
    
50. 
    9 W0 C6 A( f3 p# a
51.     time_out_flag = millis();
    0 O# S$ r# g. @& `
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
53. 
    
54.     LOW_level_read_time = micros();
    3 T2 g! M' \$ d& H6 u/ `% Y$ {
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
56.     {
    ) g/ ~! L4 A2 E, ~. r7 ^- U% h# E
57.         return 0xff;
    9 E" ]' s2 b& c( Z' o- J
58.     }
      J0 p$ J  y3 g+ E2 }& l% T
59. 
    # B1 N. F% y$ T9 Y7 _) T, k) D. g" g
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
61. 
    
62.     HIGH_level_read_time = micros();
    9 f4 Q4 z' f( u2 i' a$ L5 K
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    
64. 
      ]5 x( R7 M( ~
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
66.     {
    + h9 R+ a# O2 Q! M
67.         return 0xff;
    
68.     }
    5 r. s6 d* U. t
69. 
    
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    7 h1 j/ I# t& N( i. E
71.     {
    1 b! o$ j" ]( R' V% |, }
72.         return 0xff;
    * V( x( P8 x1 z+ K
73.     }
    0 S, w' `' h/ i0 ~
74. 
    
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    / I$ r( g, A5 v9 H9 C
76.     LOW_level_read_time  = micros();
    - C, Y$ B9 N* Q4 G" D6 Q' r
77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level
    
78. 
    ! o8 [2 j* U9 Y! l: x3 M, ?+ U
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    # C  V# ^& f* W# z3 \6 P
80.     {
    
81.         return 0xff;
    # L# m$ {% \' \" b1 g
82.     }
    
83. 
    : r3 v, W! t/ {+ O- U: s
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    
85.     {
    8 H3 c5 s( j5 E5 A$ J/ M: t% e
86.         return 0xff;
    0 R8 @0 M. c0 q) N  ?! Q2 H
87.     }
    
88. 
    ! j# \& V' Y2 y9 G/ @( }5 l
89.     for(uint8_t k=0; k<3; k++)
    + X$ f! N: c' G" F! i, p8 j
90.     {
    ' n# M: S' c* Y/ g% L
91.         Sensor_Data[k] = 0x00;
    * t! x4 b5 d8 h* W* U  c
92. 
    
93.         for(uint8_t i=0;i<8;i++)
    ( {  L3 P/ N2 f; |
94.         {
    
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    # a" W7 I7 @2 X1 d- ?: @0 ?
96.             HIGH_level_read_time = micros();
    5 [# S6 K& X3 ~. W1 ^9 n
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    
98. 
    
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    
100.             {
     
101.                 return 0xff;
     6 H9 B' n' M! h1 ^8 H) s! a1 w
102.             }
     8 C4 F% I9 \8 ~8 q+ H) J- m) K& C* f
103. 
     2 c* a2 Q* V; C
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     
105.             LOW_level_read_time  = micros();
     % B* C$ |$ M' P" n- \
106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
     
107. 
     
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     
109.             {
     8 f: m1 i6 X  J" [. \
110.                 Sensor_Data[k] |= (0x80 >> i);
     
111.             }
     ; ^. c2 k6 j' s
112.             else if(HIGH_level_read_time >= 100)
     
113.             {
     
114.                 return 0xff;
     
115.             }
     0 O$ T4 j0 m  ]; W, b, g8 v
116. 
     
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     
118.             {
     & j! x4 x0 N7 Q# p0 R8 k
119.                 return 0xff;
     
120.             }
     4 d, R/ x# n& V7 X  H
121.         }
     ) Q1 \6 p2 c3 S2 G6 l
122.     }
     
123. 
     
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     2 z6 s3 i, q" ]( Q* \* k1 z
125.     HIGH_level_read_time = micros();
     
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     8 E% o1 p5 k: B
127. 
     6 _0 M5 g$ B' P. g; W* a
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     
129.     {
     
130.         return 0xff;
     
131.     }
     
132. 
     $ l' q1 n1 X1 d( z; }% d: y/ X
133.     pinMode(_DataPin, OUTPUT);
     + c- a0 }$ ]7 ^! Z7 F
134.     digitalWrite(_DataPin, HIGH);
     : @. L# U# H! ^* N! `4 b, y: C
135. 
     : r% X0 f* |9 q9 H6 p' \
136.     if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
     ' G/ k. x) f) e% ]2 ?( s# V1 A2 V
137.     {
     
138.         old_data = Sensor_Data[0];
     % a" A( K7 h" `0 M* \$ W
139.         return Sensor_Data[0];
     
140.     }
     
141.     else
     
142.     {
     % I- Y) w7 t0 I1 M3 k! f' E
143.         return old_data;
     7 x7 {: ^9 I! h' q: v, C! j
144.     }
     . R  e# P+ I: w" W
145. }
     * R- |/ n& Z. y% [9 k4 j

複製代碼

5 B: e/ s/ W* a! {  T. FMeLineFollowerArray.h   MeLineFollowerArray.h (731 Bytes, 下載次數: 10, 售價: 100 個銅錢)

2018-2-13 01:52 上傳

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

強力推廣中

哇...要100 個銅錢

daven 發表於 2019-3-6 00:09
5 x5 N1 p2 i- g" u/ h哇...要100 個銅錢

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

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

感謝分享Thanks

能否用到mblock 5 上面呢？

roboger 發表於 2020-11-13 15:43
, H9 J( w" O) B9 G" F) E3 e能否用到mblock 5 上面呢？

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

无法添加到扩展呀

高手就是高手