Revision 1092

View differences:

branches/simulator/projects/simulator/simulator/core/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