Revision 7b5ea072

View differences:

scout/libscout/src/behaviors/line_follow.cpp
103 103
  bool first = true;
104 104
  float initial_loc = linesensor->readline();  
105 105
  float line_loc = initial_loc;
106
  ROS_INFO("initial_loc = %f",initial_loc);
107 106

  
108 107
  do
109 108
  {
scout/libscout/src/test_behaviors/maze_solve.cpp
76 76
        ROS_INFO("NO! The maze is unsolvable");
77 77
}
78 78

  
79
void maze_solve::adjust()
80
{
81

  
82
    ROS_INFO("adjusting...!");   
83
    motors->set_sides(-30, -30, MOTOR_ABSOLUTE);
84
    ros::Duration(0.3).sleep();
85
    halt();    
86
/*    int front_d = 0;
87
    vector<uint32_t> readings;
88
    int* sonar_readings = sonar->get_sonar_readings();
89
    spinOnce(); 
90
    int ideal_d = sonar_readings[36]/250*250;
91
    bool adjusted = false;   
92
    while (ok() && !adjusted)
93
    {
94
        sonar_readings = sonar->get_sonar_readings();
95
        spinOnce();
96
        front_d = sonar_readings[36];
97
        ROS_INFO("adjusting from %d to %d", front_d, ideal_d);
98
        if (front_d > ideal_d)
99
        {
100
            motors->set_sides(-10, -10, MOTOR_ABSOLUTE);
101
            readings = linesensor->query();
102
        }
103
        else
104
        {
105
            adjusted = true;
106
            halt();
107
        }
108
    }    */
109
}
110 79

  
111 80
bool maze_solve::solve(int row, int col, int dir, bool root)
112 81
{
......
137 106
        // Turn up.
138 107
        turn_from_to(dir, UP);
139 108
        follow_line();
140
        //adjust();
141 109
        // Solve recursively.
142 110
        bool solved = solve(row-2, col, DOWN, false);
143 111
        if (solved)
......
157 125
        // Turn down.
158 126
        turn_from_to(dir, DOWN);
159 127
        follow_line();
160
        //adjust();
161 128
        // Solve recursively.
162 129
        bool solved = solve(row+2, col, UP, false);
163 130
        if (solved)
......
177 144
        // Turn right.
178 145
        turn_from_to(dir, RIGHT);
179 146
        follow_line();
180
        //adjust();
181 147
        // Solve recursively.
182 148
        bool solved = solve(row, col+2, LEFT, false);
183 149
        if (solved)
......
197 163
        // Turn down.
198 164
        turn_from_to(dir, LEFT);
199 165
        follow_line();
200
        //adjust();
201 166
        // Solve recursively.
202 167
        bool solved = solve(row, col-2, RIGHT, false);
203 168
        if (solved)
......
372 337
{
373 338
    vector<uint32_t> readings = linesensor->query();
374 339

  
375

  
340
    //changed values so it detects solution
376 341
    if ( readings[0] > 75 &&
377 342
         readings[1] > 75 &&
378 343
         readings[2] < 55 &&
scout/libscout/src/test_behaviors/maze_solve.h
40 40
        bool look_around(int row, int col, int dir);
41 41
        bool at_destination();
42 42
        void print_board(int board[60][60], int c_row, int c_col);
43
        void adjust();
44 43

  
45 44
        int map[60][60];
46 45
};
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
// 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
}

Also available in: Unified diff