Revision 393
Fixed the BOM response code, needs to be tested.
| branches/slam/code/projects/colonet/DataRequests/robot/data_response.c | ||
|---|---|---|
| 1 |
#include "dragonfly_lib.h" |
|
| 1 | 2 |
#include "data_response.h" |
| 2 |
#include "dragonfly_lib.h" |
|
| 3 | 3 |
#include "wl_token_ring.h" |
| 4 | 4 |
#include "wireless.h" |
| 5 | 5 |
#include "serial.h" |
| ... | ... | |
| 7 | 7 |
PacketGroupHandler pgh; |
| 8 | 8 |
|
| 9 | 9 |
void handle_receive(char type, int source, unsigned char* packet, int length); |
| 10 |
|
|
| 11 | 10 |
int generate_bom_packet(char** buf); |
| 12 | 11 |
int generate_IR_packet(char** buf); |
| 13 | 12 |
|
| 13 |
|
|
| 14 |
/** |
|
| 15 |
* @brief The only function that should be called by user code. |
|
| 16 |
Prepares the robot to receive and respond to data requests. |
|
| 17 |
*/ |
|
| 14 | 18 |
void data_response_init(void){
|
| 15 | 19 |
usb_init(); |
| 16 | 20 |
usb_puts("USB initialized!\n\r");
|
| ... | ... | |
| 24 | 28 |
usb_puts("Packet Group Registered!\n\r");
|
| 25 | 29 |
} |
| 26 | 30 |
|
| 31 |
|
|
| 32 |
/** |
|
| 33 |
* Sorry for the lack of modularity, I didn't want to deal with memory issues for too long, plus it'll be faster! |
|
| 34 |
*/ |
|
| 27 | 35 |
void handle_receive(char type, int source, unsigned char* packet, int length){
|
| 36 |
|
|
| 28 | 37 |
char buf[MAX_PACKET_LENGTH]; |
| 29 | 38 |
int size, i; |
| 30 | 39 |
usb_puts("Got a packet!\n\r");
|
| 31 | 40 |
switch(type){
|
| 32 | 41 |
case BOM_TYPE: |
| 33 |
size = generate_bom_packet(&buf); |
|
| 34 |
wl_send_robot_to_robot_global_packet(DATA_REQUEST_GROUP,BOM_TYPE,buf,size,source,0); |
|
| 42 |
|
|
| 43 |
short int i=0, robot_id; |
|
| 44 |
wl_token_iterator_begin(); |
|
| 45 |
while(wl_token_iterator_has_next() && i<=96){
|
|
| 46 |
robot_id = wl_token_iterator_next(); |
|
| 47 |
(*buf)[i++] = (char)((robot_id>>8) & 255); |
|
| 48 |
(*buf)[i++] = (char)(robot_id & 255); |
|
| 49 |
//TODO Ensure that wl_token_get.... returns the expected char. |
|
| 50 |
(*buf)[i++] = (char)(wl_token_get_my_sensor_reading(robot_id)); |
|
| 51 |
} |
|
| 52 |
|
|
| 53 |
//i is one greater than the index of the last byte... the size. |
|
| 54 |
wl_send_robot_to_robot_global_packet(DATA_REQUEST_GROUP,BOM_TYPE,buf,i,source,0); |
|
| 35 | 55 |
|
| 36 |
|
|
| 37 | 56 |
case IR_TYPE: |
| 38 | 57 |
|
| 39 | 58 |
usb_puts("Generating an IR packet.\n");
|
| ... | ... | |
| 65 | 84 |
} |
| 66 | 85 |
} |
| 67 | 86 |
|
| 87 |
/** |
|
| 88 |
* @brief Generates a BOM Data packet. **NO LONGER USED** Delete once this code is used often. |
|
| 89 |
*/ |
|
| 68 | 90 |
int generate_bom_packet(char** buf){
|
| 69 | 91 |
short int i=0, robot_id; |
| 70 | 92 |
wl_token_iterator_begin(); |
| ... | ... | |
| 78 | 100 |
return i; |
| 79 | 101 |
} |
| 80 | 102 |
|
| 103 |
/** |
|
| 104 |
* @brief Generates an IR Packet **NO LONGER USED** Delete once this code is used often. |
|
| 105 |
* |
|
| 106 |
*/ |
|
| 81 | 107 |
int generate_IR_packet(char** buf){
|
| 82 | 108 |
usb_puts("Generating an IR packet.\n");
|
| 83 | 109 |
char ranges[] = {IR1,IR2,IR3,IR4,IR5};
|
Also available in: Unified diff