Revision 912
Robot sends updates 50 times per second. Robot remote control program doesn't mess up terminal upon exit.
robot_main.c | ||
---|---|---|
1 | 1 |
#include <dragonfly_lib.h> |
2 | 2 |
#include <wireless.h> |
3 |
#include "smart_run_around_fsm.h"
|
|
3 |
#include <encoders.h>
|
|
4 | 4 |
//#include "odometry.h" |
5 | 5 |
|
6 |
#define POLLING_INTERVAL 500 /* interval in ms to get sensor data */
|
|
6 |
#define POLLING_INTERVAL 50 /* 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 |
10 | 10 |
|
11 | 11 |
#define WL_CNTL_GROUP 4 |
12 | 12 |
|
13 |
#define FORWARD 0 |
|
14 |
#define BACK 1 |
|
15 |
#define LEFT 2 |
|
16 |
#define RIGHT 3 |
|
17 |
#define LEFT_CURVE 4 |
|
18 |
#define RIGHT_CURVE 5 |
|
19 |
#define STOP 6 |
|
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 | 20 |
#define VEL_UP 7 |
21 | 21 |
#define VEL_DOWN 8 |
22 | 22 |
|
... | ... | |
36 | 36 |
dragonfly_init(ALL_ON); |
37 | 37 |
range_init(); |
38 | 38 |
wl_init(); |
39 |
encoders_init(); |
|
39 | 40 |
wl_set_channel(0xF); |
40 | 41 |
wl_register_packet_group(&love_handle); |
41 | 42 |
wl_register_packet_group(&packetHandler); |
... | ... | |
47 | 48 |
//odometry_init(); |
48 | 49 |
|
49 | 50 |
velocity = 160; |
50 |
state = STOP; |
|
51 |
state = CNTL_STOP;
|
|
51 | 52 |
|
52 | 53 |
//int x, y; // use when sending odometry data |
53 | 54 |
//float theta; |
... | ... | |
63 | 64 |
//x = odometry_dx(); // use when sending odometry data |
64 | 65 |
//y = odometry_dy(); |
65 | 66 |
//theta = odometry_angle(); |
66 |
store[0] = -1;//encoder_get_dx(LEFT); // left encoder distance
|
|
67 |
store[1] = -1;//encoder_get_dx(RIGHT); // right encoder distance
|
|
67 |
store[0] = encoder_get_dx(LEFT); // left encoder distance |
|
68 |
store[1] = encoder_get_dx(RIGHT); // right encoder distance |
|
68 | 69 |
store[2] = range_read_distance(IR1); // IR1 range |
69 | 70 |
store[3] = range_read_distance(IR2); // IR2 range |
70 | 71 |
store[4] = range_read_distance(IR3); // IR3 range |
... | ... | |
109 | 110 |
sprintf(buf, "received packet: %d\n", type); |
110 | 111 |
usb_puts(buf); |
111 | 112 |
switch (type) { |
112 |
case FORWARD: |
|
113 |
state = FORWARD; |
|
113 |
case CNTL_FORWARD:
|
|
114 |
state = CNTL_FORWARD;
|
|
114 | 115 |
break; |
115 |
case BACK: |
|
116 |
state = BACK; |
|
116 |
case CNTL_BACK:
|
|
117 |
state = CNTL_BACK;
|
|
117 | 118 |
break; |
118 |
case LEFT: |
|
119 |
state = LEFT; |
|
119 |
case CNTL_LEFT:
|
|
120 |
state = CNTL_LEFT;
|
|
120 | 121 |
break; |
121 |
case RIGHT: |
|
122 |
state = RIGHT; |
|
122 |
case CNTL_RIGHT:
|
|
123 |
state = CNTL_RIGHT;
|
|
123 | 124 |
break; |
124 |
case LEFT_CURVE: |
|
125 |
state = LEFT_CURVE; |
|
125 |
case CNTL_LEFT_CURVE:
|
|
126 |
state = CNTL_LEFT_CURVE;
|
|
126 | 127 |
break; |
127 |
case RIGHT_CURVE: |
|
128 |
state = RIGHT_CURVE; |
|
128 |
case CNTL_RIGHT_CURVE:
|
|
129 |
state = CNTL_RIGHT_CURVE;
|
|
129 | 130 |
break; |
130 |
case STOP: |
|
131 |
state = STOP; |
|
131 |
case CNTL_STOP:
|
|
132 |
state = CNTL_STOP;
|
|
132 | 133 |
break; |
133 | 134 |
case VEL_UP: |
134 | 135 |
if (velocity > 250) |
... | ... | |
142 | 143 |
velocity = 150; |
143 | 144 |
break; |
144 | 145 |
default: |
145 |
state = STOP; |
|
146 |
state = CNTL_STOP;
|
|
146 | 147 |
break; |
147 | 148 |
} |
148 | 149 |
set_motion(); |
... | ... | |
150 | 151 |
|
151 | 152 |
void set_motion (void) { |
152 | 153 |
switch (state) { |
153 |
case FORWARD: |
|
154 |
case CNTL_FORWARD:
|
|
154 | 155 |
orb_set_color(GREEN); |
155 | 156 |
move(velocity, 0); |
156 | 157 |
break; |
157 |
case BACK: |
|
158 |
case CNTL_BACK:
|
|
158 | 159 |
orb_set_color(BLUE); |
159 | 160 |
move(-velocity, 0); |
160 | 161 |
break; |
161 |
case LEFT: |
|
162 |
case CNTL_LEFT:
|
|
162 | 163 |
orb_set_color(ORANGE); |
163 | 164 |
motor1_set(0, velocity); |
164 | 165 |
motor2_set(1, velocity); |
165 | 166 |
break; |
166 |
case RIGHT: |
|
167 |
case CNTL_RIGHT:
|
|
167 | 168 |
orb_set_color(YELLOW); |
168 | 169 |
motor1_set(1, velocity); |
169 | 170 |
motor2_set(0, velocity); |
170 | 171 |
break; |
171 |
case LEFT_CURVE: |
|
172 |
case CNTL_LEFT_CURVE:
|
|
172 | 173 |
orb_set_color(MAGENTA); |
173 | 174 |
motor1_set(1, 150); |
174 | 175 |
motor2_set(1, velocity); |
175 | 176 |
break; |
176 |
case RIGHT_CURVE: |
|
177 |
case CNTL_RIGHT_CURVE:
|
|
177 | 178 |
orb_set_color(CYAN); |
178 | 179 |
motor1_set(1, velocity); |
179 | 180 |
motor2_set(1, 150); |
180 | 181 |
break; |
181 |
case STOP: |
|
182 |
case CNTL_STOP:
|
|
182 | 183 |
orb_set_color(RED); |
183 | 184 |
move(0, 0); |
184 | 185 |
break; |
Also available in: Unified diff