Revision 1010
Motion works, but needs fine tuning.
| branches/simulator/projects/simulator/simulator/core/motion.c | ||
|---|---|---|
| 3 | 3 |
|
| 4 | 4 |
#include "motion.h" |
| 5 | 5 |
|
| 6 |
#define CUTOFF 120 |
|
| 6 |
#include "robot.h" |
|
| 7 |
|
|
| 8 |
#define CUTOFF 120 |
|
| 7 | 9 |
#define ABS(x) (x<0?-x:x) |
| 8 |
#define TIME 1 /*sec*/
|
|
| 10 |
#define TIME 1 /*sec*/
|
|
| 9 | 11 |
#define ROBOT_WIDTH 131 /*mm*/ |
| 10 | 12 |
|
| 11 | 13 |
/** move will move a robot from its initial position, (x,y), and theta (in radians) to a new position given speed. |
| ... | ... | |
| 15 | 17 |
* speed is between 0 - 255, there is some magical cutoff point before the motors actually starts running |
| 16 | 18 |
* move will return 0 if successful |
| 17 | 19 |
**/ |
| 18 |
int move (int *x, int *y, double *theta, int speed1, int speed2) {
|
|
| 19 |
if (*theta < 0 || *theta > 2*M_PI) return 1; |
|
| 20 |
if (speed1 < 0 || speed1 > 255) return 1; |
|
| 21 |
if (speed2 < 0 || speed2 > 255) return 1; |
|
| 20 |
int move_robot(Robot* r) |
|
| 21 |
{
|
|
| 22 |
short speed1 = r->shared->motor1; |
|
| 23 |
short speed2 = r->shared->motor2; |
|
| 24 |
float theta = r->pose.theta; |
|
| 22 | 25 |
|
| 23 |
/* if speed is lower than the cut off, don't move */ |
|
| 24 |
if (ABS(speed1) < CUTOFF) {
|
|
| 25 |
speed1 = 0; |
|
| 26 |
} |
|
| 27 |
if (ABS(speed2) < CUTOFF) {
|
|
| 28 |
speed2 = 0; |
|
| 29 |
} |
|
| 30 |
double radius; |
|
| 31 |
if (speed1 == speed2) {
|
|
| 32 |
/* go straight */ |
|
| 33 |
*x += cos(*theta) * speed1; |
|
| 34 |
*y += sin(*theta) * speed1; |
|
| 35 |
return 0; |
|
| 36 |
} |
|
| 37 |
radius = ROBOT_WIDTH * speed1 / (speed1 - speed2); |
|
| 38 |
|
|
| 39 |
double t = speed1 / radius; |
|
| 26 |
if (theta < 0 || theta > 2*M_PI) return 1; |
|
| 27 |
if (speed1 < 0 || speed1 > 255) return 1; |
|
| 28 |
if (speed2 < 0 || speed2 > 255) return 1; |
|
| 40 | 29 |
|
| 41 |
double newx = radius * sin(t); |
|
| 42 |
double newy = radius - radius * cos(t); |
|
| 30 |
/* if speed is lower than the cut off, don't move */ |
|
| 31 |
if (ABS(speed1) < CUTOFF) {
|
|
| 32 |
speed1 = 0; |
|
| 33 |
} |
|
| 34 |
if (ABS(speed2) < CUTOFF) {
|
|
| 35 |
speed2 = 0; |
|
| 36 |
} |
|
| 37 |
double radius; |
|
| 38 |
if (speed1 == speed2) {
|
|
| 39 |
/* go straight */ |
|
| 40 |
r->pose.x += cos(theta) * speed1; |
|
| 41 |
r->pose.y += sin(theta) * speed1; |
|
| 42 |
return 0; |
|
| 43 |
} |
|
| 44 |
radius = ROBOT_WIDTH * speed1 / (speed1 - speed2); |
|
| 45 |
|
|
| 46 |
double t = speed1 / radius; |
|
| 43 | 47 |
|
| 44 |
*x += newx * cos(*theta);
|
|
| 45 |
*y += newx * sin(*theta);
|
|
| 48 |
double newx = radius * sin(t);
|
|
| 49 |
double newy = radius - radius * cos(t);
|
|
| 46 | 50 |
|
| 47 |
*x += newy * - sin(*theta);
|
|
| 48 |
*y += newy * cos(*theta);
|
|
| 51 |
r->pose.x += newx * cos(theta);
|
|
| 52 |
r->pose.y += newx * sin(theta);
|
|
| 49 | 53 |
|
| 50 |
*theta = fmod((t + *theta), (2 * M_PI));
|
|
| 51 |
if (*theta<0) *theta += 2 * M_PI;
|
|
| 54 |
r->pose.x += newy * - sin(theta);
|
|
| 55 |
r->pose.y += newy * cos(theta);
|
|
| 52 | 56 |
|
| 53 |
return 0; |
|
| 57 |
r->pose.theta = fmod((t + theta), (2 * M_PI)); |
|
| 58 |
if (r->pose.theta<0) r->pose.theta += 2 * M_PI; |
|
| 59 |
|
|
| 60 |
return 0; |
|
| 54 | 61 |
} |
| 55 | 62 |
|
| branches/simulator/projects/simulator/simulator/core/robot.c | ||
|---|---|---|
| 23 | 23 |
#include <pthread.h> |
| 24 | 24 |
|
| 25 | 25 |
#include "robot.h" |
| 26 |
#include "motion.h" |
|
| 26 | 27 |
#include "gtk_gui.h" |
| 27 | 28 |
|
| 28 | 29 |
void sig_chld_handler(int sig); |
| 30 |
void robot_update(int i); |
|
| 29 | 31 |
|
| 30 | 32 |
// global variables |
| 31 | 33 |
int first_available_id = 0; |
| ... | ... | |
| 236 | 238 |
finished = 0; |
| 237 | 239 |
pthread_mutex_unlock(&all_finished_mutex); |
| 238 | 240 |
for (i = 0; i < robots_size; i++) |
| 241 |
if (robots[i].id != -1) |
|
| 242 |
robot_update(i); |
|
| 243 |
for (i = 0; i < robots_size; i++) |
|
| 239 | 244 |
{
|
| 240 | 245 |
if (robots[i].id == -1) |
| 241 | 246 |
continue; |
| ... | ... | |
| 246 | 251 |
} |
| 247 | 252 |
} |
| 248 | 253 |
|
| 254 |
void robot_update(int i) |
|
| 255 |
{
|
|
| 256 |
Robot* r = &(robots[i]); |
|
| 257 |
if (r->id == -1) |
|
| 258 |
return; |
|
| 259 |
move_robot(r); |
|
| 260 |
} |
|
| 261 |
|
|
| branches/simulator/projects/simulator/simulator/Makefile | ||
|---|---|---|
| 1 | 1 |
# Add new files here |
| 2 | 2 |
COMMON_SRCS := |
| 3 |
CORE_SRCS := main.c robot.c |
|
| 3 |
CORE_SRCS := main.c robot.c motion.c
|
|
| 4 | 4 |
GUI_SRCS := gtk_gui.c gtk_environment_view.c draw_funcs.c |
| 5 | 5 |
|
| 6 | 6 |
CORE_DIR := core |
Also available in: Unified diff