Statistics
| Branch: | Revision:

root / scout / libscout / src / test_behaviors / smart_runaround.cpp @ c43cef09

History | View | Annotate | Download (11.5 KB)

1
/**
2
 * Copyright (c) 2011 Colony Project
3
 * 
4
 * Permission is hereby granted, free of charge, to any person
5
 * obtaining a copy of this software and associated documentation
6
 * files (the "Software"), to deal in the Software without
7
 * restriction, including without limitation the rights to use,
8
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
9
 * copies of the Software, and to permit persons to whom the
10
 * Software is furnished to do so, subject to the following
11
 * conditions:
12
 * 
13
 * The above copyright notice and this permission notice shall be
14
 * included in all copies or substantial portions of the Software.
15
 * 
16
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
17
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
18
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
19
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
20
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
21
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
22
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
23
 * OTHER DEALINGS IN THE SOFTWARE.
24
 */
25

    
26
#include "smart_runaround.h"
27

    
28
using namespace std;
29

    
30

    
31
// want to have a minimal working thing, use a big enough 
32
// static array and start in the middle
33
// we assume we are facing right, that affects where we store
34
// wall information
35
// -1 for wall, 0 for unseen, 1 for traveled, 2 for critical
36
#define WALL -1
37
#define UNSEEN 0
38
#define SEEN 1
39
#define CRITICAL 2
40
// facings on map, NOT on robot
41
#define UP 0
42
#define RIGHT 1
43
#define DOWN 2
44
#define LEFT 3
45
// number of pixels on one row/column of map block
46
#define BLOCK_LENGTH 13
47

    
48
// robot control variables
49
#define BASESPEED 50
50

    
51
// robots row and column on map, starting at center
52
int row = MAP_LENGTH/2+1; // add 1 because we know MAP_LENGTH is odd
53
int col = MAP_LENGTH/2+1;
54

    
55
// utility functions
56
// pixels to meters
57
float pix_to_m(int pixels)
58
{
59
    return pixels/200.0;
60
}
61

    
62
// meters to pixels
63
int m_to_idx(float meters)
64
{
65
    float pixels = meters*200.0;
66
    float idx = pixels/BLOCK_LENGTH;
67
    return floor(idx+0.5);
68
}
69

    
70
/* return a direction (if any) where adjacent block
71
 * is labeled "info" on map. Searches clockwise
72
 * starting at up. Returns -1 if no direction valid.
73
 */
