root / branches / autonomous_recharging / code / projects / colonet / utilities / robot_wireless_relay / localization.c @ 1390
History | View | Annotate | Download (1.32 KB)
1 | 13 | emarinel | #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 | */ |