## root / scout / libscout / src / behaviors / line_follow.cpp @ faa11f08

History | View | Annotate | Download (3.2 KB)

1 | 9143e077 | Lalitha Ganesan | ```
/**
``` |
---|---|---|---|

2 | 58371433 | Priya | ```
* Copyright (c) 2011 Colony Project
``` |

3 | ```
*
``` |
||

4 | ```
* Permission is hereby granted, free of charge, to any person
``` |
||

5 | ```
* obtaining a copy of this software and associated documentation
``` |
||

6 | ```
* files (the "Software"), to deal in the Software without
``` |
||

7 | ```
* restriction, including without limitation the rights to use,
``` |
||

8 | ```
* copy, modify, merge, publish, distribute, sublicense, and/or sell
``` |
||

9 | ```
* copies of the Software, and to permit persons to whom the
``` |
||

10 | ```
* Software is furnished to do so, subject to the following
``` |
||

11 | ```
* conditions:
``` |
||

12 | ```
*
``` |
||

13 | ```
* The above copyright notice and this permission notice shall be
``` |
||

14 | ```
* included in all copies or substantial portions of the Software.
``` |
||

15 | ```
*
``` |
||

16 | ```
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
``` |
||

17 | ```
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
``` |
||

18 | ```
* OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
``` |
||

19 | ```
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
``` |
||

20 | ```
* HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
``` |
||

21 | ```
* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
``` |
||

22 | ```
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
``` |
||

23 | ```
* OTHER DEALINGS IN THE SOFTWARE.
``` |
||

24 | 9143e077 | Lalitha Ganesan | ```
*/
``` |

25 | |||

26 | 60b98383 | Priya | #include "line_follow.h" |

27 | 9143e077 | Lalitha Ganesan | |

28 | 58371433 | Priya | using namespace std; |

29 | 9143e077 | Lalitha Ganesan | |

30 | 58371433 | Priya | static int motor_l; |

31 | static int motor_r; |
||

32 | 9143e077 | Lalitha Ganesan | |

33 | c840fbe6 | Priya | Duration init_turn_time(1.5); |

34 | afa9104d | Priya | |

35 | 58371433 | Priya | ```
void line_follow::follow_line()
``` |

36 | { |
||

37 | afa9104d | Priya | ```
do
``` |

38 | 58371433 | Priya | { |

39 | ```
double line_loc = linesensor->readline();
``` |
||

40 | 9143e077 | Lalitha Ganesan | |

41 | 6fab3966 | Alex | 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); |
||

43 | 9143e077 | Lalitha Ganesan | |

44 | 58371433 | Priya | motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE); |

45 | } |
||

46 | afa9104d | Priya | ```
while(!linesensor->fullline());
``` |

47 | 58371433 | Priya | halt(); |

48 | 1905324e | Priya | ```
ROS_INFO("Intersection reached!");
``` |

49 | 58371433 | Priya | } |

50 | 9143e077 | Lalitha Ganesan | |

51 | afa9104d | Priya | ```
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 | |||

63 | 58371433 | Priya | ```
void line_follow::turn_left()
``` |

64 | 60b98383 | Priya | { |

65 | 58371433 | Priya | bool first = true; |

66 | afa9104d | Priya | ```
float line_loc;
``` |

67 | 58371433 | Priya | ```
do
``` |

68 | { |
||

69 | motor_l = -MOTOR_BASE; |
||

70 | c840fbe6 | Priya | motor_r = 3*MOTOR_BASE/5; |

71 | 60b98383 | Priya | |

72 | 58371433 | Priya | motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE); |

73 | 60b98383 | Priya | |

74 | 58371433 | Priya | ```
if(first)
``` |

75 | 9143e077 | Lalitha Ganesan | { |

76 | afa9104d | Priya | init_turn_time.sleep(); |

77 | 58371433 | Priya | ```
first = false;
``` |

78 | 9143e077 | Lalitha Ganesan | } |

79 | afa9104d | Priya | |

80 | line_loc = linesensor->readline(); |
||

81 | 60b98383 | Priya | } |

82 | 58c19c15 | Priya | while(!(-1 < line_loc && line_loc < 1)); |

83 | 9143e077 | Lalitha Ganesan | } |

84 | |||

85 | 58371433 | Priya | ```
void line_follow::halt()
``` |

86 | 9143e077 | Lalitha Ganesan | { |

87 | 58371433 | Priya | motors->set_sides(0, 0, MOTOR_ABSOLUTE); |

88 | 9143e077 | Lalitha Ganesan | } |

89 | |||

90 | afa9104d | Priya | ```
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 | c840fbe6 | Priya | ```
do
``` |

104 | { |
||

105 | afa9104d | Priya | line_loc = linesensor->readline(); |

106 | c840fbe6 | Priya | } |

107 | while(line_loc < 2 && line_loc > -2); |
||

108 | afa9104d | Priya | |

109 | ```
first = false;
``` |
||

110 | } |
||

111 | line_loc = linesensor->readline(); |
||

112 | } |
||

113 | while(!(-1 < line_loc && line_loc < 1)); |
||

114 | } |
||

115 | |||

116 | 58371433 | Priya | ```
void line_follow::turn_right()
``` |

117 | 9143e077 | Lalitha Ganesan | { |

118 | 58371433 | Priya | bool first = true; |

119 | afa9104d | Priya | ```
float line_loc;
``` |

120 | 58371433 | Priya | ```
do
``` |

121 | { |
||

122 | c840fbe6 | Priya | motor_l = 3*MOTOR_BASE/5; |

123 | 58371433 | Priya | motor_r = -MOTOR_BASE; |

124 | 60b98383 | Priya | |

125 | 58371433 | Priya | motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE); |

126 | 60b98383 | Priya | |

127 | 58371433 | Priya | ```
if(first)
``` |

128 | 60b98383 | Priya | { |

129 | afa9104d | Priya | init_turn_time.sleep(); |

130 | 58371433 | Priya | ```
first = false;
``` |

131 | 60b98383 | Priya | } |

132 | afa9104d | Priya | |

133 | line_loc = linesensor->readline(); |
||

134 | 60b98383 | Priya | } |

135 | afa9104d | Priya | while(!(-1 < line_loc && line_loc < 1)); |

136 | 9143e077 | Lalitha Ganesan | } |

137 | |||

138 | 1905324e | Priya | ```
void line_follow::u_turn()
``` |

139 | 9143e077 | Lalitha Ganesan | { |

140 | 1905324e | Priya | turn_right(); |

141 | 58371433 | Priya | follow_line(); |

142 | turn_right(); |
||

143 | 1905324e | Priya | } |

144 | |||

145 | ```
void line_follow::run()
``` |
||

146 | { |
||

147 | 25694a03 | Priya | ```
while(ok())
``` |

148 | { |
||

149 | follow_line(); |
||

150 | } |
||

151 | 9143e077 | Lalitha Ganesan | } |