Project

General

Profile

Revision 58371433

ID5837143311f6de5068f1724a2d1dc8377610237d

Added by Priya almost 12 years ago

Renaming to line follow and getting rid of old linefollowing files.

View differences:

scout/libscout/CMakeLists.txt
30 30
#target_link_libraries(example ${PROJECT_NAME})
31 31

  
32 32
set(MAIN_FILES src/Behavior.cpp src/BehaviorList.cpp src/BehaviorProcess.cpp)
33
set(BEHAVIOR_FILES src/behaviors/Odometry.cpp src/behaviors/draw_cw_circle.cpp src/behaviors/draw_ccw_circle.cpp src/behaviors/navigationMap.cpp src/behaviors/sim_line.cpp src/behaviors/wl_test.cpp)
33
set(BEHAVIOR_FILES src/behaviors/Odometry.cpp src/behaviors/draw_cw_circle.cpp src/behaviors/draw_ccw_circle.cpp src/behaviors/navigationMap.cpp src/behaviors/line_follow.cpp src/behaviors/wl_test.cpp)
34 34
set(TEST_BEHAVIOR_FILES src/behaviors/Scheduler.cpp src/behaviors/WH_Robot.cpp)
35 35
set(HELPER_FILES src/helper_classes/Order.cpp src/helper_classes/PQWrapper.cpp)
36 36
set(CONTROL_CLASSES src/MotorControl.cpp src/SonarControl.cpp src/HeadlightControl.cpp src/ButtonControl.cpp src/WirelessSender.cpp src/WirelessReceiver.cpp src/EncodersControl.cpp src/LinesensorControl.cpp)
scout/libscout/src/BehaviorList.cpp
8 8
  behavior_list.push_back((Behavior*)new navigationMap(scoutname));
9 9
  behavior_list.push_back((Behavior*)new Scheduler(scoutname));
10 10
  behavior_list.push_back((Behavior*)new WH_Robot(scoutname));
11
  behavior_list.push_back((Behavior*)new sim_line(scoutname));
11
  behavior_list.push_back((Behavior*)new line_follow(scoutname));
12 12
  behavior_list.push_back((Behavior*)new wl_test(scoutname));
13 13
  return;
14 14
}
scout/libscout/src/BehaviorList.h
2 2
#define _BEHAVIOR_LIST_H_
3 3

  
4 4
#include "Behavior.h"
5
#include "behaviors/sim_line.h"
5
#include "behaviors/line_follow.h"
6 6
#include "behaviors/draw_cw_circle.h"
7 7
#include "behaviors/draw_ccw_circle.h"
8 8
#include "behaviors/Odometry.h"
scout/libscout/src/behaviors/lineDrive.cpp
1
/**
2
 * @file line_drive.cpp
3
 *
4
 * Provides functions to implement line driving behavior.  This program extends
5
 * the behavior of the line-following program by following lines automatically
6
 * and implementing behaviors to deal with commands passed to line_drive.
7
 *
8
 * Specific implementation for the colony scout robots in a warehouse behaviour
9
 * setting.
10
 * 
11
 * @author James Carroll(キヤロル)
12
 * Cannabalized from Dan Jacobs code.
13
 * @date 2-6-2012
14
 */
15

  
16
#include "line_drive.h"
17

  
18
line_drive::line_drive(String scoutname): Behavior(scoutname, "line_drive")
19
{
20
  line_drive_init();
21
  line_follow_init();
22
  return;
23
};
24

  
25
/**
26
 * Starts the line following procedure. Must be called before other
27
 * line-following functions will work.  This function essentially resets the
28
 * state of line-following.
29
 */
30
void line_drive_init()
31
{
32
	lineFollow_init();
33
	for(int i=0; i<5; i++)state[i]=0;
34
	stateCounter=0;
35
	stateLength=0;
36
	stopped=0;
37
}
38

  
39

  
40
/**
41
 * Follows a line and executes whatever command is next on the queue.
42
 * @param speed The speed with which to drive along the line.
43
 */
