examples/drive/DriveRobot.cpp - libirobot in libirobot

#include "DriveRobot.h"
#include <libirobot/Robot.h>
#include <libirobot/Drive.h>
#include <libirobot/SerialControl.h>
#include <libirobot/sensors/BumpAndDropSensor.h>
#include <libirobot/sensors/BooleanSensor.h>
#include <libirobot/Song.h>
#include <libirobot/sensors/IRSensor.h>
#include <libirobot/sensors/ChargeSourceSensor.h>
#include <libirobot/sensors/UnsignedValueSensor.h>
#include <libirobot/sensors/ValueSensor.h>
#include <libirobot/Debug.h>

#include <math.h>

using namespace iRobot;
using namespace std;

#define QUARTER_NOTE 16 

DriveRobot::DriveRobot ( const char ttyDevice[] )
        : Robot ( ttyDevice ),
		  m_speed(0),
		  m_lastDir(Drive::Right),
		  m_tryingToDock(false),
		  m_dockAttempted(false),
		  m_dockTimeout(0)
{
    setMode ( Full );
    setAutoUpdate( Sensor::All, true );

	Song cliffMusic(2);
	Song::Note cliffnotes[] = {
		{ 60, QUARTER_NOTE },
		{ 57, QUARTER_NOTE },
	};
	for(int i = 0;i<2;i++) {
		cliffMusic.setNotePitch(i, cliffnotes[i].pitch);
		cliffMusic.setNoteLength(i, cliffnotes[i].length);
	}
	setSong(1, cliffMusic);
}

void
DriveRobot::avoid()
{
    Drive* d = getDrive();
	d->move(0);
    d->move(-(int)Drive::Medium );
    m_dockTimeout+=d->waitForDistance(100);
	d->move ( Drive::Medium, m_lastDir );
    d->waitForRotation(15);
}

bool
DriveRobot::reachedCliff()
{
		BooleanSensor* cliffLeft = static_cast<BooleanSensor*> ( getSensor( Sensor::CliffLeft ) );
		BooleanSensor* cliffFrontLeft = static_cast<BooleanSensor*> ( getSensor( Sensor::CliffFrontLeft ) );
		BooleanSensor* cliffFrontRight = static_cast<BooleanSensor*> ( getSensor( Sensor::CliffFrontRight ) );
		BooleanSensor* cliffRight = static_cast<BooleanSensor*> ( getSensor( Sensor::CliffRight ) );
		return cliffLeft->value() || cliffFrontRight->value() || cliffFrontLeft->value() || cliffRight->value();
}

DriveRobot::Side
DriveRobot::cliffSide()
{
    BooleanSensor* cliffLeft = static_cast<BooleanSensor*> ( getSensor( Sensor::CliffLeft ) );
    BooleanSensor* cliffFrontLeft = static_cast<BooleanSensor*> ( getSensor( Sensor::CliffFrontLeft ) );
    BooleanSensor* cliffFrontRight = static_cast<BooleanSensor*> ( getSensor( Sensor::CliffFrontRight ) );
    BooleanSensor* cliffRight = static_cast<BooleanSensor*> ( getSensor( Sensor::CliffRight ) );
    if (cliffFrontLeft->value() && cliffFrontRight->value())
        return Center;
    if (cliffLeft->value() || cliffFrontLeft->value())
        return Left;
    if (cliffRight->value() || cliffFrontRight->value())
        return Right;
}

bool
DriveRobot::fellOffCliff()
{
    BumpAndDropSensor* s = static_cast<BumpAndDropSensor*> ( getSensor ( Sensor::BumpsAndDrops ) );
	return s->flags() & BumpAndDropSensor::DropAny;
}

