Revision 915
updated robot and server code to use odometry. having problems with theta because it is a double (4 bytes on robot, 8 bytes on server)
trunk/code/projects/mapping/robot/odometry.c | ||
---|---|---|
1 |
#include "odometry.h" |
|
2 |
#include <encoders.h> |
|
3 |
#include <math.h> |
|
4 |
#include <avr/interrupt.h> |
|
5 |
#include <dragonfly_lib.h> |
|
6 |
|
|
7 |
long encoder_ltm1, encoder_rtm1; |
|
8 |
|
|
9 |
long diff_x,diff_y; |
|
10 |
double angle; |
|
11 |
|
|
12 |
|
|
13 |
int odometry_dx(void){ |
|
14 |
return diff_x; |
|
15 |
} |
|
16 |
|
|
17 |
int odometry_dy(void){ |
|
18 |
return diff_y; |
|
19 |
} |
|
20 |
|
|
21 |
double odometry_angle(void){ |
|
22 |
return angle; |
|
23 |
} |
|
24 |
|
|
25 |
void odometry_init(void){ |
|
26 |
encoders_init(); |
|
27 |
odometry_reset(); |
|
28 |
|
|
29 |
//CTC Mode. CLK / 1024 |
|
30 |
TCCR2 = 0; // (Fully disconnected) |
|
31 |
TCCR2 = _BV(WGM21) | _BV(CS22) | _BV(CS20); //CLK/1024 , CTC Mode. |
|
32 |
|
|
33 |
TIMSK |= _BV(OCIE2); |
|
34 |
|
|
35 |
OCR2 = ODOMETRY_CLK; |
|
36 |
} |
|
37 |
|
|
38 |
void odometry_reset(void){ |
|
39 |
diff_x = 0; |
|
40 |
diff_y = 0; |
|
41 |
angle = 0.0; |
|
42 |
} |
|
43 |
|
|
44 |
/*Currently assumes robot only goes forward.*/ |
|
45 |
void odometry_run(void){ |
|
46 |
double theta; |
|
47 |
int encoder_left, encoder_right,dl,dr; |
|
48 |
long distance_um; |
|
49 |
char buf[100]; |
|
50 |
//Relative to the inner wheel, the radius of its turn. |
|
51 |
double turning_radius; |
|
52 |
|
|
53 |
encoder_left = encoder_read(LEFT); |
|
54 |
encoder_right = encoder_read(RIGHT); |
|
55 |
|
|
56 |
dl = encoder_left - encoder_ltm1; |
|
57 |
dr = encoder_right - encoder_rtm1; |
|
58 |
|
|
59 |
dl = dl > 512 ? dl - 1024 :dl; |
|
60 |
dl = dl < -512 ? dl + 1024 :dl; |
|
61 |
dr = dr > 512 ? dr - 1024 :dr; |
|
62 |
dr = dr < -512 ? dr + 1024 :dr; |
|
63 |
|
|
64 |
//idle? |
|
65 |
if(dl < 2 && dr < 2) return; |
|
66 |
|
|
67 |
if(dl == dr){ |
|
68 |
distance_um = dl*CLICK_DISTANCE_UM; |
|
69 |
//sprintf(buf,"Distance in um: %ld\n\r",distance_um); |
|
70 |
//usb_puts(buf); |
|
71 |
diff_x += distance_um*cos(angle)/1000; |
|
72 |
diff_y += distance_um*sin(angle)/1000; |
|
73 |
return; |
|
74 |
} |
|
75 |
|
|
76 |
if(dr > dl){ //CCW positive. |
|
77 |
turning_radius = ROBOT_WIDTH_UM * dl / (dr - dl); //um |
|
78 |
theta = ((double)(dr-dl)*CLICK_DISTANCE_UM) / ((double)ROBOT_WIDTH_UM); |
|
79 |
distance_um = theta*((double)(turning_radius + ROBOT_WIDTH_UM/2)); |
|
80 |
//sprintf(buf,"Distance in um: %d\n\r",distance_um); |
|
81 |
//usb_puts(buf); |
|
82 |
} |
|
83 |
|
|
84 |
else{ //CW negative. |
|
85 |
turning_radius = ROBOT_WIDTH_UM * dr / (dl - dr); //um |
|
86 |
theta = -((double)(dl-dr)*CLICK_DISTANCE_UM) / ((double)ROBOT_WIDTH_UM); |
|
87 |
distance_um = -theta*((double)(turning_radius + ROBOT_WIDTH_UM/2)); |
|
88 |
//sprintf(buf,"Distance in um: %d\n\r",distance_um); |
|
89 |
//usb_puts(buf); |
|
90 |
|
|
91 |
} |
|
92 |
|
|
93 |
diff_x += distance_um * cos(angle)/1000; |
|
94 |
diff_y += distance_um * sin(angle)/1000; |
|
95 |
|
|
96 |
angle += theta; |
|
97 |
|
|
98 |
encoder_ltm1 = encoder_left; |
|
99 |
encoder_rtm1 = encoder_right; |
|
100 |
} |
|
101 |
|
|
102 |
ISR(TIMER2_COMP_vect){ |
|
103 |
odometry_run(); |
|
104 |
} |
|
105 |
|
|
106 |
|
trunk/code/projects/mapping/robot/odometry.h | ||
---|---|---|
1 |
|
|
2 |
#ifndef __ODOMETRY_C__ |
|
3 |
#define __ODOMETRY_C__ |
|
4 |
|
|
5 |
//Odometry resolution, *64 microseconds. |
|
6 |
//= approx. 100 ms |
|
7 |
#define ODOMETRY_CLK 255u |
|
8 |
|
|
9 |
//Wheel = 2.613 in. |
|
10 |
//Circumference = 208.508133 mm |
|
11 |
//Distance per encoder click (circumference / 1024) = 203.621224 um. |
|
12 |
//Robot width = 5.3745 in. = 136.5123 mm |
|
13 |
|
|
14 |
#define ROBOT_WIDTH_UM 137000 //um |
|
15 |
#define CLICK_DISTANCE_UM 204 //um |
|
16 |
|
|
17 |
//Standard measures will be mm and us |
|
18 |
|
|
19 |
int odometry_dx(void); |
|
20 |
int odometry_dy(void); |
|
21 |
double odometry_angle(void); |
|
22 |
void odometry_init(void); |
|
23 |
void odometry_reset(void); |
|
24 |
|
|
25 |
#endif |
trunk/code/projects/mapping/robot/robot_main.c | ||
---|---|---|
1 | 1 |
#include <dragonfly_lib.h> |
2 | 2 |
#include <wireless.h> |
3 | 3 |
#include <encoders.h> |
4 |
//#include "odometry.h"
|
|
4 |
#include "odometry.h" |
|
5 | 5 |
|
6 |
#define POLLING_INTERVAL 50 /* interval in ms to get sensor data */
|
|
6 |
#define POLLING_INTERVAL 200 /* interval in ms to get sensor data */
|
|
7 | 7 |
#define MAP 1 // packet group for sending data points |
8 | 8 |
#define POINT_RAW 1 // packet type for map data points w/ raw encoder data |
9 | 9 |
#define POINT_ODO 2 // packet type for map data points w/ odometry data |
... | ... | |
17 | 17 |
#define CNTL_LEFT_CURVE 4 |
18 | 18 |
#define CNTL_RIGHT_CURVE 5 |
19 | 19 |
#define CNTL_STOP 6 |
20 |
#define VEL_UP 7 |
|
21 |
#define VEL_DOWN 8 |
|
20 |
#define CNTL_VEL_UP 7
|
|
21 |
#define CNTL_VEL_DOWN 8
|
|
22 | 22 |
|
23 | 23 |
int state; |
24 | 24 |
int velocity; |
... | ... | |
34 | 34 |
{ |
35 | 35 |
/* initialize components and join the token ring */ |
36 | 36 |
dragonfly_init(ALL_ON); |
37 |
range_init(); |
|
37 |
//range_init(); |
|
38 |
//encoders_init(); |
|
39 |
odometry_init(); |
|
38 | 40 |
wl_init(); |
39 |
encoders_init(); |
|
40 | 41 |
wl_set_channel(0xF); |
41 | 42 |
wl_register_packet_group(&love_handle); |
42 | 43 |
wl_register_packet_group(&packetHandler); |
... | ... | |
45 | 46 |
//wl_token_ring_register(); |
46 | 47 |
//wl_token_ring_join(); |
47 | 48 |
//run_around_init(); |
48 |
//odometry_init(); |
|
49 | 49 |
|
50 | 50 |
velocity = 160; |
51 | 51 |
state = CNTL_STOP; |
52 | 52 |
|
53 |
//int x, y; // use when sending odometry data
|
|
54 |
//float theta;
|
|
53 |
int x, y; // use when sending odometry data |
|
54 |
double theta;
|
|
55 | 55 |
|
56 |
int store[7];
|
|
56 |
int store[9];
|
|
57 | 57 |
char *send; // for transmission |
58 | 58 |
|
59 | 59 |
while(1) |
... | ... | |
61 | 61 |
wl_do(); |
62 | 62 |
//run_around_FSM(); /* traverse the environment and avoid obstacles ... */ |
63 | 63 |
delay_ms(POLLING_INTERVAL); /* until it's time to grab map data */ |
64 |
//x = odometry_dx(); // use when sending odometry data |
|
65 |
//y = odometry_dy(); |
|
66 |
//theta = odometry_angle(); |
|
67 |
store[0] = encoder_get_dx(LEFT); // left encoder distance |
|
68 |
store[1] = encoder_get_dx(RIGHT); // right encoder distance |
|
69 |
store[2] = range_read_distance(IR1); // IR1 range |
|
70 |
store[3] = range_read_distance(IR2); // IR2 range |
|
71 |
store[4] = range_read_distance(IR3); // IR3 range |
|
72 |
store[5] = range_read_distance(IR4); // IR4 range |
|
73 |
store[6] = range_read_distance(IR5); // IR5 range |
|
64 |
x = odometry_dx(); // use when sending odometry data |
|
65 |
y = odometry_dy(); |
|
66 |
theta = odometry_angle(); |
|
67 |
//store[0] = encoder_get_dx(LEFT); // left encoder distance |
|
68 |
//store[1] = encoder_get_dx(RIGHT); // right encoder distance |
|
69 |
store[0] = x; |
|
70 |
store[1] = y; |
|
71 |
*((double*)((int*)store + 2)) = theta; |
|
72 |
store[4] = range_read_distance(IR1); // IR1 range |
|
73 |
store[5] = range_read_distance(IR2); // IR2 range |
|
74 |
store[6] = range_read_distance(IR3); // IR3 range |
|
75 |
store[7] = range_read_distance(IR4); // IR4 range |
|
76 |
store[8] = range_read_distance(IR5); // IR5 range |
|
77 |
sprintf( buf, "%d %d %f %d %d %d %d %d\n", store[0], store[1], |
|
78 |
*((double *)((int*)store+2)), store[4], store[5], store[6], |
|
79 |
store[7], store[8] ); |
|
80 |
usb_puts(buf); |
|
74 | 81 |
|
75 | 82 |
// convert to chars for transmission |
76 | 83 |
//for (int i = 0; i < 7; i++) |
... | ... | |
82 | 89 |
send = (char*) store; |
83 | 90 |
|
84 | 91 |
// send point w/ raw encoder data |
85 |
wl_send_global_packet(MAP, POINT_RAW, send, 14, 0); |
|
92 |
// wl_send_global_packet(MAP, POINT_RAW, send, 14, 0);
|
|
86 | 93 |
|
87 | 94 |
// send point w/ odometry data |
88 |
// wl_send_global_packet(MAP, POINT_ODO, send, 8, 0);
|
|
95 |
wl_send_global_packet(MAP, POINT_ODO, send, 18, 0);
|
|
89 | 96 |
|
90 | 97 |
// send raw encoder data point over serial as a string |
91 | 98 |
// send[7] = 0; |
92 | 99 |
// usb_puts(send); // this is probably ugly, but I don't know any better |
93 | 100 |
|
94 | 101 |
// send raw encoder data point over serial as integers |
95 |
for (int i = 0; i < 7; i++) |
|
102 |
/*for (int i = 0; i < 7; i++)
|
|
96 | 103 |
{ |
97 | 104 |
usb_puti(store[i]); |
98 | 105 |
usb_putc(' '); |
99 | 106 |
} |
100 |
usb_putc('\n'); |
|
101 |
usb_puti(range_read_distance(IR1)); |
|
102 |
usb_putc('\n'); |
|
107 |
usb_putc('\n');*/ |
|
103 | 108 |
} |
104 | 109 |
|
105 | 110 |
wl_terminate(); |
... | ... | |
131 | 136 |
case CNTL_STOP: |
132 | 137 |
state = CNTL_STOP; |
133 | 138 |
break; |
134 |
case VEL_UP: |
|
139 |
case CNTL_VEL_UP:
|
|
135 | 140 |
if (velocity > 250) |
136 | 141 |
velocity = 250; |
137 | 142 |
else |
138 | 143 |
velocity += 5; |
139 | 144 |
break; |
140 |
case VEL_DOWN: |
|
145 |
case CNTL_VEL_DOWN:
|
|
141 | 146 |
velocity -= 5; |
142 | 147 |
if (velocity < 150) |
143 | 148 |
velocity = 150; |
trunk/code/projects/mapping/robot/Makefile | ||
---|---|---|
156 | 156 |
PRINTF_LIB_FLOAT = -Wl,-u,vfprintf -lprintf_flt |
157 | 157 |
|
158 | 158 |
# If this is left blank, then it will use the Standard printf version. |
159 |
PRINTF_LIB = |
|
159 |
#PRINTF_LIB =
|
|
160 | 160 |
#PRINTF_LIB = $(PRINTF_LIB_MIN) |
161 |
#PRINTF_LIB = $(PRINTF_LIB_FLOAT)
|
|
161 |
PRINTF_LIB = $(PRINTF_LIB_FLOAT) |
|
162 | 162 |
|
163 | 163 |
|
164 | 164 |
# Minimalistic scanf version |
trunk/code/projects/mapping/server/test.c | ||
---|---|---|
3 | 3 |
#include <curses.h> // you need to install the ncurses library |
4 | 4 |
#include "../../libwireless/lib/wireless.h" |
5 | 5 |
|
6 |
#define MAP 1 // packet group for receiving points |
|
7 |
#define POINT_RAW 1 // packet type for map data points w/ raw encoder data |
|
8 |
#define POINT_ODO 2 // packet type for map data points w/ odometry data |
|
9 |
|
|
6 | 10 |
#define WL_CNTL_GROUP 4 |
7 | 11 |
|
8 | 12 |
#define CNTL_FORWARD 0 |
... | ... | |
38 | 42 |
return 1; |
39 | 43 |
} |
40 | 44 |
WINDOW* win = initscr(); |
45 |
nodelay(win, TRUE); |
|
41 | 46 |
printf("Beginning: robot=%d\r\n", robot); |
42 |
wl_set_com_port("/dev/ttyUSB0");
|
|
47 |
wl_set_com_port("/dev/ttyUSB1");
|
|
43 | 48 |
wl_init(); |
44 | 49 |
wl_set_channel(0xF); |
45 | 50 |
printf("Wireless initialized.\r\n"); |
... | ... | |
101 | 106 |
} |
102 | 107 |
|
103 | 108 |
void packet_receive (char type, int source, unsigned char* packet, int length) { |
104 |
//expected input: x y theta IR1 IR2 IR3 IR4 IR5 |
|
105 |
int i, temp; |
|
106 |
for (i = 0; i < length; i += 2) { |
|
107 |
temp = (char)packet[i+1] << 8; |
|
108 |
temp |= (char)packet[i]; |
|
109 |
fprintf(file, "%d ", temp); |
|
109 |
if (type == POINT_RAW) { |
|
110 |
//expected input: enc_l enc_r IR1 IR2 IR3 IR4 IR5 |
|
111 |
int i, temp; |
|
112 |
for (i = 0; i < length; i += 2) { |
|
113 |
temp = (char)packet[i+1] << 8; |
|
114 |
temp |= (char)packet[i]; |
|
115 |
fprintf(file, "%d ", temp); |
|
116 |
} |
|
117 |
fprintf(file, "\n"); |
|
110 | 118 |
} |
111 |
fprintf(file, "\n"); |
|
119 |
else if (type == POINT_ODO) { |
|
120 |
// expected input: x y theta(double) IR1 IR2 IR3 IR4 IR5 |
|
121 |
int x,y,ir1,ir2,ir3,ir4,ir5; |
|
122 |
//double theta; |
|
123 |
float theta; |
|
124 |
long theta_temp; |
|
125 |
x = (char)packet[0] + ((char)packet[1] << 8); |
|
126 |
y = (char)packet[2] + ((char)packet[3] << 8); |
|
127 |
theta_temp = (char)packet[4] + ((char)packet[5] << 8) |
|
128 |
+ ((char)packet[5] << 16) + ((char)packet[6] << 24); |
|
129 |
theta = (float)theta_temp; |
|
130 |
ir1 = (char)packet[8] + ((char)packet[9] << 8); |
|
131 |
ir2 = (char)packet[10] + ((char)packet[11] << 8); |
|
132 |
ir3 = (char)packet[12] + ((char)packet[13] << 8); |
|
133 |
ir4 = (char)packet[14] + ((char)packet[15] << 8); |
|
134 |
ir5 = (char)packet[16] + ((char)packet[17] << 8); |
|
135 |
|
|
136 |
fprintf(file, "%d %d %f %d %d %d %d %d\n", x, y, theta, ir1, ir2, ir3, ir4, ir5); |
|
137 |
} |
|
112 | 138 |
} |
Also available in: Unified diff