Project

General

Profile

Statistics
| Revision:

root / branches / rbom / code / projects / slam / robot_main.c @ 1390

History | View | Annotate | Download (2.89 KB)

1 89 jscheine
#include "dragonfly_lib.h"
2
#include "wireless.h"
3
#include "wl_token_ring.h"
4
#include "smart_run_around_fsm.h"
5
#include "slam_defs.h"
6 50 jscheine
7
#define BOM_POST_DELAY 50
8 89 jscheine
#define PACKET_BUFFER_SIZE 101
9 50 jscheine
10
void bom_pre(void);
11
void bom_post(void);
12
13
//Packet Handler Functions
14
void timeout_handle(void);
15
void response_handle(int frame, int received);
16
void receive_handle(char type, int source, unsigned char* packet, int length);
17
void unregister(void);
18
19 89 jscheine
int make_request_packet(char** packet);
20 50 jscheine
21
int IR[5];
22
23
int main(void)
24
{
25
26
  dragonfly_init(ALL_ON);
27
28
  wl_init();
29
30
  wl_token_ring_register();
31
  wl_token_ring_join();
32
  wl_token_ring_set_bom_functions(bom_pre, bom_post, get_max_bom);
33
34
  PacketGroupHandler packet_group_handler;
35
  packet_group_handler.groupCode=SLAM_PACKET_GROUP;
36
  packet_group_handler.timeout_handler=timeout_handle;
37
  packet_group_handler.handle_response=response_handle;
38
  packet_group_handler.handle_receive=receive_handle;
39
  packet_group_handler.unregister=unregister;
40
41
  wl_register_packet_group(&packet_group_handler);
42
43
  int i;
44
  while(1){
45
    wl_do();
46
    run_around_FSM();
47
    for(i=0;i<5;i++){
48
      IR[i]=range_read_distance(i);
49
    }
50
  }
51
52
  wl_unregister_packet_group(&packet_group_handler);
53
}
54
55
void bom_pre(void){
56
    bom_on();
57
}
58
59
void bom_post(void){
60
    delay_ms(BOM_POST_DELAY);
61
    bom_off();
62
}
63
64 71 jscheine
//These can probably be NULL..
65
66 50 jscheine
void timeout_handle(void){
67 71 jscheine
68 50 jscheine
}
69
70
void response_handle(int frame, int received){
71
72
}
73
74
void receive_handle(char type, int source, unsigned char* packet, int length){
75
  int i;
76
  if(type==REQUEST_PACKET_TYPE){
77 89 jscheine
78
    /*Send a message back to our computer overlord.*/
79 71 jscheine
    char buf[PACKET_BUFFER_SIZE];
80
    int len = make_request_packet(&buf);
81
    char packet[len];
82
    for(i=0; i< len ; i++){
83
      packet[i]=buf[i];
84
    }
85
86 53 jscheine
    wl_send_robot_to_robot_packet(SLAM_PACKET_GROUP, ROBOT_RESPONSE, packet, len, source , 0);
87 50 jscheine
  }
88
  else if(type == CONTROL_PACKET_TYPE){
89
    for(i=0;i<length;i++){
90 71 jscheine
91 50 jscheine
    }
92
  }
93
}
94
95 71 jscheine
96
/**
97

98
Packets are structured {IR1, IR2, IR3, IR4, IR5,
99
BomID1, ... , BomValue1, BomID2, ... , BomValue2, etc.}
100
Total size is 5 + 3*numRobots. So Xbee should support
101
up to 31 robots before we need to restructure the
102
communication between the computer and the robot.
103

104
*/
105
int make_request_packet(char** packet){
106 53 jscheine
  int i,j;
107 50 jscheine
  for(i=0;i<5;i++){
108 71 jscheine
      (*packet)[i] = IR[i];
109 50 jscheine
  }
110 71 jscheine
  i=5;
111
  wl_token_iterator_begin();
112
  int id = wl_get_xbee_id();
113
114
  while(wl_token_iterator_has_next()){
115
    int robot_in_view = wl_token_iterator_next();
116
117
    //Robot int's are two bytes.
118
    (*packet)[i] = robot_in_view;
119
    i+=2;
120
121
    int bom_number = wl_token_get_sensor_reading(id,robot_in_view);
122
    //Bom reading of 255 denotes an unseen node.
123
    if(bom_number<0) bom_number = 255;
124
    bom_number %= 256;
125
    char bom_value = (char) bom_number;
126
    (*packet)[i] = bom_value;
127
    i++;
128 53 jscheine
  }
129 71 jscheine
  //Size is one bom byte towards every robot, and 5 IR bytes.
130
  return wl_token_get_num_robots()+5;
131 50 jscheine
}