root / branches / simulator / projects / simulator / simulator / core / motion.c @ 1140
History  View  Annotate  Download (2.95 KB)
1 
#include <stdio.h> 

2 
#include <stdlib.h> 
3 
#include <math.h> 
4  
5 
#include "motion.h" 
6 
#include "rangefinders.h" 
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 100.0 
14  
15 
#define FUDGE 10 /* minimum rangefinder distance until collision */ 
16  
17 
/** move_robot will move a robot from its initial position, (x,y), and theta (in radians) to a new position given speed.

18 
* (x,y) and theta will be updated by the move_robot function instead of returning a value

19 
* (x,y) is some kind of absolute position in the "world", (0,0) is the top left of the "world"

20 
* theta will an angle be between 0 and 2*Pi (0 being faces east and goes clockwise)

21 
* speed is between 0 and 255, there is some magical cutoff point before the motors actually starts running

22 
* move will return 0 if successful

23 
**/

24 
int move_robot(Robot* r)

25 
{ 
26 
Pose old_pose = r>pose; 
27 

28 
short speed1 = r>shared>motor1;

29 
short speed2 = r>shared>motor2;

30 
float theta = r>pose.theta;

31 

32 
if (theta < 0  theta > 2*M_PI) return 1; 
33 
if (speed1 < 255  speed1 > 255) return 1; 
34 
if (speed2 < 255  speed2 > 255) return 1; 
35  
36 
/* if speed is lower than the cut off, don't move */

37 
if (abs(speed1) < CUTOFF) {

38 
speed1 = 0;

39 
} 
40 
if (abs(speed2) < CUTOFF) {

41 
speed2 = 0;

42 
} 
43  
44 
double radius;

45 
int divide;

46 
if (speed1 == speed2) {

47 
/* go straight */

48 
r>pose.x += cos(theta) * speed1 / MOTOR_CONVERSION_FACTOR; 
49 
r>pose.y += sin(theta) * speed1 / MOTOR_CONVERSION_FACTOR; 
50 
for (divide = 0; divide < 5; divide++) { 
51 
/* Lets just call this a collision... */

52 
//printf("%d: %d\n",divide,r>shared>ranges.d[divide]);

53 
if (r>shared>ranges.d[divide] < FUDGE) {

54 
/* Restore x,y, but allow rotation */

55 
r>pose.x = old_pose.x; 
56 
r>pose.y = old_pose.y; 
57  
58 
/* Rotated robot, need to recalculate */

59 
update_rangefinders(r); 
60 
return 0; 
61 
} 
62 
} 
63 
return 0; 
64 
} 
65  
66 
double t;

67 
if (speed1 != 0) 
68 
{ 
69 
radius = ROBOT_WIDTH * speed1 / (speed1  speed2); 
70 
t = speed1 / radius / MOTOR_CONVERSION_FACTOR; 
71 
} 
72 
else

73 
{ 
74 
radius = ROBOT_WIDTH * speed2 / (speed1  speed2); 
75 
t = speed2 / radius / MOTOR_CONVERSION_FACTOR; 
76 
} 
77  
78 
double newx = (radius * sin(t)) / MOTOR_CONVERSION_FACTOR;

79 
double newy = (radius  radius * cos(t)) / MOTOR_CONVERSION_FACTOR;

80 

81 
r>pose.x += newx * cos(theta); 
82 
r>pose.y += newx * sin(theta); 
83  
84 
r>pose.x += newy *  sin(theta); 
85 
r>pose.y += newy * cos(theta); 
86 

87 
divide = (t+r>pose.theta)/(2 * M_PI);

88 
r>pose.theta = (t+r>pose.theta)  (2 * M_PI * divide);

89 
if (r>pose.theta<0) r>pose.theta += 2 * M_PI; 
90  
91 
/* XXX: this is a terrible hack */

92 
update_rangefinders(r); 
93 
for (divide = 0; divide < 5; divide++) { 
94 
/* Lets just call this a collision... */

95 
//printf("%d: %d\n",divide,r>shared>ranges.d[divide]);

96 
if (r>shared>ranges.d[divide] < FUDGE) {

97 
/* Restore x,y, but allow rotation */

98 
r>pose.x = old_pose.x; 
99 
r>pose.y = old_pose.y; 
100  
101 
/* Rotated robot, need to recalculate */

102 
update_rangefinders(r); 
103 
return 0; 
104 
} 
105 
} 
106  
107 
return 0; 
108 
} 
109 