diff --git a/CMakeLists.txt b/CMakeLists.txt index 727852b..ea5e467 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,8 +13,11 @@ include_directories( add_library(create src/create.cpp src/serial.cpp + src/serial_stream.cpp + src/serial_query.cpp src/data.cpp src/packet.cpp + src/types.cpp ) if(THREADS_HAVE_PTHREAD_ARG) diff --git a/README.md b/README.md index 6f31b4c..22cc852 100644 --- a/README.md +++ b/README.md @@ -1,9 +1,13 @@ # libcreate -C++ library for interfacing with iRobot's [Create 1 and 2](http://www.irobot.com/About-iRobot/STEM/Create-2.aspx). This library forms the basis of the ROS driver in [create_autonomy](https://github.com/autonomylab/create_autonomy). +C++ library for interfacing with iRobot's [Create 1 and 2](http://www.irobot.com/About-iRobot/STEM/Create-2.aspx) as well as most models of Roombas. This library forms the basis of the ROS driver in [create_autonomy](https://github.com/autonomylab/create_autonomy). * Documentation: TODO * Code API: TODO +* Protocol documentation: + - [`V_1`](http://www.ecsl.cs.sunysb.edu/mint/Roomba_SCI_Spec_Manual.pdf) (Roomba 400 series ) + - [`V_2`](http://www.irobot.com/filelibrary/pdfs/hrd/create/Create%20Open%20Interface_v2.pdf) (Create 1, Roomba 500 series) + - [`V_3`](https://cdn-shop.adafruit.com/datasheets/create_2_Open_Interface_Spec.pdf) (Create 2, Roomba 600-800 series) * Author: [Jacob Perron](http://jacobperron.ca) ([Autonomy Lab](http://autonomylab.org), [Simon Fraser University](http://www.sfu.ca)) * Contributors: [Mani Monajjemi](http:mani.im), [Ben Wolsieffer](https://github.com/lopsided98) diff --git a/examples/bumper_example.cpp b/examples/bumper_example.cpp index e6543ee..11e0ab8 100644 --- a/examples/bumper_example.cpp +++ b/examples/bumper_example.cpp @@ -3,7 +3,7 @@ int main(int argc, char** argv) { std::string port = "/dev/ttyUSB0"; int baud = 115200; - create::RobotModel model = create::CREATE_2; + create::RobotModel model = create::RobotModel::CREATE_2; create::Create* robot = new create::Create(model); diff --git a/examples/create_demo.cpp b/examples/create_demo.cpp index 668014b..243eaa6 100644 --- a/examples/create_demo.cpp +++ b/examples/create_demo.cpp @@ -5,10 +5,10 @@ create::Create* robot; int main(int argc, char** argv) { std::string port = "/dev/ttyUSB0"; int baud = 115200; - create::RobotModel model = create::CREATE_2; + create::RobotModel model = create::RobotModel::CREATE_2; if (argc > 1 && std::string(argv[1]) == "create1") { - model = create::CREATE_1; + model = create::RobotModel::CREATE_1; baud = 57600; std::cout << "1st generation Create selected" << std::endl; } diff --git a/examples/odom_example.cpp b/examples/odom_example.cpp index c45c5c7..4499f9f 100644 --- a/examples/odom_example.cpp +++ b/examples/odom_example.cpp @@ -5,10 +5,10 @@ create::Create* robot; int main(int argc, char** argv) { std::string port = "/dev/ttyUSB0"; int baud = 115200; - create::RobotModel model = create::CREATE_2; + create::RobotModel model = create::RobotModel::CREATE_2; if (argc > 1 && std::string(argv[1]) == "create1") { - model = create::CREATE_1; + model = create::RobotModel::CREATE_1; baud = 57600; std::cout << "1st generation Create selected" << std::endl; } diff --git a/include/create/create.h b/include/create/create.h index 3d37e97..bc2f39b 100644 --- a/include/create/create.h +++ b/include/create/create.h @@ -37,7 +37,8 @@ POSSIBILITY OF SUCH DAMAGE. #include #include -#include "create/serial.h" +#include "create/serial_stream.h" +#include "create/serial_query.h" #include "create/data.h" #include "create/types.h" #include "create/util.h" @@ -68,6 +69,8 @@ namespace create { uint8_t powerLED; uint8_t powerLEDIntensity; + CreateMode mode; + create::Pose pose; create::Vel vel; @@ -80,6 +83,9 @@ namespace create { Matrix poseCovar; + float requestedLeftVel; + float requestedRightVel; + void init(); // Add two matrices and handle overflow case Matrix addMatrices(const Matrix &A, const Matrix &B) const; @@ -94,7 +100,7 @@ namespace create { /* Default constructor. * Does not attempt to establish serial connection to Create. */ - Create(RobotModel = CREATE_2); + Create(RobotModel = RobotModel::CREATE_2); /* Attempts to establish serial connection to Create. * \param port of your computer that is connected to Create. @@ -102,7 +108,7 @@ namespace create { * 115200 for Create 2 and 57600 for Create 1. * \param model type of robot. */ - Create(const std::string& port, const int& baud, RobotModel model = CREATE_2); + Create(const std::string& port, const int& baud, RobotModel model = RobotModel::CREATE_2); ~Create(); @@ -152,21 +158,21 @@ namespace create { * turn in place clockwise = -CREATE_2_IN_PLACE_RADIUS * \return true if successful, false otherwise */ - bool driveRadius(const float& velocity, const float& radius) const; + bool driveRadius(const float& velocity, const float& radius); /* Set the velocities for the left and right wheels. * \param leftWheel velocity in m/s. * \param rightWheel veloctiy in m/s. * \return true if successful, false otherwise */ - bool driveWheels(const float& leftWheel, const float& rightWheel) const; + bool driveWheels(const float& leftWheel, const float& rightWheel); /* Set the forward and angular velocity of Create. * \param xVel in m/s * \param angularVel in rads/s * \return true if successful, false otherwise */ - bool drive(const float& xVel, const float& angularVel) const; + bool drive(const float& xVel, const float& angularVel); /* Set the power to the side brush motor. * \param power is in the range [-1, 1] @@ -453,10 +459,12 @@ namespace create { float getRightWheelDistance() const; /* Get the requested velocity (in meters/sec) of the left wheel. + * This value is bounded at the maximum velocity of the robot model. */ float getRequestedLeftWheelVel() const; /* Get the requested velocity (in meters/sec) of the right wheel. + * This value is bounded at the maximum velocity of the robot model. */ float getRequestedRightWheelVel() const; @@ -466,7 +474,7 @@ namespace create { /* Get the current mode reported by Create. */ - create::CreateMode getMode() const; + create::CreateMode getMode(); /* Get the estimated position of Create based on wheel encoders. */ diff --git a/include/create/data.h b/include/create/data.h index 02728f8..345bd1a 100644 --- a/include/create/data.h +++ b/include/create/data.h @@ -48,7 +48,7 @@ namespace create { std::vector ids; public: - Data(RobotModel = CREATE_2); + Data(ProtocolVersion version = V_3); ~Data(); bool isValidPacketID(const uint8_t id) const; diff --git a/include/create/serial.h b/include/create/serial.h index 2f641e7..9dd9bc3 100644 --- a/include/create/serial.h +++ b/include/create/serial.h @@ -47,39 +47,20 @@ POSSIBILITY OF SUCH DAMAGE. namespace create { class Serial { - private: - enum ReadState { - READ_HEADER, - READ_NBYTES, - READ_PACKET_ID, - READ_PACKET_BYTES, - READ_CHECKSUM - }; - boost::shared_ptr data; + protected: boost::asio::io_service io; boost::asio::serial_port port; + + private: boost::thread ioThread; boost::condition_variable dataReadyCond; boost::mutex dataReadyMut; - ReadState readState; bool dataReady; bool isReading; bool firstRead; - - // These are for possible diagnostics - 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 void onData(const boost::system::error_code& e, const std::size_t& size); @@ -88,11 +69,21 @@ namespace create { // Start and stop reading data from Create bool startReading(); void stopReading(); + + protected: + boost::shared_ptr data; + // These are for possible diagnostics + uint64_t corruptPackets; + uint64_t totalPackets; + + virtual bool startSensorStream() = 0; + virtual void processByte(uint8_t byteRead) = 0; + // Notifies main thread that data is fresh and makes the user callback void notifyDataReady(); public: - Serial(boost::shared_ptr data, const uint8_t& header = create::util::CREATE_2_HEADER); + Serial(boost::shared_ptr data); ~Serial(); bool connect(const std::string& port, const int& baud = 115200, boost::function cb = 0); void disconnect(); diff --git a/include/create/serial_query.h b/include/create/serial_query.h new file mode 100644 index 0000000..033785a --- /dev/null +++ b/include/create/serial_query.h @@ -0,0 +1,76 @@ +/** +Software License Agreement (BSD) + +\file serial_query.h +\authors Jacob Perron +\authors Ben Wolsieffer +\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_QUERY_H +#define CREATE_SERIAL_QUERY_H + +#include +#include +#include +#include +#include + +#include "create/data.h" +#include "create/types.h" +#include "create/util.h" +#include "create/serial.h" + +namespace create { + class SerialQuery : public Serial { + + private: + boost::asio::deadline_timer streamRecoveryTimer; + uint8_t packetID; + int8_t packetByte; + uint16_t packetData; + const uint8_t maxPacketID; + + bool started; + + void requestSensorData(); + void restartSensorStream(const boost::system::error_code& err); + + void flushInput(); + + protected: + bool startSensorStream(); + void processByte(uint8_t byteRead); + + public: + SerialQuery(boost::shared_ptr data); + }; +} // namespace create + +#endif // CREATE_SERIAL_H diff --git a/include/create/serial_stream.h b/include/create/serial_stream.h new file mode 100644 index 0000000..d59d126 --- /dev/null +++ b/include/create/serial_stream.h @@ -0,0 +1,81 @@ +/** +Software License Agreement (BSD) + +\file serial_stream.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_STREAM_H +#define CREATE_SERIAL_STREAM_H + +#include +#include +#include +#include +#include + +#include "create/data.h" +#include "create/types.h" +#include "create/util.h" +#include "create/serial.h" + +namespace create { + class SerialStream : public Serial { + private: + enum ReadState { + READ_HEADER, + READ_NBYTES, + READ_PACKET_ID, + READ_PACKET_BYTES, + READ_CHECKSUM + }; + + // State machine variables + ReadState readState; + uint8_t headerByte; + uint8_t packetID; + uint8_t expectedNumBytes; + uint16_t packetBytes; + uint8_t numBytesRead; + uint32_t byteSum; + uint8_t numDataBytesRead; + uint8_t expectedNumDataBytes; + + protected: + bool startSensorStream(); + void processByte(uint8_t byteRead); + + public: + SerialStream(boost::shared_ptr data, const uint8_t& header = create::util::STREAM_HEADER); + + }; +} // namespace create + +#endif // CREATE_SERIAL_H diff --git a/include/create/types.h b/include/create/types.h index 1495094..ffbce6c 100644 --- a/include/create/types.h +++ b/include/create/types.h @@ -33,11 +33,46 @@ POSSIBILITY OF SUCH DAMAGE. #define CREATE_TYPES_H #include +#include +#include +#include namespace create { - enum RobotModel { - CREATE_1 = 0, // Roomba 400 series - CREATE_2 // Roomba 600 series + enum ProtocolVersion { + V_1 = 1, + V_2 = 2, + V_3 = 4, + V_ALL = 0xFFFFFFFF + }; + + + + class RobotModel { + public: + bool operator==(RobotModel& other) const; + operator uint32_t() const; + + uint32_t getId() const; + ProtocolVersion getVersion() const; + float getAxleLength() const; + unsigned int getBaud() const; + float getMaxVelocity() const; + float getWheelDiameter() const; + + static RobotModel ROOMBA_400; // Roomba 400 series + static RobotModel CREATE_1; // Roomba 500 series + static RobotModel CREATE_2; // Roomba 600 series + + private: + uint32_t id; + ProtocolVersion version; + float axleLength; + unsigned int baud; + float maxVelocity; + float wheelDiameter; + + RobotModel(const ProtocolVersion version, const float axleLength, const unsigned int baud, const float maxVelocity = 0.5, const float wheelDiameter = 0.078); + static uint32_t nextId; }; enum SensorPacketID { @@ -60,8 +95,8 @@ namespace create { ID_CLIFF_RIGHT = 12, ID_VIRTUAL_WALL = 13, ID_OVERCURRENTS = 14, - ID_DIRT_DETECT = 15, - ID_UNUSED_1 = 16, + ID_DIRT_DETECT_LEFT = 15, + ID_DIRT_DETECT_RIGHT = 16, ID_IR_OMNI = 17, ID_IR_LEFT = 52, ID_IR_RIGHT = 53, @@ -79,8 +114,8 @@ namespace create { 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_CARGO_BAY_DIGITAL_INPUTS = 32, + ID_CARGO_BAY_ANALOG_SIGNAL = 33, ID_CHARGE_SOURCE = 34, ID_OI_MODE = 35, ID_SONG_NUM = 36, @@ -112,6 +147,7 @@ namespace create { OC_RESET = 7, OC_STOP = 173, OC_BAUD = 129, + OC_CONTROL = 130, OC_SAFE = 131, OC_FULL = 132, OC_CLEAN = 135, diff --git a/include/create/util.h b/include/create/util.h index 7304209..bbeaacd 100644 --- a/include/create/util.h +++ b/include/create/util.h @@ -40,17 +40,12 @@ POSSIBILITY OF SUCH DAMAGE. namespace create { namespace util { - static const float CREATE_1_AXLE_LENGTH = 0.26; - - 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 CREATE_2_MAX_RADIUS = 2.0; - static const float CREATE_2_STRAIGHT_RADIUS = 32.768; - static const float CREATE_2_IN_PLACE_RADUIS = 0.001; + static const uint8_t STREAM_HEADER = 19; + static const float V_3_TICKS_PER_REV = 508.8; + static const uint32_t V_3_MAX_ENCODER_TICKS = 65535; + static const float MAX_RADIUS = 2.0; + static const float STRAIGHT_RADIUS = 32.768; + static const float IN_PLACE_RADIUS = 0.001; static const float PI = 3.14159; static const float TWO_PI = 6.28318; static const float EPS = 0.0001; diff --git a/src/create.cpp b/src/create.cpp index c34764a..1c6048f 100644 --- a/src/create.cpp +++ b/src/create.cpp @@ -8,7 +8,8 @@ #include "create/create.h" #define GET_DATA(id) (data->getPacket(id)->getData()) -#define BOUND(val,min,max) (valmax?val=max:val=val)) +#define BOUND_CONST(val,min,max) (valmax?max:val)) +#define BOUND(val,min,max) (val = BOUND_CONST(val,min,max)) namespace create { @@ -31,6 +32,7 @@ namespace create { totalLeftDist = 0.0; totalRightDist = 0.0; firstOnData = true; + mode = MODE_OFF; pose.x = 0; pose.y = 0; pose.yaw = 0; @@ -40,8 +42,14 @@ namespace create { vel.yaw = 0; vel.covariance = std::vector(9, 0.0); poseCovar = Matrix(3, 3, 0.0); - data = boost::shared_ptr(new Data(model)); - serial = boost::make_shared(data); + requestedLeftVel = 0; + requestedRightVel = 0; + data = boost::shared_ptr(new Data(model.getVersion())); + if (model.getVersion() == V_1) { + serial = boost::make_shared(data); + } else { + serial = boost::make_shared(data); + } } Create::Create(RobotModel m) : model(m) { @@ -83,7 +91,7 @@ namespace create { void Create::onData() { if (firstOnData) { - if (model == CREATE_2) { + if (model.getVersion() >= V_3) { // Initialize tick counts prevTicksLeft = GET_DATA(ID_LEFT_ENC); prevTicksRight = GET_DATA(ID_RIGHT_ENC); @@ -95,22 +103,45 @@ namespace create { // Get current time util::timestamp_t curTime = util::getTimestamp(); float dt = (curTime - prevOnDataTime) / 1000000.0; - float deltaDist, deltaX, deltaY, deltaYaw, leftWheelDist, rightWheelDist; - if (model == CREATE_1) { - /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * - * Angle returned is NOT correct if your robot is using older firmware: * - * http://answers.ros.org/question/31935/createroomba-odometry/ * - * TODO: Consider using velocity command as substitute for pose estimation. * - * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ - deltaDist = ((int16_t) GET_DATA(ID_DISTANCE)) / 1000.0; //mm -> m - deltaYaw = ((int16_t) GET_DATA(ID_ANGLE)) * (util::PI / 180.0); // D2R - deltaX = deltaDist * cos( util::normalizeAngle(pose.yaw + deltaYaw) ); - deltaY = deltaDist * sin( util::normalizeAngle(pose.yaw + deltaYaw) ); - const float deltaYawWheelDist = (util::CREATE_1_AXLE_LENGTH / 2.0) * deltaYaw; - leftWheelDist = deltaDist - deltaYawWheelDist; - rightWheelDist = deltaDist + deltaYawWheelDist; + float deltaDist, deltaX, deltaY, deltaYaw, leftWheelDist, rightWheelDist, wheelDistDiff; + + // Protocol versions 1 and 2 use distance and angle fields for odometry + int16_t angleField; + if (model.getVersion() <= V_2) { + // This is a standards compliant way of doing unsigned to signed conversion + uint16_t distanceRaw = GET_DATA(ID_DISTANCE); + int16_t distance; + std::memcpy(&distance, &distanceRaw, sizeof(distance)); + deltaDist = distance / 1000.0; // mm -> m + + // Angle is processed differently in versions 1 and 2 + uint16_t angleRaw = GET_DATA(ID_ANGLE); + std::memcpy(&angleField, &angleRaw, sizeof(angleField)); } - else if (model == CREATE_2) { + + if (model.getVersion() == V_1) { + wheelDistDiff = 2.0 * angleField / 1000.0; + leftWheelDist = deltaDist - (wheelDistDiff / 2.0); + rightWheelDist = deltaDist + (wheelDistDiff / 2.0); + deltaYaw = wheelDistDiff / model.getAxleLength(); + } else if (model.getVersion() == V_2) { + /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * Certain older Creates have major problems with odometry * + * http://answers.ros.org/question/31935/createroomba-odometry/ * + * * + * All Creates have an issue with rounding of the angle field, which causes * + * major errors to accumulate in the odometry yaw. * + * http://wiki.tekkotsu.org/index.php/Create_Odometry_Bug * + * https://github.com/AutonomyLab/create_autonomy/issues/28 * + * * + * TODO: Consider using velocity command as substitute for pose estimation * + * to mitigate both of these problems. * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ + deltaYaw = angleField * (util::PI / 180.0); // D2R + wheelDistDiff = model.getAxleLength() * deltaYaw; + leftWheelDist = deltaDist - (wheelDistDiff / 2.0); + rightWheelDist = deltaDist + (wheelDistDiff / 2.0); + } else if (model.getVersion() >= V_3) { // Get cumulative ticks (wraps around at 65535) uint16_t totalTicksLeft = GET_DATA(ID_LEFT_ENC); uint16_t totalTicksRight = GET_DATA(ID_RIGHT_ENC); @@ -121,35 +152,33 @@ namespace create { 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(ticksLeft) > 0.9 * util::V_3_MAX_ENCODER_TICKS) { + ticksLeft = (ticksLeft % util::V_3_MAX_ENCODER_TICKS) + 1; } - if (fabs(ticksRight) > 0.9 * util::CREATE_2_MAX_ENCODER_TICKS) { - ticksRight = (ticksRight % util::CREATE_2_MAX_ENCODER_TICKS) + 1; + if (fabs(ticksRight) > 0.9 * util::V_3_MAX_ENCODER_TICKS) { + ticksRight = (ticksRight % util::V_3_MAX_ENCODER_TICKS) + 1; } // Compute distance travelled by each wheel - leftWheelDist = (ticksLeft / util::CREATE_2_TICKS_PER_REV) - * util::CREATE_2_WHEEL_DIAMETER * util::PI; - rightWheelDist = (ticksRight / util::CREATE_2_TICKS_PER_REV) - * util::CREATE_2_WHEEL_DIAMETER * util::PI; - - float wheelDistDiff = rightWheelDist - leftWheelDist; + leftWheelDist = (ticksLeft / util::V_3_TICKS_PER_REV) + * model.getWheelDiameter() * util::PI; + rightWheelDist = (ticksRight / util::V_3_TICKS_PER_REV) + * model.getWheelDiameter() * util::PI; deltaDist = (rightWheelDist + leftWheelDist) / 2.0; - // Moving straight - if (fabs(wheelDistDiff) < util::EPS) { - deltaX = deltaDist * cos(pose.yaw); - deltaY = deltaDist * sin(pose.yaw); - deltaYaw = 0.0; - } - else { - float turnRadius = (util::CREATE_2_AXLE_LENGTH / 2.0) * (leftWheelDist + rightWheelDist) / wheelDistDiff; - deltaYaw = wheelDistDiff / util::CREATE_2_AXLE_LENGTH; - deltaX = turnRadius * (sin(pose.yaw + deltaYaw) - sin(pose.yaw)); - deltaY = -turnRadius * (cos(pose.yaw + deltaYaw) - cos(pose.yaw)); - } - } // if CREATE_2 + wheelDistDiff = rightWheelDist - leftWheelDist; + deltaYaw = wheelDistDiff / model.getAxleLength(); + } + + // Moving straight + if (fabs(wheelDistDiff) < util::EPS) { + deltaX = deltaDist * cos(pose.yaw); + deltaY = deltaDist * sin(pose.yaw); + } else { + float turnRadius = (model.getAxleLength() / 2.0) * (leftWheelDist + rightWheelDist) / wheelDistDiff; + deltaX = turnRadius * (sin(pose.yaw + deltaYaw) - sin(pose.yaw)); + deltaY = -turnRadius * (cos(pose.yaw + deltaYaw) - cos(pose.yaw)); + } totalLeftDist += leftWheelDist; totalRightDist += rightWheelDist; @@ -158,8 +187,7 @@ namespace create { vel.x = deltaDist / dt; vel.y = 0.0; vel.yaw = deltaYaw / dt; - } - else { + } else { vel.x = 0.0; vel.y = 0.0; vel.yaw = 0.0; @@ -171,7 +199,7 @@ namespace create { float kl = 1.0; float cosYawAndHalfDelta = cos(pose.yaw + (deltaYaw / 2.0)); // deltaX? float sinYawAndHalfDelta = sin(pose.yaw + (deltaYaw / 2.0)); // deltaY? - float distOverTwoWB = deltaDist / (util::CREATE_2_AXLE_LENGTH * 2.0); + float distOverTwoWB = deltaDist / (model.getAxleLength() * 2.0); Matrix invCovar(2, 2); invCovar(0, 0) = kr * fabs(rightWheelDist); @@ -184,8 +212,8 @@ namespace create { Finc(0, 1) = (cosYawAndHalfDelta / 2.0) + (distOverTwoWB * sinYawAndHalfDelta); Finc(1, 0) = (sinYawAndHalfDelta / 2.0) + (distOverTwoWB * cosYawAndHalfDelta); Finc(1, 1) = (sinYawAndHalfDelta / 2.0) - (distOverTwoWB * cosYawAndHalfDelta); - Finc(2, 0) = (1.0 / util::CREATE_2_AXLE_LENGTH); - Finc(2, 1) = (-1.0 / util::CREATE_2_AXLE_LENGTH); + Finc(2, 0) = (1.0 / model.getAxleLength()); + Finc(2, 1) = (-1.0 / model.getAxleLength()); Matrix FincT = boost::numeric::ublas::trans(Finc); Matrix Fp(3, 3); @@ -271,24 +299,39 @@ namespace create { //} bool Create::setMode(const CreateMode& mode) { + if (model.getVersion() == V_1){ + // Switch to safe mode (required for compatibility with V_1) + if (!(serial->sendOpcode(OC_START) && serial->sendOpcode(OC_CONTROL))) return false; + } + bool ret; switch (mode) { case MODE_OFF: - return serial->sendOpcode(OC_POWER); + if (model.getVersion() == V_2) { + CERR("[create::Create] ", "protocol version 2 does not support turning robot off"); + ret = false; + } else { + ret = serial->sendOpcode(OC_POWER); + } break; case MODE_PASSIVE: - return serial->sendOpcode(OC_START); + ret = serial->sendOpcode(OC_START); break; case MODE_SAFE: - return serial->sendOpcode(OC_SAFE); + if (model.getVersion() > V_1) { + ret = serial->sendOpcode(OC_SAFE); + } break; case MODE_FULL: - return serial->sendOpcode(OC_FULL); + ret = serial->sendOpcode(OC_FULL); break; default: CERR("[create::Create] ", "cannot set robot to mode '" << mode << "'"); - break; + ret = false; } - return false; + if (ret) { + this->mode = mode; + } + return ret; } bool Create::clean(const CleanMode& mode) { @@ -309,16 +352,18 @@ namespace create { return serial->send(cmd, 4); } - bool Create::driveRadius(const float& vel, const float& radius) const { + bool Create::driveRadius(const float& vel, const float& radius) { + // Bound velocity + float boundedVel = BOUND_CONST(vel, -model.getMaxVelocity(), model.getMaxVelocity()); + // Expects each parameter as two bytes each and in millimeters - int16_t vel_mm = roundf(vel * 1000); + int16_t vel_mm = roundf(boundedVel * 1000); int16_t radius_mm = roundf(radius * 1000); - BOUND(vel_mm, -util::CREATE_2_MAX_VEL * 1000, util::CREATE_2_MAX_VEL * 1000); // Bound radius if not a special case if (radius_mm != 32768 && radius_mm != 32767 && radius_mm != -1 && radius_mm != 1) { - BOUND(radius_mm, -util::CREATE_2_MAX_RADIUS, util::CREATE_2_MAX_RADIUS); + BOUND(radius_mm, -util::MAX_RADIUS * 1000, util::MAX_RADIUS * 1000); } uint8_t cmd[5] = { OC_DRIVE, @@ -331,25 +376,58 @@ namespace create { return 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); + bool Create::driveWheels(const float& leftVel, const float& rightVel) { + const float boundedLeftVel = BOUND_CONST(leftVel, -model.getMaxVelocity(), model.getMaxVelocity()); + const float boundedRightVel = BOUND_CONST(rightVel, -model.getMaxVelocity(), model.getMaxVelocity()); + requestedLeftVel = boundedLeftVel; + requestedRightVel = boundedRightVel; + if (model.getVersion() > V_1) { + int16_t leftCmd = roundf(boundedLeftVel * 1000); + int16_t rightCmd = roundf(boundedRightVel * 1000); - uint8_t cmd[5] = { OC_DRIVE_DIRECT, - rightCmd >> 8, - rightCmd & 0xff, - leftCmd >> 8, - leftCmd & 0xff - }; - return serial->send(cmd, 5); + uint8_t cmd[5] = { OC_DRIVE_DIRECT, + rightCmd >> 8, + rightCmd & 0xff, + leftCmd >> 8, + leftCmd & 0xff + }; + return serial->send(cmd, 5); + } else { + float radius; + // Prevent divide by zero when driving straight + if (boundedLeftVel != boundedRightVel) { + radius = -((model.getAxleLength() / 2.0) * (boundedLeftVel + boundedRightVel)) / + (boundedLeftVel - boundedRightVel); + } else { + radius = util::STRAIGHT_RADIUS; + } + + float vel; + // Fix signs for spin in place + if (boundedLeftVel == -boundedRightVel || std::abs(roundf(radius * 1000)) <= 1) { + radius = util::IN_PLACE_RADIUS; + vel = boundedRightVel; + } else { + vel = (std::abs(boundedLeftVel) + std::abs(boundedRightVel)) / 2.0 * ((boundedLeftVel + boundedRightVel) > 0 ? 1.0 : -1.0); + } + + // Radius turns have a maximum radius of 2.0 meters + // When the radius is > 2 but <= 10, use a 2 meter radius + // When it is > 10, drive straight + // TODO: alternate between these 2 meter radius and straight to + // fake a larger radius + if (radius > 10.0) { + radius = util::STRAIGHT_RADIUS; + } + + return driveRadius(vel, radius); + } } - bool Create::drive(const float& xVel, const float& angularVel) const { + bool Create::drive(const float& xVel, const float& angularVel) { // 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); + float leftVel = xVel - ((model.getAxleLength() / 2.0) * angularVel); + float rightVel = xVel + ((model.getAxleLength() / 2.0) * angularVel); return driveWheels(leftVel, rightVel); } @@ -552,8 +630,8 @@ namespace create { } uint8_t Create::getDirtDetect() const { - if (data->isValidPacketID(ID_DIRT_DETECT)) { - return GET_DATA(ID_DIRT_DETECT); + if (data->isValidPacketID(ID_DIRT_DETECT_LEFT)) { + return GET_DATA(ID_DIRT_DETECT_LEFT); } else { CERR("[create::Create] ", "Dirt detector not supported!"); @@ -869,47 +947,26 @@ namespace create { } float Create::getLeftWheelDistance() const { - return totalLeftDist; + return totalLeftDist; } float Create::getRightWheelDistance() const { - return totalRightDist; + return totalRightDist; } float Create::getRequestedLeftWheelVel() const { - if (data->isValidPacketID(ID_LEFT_VEL)) { - uint16_t uvel = GET_DATA(ID_LEFT_VEL); - int16_t vel; - std::memcpy(&vel, &uvel, sizeof(vel)); - return (vel / 1000.0); - } - else { - CERR("[create::Create] ", "Left wheel velocity not supported!"); - return 0; - } + return requestedLeftVel; } float Create::getRequestedRightWheelVel() const { - if (data->isValidPacketID(ID_RIGHT_VEL)) { - uint16_t uvel = GET_DATA(ID_RIGHT_VEL); - int16_t vel; - std::memcpy(&vel, &uvel, sizeof(vel)); - return (vel / 1000.0); - } - else { - CERR("[create::Create] ", "Right wheel velocity not supported!"); - return 0; - } + return requestedRightVel; } - create::CreateMode Create::getMode() const { + create::CreateMode Create::getMode() { if (data->isValidPacketID(ID_OI_MODE)) { - return (create::CreateMode) GET_DATA(ID_OI_MODE); - } - else { - CERR("[create::Create] ", "Querying Mode not supported!"); - return create::MODE_UNAVAILABLE; + mode = (create::CreateMode) GET_DATA(ID_OI_MODE); } + return mode; } Pose Create::getPose() const { diff --git a/src/data.cpp b/src/data.cpp index 198bcca..ef226e8 100644 --- a/src/data.cpp +++ b/src/data.cpp @@ -1,72 +1,50 @@ #include "create/data.h" -#define ADD_PACKET(id,nbytes,info) (packets[id]=boost::make_shared(nbytes,info)) +#define ADD_PACKET(id,nbytes,info,enabledVersion) if ((enabledVersion) & version) packets[id]=boost::make_shared(nbytes,info) namespace create { - Data::Data(RobotModel model) { + Data::Data(ProtocolVersion version) { // Populate data map /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * WARNING: Adding too many packets will flood the serial * * buffer and corrupt the stream. * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ - 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_IR_OMNI, 1, "ir_opcode"); - ADD_PACKET(ID_BUTTONS, 1, "buttons"); - 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_VIRTUAL_WALL, 1, "virtual_wall"); - ADD_PACKET(ID_OI_MODE, 1, "oi_mode"); - ADD_PACKET(ID_RIGHT_VEL, 2, "velocity_right"); - ADD_PACKET(ID_LEFT_VEL, 2, "velocity_left"); - - if (model == CREATE_1) { - ADD_PACKET(ID_DISTANCE, 2, "distance"); - ADD_PACKET(ID_ANGLE, 2, "angle"); - } - else if (model == CREATE_2) { - //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_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_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_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"); - } // if CREATE_2 + ADD_PACKET(ID_BUMP_WHEELDROP, 1, "bumps_wheeldrops", V_ALL); + ADD_PACKET(ID_WALL, 1, "wall", V_ALL); + ADD_PACKET(ID_CLIFF_LEFT, 1, "cliff_left", V_ALL); + ADD_PACKET(ID_CLIFF_FRONT_LEFT, 1, "cliff_front_left", V_ALL); + ADD_PACKET(ID_CLIFF_FRONT_RIGHT, 1, "cliff_front_right", V_ALL); + ADD_PACKET(ID_CLIFF_RIGHT, 1, "cliff_right", V_ALL); + ADD_PACKET(ID_VIRTUAL_WALL, 1, "virtual_wall", V_ALL); + ADD_PACKET(ID_OVERCURRENTS, 1, "overcurrents", V_1); + ADD_PACKET(ID_DIRT_DETECT_LEFT, 1, "dirt_detect_left", V_ALL); + ADD_PACKET(ID_DIRT_DETECT_RIGHT, 1, "dirt_detect_right", V_1); + ADD_PACKET(ID_IR_OMNI, 1, "ir_opcode", V_ALL); + ADD_PACKET(ID_BUTTONS, 1, "buttons", V_ALL); + ADD_PACKET(ID_DISTANCE, 2, "distance", V_1 | V_2); + ADD_PACKET(ID_ANGLE, 2, "angle", V_1 | V_2); + ADD_PACKET(ID_CHARGE_STATE, 1, "charging_state", V_ALL); + ADD_PACKET(ID_VOLTAGE, 2, "voltage", V_ALL); + ADD_PACKET(ID_CURRENT, 2, "current", V_ALL); + ADD_PACKET(ID_TEMP, 1, "temperature", V_ALL); + ADD_PACKET(ID_CHARGE , 2, "battery_charge", V_ALL); + ADD_PACKET(ID_CAPACITY, 2, "battery_capacity", V_ALL); + ADD_PACKET(ID_OI_MODE, 1, "oi_mode", V_2 | V_3); + ADD_PACKET(ID_LEFT_ENC, 2, "enc_counts_left", V_3); + ADD_PACKET(ID_RIGHT_ENC, 2, "enc_counts_right", V_3); + ADD_PACKET(ID_LEFT_ENC, 2, "enc_counts_left", V_3); + ADD_PACKET(ID_RIGHT_ENC, 2, "enc_counts_right", V_3); + ADD_PACKET(ID_LIGHT, 1, "light_bumper", V_3); + ADD_PACKET(ID_LIGHT_LEFT, 2, "light_bumper_left", V_3); + ADD_PACKET(ID_LIGHT_FRONT_LEFT, 2, "light_bumper_front_left", V_3); + ADD_PACKET(ID_LIGHT_CENTER_LEFT, 2, "light_bumper_center_left", V_3); + ADD_PACKET(ID_LIGHT_CENTER_RIGHT, 2, "light_bumper_center_right", V_3); + ADD_PACKET(ID_LIGHT_FRONT_RIGHT, 2, "light_bumper_front_right", V_3); + ADD_PACKET(ID_LIGHT_RIGHT, 2, "light_bumper_right", V_3); + ADD_PACKET(ID_IR_LEFT, 1, "ir_opcode_left", V_3); + ADD_PACKET(ID_IR_RIGHT, 1, "ir_opcode_right", V_3); + ADD_PACKET(ID_STASIS, 1, "stasis", V_3); totalDataBytes = 0; for (std::map >::iterator it = packets.begin(); diff --git a/src/serial.cpp b/src/serial.cpp index 62206f5..3894b9e 100644 --- a/src/serial.cpp +++ b/src/serial.cpp @@ -5,11 +5,9 @@ namespace create { - Serial::Serial(boost::shared_ptr d, const uint8_t& header) : + Serial::Serial(boost::shared_ptr d) : data(d), - headerByte(header), port(io), - readState(READ_HEADER), isReading(false), dataReady(false), corruptPackets(0), @@ -64,27 +62,10 @@ namespace create { // Only allow once if (isReading) return true; - // 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 + if (!startSensorStream()) return false; io.reset(); @@ -112,7 +93,7 @@ namespace create { // Request data again sendOpcode(OC_START); - send(streamReq, 2 + numPackets); + startSensorStream(); } } @@ -158,74 +139,7 @@ namespace create { // 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 { - //notifyDataReady(); - 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 { - //notifyDataReady(); - 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) { - notifyDataReady(); - } - else { - // Corrupt data - corruptPackets++; - } - totalPackets++; - // Start again - readState = READ_HEADER; - break; - } // end switch (readState) + processByte(byteRead); } // end if (size == 1) // Read the next byte diff --git a/src/serial_query.cpp b/src/serial_query.cpp new file mode 100644 index 0000000..1cc23d2 --- /dev/null +++ b/src/serial_query.cpp @@ -0,0 +1,70 @@ +#include + +#include "create/serial_query.h" +#include "create/types.h" + +#define SENSORS_RESPONSE_LENGTH 20 + +namespace create { + + SerialQuery::SerialQuery(boost::shared_ptr d) : Serial(d), + streamRecoveryTimer(io), + packetID(ID_BUMP_WHEELDROP), + packetByte(0), + packetData(0), + maxPacketID(ID_CAPACITY) { + } + + bool SerialQuery::startSensorStream() { + if (!started) { + requestSensorData(); + started = true; + } + return true; + } + + void SerialQuery::requestSensorData() { + static const uint8_t requestPacket[2] = { OC_SENSORS, ID_GROUP_0 }; + // Prevents previous packet from corrupting next one + flushInput(); + send(requestPacket, 2); + // Automatically resend request if no response is received + streamRecoveryTimer.expires_from_now(boost::posix_time::milliseconds(50)); + streamRecoveryTimer.async_wait(boost::bind(&SerialQuery::restartSensorStream, this, _1)); + } + + void SerialQuery::restartSensorStream(const boost::system::error_code& err) { + if (err != boost::asio::error::operation_aborted) { + if (packetID != ID_BUMP_WHEELDROP) { + ++corruptPackets; + } + requestSensorData(); + } + } + + void SerialQuery::flushInput() { + // Only works with POSIX support + tcflush(port.lowest_layer().native(), TCIFLUSH); + } + + void SerialQuery::processByte(uint8_t byteRead) { + packetData |= (static_cast(byteRead) << (8 * packetByte)); + + if (packetByte > 0) { + --packetByte; + } else if (packetID < maxPacketID) { + // New packet + data->getPacket(packetID)->setTempData(packetData); + packetData = 0; + ++packetID; + packetByte = data->getPacket(packetID)->nbytes - 1; + } else { + // Response finished + packetID = ID_BUMP_WHEELDROP; + packetByte = 0; + packetData = 0; + notifyDataReady(); + requestSensorData(); + } + } +} // namespace create diff --git a/src/serial_stream.cpp b/src/serial_stream.cpp new file mode 100644 index 0000000..8674718 --- /dev/null +++ b/src/serial_stream.cpp @@ -0,0 +1,103 @@ +#include + +#include "create/serial_stream.h" +#include "create/types.h" + +namespace create { + + SerialStream::SerialStream(boost::shared_ptr d, const uint8_t& header) : Serial(d), headerByte(header){ + } + + bool SerialStream::startSensorStream() { + // 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 streaming data + send(streamReq, 2 + numPackets); + + expectedNumBytes = data->getTotalDataBytes() + numPackets; + + return true; + } + + void SerialStream::processByte(uint8_t byteRead) { + 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 { + //notifyDataReady(); + 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 { + //notifyDataReady(); + 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) { + notifyDataReady(); + } + else { + // Corrupt data + corruptPackets++; + } + totalPackets++; + // Start again + readState = READ_HEADER; + break; + } // end switch (readState) + } + +} // namespace create diff --git a/src/types.cpp b/src/types.cpp new file mode 100644 index 0000000..3e237a7 --- /dev/null +++ b/src/types.cpp @@ -0,0 +1,52 @@ +#include "create/types.h" + +namespace create { + + RobotModel::RobotModel(const ProtocolVersion version, const float axleLength, const unsigned int baud, const float maxVelocity, const float wheelDiameter): + id(nextId), + version(version), + axleLength(axleLength), + baud(baud), + maxVelocity(maxVelocity), + wheelDiameter(wheelDiameter) { + nextId <<= 1; + } + + bool RobotModel::operator ==(RobotModel& other) const { + return id == other.id; + } + + RobotModel::operator uint32_t() const { + return id; + } + + uint32_t RobotModel::getId() const { + return id; + } + + ProtocolVersion RobotModel::getVersion() const { + return version; + } + + float RobotModel::getAxleLength() const { + return axleLength; + } + + unsigned int RobotModel::getBaud() const { + return baud; + } + + float RobotModel::getMaxVelocity() const { + return maxVelocity; + } + + float RobotModel::getWheelDiameter() const { + return wheelDiameter; + } + + uint32_t RobotModel::nextId = 1; + + RobotModel RobotModel::ROOMBA_400(V_1, 0.258, 57600); + RobotModel RobotModel::CREATE_1(V_2, 0.258, 57600); + RobotModel RobotModel::CREATE_2(V_3, 0.235, 115200); +}