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>

#ifndef __AVR__
#include <iostream>
#endif

using namespace iRobot;

#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 );
}

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

Gitorious