Project

General

Profile

Revision 965

Cleaned up test.c and robot_main.c in mapping.

View differences:

trunk/code/projects/mapping/robot/robot_main.c
5 5

  
6 6
#define POLLING_INTERVAL 50    //interval in ms to get sensor data
7 7
#define MAP 1	// packet group for sending data points
8
#define POINT_RAW 1 // packet type for map data points w/ raw encoder data
9
#define POINT_ODO 2 // packet type for map data points w/ odometry data
8
#define POINT_ODO 2
10 9

  
11 10
#define WL_CNTL_GROUP 4
12 11

  
......
28 27
void set_motion (void);
29 28

  
30 29
PacketGroupHandler packetHandler = {WL_CNTL_GROUP, NULL, NULL, &packet_receive, NULL};
31
PacketGroupHandler love_handle = {MAP, NULL, NULL, NULL, NULL};
32 30

  
33 31
int main(void)
34 32
{
35 33
    /* initialize components and join the token ring */
36 34
    dragonfly_init(ALL_ON);
37
    //range_init();
38
    //encoders_init();
39 35
    odometry_init();
36
    orb_enable();
40 37
    wl_init();
41 38
    wl_set_channel(0xE);
42
    wl_register_packet_group(&love_handle);
43 39
    wl_register_packet_group(&packetHandler);
44
    orb_enable();
45
    //data_response_init();
46
    //wl_token_ring_register();
47
    //wl_token_ring_join();
48
    //run_around_init();
49

  
50
    //usb_puti(xbee_get_address());
51
    //return 0;
52

  
40
    
53 41
    velocity = 190;
54 42
    state = CNTL_STOP;
55 43

  
......
62 50
    while(1)
63 51
    {
64 52
	wl_do();
65
	//run_around_FSM();	/* traverse the environment and avoid obstacles ... */
66 53
	delay_ms(POLLING_INTERVAL);	/* until it's time to grab map data */
67 54
	x = odometry_dx(); // use when sending odometry data
68 55
	y = odometry_dy();
69 56
	theta = odometry_angle();
70
	//store[0] = encoder_get_dx(LEFT);    // left encoder distance
71
	//store[1] = encoder_get_dx(RIGHT);   // right encoder distance
72 57
	store[0] = x;
73
    store[1] = y;
74
    *((double*)(store + 2)) = theta;
75
    store[4] = range_read_distance(IR1);	// IR1 range
58
	store[1] = y;
59
	*((double*)(store + 2)) = theta;
60
	store[4] = range_read_distance(IR1);	// IR1 range
76 61
	store[5] = range_read_distance(IR2);	// IR2 range
77 62
	store[6] = range_read_distance(IR3);	// IR3 range
78 63
	store[7] = range_read_distance(IR4);	// IR4 range
79 64
	store[8] = range_read_distance(IR5);	// IR5 range
80
    sprintf( buf, "%d %d %f %d %d %d %d %d\n", store[0], store[1],
81
          *((double *)(store+2)), store[4], store[5], store[6],
82
          store[7], store[8] );
83
    usb_puts(buf);
65
	sprintf( buf, "%d %d %f %d %d %d %d %d\n", store[0], store[1],
66
	    *((double *)(store+2)), store[4], store[5], store[6],
67
	    store[7], store[8] );
68
	usb_puts(buf);
84 69

  
85 70
	send = (char*) store;
86 71

  
87
	// send point w/ raw encoder data
88
	// wl_send_global_packet(MAP, POINT_RAW, send, 14, 0);
89

  
90
	// send point w/ odometry data
91 72
	wl_send_global_packet(MAP, POINT_ODO, send, 18, 0);
92
	
93 73
    }
94 74

  
95 75
    wl_terminate();
trunk/code/projects/mapping/server/test.c
24 24
void packet_receive(char type, int source, unsigned char* packet, int length);
25 25

  
26 26
PacketGroupHandler cntlHandler = {WL_CNTL_GROUP, NULL, NULL, NULL, NULL};
27
PacketGroupHandler receiveHandler = {1, NULL, NULL, &packet_receive, NULL};
27
PacketGroupHandler receiveHandler = {MAP, NULL, NULL, &packet_receive, NULL};
28 28

  
29 29
FILE *file;
30 30
int parent_running = 1;
......
108 108
void send_packet (char type, int dst_robot) {
109 109
    wl_send_robot_to_robot_global_packet(WL_CNTL_GROUP, type,
110 110
            NULL, 0, dst_robot, 0);
111
    //wl_send_global_packet(WL_CNTL_GROUP, type, NULL, 0, 0);
112 111
}
113 112

  
114 113
void packet_receive (char type, int source, unsigned char* packet, int length) {
115
    if (type == POINT_RAW) {
116
        //expected input: enc_l enc_r IR1 IR2 IR3 IR4 IR5
117
        int i, temp;
118
        for (i = 0; i < length; i += 2) {
119
            temp = (char)packet[i+1] << 8;
120
            temp |= (char)packet[i];
121
            fprintf(file, "%d ", temp);
122
        }
123
        fprintf(file, "\n");
124
    }
125
    //else if (type == POINT_ODO) {
126
        //printf("received packet!\r\n");
127
        // expected input: x y theta(double) IR1 IR2 IR3 IR4 IR5
128
        short x,y,ir1,ir2,ir3,ir4,ir5;
129
        //int theta_temp;
130
        x = ((short)packet[1] << 8) | (short)packet[0];
131
        y = ((short)packet[3] << 8) | (short)packet[2];
132
        ir1 = ((short)packet[9] << 8) | (short)packet[8];
133
        ir2 = ((short)packet[11] << 8) | (short)packet[10];
134
        ir3 = ((short)packet[13] << 8) | (short)packet[12];
135
        ir4 = ((short)packet[15] << 8) | (short)packet[14];
136
        ir5 = ((short)packet[17] << 8) | (short)packet[16];
114
    short x,y,ir1,ir2,ir3,ir4,ir5;
137 115

  
138
        char tarr[] = {packet[4],packet[5],packet[6],packet[7]};
139
        float *theta_ptr = (float *)tarr;
140
        float theta = *theta_ptr;
116
    /* convert received data from 2 chars to 1 short */
117
    x = ((short)packet[1] << 8) | (short)packet[0];
118
    y = ((short)packet[3] << 8) | (short)packet[2];
119
    ir1 = ((short)packet[9] << 8) | (short)packet[8];
120
    ir2 = ((short)packet[11] << 8) | (short)packet[10];
121
    ir3 = ((short)packet[13] << 8) | (short)packet[12];
122
    ir4 = ((short)packet[15] << 8) | (short)packet[14];
123
    ir5 = ((short)packet[17] << 8) | (short)packet[16];
141 124

  
142
        //fprintf(file, "theta = %f\n", theta);
143
        fprintf(file, "%d %d %f %d %d %d %d %d\n", x, y, theta,
144
                ir1, ir2, ir3, ir4, ir5);
145
        //printf("%d %d %f %d %d %d %d %d\n", x, y, theta,
146
        //        ir1, ir2, ir3, ir4, ir5);
147
        //fflush(stdout);
148
    //}
125
    char tarr[] = {packet[4],packet[5],packet[6],packet[7]};
126
    float *theta_ptr = (float *)tarr;
127
    float theta = *theta_ptr;
128

  
129
    fprintf(file, "%d %d %f %d %d %d %d %d\n", x, y, theta,
130
	    ir1, ir2, ir3, ir4, ir5);
149 131
}

Also available in: Unified diff