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