Project

General

Profile

Statistics
| Revision:

root / branches / autonomous_recharging / code / projects / autonomous_recharging / charging_station / bays.c @ 98

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
}