root / branches / simulator / projects / simulator / simulator / core / motion.c @ 1092
History  View  Annotate  Download (2.83 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 1000.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 
printf("Called motion\n");

27 
Pose old_pose = r>pose; 
28 

29 
short speed1 = r>shared>motor1;

30 
short speed2 = r>shared>motor2;

31 
float theta = r>pose.theta;

32 

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

38 
if (abs(speed1) < CUTOFF) {

39 
speed1 = 0;

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

42 
speed2 = 0;

43 
} 
44  
45 
double radius;

46 
int divide;

47 
if (speed1 == speed2) {

48 
/* go straight */

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

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

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

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

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

60 
update_rangefinders(r); 
61 
return 0; 
62 
} 
63 
} 
64 
return 0; 
65 
} 
66 
radius = ROBOT_WIDTH * speed1 / (speed1  speed2); 
67 

68 
double t = speed1 / radius / MOTOR_CONVERSION_FACTOR;

69  
70 
double newx = (radius * sin(t)) / MOTOR_CONVERSION_FACTOR;

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

72 

73 
r>pose.x += newx * cos(theta); 
74 
r>pose.y += newx * sin(theta); 
75  
76 
r>pose.x += newy *  sin(theta); 
77 
r>pose.y += newy * cos(theta); 
78 

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

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

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

84 
update_rangefinders(r); 
85 
for (divide = 0; divide < 5; divide++) { 
86 
/* Lets just call this a collision... */

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

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

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

90 
r>pose.x = old_pose.x; 
91 
r>pose.y = old_pose.y; 
92  
93 
/* Rotated robot, need to recalculate */

94 
update_rangefinders(r); 
95 
return 0; 
96 
} 
97 
} 
98  
99 
return 0; 
100 
} 
101 