44
int doDrive(int speed)
45
{
46
	if(stopped)
47
	{
48
		motors->set_sides(0,0,ABSOLUTE);
49
		return NORMAL;
50
	}
51

  
52

  
53
	int code;
54
	switch(state[0])
55
	{	
56
		case ISTRAIGHT:
57
			code = lineFollow(speed);
58
			if(code==INTERSECTION)
59
			{
60
				for(int i=0; i<4; i++) state[i]=state[i+1];
61
				state[4]=0;
62
				if(state[0]==0)stateCounter++;
63
				break;
64
			}
65
			else if(code==NOBARCODE) return NORMAL;
66
			return code;
67
	
68
	
69
		case ILEFT:
70
			code = turnLeft();
71
			if(code==0)
72
			{
73
				state[0]=0;
74
				stateCounter++;
75
			}
76
			break;
77
	
78
		case IRIGHT:
79
	    	code = turnRight();
80
	        if(code==0)
81
	        {
82
				state[0]=0;
83
				stateCounter++;
84
			}
85
	        break;
86
	
87
	
88
		default:
89
			return LOST;
90

  
91
	}
92

  
93
	if(stateCounter>=stateLength)
94
	{
95
		stateCounter=stateLength=0;
96
		return FINISHED;
97
	}
98
	return NORMAL;
99
}
100

  
101

  
102
/** Starts the line-drive process if paused. */
103
void start(void){
104
	stopped=0;
105
}
106

  
107
/** Pauses the line-drive process. Default is started. */
108
void stop(void){
109
	stopped=1;
110
}
111

  
112

  
113

  
114
/**
115
 * Executes an intersection turn where the intersection type is specified by the
116
 * parameters. 
117
 * @param type A valid defined intersection type
118
 * @param dir The direction to turn at the intersection
119
 */
120
int turn(int type, int dir)
121
{
122
	if(stateLength!=0)
123
		return ERROR;
124

  
125
	//turns that don't depend on what kind of road your on
126
	if(dir==ISTRAIGHT)
127
	{
128
		stateLength++;
129
		state[1]=ISTRAIGHT;
130
		return NORMAL;
131
	}
132
	if(dir==IUTURN)
133
	{
134
		stateLength+=2;
135
		state[1]=state[2]=ILEFT;
136
		return NORMAL;
137
	}
138

  
139
	if(type==ON_ROAD)
140
	{
141
		if(dir==IRIGHT){
142
			stateLength++;
143
			state[1] = IRIGHT;
144
			return NORMAL;
145
		}
146
		if(dir==ILEFT){
147
			stateLength += 2;
148
			state[1] = ILEFT;
149
			state[2] = ISTRAIGHT;
150
			return NORMAL
151
		}
152
	}
153
	if(type==OFF_ROAD)
154
	{
155
		if(dir==IRIGHT){
156
			stateLength++;
157
			state[1]=IRIGHT;
158
			return NORMAL;
159
		}
160
		if(dir==ILEFT){
161
			stateLength+=2;
162
			state[1]=ISTRAIGHT;
163
			state[2]=ILEFT;
164
		}
165
	}
166

  
167
	//Should never get here
168
	return ERROR;
169
}
170

  
scout/libscout/src/behaviors/lineDrive.h
1
#ifndef _LINE_DRIVE_H_
2
#define _LINE_DRIVE_H_
3

  
4
#include "../Behavior.h"
5
#include "lineFollow.h"
6

  
7
/*
8
 * Defines whether your making a turn from the main two-way road
9
 * or from a one-way sub road.
10
*/
11
#define ON_ROAD		0
12
#define OFF_ROAD	0
13

  
14
/*
15
 * The type of turn you are making from the road you are on.
16
*/
17
#define ISTRAIGHT	0
18
#define ILEFT		1
19
#define IRIGHT		2
20
#define IUTURN		3
21

  
22
#define NORMAL		-1
23
#define FINISHED	-2
24
#define LOST		-3
25
#define ERROR		-4
26

  
27
class line_drive : Behavior{
28
	line_drive(String scoutname);
29
	run();
30

  
31
	void line_drive_init(void);
32

  
33
	int doDrive(int speed);
34

  
35
	void start(void);
36
	void stop(void);
37

  
38

  
39
	int turn(int type, iint dir);
40

  
41
  private:
42
  int state[5];       //! Stores a queue of sub-commands to be executed
43
  int stateCounter;
44
  int stateLength;
45

  
46
  //! Whether line_drive is currently paused. Set to 0 on initialization.
47
  int stopped=1;
48

  
49
};
50

  
51
#endif
scout/libscout/src/behaviors/line_follow.cpp
1 1
/**
2
 * @file line_follow.cpp
3
 * @defgroup lineFollwing Line Following
4
 *
5
 * Takes care of following a line. Running this program is done by calling the
6
 * init() function and then the line_follow(speed) command.  However, direct use
7
 * of this class is discouraged as its behavior is used by lineDrive.cpp, which
8
 * extends this class to provide behavior functionality.
9
 *
10
 * @author Dan Jacobs and the Colony Project
11
 * @date 11-1-2010
2
 * 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.
12 24
 */