void
DriveRobot::dock()
{
    Drive* d = getDrive();
	IRSensor* ir = static_cast<IRSensor*> ( getSensor( Sensor::IR ) );
	unsigned char c = ir->value();
	ChargeSourceSensor* sources = static_cast<ChargeSourceSensor*> ( getSensor( Sensor::ChargeSources ) );
	ValueSensor* distance = static_cast<ValueSensor*> ( getSensor( Sensor::Distance ) );
	m_dockTimeout+=distance->value();
	if (m_dockTimeout > 2000) {
		m_dockTimeout = 0;
		m_tryingToDock = false;
		m_dockAttempted = false;
	} else if (sources->sources() & ChargeSourceSensor::Base) {
		d->move(0);
		m_tryingToDock = false;
		return;
	} else if (m_tryingToDock) {
		if (m_dockAttempted) {
			d->move(-Drive::Medium);
            m_dockTimeout+=d->waitForDistance(300);
			if (m_lastDir == Drive::Right)
				d->move(Drive::Fast, Drive::Left);
			else
				d->move(Drive::Fast, Drive::Right);
			d->waitForRotation(180);
            d->move(Drive::Medium);
            m_dockTimeout+=d->waitForDistance(100);
            m_dockAttempted = false;
		} else if ((c == (unsigned char)IRSensor::RedAndGreenBuoysAndForceField || c == (unsigned char)IRSensor::ForceField) && bumped()) {
			d->move(-Drive::Slow);
            m_dockTimeout+=d->waitForDistance(15);
			d->move(0);
			delay(1000000);
			m_dockAttempted = true;
		} else if (c == (unsigned char)IRSensor::RedAndGreenBuoys || c == (unsigned char)IRSensor::RedAndGreenBuoysAndForceField) {
			d->move(170);
		} else if (c == (unsigned char)IRSensor::RedBuoy || c == (unsigned char)IRSensor::RedBuoyAndForceField) {
			d->move(Drive::Medium, 100);
			m_lastDir = Drive::Left;
		} else if (c == (unsigned char)IRSensor::GreenBuoy || c == (unsigned char)IRSensor::GreenBuoyAndForceField) {
			d->move(Drive::Medium, -100);
			m_lastDir = Drive::Right;
		} else {
			if (m_lastDir == Drive::Right)
				d->move(Drive::Medium, 200);
			else
				d->move(Drive::Medium, -200);
		}
	} else {
		if (c == (unsigned char) IRSensor::ForceField || c == (unsigned char)IRSensor::RedBuoy || c == (unsigned char)IRSensor::GreenBuoy || c == (unsigned char)IRSensor::RedAndGreenBuoys) {
			m_tryingToDock = true;
		} else {
			d->move(Drive::Medium);
		}
	}
}

bool
DriveRobot::bumped()
{
    BumpAndDropSensor* s = static_cast<BumpAndDropSensor*> ( getSensor ( Sensor::BumpsAndDrops ) );
    BooleanSensor* wall = static_cast<BooleanSensor*> ( getSensor ( Sensor::VirtualWall ) );
	return s->flags() & BumpAndDropSensor::BumpAny || wall->value();
}

bool
DriveRobot::bumpedLeft()
{
    BumpAndDropSensor* s = static_cast<BumpAndDropSensor*> ( getSensor ( Sensor::BumpsAndDrops ) );
	return s->flags() & BumpAndDropSensor::BumpLeft;
}

bool
DriveRobot::bumpedRight()
{
    BumpAndDropSensor* s = static_cast<BumpAndDropSensor*> ( getSensor ( Sensor::BumpsAndDrops ) );
	return s->flags() & BumpAndDropSensor::BumpRight;
}

