Project

General

Profile

Statistics
| Revision:

root / trunk / code / projects / slam / robot_main.c @ 89

History | View | Annotate | Download (2.89 KB)

1
#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

    
7
#define BOM_POST_DELAY 50
8
#define PACKET_BUFFER_SIZE 101 
9

    
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
int make_request_packet(char** packet);
20

    
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
//These can probably be NULL..
65

    
66
void timeout_handle(void){
67

    
68
}
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
    
78
    /*Send a message back to our computer overlord.*/
79
    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
    wl_send_robot_to_robot_packet(SLAM_PACKET_GROUP, ROBOT_RESPONSE, packet, len, source , 0);
87
  }
88
  else if(type == CONTROL_PACKET_TYPE){
89
    for(i=0;i<length;i++){
90
      
91
    }
92
  }
93
}
94

    
95

    
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
  int i,j;
107
  for(i=0;i<5;i++){
108
      (*packet)[i] = IR[i];
109
  } 
110
  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
  }
129
  //Size is one bom byte towards every robot, and 5 IR bytes. 
130
  return wl_token_get_num_robots()+5;
131
}
132