## Revision b9e59a3c

 ID b9e59a3c06de728f48cd63131235c46ae0b2de98 Parent 5d1c5d81 Child 98a7f961

Compiles.

View differences:

scout/libscout/src/test_behaviors/smart_runaround.cpp
178 178
```    spinOnce();
```
179 179

180 180
```    // Assumption: readings are given in millimeters - Zane
```
181

182
```<<<<<<< HEAD
```
183
```    // matrices for going from robot's frame to base frame
```
184
```    float rightMat[2][2] = {{0, 1}, {-1, 0}};
```
185
```    float downMat[2][2] = {{-1, 0}, {0, -1}};
```
186
```    float leftMat[2][2] = {{0, 1}, {1, 0}};
```
187

188
```    // Look to the left (and update map).
```
189
```    float left_distance = readings[0]; // w.r.t. robot
```
190
```    if(left_distance == 0)
```
191
```        return false;
```
192
```    int left_idx = -mm_to_idx(left_distance); // w.r.t. map
```
193
```    // plot to map if indices are valid
```
194
```    if (0 <= left_idx && left_idx < MAP_LENGTH)
```
195
```        map[row][col+left_idx] = SEEN;
```
196

197
```    // Look in the other directions (and update map).
```
198
```    for (int i = 24; i < 48; i++) {
```
199
```        float distance = readings[i]; // w.r.t. robot
```
200
```        if(distance == 0)
```
201
```            return false;
```
202
```        if(distance >= 500)
```
203
```            break; // too far to be accurate
```
204
```        float theta = (M_PI/24)*i - M_PI;
```
205
```        float xDist = distance*cos(theta); // w.r.t. robot
```
206
```        float yDist = distance*sin(theta); // w.r.t. robot
```
207
```        float inputs[2] = {xDist, yDist};
```
208
```        float *ans;
```
209

210
```        // re-orient x and y distances based on direction
```
211
```        switch(dir) {
```
212
```            case UP:
```
213
```                ans[0] = xDist;
```
214
```                ans[1] = yDist;
```
215
```                break;
```
216
```            case RIGHT:
```
217
```                ans = matrix_mult(inputs, rightMat);
```
218
```                break;
```
219
```            case DOWN:
```
220
```                ans = matrix_mult(inputs, downMat);
```
221
```                break;
```
222
```            case LEFT:
```
223
```                ans = matrix_mult(inputs, leftMat);
```
224
```                break;
```
225
```        }
```
226
```        // indices into the map
```
227
```        int pixDistX = row + mm_to_idx(ans[0]);
```
228
```        int pixDistY = col + mm_to_idx(ans[1]);
```
229
```        // plot to map if indices are valid
```
230
```        if (0 <= pixDistX && pixDistX < MAP_LENGTH
```
231
```            && 0 <= pixDistY && pixDistY < MAP_LENGTH)
```
232
```                map[pixDistX][pixDistY] = SEEN;
```
233
```=======
```
234 181
```    // distances with respect to robot, NOT map
```
235 182
```    // Look to the left.
```
236 183
```    float left_distance = readings[0]/1000.0;
```
237
```    int left_idx = m_to_idx(left_distance);
```
184
```    int left_idx = mm_to_idx(left_distance);
```
238 185
```    // Look to the front.
```
239 186
```    float front_distance = readings[36]/1000.0;
```
240
```    int front_idx = m_to_idx(front_distance);
```
187
```    int front_idx = mm_to_idx(front_distance);
```
241 188
```    // Look to the right.
```
242 189
```    float right_distance = readings[24]/1000.0;
```
243
```    int right_idx = m_to_idx(right_distance);
```
190
```    int right_idx = mm_to_idx(right_distance);
```
244 191

245 192
```    ROS_INFO("front: %lf  left: %lf  right: %lf",
```
246 193
```        front_distance, left_distance, right_distance);
```
......
323 270
```                break;
```
324 271
```        }
```
325 272
```        map[row][col-l] = SEEN;
```
326
```>>>>>>> dbbad8ed5cba08f5500e7d333beab2d089f9956c
```
327 273
```    }
```
328 274
```    return true;
```
329 275
```}
```

Also available in: Unified diff