void
DriveRobot::loop()
{
	m_powerColor++;
	uint8_t leds;
	if (m_powerColor > 128)
		leds = Play;
	else
		leds = Advance;
	setLEDs(leds, m_powerColor, ((float)m_powerColor/255)*255);
	BooleanSensor* song = static_cast<BooleanSensor*> ( getSensor( Sensor::SongPlaying ) );
	if (song->value())
		return;
	ChargeSourceSensor* sources = static_cast<ChargeSourceSensor*> ( getSensor( Sensor::ChargeSources ) );
	if (sources->sources() & ChargeSourceSensor::Base) {
		UnsignedValueSensor* battery = static_cast<UnsignedValueSensor*> ( getSensor( Sensor::BatteryCharge ) );
		UnsignedValueSensor* capacity = static_cast<UnsignedValueSensor*> ( getSensor( Sensor::BatteryCapacity ) );
		IROBOT_DEBUG("Battery: " << battery->value() << "/" << capacity->value() << " " << (int)((battery->value()/(float)capacity->value())*100) << "%");
		return;
	}
	dock();
    Drive* d = getDrive();
	if (fellOffCliff()) {
		d->move(0);
		if (!song->value()) {
			playSong(1);
			delay(1000000);
		}
	} else if (reachedCliff()) {
        if (cliffSide() == Left)
            m_lastDir = Drive::Left;
        if (cliffSide() == Right)
            m_lastDir = Drive::Right;
		avoid();
	} else if ( bumped() ) {
		if ( bumpedLeft() && bumpedRight() ) {
			avoid();
		} else if ( bumpedLeft() ) {
			d->move ( Drive::Medium, Drive::Right);
			m_lastDir = Drive::Right;
            delay ( 3000 );
		} else {
			d->move ( Drive::Medium, Drive::Left );
			m_lastDir = Drive::Left;
            BooleanSensor* wall = static_cast<BooleanSensor*> ( getSensor( Sensor::Wall ) );
            do {
                updateSensor( Sensor::Wall );
            } while (wall->value());
		}
		m_speed = 30;
	} else if (!m_tryingToDock) {
		if (m_speed < Drive::Medium)
			m_speed+=20;
		d->move ( m_speed );
	}
}

void
DriveRobot::disconnected()
{
    IROBOT_DEBUG("Disconnected.");
    exit ( 0 );
}

Blob contents
Blob history
Raw blob data

