Revision 6ee555a3 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(1.5);
33
Duration init_turn_time(0.4);
34 34

  
35 35
void line_follow::follow_line()
36 36
{
......
38 38
    {
39 39
        double line_loc = linesensor->readline();
40 40

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

  
47
        motor_l = min(max((int) (-MOTOR_BASE + KP * line_loc), -128), 127);
48
        motor_r = min(max((int) (-MOTOR_BASE - KP * line_loc), -128), 127);
43 49

  
44 50
        motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE);
45 51
    }
46 52
    while(!linesensor->fullline());
47 53
    halt();
48
    ROS_INFO("Intersection reached!");
49 54
}
50 55

  
51 56
void line_follow::turn_straight()
......
67 72
  do
68 73
  {
69 74
    motor_l = -MOTOR_BASE;
70
    motor_r = 3*MOTOR_BASE/5;
75
    motor_r = 2*MOTOR_BASE/5;
71 76

  
72 77
    motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE);
73 78

  
......
119 124
  float line_loc;
120 125
  do
121 126
  {
122
    motor_l = 3*MOTOR_BASE/5;
127
    motor_l = 2*MOTOR_BASE/5;
123 128
    motor_r = -MOTOR_BASE;
124 129

  
125 130
    motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE);

Also available in: Unified diff