Revision 979
minor changes to remote control / mapping code...wireless control works with libwireless reverted to rev. 887. code in odometry interrupt conflicts with motors, not allowing the robot to move motors in response to commands.
trunk/code/projects/mapping/odometry/odometry.c | ||
---|---|---|
5 | 5 |
#include <avr/interrupt.h> |
6 | 6 |
#include <dragonfly_lib.h> |
7 | 7 |
|
8 |
void modify_velocity(void); |
|
9 |
|
|
10 |
|
|
8 | 11 |
long lround(double d); |
9 | 12 |
|
10 | 13 |
char control_velocity = 0; |
... | ... | |
148 | 151 |
} |
149 | 152 |
|
150 | 153 |
void modify_velocity(){ |
151 |
int diff = target_velocity - velocity; |
|
154 |
usb_puts("modify_velocity\n"); |
|
155 |
int diff = target_velocity - velocity; |
|
152 | 156 |
if(diff > 0){ |
153 | 157 |
move_speed++; |
154 | 158 |
move(move_speed,move_angle); |
... | ... | |
160 | 164 |
} |
161 | 165 |
|
162 | 166 |
ISR(TIMER2_COMP_vect){ |
163 |
odometry_run(); |
|
167 |
//odometry_run();
|
|
164 | 168 |
} |
165 | 169 |
|
166 | 170 |
long lround(double d){ |
trunk/code/projects/mapping/robot/robot_main.c | ||
---|---|---|
3 | 3 |
#include <encoders.h> |
4 | 4 |
#include "odometry.h" |
5 | 5 |
|
6 |
#define CHAN 0xE |
|
7 |
|
|
6 | 8 |
#define POLLING_INTERVAL 50 //interval in ms to get sensor data |
7 | 9 |
#define MAP 1 // packet group for sending data points |
8 | 10 |
#define POINT_ODO 2 |
... | ... | |
32 | 34 |
{ |
33 | 35 |
/* initialize components and join the token ring */ |
34 | 36 |
dragonfly_init(ALL_ON); |
35 |
odometry_init(); |
|
37 |
odometry_init(); // breaks motors (interrupt-related?)
|
|
36 | 38 |
orb_enable(); |
37 | 39 |
wl_init(); |
38 |
wl_set_channel(0xE);
|
|
40 |
wl_set_channel(CHAN);
|
|
39 | 41 |
wl_register_packet_group(&packetHandler); |
40 |
|
|
42 |
|
|
43 |
// initial control values |
|
41 | 44 |
velocity = 190; |
42 | 45 |
state = CNTL_STOP; |
43 |
|
|
44 |
int x, y; // use when sending odometry data |
|
46 |
|
|
47 |
// use when sending odometry data |
|
48 |
int x, y; |
|
45 | 49 |
double theta; |
46 | 50 |
|
47 | 51 |
int store[9]; |
... | ... | |
49 | 53 |
|
50 | 54 |
while(1) |
51 | 55 |
{ |
52 |
wl_do();
|
|
53 |
delay_ms(POLLING_INTERVAL); /* until it's time to grab map data */
|
|
54 |
x = odometry_dx(); // use when sending odometry data
|
|
55 |
y = odometry_dy();
|
|
56 |
theta = odometry_angle();
|
|
57 |
store[0] = x;
|
|
58 |
store[1] = y;
|
|
59 |
*((double*)(store + 2)) = theta;
|
|
60 |
store[4] = range_read_distance(IR1); // IR1 range
|
|
61 |
store[5] = range_read_distance(IR2); // IR2 range
|
|
62 |
store[6] = range_read_distance(IR3); // IR3 range
|
|
63 |
store[7] = range_read_distance(IR4); // IR4 range
|
|
64 |
store[8] = range_read_distance(IR5); // IR5 range
|
|
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);
|
|
56 |
wl_do();
|
|
57 |
delay_ms(POLLING_INTERVAL); // until it's time to grab map data
|
|
58 |
x = odometry_dx(); // use when sending odometry data
|
|
59 |
y = odometry_dy();
|
|
60 |
theta = odometry_angle();
|
|
61 |
store[0] = x;
|
|
62 |
store[1] = y;
|
|
63 |
*((double*)(store + 2)) = theta;
|
|
64 |
store[4] = range_read_distance(IR1); // IR1 range
|
|
65 |
store[5] = range_read_distance(IR2); // IR2 range
|
|
66 |
store[6] = range_read_distance(IR3); // IR3 range
|
|
67 |
store[7] = range_read_distance(IR4); // IR4 range
|
|
68 |
store[8] = range_read_distance(IR5); // IR5 range
|
|
69 |
sprintf( buf, "%d %d %f %d %d %d %d %d\n", store[0], store[1],
|
|
70 |
*((double *)(store+2)), store[4], store[5], store[6],
|
|
71 |
store[7], store[8] );
|
|
72 |
usb_puts(buf);
|
|
69 | 73 |
|
70 |
send = (char*) store;
|
|
74 |
send = (char*) store;
|
|
71 | 75 |
|
72 |
wl_send_global_packet(MAP, POINT_ODO, send, 18, 0);
|
|
76 |
wl_send_global_packet(MAP, POINT_ODO, send, 18, 0);
|
|
73 | 77 |
} |
74 | 78 |
|
75 | 79 |
wl_terminate(); |
trunk/code/projects/mapping/server/test.c | ||
---|---|---|
4 | 4 |
#include "../../libwireless/lib/wireless.h" |
5 | 5 |
#include <unistd.h> |
6 | 6 |
|
7 |
#define CHAN 0xE |
|
8 |
|
|
7 | 9 |
#define MAP 1 // packet group for receiving points |
8 | 10 |
#define POINT_RAW 1 // packet type for map data points w/ raw encoder data |
9 | 11 |
#define POINT_ODO 2 // packet type for map data points w/ odometry data |
10 | 12 |
|
11 | 13 |
#define WL_CNTL_GROUP 4 |
12 | 14 |
|
13 |
#define CNTL_FORWARD 0 |
|
14 |
#define CNTL_BACK 1 |
|
15 |
#define CNTL_LEFT 2 |
|
16 |
#define CNTL_RIGHT 3 |
|
17 |
#define CNTL_LEFT_CURVE 4 |
|
18 |
#define CNTL_RIGHT_CURVE 5 |
|
19 |
#define CNTL_STOP 6 |
|
20 |
#define CNTL_VEL_UP 7 |
|
21 |
#define CNTL_VEL_DOWN 8 |
|
15 |
#define CNTL_FORWARD 0
|
|
16 |
#define CNTL_BACK 1
|
|
17 |
#define CNTL_LEFT 2
|
|
18 |
#define CNTL_RIGHT 3
|
|
19 |
#define CNTL_LEFT_CURVE 4
|
|
20 |
#define CNTL_RIGHT_CURVE 5
|
|
21 |
#define CNTL_STOP 6
|
|
22 |
#define CNTL_VEL_UP 7
|
|
23 |
#define CNTL_VEL_DOWN 8
|
|
22 | 24 |
|
23 |
void send_packet (char color, int dst_robot);
|
|
25 |
void send_packet (char type, int dst_robot);
|
|
24 | 26 |
void packet_receive(char type, int source, unsigned char* packet, int length); |
25 | 27 |
|
26 | 28 |
PacketGroupHandler cntlHandler = {WL_CNTL_GROUP, NULL, NULL, NULL, NULL}; |
... | ... | |
39 | 41 |
int robot = atoi(argv[1]); |
40 | 42 |
|
41 | 43 |
printf("Beginning: robot=%d\r\n", robot); |
42 |
wl_init();
|
|
44 |
wl_init();
|
|
43 | 45 |
wl_set_com_port("/dev/ttyUSB1"); |
44 |
wl_set_channel(0xE);
|
|
46 |
wl_set_channel(CHAN);
|
|
45 | 47 |
printf("Wireless initialized.\r\n"); |
46 | 48 |
wl_register_packet_group(&cntlHandler); |
47 | 49 |
wl_register_packet_group(&receiveHandler); |
Also available in: Unified diff