root / trunk / code / projects / mapping / robot / robot_main.c @ 912
History | View | Annotate | Download (4.88 KB)
1 |
#include <dragonfly_lib.h> |
---|---|
2 |
#include <wireless.h> |
3 |
#include <encoders.h> |
4 |
//#include "odometry.h"
|
5 |
|
6 |
#define POLLING_INTERVAL 50 /* 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 VEL_UP 7 |
21 |
#define 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 |
wl_init(); |
39 |
encoders_init(); |
40 |
wl_set_channel(0xF);
|
41 |
wl_register_packet_group(&love_handle); |
42 |
wl_register_packet_group(&packetHandler); |
43 |
orb_enable(); |
44 |
//data_response_init();
|
45 |
//wl_token_ring_register();
|
46 |
//wl_token_ring_join();
|
47 |
//run_around_init();
|
48 |
//odometry_init();
|
49 |
|
50 |
velocity = 160;
|
51 |
state = CNTL_STOP; |
52 |
|
53 |
//int x, y; // use when sending odometry data
|
54 |
//float theta;
|
55 |
|
56 |
int store[7]; |
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[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 |
74 |
|
75 |
// convert to chars for transmission
|
76 |
//for (int i = 0; i < 7; i++)
|
77 |
//{
|
78 |
// send[i] = (char) store[i];
|
79 |
//}
|
80 |
//send[7] = '\0';
|
81 |
//memcpy(store, send, 14);
|
82 |
send = (char*) store;
|
83 |
|
84 |
// send point w/ raw encoder data
|
85 |
wl_send_global_packet(MAP, POINT_RAW, send, 14, 0); |
86 |
|
87 |
// send point w/ odometry data
|
88 |
// wl_send_global_packet(MAP, POINT_ODO, send, 8, 0);
|
89 |
|
90 |
// send raw encoder data point over serial as a string
|
91 |
// send[7] = 0;
|
92 |
// usb_puts(send); // this is probably ugly, but I don't know any better
|
93 |
|
94 |
// send raw encoder data point over serial as integers
|
95 |
for (int i = 0; i < 7; i++) |
96 |
{ |
97 |
usb_puti(store[i]); |
98 |
usb_putc(' ');
|
99 |
} |
100 |
usb_putc('\n');
|
101 |
usb_puti(range_read_distance(IR1)); |
102 |
usb_putc('\n');
|
103 |
} |
104 |
|
105 |
wl_terminate(); |
106 |
return 0; |
107 |
} |
108 |
|
109 |
void packet_receive (char type, int source, unsigned char* packet, int length) { |
110 |
sprintf(buf, "received packet: %d\n", type);
|
111 |
usb_puts(buf); |
112 |
switch (type) {
|
113 |
case CNTL_FORWARD:
|
114 |
state = CNTL_FORWARD; |
115 |
break;
|
116 |
case CNTL_BACK:
|
117 |
state = CNTL_BACK; |
118 |
break;
|
119 |
case CNTL_LEFT:
|
120 |
state = CNTL_LEFT; |
121 |
break;
|
122 |
case CNTL_RIGHT:
|
123 |
state = CNTL_RIGHT; |
124 |
break;
|
125 |
case CNTL_LEFT_CURVE:
|
126 |
state = CNTL_LEFT_CURVE; |
127 |
break;
|
128 |
case CNTL_RIGHT_CURVE:
|
129 |
state = CNTL_RIGHT_CURVE; |
130 |
break;
|
131 |
case CNTL_STOP:
|
132 |
state = CNTL_STOP; |
133 |
break;
|
134 |
case VEL_UP:
|
135 |
if (velocity > 250) |
136 |
velocity = 250;
|
137 |
else
|
138 |
velocity += 5;
|
139 |
break;
|
140 |
case VEL_DOWN:
|
141 |
velocity -= 5;
|
142 |
if (velocity < 150) |
143 |
velocity = 150;
|
144 |
break;
|
145 |
default:
|
146 |
state = CNTL_STOP; |
147 |
break;
|
148 |
} |
149 |
set_motion(); |
150 |
} |
151 |
|
152 |
void set_motion (void) { |
153 |
switch (state) {
|
154 |
case CNTL_FORWARD:
|
155 |
orb_set_color(GREEN); |
156 |
move(velocity, 0);
|
157 |
break;
|
158 |
case CNTL_BACK:
|
159 |
orb_set_color(BLUE); |
160 |
move(-velocity, 0);
|
161 |
break;
|
162 |
case CNTL_LEFT:
|
163 |
orb_set_color(ORANGE); |
164 |
motor1_set(0, velocity);
|
165 |
motor2_set(1, velocity);
|
166 |
break;
|
167 |
case CNTL_RIGHT:
|
168 |
orb_set_color(YELLOW); |
169 |
motor1_set(1, velocity);
|
170 |
motor2_set(0, velocity);
|
171 |
break;
|
172 |
case CNTL_LEFT_CURVE:
|
173 |
orb_set_color(MAGENTA); |
174 |
motor1_set(1, 150); |
175 |
motor2_set(1, velocity);
|
176 |
break;
|
177 |
case CNTL_RIGHT_CURVE:
|
178 |
orb_set_color(CYAN); |
179 |
motor1_set(1, velocity);
|
180 |
motor2_set(1, 150); |
181 |
break;
|
182 |
case CNTL_STOP:
|
183 |
orb_set_color(RED); |
184 |
move(0, 0); |
185 |
break;
|
186 |
} |
187 |
} |
188 |
|