13 25

  
14 26
#include "line_follow.h"
15 27

  
16
//! Anything lower than this value is white
17
int GREY_THRESHOLD = 300;
18
//! Anything higher than this value is black
19
int BLACK_THRESHOLD = 750;
28
using namespace std;
20 29

  
21
int countHi = 0;
22
int countLo = 0;
23
int maxAvg, avg;
30
static int motor_l;
31
static int motor_r;
24 32

  
25
int duration[2] = {0};
26
int lastColor[2] = {0};
27
char isReset[2] = {1};
28
int lastReadings[2][ NUM_READINGS ] = {{0}};
29
int lastReadingsPtr[2] = {0};
30
int numLast[2][4] = { {0, 0, 0, NUM_READINGS}, {0, 0, 0, NUM_READINGS} };
31
int bitColor[2] = {0};
32

  
33
int turnDistance=0;
34
//! Counts the number of full line readings before we determine an intersection
35
int intersectionFilter=0;
33
void line_follow::follow_line()
34
{
35
    while(!linesensor->fullline())
36
    {
37
        double line_loc = linesensor->readline();
36 38

  
37
//! Keeps track of where the encoder of one motor started, for use in turns.
38
int encoderStart = -1;
39
int encoderReset = 0;   // 0 if encoderStart has no value set
39
        cout << "line_loc: " << line_loc << endl;
40 40

  
41
int min(int x, int y){return x>y ? y : x;}
42
int max(int x, int y){return x<y ? y : x;}
41
        motor_l = -MOTOR_BASE + SCALE * line_loc;
42
        motor_r = -MOTOR_BASE - SCALE * line_loc;
43 43

  
44
        motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE);
45
    }
46
    halt();
47
}
44 48

  
45
void line_follow::init()
49
void line_follow::turn_left()
46 50
{
47
  int i, j, curReading;
48
  int lowGrey = 1000, highGrey = 0, lowBlack = 1000,
49
      highBlack = 0;
51
  bool first = true;
52
  do
53
  {
54
    motor_l = -MOTOR_BASE;
55
    motor_r = MOTOR_BASE/8;
50 56

  
51
  analog_init(0);
52
  encoders_init();
53
  lost = 0;
54
  intersectionFilter=0;
57
    motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE);
55 58

  
56
  for(i=0; i<2; i++)
57
  {
58
    for(j=0; j<NUM_READINGS; j++)
59
    double line_loc = linesensor->readline();
60
    cout << "line_loc: " << line_loc << endl;
61
    if(first)
59 62
    {
60
      lastReadings[i][j] = BAD_READING;
63
        loop_rate->sleep();
64
        loop_rate->sleep();
65
        loop_rate->sleep();
66
        first = false;
61 67
    }
62
    isReset[i] = 1;
63
  }
64

  
65
  // Calibrate thresholds
66
  orb_set_color(YELLOW);
67
  delay_ms(2000);
68

  
69
  orb_set_color(BLUE);
70

  
71
  for(i=0; i<100; i++)
72
  {
73
    curReading = read_line(LEFT_SENSOR + 6);
74
    if(curReading < lowGrey)
75
      lowGrey = curReading;
76
    if(curReading > highGrey)
77
      highGrey = curReading;
78

  
79
    delay_ms(20);
80 68
  }
81

  
82
  orb_set_color(YELLOW);
83
  delay_ms(2000);
84

  
85
  orb_set_color(GREEN);
86

  
87
  for(i=0; i<100; i++)
88
  {
89
    curReading = read_line(LEFT_SENSOR + 6);
90
    if(curReading < lowBlack)
91
      lowBlack = curReading;
92
    if(curReading > highBlack)
93
      highBlack = curReading;
94

  
95
    delay_ms(20);
96
  }
97

  
98
  orbs_set(0,0,0,0,0,0);
99

  
100
  GREY_THRESHOLD = lowGrey / 2;
101
  BLACK_THRESHOLD = (highGrey + lowBlack) / 2;
102

  
103
  usb_puts("Grey: ");
104
  usb_puti(lowGrey);
105
  usb_puts(", ");
106
  usb_puti(highGrey);
107
  usb_puts("\nBlack: ");
108
  usb_puti(lowBlack);
109
  usb_puts(", ");
110
  usb_puti(highBlack);
111
  usb_puts("\nThresholds: ");
112
  usb_puti(GREY_THRESHOLD);
113
  usb_puts(", ");
114
  usb_puti(BLACK_THRESHOLD);
115
  usb_puts("\n\n");
116

  
117
  delay_ms(1500);
69
  while(linesensor->readline());
118 70
}
119 71

  
120

  
121
/** 
122
 * Follows a line at the given speed.
123
 * @param speed The speed with which to follow the line.
124
 */
