Statistics
| Branch: | Revision:

scoutos / scout / libscout / src / test_behaviors / smart_runaround.cpp @ b9e59a3c

History | View | Annotate | Download (9.3 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
// millimeters to pixels
63
int mm_to_idx(float meters)
64
{
65
    // 200 pixels per meter, and 1000 millimeters per meter
66
    float pixels = meters*0.2;
67
    float idx = pixels/BLOCK_LENGTH;
68
    return floor(idx+0.5);
69
}
70

    
71
float *matrix_mult(float inputs[2], float matrix[2][2])
72
{
73
    float newX = matrix[0][0]*inputs[0]+matrix[0][1]*inputs[1];
74
    float newY = matrix[1][0]*inputs[0]+matrix[1][1]*inputs[1];
75
    float output[2] = {newX, newY};
76
    return output;
77
}
78

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

    
82
// THIS VERSION MODIFIED BY ZANE
83
void smart_runaround::run(){
84
    
85
    ROS_INFO("Starting to solve the maze");
86
    // Go up to the first line.
87
    //follow_line();
88
    // Turn the sonar on.
89
    sonar->set_on();
90
    sonar->set_range(0, 23);
91
    // initialize map to all UNSEEN
92
    for(int r = 0; r < MAP_LENGTH; r++) {
93
        for(int c = 0; c < MAP_LENGTH; c++)
94
            map[r][c] = UNSEEN;
95
    }
96

    
97
    // Wait for the sonar to initialize.
98
    while(!look_around(25, 25, RIGHT) && ok())
99
    {
100
      spinOnce();
101
    }
102

    
103
    /* Assumptions:
104
     * Grid has a fixed size
105
     * Start location unknown
106
     * When robot moves forward, it moves to exact center
107
     * of the block in front.
108
     */
109

    
110

    
111
    int dir = RIGHT; // current direction
112
    //int new_dir = RIGHT; // direction in which to turn after a scan
113
    bool success = false; // true when maze solved
114
    while(ok())
115
    {
116
        look_around(row, col, dir);
117
        // Try moving in each direction
118
        /*new_dir = choose_direc(row, col, UNSEEN);
119
        if(new_dir < 0)
120
            new_dir = choose_direc(row, col, SEEN);
121
        if(new_dir >= 0) {
122
            turn_from_to(dir, new_dir);
123
            dir = new_dir;
124
        }*/
125
    }
126

    
127
    // Check and report final condition.
128
    if (success)
129
        ROS_INFO("YAY! I have solved the maze");
130
    else
131
        ROS_INFO("NO! The maze is unsolvable");
132
}
133

    
134
/* return a direction (if any) where adjacent block
135
 * is labeled "info" on map. Searches clockwise
136
 * starting at up. Returns -1 if no direction valid.
137
 */
138
int smart_runaround::choose_direc(int row, int col, int info)
139
{
140
    if (map[row-1][col] == info)
141
        return UP;
142
    else if (map[row][col+1] == info)
143
        return RIGHT;
144
    else if (map[row+1][col] == info)
145
        return DOWN;
146
    else if (map[row][col-1] == info)
147
        return LEFT;
148
    return -1;
149
}
150

    
151
/* this function takes in the current direction,
152
 * and turns the scout to the intended direction
153
 */
154
void smart_runaround::turn_from_to(int current_dir, int intended_dir) {
155
    switch ((4 + intended_dir - current_dir) % 4) // TODO: Try without "4 +" at start
156
    {
157
        case 0:
158
            turn_straight(intended_dir);
159
            break;
160
        case 1:
161
            turn_right();
162
            break;
163
        case 2:
164
            spot_turn();
165
            break;
166
        case 3:
167
            turn_left();
168
            break;
169
    }
170
}
171

    
172
/* Purpose: look front, left, and right using sonar, and update
173
 * map accordingly. Returns true if and only if sonar is initialized.
174
 */
175
bool smart_runaround::look_around(int row, int col, int dir)
176
{
177
    int* readings = sonar->get_sonar_readings();
178
    spinOnce();
179

    
180
    // Assumption: readings are given in millimeters - Zane
181
    // distances with respect to robot, NOT map
182
    // Look to the left.
183
    float left_distance = readings[0]/1000.0;
184
    int left_idx = mm_to_idx(left_distance);
185
    // Look to the front.
186
    float front_distance = readings[36]/1000.0;
187
    int front_idx = mm_to_idx(front_distance);
188
    // Look to the right.
189
    float right_distance = readings[24]/1000.0;
190
    int right_idx = mm_to_idx(right_distance);
191

    
192
    ROS_INFO("front: %lf  left: %lf  right: %lf",
193
        front_distance, left_distance, right_distance);
194
    if (right_distance == 0 || front_distance == 0 || left_distance == 0)
195
      return false;
196

    
197
    // determine relative distances on map, based on robot position
198
    int up_d = 0;
199
    int right_d = 0;
200
    int down_d = 0;
201
    int left_d = 0;
202

    
203
    // determine upward distance
204
    switch (dir)
205
    {
206
        case UP:
207
            up_d = front_idx;
208
            right_d = right_idx;
209
            down_d = 0; // unknown
210
            left_d = left_idx;
211
            break;
212
        case RIGHT:
213
            up_d = left_idx;
214
            right_d = front_idx;
215
            down_d = right_idx;
216
            left_d = 0; // unknown
217
            break;
218
        case DOWN:
219
            up_d = 0; // unknown
220
            right_d = left_idx;
221
            down_d = front_idx;
222
            left_d = right_idx;
223
            break;
224
        case LEFT:
225
            up_d = right_idx;
226
            right_d = 0; // unknown
227
            down_d = left_idx;
228
            left_d = front_idx;
229
            break;
230
    }
231

    
232
    // change map until wall index, or until reading < 500
233
    // reading < 500 <=> left_idx < 8 (approx.)
234

    
235
    // map blocks above robot (on map)
236
    for(int u = 0; u < 8; u++)
237
    {
238
        if(u == up_d) {
239
            map[row-u][col] = (up_d)?WALL:SEEN;
240
            break;
241
        }
242
        map[row-u][col] = SEEN;
243
    }
244

    
245
    // map blocks to right of robot
246
    for(int r = 0; r < 8; r++)
247
    {
248
        if(r == right_d) {
249
            map[row][col+r] = (right_d)?WALL:SEEN;
250
                break;
251
        }
252
        map[row][col+r] = SEEN;
253
    }
254

    
255
    // map blocks under robot (on map)
256
    for(int d = 0; d < 8; d++)
257
    {
258
        if(d == down_d) {
259
            map[row+d][col] = (down_d)?WALL:SEEN;
260
                break;
261
        }
262
        map[row+d][col] = SEEN;
263
    }
264

    
265
    // map blocks to left of robot
266
    for(int l = 0; l < 8; l++)
267
    {
268
        if(l == left_d) {
269
            map[row][col-l] = (left_d)?WALL:SEEN;
270
                break;
271
        }
272
        map[row][col-l] = SEEN;
273
    }
274
    return true;
275
}
276

    
277
// TODO: test this function
278
// return true iff the map has been completely explored
279
// and only true if it "returned" to home destination after solving
280
bool smart_runaround::at_destination() 
281
{
282
    // check whether there is a square portion with
283
    // only SEEN or WALL blocks, of the size of the map.
284
    //int start_row = -1;       // Unused
285
    //int start_col = -1;       // Unused
286
    int seen_width = 0;
287
    int seen_height = 0;
288
    for(int r = 0; r < MAP_LENGTH; r++) {
289
        for(int c = 0; c < MAP_LENGTH; c++) {
290
            if(map[r][c] == UNSEEN) {
291
            //start_row = -1;
292
            //start_col = -1;
293
            seen_width = 0;
294
            seen_height = 0;
295
            }
296
            else {
297
            //start_row = r;
298
            //start_col = c;
299
            seen_width++;
300
            seen_height++;
301
            }
302
            if(seen_width >= 15)
303
            return true;
304
        }
305
    }
306
    return false;
307
}
308

    
309
// TODO: test these functions to make sure robot moves well
310
// move forward one block in direction "dir"
311
void smart_runaround::turn_straight(int dir)
312
{
313
    // TODO: try to use motor encoder values to move forward enough
314
    motors->set_sides(BASESPEED, BASESPEED, MOTOR_ABSOLUTE);
315
    spinOnce();
316
    Duration moveTime(1.0);
317
    moveTime.sleep();
318

    
319
    // update robot's position on map
320
    switch(dir)
321
    {
322
        case UP:
323
          row--;
324
          break;
325
        case RIGHT:
326
          col++;
327
          break;
328
        case DOWN:
329
          row++;
330
          break;
331
        case LEFT:
332
          col--;
333
          break;
334
    }
335
}
336

    
337
// turn clockwise (right)
338
void smart_runaround::turn_right()
339
{
340
    motors->set_sides(BASESPEED, -BASESPEED, MOTOR_ABSOLUTE);
341
    spinOnce();
342
    Duration moveTime(3.3);
343
    moveTime.sleep();
344
}
345

    
346
// do a 180 deg turn, NOT a barrel roll
347
void smart_runaround::spot_turn()
348
{
349
    motors->set_sides(BASESPEED, -BASESPEED, MOTOR_ABSOLUTE);
350
    spinOnce();
351
    Duration moveTime(6.6);
352
    moveTime.sleep();
353
}
354

    
355
// turn counter-clockwise (left)
356
void smart_runaround::turn_left()
357
{
358
    motors->set_sides(-BASESPEED, BASESPEED, MOTOR_ABSOLUTE);
359
    spinOnce();
360
    Duration moveTime(3.3);
361
    moveTime.sleep();
362
}