Revision af7e0f94 scout/libscout/src/behaviors/line_follow.cpp

View differences:

scout/libscout/src/behaviors/line_follow.cpp
34 34

  
35 35
void line_follow::follow_line()
36 36
{
37
    vector<uint32_t> readings;
37 38
    do
38 39
    {
39 40
        double line_loc = linesensor->readline();
41
        ROS_INFO("Line loc: %lf", line_loc);
40 42

  
41 43
        if (line_loc == 0.0)
42 44
        {
......
48 50
        motor_r = min(max((int) (-MOTOR_BASE - KP * line_loc), -128), 127);
49 51

  
50 52
        motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE);
53
        readings = linesensor->query();
51 54
    }
52
    while(!linesensor->fullline());
55
    while(!linesensor->fullline(readings) &&
56
          !linesensor->destination(readings) &&
57
          ok());
53 58
    halt();
54 59
}
55 60

  
56 61
void line_follow::turn_straight()
57 62
{
63
  vector<uint32_t> readings;
58 64
  do
59 65
  {
60 66
    motor_l = -MOTOR_BASE;
61 67
    motor_r = -MOTOR_BASE;
62 68

  
63 69
    motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE);
70
    readings = linesensor->query();
64 71
  }
65
  while(linesensor->fullline());
72
  while(!linesensor->fullline(readings) &&
73
        !linesensor->destination(readings) &&
74
        ok());
66 75
}
67 76

  
68 77
void line_follow::turn_left()
......
84 93

  
85 94
    line_loc = linesensor->readline();
86 95
  }
87
  while(!(-1 < line_loc && line_loc < 1));
96
  while(!(-1 < line_loc && line_loc < 1) && ok());
88 97
}
89 98

  
90 99
void line_follow::halt()
......
109 118
        {
110 119
          line_loc = linesensor->readline();
111 120
        }
112
        while(line_loc < 2 && line_loc > -2);
121
        while(line_loc < 2 && line_loc > -2 && ok());
113 122

  
114 123
        first = false;
115 124
    }
116 125
    line_loc = linesensor->readline();
117 126
  }
118
  while(!(-1 < line_loc && line_loc < 1));
127
  while(!(-1 < line_loc && line_loc < 1) && ok());
119 128
}
120 129

  
121 130
void line_follow::turn_right()
......
137 146

  
138 147
    line_loc = linesensor->readline();
139 148
  }
140
  while(!(-1 < line_loc && line_loc < 1));
149
  while(!(-1 < line_loc && line_loc < 1) && ok());
141 150
}
142 151

  
143 152
void line_follow::u_turn()

Also available in: Unified diff