125
int line_follow::follow_line(int speed)
72
void line_follow::halt()
126 73
{
127
	int colors[5];
128
	int position;
129
	
130

  
131
	updateLine(colors);
132
	position = lineLocate(colors); 	
133
	
134
	//not on line
135
	if(position == NOLINE)
136
	{
137
		if(lost++ > 20)
138
		{
139
			orb2_set_color(GREEN);
140
			motors_off();
141
			return LINELOST;
142
		}
143
	}
144
	else if(position == FULL_LINE)
145
	{
146
		if(intersectionFilter++ > 4)
147
		{
148
			orb2_set_color(RED);
149
			barCodePosition[0]=0;
150
            barCodePosition[1]=0;
151
			disableBarCode=50;
152
		}
153
	}
154
	//on line
155
	else
156
	{
157
		position*=30;
158
		orb2_set_color(ORB_OFF);
159
		motorLeft(min(speed+position, 255));
160
		motorRight(min(speed-position, 255));
161
		lost=0;
162
		intersectionFilter=0;
163
	}
74
    motors->set_sides(0, 0, MOTOR_ABSOLUTE);
164 75
}
165 76

  
166

  
167
/**
168
 * Turns left at a cross of two lines.  Assumes that we are at lines in a cross
169
 * pattern, and turns until it sets straight on the new line.
170
 * @return 0 if turn finishes otherwise return 1
171
 */
172
int line_follow::turnLeft()
77
void line_follow::turn_right()
173 78
{
174
        motors->set_sides(-20, 20, MOTOR_ABSOLUTE);
175
		
176
        if(!encoderReset)
177
        {
178
            encoderStart = encoder_get_x(RIGHT);
179
            encoderReset = 1;
180
        }
181

  
182
        if(encoder_get_x(RIGHT) < encoderStart)
183
        {
184
            encoderStart = 0;
185
            motors->set_sides(0, 0, MOTOR_ABSOLUTE);
186
            delay_ms(2000);
187
        }
188

  
189
        if(encoder_get_x(RIGHT) - encoderStart > 300)
190
        {
191
            encoderReset = 0;
192
            return 0;
193
        }
194
        return 1;
195
}
196

  
197

  
198

  
199
/**
200
 * Turns right at a cross of two lines.  Assumes that we are at lines in a cross
201
 * pattern, and turns until it sets straight on the new line.
202
 * @return 0 if the turn finishes otherwise return 1
203
 */
