Project

General

Profile

Statistics
| Revision:

root / trunk / code / projects / mapping / matlab / testRobot / test.c @ 1032

History | View | Annotate | Download (2.7 KB)

1
/** This test should just have the robot stay in place,
2
 * register the token ring, participate in the token ring,
3
 * and reply to packets when asked by the computer.*/
4

    
5
#include <dragonfly_lib.h>
6
#include <wireless.h>
7
#include <wl_token_ring.h>
8
#include <encoders.h>
9
#include <bom.h>
10
#include "odometry.h"
11

    
12
#define MAP_REQUEST_GROUP 23
13

    
14
#define DATA_SERVER_IDENTIFY 5
15
#define DATA_SERVER_REPLY 4
16
#define DATA_POINT 2 
17
#define DATA_REQUEST 3
18

    
19
#define WL_CHANNEL 0xE
20

    
21
int state; int velocity;
22
int server;
23
char buf[100];
24

    
25
void respond(char type, int source, unsigned char* packet, int length);
26
void transmit(void);
27

    
28
PacketGroupHandler requestHandler = {MAP_REQUEST_GROUP, 
29
        NULL, NULL, respond, NULL};
30

    
31
int main(void)
32
{
33
        server = -1;
34
                
35
    /* initialize components and join the token ring */
36
    dragonfly_init(RANGE | BOM | COMM);
37
        
38
        usb_init(); usb_puts("usb turned on\r\n");
39
                
40
    //odometry_init();
41
    
42
        wl_init(); usb_puts("wireless turned on\r\n");
43
    wl_set_channel(WL_CHANNEL);
44
    wl_register_packet_group(&requestHandler);
45
    
46
    wl_token_ring_register();
47
        wl_token_ring_set_bom_functions(transmit, bom_off, get_max_bom);
48
    wl_token_ring_join();
49
        usb_puts("token ring joined\r\n");
50

    
51
        char buf[500];
52
        
53
        int prescalar = 0;
54

    
55
    while(1)
56
    {
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
                }
64
    }
65

    
66
        usb_puts("Terminating\r\n");
67

    
68
    wl_terminate();
69
    return 0;
70
}
71

    
72
void respond(char type, int source, unsigned char* packet, int length) {
73
           if(type==DATA_REQUEST){ 
74
                sprintf(buf, "received packet: %d\n", type);
75
            usb_puts(buf);
76
        
77
                int store[9];
78
                store[0] = 0;
79
            store[1] = 0;
80
            *((double*)(store + 2)) = 5.0;
81
            store[4] = 125;        /* IR1 range*/
82
                store[5] = 125;        // IR2 range
83
                store[6] = 125;        // IR3 range
84
                store[7] = 125;        // IR4 range
85
                store[8] = 125;        // IR5 range
86

    
87
                char* data = (char*)store;
88

    
89
                wl_send_robot_to_robot_global_packet(MAP_REQUEST_GROUP, 
90
                        DATA_POINT, data, 18, source, 0);
91
                delay_ms(50);
92
        }
93
        else if(type==DATA_SERVER_REPLY){
94
                server = source;
95
        }
96
}
97

    
98
void transmit(){
99
        /*If a server hasn't declared itself, 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{
106
        
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
116

    
117
                char* data = (char*)store;
118

    
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
        }
125
}
126

    
127

    
128