Revision 1090

View differences:

branches/simulator/projects/simulator/simulator/core/motion.c
12 12

  
13 13
#define MOTOR_CONVERSION_FACTOR 10.0
14 14

  
15
#define FUDGE 10 /* minimum rangefinder distance until collision */
16

  
15 17
/** move_robot will move a robot from its initial position, (x,y), and theta (in radians) to a new position given speed.
16 18
 * (x,y) and theta will be updated by the move_robot function instead of returning a value
17 19
 * (x,y) is some kind of absolute position in the "world", (0,0) is the top left of the "world"
......
21 23
 **/
22 24
int move_robot(Robot* r)
23 25
{
26
	Pose old_pose = r->pose;
27
	
24 28
	short speed1 = r->shared->motor1;
25 29
	short speed2 = r->shared->motor2;
26 30
	float theta = r->pose.theta;
......
61 65
	r->pose.theta = (t+r->pose.theta) - (2 * M_PI * divide);
62 66
	if (r->pose.theta<0) r->pose.theta += 2 * M_PI;
63 67

  
68
	/* XXX: this is a terrible hack  */
69
	update_rangefinders(r);
70
	for (divide = 0; divide < 5; divide++) {
71
	    if (r->shared->ranges.d[divide] < FUDGE) {
72
		r->pose = old_pose;
73
		return 0;
74
	    }
75
	}
76
	/* XXX: should store values from before we alter rangefinders 
77
	 * and restore here, but this is all temporary...right? */
78
	update_rangefinders(r);
79

  
64 80
	return 0;
65 81
}
66 82

  

Also available in: Unified diff