Revision c840fbe6
| scout/libscout/src/behaviors/line_follow.cpp | ||
|---|---|---|
| 30 | 30 |
static int motor_l; |
| 31 | 31 |
static int motor_r; |
| 32 | 32 |
|
| 33 |
Duration init_turn_time(0.5);
|
|
| 33 |
Duration init_turn_time(1.5);
|
|
| 34 | 34 |
|
| 35 | 35 |
void line_follow::follow_line() |
| 36 | 36 |
{
|
| ... | ... | |
| 67 | 67 |
do |
| 68 | 68 |
{
|
| 69 | 69 |
motor_l = -MOTOR_BASE; |
| 70 |
motor_r = MOTOR_BASE/8;
|
|
| 70 |
motor_r = 3*MOTOR_BASE/5;
|
|
| 71 | 71 |
|
| 72 | 72 |
motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE); |
| 73 | 73 |
|
| ... | ... | |
| 100 | 100 |
|
| 101 | 101 |
if(first) |
| 102 | 102 |
{
|
| 103 |
while(line_loc < 2 && line_loc > -2) |
|
| 103 |
do |
|
| 104 |
{
|
|
| 104 | 105 |
line_loc = linesensor->readline(); |
| 106 |
} |
|
| 107 |
while(line_loc < 2 && line_loc > -2); |
|
| 105 | 108 |
|
| 106 | 109 |
first = false; |
| 107 | 110 |
} |
| ... | ... | |
| 116 | 119 |
float line_loc; |
| 117 | 120 |
do |
| 118 | 121 |
{
|
| 119 |
motor_l = MOTOR_BASE/8;
|
|
| 122 |
motor_l = 3*MOTOR_BASE/5;
|
|
| 120 | 123 |
motor_r = -MOTOR_BASE; |
| 121 | 124 |
|
| 122 | 125 |
motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE); |
| scout/libscout/src/behaviors/line_follow.h | ||
|---|---|---|
| 29 | 29 |
#include "../Behavior.h" |
| 30 | 30 |
#include "../Sensors.h" |
| 31 | 31 |
|
| 32 |
#define MOTOR_BASE 60.0f
|
|
| 33 |
#define SCALE 60.0f
|
|
| 32 |
#define MOTOR_BASE 40.0f
|
|
| 33 |
#define SCALE 40.0f
|
|
| 34 | 34 |
|
| 35 | 35 |
class line_follow : public Behavior |
| 36 | 36 |
{
|
| scout/libscout/src/behaviors/maze_solve.cpp | ||
|---|---|---|
| 43 | 43 |
#define DOWN 2 |
| 44 | 44 |
#define LEFT 3 |
| 45 | 45 |
|
| 46 |
Duration sonar_update_time(1.0); |
|
| 47 |
|
|
| 46 | 48 |
void maze_solve::run(){
|
| 47 | 49 |
|
| 48 | 50 |
// TODO:first initialize map to all 0's |
| ... | ... | |
| 73 | 75 |
{
|
| 74 | 76 |
int initial_dir = dir; |
| 75 | 77 |
|
| 78 |
ROS_INFO("I am at direction %d", dir);
|
|
| 79 |
|
|
| 76 | 80 |
// use backtracking to solve the maze |
| 77 | 81 |
if (at_destination()) |
| 78 | 82 |
return true; |
| 79 | 83 |
|
| 84 |
// Wait for sonar to update. |
|
| 85 |
sonar_update_time.sleep(); |
|
| 86 |
|
|
| 80 | 87 |
// this function should fill the adjacent cells around me with |
| 81 | 88 |
// wall's or paths |
| 82 | 89 |
while(!look_around(row, col, dir) && ok()) |
| ... | ... | |
| 85 | 92 |
} |
| 86 | 93 |
|
| 87 | 94 |
/* try go up */ |
| 88 |
ROS_INFO("GOING UP!");
|
|
| 89 | 95 |
if (map[row-1][col] != WALL && initial_dir != UP) |
| 90 | 96 |
{
|
| 97 |
ROS_INFO("GOING UP!");
|
|
| 91 | 98 |
// Turn up. |
| 92 | 99 |
turn_from_to(dir, UP); |
| 93 | 100 |
follow_line(); |
| ... | ... | |
| 104 | 111 |
} |
| 105 | 112 |
} |
| 106 | 113 |
/* try right */ |
| 107 |
ROS_INFO("GOING RIGHT!");
|
|
| 108 | 114 |
if (map[row][col+1] != WALL && initial_dir != RIGHT) |
| 109 | 115 |
{
|
| 116 |
ROS_INFO("GOING RIGHT!");
|
|
| 110 | 117 |
// Turn right. |
| 111 | 118 |
turn_from_to(dir, RIGHT); |
| 112 | 119 |
follow_line(); |
| ... | ... | |
| 123 | 130 |
} |
| 124 | 131 |
} |
| 125 | 132 |
/* try down */ |
| 126 |
ROS_INFO("GOING DOWN!");
|
|
| 127 | 133 |
if (map[row+1][col] != WALL && initial_dir != DOWN) |
| 128 | 134 |
{
|
| 135 |
ROS_INFO("GOING DOWN!");
|
|
| 129 | 136 |
// Turn down. |
| 130 | 137 |
turn_from_to(dir, DOWN); |
| 131 | 138 |
follow_line(); |
| ... | ... | |
| 142 | 149 |
} |
| 143 | 150 |
} |
| 144 | 151 |
/* try left */ |
| 145 |
ROS_INFO("GOING LEFT!");
|
|
| 146 | 152 |
if (map[row][col-1] != WALL && initial_dir != LEFT) |
| 147 | 153 |
{
|
| 154 |
ROS_INFO("GOING LEFT!");
|
|
| 148 | 155 |
// Turn down. |
| 149 | 156 |
turn_from_to(dir, LEFT); |
| 150 | 157 |
follow_line(); |
| ... | ... | |
| 176 | 183 |
spot_turn(); |
| 177 | 184 |
break; |
| 178 | 185 |
case 1: |
| 179 |
turn_right();
|
|
| 186 |
turn_left();
|
|
| 180 | 187 |
break; |
| 181 | 188 |
case 2: |
| 182 | 189 |
turn_straight(); |
| 183 | 190 |
break; |
| 184 | 191 |
case 3: |
| 185 |
turn_left();
|
|
| 192 |
turn_right();
|
|
| 186 | 193 |
break; |
| 187 | 194 |
} |
| 188 | 195 |
} |
Also available in: Unified diff