compAIR/libcreate
compAIR/libcreate
4
0
Fork 1
Code Issues Pull requests Projects Releases Packages 1 Wiki Activity Actions

Initial commit.

Browse source
This commit is contained in:
jacobperron 2015-12-11 14:08:34 -08:00
commit 43c7b95361
14 changed files with 1919 additions and 0 deletions
Download patch file Download diff file Expand all files Collapse all files

42
CMakeLists.txt Normal file
View file

@ -0,0 +1,42 @@
cmake_minimum_required(VERSION 2.8.3)
project(libcreate)
find_package(Boost REQUIRED system thread)
## Specify additional locations of header files
include_directories(
include
)
## Declare cpp library
add_library(create
src/create.cpp
src/serial.cpp
src/data.cpp
src/packet.cpp
)
target_link_libraries(create
${Boost_LIBRARIES}
)
## Declare example executables
add_executable(create_demo examples/create_demo.cpp)
## Specify libraries to link a library or executable target against
target_link_libraries(create_demo
${Boost_LIBRARIES}
create
)
## Install
## I'm not familiar with install rules, do these make sense?
install(TARGETS create DESTINATION lib)
install(FILES
include/create/create.h
include/create/serial.h
include/create/types.h
include/create/data.h
include/create/packet.h
include/create/util.h
DESTINATION include/create)

25
LICENSE Normal file
View file

@ -0,0 +1,25 @@
Copyright (c) 2015, Jacob Perron (Autonomy Lab, Simon Fraser University)
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice,
this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of Autonomy Lab nor the names of its contributors may
be used to endorse or promote products derived from this software without
specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.

24
README.md Normal file
View file

