Revision 7f2ccb46

View differences:

scout/libscout/src/BehaviorList.cpp
26 26
  behavior_list.push_back(behavior<draw_cw_circle>);
27 27
  behavior_list.push_back(behavior<line_follow>);
28 28
  behavior_list.push_back(behavior<navigationMap>);
29
  behavior_list.push_back(behavior<smart_runaround>);
30 29
  behavior_list.push_back(behavior<wl_test>);
31 30
  behavior_list.push_back(behavior<Scheduler>);
32 31
  behavior_list.push_back(behavior<WH_Robot>);
33 32
  behavior_list.push_back(behavior<maze_solve>);
33
  behavior_list.push_back(behavior<smart_runaround>);
34 34
  return;
35 35
}
36 36

  
scout/libscout/src/BehaviorList.h
27 27
#include "behaviors/draw_cw_circle.h"
28 28
#include "behaviors/line_follow.h"
29 29
#include "behaviors/navigationMap.h"
30
#include "behaviors/smart_runaround.h"
31 30
#include "behaviors/wl_test.h"
32 31
#include "test_behaviors/Scheduler.h"
33 32
#include "test_behaviors/WH_Robot.h"
34 33
#include "test_behaviors/maze_solve.h"
34
#include "test_behaviors/smart_runaround.h"
35 35

  
36 36
template<typename T> Behavior* behavior(std::string scoutname, Sensors* sensors){ return (Behavior*)new T(scoutname, sensors); } 
37 37

  
scout/libscout/src/behaviors/smart_runaround.cpp
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 "maze_solve.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 maze_solve::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
Duration sonar_update_time(1.5);
88

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

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

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

  
117

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

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

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

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

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

  
153
    // Wait for sonar to update.
154
    sonar_update_time.sleep();
155

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

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

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

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

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

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

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

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

  
296
    // determine relative distances on map, based on robot position
297
    int up_d, right_d, down_d, left_d;
298
    // determine upward distance
299
    switch (dir)
300
    {
301
	case UP:
302
	    up_d = front_idx;
303
	    right_d = right_idx;
304
	    down_d = 0; // unknown
305
	    left_d = left_idx;
306
	    break;
307
	case RIGHT:
308
	    up_d = left_idx;
309
	    right_d = front_idx;
310
	    down_d = right_idx;
311
	    left_d = 0; // unknown
312
	    break;
313
	case DOWN:
314
	    up_d = 0; // unknown
315
	    right_d = left_idx;
316
	    down_d = front_idx;
317
	    left_d = right_idx;
318
	    break;
319
	case LEFT:
320
	    up_d = right_idx;
321
	    right_d = 0; // unknown
322
	    down_d = left_idx;
323
	    left_d = front_idx;
324
	    break;
325
    }
326

  
327
    // change map until wall index, or until reading < 500
328
    // reading < 500 <=> left_idx < 8 (approx.)
329

  
330
    // map blocks above robot (on map)
331
    for(int u = 0; u < 8; u++)
332
    {
333
	if(u = up_d) {
334
	    map[row-u][col] = (up_d)?WALL:SEEN;
335
            break;
336
	}
337
	map[row-u][col] = SEEN;
338
    }
339

  
340
    // map blocks to right of robot
341
    for(int r = 0; r < 8; r++)
342
    {
343
	if(r = right_d) {
344
	    map[row][col+r] = (right_d)?WALL:SEEN;
345
            break;
346
	}
347
	map[row][col+r] = SEEN;
348
    }
349

  
350
    // map blocks under robot (on map)
351
    for(int d = 0; d < 8; d++)
352
    {
353
	if(d = down_d) {
354
	    map[row+d][col] = (down_d)?WALL:SEEN;
355
            break;
356
	}
357
	map[row+d][col] = SEEN;
358
    }
359

  
360
    // map blocks to left of robot
361
    for(int l = 0; l < 8; l++)
362
    {
363
	if(l = left_d) {
364
	    map[row][col-l] = (left_d)?WALL:SEEN;
365
            break;
366
	}
367
	map[row][col-l] = SEEN;
368
    }
369

  
370
    return true;