74
int smart_runaround::choose_direc(int row, int col, int info)
75
{
76
    if (map[row-1][col] == info)
77
        return UP;
78
    else if (map[row][col+1] == info)
79
        return RIGHT;
80
    else if (map[row+1][col] == info)
81
        return DOWN;
82
    else if (map[row][col-1] == info)
83
        return LEFT;
84
    return -1;
85
}
86

    
87
// TODO This is bad! It's defined globally across all behaviors. Please fix this. -Alex
88
Duration sonar_update_time2(1.5);
89

    
90
// THIS VERSION MODIFIED BY ZANE
91
void smart_runaround::run(){
92
    
93
    ROS_INFO("Starting to solve the maze");
94
    // Go up to the first line.
95
    //follow_line();
96
    // Turn the sonar on.
97
    sonar->set_on();
98
    sonar->set_range(0, 23);
99
    // initialize map to all UNSEEN
100
    for(int r = 0; r < MAP_LENGTH; r++) {
101
        for(int c = 0; c < MAP_LENGTH; c++)
102
            map[r][c] = UNSEEN;
103
    }
104

    
105
    // Wait for the sonar to initialize.
106
    while(!look_around(25, 25, RIGHT) && ok())
107
    {
108
      spinOnce();
109
    }
110

    
111
    /* Assumptions:
112
     * Grid has a fixed size
113
     * Start location unknown
114
     * When robot moves forward, it moves to exact center
115
     * of the block in front.
116
     */
117

    
118

    
119
    int dir = RIGHT; // current direction
120
    int new_dir = RIGHT; // direction in which to turn after a scan
121
    bool success = false; // true when maze solved
122
    while(ok())
123
    {
124
        // Look left, right, and forward
125
        look_around(row, col, dir);
126
        // Try moving in each direction
127
        new_dir = choose_direc(row, col, UNSEEN);
128
        if(new_dir < 0)
129
            new_dir = choose_direc(row, col, SEEN);
130
        if(new_dir >= 0) {
131
            turn_from_to(dir, new_dir);
132
            dir = new_dir;
133
        }
134
    }
135

    
136
    // Check and report final condition.
137
    if (success)
138
        ROS_INFO("YAY! I have solved the maze");
139
    else
140
        ROS_INFO("NO! The maze is unsolvable");
141
}
142

    
143
// NOT CURRENTLY USED!!!
144
bool smart_runaround::solve(int row, int col, int dir)
145
{
146
    int initial_dir = dir;
147

    
148
    ROS_INFO("I am at direction %d", dir);
149

    
150
    // use backtracking to solve the maze
151
    if (at_destination())
152
        return true;
153

    
154
    // Wait for sonar to update.
155
    sonar_update_time2.sleep();
156

    
157
    // this function should fill the adjacent cells around me with
158
    // wall's or paths
159
    while(!look_around(row, col, dir) && ok())
160
    {
161
        spinOnce();
162
    }
163

    
164
    /* try go up */
165
    if (map[row-1][col] != WALL && initial_dir != UP)
166
    {
167
    ROS_INFO("GOING UP!");
168
        // Turn up.
169
        turn_from_to(dir, UP);
170
        follow_line();
171
        // Solve recursively.
172
        bool solved = solve(row-1, col, DOWN);
173
        if (solved)
174
        {
175
            return solved;
176
        }
177
        else
178
        {
179
            //Update where we are.
180
            dir = UP;
181
        }
182
    }
183
    /* try right */
184
    if (map[row][col+1] != WALL && initial_dir != RIGHT)
185
    {
186
    ROS_INFO("GOING RIGHT!");
187
        // Turn right.
188
        turn_from_to(dir, RIGHT);
189
        follow_line();
190
        // Solve recursively.
191
        bool solved = solve(row, col+1, LEFT);
192
        if (solved)
193
        {
194
            return solved;
195
        }
196
        else
197
        {
198
            //Update where we are.
199
            dir = RIGHT;
200
        }
201
    }
202
    /* try down */
203
    if (map[row+1][col] != WALL && initial_dir != DOWN)
204
    {
205
    ROS_INFO("GOING DOWN!");
206
        // Turn down.
207
        turn_from_to(dir, DOWN);
208
        follow_line();
209
        // Solve recursively.
210
        bool solved = solve(row+1, col, UP);
211
        if (solved)
212
        {
213
            return solved;
214
        }
215
        else
216
        {
217
            //Update where we are.
218
            dir = DOWN;
219
        }
220
    }
221
    /* try left */
222
    if (map[row][col-1] != WALL && initial_dir != LEFT)
223
    {
224
    ROS_INFO("GOING LEFT!");
225
        // Turn down.
226
        turn_from_to(dir, LEFT);
227
        follow_line();
228
        // Solve recursively.
229
        bool solved = solve(row, col-1, RIGHT);
230
        if (solved)
231
        {
232
            return solved;
233
        }
234
        else
235
        {
236
            //Update where we are.
237
            dir = LEFT;
238
        }
239
    }
240

    
241
    ROS_INFO("DEAD END FOUND, TURNING BACK.");
242
    // we have exhausted all the options. This path is clearly a
243
    // dead end. go back to where we come from and return false.
244
    turn_from_to(dir, initial_dir);
245
    follow_line();
246
    return false;
247
}
248

    
249
/* this function takes in the current direction,
250
 * and turns the scout to the intended direction
251
 */
