Revision 648
working on robot point and click - kinda works
| colonet_dragonfly.c | ||
|---|---|---|
| 17 | 17 |
#include <string.h> |
| 18 | 18 |
#include <wireless.h> |
| 19 | 19 |
|
| 20 |
#define TARGET_POSITION_STOP_DISTANCE_THRESHOLD (40)
|
|
| 20 |
#define TARGET_POSITION_STOP_DISTANCE_THRESHOLD (550)
|
|
| 21 | 21 |
|
| 22 | 22 |
typedef struct {
|
| 23 | 23 |
unsigned char msgId; //is this necessary? |
| ... | ... | |
| 31 | 31 |
static unsigned last_x, last_y; |
| 32 | 32 |
static volatile int updated_robot_pos_ready; |
| 33 | 33 |
static volatile int new_movement_command_received; |
| 34 |
static volatile int robot_lost; //set to 1 if robot's position is not known by the server, else 0. |
|
| 35 |
|
|
| 34 | 36 |
#define ERROR_MAG_TO_MOTOR_DIFFENTIAL_CONST (1) |
| 35 | 37 |
|
| 36 | 38 |
/* Internal function prototypes */ |
| ... | ... | |
| 63 | 65 |
robot_y = last_y = 0; |
| 64 | 66 |
updated_robot_pos_ready = 0; |
| 65 | 67 |
new_movement_command_received = 0; |
| 68 |
robot_lost = 1; |
|
| 66 | 69 |
|
| 67 | 70 |
return 0; |
| 68 | 71 |
} |
| ... | ... | |
| 107 | 110 |
|
| 108 | 111 |
if (differential >= 0) {
|
| 109 | 112 |
/* Going left or straight. */ |
| 110 |
ml = 250;
|
|
| 113 |
ml = 200;
|
|
| 111 | 114 |
mr = ml - differential; |
| 112 | 115 |
} else {
|
| 113 | 116 |
/* Turning right. */ |
| 114 |
mr = 250;
|
|
| 117 |
mr = 200;
|
|
| 115 | 118 |
ml = mr + differential; |
| 116 | 119 |
} |
| 117 | 120 |
|
| ... | ... | |
| 122 | 125 |
motor2_set(motor2_dir, ml); |
| 123 | 126 |
} |
| 124 | 127 |
|
| 128 |
static int get_motor_differential(int e_x, int e_y, int v_x, int v_y) {
|
|
| 129 |
// char buf[80]; |
|
| 130 |
// sprintf(buf,"ex:%d ey:%d vx:%d vy:%d\n", e_x, e_y, v_x, v_y); |
|
| 131 |
// usb_puts(buf); |
|
| 132 |
|
|
| 133 |
return 150; |
|
| 134 |
|
|
| 135 |
/* |
|
| 136 |
long e_mag = sqrt_100_approx(e_x*e_x + e_y*e_y); |
|
| 137 |
long v_mag = sqrt_100_approx(v_x*v_x + v_y*v_y); |
|
| 138 |
long e_norm_x = (1000 * e_x) / e_mag; |
|
| 139 |
long e_norm_y = (1000 * e_y) / e_mag; |
|
| 140 |
long v_norm_x = (1000 * v_x) / v_mag; |
|
| 141 |
long v_norm_y = (1000 * v_y) / v_mag; |
|
| 142 |
long e_dot_v = e_norm_x*v_norm_x + e_norm_y*v_norm_y; |
|
| 143 |
*/ |
|
| 144 |
|
|
| 145 |
|
|
| 146 |
} |
|
| 147 |
|
|
| 125 | 148 |
static void move_to_position_routine(unsigned target_x, unsigned target_y) {
|
| 126 | 149 |
new_movement_command_received = 0; |
| 127 | 150 |
|
| ... | ... | |
| 139 | 162 |
} |
| 140 | 163 |
} |
| 141 | 164 |
|
| 165 |
int dist = 0; |
|
| 166 |
|
|
| 142 | 167 |
while (!new_movement_command_received && |
| 143 |
dist_squared(robot_x, robot_y, target_x, target_y) > TARGET_POSITION_STOP_DISTANCE_THRESHOLD) {
|
|
| 168 |
(dist = dist_squared(robot_x, robot_y, target_x, target_y)) > TARGET_POSITION_STOP_DISTANCE_THRESHOLD) {
|
|
| 169 |
char buf[80]; |
|
| 170 |
//sprintf(buf, "dist:%d\n", dist); |
|
| 171 |
//usb_puts(buf); |
|
| 172 |
|
|
| 144 | 173 |
updated_robot_pos_ready = 0; |
| 145 | 174 |
request_abs_position(); // While we're doing this computation, server can be reporting next position. |
| 146 | 175 |
|
| 147 |
// e is the error vector (where we want to go) |
|
| 148 |
int e_x = target_x - robot_x; |
|
| 149 |
int e_y = target_y - robot_y; |
|
| 176 |
if (robot_lost) {
|
|
| 177 |
usb_puts("lost known\n");
|
|
| 178 |
motors_off(); |
|
| 179 |
} else {
|
|
| 180 |
// e is the error vector (where we want to go) |
|
| 181 |
//char buf[80]; |
|
| 182 |
// sprintf(buf, "target_x:%d target_y:%d robot_x:%d robot_y:%d\n", target_x, target_y, robot_x, robot_y); |
|
| 183 |
// usb_puts(buf); |
|
| 150 | 184 |
|
| 151 |
// v is the velocity vector (wehere we just went) |
|
| 152 |
int v_x = robot_x - last_x; |
|
| 153 |
int v_y = robot_y - last_y; |
|
| 185 |
int e_x = (int)target_x - (int)robot_x; |
|
| 186 |
int e_y = (int)target_y - (int)robot_y; |
|
| 187 |
|
|
| 188 |
// sprintf(buf, "e: (x:%d,y:%d)\n", e_x, e_y); |
|
| 189 |
// usb_puts(buf); |
|
| 190 |
|
|
| 191 |
// v is the velocity vector (where we just went) |
|
| 192 |
int v_x = (int)robot_x - (int)last_x; |
|
| 193 |
int v_y = (int)robot_y - (int)last_y; |
|
| 194 |
|
|
| 195 |
//sprintf(buf, "v: (x:%d,y:%d)\n", v_x, v_y); |
|
| 196 |
//usb_puts(buf); |
|
| 154 | 197 |
|
| 155 |
if (abs(v_x) < 2 && abs(v_x) < 2) {
|
|
| 156 |
// if we didn't move since the last check, just go forward. |
|
| 157 |
motor1_set(0, 240); |
|
| 158 |
motor2_set(0, 240); |
|
| 159 |
} else {
|
|
| 160 |
// move according to algortihm |
|
| 161 |
int e_mag = e_x*e_x + e_y*e_y; |
|
| 162 |
int motor_differential = e_mag >> 7; // 128; |
|
| 198 |
if (abs(v_x) < 2 && abs(v_x) < 2) {
|
|
| 199 |
// if we didn't move since the last check, just go forward. |
|
| 200 |
motor1_set(1, 180); |
|
| 201 |
motor2_set(1, 180); |
|
| 202 |
} else {
|
|
| 203 |
int e_dot_v = e_x * v_x + e_y * v_y; |
|
| 163 | 204 |
|
| 164 |
// p is perpendicular to v, directed to the "left" of the robot.
|
|
| 165 |
int p_x = v_y;
|
|
| 166 |
int p_y = -v_x;
|
|
| 167 |
int dot_product_pe = p_x * e_x + p_y * e_y;
|
|
| 205 |
// move according to algortihm
|
|
| 206 |
int e_mag = e_x*e_x + e_y*e_y;
|
|
| 207 |
// int motor_differential = 180;//e_mag >> 7;
|
|
| 208 |
int motor_differential = get_motor_differential(e_x, e_y, v_x, v_y);
|
|
| 168 | 209 |
|
| 169 |
// if the dot product of p and e is negative, we should turn right. otherwise turn left. |
|
| 170 |
if (dot_product_pe < 0) {
|
|
| 171 |
motor_differential = -motor_differential; |
|
| 210 |
// p is perpendicular to v, directed to the "left" of the robot. |
|
| 211 |
int p_x = v_y; |
|
| 212 |
int p_y = -v_x; |
|
| 213 |
int dot_product_pe = p_x * e_x + p_y * e_y; |
|
| 214 |
|
|
| 215 |
// if the dot product of p and e is negative, we should turn right. otherwise turn left. |
|
| 216 |
if (dot_product_pe < 0) {
|
|
| 217 |
motor_differential = -motor_differential; |
|
| 218 |
} |
|
| 219 |
|
|
| 220 |
set_motors_with_differential(motor_differential); |
|
| 172 | 221 |
} |
| 173 |
set_motors_with_differential(motor_differential); |
|
| 174 | 222 |
} |
| 175 | 223 |
|
| 224 |
if (robot_lost) {
|
|
| 225 |
motors_off(); |
|
| 226 |
} |
|
| 227 |
|
|
| 176 | 228 |
// once we move, ask for the position again. |
| 177 | 229 |
count = 0; |
| 178 | 230 |
while (!updated_robot_pos_ready) {
|
| ... | ... | |
| 184 | 236 |
} |
| 185 | 237 |
} |
| 186 | 238 |
|
| 239 |
motors_off(); |
|
| 240 |
|
|
| 187 | 241 |
// Once this function terminates, the robot be at its destination. |
| 188 | 242 |
} |
| 189 | 243 |
|
| ... | ... | |
| 285 | 339 |
/* pkt.msg_code); */ |
| 286 | 340 |
/* break; */ |
| 287 | 341 |
|
| 342 |
case SERVER_REPORT_ROBOT_LOST: |
|
| 343 |
last_x = robot_x; |
|
| 344 |
last_y = robot_y; |
|
| 345 |
|
|
| 346 |
usb_puts("lost\n");
|
|
| 347 |
|
|
| 348 |
robot_lost = 1; |
|
| 349 |
|
|
| 350 |
motors_off(); |
|
| 351 |
break; |
|
| 352 |
|
|
| 288 | 353 |
case SERVER_REPORT_POSITION_TO_ROBOT: |
| 354 |
robot_lost = 0; |
|
| 355 |
|
|
| 289 | 356 |
last_x = robot_x; |
| 290 | 357 |
last_y = robot_y; |
| 291 | 358 |
robot_x = (unsigned)int_args[0]; |
| ... | ... | |
| 293 | 360 |
|
| 294 | 361 |
updated_robot_pos_ready = 1; |
| 295 | 362 |
|
| 296 |
sprintf(buf, "pos is: %u %u\n", robot_x, robot_y);
|
|
| 297 |
usb_puts(buf); |
|
| 363 |
// sprintf(buf, "pos is: (x:%d, y:%d)\n", robot_x, robot_y);
|
|
| 364 |
//usb_puts(buf);
|
|
| 298 | 365 |
break; |
| 299 | 366 |
|
| 300 | 367 |
case MOVE_TO_ABSOLUTE_POSITION: |
| 301 | 368 |
new_movement_command_received = 1; |
| 302 | 369 |
|
| 303 |
usb_puts("move to abs position!\n");
|
|
| 370 |
sprintf(buf, "move to abs (x:%u, y:%u)\n", (unsigned)int_args[0], (unsigned)int_args[1]); |
|
| 371 |
usb_puts(buf); |
|
| 372 |
|
|
| 304 | 373 |
move_to_position_routine((unsigned)int_args[0], (unsigned)int_args[1]); |
| 305 | 374 |
break; |
| 306 | 375 |
|
Also available in: Unified diff