Revision 1048

rangefinders are in fact working now

View differences:

world.c
83 83
```    xint = p3.x+ua*(p4.x-p3.x);
```
84 84
```    yint = p3.y+ua*(p4.y-p3.y);
```
85 85
```    dist = sqrt((xint-p1.x)*(xint-p1.x)+(yint-p1.y)*(yint-p1.y));
```
86
```    //printf("Got: %g\n", dist);
```
86 87
```    return dist;
```
87 88

88 89
```}
```
......
119 120
```double collide_world(ray_t *ray)
```
120 121
```{
```
121 122
```    double min = RAY_MISS;
```
122
```    int i, x;
```
123
```    double x;
```
124
```    int i;
```
123 125
```    for (i = 0; i < world.cur_objs; i++) {
```
124 126
```	x = collide(ray, &world.objs[i]);
```
125 127
```	min = (x < min) ? x : min;
```
......
231 233
``` */
```
232 234
```/* These need to be measured... */
```
233 235
```double rf_thetas[5] = {0.0, 2*M_PI/5, 4*M_PI/5, 6*M_PI/5, 8*M_PI/5};
```
236
```//double rf_thetas[5] = {0.0, 0.0, M_PI/2, M_PI, 3*M_PI/2};
```
234 237
```void update_rangefinders(Robot *bot)
```
235 238
```{
```
236 239
```    RangeFinder r = bot->shared->ranges;
```
......
238 241
```    /* Motion code has +y going down, I don't.
```
239 242
```     * So to compensate, just reflect theta up to first quadrant.*/
```
240 243
```    double theta = bot->pose.theta;
```
241
```    theta = 2*M_PI - theta;
```
242
```    //ray_t rf = {0,0,0};
```
243 244
```    int ir;
```
244 245
```    double x;
```
245 246
```    for (ir = 0; ir < 5; ir++)
```
246 247
```    {
```
247
```	rf.d = rf_thetas[ir];
```
248
```	rf.d = theta + rf_thetas[ir];
```
248 249
```	x = collide_world(&rf);
```
249
```	//printf("[%d] = %g\n", ir, x);
```
250
```	printf("@(%g) - [%d] = %g --> %d\n",rf.d, ir, x, (short)x);
```
250 251
```	bot->shared->ranges.d[ir] = x;
```
251 252
```    }
```
252 253
```}
```
......
332 333
```  fclose(fin);
```
333 334
```  return 0;
```
334 335
```}
```
335

Also available in: Unified diff