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

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

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