Project

General

Profile

Statistics
| Revision:

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

History | View | Annotate | Download (1.6 KB)

1 960 justin
/** 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 953 justin
#include <dragonfly_lib.h>
6
#include <wireless.h>
7
#include <encoders.h>
8
9 960 justin
#define MAP_RECEIVE_GROUP 17
10
#define MAP_REQUEST_GROUP 23
11 953 justin
12
#define POINT_RAW 1 // packet type for map data points w/ raw encoder data
13
#define POINT_ODO 2 // packet type for map data points w/ odometry data
14
15
#define WL_CNTL_GROUP 4
16
17
int state;
18
int velocity;
19
char buf[100];
20
21
void respond(char type, int source, unsigned char* packet, int length);
22
23
PacketGroupHandler receiveHandler = {MAP_REQUEST_GROUP,
24
        NULL, NULL, NULL, NULL};
25
26
PacketGroupHandler requestHandler = {MAP_RECEIVE_GROUP,
27
        NULL, NULL, respond, NULL};
28
29
int main(void)
30
{
31
    /* initialize components and join the token ring */
32
    dragonfly_init(ALL_ON);
33
    odometry_init();
34
35
        wl_init();
36
    wl_set_channel(0xE);
37
    wl_register_packet_group(&receiveHandler);
38
    wl_register_packet_group(&requestHandler);
39
40
    wl_token_ring_register();
41
    wl_token_ring_join();
42
43
    while(1)
44
    {
45
                wl_do();
46
    }
47
48
    wl_terminate();
49
    return 0;
50
}
51
52
void respond(char type, int source, unsigned char* packet, int length) {
53
    sprintf(buf, "received packet: %d\n", type);
54
    usb_puts(buf);
55
56
        int store[9];
57
        store[0] = 0;
58
    store[1] = 0;
59
    *((double*)(store + 2)) = 5.0;
60
    store[4] = 125;        // IR1 range
61
        store[5] = 125;        // IR2 range
62
        store[6] = 125;        // IR3 range
63
        store[7] = 125;        // IR4 range
64
        store[8] = 125;        // IR5 range
65
66
        wl_send_robot_to_robot_global_packet(MAP_RECEIVE_GROUP,
67
                POINT_ODO, &store, 0, source, 0);
68
69
}
70 960 justin