SHOW:
|
|
- or go back to the newest paste.
1 | - | #include <popx2.h> |
1 | + | #include <popx2.h> |
2 | - | |
2 | + | |
3 | - | #define STRAING_DELAY 10 |
3 | + | #define STRAING_DELAY 10 |
4 | - | #define BACKWARD_DELAY 300 |
4 | + | #define BACKWARD_DELAY 300 |
5 | - | #define WALL_DELAY 100 |
5 | + | #define WALL_DELAY 100 |
6 | - | #define SPEED 30 |
6 | + | #define SPEED 30 |
7 | - | |
7 | + | |
8 | - | int TURN_DELAY=350; |
8 | + | int TURN_DELAY=350; |
9 | - | float ANGLE = 0; |
9 | + | float ANGLE = 0; |
10 | - | |
10 | + | |
11 | - | void FF(){ |
11 | + | void FF(){ |
12 | - | /////////// Wall detect ///////////// |
12 | + | /////////// Wall detect ///////////// |
13 | - | while(in(26)){ |
13 | + | while(in(26)){ |
14 | - | if(!in(24)){ |
14 | + | if(!in(24)){ |
15 | - | tr(SPEED); |
15 | + | tr(SPEED); |
16 | - | delay(WALL_DELAY); |
16 | + | delay(WALL_DELAY); |
17 | - | } |
17 | + | } |
18 | - | else if(!in(25)){ |
18 | + | else if(!in(25)){ |
19 | - | tl(SPEED); |
19 | + | tl(SPEED); |
20 | - | delay(WALL_DELAY); |
20 | + | delay(WALL_DELAY); |
21 | - | } |
21 | + | } |
22 | - | else{ |
22 | + | else{ |
23 | - | fd2(SPEED, SPEED); |
23 | + | fd2(SPEED, SPEED); |
24 | - | } |
24 | + | } |
25 | - | } |
25 | + | } |
26 | - | //////////// Straing alignment /////////////// |
26 | + | //////////// Straing alignment /////////////// |
27 | - | while(in(24) || in(25)){ |
27 | + | while(in(24) || in(25)){ |
28 | - | if(!in(24)){ |
28 | + | if(!in(24)){ |
29 | - | tl(SPEED); |
29 | + | tl(SPEED); |
30 | - | } |
30 | + | } |
31 | - | else if(!in(25)){ |
31 | + | else if(!in(25)){ |
32 | - | tr(SPEED); |
32 | + | tr(SPEED); |
33 | - | } |
33 | + | } |
34 | - | } |
34 | + | } |
35 | - | delay(STRAING_DELAY); |
35 | + | delay(STRAING_DELAY); |
36 | - | ao(); |
36 | + | ao(); |
37 | - | delay(500); |
37 | + | delay(500); |
38 | - | ///////////////////////////////////////////// |
38 | + | ///////////////////////////////////////////// |
39 | - | |
39 | + | |
40 | - | bk(SPEED); |
40 | + | bk(SPEED); |
41 | - | delay(BACKWARD_DELAY); |
41 | + | delay(BACKWARD_DELAY); |
42 | - | ao(); |
42 | + | ao(); |
43 | } | |
44 | - | |
44 | + | |
45 | - | void FR(){ |
45 | + | void FR(){ |
46 | - | delay(500); |
46 | + | delay(500); |
47 | - | sr(SPEED*0.8); |
47 | + | sr(SPEED*0.8); |
48 | - | delay(TURN_DELAY); |
48 | + | delay(TURN_DELAY); |
49 | - | ao(); |
49 | + | ao(); |
50 | - | delay(500); |
50 | + | delay(500); |
51 | } | |
52 | - | |
52 | + | |
53 | - | void FL(){ |
53 | + | void FL(){ |
54 | - | delay(500); |
54 | + | delay(500); |
55 | - | sl(SPEED*0.8); |
55 | + | sl(SPEED*0.8); |
56 | - | delay(TURN_DELAY); |
56 | + | delay(TURN_DELAY); |
57 | - | ao(); |
57 | + | ao(); |
58 | - | delay(500); |
58 | + | delay(500); |
59 | } | |
60 | - | |
60 | + | |
61 | - | void KICK(){ |
61 | + | void KICK(){ |
62 | - | bk(SPEED); |
62 | + | bk(SPEED); |
63 | - | delay(BACKWARD_DELAY); |
63 | + | delay(BACKWARD_DELAY); |
64 | - | ao(); |
64 | + | ao(); |
65 | - | FL(); |
65 | + | FL(); |
66 | - | FL(); |
66 | + | FL(); |
67 | - | ao(); delay(500); |
67 | + | ao(); delay(500); |
68 | - | bk(SPEED); delay(BACKWARD_DELAY); |
68 | + | bk(SPEED); delay(BACKWARD_DELAY); |
69 | - | ao(); delay(500); |
69 | + | ao(); delay(500); |
70 | - | delay(500);servo(1,100); |
70 | + | delay(500);servo(1,100); |
71 | - | delay(500);servo(1,115); |
71 | + | delay(500);servo(1,115); |
72 | - | delay(500);servo(1, 90); |
72 | + | delay(500);servo(1, 90); |
73 | - | delay(1000); |
73 | + | delay(1000); |
74 | } | |
75 | - | |
75 | + | |
76 | - | void setup() { |
76 | + | void setup() { |
77 | - | servo(1, 90); |
77 | + | servo(1, 90); |
78 | - | setTextSize(2); |
78 | + | setTextSize(2); |
79 | - | /////////////////////////////////// |
79 | + | /////////////////////////////////// |
80 | - | |
80 | + | |
81 | - | /* Press OK */ |
81 | + | /* Press OK */ |
82 | - | glcd(0,0,"Press OK"); |
82 | + | glcd(0,0,"Press OK"); |
83 | - | while(!sw_OK()){ |
83 | + | while(!sw_OK()){ |
84 | - | TURN_DELAY = map(knob(),0,1024,300,800); |
84 | + | TURN_DELAY = map(knob(),0,1024,300,800); |
85 | - | glcd(1,0,"T=%d",TURN_DELAY); |
85 | + | glcd(1,0,"T=%d",TURN_DELAY); |
86 | - | delay(100); |
86 | + | delay(100); |
87 | - | } |
87 | + | } |
88 | - | glcd(0,0,"Running "); |
88 | + | glcd(0,0,"Running "); |
89 | - | /////////////////////////////////// |
89 | + | /////////////////////////////////// |
90 | - | delay(500); |
90 | + | delay(500); |
91 | - | FF();FL(); |
91 | + | FF();FL(); |
92 | - | |
92 | + | |
93 | - | glcd(0,0,"Finished"); |
93 | + | glcd(0,0,"Finished"); |
94 | } | |
95 | - | |
95 | + | |
96 | - | void loop() { |
96 | + | void loop() { |
97 | - | //FF();FR(); |
97 | + | //FF();FR(); |
98 | } |