Revision 1032
Having trouble with wireless (I think) hopefully this will work by friday.
| test.c | ||
|---|---|---|
| 11 | 11 |
|
| 12 | 12 |
#define MAP_REQUEST_GROUP 23 |
| 13 | 13 |
|
| 14 |
#define DATA_INIT 4 |
|
| 14 |
#define DATA_SERVER_IDENTIFY 5 |
|
| 15 |
#define DATA_SERVER_REPLY 4 |
|
| 15 | 16 |
#define DATA_POINT 2 |
| 16 | 17 |
#define DATA_REQUEST 3 |
| 17 | 18 |
|
| 18 |
#define WL_CHANNEL = 0xE
|
|
| 19 |
#define WL_CHANNEL 0xE |
|
| 19 | 20 |
|
| 20 |
int state; |
|
| 21 |
int velocity; |
|
| 22 |
int server = -1; |
|
| 21 |
int state; int velocity; |
|
| 22 |
int server; |
|
| 23 | 23 |
char buf[100]; |
| 24 | 24 |
|
| 25 | 25 |
void respond(char type, int source, unsigned char* packet, int length); |
| ... | ... | |
| 30 | 30 |
|
| 31 | 31 |
int main(void) |
| 32 | 32 |
{
|
| 33 |
server = -1; |
|
| 34 |
|
|
| 33 | 35 |
/* initialize components and join the token ring */ |
| 34 | 36 |
dragonfly_init(RANGE | BOM | COMM); |
| 35 |
odometry_init(); |
|
| 37 |
|
|
| 38 |
usb_init(); usb_puts("usb turned on\r\n");
|
|
| 39 |
|
|
| 40 |
//odometry_init(); |
|
| 36 | 41 |
|
| 37 |
wl_init(); |
|
| 42 |
wl_init(); usb_puts("wireless turned on\r\n");
|
|
| 38 | 43 |
wl_set_channel(WL_CHANNEL); |
| 39 | 44 |
wl_register_packet_group(&requestHandler); |
| 40 | 45 |
|
| 41 | 46 |
wl_token_ring_register(); |
| 42 | 47 |
wl_token_ring_set_bom_functions(transmit, bom_off, get_max_bom); |
| 43 | 48 |
wl_token_ring_join(); |
| 49 |
usb_puts("token ring joined\r\n");
|
|
| 44 | 50 |
|
| 51 |
char buf[500]; |
|
| 52 |
|
|
| 53 |
int prescalar = 0; |
|
| 54 |
|
|
| 45 | 55 |
while(1) |
| 46 | 56 |
{
|
| 47 | 57 |
wl_do(); |
| 58 |
|
|
| 59 |
if(!(++prescalar%1000)){
|
|
| 60 |
sprintf(buf, "# Robots = %d, # Robots in matrix = %d\r\n", |
|
| 61 |
wl_token_get_robots_in_ring(), wl_token_get_num_robots()); |
|
| 62 |
usb_puts(buf); |
|
| 63 |
} |
|
| 48 | 64 |
} |
| 49 | 65 |
|
| 66 |
usb_puts("Terminating\r\n");
|
|
| 67 |
|
|
| 50 | 68 |
wl_terminate(); |
| 51 | 69 |
return 0; |
| 52 | 70 |
} |
| ... | ... | |
| 60 | 78 |
store[0] = 0; |
| 61 | 79 |
store[1] = 0; |
| 62 | 80 |
*((double*)(store + 2)) = 5.0; |
| 63 |
store[4] = 125; // IR1 range
|
|
| 81 |
store[4] = 125; /* IR1 range*/
|
|
| 64 | 82 |
store[5] = 125; // IR2 range |
| 65 | 83 |
store[6] = 125; // IR3 range |
| 66 | 84 |
store[7] = 125; // IR4 range |
| ... | ... | |
| 70 | 88 |
|
| 71 | 89 |
wl_send_robot_to_robot_global_packet(MAP_REQUEST_GROUP, |
| 72 | 90 |
DATA_POINT, data, 18, source, 0); |
| 91 |
delay_ms(50); |
|
| 73 | 92 |
} |
| 74 |
else if(type==DATA_INIT){
|
|
| 93 |
else if(type==DATA_SERVER_REPLY){
|
|
| 75 | 94 |
server = source; |
| 76 | 95 |
} |
| 77 | 96 |
} |
| 78 | 97 |
|
| 79 | 98 |
void transmit(){
|
| 80 |
bom_on(); |
|
| 81 |
|
|
| 82 | 99 |
/*If a server hasn't declared itself, return.*/ |
| 83 |
if(server == -1) return; |
|
| 100 |
if(server == -1){
|
|
| 101 |
wl_send_global_packet(MAP_REQUEST_GROUP, |
|
| 102 |
DATA_SERVER_IDENTIFY, NULL, 0, 0); |
|
| 103 |
delay_ms(50); |
|
| 104 |
} |
|
| 105 |
else{
|
|
| 84 | 106 |
|
| 85 |
int store[9]; |
|
| 86 |
store[0] = 0; //X |
|
| 87 |
store[1] = 0; //Y |
|
| 88 |
*((double*)(store + 2)) = 5.0; //Theta. |
|
| 89 |
store[4] = 125; // IR1 range |
|
| 90 |
store[5] = 125; // IR2 range |
|
| 91 |
store[6] = 125; // IR3 range |
|
| 92 |
store[7] = 125; // IR4 range |
|
| 93 |
store[8] = 125; // IR5 range |
|
| 107 |
int store[9];
|
|
| 108 |
store[0] = 0; //X
|
|
| 109 |
store[1] = 0; //Y
|
|
| 110 |
*((double*)(store + 2)) = 5.0; //Theta.
|
|
| 111 |
store[4] = 125; // IR1 range
|
|
| 112 |
store[5] = 125; // IR2 range
|
|
| 113 |
store[6] = 125; // IR3 range
|
|
| 114 |
store[7] = 125; // IR4 range
|
|
| 115 |
store[8] = 125; // IR5 range
|
|
| 94 | 116 |
|
| 95 |
char* data = (char*)store; |
|
| 117 |
char* data = (char*)store;
|
|
| 96 | 118 |
|
| 97 |
wl_send_robot_to_robot_global_packet(MAP_REQUEST_GROUP, |
|
| 98 |
DATA_POINT, data, 18, server, 0); |
|
| 119 |
wl_send_robot_to_robot_global_packet(MAP_REQUEST_GROUP, |
|
| 120 |
DATA_POINT, data, 18, server, 0); |
|
| 121 |
delay_ms(50); |
|
| 122 |
|
|
| 123 |
bom_on(); |
|
| 124 |
} |
|
| 99 | 125 |
} |
| 100 | 126 |
|
| 101 | 127 |
|
Also available in: Unified diff