@ -0,0 +1,24 @@
# libcreate
C++ library for interfacing with iRobot's [Create 2](http://www.irobot.com/About-iRobot/STEM/Create-2.aspx).
* Documentation: TODO
* Author: [Jacob Perron](http://jacobperron.ca) ([Autonomy Lab](http://autonomylab.org), [Simon Fraser University](http://www.sfu.ca))
* Contributors: [Mani Monajjemi](http:mani.im)
## Dependencies
* [Boost System Library](http://www.boost.org/doc/libs/1_59_0/libs/system/doc/index.html)
* [Boost Thread Library](http://www.boost.org/doc/libs/1_59_0/doc/html/thread.html)
## Install
* `cmake CMakeLists.txt`
* `make`
* `sudo make install`
## Example
See source for examples.
Example compile line: `g++ create_demo.cpp -lcreate -lboost_system -lboost_thread`

125
examples/create_demo.cpp Normal file
View file

@ -0,0 +1,125 @@
#include "create/create.h"
create::Create* robot;
int main(int argc, char** argv) {
std::string port = "/dev/ttyUSB0";
robot = new create::Create();
// Attempt to connect to Create
if (robot->connect(port, 115200))
std::cout << "Successfully connected to Create" << std::endl;
else {
std::cout << "Failed to connect to Create on port " << port.c_str() << std::endl;
return 1;
}
// Note: Some functionality does not work as expected in Safe mode
robot->setMode(create::MODE_FULL);
std::cout << "battery level: " <<
robot->getBatteryCharge() / (float) robot->getBatteryCapacity() * 100.0 << "%" << std::endl;
bool drive = false;
// Make a song
//uint8_t songLength = 16;
//uint8_t notes[16] = { 67, 67, 66, 66, 65, 65, 66, 66,
// 67, 67, 66, 66, 65, 65, 66, 66 };
//float durations[songLength];
//for (int i = 0; i < songLength; i++) {
// durations[i] = 0.25;
//}
//robot->createSong(0, songLength, notes, durations);
//usleep(1000000);
//robot->playSong(0);
// Quit when center "Clean" button pressed
while (!robot->isCleanButtonPressed()) {
// Check for button presses
if (robot->isDayButtonPressed())
std::cout << "day button press" << std::endl;
if (robot->isMinButtonPressed())
std::cout << "min button press" << std::endl;
if (robot->isDockButtonPressed()) {
std::cout << "dock button press" << std::endl;
robot->enableCheckRobotLED(false);
}
if (robot->isSpotButtonPressed()) {
std::cout << "spot button press" << std::endl;
robot->enableCheckRobotLED(true);
}
if (robot->isHourButtonPressed()) {
std::cout << "hour button press" << std::endl;
drive = !drive;
}
// Check for wheeldrop or cliffs
if (robot->isWheeldrop() || robot->isCliff()) {
drive = false;
robot->setPowerLED(255);
}
// If everything is ok, drive forward using IR's to avoid obstacles
if (drive) {
robot->setPowerLED(0); // green
if (robot->isIRDetectLeft() ||
robot->isIRDetectFrontLeft() ||
robot->isIRDetectCenterLeft()) {
// turn right
robot->drive(0.1, -1.0);
robot->setDigitsASCII('-','-','-',']');
}
else if (robot->isIRDetectRight() ||
robot->isIRDetectFrontRight() ||
robot->isIRDetectCenterRight()) {
// turn left
robot->drive(0.1, 1.0);
robot->setDigitsASCII('[','-','-','-');
}
else {
// drive straight
robot->drive(0.1, 0.0);
robot->setDigitsASCII(' ','^','^',' ');
}
}
else { // drive == false
// stop moving
robot->drive(0, 0);
robot->setDigitsASCII('S','T','O','P');
}
// Turn on blue 'debris' light if moving forward
if (robot->isMovingForward()) {
robot->enableDebrisLED(true);
}
else {
robot->enableDebrisLED(false);
}
// Check bumpers
if (robot->isLeftBumper()) {
std::cout << "left bump detected!" << std::endl;
}
if (robot->isRightBumper()) {
std::cout << "right bump detected!" << std::endl;
}
usleep(1000 * 100); //10hz
}
std::cout << "Stopping Create." << std::endl;
// Turn off lights
robot->setPowerLED(0, 0);
robot->enableDebrisLED(false);
robot->enableCheckRobotLED(false);
robot->setDigitsASCII(' ', ' ', ' ', ' ');
// Make sure to disconnect to clean up
robot->disconnect();
delete robot;
return 0;
}

353
include/create/create.h Normal file
View file

@ -0,0 +1,353 @@
/**
Software License Agreement (BSD)
\file create.h
\authors Jacob Perron <jperron@sfu.ca>
\copyright Copyright (c) 2015, Autonomy Lab (Simon Fraser University), All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice,
this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of Autonomy Lab nor the names of its contributors may
be used to endorse or promote products derived from this software without
specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef CREATE_H
#define CREATE_H
#include <boost/shared_ptr.hpp>
#include <string>
#include <sys/time.h>
#include <unistd.h>
#include "create/serial.h"
#include "create/data.h"
#include "create/types.h"
namespace create {
class Create {
private:
enum CreateLED {
LED_DEBRIS = 1,
LED_SPOT = 2,
LED_DOCK = 4,
LED_CHECK = 8
};
uint8_t mainMotorPower;
uint8_t sideMotorPower;
uint8_t vacuumMotorPower;
// LEDs
uint8_t debrisLED;
uint8_t spotLED;
uint8_t dockLED;
uint8_t checkLED;
uint8_t powerLED;
uint8_t powerLEDIntensity;
create::Pose pose;
uint32_t prevTicksLeft;
uint32_t prevTicksRight;
float prevLeftVel;
float prevRightVel;
bool firstOnData;
void init();
bool updateLEDs();
void onData();
protected:
boost::shared_ptr<create::Data> data;
boost::shared_ptr<create::Serial> serial;
public:
/* Default constructor.
* Does not attempt to establish serial connection to Create.
*/
Create();
/* Attempts to establish serial connection to Create.
*/
Create(const std::string& port, const int& baud);
~Create();
/* Make a serial connection to Create.
* This is the first thing that should be done after instantiated this class.
* \return true if a successful connection is established, false otherwise.
*/
bool connect(const std::string& port, const int& baud);
inline bool connected() const { return serial->connected(); };
/* Disconnect from serial.
*/
void disconnect();
/* Resets as if you have removed the battery.
* Changes mode to MODE_PASSIVE.
*/
//void reset();
//void setBaud(int baudcode);
/* Change Create mode.
*/
bool setMode(const create::CreateMode& mode);
/* Starts a cleaning mode.
* Changes mode to MODE_PASSIVE.
*/
bool clean(const create::CleanMode& mode = CLEAN_DEFAULT);
/* Starts the docking behaviour.
* Changes mode to MODE_PASSIVE.
*/
bool dock() const;
/* Sets the internal clock of Create.
* \param day in range [0, 6]
* \param hour in range [0, 23]
* \param min in range [0, 59]
*/
bool setDate(const create::DayOfWeek& day, const uint8_t& hour, const uint8_t& min) const;
/* Set the average wheel velocity and turning radius of Create.
* \param vel is in m/s
* \param radius in meters.
*/
//void driveRadius(const float& vel, const float& radius) const;
/* Set the velocities for the left and right wheels (m/s).
*/
bool driveWheels(const float& leftWheel, const float& rightWheel) const;
/* Set the PWM for each wheel.
*/
//void drivePWM(const int16_t& leftWheel, const int16_t& rightWheel) const;
/* Set the forward and angular velocity of Create.
* \param xVel in m/s
* \param angularVel in rads/s
*/
bool drive(const float& xVel, const float& angularVel) const;
/* Set the power to the side brush motor.
* \param power is in the range [-1, 1]
*/
bool setSideMotor(const float& power);
/* Set the power to the main brush motor.
* \param power is in the range [-1, 1]
*/
bool setMainMotor(const float& power);
/* Set the power to the vacuum motor.
* \param power is in the range [0, 1]
*/
bool setVacuumMotor(const float& power);
/* Set the power of all motors.
* \param mainPower in the range [-1, 1]
* \param sidePower in the range [-1, 1]
* \param vacuumPower in the range [0, 1]
*/
bool setAllMotors(const float& mainPower, const float& sidePower, const float& vacuumPower);
/* Set the blue "debris" LED on/off.
*/
bool enableDebrisLED(const bool& enable);
/* Set the green "spot" LED on/off.
*/
bool enableSpotLED(const bool& enable);
/* Set the green "dock" LED on/off.
*/
bool enableDockLED(const bool& enable);
/* Set the orange "check Create" LED on/off.
*/
bool enableCheckRobotLED(const bool& enable);
/* Set the center power LED.
* \param power in range [0, 255] where 0 = green and 255 = red
* \param intensity in range [0, 255]
*/
bool setPowerLED(const uint8_t& power, const uint8_t& intensity = 255);
// TODO
//void setScheduleLED(...);
/* Set the four 7-segment display digits from left to right.
*/
// TODO
//void setDigits(uint8_t digit1, uint8_t digit2,
// uint8_t digit3, uint8_t digit4);
// pushButton(...);
/* Set the four 7-segment display digits from left to right with ASCII codes.
* Any code out side the accepted ascii ranges results in blank display.
* \param digit1 is left most digit with ascii range [32, 126]
* \param digit2 is second to left digit with ascii range [32, 126]
* \param digit3 is second to right digit with ascii range [32, 126]
* \param digit4 is right most digit with ascii range [32, 126]
*/
bool setDigitsASCII(const uint8_t& digit1, const uint8_t& digit2,
const uint8_t& digit3, const uint8_t& digit4) const;
/* Defines a song from the provided notes and labels it with a song number.
* \param songNumber can be one of four possible song slots, [0, 4]
* \param songLength is the number of notes, maximum 16.
* length(notes) = length(durations) = songLength should be true.
* \param notes is a sequence of notes. Each note is in the range [31, 127].
* Anything outside this range is considered a rest note.
* \param durations for each note in fractions of a second from the range [0, 4)
*/
bool defineSong(const uint8_t& songNumber,
const uint8_t& songLength,
const uint8_t* notes,
const float* durations) const;
/* Play a previously created song.
* This command will not work if a song was not already defined with the specified song number.
*/
bool playSong(const uint8_t& songNumber) const;
//void registerCallback(...);
/* True if a left or right wheeldrop is detected.
*/
bool isWheeldrop() const;
/* Returns true if left bumper is pressed, false otherwise.
*/
bool isLeftBumper() const;
/* Returns true if right bumper is pressed, false otherwise.
*/
bool isRightBumper() const;
/* True if wall is seen to right of Create, false otherwise.
*/
bool isWall() const;
/* True if there are any cliff detections, false otherwise.
*/
bool isCliff() const;
//TODO
//bool isVirtualWall() const;
//TODO
//bool isWheelOvercurrent() const;
/* Get level of the dirt detect sensor.
* \return value in range [0, 255]
*/
uint8_t getDirtDetect() const;
/* Get value of 8-bit IR character currently being received by omnidirectional sensor.
* \return value in range [0, 255]
*/
uint8_t getIROmni() const;
/* Get value of 8-bit IR character currently being received by left sensor.
* \return value in range [0, 255]
*/
uint8_t getIRLeft() const;
/* Get value of 8-bit IR character currently being received by right sensor.
* \return value in range [0, 255]
*/
uint8_t getIRRight() const;
bool isCleanButtonPressed() const;
bool isClockButtonPressed() const;
bool isScheduleButtonPressed() const;
bool isDayButtonPressed() const;
bool isHourButtonPressed() const;
bool isMinButtonPressed() const;
bool isDockButtonPressed() const;
bool isSpotButtonPressed() const;
/* Get battery voltage.
* \return value in millivolts
*/
uint16_t getVoltage() const;
/* Get current flowing in/out of battery.
* \return value in milliamps
*/
uint16_t getCurrent() const;
/* Get the temperature of battery.
* \return value in Celsius
*/
uint8_t getTemperature() const;
/* Get battery's remaining charge.
* \return value in milliamp-hours
*/
uint16_t getBatteryCharge() const;
/* Get estimated battery charge capacity.
* \return in milliamp-hours
*/
uint16_t getBatteryCapacity() const;
bool isIRDetectLeft() const;
bool isIRDetectFrontLeft() const;
bool isIRDetectCenterLeft() const;
bool isIRDetectRight() const;
bool isIRDetectFrontRight() const;
bool isIRDetectCenterRight() const;
uint16_t getDistLeft() const;
uint16_t getDistFrontLeft() const;
uint16_t getDistCenterLeft() const;
uint16_t getDistRight() const;
uint16_t getDistFrontRight() const;
uint16_t getDistCenterRight() const;
/* Return true if Create is moving forward.
*/
bool isMovingForward() const;
/* Get the current charging state.
*/
create::ChargingState getChargingState() const;
/* Get the current mode reported by Create.
*/
create::CreateMode getMode() const;
/* Get the estimated position of Create based on it's wheel encoders.
*/
const create::Pose& getPose() const;
}; // end Create class
} // namespace create
#endif // CREATE_DRIVER_H

63
include/create/data.h Normal file
View file

@ -0,0 +1,63 @@
/**
Software License Agreement (BSD)
\file data.h
\authors Jacob Perron <jperron@sfu.ca>
\copyright Copyright (c) 2015, Autonomy Lab (Simon Fraser University), All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice,
this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of Autonomy Lab nor the names of its contributors may
be used to endorse or promote products derived from this software without
specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef CREATE_DATA_H
#define CREATE_DATA_H
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <map>
#include <vector>
#include "create/packet.h"
#include "create/types.h"
namespace create {
class Data {
private:
std::map<uint8_t, boost::shared_ptr<Packet> > packets;
uint32_t totalDataBytes;
std::vector<uint8_t> ids;
public:
Data();
~Data();
bool isValidPacketID(const uint8_t id) const;
boost::shared_ptr<Packet> getPacket(const uint8_t id);
void validateAll();
uint32_t getTotalDataBytes() const;
uint8_t getNumPackets() const;
std::vector<uint8_t> getPacketIDs();
};
} // namespace create
#endif // CREATE_DATA_H

61
include/create/packet.h Normal file
View file

@ -0,0 +1,61 @@
/**
Software License Agreement (BSD)
\file packet.h
\authors Jacob Perron <jperron@sfu.ca>
\copyright Copyright (c) 2015, Autonomy Lab (Simon Fraser University), All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice,
this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of Autonomy Lab nor the names of its contributors may
be used to endorse or promote products derived from this software without
specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef CREATE_PACKET_H
#define CREATE_PACKET_H
#include <boost/thread/mutex.hpp>
namespace create {
class Packet {
private:
uint16_t data;
uint16_t tmpData;
mutable boost::mutex dataMutex;
public:
// TODO: Do they really need to be const? then they better be static
// I am actually not sure if const member vars are valid
const uint8_t nbytes;
const std::string info;
Packet(const uint8_t& nbytes, const std::string& info);
~Packet();
void setTempData(const uint16_t& td);
void validate();
// thread safe
void setData(const uint16_t& d);
// thread safe
uint16_t getData() const;
};
} // namepsace create
#endif // CREATE_PACKET_H

106
include/create/serial.h Normal file
View file

@ -0,0 +1,106 @@
/**
Software License Agreement (BSD)
\file serial.h
\authors Jacob Perron <jperron@sfu.ca>
\copyright Copyright (c) 2015, Autonomy Lab (Simon Fraser University), All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice,
this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of Autonomy Lab nor the names of its contributors may
be used to endorse or promote products derived from this software without
specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.
*/
// Based on example from:
// https://github.com/labust/labust-ros-pkg/wiki/Create-a-Serial-Port-application
#ifndef CREATE_SERIAL_H
#define CREATE_SERIAL_H
#include <boost/asio.hpp>
#include <boost/thread.hpp>
#include <boost/thread/condition_variable.hpp>
#include <boost/function.hpp>
#include <boost/shared_ptr.hpp>
#include "create/data.h"
#include "create/types.h"
#include "create/util.h"
namespace create {
class Serial {
private:
enum ReadState {
READ_HEADER,
READ_NBYTES,
READ_PACKET_ID,
READ_PACKET_BYTES,
READ_CHECKSUM
};
boost::shared_ptr<Data> data;
boost::asio::io_service io;
boost::asio::serial_port port;
boost::thread ioThread;
boost::condition_variable dataReadyCond;
boost::mutex dataReadyMut;
ReadState readState;
bool dataReady;
bool isReading;
bool firstRead;
// These are just for diagnostics, maybe not necessary
// TODO: Investigate
uint64_t corruptPackets;
uint64_t totalPackets;
// State machine variables
uint8_t headerByte;
uint8_t packetID;
uint8_t expectedNumBytes;
uint8_t byteRead;
uint16_t packetBytes;
uint8_t numBytesRead;
uint32_t byteSum;
uint8_t numDataBytesRead;
uint8_t expectedNumDataBytes;
// Callback executed when data arrives from Create
// TODO: Should size be const?
// Not sure, this was from example
void onData(const boost::system::error_code& e, const std::size_t& size);
// Callback to execute once data arrives
boost::function<void()> callback;
// Start and stop reading data from Create
bool startReading();
void stopReading();
public:
Serial(boost::shared_ptr<Data> data, const uint8_t& header = create::util::CREATE_2_HEADER);
~Serial();
bool connect(const std::string& port, const int& baud = 115200, boost::function<void()> cb = 0);
void disconnect();
inline bool connected() const { return port.is_open(); };
bool send(const uint8_t* bytes, const uint32_t numBytes);
bool sendOpcode(const Opcode& code);
};
} // namespace create
#endif // CREATE_SERIAL_H

190
include/create/types.h Normal file
View file

@ -0,0 +1,190 @@
/**
Software License Agreement (BSD)
\file types.h
\authors Jacob Perron <jperron@sfu.ca>
\copyright Copyright (c) 2015, Autonomy Lab (Simon Fraser University), All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice,
this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of Autonomy Lab nor the names of its contributors may
be used to endorse or promote products derived from this software without
specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef CREATE_TYPES_H
#define CREATE_TYPES_H
namespace create {
enum SensorPacketID {
ID_GROUP_0 = 0,
ID_GROUP_1 = 1,
ID_GROUP_2 = 2,
ID_GROUP_3 = 3,
ID_GROUP_4 = 4,
ID_GROUP_5 = 5,
ID_GROUP_6 = 6,
ID_GROUP_100 = 100,
ID_GROUP_101 = 101,
ID_GROUP_106 = 106,
ID_GROUP_107 = 107,
ID_BUMP_WHEELDROP = 7,
ID_WALL = 8,
ID_CLIFF_LEFT = 9,
ID_CLIFF_FRONT_LEFT = 10,
ID_CLIFF_FRONT_RIGHT = 11,
ID_CLIFF_RIGHT = 12,
ID_VIRTUAL_WALL = 13,
ID_OVERCURRENTS = 14,
ID_DIRT_DETECT = 15,
ID_UNUSED_1 = 16,
ID_IR_OMNI = 17,
ID_IR_LEFT = 52,
ID_IR_RIGHT = 53,
ID_BUTTONS = 18,
ID_DISTANCE = 19,
ID_ANGLE = 20,
ID_CHARGE_STATE = 21,
ID_VOLTAGE = 22,
ID_CURRENT = 23,
ID_TEMP = 24,
ID_CHARGE = 25,
ID_CAPACITY = 26,
ID_WALL_SIGNAL = 27,
ID_CLIFF_LEFT_SIGNAL = 28,
ID_CLIFF_FRONT_LEFT_SIGNAL = 29,
ID_CLIFF_FRONT_RIGHT_SIGNAL = 30,
ID_CLIFF_RIGHT_SIGNAL = 31,
ID_UNUSED_2 = 32,
ID_UNUSED_3 = 33,
ID_CHARGE_SOURCE = 34,
ID_IO_MODE = 35,
ID_SONG_NUM = 36,
ID_PLAYING = 37,
ID_NUM_STREAM_PACKETS = 38,
ID_VEL = 39,
ID_RADIUS = 40,
ID_RIGHT_VEL = 41,
ID_LEFT_VEL = 42,
ID_LEFT_ENC = 43,
ID_RIGHT_ENC = 44,
ID_LIGHT = 45,
ID_LIGHT_LEFT = 46,
ID_LIGHT_FRONT_LEFT = 47,
ID_LIGHT_CENTER_LEFT = 48,
ID_LIGHT_CENTER_RIGHT = 49,
ID_LIGHT_FRONT_RIGHT = 50,
ID_LIGHT_RIGHT = 51,
ID_LEFT_MOTOR_CURRENT = 54,
ID_RIGHT_MOTOR_CURRENT = 55,
ID_MAIN_BRUSH_CURRENT = 56,
ID_SIDE_BRUSH_CURRENT = 57,
ID_STASIS = 58,
ID_NUM = 52
};
enum Opcode {
OC_START = 128,
OC_RESET = 7,
OC_STOP = 173,
OC_BAUD = 129,
OC_SAFE = 131,
OC_FULL = 132,
OC_CLEAN = 135,
OC_MAX = 136,
OC_SPOT = 134,
OC_DOCK = 143,
OC_POWER = 133,
OC_SCHEDULE = 167,
OC_DATE = 168,
OC_DRIVE = 137,
OC_DRIVE_DIRECT = 145,
OC_DRIVE_PWM = 146,
OC_MOTORS = 138,
OC_MOTORS_PWM = 144,
OC_LEDS = 139,
OC_SCHEDULING_LEDS = 162,
OC_DIGIT_LEDS_RAW = 163,
OC_BUTTONS = 165,
OC_DIGIT_LEDS_ASCII = 164,
OC_SONG = 140,
OC_PLAY = 141,
OC_SENSORS= 142,
OC_QUERY_LIST=149,
OC_STREAM = 148,
OC_TOGGLE_STREAM = 150,
};
enum BAUDCODE {
BAUD_300 = 0,
BAUD_600 = 1,
BAUD_1200 = 2,
BAUD_2400 = 3,
BAUD_4800 = 4,
BAUD_9600 = 5,
BAUD_14400 = 6,
BAUD_19200 = 7,
BAUD_28800 = 8,
BAUD_38400 = 9,
BAUD_57600 = 10,
BAUD_115200 = 11
};
enum CreateMode {
MODE_OFF = OC_POWER,
MODE_PASSIVE = OC_START,
MODE_SAFE = OC_SAFE,
MODE_FULL = OC_FULL
};
enum CleanMode {
CLEAN_DEFAULT = OC_CLEAN,
CLEAN_MAX = OC_MAX,
CLEAN_SPOT = OC_SPOT
};
enum ChargingState {
CHARGE_NONE = 0,
CHARGE_RECONDITION = 1,
CHARGE_FULL = 2,
CHARGE_TRICKLE = 3,
CHARGE_WAITING = 4,
CHARGE_FAULT = 5
};
enum DayOfWeek {
SUN = 0,
MON = 1,
TUE = 2,
WED = 3,
THU = 4,
FRI = 5,
SAT = 6
};
struct Pose {
float x;
float y;
float yaw;
};
} // namespace create
#endif // CREATE_TYPES_H

61
include/create/util.h Normal file
View file

@ -0,0 +1,61 @@
/**
Software License Agreement (BSD)
\file util.h
\authors Jacob Perron <jperron@sfu.ca>
\copyright Copyright (c) 2015, Autonomy Lab (Simon Fraser University), All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice,
this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of Autonomy Lab nor the names of its contributors may
be used to endorse or promote products derived from this software without
specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef CREATE_UTIL_H
#define CREATE_UTIL_H
#define COUT(prefix,msg) (std::cout<<prefix<<msg<<std::endl)
#define CERR(prefix,msg) (std::cerr<<prefix<<msg<<std::endl)
namespace create {
namespace util {
static const uint8_t CREATE_2_HEADER = 19;
static const float CREATE_2_AXLE_LENGTH = 0.235;
static const float CREATE_2_TICKS_PER_REV = 508.8;
static const uint32_t CREATE_2_MAX_ENCODER_TICKS = 65535;
static const float CREATE_2_WHEEL_DIAMETER = 0.078;
static const float CREATE_2_MAX_VEL = 0.5;
static const float PI = 3.14159;
static const float TWO_PI = 6.28318;
static const float EPS = 0.001;
inline float normalizeAngle(const float& angle) {
float a = angle;
while (a < -PI) a += TWO_PI;
while (a > PI) a -= TWO_PI;
return a;
};
} // namespace util
} // namespace create
#endif // CREATE_UTIL_H

475
src/create.cpp Normal file
View file

@ -0,0 +1,475 @@
#include <boost/bind.hpp>
#include <boost/make_shared.hpp>
#include <iostream>
#include <cmath>
#include <ctime>
#include <assert.h>
#include "create/create.h"
#define GET_DATA(id) (data->getPacket(id)->getData())
#define BOUND(val,min,max) (val<min?val=min:(val>max?val=max:val=val))
namespace create {
// TODO: Handle SIGINT to do clean disconnect
void Create::init() {
mainMotorPower = 0;
sideMotorPower = 0;
vacuumMotorPower = 0;
debrisLED = 0;
spotLED = 0;
dockLED = 0;
checkLED = 0;
powerLED = 0;
powerLEDIntensity = 0;
prevTicksLeft = 0;
prevTicksRight = 0;
firstOnData = true;
pose.x = 0;
pose.y = 0;
pose.yaw = 0;
data = boost::shared_ptr<Data>(new Data());
serial = boost::make_shared<Serial>(data);
}
Create::Create() {
init();
}
Create::Create(const std::string& dev, const int& baud) {
init();
serial->connect(dev, baud);
}
Create::~Create() {
disconnect();
}
void Create::onData() {
if (firstOnData) {
// Initialize tick counts
prevTicksLeft = GET_DATA(ID_LEFT_ENC);
prevTicksRight = GET_DATA(ID_RIGHT_ENC);
firstOnData = false;
}
// Get cumulative ticks (wraps around at 65535)
uint16_t totalTicksLeft = GET_DATA(ID_LEFT_ENC);
uint16_t totalTicksRight = GET_DATA(ID_RIGHT_ENC);
// Compute ticks since last update
int ticksLeft = totalTicksLeft - prevTicksLeft;
int ticksRight = totalTicksRight - prevTicksRight;
prevTicksLeft = totalTicksLeft;
prevTicksRight = totalTicksRight;
// Handle wrap around
if (fabs(ticksLeft) > 0.9 * util::CREATE_2_MAX_ENCODER_TICKS) {
ticksLeft = (ticksLeft % util::CREATE_2_MAX_ENCODER_TICKS) + 1;
}
if (fabs(ticksRight) > 0.9 * util::CREATE_2_MAX_ENCODER_TICKS) {
ticksRight = (ticksRight % util::CREATE_2_MAX_ENCODER_TICKS) + 1;
}
// Compute distance travelled by each wheel
float leftWheelDist = (ticksLeft / util::CREATE_2_TICKS_PER_REV)
* util::CREATE_2_WHEEL_DIAMETER * util::PI;
float rightWheelDist = (ticksRight / util::CREATE_2_TICKS_PER_REV)
* util::CREATE_2_WHEEL_DIAMETER * util::PI;
float wheelDistDiff = rightWheelDist - leftWheelDist;
float deltaDist = (rightWheelDist + leftWheelDist) / 2.0;
// Moving straight
if (fabs(wheelDistDiff) < util::EPS) {
pose.x += deltaDist * cos(pose.yaw);
pose.y += deltaDist * sin(pose.yaw);
}
else {
float turnRadius = (util::CREATE_2_AXLE_LENGTH / 2.0) * (leftWheelDist + rightWheelDist) / wheelDistDiff;
float deltaYaw = (rightWheelDist - leftWheelDist) / util::CREATE_2_AXLE_LENGTH;
pose.x += turnRadius * (sin(pose.yaw + deltaYaw) - sin(pose.yaw));
pose.y += turnRadius * (cos(pose.yaw + deltaYaw) - cos(pose.yaw));
pose.yaw = util::normalizeAngle(pose.yaw + deltaYaw);
}
// Make user registered callbacks, if any
// TODO
}
bool Create::connect(const std::string& port, const int& baud) {
bool timeout = false;
time_t start, now;
float maxWait = 30; // seconds
float retryInterval = 5; //seconds
time(&start);
while (!serial->connect(port, baud, boost::bind(&Create::onData, this)) && !timeout) {
time(&now);
if (difftime(now, start) > maxWait) {
timeout = true;
CERR("[create::Create] ", "failed to connect over serial: timeout");
}
else {
usleep(retryInterval * 1000000);
COUT("[create::Create] ", "retrying to establish serial connection...");
}
}
return !timeout;
}
void Create::disconnect() {
serial->disconnect();
firstOnData = true;
}
//void Create::reset() {
// serial->sendOpcode(OC_RESET);
// serial->reset(); // better
// TODO : Should we request reading packets again?
//}
bool Create::setMode(const CreateMode& mode) {
return serial->sendOpcode((Opcode) mode);
}
bool Create::clean(const CleanMode& mode) {
return serial->sendOpcode((Opcode) mode);
}
bool Create::dock() const {
return serial->sendOpcode(OC_DOCK);
}
bool Create::setDate(const DayOfWeek& day, const uint8_t& hour, const uint8_t& min) const {
if (day < 0 || day > 6 ||
hour < 0 || hour > 23 ||
min < 0 || min > 59)
return false;
uint8_t cmd[4] = { OC_DATE, day, hour, min };
return serial->send(cmd, 4);
}
/*void Create::driveRadius(const float& vel, const float& radius) const {
// Expects each parameter as two bytes each and in millimeters
int16_t vel_mm = roundf(vel * 1000);
int16_t radius_mm = roundf(radius * 1000);
BOUND(vel_mm, -500, 500);
// Consider special cases for radius
if (radius_mm != 32768 && radius_mm != 32767 &&
radius_mm != -1 && radius_mm != 1) {
BOUND(radius_mm, -2000, 2000);
}
uint8_t cmd[5] = { OC_DRIVE,
vel_mm >> 8,
vel_mm & 0xff,
radius_mm >> 8,
radius_mm & 0xff
};
serial->send(cmd, 5);
}
*/
bool Create::driveWheels(const float& leftVel, const float& rightVel) const {
int16_t leftCmd = roundf(leftVel * 1000);
int16_t rightCmd = roundf(rightVel * 1000);
BOUND(leftCmd, -util::CREATE_2_MAX_VEL * 1000, util::CREATE_2_MAX_VEL * 1000);
BOUND(rightCmd, -util::CREATE_2_MAX_VEL * 1000, util::CREATE_2_MAX_VEL * 1000);
uint8_t cmd[5] = { OC_DRIVE_DIRECT,
rightCmd >> 8,
rightCmd & 0xff,
leftCmd >> 8,
leftCmd & 0xff
};
return serial->send(cmd, 5);
}
/*void Create::drivePWM(const int16_t& leftPWM, const int16_t& rightPWM) const {
uint8_t cmd[5] = { OC_DRIVE_PWM,
rightPWM >> 8,
rightPWM & 0xff,
leftPWM >> 8,
leftPWM & 0xff
};
serial->send(cmd, 5);
}
*/
bool Create::drive(const float& xVel, const float& angularVel) const {
// Compute left and right wheel velocities
float leftVel = xVel - ((util::CREATE_2_AXLE_LENGTH / 2.0) * angularVel);
float rightVel = xVel + ((util::CREATE_2_AXLE_LENGTH / 2.0) * angularVel);
return driveWheels(leftVel, rightVel);
}
bool Create::setAllMotors(const float& main, const float& side, const float& vacuum) {
if (main < -1.0 || main > 1.0 ||
side < -1.0 || side > 1.0 ||
vacuum < -1.0 || vacuum > 1.0)
return false;
mainMotorPower = roundf(main * 127);
sideMotorPower = roundf(side * 127);
vacuumMotorPower = roundf(vacuum * 127);
uint8_t cmd[4] = { OC_MOTORS_PWM,
mainMotorPower,
sideMotorPower,
vacuumMotorPower
};
return serial->send(cmd, 4);
}
bool Create::setMainMotor(const float& main) {
return setAllMotors(main, sideMotorPower, vacuumMotorPower);
}
bool Create::setSideMotor(const float& side) {
return setAllMotors(mainMotorPower, side, vacuumMotorPower);
}
bool Create::setVacuumMotor(const float& vacuum) {
return setAllMotors(mainMotorPower, sideMotorPower, vacuum);
}
bool Create::updateLEDs() {
uint8_t LEDByte = debrisLED + spotLED + dockLED + checkLED;
uint8_t cmd[4] = { OC_LEDS,
LEDByte,
powerLED,
powerLEDIntensity
};
return serial->send(cmd, 4);
}
bool Create::enableDebrisLED(const bool& enable) {
if (enable)
debrisLED = LED_DEBRIS;
else
debrisLED = 0;
return updateLEDs();
}
bool Create::enableSpotLED(const bool& enable) {
if (enable)
spotLED = LED_SPOT;
else
spotLED = 0;
return updateLEDs();
}
bool Create::enableDockLED(const bool& enable) {
if (enable)
dockLED = LED_DOCK;
else
dockLED = 0;
return updateLEDs();
}
bool Create::enableCheckRobotLED(const bool& enable) {
if (enable)
checkLED = LED_CHECK;
else
checkLED = 0;
return updateLEDs();
}
bool Create::setPowerLED(const uint8_t& power, const uint8_t& intensity) {
powerLED = power;
powerLEDIntensity = intensity;
return updateLEDs();
}
//void Create::setDigits(uint8_t digit1, uint8_t digit2,
// uint8_t digit3, uint8_t digit4) {
//}
bool Create::setDigitsASCII(const uint8_t& digit1, const uint8_t& digit2,
const uint8_t& digit3, const uint8_t& digit4) const {
if (digit1 < 32 || digit1 > 126 ||
digit2 < 32 || digit2 > 126 ||
digit3 < 32 || digit3 > 126 ||
digit4 < 32 || digit4 > 126)
return false;
uint8_t cmd[5] = { OC_DIGIT_LEDS_ASCII,
digit1,
digit2,
digit3,
digit4
};
return serial->send(cmd, 5);
}
bool Create::defineSong(const uint8_t& songNumber,
const uint8_t& songLength,
const uint8_t* notes,
const float* durations) const {
int i, j;
uint8_t duration;
uint8_t cmd[2 * songLength + 3];
cmd[0] = OC_SONG;
cmd[1] = songNumber;
cmd[2] = songLength;
j = 0;
for (i = 3; i < 2 * songLength + 3; i = i + 2) {
if (durations[j] < 0 || durations[j] >= 4)
return false;
duration = durations[j] * 64;
cmd[i] = notes[j];
cmd[i + 1] = duration;
j++;
}
return serial->send(cmd, 2 * songLength + 3);
}
bool Create::playSong(const uint8_t& songNumber) const {
if (songNumber < 0 || songNumber > 4)
return false;
uint8_t cmd[2] = { OC_PLAY, songNumber };
return serial->send(cmd, 2);
}
bool Create::isWheeldrop() const {
return (GET_DATA(ID_BUMP_WHEELDROP) & 0x0C) != 0;
}
bool Create::isLeftBumper() const {
return (GET_DATA(ID_BUMP_WHEELDROP) & 0x02) != 0;
}
bool Create::isRightBumper() const {
return (GET_DATA(ID_BUMP_WHEELDROP) & 0x01) != 0;
}
bool Create::isWall() const {
return GET_DATA(ID_WALL) == 1;
}
bool Create::isCliff() const {
return GET_DATA(ID_CLIFF_LEFT) == 1 ||
GET_DATA(ID_CLIFF_FRONT_LEFT) == 1 ||
GET_DATA(ID_CLIFF_FRONT_RIGHT) == 1 ||
GET_DATA(ID_CLIFF_RIGHT) == 1;
}
uint8_t Create::getDirtDetect() const {
return GET_DATA(ID_DIRT_DETECT);
}
uint8_t Create::getIROmni() const {
return GET_DATA(ID_IR_OMNI);
}
uint8_t Create::getIRLeft() const {
return GET_DATA(ID_IR_LEFT);
}
uint8_t Create::getIRRight() const {
return GET_DATA(ID_IR_RIGHT);
}
ChargingState Create::getChargingState() const {
uint8_t chargeState = GET_DATA(ID_CHARGE_STATE);
assert(chargeState >= 0);
assert(chargeState <= 5);
return (ChargingState) chargeState;
}
bool Create::isCleanButtonPressed() const {
return (GET_DATA(ID_BUTTONS) & 0x01) != 0;
}
// Not working. TODO Fix/report
bool Create::isClockButtonPressed() const {
return (GET_DATA(ID_BUTTONS) & 0x80) != 0;
}
// Not working. TODO Fix/report
bool Create::isScheduleButtonPressed() const {
return (GET_DATA(ID_BUTTONS) & 0x40) != 0;
}
bool Create::isDayButtonPressed() const {
return (GET_DATA(ID_BUTTONS) & 0x20) != 0;
}
bool Create::isHourButtonPressed() const {
return (GET_DATA(ID_BUTTONS) & 0x10) != 0;
}
bool Create::isMinButtonPressed() const {
return (GET_DATA(ID_BUTTONS) & 0x08) != 0;
}
bool Create::isDockButtonPressed() const {
return (GET_DATA(ID_BUTTONS) & 0x04) != 0;
}
bool Create::isSpotButtonPressed() const {
return (GET_DATA(ID_BUTTONS) & 0x02) != 0;
}
uint16_t Create::getVoltage() const {
return GET_DATA(ID_VOLTAGE);
}
uint16_t Create::getCurrent() const {
return GET_DATA(ID_CURRENT);
}
uint8_t Create::getTemperature() const {
return GET_DATA(ID_TEMP);
}
uint16_t Create::getBatteryCharge() const {
return GET_DATA(ID_CHARGE);
}
uint16_t Create::getBatteryCapacity() const {
return GET_DATA(ID_CAPACITY);
}
bool Create::isIRDetectLeft() const {
return (GET_DATA(ID_LIGHT) & 0x01) != 0;
}
bool Create::isIRDetectFrontLeft() const {
return (GET_DATA(ID_LIGHT) & 0x02) != 0;
}
bool Create::isIRDetectCenterLeft() const {
return (GET_DATA(ID_LIGHT) & 0x04) != 0;
}
bool Create::isIRDetectCenterRight() const {
return (GET_DATA(ID_LIGHT) & 0x08) != 0;
}
bool Create::isIRDetectFrontRight() const {
return (GET_DATA(ID_LIGHT) & 0x10) != 0;
}
bool Create::isIRDetectRight() const {
return (GET_DATA(ID_LIGHT) & 0x20) != 0;
}
bool Create::isMovingForward() const {
return GET_DATA(ID_STASIS) == 1;
}
const Pose& Create::getPose() const {
return pose;
}
} // end namespace

