Revision 1410
Hunter-prey sort of works!
| trunk/code/projects/hunter_prey/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