Project

General

Profile

Statistics
| Revision:

root / trunk / code / projects / hunter_prey / main.c @ 1469

History | View | Annotate | Download (4.68 KB)

1 1373 bneuman
#include <dragonfly_lib.h>
2
#include <wireless.h>
3 1408 bneuman
#include <wl_basic.h>
4
#include "smart_run_around_fsm.h"
5 1431 bneuman
#include "hunter_prey.h"
6 1373 bneuman
7 1466 bneuman
#define CHAN 0xC
8 1408 bneuman
#define TAG_PAUSE 2000
9 1373 bneuman
10 1408 bneuman
#define ROBOT_HUNTER 0
11
#define ROBOT_PREY 1
12
#define ROBOT_TAGGED 2
13
14
void packet_receive(char type, int source, unsigned char* packet, int length);
15
16
volatile uint8_t robotState = ROBOT_HUNTER;
17 1411 bneuman
volatile uint8_t color1, color2;
18 1408 bneuman
volatile uint8_t tagAck;
19
volatile uint8_t tagger;
20
uint8_t id;
21
22 1431 bneuman
void test_bom(void) {
23 1409 bneuman
  int i;
24
  int val;
25
26
  while(1) {
27
    bom_refresh(BOM_ALL);
28
29
    for(i=0; i<16; i++) {
30
      val = bom_get(i);
31
      usb_puti(val);
32
      usb_putc(' ');
33
    }
34
35 1410 bneuman
    usb_puts("| ");
36
37
    usb_puti(range_read_distance (IR1));
38
    usb_putc(' ');
39
    usb_puti(range_read_distance (IR2));
40
    usb_putc(' ');
41
    usb_puti(range_read_distance (IR3));
42
    usb_putc(' ');
43
    usb_puti(range_read_distance (IR4));
44
    usb_putc(' ');
45
    usb_puti(range_read_distance (IR5));
46
47 1409 bneuman
    usb_putc('\n');
48
    delay_ms(500);
49
  }
50
51
}
52
53 1410 bneuman
void chase(uint8_t angle) {
54
55
  int omega = angle - 4;
56
57
  if(angle > 16)
58
    return;
59
60
  if(omega >  8)
61
    omega = omega - 16;
62
63
  omega *= 255/8;
64
65
/*   usb_puti(omega); */
66
/*   usb_putc('\n'); */
67
68
  move(140, omega);
69
70
}
71
72 1373 bneuman
int main(void) {
73
74
  int frontRange;
75
  uint8_t angle;
76 1431 bneuman
  unsigned char packet[2];
77 1373 bneuman
78
  //intialize the robot and wireless
79
  dragonfly_init(ALL_ON);
80 1374 bneuman
81 1410 bneuman
  //  test_bom();
82 1409 bneuman
83 1408 bneuman
  id = get_robotid();
84
  packet[1] = id;
85
  usb_puts("hello, I am robot \n");
86
  usb_puti(id);
87
  usb_putc('\n');
88 1374 bneuman
89 1408 bneuman
  usb_puts("wireless.\n");
90 1373 bneuman
91 1408 bneuman
  wl_basic_init(&packet_receive);
92
93
  usb_puts("run around.\n");
94
95
  run_around_init();
96
97
  usb_puts("channel\n");
98
99 1373 bneuman
  //use a specific channel to avoid interfering with other tasks
100
  wl_set_channel(CHAN);
101
102 1408 bneuman
  usb_puts("orb set\n");
103
104 1373 bneuman
  orb_set_color(BLUE);
105 1411 bneuman
  color1 = BLUE;
106
  color2 = BLUE;
107 1373 bneuman
108 1408 bneuman
  //if button 2 is held, you are prey
109
  if(button2_read()) {
110
    robotState = ROBOT_PREY;
111 1409 bneuman
    bom_on();
112 1408 bneuman
  }
113
114
115 1373 bneuman
  while(1) {
116
117 1408 bneuman
    if(robotState==ROBOT_TAGGED) {
118
      usb_puts("tagged, waiting to send ACK\n");
119 1447 bneuman
      //delay_ms(TAG_PAUSE);
120 1408 bneuman
      move(0,0);
121
      usb_puts("sending ACK to ");
122
      usb_puti(tagger);
123
      usb_putc('\n');
124 1431 bneuman
      packet[0] = HUNTER_PREY_ACTION_ACK;
125 1408 bneuman
      packet[1] = tagger;
126 1447 bneuman
      wl_basic_send_global_packet (42, &packet, 2);
127 1408 bneuman
      usb_puts("sent\n");
128
      robotState = ROBOT_HUNTER; //no longer prey
129
    }
130 1373 bneuman
131 1408 bneuman
    else if(robotState == ROBOT_PREY) {
132
      run_around_FSM();
133
    }
134 1373 bneuman
135 1408 bneuman
    else { //ROBOT_HUNTER
136 1373 bneuman
137 1409 bneuman
      bom_refresh(BOM_ALL);
138 1373 bneuman
139 1411 bneuman
      delay_ms(100);
140
141 1408 bneuman
      frontRange = range_read_distance (IR2);
142
143 1410 bneuman
/*       usb_puti(frontRange); */
144
/*       usb_putc(','); */
145 1408 bneuman
146
      //BUG: when get_max_bom is called, the rangefinder seems to stop
147
      //working
148 1409 bneuman
      angle = bom_get_max();
149 1410 bneuman
/*       usb_puti(angle); */
150
/*       usb_putc('\n'); */
151 1408 bneuman
152 1410 bneuman
      chase(angle);
153
154 1431 bneuman
      //debugging to determine tagging conditions
155 1411 bneuman
      if(angle < 7 && angle > 1) {
156
        if(color1 != RED) {
157
          color1 = RED;
158
          orb1_set_color(RED);
159
        }
160
      }
161
      else {
162
        if(color1 != BLUE) {
163
          color1 = BLUE;
164
          orb1_set_color(BLUE);
165
        }
166
      }
167
168
169
      usb_puti(frontRange);
170
      usb_putc('\n');
171
172 1431 bneuman
/*       if(frontRange > 0 && frontRange < TAG_RANGE) { */
173
/*         if(color2 != RED) { */
174
/*           color2 = RED; */
175
/*           orb2_set_color(RED); */
176
/*         } */
177
/*       } */
178
/*       else { */
179
/*         if(color2 != BLUE) { */
180
/*           color2 = BLUE; */
181
/*           orb2_set_color(BLUE); */
182
/*         } */
183
/*       } */
184 1411 bneuman
185
186
187 1447 bneuman
      if(hunter_prey_tagged(angle, frontRange) || button1_read()) {
188 1431 bneuman
        orb_set_color(YELLOW);
189
        color1 = YELLOW;
190
        color2 = YELLOW;
191
        usb_puts("TAG!\n");
192
        tagAck = 0;
193
        packet[0] = HUNTER_PREY_ACTION_TAG;
194
        packet[1] = id;
195 1447 bneuman
        wl_basic_send_global_packet (42, &packet, 2);
196 1431 bneuman
        move(0,0);
197
        while(!tagAck)
198
          wl_do();
199 1408 bneuman
200 1431 bneuman
        if(tagAck == id) { //if the ack was for us
201
          usb_puts("ACK!\n");
202 1411 bneuman
203 1431 bneuman
          move(-230,0); //back up to give the new prey some room
204
          delay_ms(TAG_PAUSE/2);
205
          move(0,0);
206 1411 bneuman
207 1431 bneuman
          robotState = ROBOT_PREY;
208
          bom_on();
209 1408 bneuman
        }
210 1431 bneuman
        else {
211
          usb_puts("ACK failed!\n");
212
        }
213 1373 bneuman
      }
214
    }
215
216
    wl_do();
217
  }
218 1431 bneuman
219 1373 bneuman
  return 0;
220
}
221 1408 bneuman
222
void packet_receive(char type, int source, unsigned char* packet, int length)
223
{
224 1431 bneuman
225 1447 bneuman
  if(type==42 && length>=2){
226 1431 bneuman
    if(packet[0] == HUNTER_PREY_ACTION_TAG) {
227 1408 bneuman
      if(robotState == ROBOT_PREY) {
228
        robotState = ROBOT_TAGGED;
229 1409 bneuman
        bom_off();
230 1411 bneuman
        color1 = BLUE;
231
        color2 = BLUE;
232 1408 bneuman
        orb_set_color(BLUE);
233
        tagger = packet[1];
234
      }
235
      else {
236
        usb_puts("tagged, but I'm not it!\n");
237
      }
238
    }
239 1431 bneuman
    else if(packet[0] == HUNTER_PREY_ACTION_ACK && robotState == ROBOT_HUNTER) {
240 1408 bneuman
      tagAck = packet[1];
241
    }
242
    else {
243
      usb_puts("got '");
244
      usb_putc(packet[0]);
245
      usb_puti(packet[1]);
246
      usb_puts("'\n");
247
    }
248
  }
249
  else {
250
    usb_puts("got unkown packet! type=");
251
    usb_puti(type);
252
    usb_puts(" length=");
253
    usb_puti(length);
254
    usb_putc('\n');
255
  }
256
}
257
258 1431 bneuman
#define HP_BUF_IDX_ACTION 0