Statistics
| Revision:

root / branches / colonetmk2 / code / projects / swarm / test.cpp @ 1456

History | View | Annotate | Download (3.07 KB)

1
#include <colonet_wireless.h>
2
#include <wireless.h>
3
#include <stdio.h>
4
#include <string.h>
5
#include <stdlib.h>
6
#include <signal.h>
7
#include <unistd.h>
8

    
9
#include <pthread.h>
10

    
11
#define SWARM_PACKET_GROUP_ID 0x8
12

    
13
static PacketGroupHandler colonet_pgh;
14
static void colonet_handle_receive(char type, int wl_source, unsigned char* packet, int length);
15

    
16
void cleanup(int a)
17
{
18
    a = a;
19

    
20
    wl_terminate();
21
    printf("todo - close wl port\n");
22

    
23
    exit(0);
24
}
25

    
26
int main(int argc, char* argv[])
27
{
28
    struct sigaction act;
29
    memset(&act, 0, sizeof(struct sigaction));
30
    act.sa_handler = cleanup;
31

    
32
    if (argc < 2) return -1;
33

    
34
    //register a signal handler to be called when we get a SIGINT signal (killing the server by cntrl+c would generate this)
35
    sigaction(SIGINT, &act, NULL);
36

    
37
    char *wireless_port = argv[1];
38

    
39
    //initialize the wireless library giving it the port and the log file name
40
    if (colonet_wl_init(wireless_port) != 0) {
41
        printf("colonet_wl_init failed.\n");
42
        return -1;
43
    }
44

    
45
    colonet_pgh.groupCode = SWARM_PACKET_GROUP_ID;
46
    colonet_pgh.timeout_handler = NULL;
47
    colonet_pgh.handle_response = NULL;
48
    colonet_pgh.handle_receive = colonet_handle_receive;
49
    colonet_pgh.unregister = NULL;
50
    wl_register_packet_group(&colonet_pgh);
51

    
52
    //call the function that will start the thread that will listen for wireless messages
53
    if (colonet_wl_run_listener_thread()) {
54
        printf("colonet_wl_run_listener_thread failed.\n");
55
        return -1;
56
    }
57

    
58
    unsigned char i, j;
59
    int numRobots = 1;
60
    //int* robots = (int *)malloc(0);
61
    int robots[] = {13};
62

    
63
    for(i=0; ; i++)
64
    {
65
        if (i==0)
66
        {
67
            //numRobots = colonet_get_num_robots();
68
            //robots = (int *)realloc(robots, sizeof(int) * numRobots);
69
            //colonet_get_xbee_ids(robots);
70
        }
71

    
72
        /*for (j=0; j < numRobots; j++)
73
        {
74
            usleep(1000);
75
            
76
            printf("\rSending %d to %d    ", i, robots[j]);
77

78
            char packet[] = { i, i, i, i };
79
            wl_send_robot_to_robot_packet(SWARM_PACKET_GROUP_ID, 0, packet, sizeof(char) * 4, robots[j], 0);
80
        }*/
81

    
82
        pthread_yield();
83
    }
84

    
85
    //colonet_wl_join();
86
    printf("Exiting normally\n");
87

    
88
    return 0;
89
}
90

    
91
unsigned char count[] = { -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1};
92

    
93
static void colonet_handle_receive(char type, int wl_source, unsigned char* packet, int length)
94
{
95
    type = type;
96
    
97
    if (length != 17)
98
    {
99
        printf("Bad packet length from %d\n", wl_source);
100
        return;
101
    }
102

    
103
    for (int i = 0; i < length; i++)
104
    {
105
        /*if (packet[i] != i)
106
        {
107
            printf("Corrupted packet from %d\n", wl_source);
108
            break;
109
        }*/
110
        printf("%d ", packet[i]);
111
    }
112
    printf("\n");
113

    
114
    if (packet[16]!=++count[wl_source])
115
        printf("Bad packet number from %d (%d, should be %d)\n", wl_source, packet[16], count[wl_source]);
116

    
117
    if (packet[16] == 0)
118
    {
119
        printf("Packet 0 received from %d\n", wl_source);
120
    }
121

    
122
    count[wl_source] = packet[16];
123

    
124
    fflush(stdout);
125
}