diff --git a/include/create/create.h b/include/create/create.h index 619205f..512d264 100644 --- a/include/create/create.h +++ b/include/create/create.h @@ -406,6 +406,26 @@ namespace create { */ bool isCliffFrontRight() const; + /** + * \return the IR value of the left cliff sensor. + */ + uint16_t getCliffSignalLeft() const; + + /** + * \return the IR value of the front left cliff sensor. + */ + uint16_t getCliffSignalFrontLeft() const; + + /** + * \return the IR value of the right cliff sensor. + */ + uint16_t getCliffSignalRight() const; + + /** + * \return the IR value of the front right cliff sensor. + */ + uint16_t getCliffSignalFrontRight() const; + /** * \return true if there is a virtual wall signal is being received. */ diff --git a/src/create.cpp b/src/create.cpp index 0517b4a..87c12c9 100644 --- a/src/create.cpp +++ b/src/create.cpp @@ -156,17 +156,23 @@ namespace create { // Compute ticks since last update int ticksLeft = totalTicksLeft - prevTicksLeft; int ticksRight = totalTicksRight - prevTicksRight; + + // Handle wrap around + if (ticksLeft > 0.9 * util::V_3_MAX_ENCODER_TICKS) { + ticksLeft -= util::V_3_MAX_ENCODER_TICKS; + } else if (ticksLeft < -0.9 * util::V_3_MAX_ENCODER_TICKS) { + ticksLeft += util::V_3_MAX_ENCODER_TICKS; + } + + if (ticksRight > 0.9 * util::V_3_MAX_ENCODER_TICKS) { + ticksRight -= util::V_3_MAX_ENCODER_TICKS; + } else if (ticksRight < -0.9 * util::V_3_MAX_ENCODER_TICKS) { + ticksRight += util::V_3_MAX_ENCODER_TICKS; + } + prevTicksLeft = totalTicksLeft; prevTicksRight = totalTicksRight; - // Handle wrap around - if (std::abs(ticksLeft) > 0.9 * util::V_3_MAX_ENCODER_TICKS) { - ticksLeft = (ticksLeft % util::V_3_MAX_ENCODER_TICKS) + 1; - } - if (std::abs(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::V_3_TICKS_PER_REV) * model.getWheelDiameter() * util::PI; @@ -738,6 +744,46 @@ namespace create { } } + uint16_t Create::getCliffSignalLeft() const { + if (data->isValidPacketID(ID_CLIFF_LEFT)) { + return GET_DATA(ID_CLIFF_LEFT_SIGNAL); + } + else { + CERR("[create::Create] ", "Left cliff sensor signals not supported!"); + return false; + } + } + + uint16_t Create::getCliffSignalFrontLeft() const { + if (data->isValidPacketID(ID_CLIFF_FRONT_LEFT)) { + return GET_DATA(ID_CLIFF_FRONT_LEFT_SIGNAL); + } + else { + CERR("[create::Create] ", "Front left cliff sensor signals not supported!"); + return false; + } + } + + uint16_t Create::getCliffSignalRight() const { + if (data->isValidPacketID(ID_CLIFF_RIGHT)) { + return GET_DATA(ID_CLIFF_RIGHT_SIGNAL); + } + else { + CERR("[create::Create] ", "Rightt cliff sensor signals not supported!"); + return false; + } + } + + uint16_t Create::getCliffSignalFrontRight() const { + if (data->isValidPacketID(ID_CLIFF_FRONT_RIGHT)) { + return GET_DATA(ID_CLIFF_FRONT_RIGHT_SIGNAL); + } + else { + CERR("[create::Create] ", "Front right cliff sensor signals not supported!"); + return false; + } + } + bool Create::isVirtualWall() const { if (data->isValidPacketID(ID_VIRTUAL_WALL)) { return GET_DATA(ID_VIRTUAL_WALL); diff --git a/src/data.cpp b/src/data.cpp index 5e8acce..be20717 100644 --- a/src/data.cpp +++ b/src/data.cpp @@ -16,6 +16,10 @@ namespace create { 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_CLIFF_LEFT_SIGNAL, 2, "cliff_left_signal", V_3); + ADD_PACKET(ID_CLIFF_FRONT_LEFT_SIGNAL, 2, "cliff_front_left_signal", V_3); + ADD_PACKET(ID_CLIFF_FRONT_RIGHT_SIGNAL, 2, "cliff_front_right_signal", V_3); + ADD_PACKET(ID_CLIFF_RIGHT_SIGNAL, 2, "cliff_right_signal", V_3); ADD_PACKET(ID_VIRTUAL_WALL, 1, "virtual_wall", V_ALL); ADD_PACKET(ID_OVERCURRENTS, 1, "overcurrents", V_ALL); ADD_PACKET(ID_DIRT_DETECT_LEFT, 1, "dirt_detect_left", V_ALL);