From 43c7b95361e2abf4d6ed3ddb35667a70ed082d07 Mon Sep 17 00:00:00 2001 From: jacobperron Date: Fri, 11 Dec 2015 14:08:34 -0800 Subject: [PATCH] Initial commit. --- CMakeLists.txt | 42 ++++ LICENSE | 25 +++ README.md | 24 ++ examples/create_demo.cpp | 125 +++++++++++ include/create/create.h | 353 +++++++++++++++++++++++++++++ include/create/data.h | 63 ++++++ include/create/packet.h | 61 +++++ include/create/serial.h | 106 +++++++++ include/create/types.h | 190 ++++++++++++++++ include/create/util.h | 61 +++++ src/create.cpp | 475 +++++++++++++++++++++++++++++++++++++++ src/data.cpp | 107 +++++++++ src/packet.cpp | 32 +++ src/serial.cpp | 255 +++++++++++++++++++++ 14 files changed, 1919 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 LICENSE create mode 100644 README.md create mode 100644 examples/create_demo.cpp create mode 100644 include/create/create.h create mode 100644 include/create/data.h create mode 100644 include/create/packet.h create mode 100644 include/create/serial.h create mode 100644 include/create/types.h create mode 100644 include/create/util.h create mode 100644 src/create.cpp create mode 100644 src/data.cpp create mode 100644 src/packet.cpp create mode 100644 src/serial.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..c4fccce --- /dev/null +++ b/CMakeLists.txt @@ -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) diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..b0ce620 --- /dev/null +++ b/LICENSE @@ -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. diff --git a/README.md b/README.md new file mode 100644 index 0000000..43af84c --- /dev/null +++ b/README.md @@ -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` \ No newline at end of file diff --git a/examples/create_demo.cpp b/examples/create_demo.cpp new file mode 100644 index 0000000..34d1f9c --- /dev/null +++ b/examples/create_demo.cpp @@ -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; +} diff --git a/include/create/create.h b/include/create/create.h new file mode 100644 index 0000000..34b5643 --- /dev/null +++ b/include/create/create.h @@ -0,0 +1,353 @@ +/** +Software License Agreement (BSD) + +\file create.h +\authors Jacob Perron +\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 +#include +#include +#include + +#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 data; + boost::shared_ptr 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 diff --git a/include/create/data.h b/include/create/data.h new file mode 100644 index 0000000..3add78a --- /dev/null +++ b/include/create/data.h @@ -0,0 +1,63 @@ +/** +Software License Agreement (BSD) + +\file data.h +\authors Jacob Perron +\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 +#include +#include +#include + +#include "create/packet.h" +#include "create/types.h" + +namespace create { + class Data { + private: + std::map > packets; + uint32_t totalDataBytes; + std::vector ids; + + public: + Data(); + ~Data(); + + bool isValidPacketID(const uint8_t id) const; + boost::shared_ptr getPacket(const uint8_t id); + void validateAll(); + uint32_t getTotalDataBytes() const; + uint8_t getNumPackets() const; + std::vector getPacketIDs(); + }; +} // namespace create + +#endif // CREATE_DATA_H diff --git a/include/create/packet.h b/include/create/packet.h new file mode 100644 index 0000000..217aedb --- /dev/null +++ b/include/create/packet.h @@ -0,0 +1,61 @@ +/** +Software License Agreement (BSD) + +\file packet.h +\authors Jacob Perron +\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 + +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 diff --git a/include/create/serial.h b/include/create/serial.h new file mode 100644 index 0000000..d7b1f11 --- /dev/null +++ b/include/create/serial.h @@ -0,0 +1,106 @@ +/** +Software License Agreement (BSD) + +\file serial.h +\authors Jacob Perron +\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 +#include +#include +#include +#include + +#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; + 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 callback; + // Start and stop reading data from Create + bool startReading(); + void stopReading(); + + public: + Serial(boost::shared_ptr data, const uint8_t& header = create::util::CREATE_2_HEADER); + ~Serial(); + bool connect(const std::string& port, const int& baud = 115200, boost::function 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 diff --git a/include/create/types.h b/include/create/types.h new file mode 100644 index 0000000..6d8e3e0 --- /dev/null +++ b/include/create/types.h @@ -0,0 +1,190 @@ +/** +Software License Agreement (BSD) + +\file types.h +\authors Jacob Perron +\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 diff --git a/include/create/util.h b/include/create/util.h new file mode 100644 index 0000000..8817719 --- /dev/null +++ b/include/create/util.h @@ -0,0 +1,61 @@ +/** +Software License Agreement (BSD) + +\file util.h +\authors Jacob Perron +\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< PI) a -= TWO_PI; + return a; + }; + + } // namespace util +} // namespace create + +#endif // CREATE_UTIL_H diff --git a/src/create.cpp b/src/create.cpp new file mode 100644 index 0000000..8c2343b --- /dev/null +++ b/src/create.cpp @@ -0,0 +1,475 @@ +#include +#include +#include +#include +#include +#include + +#include "create/create.h" + +#define GET_DATA(id) (data->getPacket(id)->getData()) +#define BOUND(val,min,max) (valmax?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(new Data()); + serial = boost::make_shared(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 diff --git a/src/data.cpp b/src/data.cpp new file mode 100644 index 0000000..8f8a1bf --- /dev/null +++ b/src/data.cpp @@ -0,0 +1,107 @@ +#include "create/data.h" + +#define ADD_PACKET(id,nbytes,info) (packets[id]=boost::make_shared(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 >::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 Data::getPacket(uint8_t id) { + if (isValidPacketID(id)) { + return packets[id]; + } + return boost::shared_ptr(); //NULL; + } + + void Data::validateAll() { + for (std::map >::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 Data::getPacketIDs() { + return ids; + } + +} // namespace create diff --git a/src/packet.cpp b/src/packet.cpp new file mode 100644 index 0000000..e37a823 --- /dev/null +++ b/src/packet.cpp @@ -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 diff --git a/src/serial.cpp b/src/serial.cpp new file mode 100644 index 0000000..6f7a529 --- /dev/null +++ b/src/serial.cpp @@ -0,0 +1,255 @@ +#include + +#include "create/serial.h" +#include "create/types.h" + +namespace create { + + Serial::Serial(boost::shared_ptr 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 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 packetIDs = data->getPacketIDs(); + uint8_t streamReq[2 + numPackets]; + streamReq[0] = OC_STREAM; + streamReq[1] = numPackets; + int i = 2; + for (std::vector::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 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 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 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