Revision 414d2b48 scout/libscout/src/behaviors/line_follow.cpp

View differences:

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.4);
33
Duration init_turn_time(0.8);
34 34

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

  
43 42
        if (line_loc == 0.0)
44 43
        {
45 44
            motors->set_sides(-60, -60, MOTOR_ABSOLUTE);
46
            continue;
47 45
        }
46
        else
47
        {
48
            motor_l = min(max((int) (-MOTOR_BASE + KP * line_loc), -128), 127);
49
            motor_r = min(max((int) (-MOTOR_BASE - KP * line_loc), -128), 127);
48 50

  
49
        motor_l = min(max((int) (-MOTOR_BASE + KP * line_loc), -128), 127);
50
        motor_r = min(max((int) (-MOTOR_BASE - KP * line_loc), -128), 127);
51

  
52
        motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE);
51
            motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE);
52
        }
53 53
        readings = linesensor->query();
54 54
    }
55 55
    while(!linesensor->fullline(readings) &&
......
63 63
  vector<uint32_t> readings;
64 64
  do
65 65
  {
66
    motor_l = -MOTOR_BASE;
67
    motor_r = -MOTOR_BASE;
68

  
69
    motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE);
66
    motors->set_sides(-MOTOR_BASE, -MOTOR_BASE, MOTOR_ABSOLUTE);
70 67
    readings = linesensor->query();
71 68
  }
72 69
  while(!linesensor->fullline(readings) &&
......
81 78
  do
82 79
  {
83 80
    motor_l = -MOTOR_BASE;
84
    motor_r = 2*MOTOR_BASE/5;
81
    motor_r = 0;
85 82

  
86 83
    motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE);
87 84

  
......
133 130
  float line_loc;
134 131
  do
135 132
  {
136
    motor_l = 2*MOTOR_BASE/5;
133
    motor_l = 0;
137 134
    motor_r = -MOTOR_BASE;
138 135

  
139 136
    motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE);

Also available in: Unified diff