root / branches / autonomous_recharging / code / projects / autonomous_recharging / charging_station / bays.c @ 98
History | View | Annotate | Download (4.1 KB)
1 | 85 | bcoltin | #include "firefly+_lib.h" |
---|---|---|---|
2 | #include "bays.h" |
||
3 | |||
4 | #define MAX(a,b) ((a) > (b) ? (a) : (b))
|
||
5 | #define MIN(a,b) ((a) < (b) ? (a) : (b))
|
||
6 | |||
7 | |||
8 | void bay_tester(void) |
||
9 | { |
||
10 | bayStates[1] = 50; |
||
11 | bayStates[2] = 51; |
||
12 | bayStates[3] = 52; |
||
13 | } |
||
14 | |||
15 | void print_bays_debug(void) |
||
16 | { |
||
17 | |||
18 | usb_puts("\n\r");
|
||
19 | usb_puts("Bays: ");
|
||
20 | for(int i = 0; i < NUM_BAYS; i++) { |
||
21 | usb_puti(bayStates[i]); |
||
22 | usb_putc(' ');
|
||
23 | } |
||
24 | usb_puts("\n\r");
|
||
25 | |||
26 | usb_puts("Seeking robot: ");
|
||
27 | usb_puti(robot_seeking); |
||
28 | usb_puts("\n\r");
|
||
29 | } |
||
30 | |||
31 | void do_consistency_checks(void) |
||
32 | { |
||
33 | if(robot_seeking != 255 && robot_assignment(robot_seeking) == 255) // the only free bay from bay_tester() |
||
34 | { |
||
35 | usb_puts("BAD STUFF HAPPENED...\n\r");
|
||
36 | usb_puts("robot assigned to BAY 1 does not equal robot_seeking\n\r");
|
||
37 | usb_puts("\n\rrobot_seeking: ");
|
||
38 | usb_puti(robot_seeking); |
||
39 | usb_puts("Bay 1 contains: ");
|
||
40 | usb_puti(bayStates[1]);
|
||
41 | usb_puts("\n\r");
|
||
42 | //while(1);
|
||
43 | } |
||
44 | } |
||
45 | // END DEBUGGING FUNCTIONS
|
||
46 | |||
47 | void bays_init(void) |
||
48 | { |
||
49 | int i;
|
||
50 | for (i = 0; i < NUM_BAYS; i++) |
||
51 | bayStates[i] = 255;
|
||
52 | robot_seeking = 255;
|
||
53 | bay_tester(); |
||
54 | |||
55 | digital_output(ENABLE_PIN, 0);
|
||
56 | digital_output(BOM_PIN, 1);
|
||
57 | digital_output(SELECT_0, 0);
|
||
58 | digital_output(SELECT_1, 0);
|
||
59 | digital_output(SELECT_2, 0);
|
||
60 | |||
61 | active_bay = -1;
|
||
62 | } |
||
63 | |||
64 | /*int assign_bay(int robot)
|
||
65 | {
|
||
66 | int max_bom = sensors[robot]; // where the station sees the robot (max bom)
|
||
67 | int closest = 255;
|
||
68 | int distance = 16;
|
||
69 | int i;
|
||
70 | for (i = 0; i < NUM_BAYS; i++)
|
||
71 | {
|
||
72 | if (bayStates[i] != 255)
|
||
73 | continue;
|
||
74 | int d = max_bom - i * 4; // distance (BOM-wise) from bay
|
||
75 | if (d < -8)
|
||
76 | d += 16;
|
||
77 | else
|
||
78 | if (d > 8)
|
||
79 | d -= 16;
|
||
80 | if (d < 0)
|
||
81 | d *= -1;
|
||
82 | if (d < distance) // finds closest bay to robot's current position
|
||
83 | {
|
||
84 | distance = d; // distance (BOM-wise)
|
||
85 | closest = i; // actual bay number
|
||
86 | }
|
||
87 | }
|
||
88 | if (closest == 255) // all full
|
||
89 | return 255;
|
||
90 | bayStates[closest] = robot;
|
||
91 | return closest;
|
||
92 | }*/
|
||
93 | |||
94 | /*int assign_bay(int robot) // returns number of an empty bay (255 if all full)
|
||
95 | {
|
||
96 | for(int i = 0; i < NUM_BAYS; i++) // right now, go thru linearly and allocate first available bay
|
||
97 | {
|
||
98 | if(bayStates[i] == 255)
|
||
99 | {
|
||
100 | bayStates[i] = robot;
|
||
101 | return i;
|
||
102 | }
|
||
103 | }
|
||
104 | }*/
|
||
105 | |||
106 | int left_index( int pos ) |
||
107 | { |
||
108 | int i;
|
||
109 | for(i = pos; i >= 0; i--) |
||
110 | { |
||
111 | if(bayStates[i] != 255) |
||
112 | return i;
|
||
113 | } |
||
114 | return -1; |
||
115 | } |
||
116 | |||
117 | int right_index( int pos ) |
||
118 | { |
||
119 | int i;
|
||
120 | for(i = pos; i < NUM_BAYS; i++)
|
||
121 | { |
||
122 | if(bayStates[i] != 255) |
||
123 | return i;
|
||
124 | } |
||
125 | return NUM_BAYS;
|
||
126 | } |
||
127 | |||
128 | 98 | bcoltin | /*int assign_bay( int robot ) // 'smart' assign_bay()
|
129 | 85 | bcoltin | {
|
130 | int ldist = 0;
|
||
131 | int rdist = 0;
|
||
132 | int score = 0;
|
||
133 | int max = 0;
|
||
134 | |||
135 | int bay = 255;
|
||
136 | |||
137 | int count = 0;
|
||
138 | |||
139 | int i;
|
||
140 | for(i = 0; i < NUM_BAYS; i++)
|
||
141 | {
|
||
142 | if(bayStates[i] == 255)
|
||
143 | count++;
|
||
144 | }
|
||
145 | if(count == NUM_BAYS)
|
||
146 | {
|
||
147 | bayStates[0] = robot;
|
||
148 | return 0;
|
||
149 | }
|
||
150 | |||
151 | for(i = 0; i < NUM_BAYS; i++)
|
||
152 | {
|
||
153 | if(bayStates[i] == 255)
|
||
154 | {
|
||
155 | ldist = i - left_index(i);
|
||
156 | rdist = right_index(i) - i;
|
||
157 | if(ldist - i == 1)
|
||
158 | score = rdist;
|
||
159 | else if(i + rdist == NUM_BAYS)
|
||
160 | score = ldist;
|
||
161 | else
|
||
162 | score = MIN(ldist,rdist);
|
||
163 | if(score > max)
|
||
164 | {
|
||
165 | max = score;
|
||
166 | bay = i;
|
||
167 | }
|
||
168 | }
|
||
169 | ldist = rdist = 0;
|
||
170 | }
|
||
171 | bayStates[bay] = robot;
|
||
172 | return bay;
|
||
173 | 98 | bcoltin | }*/
|
174 | 85 | bcoltin | |
175 | int robot_assignment(int robot) //returns bay to which 'robot' has been assigned, 255 if not yet assigned |
||
176 | { |
||
177 | for(int i = 0; i < NUM_BAYS; i++) |
||
178 | { |
||
179 | if(bayStates[i] == robot)
|
||
180 | return i;
|
||
181 | } |
||
182 | return 255; |
||
183 | } |
||
184 | |||
185 | int is_empty(void) //returns 1 if there is an empty bay, 0 if not |
||
186 | { |
||
187 | for(int i = 0; i < NUM_BAYS; i++) |
||
188 | { |
||
189 | if(bayStates[i] == 255) |
||
190 | return 1; |
||
191 | } |
||
192 | return 0; |
||
193 | } |
||
194 | |||
195 | void lbom_on(void) |
||
196 | { |
||
197 | digital_output(BOM_PIN, 0);
|
||
198 | } |
||
199 | |||
200 | void lbom_off(void) |
||
201 | { |
||
202 | digital_output(BOM_PIN, 1);
|
||
203 | } |
||
204 | |||
205 | void bay_enable(int which) |
||
206 | { |
||
207 | active_bay = which; |
||
208 | lbom_off(); |
||
209 | bay_disable(); |
||
210 | digital_output(SELECT_0, which & 1);
|
||
211 | digital_output(SELECT_1, (which>>1) & 1); |
||
212 | digital_output(SELECT_2, (which>>2) & 1); |
||
213 | digital_output(ENABLE_PIN, 1);
|
||
214 | } |
||
215 | |||
216 | void bay_disable(void) |
||
217 | { |
||
218 | digital_output(ENABLE_PIN, 0);
|
||
219 | } |