Revision 1410
Hunter-prey sort of works!
main.c | ||
---|---|---|
4 | 4 |
#include "smart_run_around_fsm.h" |
5 | 5 |
|
6 | 6 |
#define CHAN 0xF |
7 |
#define TAG_TIME 10
|
|
7 |
#define TAG_TIME 5
|
|
8 | 8 |
#define TAG_RANGE 150 |
9 | 9 |
#define TAG_PAUSE 2000 |
10 | 10 |
|
... | ... | |
33 | 33 |
usb_putc(' '); |
34 | 34 |
} |
35 | 35 |
|
36 |
usb_puts("| "); |
|
37 |
|
|
38 |
usb_puti(range_read_distance (IR1)); |
|
39 |
usb_putc(' '); |
|
40 |
usb_puti(range_read_distance (IR2)); |
|
41 |
usb_putc(' '); |
|
42 |
usb_puti(range_read_distance (IR3)); |
|
43 |
usb_putc(' '); |
|
44 |
usb_puti(range_read_distance (IR4)); |
|
45 |
usb_putc(' '); |
|
46 |
usb_puti(range_read_distance (IR5)); |
|
47 |
|
|
36 | 48 |
usb_putc('\n'); |
37 | 49 |
delay_ms(500); |
38 | 50 |
} |
39 | 51 |
|
40 | 52 |
} |
41 | 53 |
|
54 |
void chase(uint8_t angle) { |
|
55 |
|
|
56 |
int omega = angle - 4; |
|
57 |
|
|
58 |
if(angle > 16) |
|
59 |
return; |
|
60 |
|
|
61 |
if(omega > 8) |
|
62 |
omega = omega - 16; |
|
63 |
|
|
64 |
omega *= 255/8; |
|
65 |
|
|
66 |
/* usb_puti(omega); */ |
|
67 |
/* usb_putc('\n'); */ |
|
68 |
|
|
69 |
move(140, omega); |
|
70 |
|
|
71 |
} |
|
72 |
|
|
42 | 73 |
int main(void) { |
43 | 74 |
|
44 | 75 |
int frontRange; |
... | ... | |
51 | 82 |
//intialize the robot and wireless |
52 | 83 |
dragonfly_init(ALL_ON); |
53 | 84 |
|
54 |
//test_bom(); |
|
85 |
// test_bom();
|
|
55 | 86 |
|
56 | 87 |
id = get_robotid(); |
57 | 88 |
packet[1] = id; |
... | ... | |
111 | 142 |
|
112 | 143 |
frontRange = range_read_distance (IR2); |
113 | 144 |
|
114 |
usb_puti(frontRange);
|
|
115 |
usb_putc(',');
|
|
145 |
/* usb_puti(frontRange); */
|
|
146 |
/* usb_putc(','); */
|
|
116 | 147 |
|
117 | 148 |
//BUG: when get_max_bom is called, the rangefinder seems to stop |
118 | 149 |
//working |
119 | 150 |
angle = bom_get_max(); |
120 |
usb_puti(angle);
|
|
121 |
usb_putc('\n');
|
|
151 |
/* usb_puti(angle); */
|
|
152 |
/* usb_putc('\n'); */
|
|
122 | 153 |
|
123 |
if(angle == 4 && frontRange > 0 && frontRange < TAG_RANGE) { |
|
154 |
chase(angle); |
|
155 |
|
|
156 |
if(angle < 7 && angle > 1 && frontRange > 0 && frontRange < TAG_RANGE) { |
|
124 | 157 |
if(onTarget == 0) { |
125 | 158 |
onTarget = TAG_TIME; |
126 | 159 |
if(color != GREEN) { |
Also available in: Unified diff