root / trunk / code / projects / mapping / robot / robot_main.c @ 915
History | View | Annotate | Download (5.11 KB)
1 |
#include <dragonfly_lib.h> |
---|---|
2 |
#include <wireless.h> |
3 |
#include <encoders.h> |
4 |
#include "odometry.h" |
5 |
|
6 |
#define POLLING_INTERVAL 200 /* interval in ms to get sensor data */ |
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 |
10 |
|
11 |
#define WL_CNTL_GROUP 4 |
12 |
|
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 |
22 |
|
23 |
int state;
|
24 |
int velocity;
|
25 |
char buf[100]; |
26 |
|
27 |
void packet_receive (char type, int source, unsigned char* packet, int length); |
28 |
void set_motion (void); |
29 |
|
30 |
PacketGroupHandler packetHandler = {WL_CNTL_GROUP, NULL, NULL, &packet_receive, NULL}; |
31 |
PacketGroupHandler love_handle = {MAP, NULL, NULL, NULL, NULL}; |
32 |
|
33 |
int main(void) |
34 |
{ |
35 |
/* initialize components and join the token ring */
|
36 |
dragonfly_init(ALL_ON); |
37 |
//range_init();
|
38 |
//encoders_init();
|
39 |
odometry_init(); |
40 |
wl_init(); |
41 |
wl_set_channel(0xF);
|
42 |
wl_register_packet_group(&love_handle); |
43 |
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 |
velocity = 160;
|
51 |
state = CNTL_STOP; |
52 |
|
53 |
int x, y; // use when sending odometry data |
54 |
double theta;
|
55 |
|
56 |
int store[9]; |
57 |
char *send; // for transmission |
58 |
|
59 |
while(1) |
60 |
{ |
61 |
wl_do(); |
62 |
//run_around_FSM(); /* traverse the environment and avoid obstacles ... */
|
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[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); |
81 |
|
82 |
// convert to chars for transmission
|
83 |
//for (int i = 0; i < 7; i++)
|
84 |
//{
|
85 |
// send[i] = (char) store[i];
|
86 |
//}
|
87 |
//send[7] = '\0';
|
88 |
//memcpy(store, send, 14);
|
89 |
send = (char*) store;
|
90 |
|
91 |
// send point w/ raw encoder data
|
92 |
// wl_send_global_packet(MAP, POINT_RAW, send, 14, 0);
|
93 |
|
94 |
// send point w/ odometry data
|
95 |
wl_send_global_packet(MAP, POINT_ODO, send, 18, 0); |
96 |
|
97 |
// send raw encoder data point over serial as a string
|
98 |
// send[7] = 0;
|
99 |
// usb_puts(send); // this is probably ugly, but I don't know any better
|
100 |
|
101 |
// send raw encoder data point over serial as integers
|
102 |
/*for (int i = 0; i < 7; i++)
|
103 |
{
|
104 |
usb_puti(store[i]);
|
105 |
usb_putc(' ');
|
106 |
}
|
107 |
usb_putc('\n');*/
|
108 |
} |
109 |
|
110 |
wl_terminate(); |
111 |
return 0; |
112 |
} |
113 |
|
114 |
void packet_receive (char type, int source, unsigned char* packet, int length) { |
115 |
sprintf(buf, "received packet: %d\n", type);
|
116 |
usb_puts(buf); |
117 |
switch (type) {
|
118 |
case CNTL_FORWARD:
|
119 |
state = CNTL_FORWARD; |
120 |
break;
|
121 |
case CNTL_BACK:
|
122 |
state = CNTL_BACK; |
123 |
break;
|
124 |
case CNTL_LEFT:
|
125 |
state = CNTL_LEFT; |
126 |
break;
|
127 |
case CNTL_RIGHT:
|
128 |
state = CNTL_RIGHT; |
129 |
break;
|
130 |
case CNTL_LEFT_CURVE:
|
131 |
state = CNTL_LEFT_CURVE; |
132 |
break;
|
133 |
case CNTL_RIGHT_CURVE:
|
134 |
state = CNTL_RIGHT_CURVE; |
135 |
break;
|
136 |
case CNTL_STOP:
|
137 |
state = CNTL_STOP; |
138 |
break;
|
139 |
case CNTL_VEL_UP:
|
140 |
if (velocity > 250) |
141 |
velocity = 250;
|
142 |
else
|
143 |
velocity += 5;
|
144 |
break;
|
145 |
case CNTL_VEL_DOWN:
|
146 |
velocity -= 5;
|
147 |
if (velocity < 150) |
148 |
velocity = 150;
|
149 |
break;
|
150 |
default:
|
151 |
state = CNTL_STOP; |
152 |
break;
|
153 |
} |
154 |
set_motion(); |
155 |
} |
156 |
|
157 |
void set_motion (void) { |
158 |
switch (state) {
|
159 |
case CNTL_FORWARD:
|
160 |
orb_set_color(GREEN); |
161 |
move(velocity, 0);
|
162 |
break;
|
163 |
case CNTL_BACK:
|
164 |
orb_set_color(BLUE); |
165 |
move(-velocity, 0);
|
166 |
break;
|
167 |
case CNTL_LEFT:
|
168 |
orb_set_color(ORANGE); |
169 |
motor1_set(0, velocity);
|
170 |
motor2_set(1, velocity);
|
171 |
break;
|
172 |
case CNTL_RIGHT:
|
173 |
orb_set_color(YELLOW); |
174 |
motor1_set(1, velocity);
|
175 |
motor2_set(0, velocity);
|
176 |
break;
|
177 |
case CNTL_LEFT_CURVE:
|
178 |
orb_set_color(MAGENTA); |
179 |
motor1_set(1, 150); |
180 |
motor2_set(1, velocity);
|
181 |
break;
|
182 |
case CNTL_RIGHT_CURVE:
|
183 |
orb_set_color(CYAN); |
184 |
motor1_set(1, velocity);
|
185 |
motor2_set(1, 150); |
186 |
break;
|
187 |
case CNTL_STOP:
|
188 |
orb_set_color(RED); |
189 |
move(0, 0); |
190 |
break;
|
191 |
} |
192 |
} |
193 |
|