1	#include "DriveRobot.h"
2	#include <libirobot/Robot.h>
3	#include <libirobot/Drive.h>
4	#include <libirobot/SerialControl.h>
5	#include <libirobot/sensors/BumpAndDropSensor.h>
6	#include <libirobot/sensors/BooleanSensor.h>
7	#include <libirobot/Song.h>
8	#include <libirobot/sensors/IRSensor.h>
9	#include <libirobot/sensors/ChargeSourceSensor.h>
10	#include <libirobot/sensors/UnsignedValueSensor.h>
11	#include <libirobot/sensors/ValueSensor.h>
12	#include <libirobot/Debug.h>
13
14	#include <math.h>
15
16	using namespace iRobot;
17	using namespace std;
18
19	#define QUARTER_NOTE 16
20
21	DriveRobot::DriveRobot ( const char ttyDevice[] )
22	: Robot ( ttyDevice ),
23	m_speed(0),
24	m_lastDir(Drive::Right),
25	m_tryingToDock(false),
26	m_dockAttempted(false),
27	m_dockTimeout(0)
28	{
29	setMode ( Full );
30	setAutoUpdate( Sensor::All, true );
31
32	Song cliffMusic(2);
33	Song::Note cliffnotes[] = {
34	{ 60, QUARTER_NOTE },
35	{ 57, QUARTER_NOTE },
36	};
37	for(int i = 0;i<2;i++) {
38	cliffMusic.setNotePitch(i, cliffnotes[i].pitch);
39	cliffMusic.setNoteLength(i, cliffnotes[i].length);
40	}
41	setSong(1, cliffMusic);
42	}
43
44	void
45	DriveRobot::avoid()
46	{
47	Drive* d = getDrive();
48	d->move(0);
49	d->move(-(int)Drive::Medium );
50	m_dockTimeout+=d->waitForDistance(100);
51	d->move ( Drive::Medium, m_lastDir );
52	d->waitForRotation(15);
53	}
54
55	bool
56	DriveRobot::reachedCliff()
57	{
58	BooleanSensor* cliffLeft = static_cast<BooleanSensor*> ( getSensor( Sensor::CliffLeft ) );
59	BooleanSensor* cliffFrontLeft = static_cast<BooleanSensor*> ( getSensor( Sensor::CliffFrontLeft ) );
60	BooleanSensor* cliffFrontRight = static_cast<BooleanSensor*> ( getSensor( Sensor::CliffFrontRight ) );
61	BooleanSensor* cliffRight = static_cast<BooleanSensor*> ( getSensor( Sensor::CliffRight ) );
62	return cliffLeft->value() \|\| cliffFrontRight->value() \|\| cliffFrontLeft->value() \|\| cliffRight->value();
63	}
64
65	DriveRobot::Side
66	DriveRobot::cliffSide()
67	{
68	BooleanSensor* cliffLeft = static_cast<BooleanSensor*> ( getSensor( Sensor::CliffLeft ) );
69	BooleanSensor* cliffFrontLeft = static_cast<BooleanSensor*> ( getSensor( Sensor::CliffFrontLeft ) );
70	BooleanSensor* cliffFrontRight = static_cast<BooleanSensor*> ( getSensor( Sensor::CliffFrontRight ) );
71	BooleanSensor* cliffRight = static_cast<BooleanSensor*> ( getSensor( Sensor::CliffRight ) );
72	if (cliffFrontLeft->value() && cliffFrontRight->value())
73	return Center;
74	if (cliffLeft->value() \|\| cliffFrontLeft->value())
75	return Left;
76	if (cliffRight->value() \|\| cliffFrontRight->value())
77	return Right;
78	}
79
80	bool
81	DriveRobot::fellOffCliff()
82	{
83	BumpAndDropSensor* s = static_cast<BumpAndDropSensor*> ( getSensor ( Sensor::BumpsAndDrops ) );
84	return s->flags() & BumpAndDropSensor::DropAny;
85	}
86
87	void
88	DriveRobot::dock()
89	{
90	Drive* d = getDrive();
91	IRSensor* ir = static_cast<IRSensor*> ( getSensor( Sensor::IR ) );
92	unsigned char c = ir->value();
93	ChargeSourceSensor* sources = static_cast<ChargeSourceSensor*> ( getSensor( Sensor::ChargeSources ) );
94	ValueSensor* distance = static_cast<ValueSensor*> ( getSensor( Sensor::Distance ) );
95	m_dockTimeout+=distance->value();
96	if (m_dockTimeout > 2000) {
97	m_dockTimeout = 0;
98	m_tryingToDock = false;
99	m_dockAttempted = false;
100	} else if (sources->sources() & ChargeSourceSensor::Base) {
101	d->move(0);
102	m_tryingToDock = false;
103	return;
104	} else if (m_tryingToDock) {
105	if (m_dockAttempted) {
106	d->move(-Drive::Medium);
107	m_dockTimeout+=d->waitForDistance(300);
108	if (m_lastDir == Drive::Right)
109	d->move(Drive::Fast, Drive::Left);
110	else
111	d->move(Drive::Fast, Drive::Right);
112	d->waitForRotation(180);
113	d->move(Drive::Medium);
114	m_dockTimeout+=d->waitForDistance(100);
115	m_dockAttempted = false;
116	} else if ((c == (unsigned char)IRSensor::RedAndGreenBuoysAndForceField \|\| c == (unsigned char)IRSensor::ForceField) && bumped()) {
117	d->move(-Drive::Slow);
118	m_dockTimeout+=d->waitForDistance(15);
119	d->move(0);
120	delay(1000000);
121	m_dockAttempted = true;
122	} else if (c == (unsigned char)IRSensor::RedAndGreenBuoys \|\| c == (unsigned char)IRSensor::RedAndGreenBuoysAndForceField) {
123	d->move(170);
124	} else if (c == (unsigned char)IRSensor::RedBuoy \|\| c == (unsigned char)IRSensor::RedBuoyAndForceField) {
125	d->move(Drive::Medium, 100);
126	m_lastDir = Drive::Left;
127	} else if (c == (unsigned char)IRSensor::GreenBuoy \|\| c == (unsigned char)IRSensor::GreenBuoyAndForceField) {
128	d->move(Drive::Medium, -100);
129	m_lastDir = Drive::Right;
130	} else {
131	if (m_lastDir == Drive::Right)
132	d->move(Drive::Medium, 200);
133	else
134	d->move(Drive::Medium, -200);
135	}
136	} else {
137	if (c == (unsigned char) IRSensor::ForceField \|\| c == (unsigned char)IRSensor::RedBuoy \|\| c == (unsigned char)IRSensor::GreenBuoy \|\| c == (unsigned char)IRSensor::RedAndGreenBuoys) {
138	m_tryingToDock = true;
139	} else {
140	d->move(Drive::Medium);
141	}
142	}
143	}
144
145	bool
146	DriveRobot::bumped()
147	{
148	BumpAndDropSensor* s = static_cast<BumpAndDropSensor*> ( getSensor ( Sensor::BumpsAndDrops ) );
149	BooleanSensor* wall = static_cast<BooleanSensor*> ( getSensor ( Sensor::VirtualWall ) );
150	return s->flags() & BumpAndDropSensor::BumpAny \|\| wall->value();
151	}
152
153	bool
154	DriveRobot::bumpedLeft()
155	{
156	BumpAndDropSensor* s = static_cast<BumpAndDropSensor*> ( getSensor ( Sensor::BumpsAndDrops ) );
157	return s->flags() & BumpAndDropSensor::BumpLeft;
158	}
159
160	bool
161	DriveRobot::bumpedRight()
162	{
163	BumpAndDropSensor* s = static_cast<BumpAndDropSensor*> ( getSensor ( Sensor::BumpsAndDrops ) );
164	return s->flags() & BumpAndDropSensor::BumpRight;
165	}
166
167	void
168	DriveRobot::loop()
169	{
170	m_powerColor++;
171	uint8_t leds;
172	if (m_powerColor > 128)
173	leds = Play;
174	else
175	leds = Advance;
176	setLEDs(leds, m_powerColor, ((float)m_powerColor/255)*255);
177	BooleanSensor* song = static_cast<BooleanSensor*> ( getSensor( Sensor::SongPlaying ) );
178	if (song->value())
179	return;
180	ChargeSourceSensor* sources = static_cast<ChargeSourceSensor*> ( getSensor( Sensor::ChargeSources ) );
181	if (sources->sources() & ChargeSourceSensor::Base) {
182	UnsignedValueSensor* battery = static_cast<UnsignedValueSensor*> ( getSensor( Sensor::BatteryCharge ) );
183	UnsignedValueSensor* capacity = static_cast<UnsignedValueSensor*> ( getSensor( Sensor::BatteryCapacity ) );
184	IROBOT_DEBUG("Battery: " << battery->value() << "/" << capacity->value() << " " << (int)((battery->value()/(float)capacity->value())*100) << "%");
185	return;
186	}
187	dock();
188	Drive* d = getDrive();
189	if (fellOffCliff()) {
190	d->move(0);
191	if (!song->value()) {
192	playSong(1);
193	delay(1000000);
194	}
195	} else if (reachedCliff()) {
196	if (cliffSide() == Left)
197	m_lastDir = Drive::Left;
198	if (cliffSide() == Right)
199	m_lastDir = Drive::Right;
200	avoid();
201	} else if ( bumped() ) {
202	if ( bumpedLeft() && bumpedRight() ) {
203	avoid();
204	} else if ( bumpedLeft() ) {
205	d->move ( Drive::Medium, Drive::Right);
206	m_lastDir = Drive::Right;
207	delay ( 3000 );
208	} else {
209	d->move ( Drive::Medium, Drive::Left );
210	m_lastDir = Drive::Left;
211	BooleanSensor* wall = static_cast<BooleanSensor*> ( getSensor( Sensor::Wall ) );
212	do {
213	updateSensor( Sensor::Wall );
214	} while (wall->value());
215	}
216	m_speed = 30;
217	} else if (!m_tryingToDock) {
218	if (m_speed < Drive::Medium)
219	m_speed+=20;
220	d->move ( m_speed );
221	}
222	}
223
224	void
225	DriveRobot::disconnected()
226	{
227	IROBOT_DEBUG("Disconnected.");
228	exit ( 0 );
229	}