Revision 1092 branches/simulator/projects/simulator/simulator/core/motion.c
motion.c  

10  10 
#define TIME 1 /*sec*/ 
11  11 
#define ROBOT_WIDTH 131 /*mm*/ 
12  12  
13 
#define MOTOR_CONVERSION_FACTOR 10.0 

13 
#define MOTOR_CONVERSION_FACTOR 1000.0


14  14  
15  15 
#define FUDGE 10 /* minimum rangefinder distance until collision */ 
16  16  
...  ...  
23  23 
**/ 
24  24 
int move_robot(Robot* r) 
25  25 
{ 
26 
printf("Called motion\n"); 

26  27 
Pose old_pose = r>pose; 
27  28 

28  29 
short speed1 = r>shared>motor1; 
...  ...  
42  43 
} 
43  44  
44  45 
double radius; 
46 
int divide; 

45  47 
if (speed1 == speed2) { 
46  48 
/* go straight */ 
47  49 
r>pose.x += cos(theta) * speed1 / MOTOR_CONVERSION_FACTOR; 
48  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 
} 

49  64 
return 0; 
50  65 
} 
51  66 
radius = ROBOT_WIDTH * speed1 / (speed1  speed2); 
...  ...  
61  76 
r>pose.x += newy *  sin(theta); 
62  77 
r>pose.y += newy * cos(theta); 
63  78 

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


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

65  80 
r>pose.theta = (t+r>pose.theta)  (2 * M_PI * divide); 
66  81 
if (r>pose.theta<0) r>pose.theta += 2 * M_PI; 
67  82  
...  ...  
69  84 
update_rangefinders(r); 
70  85 
for (divide = 0; divide < 5; divide++) { 
71  86 
/* Lets just call this a collision... */ 
87 
printf("%d: %d\n",divide,r>shared>ranges.d[divide]); 

72  88 
if (r>shared>ranges.d[divide] < FUDGE) { 
73  89 
/* Restore x,y, but allow rotation */ 
74  90 
r>pose.x = old_pose.x; 
Also available in: Unified diff