root / trunk / code / projects / colonet / utilities / robot_slave / localization.c @ 13
History | View | Annotate | Download (1.3 KB)
1 |
#include "localization.h" |
---|---|
2 |
|
3 |
|
4 |
|
5 |
|
6 |
|
7 |
void addSensor (int maxBom, int self, int other, int NUM_BOTS) |
8 |
{ |
9 |
//make sure you don't add to the diagonal
|
10 |
if(self == other) {
|
11 |
//lcd_putchar('w');
|
12 |
return;
|
13 |
} |
14 |
if(self >= NUM_BOTS || other >= NUM_BOTS)
|
15 |
{ |
16 |
// lcd_putchar('y');
|
17 |
return;
|
18 |
} |
19 |
|
20 |
|
21 |
//lcd_putchar('a');
|
22 |
//lcd_putchar(self+48);
|
23 |
//lcd_putchar(':');
|
24 |
//lcd_putchar(other+48);
|
25 |
//lcd_putchar('.');
|
26 |
//add this robot to your own row's sensor numbers
|
27 |
//row = your index
|
28 |
//column = robotNum (input)
|
29 |
sensors[self][other] = maxBom; |
30 |
|
31 |
} |
32 |
|
33 |
unsigned char getSelfSensor (unsigned char self, unsigned char other, int NUM_BOTS) |
34 |
{ |
35 |
|
36 |
if(other < NUM_BOTS){
|
37 |
return sensors[self][other];
|
38 |
} |
39 |
else {
|
40 |
return 9; |
41 |
} |
42 |
|
43 |
} |
44 |
|
45 |
/*
|
46 |
void printSensorsMatrix (void)
|
47 |
{
|
48 |
|
49 |
|
50 |
//print table
|
51 |
int y = 1;
|
52 |
lcd_gotoxy(0, y);
|
53 |
int i, j;
|
54 |
for(i = 0; i < NUM_BOTS; i++) {
|
55 |
for(j = 0; j < NUM_BOTS; j++) {
|
56 |
lcd_putchar( getSelfSensor(i, j, NUM_BOTS) + 48);
|
57 |
lcd_putchar(' ');
|
58 |
}
|
59 |
y++;
|
60 |
lcd_gotoxy(0, y);
|
61 |
}
|
62 |
|
63 |
delay_ms(1000);
|
64 |
|
65 |
}
|
66 |
*/
|
67 |
|
68 |
/*
|
69 |
SensorRow getSelfSensorRow (int self)
|
70 |
{
|
71 |
|
72 |
SensorRow sr;
|
73 |
int i;
|
74 |
|
75 |
sr.len = NUM_BOTS;
|
76 |
for(i = 0; i < NUM_BOTS; i++) {
|
77 |
sr.row[i] = sensors[
|
78 |
|
79 |
|
80 |
|
81 |
}
|
82 |
|
83 |
*/
|