Project

General

Profile

Statistics
| Revision:

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

History | View | Annotate | Download (4 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 ACK_PAUSE 5000
9

    
10
#define ROBOT_HUNTER 0
11
#define ROBOT_PREY 1
12
#define ROBOT_TAGGED 2
13
#define ROBOT_IDLE 3
14

    
15
void packet_receive(unsigned char* packet, int length);
16

    
17
volatile uint8_t robotState; // current state
18
volatile uint8_t tagger;    // id of tagging robot
19
uint8_t id; // my robot's id
20

    
21
void test_bom(void) {
22
  int i;
23
  int val;
24

    
25
  while(1) {
26
    bom_refresh(BOM_ALL);
27

    
28
    for(i=0; i<16; i++) {
29
      val = bom_get(i);
30
      usb_puti(val);
31
      usb_putc(' ');
32
    }
33

    
34
    usb_puts("| ");
35

    
36
    usb_puti(range_read_distance (IR1));
37
    usb_putc(' ');
38
    usb_puti(range_read_distance (IR2));
39
    usb_putc(' ');
40
    usb_puti(range_read_distance (IR3));
41
    usb_putc(' ');
42
    usb_puti(range_read_distance (IR4));
43
    usb_putc(' ');
44
    usb_puti(range_read_distance (IR5));
45

    
46
    usb_putc('\n');
47
    delay_ms(500);
48
  }
49

    
50
}
51

    
52
void chase(uint8_t angle) {
53

    
54
  int omega = angle - 4;
55

    
56
  if(angle > 15)
57
    return;
58

    
59
  if(omega >  8)
60
    omega = omega - 16;
61

    
62
  omega *= 255/8;
63

    
64
/*   usb_puti(omega); */
65
/*   usb_putc('\n'); */
66

    
67
  move(140, omega);
68

    
69
}
70

    
71
int main(void) {
72

    
73
  int frontRange;   // reading of front rangefinder
74
  uint8_t angle;    // result of bom_get_max()
75
  unsigned char *packet;  // packet to send
76
  int length;        // length of received packet
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 ");
86
  usb_puti(id);
87
  usb_putc('\n');
88

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

    
91
  wl_basic_init_default();
92

    
93
  usb_puts("run around.\n");
94

    
95
  run_around_init();
96

    
97
  usb_puts("channel");
98

    
99
  //use a specific channel to avoid interfering with other tasks
100
  wl_set_channel(CHAN);
101
  usb_puti(CHAN);
102

    
103
  usb_puts("\norb set");
104

    
105
  // initially in hunter state
106
  robotState = ROBOT_HUNTER;
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
      orb_set_color(YELLOW);
120
      //delay_ms(TAG_PAUSE);
121
      move(0,0);
122
      usb_puts("sending ACK to ");
123
      usb_puti(tagger);
124
      usb_putc('\n');
125
      packet[0] = HUNTER_PREY_ACTION_ACK;
126
      packet[1] = tagger;
127
      wl_basic_send_global_packet (42, &packet, 2);
128
      usb_puts("sent\n");
129
      robotState = ROBOT_HUNTER; //no longer prey
130
    }
131

    
132
    else if(robotState == ROBOT_PREY) {
133
        orb_set_color(GREEN);
134
      run_around_FSM();
135
    }
136

    
137
    else if (robotState == ROBOT_IDLE) {
138
        orb_set_color(BLUE);
139
        move(0,0);
140
        delay_ms(ACK_PAUSE);
141
        robotState = ROBOT_HUNTER;
142
    }
143

    
144
    else { //ROBOT_HUNTER
145
        orb_set_color(RED);
146
      bom_refresh(BOM_ALL);
147
      delay_ms(100);
148

    
149
      frontRange = range_read_distance (IR2);
150

    
151
      //BUG: when get_max_bom is called, the rangefinder seems to stop
152
      //working
153
      angle = bom_get_max();
154
      chase(angle);
155

    
156
      usb_puti(frontRange);
157
      usb_putc('\n');
158

    
159
      if ((hunter_prey_tagged(angle, frontRange) && rtc_get() >= 1) || button1_read()) {
160
            usb_puts("TAG!\n");
161
            packet[0] = HUNTER_PREY_ACTION_TAG;
162
            packet[1] = id;
163
            wl_basic_send_global_packet (42, &packet, 2);
164
            wl_do();
165
            rtc_reset();
166
        }
167
    }
168

    
169
    packet = wl_basic_do_default(&length);
170
    if (packet)
171
        packet_receive(packet, length);
172
  }
173
  
174
  return 0;
175
}
176

    
177
void packet_receive(unsigned char* packet, int length)
178
{
179

    
180
  if(length>=2){
181
    if(packet[0] == HUNTER_PREY_ACTION_TAG) {
182
        usb_puts("received a tag packet\n");
183
      if(robotState == ROBOT_PREY) {
184
          usb_puts("it was for me\n");
185
        robotState = ROBOT_TAGGED;
186
        bom_off();
187
        tagger = packet[1];
188
      }
189
      else {
190
        usb_puts("tagged, but I'm not it!\n");
191
      }
192
    }
193
    else if(packet[0] == HUNTER_PREY_ACTION_ACK) {
194
      robotState = ROBOT_IDLE;
195
    }
196
    usb_puts("got '");
197
    usb_putc(packet[0]);
198
    usb_puti(packet[1]);
199
    usb_puts("'\n");
200
  }
201
  else {
202
    usb_puts(" length=");
203
    usb_puti(length);
204
    usb_putc('\n');
205
  }
206
}
207

    
208
#define HP_BUF_IDX_ACTION 0