371
}
372

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

  
405
// TODO: test these functions to make sure robot moves well
406
// move forward one block in direction "dir"
407
void maze_solve::turn_straight(int dir)
408
{
409
    // TODO: try to use motor encoder values to move forward enough
410
    motors->set_sides(BASESPEED, BASESPEED, MOTOR_ABSOLUTE);
411
    spinOnce();
412
    Duration moveTime(1.0);
413
    moveTime.sleep();
414

  
415
    // update robot's position on map
416
    switch(dir)
417
    {
418
	case UP:
419
	  row--;
420
	  break;
421
	case RIGHT:
422
	  col++;
423
	  break;
424
	case DOWN:
425
	  row++;
426
	  break;
427
	case LEFT:
428
	  col--;
429
	  break;
430
    }
431
}
432

  
433
// turn clockwise (right)
434
void maze_solve::turn_right()
435
{
436
    motors->set_sides(BASESPEED, -BASESPEED, MOTOR_ABSOLUTE);
437
    spinOnce();
438
    Duration moveTime(3.3);
439
    moveTime.sleep();
440
}
441

  
442
// do a 180 deg turn, NOT a barrel roll
443
void maze_solve::spot_turn()
444
{
445
    motors->set_sides(BASESPEED, -BASESPEED, MOTOR_ABSOLUTE);
446
    spinOnce();
447
    Duration moveTime(6.6);
448
    moveTime.sleep();
449
}
450

  
451
// turn counter-clockwise (left)
452
void maze_solve::turn_left()
453
{
454
    motors->set_sides(-BASESPEED, BASESPEED, MOTOR_ABSOLUTE);
455
    spinOnce();
456
    Duration moveTime(3.3);
457
    moveTime.sleep();
458
}
scout/libscout/src/behaviors/smart_runaround.h
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
#ifndef _MAZE_SOLVE_H_
27
#define _MAZE_SOLVE_H_
28

  
29
#include "line_follow.h"
30
/* Details about map:
31
 * 1 meter = 200 pixels
32
 * 1 block is a 13x13 pixel section
33
 * actual meter x meter map has 15 blocks in one row/column
34
 * # blocks per robot map row = 2*(block per map's row)-1
35
 */
36

  
37
// number of blocks in one row/column of robot's map
38
#define MAP_LENGTH 29
39

  
40
class maze_solve: public line_follow
41
{
42
    public:
43
        maze_solve(std::string scoutname, Sensors* sensors):
44
            line_follow(scoutname, "maze_solve", sensors) {};
45
        void run();
46
    private:
47
	int choose_direc(int row, int col, int info);
48
        bool solve(int row, int col, int dir);
49
        void turn_from_to(int current_dir, int intended_dir);
50
        bool look_around(int row, int col, int dir);
51
        bool at_destination();
52
	// functions for changing direction or moving
53
        void turn_straight(int dir);
54
        void turn_right();
55
        void spot_turn();
56
	void turn_left();
57

  
58
	/* Initialize map so that boundaries MAP_LENGTH away
59
	 * from the center. Then car won't go outside of
60
	 * map even if it starts at a boundary and goes to
61
	 * the other side.
62
         * Rows top to bottom, and columns left to right
63
	*/
64
        int map[MAP_LENGTH][MAP_LENGTH];
65
};
66
#endif
67

  
scout/libscout/src/test_behaviors/maze_solve.cpp
43 43
#define DOWN 2
44 44
#define LEFT 3
45 45

  
46
// TODO This is bad! It's defined globally across all files. Please put it inside a good scope. -Alex
46 47
Duration sonar_update_time(1.5);
47 48

  
48 49
void maze_solve::run()
scout/libscout/src/test_behaviors/smart_runaround.cpp
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: %d  left: %d  right: %d", front_distance, left_distance, right_distance);
294
    if (right_distance == 0 || front_distance == 0 || left_distance == 0)
295
      return false;
296

  
297
    // determine relative distances on map, based on robot position
298
    int up_d, right_d, down_d, left_d;
299
    // determine upward distance
300
    switch (dir)
301
    {
302
	case UP:
303
	    up_d = front_idx;
304
	    right_d = right_idx;
305
	    down_d = 0; // unknown
306
	    left_d = left_idx;
307
	    break;
308
	case RIGHT:
309
	    up_d = left_idx;
310
	    right_d = front_idx;
311
	    down_d = right_idx;
312
	    left_d = 0; // unknown
313
	    break;
314
	case DOWN:
315
	    up_d = 0; // unknown
316
	    right_d = left_idx;
317
	    down_d = front_idx;
318
	    left_d = right_idx;
319
	    break;
320
	case LEFT:
321
	    up_d = right_idx;
322
	    right_d = 0; // unknown
323
	    down_d = left_idx;
324
	    left_d = front_idx;
325
	    break;
326
    }
