Project

General

Profile

Revision 965

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

View differences:

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();

Also available in: Unified diff