diff --git a/CMakeLists.txt b/CMakeLists.txt index c4fccce..def8c81 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,6 +22,7 @@ target_link_libraries(create ## Declare example executables add_executable(create_demo examples/create_demo.cpp) +add_executable(odom_test examples/odom_test.cpp) ## Specify libraries to link a library or executable target against target_link_libraries(create_demo @@ -29,6 +30,11 @@ target_link_libraries(create_demo create ) +target_link_libraries(odom_test + ${Boost_LIBRARIES} + create +) + ## Install ## I'm not familiar with install rules, do these make sense? install(TARGETS create DESTINATION lib) diff --git a/LICENSE b/LICENSE index b0ce620..edf152c 100644 --- a/LICENSE +++ b/LICENSE @@ -14,9 +14,9 @@ modification, are permitted provided that the following conditions are met: 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 +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 diff --git a/examples/create_demo.cpp b/examples/create_demo.cpp index 34d1f9c..9d4cd86 100644 --- a/examples/create_demo.cpp +++ b/examples/create_demo.cpp @@ -20,7 +20,7 @@ int main(int argc, char** argv) { std::cout << "battery level: " << robot->getBatteryCharge() / (float) robot->getBatteryCapacity() * 100.0 << "%" << std::endl; - + bool drive = false; // Make a song @@ -29,8 +29,8 @@ int main(int argc, char** argv) { // 67, 67, 66, 66, 65, 65, 66, 66 }; //float durations[songLength]; //for (int i = 0; i < songLength; i++) { - // durations[i] = 0.25; - //} + // durations[i] = 0.25; + //} //robot->createSong(0, songLength, notes, durations); //usleep(1000000); //robot->playSong(0); @@ -38,9 +38,9 @@ int main(int argc, char** argv) { // Quit when center "Clean" button pressed while (!robot->isCleanButtonPressed()) { // Check for button presses - if (robot->isDayButtonPressed()) + if (robot->isDayButtonPressed()) std::cout << "day button press" << std::endl; - if (robot->isMinButtonPressed()) + if (robot->isMinButtonPressed()) std::cout << "min button press" << std::endl; if (robot->isDockButtonPressed()) { std::cout << "dock button press" << std::endl; @@ -64,7 +64,7 @@ int main(int argc, char** argv) { // If everything is ok, drive forward using IR's to avoid obstacles if (drive) { robot->setPowerLED(0); // green - if (robot->isIRDetectLeft() || + if (robot->isIRDetectLeft() || robot->isIRDetectFrontLeft() || robot->isIRDetectCenterLeft()) { // turn right @@ -97,7 +97,7 @@ int main(int argc, char** argv) { else { robot->enableDebrisLED(false); } - + // Check bumpers if (robot->isLeftBumper()) { std::cout << "left bump detected!" << std::endl; @@ -110,13 +110,13 @@ int main(int argc, char** argv) { } std::cout << "Stopping Create." << std::endl; - - // Turn off lights + + // 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; diff --git a/include/create/create.h b/include/create/create.h index 1f297e8..660e84f 100644 --- a/include/create/create.h +++ b/include/create/create.h @@ -18,9 +18,9 @@ modification, are permitted provided that the following conditions are met: 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 +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 @@ -65,7 +65,7 @@ namespace create { create::Pose pose; create::Vel vel; - + uint32_t prevTicksLeft; uint32_t prevTicksRight; float prevLeftVel; @@ -108,8 +108,10 @@ namespace create { /* Resets as if you have removed the battery. * Changes mode to MODE_PASSIVE. */ + // TODO //void reset(); + // TODO //void setBaud(int baudcode); /* Change Create mode. @@ -145,6 +147,7 @@ namespace create { /* Set the PWM for each wheel. */ + // TODO //void drivePWM(const int16_t& leftWheel, const int16_t& rightWheel) const; /* Set the forward and angular velocity of Create. @@ -206,6 +209,7 @@ namespace create { //void setDigits(uint8_t digit1, uint8_t digit2, // uint8_t digit3, uint8_t digit4); + // TODO // pushButton(...); /* Set the four 7-segment display digits from left to right with ASCII codes. @@ -236,6 +240,7 @@ namespace create { */ bool playSong(const uint8_t& songNumber) const; + // TODO //void registerCallback(...); /* True if a left or right wheeldrop is detected. diff --git a/include/create/data.h b/include/create/data.h index 3add78a..93864f4 100644 --- a/include/create/data.h +++ b/include/create/data.h @@ -18,9 +18,9 @@ modification, are permitted provided that the following conditions are met: 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 +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 diff --git a/include/create/packet.h b/include/create/packet.h index 217aedb..8f2f426 100644 --- a/include/create/packet.h +++ b/include/create/packet.h @@ -18,9 +18,9 @@ modification, are permitted provided that the following conditions are met: 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 +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 @@ -39,20 +39,19 @@ namespace create { uint16_t data; uint16_t tmpData; mutable boost::mutex dataMutex; + mutable boost::mutex tmpDataMutex; 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(); + + // All of the following are thread safe void setTempData(const uint16_t& td); void validate(); - // thread safe void setData(const uint16_t& d); - // thread safe uint16_t getData() const; }; diff --git a/include/create/serial.h b/include/create/serial.h index 4506f0e..d339876 100644 --- a/include/create/serial.h +++ b/include/create/serial.h @@ -18,9 +18,9 @@ modification, are permitted provided that the following conditions are met: 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 +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 @@ -67,8 +67,7 @@ namespace create { bool isReading; bool firstRead; - // These are just for diagnostics, maybe not necessary - // TODO: Investigate + // These are for possible diagnostics uint64_t corruptPackets; uint64_t totalPackets; // State machine variables @@ -83,8 +82,6 @@ namespace create { 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; diff --git a/include/create/types.h b/include/create/types.h index ec893cc..a755214 100644 --- a/include/create/types.h +++ b/include/create/types.h @@ -18,9 +18,9 @@ modification, are permitted provided that the following conditions are met: 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 +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 diff --git a/include/create/util.h b/include/create/util.h index a8bd30a..f20b710 100644 --- a/include/create/util.h +++ b/include/create/util.h @@ -18,9 +18,9 @@ modification, are permitted provided that the following conditions are met: 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 +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 @@ -49,7 +49,7 @@ namespace create { static const float PI = 3.14159; static const float TWO_PI = 6.28318; static const float EPS = 0.0001; - + inline float normalizeAngle(const float& angle) { float a = angle; while (a < -PI) a += TWO_PI; @@ -57,9 +57,10 @@ namespace create { return a; }; + typedef unsigned long long timestamp_t; + /** Get a timestamp for the current time in micro-seconds. */ - typedef unsigned long long timestamp_t; static timestamp_t getTimestamp() { struct timeval now; gettimeofday(&now, NULL); diff --git a/src/create.cpp b/src/create.cpp index 00f1fa2..bb99910 100644 --- a/src/create.cpp +++ b/src/create.cpp @@ -1,7 +1,7 @@ #include #include #include -#include +#include #include #include @@ -45,11 +45,11 @@ namespace create { init(); serial->connect(dev, baud); } - + Create::~Create() { disconnect(); } - + void Create::onData() { if (firstOnData) { // Initialize tick counts @@ -88,7 +88,7 @@ namespace create { float wheelDistDiff = rightWheelDist - leftWheelDist; float deltaDist = (rightWheelDist + leftWheelDist) / 2.0; - + // Moving straight float deltaX, deltaY; if (fabs(wheelDistDiff) < util::EPS) { @@ -98,7 +98,7 @@ namespace create { } else { float turnRadius = (util::CREATE_2_AXLE_LENGTH / 2.0) * (leftWheelDist + rightWheelDist) / wheelDistDiff; - float deltaYaw = (rightWheelDist - leftWheelDist) / util::CREATE_2_AXLE_LENGTH; + float deltaYaw = (rightWheelDist - leftWheelDist) / util::CREATE_2_AXLE_LENGTH; deltaX = turnRadius * (sin(pose.yaw + deltaYaw) - sin(pose.yaw)); deltaY = turnRadius * (cos(pose.yaw + deltaYaw) - cos(pose.yaw)); pose.yaw = util::normalizeAngle(pose.yaw + deltaYaw); @@ -109,7 +109,7 @@ namespace create { vel.yaw = 0; } } - + if (fabs(dt) > util::EPS) { vel.x = deltaX / dt; vel.y = deltaY / dt; @@ -121,12 +121,12 @@ namespace create { pose.x += deltaDist * cos(pose.yaw); pose.y += deltaDist * sin(pose.yaw); - + prevOnDataTime = curTime; // Make user registered callbacks, if any // TODO } - + bool Create::connect(const std::string& port, const int& baud) { bool timeout = false; time_t start, now; @@ -144,33 +144,33 @@ namespace create { 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 || @@ -180,7 +180,7 @@ namespace create { 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); @@ -199,7 +199,7 @@ namespace create { radius_mm >> 8, radius_mm & 0xff }; - + serial->send(cmd, 5); } */ @@ -218,7 +218,7 @@ namespace create { }; 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, @@ -229,45 +229,45 @@ namespace create { 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; + 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, @@ -275,10 +275,10 @@ namespace create { powerLED, powerLEDIntensity }; - + return serial->send(cmd, 4); } - + bool Create::enableDebrisLED(const bool& enable) { if (enable) debrisLED = LED_DEBRIS; @@ -286,7 +286,7 @@ namespace create { debrisLED = 0; return updateLEDs(); } - + bool Create::enableSpotLED(const bool& enable) { if (enable) spotLED = LED_SPOT; @@ -294,7 +294,7 @@ namespace create { spotLED = 0; return updateLEDs(); } - + bool Create::enableDockLED(const bool& enable) { if (enable) dockLED = LED_DOCK; @@ -302,7 +302,7 @@ namespace create { dockLED = 0; return updateLEDs(); } - + bool Create::enableCheckRobotLED(const bool& enable) { if (enable) checkLED = LED_CHECK; @@ -310,22 +310,22 @@ namespace create { 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 || + digit2 < 32 || digit2 > 126 || + digit3 < 32 || digit3 > 126 || digit4 < 32 || digit4 > 126) return false; @@ -335,10 +335,10 @@ namespace create { digit3, digit4 }; - + return serial->send(cmd, 5); } - + bool Create::defineSong(const uint8_t& songNumber, const uint8_t& songLength, const uint8_t* notes, @@ -358,44 +358,44 @@ namespace create { 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 || + 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); } @@ -407,7 +407,7 @@ namespace create { 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); @@ -418,13 +418,13 @@ namespace create { bool Create::isCleanButtonPressed() const { return (GET_DATA(ID_BUTTONS) & 0x01) != 0; } - - // Not working. TODO Fix/report + + // Not working bool Create::isClockButtonPressed() const { return (GET_DATA(ID_BUTTONS) & 0x80) != 0; } - // Not working. TODO Fix/report + // Not working bool Create::isScheduleButtonPressed() const { return (GET_DATA(ID_BUTTONS) & 0x40) != 0; } @@ -448,55 +448,55 @@ namespace create { 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 { + + bool Create::isIRDetectFrontLeft() const { return (GET_DATA(ID_LIGHT) & 0x02) != 0; } - bool Create::isIRDetectCenterLeft() const { + bool Create::isIRDetectCenterLeft() const { return (GET_DATA(ID_LIGHT) & 0x04) != 0; } - bool Create::isIRDetectCenterRight() const { + bool Create::isIRDetectCenterRight() const { return (GET_DATA(ID_LIGHT) & 0x08) != 0; } - bool Create::isIRDetectFrontRight() const { + bool Create::isIRDetectFrontRight() const { return (GET_DATA(ID_LIGHT) & 0x10) != 0; } - bool Create::isIRDetectRight() const { + 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; } diff --git a/src/data.cpp b/src/data.cpp index 8f8a1bf..3cc6b59 100644 --- a/src/data.cpp +++ b/src/data.cpp @@ -3,10 +3,10 @@ #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_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"); @@ -58,7 +58,7 @@ namespace create { 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(); @@ -67,23 +67,23 @@ namespace create { 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; + return boost::shared_ptr(); } - + void Data::validateAll() { for (std::map >::iterator it = packets.begin(); it != packets.end(); @@ -91,15 +91,15 @@ namespace create { it->second->validate(); } } - + unsigned int Data::getTotalDataBytes() const { return totalDataBytes; } - + uint8_t Data::getNumPackets() const { return packets.size(); } - + std::vector Data::getPacketIDs() { return ids; } diff --git a/src/packet.cpp b/src/packet.cpp index e37a823..799d9f7 100644 --- a/src/packet.cpp +++ b/src/packet.cpp @@ -2,28 +2,29 @@ namespace create { - Packet::Packet(const uint8_t& numBytes, const std::string& comment) : + 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 ? + boost::mutex::scoped_lock lock(tmpDataMutex); tmpData = tmp; } - + void Packet::validate() { + boost::mutex::scoped_lock lock(tmpDataMutex); 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; diff --git a/src/serial.cpp b/src/serial.cpp index 61ef48a..ed2ba8d 100644 --- a/src/serial.cpp +++ b/src/serial.cpp @@ -14,16 +14,13 @@ namespace create { 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)); @@ -32,12 +29,11 @@ namespace create { if (port.is_open()) { callback = cb; bool startReadSuccess = startReading(); - if (!startReadSuccess) + if (!startReadSuccess) { port.close(); - //std::cout << "## Serial connect done" << std::endl; + } return startReadSuccess; } - //std::cout << "## Serial connect failed" << std::endl; return false; } @@ -47,13 +43,11 @@ namespace create { } 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; } } @@ -68,7 +62,6 @@ namespace create { // 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(); @@ -102,7 +95,7 @@ namespace create { // 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) { @@ -114,21 +107,19 @@ namespace create { 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; @@ -136,12 +127,10 @@ namespace create { 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; @@ -185,11 +174,11 @@ namespace create { case READ_PACKET_BYTES: numDataBytesRead++; if (expectedNumDataBytes == 2 && numDataBytesRead == 1) { - // high byte first + // High byte first packetBytes = (byteRead << 8) & 0xff00; } else { - // low byte + // Low byte packetBytes += byteRead; } if (numDataBytesRead >= expectedNumDataBytes) { @@ -209,11 +198,9 @@ namespace create { // 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