root / branches / simulator / projects / simulator / simulator / core / motion.c @ 1091
History  View  Annotate  Download (2.36 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 10.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 
if (speed1 == speed2) {

46 
/* go straight */

47 
r>pose.x += cos(theta) * speed1 / MOTOR_CONVERSION_FACTOR; 
48 
r>pose.y += sin(theta) * speed1 / MOTOR_CONVERSION_FACTOR; 
49 
return 0; 
50 
} 
51 
radius = ROBOT_WIDTH * speed1 / (speed1  speed2); 
52 

53 
double t = speed1 / radius / MOTOR_CONVERSION_FACTOR;

54  
55 
double newx = (radius * sin(t)) / MOTOR_CONVERSION_FACTOR;

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

57 

58 
r>pose.x += newx * cos(theta); 
59 
r>pose.y += newx * sin(theta); 
60  
61 
r>pose.x += newy *  sin(theta); 
62 
r>pose.y += newy * cos(theta); 
63 

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

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

69 
update_rangefinders(r); 
70 
for (divide = 0; divide < 5; divide++) { 
71 
/* Lets just call this a collision... */

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

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

74 
r>pose.x = old_pose.x; 
75 
r>pose.y = old_pose.y; 
76  
77 
/* Rotated robot, need to recalculate */

78 
update_rangefinders(r); 
79 
return 0; 
80 
} 
81 
} 
82  
83 
return 0; 
84 
} 
85 