Project

General

Profile

Statistics
| Revision:

root / trunk / code / behaviors / hunter_prey / brad / main.c @ 1485

History | View | Annotate | Download (4.68 KB)

1
#include <dragonfly_lib.h>
2
#include <wireless.h>
3
#include <wl_basic.h>
4
#include "smart_run_around_fsm.h"
5
#include "hunter_prey.h"
6

    
7
#define CHAN 0xC
8
#define TAG_PAUSE 2000
9

    
10
#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
volatile uint8_t color1, color2;
18
volatile uint8_t tagAck;
19
volatile uint8_t tagger;
20
uint8_t id;
21

    
22
void test_bom(void) {
23
  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
    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
    usb_putc('\n');
48
    delay_ms(500);
49
  }
50

    
51
}
52

    
53
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
int main(void) {
73

    
74
  int frontRange;
75
  uint8_t angle;
76
  unsigned char packet[2];
77
  
78
  //intialize the robot and wireless
79
  dragonfly_init(ALL_ON);
80

    
81
  //  test_bom();
82

    
83
  id = get_robotid();
84
  packet[1] = id;
85
  usb_puts("hello, I am robot \n");
86
  usb_puti(id);
87
  usb_putc('\n');
88

    
89
  usb_puts("wireless.\n");
90

    
91
  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
  //use a specific channel to avoid interfering with other tasks
100
  wl_set_channel(CHAN);
101

    
102
  usb_puts("orb set\n");
103

    
104
  orb_set_color(BLUE);
105
  color1 = BLUE;
106
  color2 = BLUE;
107

    
108
  //if button 2 is held, you are prey
109
  if(button2_read()) {
110
    robotState = ROBOT_PREY;
111
    bom_on();
112
  }
113

    
114

    
115
  while(1) {
116

    
117
    if(robotState==ROBOT_TAGGED) {
118
      usb_puts("tagged, waiting to send ACK\n");
119
      //delay_ms(TAG_PAUSE);
120
      move(0,0);
121
      usb_puts("sending ACK to ");
122
      usb_puti(tagger);
123
      usb_putc('\n');
124
      packet[0] = HUNTER_PREY_ACTION_ACK;
125
      packet[1] = tagger;
126
      wl_basic_send_global_packet (42, &packet, 2);
127
      usb_puts("sent\n");
128
      robotState = ROBOT_HUNTER; //no longer prey
129
    }
130

    
131
    else if(robotState == ROBOT_PREY) {
132
      run_around_FSM();
133
    }
134

    
135
    else { //ROBOT_HUNTER
136

    
137
      bom_refresh(BOM_ALL);
138

    
139
      delay_ms(100);
140

    
141
      frontRange = range_read_distance (IR2);
142

    
143
/*       usb_puti(frontRange); */
144
/*       usb_putc(','); */
145

    
146
      //BUG: when get_max_bom is called, the rangefinder seems to stop
147
      //working
148
      angle = bom_get_max();
149
/*       usb_puti(angle); */
150
/*       usb_putc('\n'); */
151

    
152
      chase(angle);
153

    
154
      //debugging to determine tagging conditions
155
      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
/*       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

    
185

    
186

    
187
      if(hunter_prey_tagged(angle, frontRange) || button1_read()) {
188
        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
        wl_basic_send_global_packet (42, &packet, 2);
196
        move(0,0);
197
        while(!tagAck)
198
          wl_do();
199

    
200
        if(tagAck == id) { //if the ack was for us
201
          usb_puts("ACK!\n");
202
              
203
          move(-230,0); //back up to give the new prey some room
204
          delay_ms(TAG_PAUSE/2);
205
          move(0,0);
206

    
207
          robotState = ROBOT_PREY;
208
          bom_on();
209
        }
210
        else {
211
          usb_puts("ACK failed!\n");
212
        }
213
      }
214
    }
215

    
216
    wl_do();
217
  }
218
  
219
  return 0;
220
}
221

    
222
void packet_receive(char type, int source, unsigned char* packet, int length)
223
{
224

    
225
  if(type==42 && length>=2){
226
    if(packet[0] == HUNTER_PREY_ACTION_TAG) {
227
      if(robotState == ROBOT_PREY) {
228
        robotState = ROBOT_TAGGED;
229
        bom_off();
230
        color1 = BLUE;
231
        color2 = BLUE;
232
        orb_set_color(BLUE);
233
        tagger = packet[1];
234
      }
235
      else {
236
        usb_puts("tagged, but I'm not it!\n");
237
      }
238
    }
239
    else if(packet[0] == HUNTER_PREY_ACTION_ACK && robotState == ROBOT_HUNTER) {
240
      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
#define HP_BUF_IDX_ACTION 0