root / branches / simulator / projects / simulator / simulator / core / motion.c @ 1046
History | View | Annotate | Download (1.86 KB)
1 |
#include <stdio.h> |
---|---|
2 |
#include <stdlib.h> |
3 |
#include <math.h> |
4 |
|
5 |
#include "motion.h" |
6 |
|
7 |
#include "robot.h" |
8 |
|
9 |
#define CUTOFF 120 |
10 |
#define TIME 1 /*sec*/ |
11 |
#define ROBOT_WIDTH 131 /*mm*/ |
12 |
|
13 |
#define MOTOR_CONVERSION_FACTOR 10.0 |
14 |
|
15 |
/** move_robot will move a robot from its initial position, (x,y), and theta (in radians) to a new position given speed.
|
16 |
* (x,y) and theta will be updated by the move_robot function instead of returning a value
|
17 |
* (x,y) is some kind of absolute position in the "world", (0,0) is the top left of the "world"
|
18 |
* theta will an angle be between 0 and 2*Pi (0 being faces east and goes clockwise)
|
19 |
* speed is between 0 and 255, there is some magical cutoff point before the motors actually starts running
|
20 |
* move will return 0 if successful
|
21 |
**/
|
22 |
int move_robot(Robot* r)
|
23 |
{ |
24 |
short speed1 = r->shared->motor1;
|
25 |
short speed2 = r->shared->motor2;
|
26 |
float theta = r->pose.theta;
|
27 |
|
28 |
if (theta < 0 || theta > 2*M_PI) return 1; |
29 |
if (speed1 < 0 || speed1 > 255) return 1; |
30 |
if (speed2 < 0 || speed2 > 255) return 1; |
31 |
|
32 |
/* if speed is lower than the cut off, don't move */
|
33 |
if (abs(speed1) < CUTOFF) {
|
34 |
speed1 = 0;
|
35 |
} |
36 |
if (abs(speed2) < CUTOFF) {
|
37 |
speed2 = 0;
|
38 |
} |
39 |
|
40 |
double radius;
|
41 |
if (speed1 == speed2) {
|
42 |
/* go straight */
|
43 |
r->pose.x += cos(theta) * speed1 / MOTOR_CONVERSION_FACTOR; |
44 |
r->pose.y += sin(theta) * speed1 / MOTOR_CONVERSION_FACTOR; |
45 |
return 0; |
46 |
} |
47 |
radius = ROBOT_WIDTH * speed1 / (speed1 - speed2); |
48 |
|
49 |
double t = speed1 / radius / MOTOR_CONVERSION_FACTOR;
|
50 |
|
51 |
double newx = (radius * sin(t)) / MOTOR_CONVERSION_FACTOR;
|
52 |
double newy = (radius - radius * cos(t)) / MOTOR_CONVERSION_FACTOR;
|
53 |
|
54 |
r->pose.x += newx * cos(theta); |
55 |
r->pose.y += newx * sin(theta); |
56 |
|
57 |
r->pose.x += newy * - sin(theta); |
58 |
r->pose.y += newy * cos(theta); |
59 |
|
60 |
int divide = (t+r->pose.theta)/(2 * M_PI); |
61 |
r->pose.theta = (t+r->pose.theta) - (2 * M_PI * divide);
|
62 |
if (r->pose.theta<0) r->pose.theta += 2 * M_PI; |
63 |
|
64 |
return 0; |
65 |
} |
66 |
|