## Revision 1092

slowed down motion and hacked more crap

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