107
src/data.cpp Normal file
View file

@ -0,0 +1,107 @@
#include "create/data.h"
#define ADD_PACKET(id,nbytes,info) (packets[id]=boost::make_shared<Packet>(nbytes,info))
namespace create {
Data::Data() {
// Populate data map
ADD_PACKET(ID_BUMP_WHEELDROP, 1, "bumps_wheeldrops");
ADD_PACKET(ID_WALL, 1, "wall");
ADD_PACKET(ID_CLIFF_LEFT, 1, "cliff_left");
ADD_PACKET(ID_CLIFF_FRONT_LEFT, 1, "cliff_front_left");
ADD_PACKET(ID_CLIFF_FRONT_RIGHT, 1, "cliff_front_right");
ADD_PACKET(ID_CLIFF_RIGHT, 1, "cliff_right");
ADD_PACKET(ID_VIRTUAL_WALL, 1, "virtual_wall");
ADD_PACKET(ID_OVERCURRENTS, 1, "overcurrents");
ADD_PACKET(ID_DIRT_DETECT, 1, "dirt_detect");
ADD_PACKET(ID_UNUSED_1, 1, "unused 1");
ADD_PACKET(ID_IR_OMNI, 1, "ir_opcode");
ADD_PACKET(ID_BUTTONS, 1, "buttons");
ADD_PACKET(ID_DISTANCE, 2, "distance");
ADD_PACKET(ID_ANGLE, 2, "angle");
ADD_PACKET(ID_CHARGE_STATE, 1, "charging_state");
ADD_PACKET(ID_VOLTAGE, 2, "voltage");
ADD_PACKET(ID_CURRENT, 2, "current");
ADD_PACKET(ID_TEMP, 1, "temperature");
ADD_PACKET(ID_CHARGE , 2, "battery_charge");
ADD_PACKET(ID_CAPACITY, 2, "battery_capacity");
ADD_PACKET(ID_WALL_SIGNAL, 2, "wall_signal");
ADD_PACKET(ID_CLIFF_LEFT_SIGNAL, 2, "cliff_left_signal");
ADD_PACKET(ID_CLIFF_FRONT_LEFT_SIGNAL, 2, "cliff_front_left_signal");
ADD_PACKET(ID_CLIFF_FRONT_RIGHT_SIGNAL, 2, "cliff_front_right_signal");
ADD_PACKET(ID_CLIFF_RIGHT_SIGNAL, 2, "cliff_right_signal");
ADD_PACKET(ID_UNUSED_2, 1, "unused 2");
ADD_PACKET(ID_UNUSED_3, 2, "unused 3");
ADD_PACKET(ID_CHARGE_SOURCE, 1, "charger_available");
ADD_PACKET(ID_IO_MODE, 1, "oi_mode");
ADD_PACKET(ID_SONG_NUM, 1, "song_number");
ADD_PACKET(ID_PLAYING, 1, "song_playing");
ADD_PACKET(ID_NUM_STREAM_PACKETS, 1, "oi_stream_num_packets");
ADD_PACKET(ID_VEL, 2, "velocity");
ADD_PACKET(ID_RADIUS, 2, "radius");
ADD_PACKET(ID_RIGHT_VEL, 2, "velocity_right");
ADD_PACKET(ID_LEFT_VEL, 2, "velocity_left");
ADD_PACKET(ID_LEFT_ENC, 2, "enc_counts_left");
ADD_PACKET(ID_RIGHT_ENC, 2, "enc_counts_right");
ADD_PACKET(ID_LIGHT, 1, "light_bumper");
ADD_PACKET(ID_LIGHT_LEFT, 2, "light_bumper_left");
ADD_PACKET(ID_LIGHT_FRONT_LEFT, 2, "light_bumper_front_left");
ADD_PACKET(ID_LIGHT_CENTER_LEFT, 2, "light_bumper_center_left");
ADD_PACKET(ID_LIGHT_CENTER_RIGHT, 2, "light_bumper_center_right");
ADD_PACKET(ID_LIGHT_FRONT_RIGHT, 2, "light_bumper_front_right");
ADD_PACKET(ID_LIGHT_RIGHT, 2, "light_bumper_right");
ADD_PACKET(ID_IR_LEFT, 1, "ir_opcode_left");
ADD_PACKET(ID_IR_RIGHT, 1, "ir_opcode_right");
ADD_PACKET(ID_LEFT_MOTOR_CURRENT, 2, "left_motor_current");
ADD_PACKET(ID_RIGHT_MOTOR_CURRENT, 2, "right_motor_current");
ADD_PACKET(ID_MAIN_BRUSH_CURRENT, 2, "main_brush_current");
ADD_PACKET(ID_SIDE_BRUSH_CURRENT, 2, "side_brush_current");
ADD_PACKET(ID_STASIS, 1, "stasis");
totalDataBytes = 0;
for (std::map<uint8_t, boost::shared_ptr<Packet> >::iterator it = packets.begin();
it != packets.end();
++it) {
ids.push_back(it->first);
totalDataBytes += it->second->nbytes;
}
}
Data::~Data() { }
bool Data::isValidPacketID(uint8_t id) const {
if (packets.count(id)) {
return true;
}
return false;
}
boost::shared_ptr<Packet> Data::getPacket(uint8_t id) {
if (isValidPacketID(id)) {
return packets[id];
}
return boost::shared_ptr<Packet>(); //NULL;
}
void Data::validateAll() {
for (std::map<uint8_t, boost::shared_ptr<Packet> >::iterator it = packets.begin();
it != packets.end();
++it) {
it->second->validate();
}
}
unsigned int Data::getTotalDataBytes() const {
return totalDataBytes;
}
uint8_t Data::getNumPackets() const {
return packets.size();
}
std::vector<uint8_t> Data::getPacketIDs() {
return ids;
}
} // namespace create

