Revision 1091

View differences:

branches/simulator/projects/simulator/simulator/core/motion.c
3 3
#include <math.h>
4 4

  
5 5
#include "motion.h"
6

  
6
#include "rangefinders.h"
7 7
#include "robot.h"
8 8

  
9 9
#define CUTOFF 120
......
68 68
	/* XXX: this is a terrible hack  */
69 69
	update_rangefinders(r);
70 70
	for (divide = 0; divide < 5; divide++) {
71
	    /* Lets just call this a collision... */
71 72
	    if (r->shared->ranges.d[divide] < FUDGE) {
72
		r->pose = old_pose;
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);
73 79
		return 0;
74 80
	    }
75 81
	}
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 82

  
80 83
	return 0;
81 84
}

Also available in: Unified diff