Statistics
| Branch: | Revision:

root / scout / libscout / src / test_behaviors / smart_runaround.cpp @ 4c9fb6ba

History | View | Annotate | Download (8.67 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

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

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

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

    
209
        // re-orient x and y distances based on direction
210
        switch(dir) {
211
            case UP:
212
                ans[0] = xDist;
213
                ans[1] = yDist;
214
                break;
215
            case RIGHT:
216
                ans = matrix_mult(inputs, rightMat);
217
                break;
218
            case DOWN:
219
                ans = matrix_mult(inputs, downMat);
220
                break;
221
            case LEFT:
222
                ans = matrix_mult(inputs, leftMat);
223
                break;
224
        }
225
        // indices into the map
226
        int pixDistX = row + mm_to_idx(ans[0]);
227
        int pixDistY = col + mm_to_idx(ans[1]);
228
        // plot to map if indices are valid
229
        if (0 <= pixDistX && pixDistX < MAP_LENGTH
230
            && 0 <= pixDistY && pixDistY < MAP_LENGTH)
231
                map[pixDistX][pixDistY] = SEEN;
232
    }
233
    return true;
234
}
235

    
236
// TODO: test this function
237
// return true iff the map has been completely explored
238
// and only true if it "returned" to home destination after solving
239
bool smart_runaround::at_destination() 
240
{
241
    // check whether there is a square portion with
242
    // only SEEN or WALL blocks, of the size of the map.
243
    int start_row = -1;
244
    int start_col = -1;
245
    int seen_width = 0;
246
    int seen_height = 0;
247
    for(int r = 0; r < MAP_LENGTH; r++) {
248
        for(int c = 0; c < MAP_LENGTH; c++) {
249
            if(map[r][c] == UNSEEN) {
250
                start_row = -1;
251
                start_col = -1;
252
                seen_width = 0;
253
                seen_height = 0;
254
            }
255
            else {
256
                start_row = r;
257
                start_col = c;
258
                seen_width++;
259
                seen_height++;
260
            }
261
            if(seen_width >= 15)
262
                return true;
263
        }
264
    }
265
    return false;
266
}
267

    
268
// TODO: test these functions to make sure robot moves well
269
// move forward one block in direction "dir"
270
void smart_runaround::turn_straight(int dir)
271
{
272
    // TODO: try to use motor encoder values to move forward enough
273
    motors->set_sides(BASESPEED, BASESPEED, MOTOR_ABSOLUTE);
274
    spinOnce();
275
    Duration moveTime(1.0);
276
    moveTime.sleep();
277

    
278
    // update robot's position on map
279
    switch(dir)
280
    {
281
        case UP:
282
          row--;
283
          break;
284
        case RIGHT:
285
          col++;
286
          break;
287
        case DOWN:
288
          row++;
289
          break;
290
        case LEFT:
291
          col--;
292
          break;
293
    }
294
}
295

    
296
// turn clockwise (right)
297
void smart_runaround::turn_right()
298
{
299
    motors->set_sides(BASESPEED, -BASESPEED, MOTOR_ABSOLUTE);
300
    spinOnce();
301
    Duration moveTime(3.3);
302
    moveTime.sleep();
303
}
304

    
305
// do a 180 deg turn, NOT a barrel roll
306
void smart_runaround::spot_turn()
307
{
308
    motors->set_sides(BASESPEED, -BASESPEED, MOTOR_ABSOLUTE);
309
    spinOnce();
310
    Duration moveTime(6.6);
311
    moveTime.sleep();
312
}
313

    
314
// turn counter-clockwise (left)
315
void smart_runaround::turn_left()
316
{
317
    motors->set_sides(-BASESPEED, BASESPEED, MOTOR_ABSOLUTE);
318
    spinOnce();
319
    Duration moveTime(3.3);
320
    moveTime.sleep();
321
}