Project

General

Profile

Revision afa9104d

IDafa9104d1edad3de88368ce4e2a93c7c566a172a
Parent 1905324e
Child 58c19c15

Added by Priya about 7 years ago

Ok Scheduler warehouse behaviour working! and linefollowing works better now! Onto better demo.

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.5);
34

  
33 35
void line_follow::follow_line()
34 36
{
35
    while(!linesensor->fullline())
37
    do
36 38
    {
37 39
        double line_loc = linesensor->readline();
38 40

  
......
41 43

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

  
51
void line_follow::turn_straight()
52
{
53
  do
54
  {
55
    motor_l = -MOTOR_BASE;
56
    motor_r = -MOTOR_BASE;
57

  
58
    motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE);
59
  }
60
  while(linesensor->fullline());
61
}
62

  
48 63
void line_follow::turn_left()
49 64
{
50 65
  bool first = true;
66
  float line_loc;
51 67
  do
52 68
  {
53 69
    motor_l = -MOTOR_BASE;
......
57 73

  
58 74
    if(first)
59 75
    {
60
        loop_rate->sleep();
61
        loop_rate->sleep();
62
        loop_rate->sleep();
76
        init_turn_time.sleep();
63 77
        first = false;
64 78
    }
79

  
80
    line_loc = linesensor->readline();
65 81
  }
66
  while(linesensor->readline());
82
  while(!(-.5 < line_loc && line_loc < .5));
67 83
}
68 84

  
69 85
void line_follow::halt()
......
71 87
    motors->set_sides(0, 0, MOTOR_ABSOLUTE);
72 88
}
73 89

  
90
void line_follow::spot_turn()
91
{
92
  bool first = true;
93
  float line_loc;
94
  do
95
  {
96
    motor_l = MOTOR_BASE;
97
    motor_r = -MOTOR_BASE;
98

  
99
    motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE);
100

  
101
    if(first)
102
    {
103
        while(line_loc < 2 && line_loc > -2)
104
          line_loc = linesensor->readline();
105

  
106
        first = false;
107
    }
108
    line_loc = linesensor->readline();
109
  }
110
  while(!(-1 < line_loc && line_loc < 1));
111
}
112

  
74 113
void line_follow::turn_right()
75 114
{
76 115
  bool first = true;
116
  float line_loc;
77 117
  do
78 118
  {
79 119
    motor_l = MOTOR_BASE/8;
......
83 123

  
84 124
    if(first)
85 125
    {
86
        loop_rate->sleep();
87
        loop_rate->sleep();
88
        loop_rate->sleep();
126
        init_turn_time.sleep();
89 127
        first = false;
90 128
    }
129

  
130
    line_loc = linesensor->readline();
91 131
  }
92
  while(linesensor->readline());
132
  while(!(-1 < line_loc && line_loc < 1));
93 133
}
94 134

  
95 135
void line_follow::u_turn()
......
102 142
void line_follow::run()
103 143
{
104 144
  follow_line();
145
  turn_right();
105 146
  follow_line();
106
  /*
107
  follow_line();
108
  u_turn();
109
  follow_line();
110
  u_turn();
111
  follow_line();
112
  follow_line();
113
  u_turn();
147
  turn_straight();
148
  motors->set_sides(-MOTOR_BASE, -MOTOR_BASE, MOTOR_ABSOLUTE);
149
  Duration meow(2);
150
  meow.sleep();
151
  spot_turn();
114 152
  follow_line();
115
  */
116 153
}

Also available in: Unified diff