204
int line_follow::turnRight()
205
{
206
  /// Set motors using new motors functions
207
  motors->set_sides(20, -20, MOTOR_ABSOLUTE);
79
  bool first = true;
80
  do
81
  {
82
    motor_l = MOTOR_BASE/8;
83
    motor_r = -MOTOR_BASE;
208 84

  
209
  int colors[5];
210
  updateLine(colors);
211
  int position = lineLocate(colors);
212
  if(position>2 || position<-2)
213
    turnDistance++;
85
    motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE);
214 86

  
215
  if(turnDistance>1) 
216
  {
217
    if(position<3 && position>-3)
87
    double line_loc = linesensor->readline();
88
    cout << "line_loc: " << line_loc << endl;
89
    if(first)
218 90
    {
219
      turnDistance = 0;
220
      return 0;
91
        loop_rate->sleep();
92
        loop_rate->sleep();
93
        loop_rate->sleep();
94
        first = false;
221 95
    }
222 96
  }
223
  return 1;
224
}
225

  
226

  
227
void line_follow::updateLine(int* values)
228
{	
229
	for(int i = 0; i<5; i++)
230
		values[i] = (read_line(4-i) < LINE_COLOR ? LWHITE : LBLACK);
231
}
232

  
233

  
234
int line_follow::lineLocate(int* colors)
235
{
236
	int i;
237
	int wsum = 0;
238
	int count=0;
239

  
240
	for(i = 0; i<5; i++)
241
	{
242
		count += colors[i]/2;
243
		wsum += (i)*colors[i];
244
	}
245
	if(count==0)
246
		return NOLINE;	
247
	if(count==5)
248
		return FULL_LINE;
249
	return (wsum/count)-4; /// Subtract 4 to center the index around the center.
250
}
251

  
252
void line_follow::motorLeft(int speed)
253
{
254
	int tempspeed = 0;
255
	if ((speed-=127)>=0)
256
	{	
257
		tempspeed = 160+speed*95/128;
258
		motors->set(MOTOR_LEFT, tempspeed, MOTOR_ABSOLUTE);
259
	}
260
	else
261
	{
262
		tempspeed = 160+speed*95/128;
263
		motors->set(MOTOR_LEFT, -tempspeed, MOTOR_ABSOLUTE);
264
	}
97
  while(linesensor->readline());
265 98
}
266 99

  
267
void line_follow::motorRight(int speed)
100
void line_follow::run()
268 101
{
269
	int tempspeed = 0;
270
	if ((speed-=127)>=0)
271
	{	
272
		tempspeed = 160+speed*95/128;
273
		motors->set(MOTOR_RIGHT, tempspeed, MOTOR_ABSOLUTE);
274
	}
275
	else
276
	{
277
		tempspeed = 160+speed*95/128;
278
		motors->set(MOTOR_RIGHT, -tempspeed, MOTOR_ABSOLUTE);
279
	}
102
  follow_line();
103
  turn_right();
104
  follow_line();
105
  turn_left();
106
  follow_line();
107
  turn_right();
108
  follow_line();
280 109
}
281
	
scout/libscout/src/behaviors/line_follow.h
1
/**
2
 * 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
 */
1 25

  
2 26
#ifndef _LINE_FOLLOW_H_
3 27
#define _LINE_FOLLOW_H_
4 28

  
5 29
#include "../Behavior.h"
6 30

  
7
#define LWHITE			0
8
#define LGREY			1
9
#define LBLACK	 		2
10
#define BAD_READING     3
11
#define CENTER			3
12
#define LINELOST		-1
13

  
14
#define INTERSECTION 	-25
15

  
16
#define NOLINE			-50
17
#define FULL_LINE 		-51
18

  
19
#define NUM_READINGS 20
20

  
21
#define LEFT_SENSOR     1
22
#define RIGHT_SENSOR    0
23

  
24
/**
25
 * Helps with debugging.
26
 * 0 - no debug output.
27
 * 1 - print out buckets
28
 * 2 - print out line sensor readings
29
 */
