root / branches / charging_station / code / projects / recharging / charging_station / bays.c @ 85
History | View | Annotate | Download (4.1 KB)
1 |
#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 |
int assign_bay( int robot ) // 'smart' assign_bay() |
129 |
{ |
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 |
} |
174 |
|
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 |
} |