32
src/packet.cpp Normal file
View file

@ -0,0 +1,32 @@
#include "create/packet.h"
namespace create {
Packet::Packet(const uint8_t& numBytes, const std::string& comment) :
nbytes(numBytes),
info(comment),
data(0),
tmpData(0) { }
Packet::~Packet() { }
void Packet::setTempData(const uint16_t& tmp) {
// mutex for tmpData ?
tmpData = tmp;
}
void Packet::validate() {
setData(tmpData);
}
void Packet::setData(const uint16_t& d) {
boost::mutex::scoped_lock lock(dataMutex);
data = d;
}
uint16_t Packet::getData() const {
boost::mutex::scoped_lock lock(dataMutex);
return data;
}
} // namespace create

255
src/serial.cpp Normal file
View file

@ -0,0 +1,255 @@
#include <iostream>
#include "create/serial.h"
#include "create/types.h"
namespace create {
Serial::Serial(boost::shared_ptr<Data> d, const uint8_t& header) :
data(d),
headerByte(header),
port(io),
readState(READ_HEADER),
isReading(false),
dataReady(false),
corruptPackets(0),
totalPackets(0) {
//std::cout << "# Serial Created" << std::endl;
}
Serial::~Serial() {
disconnect();
//std::cout << "# Serial Destroyed" << std::endl;
}
bool Serial::connect(const std::string& portName, const int& baud, boost::function<void()> cb) {
//std::cout << "## Serial connect start" << std::endl;
using namespace boost::asio;
port.open(portName);
port.set_option(serial_port::baud_rate(baud));
port.set_option(serial_port::flow_control(serial_port::flow_control::none));
if (port.is_open()) {
callback = cb;
bool startReadSuccess = startReading();
if (!startReadSuccess)
port.close();
//std::cout << "## Serial connect done" << std::endl;
return startReadSuccess;
}
//std::cout << "## Serial connect failed" << std::endl;
return false;
}
void Serial::disconnect() {
if (isReading) {
stopReading();
}
if (connected()) {
//std::cout << "## Serial disconnect start" << std::endl;
// Ensure not in Safe/Full modes
sendOpcode(OC_START);
// Stop OI
sendOpcode(OC_STOP);
port.close();
//std::cout << "## Serial disconnect done" << std::endl;
}
}
bool Serial::startReading() {
if (!connected()) return false;
if (!data) {
CERR("[create::Serial] ", "data pointer not initialized.");
return false;
}
// Only allow once
if (isReading) return true;
//std::cout << "### Serial start reading" << std::endl;
// Request from Create that we want a stream containing all packets
uint8_t numPackets = data->getNumPackets();
std::vector<uint8_t> packetIDs = data->getPacketIDs();
uint8_t streamReq[2 + numPackets];
streamReq[0] = OC_STREAM;
streamReq[1] = numPackets;
int i = 2;
for (std::vector<uint8_t>::iterator it = packetIDs.begin(); it != packetIDs.end(); ++it) {
streamReq[i] = *it;
i++;
}
// Start OI
sendOpcode(OC_START);
// Start streaming data
send(streamReq, 2 + numPackets);
expectedNumBytes = data->getTotalDataBytes() + numPackets;
//TODO: handle boost exceptions
io.reset();
// Start continuously reading one byte at a time
boost::asio::async_read(port,
boost::asio::buffer(&byteRead, 1),
boost::bind(&Serial::onData, this, _1, _2));
ioThread = boost::thread(boost::bind(&boost::asio::io_service::run, &io));
// Wait for first complete read to finish
boost::unique_lock<boost::mutex> lock(dataReadyMut);
//std::cout << "#### Waiting for dataReady" << std::endl;
int attempts = 1;
int maxAttempts = 10;
while (!dataReady) {
if (!dataReadyCond.timed_wait(lock, boost::get_system_time() + boost::posix_time::milliseconds(500))) {
if (attempts >= maxAttempts) {
CERR("[create::Serial] ", "failed to receive data from Create. Check if robot is powered!");
io.stop();
ioThread.join();
return false;
}
attempts++;
//std::cout << "Requesting data from Create. Attempt " << attempts << std::endl;
// Request data again
sendOpcode(OC_START);
send(streamReq, 2 + numPackets);
}
}
//std::cout << "#### Data is ready." << std::endl;
isReading = true;
//std::cout << "### Serial start reading DONE" << std::endl;
return true;
}
void Serial::stopReading() {
if (isReading) {
//std::cout << "### Start stopReading" << std::endl;
io.stop();
ioThread.join();
isReading = false;
{
boost::lock_guard<boost::mutex> lock(dataReadyMut);
dataReady = false;
}
//std::cout << "### End stopReading" << std::endl;
}
}
void Serial::onData(const boost::system::error_code& e, const std::size_t& size) {
//std::cout << "#### onData" << std::endl;
if (e) {
CERR("[create::Serial] ", "serial error - " << e.message());
return;
}
// Should have read exactly one byte
if (size == 1) {
numBytesRead++;
byteSum += byteRead;
switch (readState) {
case READ_HEADER:
if (byteRead == headerByte) {
readState = READ_NBYTES;
byteSum = byteRead;
}
break;
case READ_NBYTES:
if (byteRead == expectedNumBytes) {
readState = READ_PACKET_ID;
numBytesRead = 0;
}
else
readState = READ_HEADER;
break;
case READ_PACKET_ID:
packetID = byteRead;
if (data->isValidPacketID(packetID)) {
expectedNumDataBytes = data->getPacket(packetID)->nbytes;
assert(expectedNumDataBytes == 1 || expectedNumDataBytes == 2);
numDataBytesRead = 0;
packetBytes = 0;
readState = READ_PACKET_BYTES;
}
else {
readState = READ_HEADER;
}
break;
case READ_PACKET_BYTES:
numDataBytesRead++;
if (expectedNumDataBytes == 2 && numDataBytesRead == 1) {
// high byte first
packetBytes = (byteRead << 8) & 0xff00;
}
else {
// low byte
packetBytes += byteRead;
}
if (numDataBytesRead >= expectedNumDataBytes) {
data->getPacket(packetID)->setTempData(packetBytes);
if (numBytesRead >= expectedNumBytes)
readState = READ_CHECKSUM;
else
readState = READ_PACKET_ID;
}
break;
case READ_CHECKSUM:
if ((byteSum & 0xFF) == 0) {
// Validate all packets
data->validateAll();
// Notify first data packets ready
{
boost::lock_guard<boost::mutex> lock(dataReadyMut);
// std::cout << "locking." << std::endl;
if (!dataReady) {
dataReady = true;
dataReadyCond.notify_one();
//std::cout << "##### Notified." << std::endl;
}
}
// Callback to notify data is ready
if (callback)
callback();
}
else {
// Corrupt data
corruptPackets++;
}
totalPackets++;
// Start again
readState = READ_HEADER;
break;
} // end switch (readState)
} // end if (size == 1)
// Read the next byte
boost::asio::async_read(port,
boost::asio::buffer(&byteRead, 1),
boost::bind(&Serial::onData, this, _1, _2));
}
bool Serial::send(const uint8_t* bytes, unsigned int numBytes) {
if (!connected()) {
CERR("[create::Serial] ", "send failed, not connected.");
return false;
}
// TODO: catch boost exceptions
boost::asio::write(port, boost::asio::buffer(bytes, numBytes));
return true;
}
bool Serial::sendOpcode(const Opcode& code) {
uint8_t oc = (uint8_t) code;
return send(&oc, 1);
}
} // namespace create