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 