root / branches / slam / code / projects / slam / robot / robot_main.c @ 121
History | View | Annotate | Download (3.21 KB)
1 | 121 | jscheine | #include "dragonfly_lib.h" |
---|---|---|---|
2 | #include "../../libwireless/lib/wireless.h" |
||
3 | #include "../../libwireless/lib/wl_token_ring.h" |
||
4 | #include "smart_run_around_fsm.h" |
||
5 | #include "../slam_defs.h" |
||
6 | |||
7 | #define BOM_POST_DELAY 0 |
||
8 | #define PACKET_BUFFER_SIZE 101 |
||
9 | |||
10 | //Packet Handler Functions
|
||
11 | void receive_handle(char type, int source, unsigned char* packet, int length); |
||
12 | void unregister(void); |
||
13 | |||
14 | int make_request_packet(char** packet); |
||
15 | void send_response_packet(int dest); |
||
16 | |||
17 | unsigned int response_flag = 0; //Will represent the robot to respond to or 0 for no robot. |
||
18 | char control_flag=0; //Non zero when the robot is to be controlled. |
||
19 | int IR[5]; |
||
20 | |||
21 | int main(void) |
||
22 | { |
||
23 | |||
24 | dragonfly_init(ALL_ON); |
||
25 | |||
26 | usb_init(); |
||
27 | usb_puts("USB initialized\n\n\r");
|
||
28 | |||
29 | wl_init(); |
||
30 | |||
31 | wl_token_ring_register(); |
||
32 | wl_token_ring_join(); |
||
33 | |||
34 | PacketGroupHandler packet_group_handler; |
||
35 | packet_group_handler.groupCode=SLAM_PACKET_GROUP; |
||
36 | packet_group_handler.timeout_handler=NULL;
|
||
37 | packet_group_handler.handle_response=NULL;
|
||
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 | //Uncomment this if you want the robot to do something.
|
||
47 | //run_around_FSM();
|
||
48 | |||
49 | for(i=0;i<5;i++){ |
||
50 | IR[i]=range_read_distance(i); |
||
51 | } |
||
52 | if(response_flag){
|
||
53 | send_response_packet(response_flag); |
||
54 | response_flag=0;
|
||
55 | } |
||
56 | if(control_flag){
|
||
57 | //FILL IN HERE
|
||
58 | } |
||
59 | |||
60 | } |
||
61 | |||
62 | wl_unregister_packet_group(&packet_group_handler); |
||
63 | } |
||
64 | |||
65 | void send_response_packet(int dest){ |
||
66 | |||
67 | usb_puts("Sending a response packet to ");
|
||
68 | usb_puti(dest); |
||
69 | usb_puts(".\n\r");
|
||
70 | |||
71 | char buf[PACKET_BUFFER_SIZE];
|
||
72 | int len = make_request_packet((char**)&buf), i; |
||
73 | char packet[len];
|
||
74 | for(i=0; i< len ; i++){ |
||
75 | packet[i]=buf[i]; |
||
76 | } |
||
77 | |||
78 | wl_send_robot_to_robot_packet(SLAM_PACKET_GROUP, ROBOT_RESPONSE, packet, len, dest, 0);
|
||
79 | } |
||
80 | |||
81 | void receive_handle(char type, int source, unsigned char* packet, int length){ |
||
82 | if(type==REQUEST_PACKET_TYPE){
|
||
83 | usb_puts("Received a request packet\n\r");
|
||
84 | response_flag = source; |
||
85 | } |
||
86 | //Not yet implemented:
|
||
87 | else if(type == CONTROL_PACKET_TYPE){ |
||
88 | control_flag = 1;
|
||
89 | //Put packet data into some global.
|
||
90 | } |
||
91 | } |
||
92 | |||
93 | |||
94 | /**
|
||
95 | |||
96 | Packets are structured {IR1, IR2, IR3, IR4, IR5,
|
||
97 | BomID1, ... , BomValue1, BomID2, ... , BomValue2, etc.}
|
||
98 | Total size is 5 + 3*numRobots. So Xbee should support
|
||
99 | up to 31 robots before we need to restructure the
|
||
100 | communication between the computer and the robot.
|
||
101 | |||
102 | */
|
||
103 | int make_request_packet(char** packet){ |
||
104 | int i;
|
||
105 | for(i=0;i<5;i++){ |
||
106 | (*packet)[i] = IR[i]; |
||
107 | } |
||
108 | i=5;
|
||
109 | wl_token_iterator_begin(); |
||
110 | int id = wl_get_xbee_id();
|
||
111 | |||
112 | while(wl_token_iterator_has_next()){
|
||
113 | int robot_in_view = wl_token_iterator_next();
|
||
114 | |||
115 | //Robot int's are two bytes.
|
||
116 | (*packet)[i] = robot_in_view; |
||
117 | i+=2;
|
||
118 | |||
119 | int bom_number = wl_token_get_sensor_reading(id,robot_in_view);
|
||
120 | //Bom reading of 255 denotes an unseen node.
|
||
121 | if(bom_number<0) bom_number = 255; |
||
122 | bom_number %= 256;
|
||
123 | char bom_value = (char) bom_number; |
||
124 | (*packet)[i] = bom_value; |
||
125 | i++; |
||
126 | } |
||
127 | //Size is one bom byte towards every robot, and 5 IR bytes.
|
||
128 | return wl_token_get_num_robots()+5; |
||
129 | } |
||
130 | |||
131 | void unregister(void){ |
||
132 | return;
|
||
133 | } |