jakob.kampichler/libcreate
jakob.kampichler/libcreate
1
0
Fork 0
forked from compAIR/libcreate
Code Pull requests Activity

Code cleanup

Browse source
This commit is contained in:
jacobperron 2016-02-12 17:44:30 -08:00
parent 8a3209d6a1
commit bcd27b788a
13 changed files with 142 additions and 146 deletions
Download patch file Download diff file Expand all files Collapse all files

6
CMakeLists.txt
View file

@ -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)

6
LICENSE
View file

@ -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

20
examples/create_demo.cpp
View file

@ -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;

13
include/create/create.h
View file

@ -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.

6
include/create/data.h
View file

@ -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

13
include/create/packet.h
View file

@ -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;
};

11
include/create/serial.h
View file

@ -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<void()> callback;

6
include/create/types.h
View file

@ -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

11
include/create/util.h
View file

@ -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);

132
src/create.cpp
View file

@ -1,7 +1,7 @@
#include <boost/bind.hpp>
#include <boost/make_shared.hpp>
#include <iostream>
#include <cmath>
#include <cmath>
#include <ctime>
#include <assert.h>
@ -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;
}

22
src/data.cpp
View file

@ -3,10 +3,10 @@
#define ADD_PACKET(id,nbytes,info) (packets[id]=boost::make_shared<Packet>(nbytes,info))
namespace create {
Data::Data() {
// Populate data map
ADD_PACKET(ID_BUMP_WHEELDROP, 1, "bumps_wheeldrops");
ADD_PACKET(ID_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<uint8_t, boost::shared_ptr<Packet> >::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<Packet> Data::getPacket(uint8_t id) {
if (isValidPacketID(id)) {
return packets[id];
}
return boost::shared_ptr<Packet>(); //NULL;
return boost::shared_ptr<Packet>();
}
void Data::validateAll() {
for (std::map<uint8_t, boost::shared_ptr<Packet> >::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<uint8_t> Data::getPacketIDs() {
return ids;
}

15
src/packet.cpp
View file

@ -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;

27
src/serial.cpp
View file

@ -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<void()> 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<uint8_t> packetIDs = data->getPacketIDs();
@ -102,7 +95,7 @@ namespace create {
// Wait for first complete read to finish
boost::unique_lock<boost::mutex> lock(dataReadyMut);
//std::cout << "#### Waiting for dataReady" << std::endl;
int attempts = 1;
int maxAttempts = 10;
while (!dataReady) {
@ -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<boost::mutex> lock(dataReadyMut);
dataReady = false;
}
//std::cout << "### End stopReading" << std::endl;
}
}
void Serial::onData(const boost::system::error_code& e, const std::size_t& size) {
//std::cout << "#### onData" << std::endl;
if (e) {
CERR("[create::Serial] ", "serial error - " << e.message());
return;
@ -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<boost::mutex> lock(dataReadyMut);
// std::cout << "locking." << std::endl;
if (!dataReady) {
dataReady = true;
dataReadyCond.notify_one();
//std::cout << "##### Notified." << std::endl;
}
}
// Callback to notify data is ready