Revision 1027
use abs in stdlib instead of a crappy macro
motion.c | ||
---|---|---|
1 | 1 |
#include <stdio.h> |
2 |
#include <stdlib.h> |
|
2 | 3 |
#include <math.h> |
3 | 4 |
|
4 | 5 |
#include "motion.h" |
5 | 6 |
|
6 | 7 |
#include "robot.h" |
7 | 8 |
|
8 |
#define CUTOFF 120 |
|
9 |
#define ABS(x) (x<0?-x:x) |
|
10 |
#define TIME 1 /*sec*/ |
|
9 |
#define CUTOFF 120 |
|
10 |
#define TIME 1 /*sec*/ |
|
11 | 11 |
#define ROBOT_WIDTH 131 /*mm*/ |
12 | 12 |
|
13 | 13 |
#define MOTOR_CONVERSION_FACTOR 10.0 |
14 | 14 |
|
15 |
/** move 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 function instead of returning a value |
|
17 |
* (x,y) is some kind of absolute position in the "world", let's make (0,0) the top left of the "world"
|
|
18 |
* theta will an angle be between 0 - 2*Pi (0 being faces east)
|
|
19 |
* speed is between 0 - 255, there is some magical cutoff point before the motors actually starts running
|
|
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 | 20 |
* move will return 0 if successful |
21 | 21 |
**/ |
22 | 22 |
int move_robot(Robot* r) |
... | ... | |
30 | 30 |
if (speed2 < 0 || speed2 > 255) return 1; |
31 | 31 |
|
32 | 32 |
/* if speed is lower than the cut off, don't move */ |
33 |
if (ABS(speed1) < CUTOFF) {
|
|
33 |
if (abs(speed1) < CUTOFF) {
|
|
34 | 34 |
speed1 = 0; |
35 | 35 |
} |
36 |
if (ABS(speed2) < CUTOFF) {
|
|
36 |
if (abs(speed2) < CUTOFF) {
|
|
37 | 37 |
speed2 = 0; |
38 | 38 |
} |
39 | 39 |
speed1 = (speed1 - CUTOFF); |
40 | 40 |
speed2 = (speed2 - CUTOFF); |
41 | 41 |
double radius; |
42 | 42 |
if (speed1 == speed2) { |
43 |
/* go straight */
|
|
44 |
r->pose.x += cos(theta) * speed1 / MOTOR_CONVERSION_FACTOR; |
|
45 |
r->pose.y += sin(theta) * speed1 / MOTOR_CONVERSION_FACTOR; |
|
46 |
return 0;
|
|
43 |
/* go straight */
|
|
44 |
r->pose.x += cos(theta) * speed1 / MOTOR_CONVERSION_FACTOR;
|
|
45 |
r->pose.y += sin(theta) * speed1 / MOTOR_CONVERSION_FACTOR;
|
|
46 |
return 0;
|
|
47 | 47 |
} |
48 | 48 |
radius = ROBOT_WIDTH * speed1 / (speed1 - speed2); |
49 | 49 |
|
Also available in: Unified diff