30
#define DBG_LINEFOLLOW 2
31

  
31
#define MOTOR_BASE 50
32
#define SCALE 20
32 33

  
33
//! A simple function to return the minimum of two integers.
34
int min(int x, int y);
35
//! A simple function to return the maximum of two integers.
36
int max(int x, int y);
37

  
38

  
39
class line_follow 
34
class line_follow : Behavior
40 35
{
41
	public:
42
    /** Actually executes the behavior. */
43
		void init();
44

  
45
    /**	line_follow
46
     *	Must call line_follow_init first
47
     *	Must be called inside a loop
48
     */
49
    int follow_line(int speed);
50

  
51
    /**	turnLeft turnRight
52
     *	Must be called inside a loop
53
     *	returns 0 when complete
54
     */
55
    int turnLeft(void);
56
    int turnRight(void);
57

  
58
    /**
59
     * @brief Updates the values stored in the array to white or black based on
60
     * current sensor readings.
61
     *
62
     * @param values The array of five integers to be updated. 
63
     */
64
    void updateLine(int* values); 
36
    public:
37
        line_follow(std::string scoutname) : Behavior(scoutname, "line_follow") {};
65 38

  
66
    /**
67
     * @brief Returns an index of the middle of the line based on line readings.
68
     *
69
     * Two special return values are possible:
70
     *   NOLINE if none of the sensors holds a black value, and
71
     *   FULL_LINE if all of the sensors see black.
72
     *
73
     * Otherwise, returns a value from -4 (farthest left) to 4 (farthest right), with
74
     * 0 the line being centered in the middle.
75
     *
76
     * @param colors The array of 5 readings from the line sensor.  Must be either
77
     *    LWHITE or LBLACK.
78
     * @return Either a special value or an index from -4 to 4.
79
     *
80
     */
81
    int lineLocate(int* colors);
82

  
83
    /** @todo Alex: I hate these functions, but I'm keeping them so code will still work. But we should delete them sometime. */
84

  
85
    /**	motorLeft
86
     *	Commands the left motor
87
     *	Cannot be used to stop
88
     *	0-126 are backward
89
     *	127-255 are forward
90
     */
91
    void motorLeft(int speed);
92

  
93
    /**	motorRight
94
     *	Commands the right motor
95
     *	Cannot be used to stop
96
     *	0-126 are backward
97
     *	127-255 are forward
98
     */
99
    void motorRight(int speed);
39
        /** Actually executes the behavior. */
40
        void run();
100 41

  
101 42
    private:
102
    /**	lost
103
     *	Internal counter to detect if the line was lost
104
     */
105
    int lost;
43
        void turn_left();
44
        void turn_right();
45
        void follow_line();
46
        void halt();
106 47
};
48

  
107 49
#endif
scout/libscout/src/behaviors/sim_line.cpp
1
/**
2
 * 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
 */
25

  
26
#include "sim_line.h"
27

  
28
using namespace std;
29

  
30
static int motor_l;
31
static int motor_r;
32

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

  
39
        cout << "line_loc: " << line_loc << endl;
40

  
41
        motor_l = -MOTOR_BASE + SCALE * line_loc;
42
        motor_r = -MOTOR_BASE - SCALE * line_loc;
43

  
44
        motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE);
45
    }
46
    halt();
47
}
48

  
49
void sim_line::turn_left()
50
{
51
  bool first = true;
52
  do
53
  {
54
    motor_l = -MOTOR_BASE;
55
    motor_r = MOTOR_BASE/8;
56

  
57
    motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE);
58

  
59
    double line_loc = linesensor->readline();
60
    cout << "line_loc: " << line_loc << endl;
61
    if(first)
62
    {
63
        loop_rate->sleep();
64
        loop_rate->sleep();
65
        loop_rate->sleep();
66
        first = false;
67
    }
68
  }
69
  while(linesensor->readline());
70
}
71

  
72
void sim_line::halt()
73
{
74
    motors->set_sides(0, 0, MOTOR_ABSOLUTE);
75
}
76

  
77
void sim_line::turn_right()
78
{
79
  bool first = true;
80
  do
81
  {
82
    motor_l = MOTOR_BASE/8;
83
    motor_r = -MOTOR_BASE;
84

  
85
    motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE);
86

  
87
    double line_loc = linesensor->readline();
88
    cout << "line_loc: " << line_loc << endl;
89
    if(first)
90
    {
91
        loop_rate->sleep();
92
        loop_rate->sleep();
93
        loop_rate->sleep();
94
        first = false;
95
    }
96
  }
97
  while(linesensor->readline());
98
}
99

  
100
void sim_line::run()
101
{
102
  follow_line();
103
  turn_right();
104
  follow_line();
105
  turn_left();
106
  follow_line();
107
  turn_right();
108
  follow_line();
109
}
scout/libscout/src/behaviors/sim_line.h
1
/**
2
 * 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
 */
25

  
26
#ifndef _SIM_LINE_H_
27
#define _SIM_LINE_H_
28

  
29
#include "../Behavior.h"
30

  
31
#define MOTOR_BASE 50
32
#define SCALE 20
33

  
34
class sim_line : Behavior
35
{
36
    public:
37
        sim_line(std::string scoutname) : Behavior(scoutname, "sim_line") {};
38

  
39
        /** Actually executes the behavior. */
40
        void run();
41

  
42
    private:
43
        void turn_left();
44
        void turn_right();
45
        void follow_line();
46
        void halt();
47
};
48

  
49
#endif

Also available in: Unified diff