252
void smart_runaround::turn_from_to(int current_dir, int intended_dir) {
253
    switch ((4 + intended_dir - current_dir) % 4) // TODO: Try without "4 +" at start
254
    {
255
        case 0:
256
            turn_straight(intended_dir);
257
            break;
258
        case 1:
259
            turn_right();
260
            break;
261
        case 2:
262
            spot_turn();
263
            break;
264
        case 3:
265
            turn_left();
266
            break;
267
    }
268
}
269

    
270
bool smart_runaround::look_around(int row, int col, int dir)
271
{
272
    // look around current place using sonar
273
    // store whether or not
274
    // there is a wall into the map
275
    // stores at row col 2 if point is critical, 1 otherwise
276
    
277
    int* readings = sonar->get_sonar_readings();
278
    spinOnce();
279

    
280
    // Assumption: readings are given in millimeters - Zane
281

    
282
    // distances with respect to robot, NOT map
283
    // Look to the left.
284
    float left_distance = readings[0]/1000.0;
285
    int left_idx = m_to_idx(left_distance);
286
    // Look to the front.
287
    float front_distance = readings[36]/1000.0;
288
    int front_idx = m_to_idx(front_distance);
289
    // Look to the right.
290
    float right_distance = readings[24]/1000.0;
291
    int right_idx = m_to_idx(right_distance);
292

    
293
    ROS_INFO("front: %lf  left: %lf  right: %lf",
294
        front_distance, left_distance, right_distance);
295
    if (right_distance == 0 || front_distance == 0 || left_distance == 0)
296
      return false;
297

    
298
    // determine relative distances on map, based on robot position
299
    int up_d = 0;
300
    int right_d = 0;
301
    int down_d = 0;
302
    int left_d = 0;
303

    
304
    // determine upward distance
305
    switch (dir)
306
    {
307
        case UP:
308
            up_d = front_idx;
309
            right_d = right_idx;
310
            down_d = 0; // unknown
311
            left_d = left_idx;
312
            break;
313
        case RIGHT:
314
            up_d = left_idx;
315
            right_d = front_idx;
316
            down_d = right_idx;
317
            left_d = 0; // unknown
318
            break;
319
        case DOWN:
320
            up_d = 0; // unknown
321
            right_d = left_idx;
322
            down_d = front_idx;
323
            left_d = right_idx;
324
            break;
325
        case LEFT:
326
            up_d = right_idx;
327
            right_d = 0; // unknown
328
            down_d = left_idx;
329
            left_d = front_idx;
330
            break;
331
    }
332

    
333
    // change map until wall index, or until reading < 500
334
    // reading < 500 <=> left_idx < 8 (approx.)
335

    
336
    // map blocks above robot (on map)
337
    for(int u = 0; u < 8; u++)
338
    {
339
        if(u == up_d) {
340
            map[row-u][col] = (up_d)?WALL:SEEN;
341
            break;
342
        }
343
        map[row-u][col] = SEEN;
344
    }
345

    
346
    // map blocks to right of robot
347
    for(int r = 0; r < 8; r++)
348
    {
349
        if(r == right_d) {
350
            map[row][col+r] = (right_d)?WALL:SEEN;
351
                break;
352
        }
353
        map[row][col+r] = SEEN;
354
    }
355

    
356
    // map blocks under robot (on map)
357
    for(int d = 0; d < 8; d++)
358
    {
359
        if(d == down_d) {
360
            map[row+d][col] = (down_d)?WALL:SEEN;
361
                break;
362
        }
363
        map[row+d][col] = SEEN;
364
    }
365

    
366
    // map blocks to left of robot
367
    for(int l = 0; l < 8; l++)
368
    {
369
        if(l == left_d) {
370
            map[row][col-l] = (left_d)?WALL:SEEN;
371
                break;
372
        }
373
        map[row][col-l] = SEEN;
374
    }
375

    
376
    return true;
377
}
378

    
379
// TODO: test this function
380
// return true iff the map has been completely explored
381
// and only true if it "returned" to home destination after solving
382
bool smart_runaround::at_destination() 
383
{
384
    // check whether there is a square portion with
385
    // only SEEN or WALL blocks, of the size of the map.
386
    //int start_row = -1;       // Unused
387
    //int start_col = -1;       // Unused
388
    int seen_width = 0;
389
    int seen_height = 0;
390
    for(int r = 0; r < MAP_LENGTH; r++) {
391
        for(int c = 0; c < MAP_LENGTH; c++) {
392
            if(map[r][c] == UNSEEN) {
393
            //start_row = -1;
394
            //start_col = -1;
395
            seen_width = 0;
396
            seen_height = 0;
397
            }
398
            else {
399
            //start_row = r;
400
            //start_col = c;
401
            seen_width++;
402
            seen_height++;
403
            }
404
            if(seen_width >= 15)
405
            return true;
406
        }
407
    }
408
    return false;
409
}
410

    
411
// TODO: test these functions to make sure robot moves well
412
// move forward one block in direction "dir"
413
void smart_runaround::turn_straight(int dir)
414
{
415
    // TODO: try to use motor encoder values to move forward enough
416
    motors->set_sides(BASESPEED, BASESPEED, MOTOR_ABSOLUTE);
417
    spinOnce();
418
    Duration moveTime(1.0);
419
    moveTime.sleep();
420

    
421
    // update robot's position on map
422
    switch(dir)
423
    {
424
        case UP:
425
          row--;
426
          break;
427
        case RIGHT:
428
          col++;
429
          break;
430
        case DOWN:
431
          row++;
432
          break;
433
        case LEFT:
434
          col--;
435
          break;
436
    }
437
}
438

    
439
// turn clockwise (right)
440
void smart_runaround::turn_right()
441
{
442
    motors->set_sides(BASESPEED, -BASESPEED, MOTOR_ABSOLUTE);
443
    spinOnce();
444
    Duration moveTime(3.3);
445
    moveTime.sleep();
446
}
447

    
448
// do a 180 deg turn, NOT a barrel roll
449
void smart_runaround::spot_turn()
450
{
451
    motors->set_sides(BASESPEED, -BASESPEED, MOTOR_ABSOLUTE);
452
    spinOnce();
453
    Duration moveTime(6.6);
454
    moveTime.sleep();
455
}
456

    
457
// turn counter-clockwise (left)
458
void smart_runaround::turn_left()
459
{
460
    motors->set_sides(-BASESPEED, BASESPEED, MOTOR_ABSOLUTE);
461
    spinOnce();
462
    Duration moveTime(3.3);
463
    moveTime.sleep();
464
}