root / trunk / code / behaviors / formation_control / circle2 / circle.c @ 1517
History | View | Annotate | Download (2.99 KB)
1 | 1517 | azirbel | #include <dragonfly_lib.h> |
---|---|---|---|
2 | #include <encoders.h> |
||
3 | |||
4 | // TODO:
|
||
5 | // Check BOM more often and before final stop
|
||
6 | // Make BOM check more comprehensively
|
||
7 | // Use the center bot to check distances
|
||
8 | // Count them ("spam" method)
|
||
9 | // Use beacon to find relative positions
|
||
10 | // Beacon tells them how to move to be at the right distance
|
||
11 | // Wireless communication, initialization
|
||
12 | |||
13 | void forward(int speed){ |
||
14 | motor_l_set(FORWARD,speed); |
||
15 | motor_r_set(FORWARD,speed); |
||
16 | } |
||
17 | void left(int speed){ |
||
18 | motor_l_set(FORWARD,speed); |
||
19 | motor_r_set(BACKWARD,speed); |
||
20 | } |
||
21 | void right(int speed){ |
||
22 | motor_l_set(BACKWARD,speed); |
||
23 | motor_r_set(FORWARD,speed); |
||
24 | } |
||
25 | void stop(void){ |
||
26 | motor_l_set(BACKWARD,0);
|
||
27 | motor_r_set(FORWARD,0);
|
||
28 | } |
||
29 | void setforward(int spd1, int spd2){ |
||
30 | motor_l_set(FORWARD,spd1); |
||
31 | motor_r_set(FORWARD,spd2); |
||
32 | } |
||
33 | void backward(int speed){ |
||
34 | motor_l_set(BACKWARD, speed); |
||
35 | motor_r_set(BACKWARD, speed); |
||
36 | } |
||
37 | int get_distance(void){ |
||
38 | int temp,distance,kk=5; |
||
39 | distance =0;
|
||
40 | for (int i=0; i<kk; i++){ |
||
41 | temp = range_read_distance(IR2); |
||
42 | if (temp == -1) {temp=0; kk--;} |
||
43 | distance+= temp; |
||
44 | delay_ms(3);
|
||
45 | } |
||
46 | if (kk>0) |
||
47 | return (int)(distance/kk); |
||
48 | else
|
||
49 | return 0; |
||
50 | } |
||
51 | void turn_to_beacon(int max){ |
||
52 | if (max>-1 && max<16){ |
||
53 | int index = (max+12)%16; |
||
54 | if (index==0) { |
||
55 | stop(); |
||
56 | } |
||
57 | else if (index<8) right(170); |
||
58 | else left(170); |
||
59 | } |
||
60 | } |
||
61 | |||
62 | void turn_to_beacon2(int max){ |
||
63 | if (max>-1 && max<16){ |
||
64 | int index = (max+12)%16; |
||
65 | if (index==0) { |
||
66 | |||
67 | } |
||
68 | else if (index<8) right(170); |
||
69 | else left(170); |
||
70 | } |
||
71 | } |
||
72 | void orient(void){ |
||
73 | int max_index = -1; |
||
74 | while (max_index!=4) { |
||
75 | /* Refresh and make sure the table is updated */
|
||
76 | bom_refresh(BOM_ALL); |
||
77 | max_index = bom_get_max(); |
||
78 | turn_to_beacon(max_index); |
||
79 | delay_ms(22);
|
||
80 | } |
||
81 | } |
||
82 | void orient2(void){ |
||
83 | int max_index = -1; |
||
84 | while (max_index!=4) { |
||
85 | /* Refresh and make sure the table is updated */
|
||
86 | bom_refresh(BOM_ALL); |
||
87 | max_index = bom_get_max(); |
||
88 | turn_to_beacon2(max_index); |
||
89 | delay_ms(22);
|
||
90 | } |
||
91 | } |
||
92 | void go_straight(void){ |
||
93 | forward(200);
|
||
94 | encoder_rst_dx(LEFT); |
||
95 | encoder_rst_dx(RIGHT); |
||
96 | delay_ms(100);
|
||
97 | int x_left = encoder_get_x(LEFT), x_right = encoder_get_x(RIGHT);
|
||
98 | int count = 0; |
||
99 | int d;
|
||
100 | while (count<25){ //count = 25 when bot6; count <12 |
||
101 | x_left = encoder_get_x(LEFT); |
||
102 | x_right = encoder_get_x(RIGHT); |
||
103 | d = x_right-x_left; |
||
104 | if (d>13 || d<-13){ |
||
105 | if(d<50 && d>-50){ |
||
106 | d = round(1.0*d/4); |
||
107 | setforward(200+d, 200-d); |
||
108 | } |
||
109 | } |
||
110 | delay_ms(32);
|
||
111 | count++; |
||
112 | } |
||
113 | } |
||
114 | void go_straight_onefoot(void){ |
||
115 | go_straight(); |
||
116 | } |
||
117 | |||
118 | int main(void) |
||
119 | { |
||
120 | |||
121 | /* Initialize dragonfly board */
|
||
122 | dragonfly_init(ALL_ON); |
||
123 | encoders_init(); |
||
124 | /*******CODE HERE ***** */
|
||
125 | |||
126 | int distance=1000; |
||
127 | int onefoot=300, speed=200; //one foot is 490 for bot 1; one foot is 200 for bot6 |
||
128 | orient(); |
||
129 | forward(speed); |
||
130 | range_init(); |
||
131 | |||
132 | orb_set_color(YELLOW); |
||
133 | |||
134 | distance = get_distance(); |
||
135 | while (distance>=onefoot || distance==0){ |
||
136 | distance = get_distance(); |
||
137 | orient2(); |
||
138 | forward(speed); |
||
139 | delay_ms(14);
|
||
140 | } |
||
141 | stop(); |
||
142 | orient(); |
||
143 | |||
144 | //button1_wait ();
|
||
145 | //go_straight_onefoot();
|
||
146 | stop(); |
||
147 | /* ****** CODE HERE ******* */
|
||
148 | |||
149 | while(1); /* END HERE */ |
||
150 | |||
151 | return 0; |
||
152 | } |