327

  
328
    // change map until wall index, or until reading < 500
329
    // reading < 500 <=> left_idx < 8 (approx.)
330

  
331
    // map blocks above robot (on map)
332
    for(int u = 0; u < 8; u++)
333
    {
334
	if(u = up_d) {
335
	    map[row-u][col] = (up_d)?WALL:SEEN;
336
            break;
337
	}
338
	map[row-u][col] = SEEN;
339
    }
340

  
341
    // map blocks to right of robot
342
    for(int r = 0; r < 8; r++)
343
    {
344
	if(r = right_d) {
345
	    map[row][col+r] = (right_d)?WALL:SEEN;
346
            break;
347
	}
348
	map[row][col+r] = SEEN;
349
    }
350

  
351
    // map blocks under robot (on map)
352
    for(int d = 0; d < 8; d++)
353
    {
354
	if(d = down_d) {
355
	    map[row+d][col] = (down_d)?WALL:SEEN;
356
            break;
357
	}
358
	map[row+d][col] = SEEN;
359
    }
360

  
361
    // map blocks to left of robot
362
    for(int l = 0; l < 8; l++)
363
    {
364
	if(l = left_d) {
365
	    map[row][col-l] = (left_d)?WALL:SEEN;
366
            break;
367
	}
368
	map[row][col-l] = SEEN;
369
    }
370

  
371
    return true;
372
}
373

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

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

  
416
    // update robot's position on map
417
    switch(dir)
418
    {
419
	case UP:
420
	  row--;
421
	  break;
422
	case RIGHT:
423
	  col++;
424
	  break;
425
	case DOWN:
426
	  row++;
427
	  break;
428
	case LEFT:
429
	  col--;
430
	  break;
431
    }
432
}
433

  
434
// turn clockwise (right)
435
void smart_runaround::turn_right()
436
{
437
    motors->set_sides(BASESPEED, -BASESPEED, MOTOR_ABSOLUTE);
438
    spinOnce();
439
    Duration moveTime(3.3);
440
    moveTime.sleep();
441
}
442

  
443
// do a 180 deg turn, NOT a barrel roll
444
void smart_runaround::spot_turn()
445
{
446
    motors->set_sides(BASESPEED, -BASESPEED, MOTOR_ABSOLUTE);
447
    spinOnce();
448
    Duration moveTime(6.6);
449
    moveTime.sleep();
450
}
451

  
452
// turn counter-clockwise (left)
453
void smart_runaround::turn_left()
454
{
455
    motors->set_sides(-BASESPEED, BASESPEED, MOTOR_ABSOLUTE);
456
    spinOnce();
457
    Duration moveTime(3.3);
458
    moveTime.sleep();
459
}
scout/libscout/src/test_behaviors/smart_runaround.h
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
#ifndef _SMART_RUNAROUND_H_
27
#define _SMART_RUNAROUND_H_
28

  
29
#include "../behaviors/line_follow.h"
30
/* Details about map:
31
 * 1 meter = 200 pixels
32
 * 1 block is a 13x13 pixel section
33
 * actual meter x meter map has 15 blocks in one row/column
34
 * # blocks per robot map row = 2*(block per map's row)-1
35
 */
36

  
37
// number of blocks in one row/column of robot's map
38
#define MAP_LENGTH 29
39

  
40
class smart_runaround: public line_follow
41
{
42
    public:
43
        smart_runaround(std::string scoutname, Sensors* sensors):
44
            line_follow(scoutname, "smart_runaround", sensors) {};
45
        void run();
46
    private:
47
	int choose_direc(int row, int col, int info);
48
        bool solve(int row, int col, int dir);
49
        void turn_from_to(int current_dir, int intended_dir);
50
        bool look_around(int row, int col, int dir);
51
        bool at_destination();
52
	// functions for changing direction or moving
53
        void turn_straight(int dir);
54
        void turn_right();
55
        void spot_turn();
56
	void turn_left();
57

  
58
	/* Initialize map so that boundaries MAP_LENGTH away
59
	 * from the center. Then car won't go outside of
60
	 * map even if it starts at a boundary and goes to
61
	 * the other side.
62
         * Rows top to bottom, and columns left to right
63
	*/
64
        int map[MAP_LENGTH][MAP_LENGTH];
65
};
66
#endif
67

  

Also available in: Unified diff