diff --git a/examples/create_demo.cpp b/examples/create_demo.cpp index 91a7369..668014b 100644 --- a/examples/create_demo.cpp +++ b/examples/create_demo.cpp @@ -72,16 +72,16 @@ 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() || - robot->isIRDetectFrontLeft() || - robot->isIRDetectCenterLeft()) { + if (robot->isLightBumperLeft() || + robot->isLightBumperFrontLeft() || + robot->isLightBumperCenterLeft()) { // turn right robot->drive(0.1, -1.0); robot->setDigitsASCII('-','-','-',']'); } - else if (robot->isIRDetectRight() || - robot->isIRDetectFrontRight() || - robot->isIRDetectCenterRight()) { + else if (robot->isLightBumperRight() || + robot->isLightBumperFrontRight() || + robot->isLightBumperCenterRight()) { // turn left robot->drive(0.1, 1.0); robot->setDigitsASCII('[','-','-','-'); diff --git a/include/create/create.h b/include/create/create.h index d9d00e0..9a4877a 100644 --- a/include/create/create.h +++ b/include/create/create.h @@ -90,8 +90,12 @@ namespace create { Create(RobotModel = CREATE_2); /* Attempts to establish serial connection to Create. + * \param port of your computer that is connected to Create. + * \param baud rate to communicate with Create. Typically, + * 115200 for Create 2 and 57600 for Create 1. + * \param model type of robot. */ - Create(const std::string& port, const int& baud, RobotModel = CREATE_2); + Create(const std::string& port, const int& baud, RobotModel model = CREATE_2); ~Create(); @@ -107,26 +111,21 @@ namespace create { */ void disconnect(); - /* Resets as if you have removed the battery. - * Changes mode to MODE_PASSIVE. - */ - // TODO - //void reset(); - - // TODO - //void setBaud(int baudcode); - /* Change Create mode. + * \param mode to put Create in. + * \return true if successful, false otherwise */ bool setMode(const create::CreateMode& mode); /* Starts a cleaning mode. * Changes mode to MODE_PASSIVE. + * \return true if successful, false otherwise */ bool clean(const create::CleanMode& mode = CLEAN_DEFAULT); /* Starts the docking behaviour. * Changes mode to MODE_PASSIVE. + * \return true if successful, false otherwise */ bool dock() const; @@ -134,42 +133,49 @@ namespace create { * \param day in range [0, 6] * \param hour in range [0, 23] * \param min in range [0, 59] + * \return true if successful, false otherwise */ bool setDate(const create::DayOfWeek& day, const uint8_t& hour, const uint8_t& min) const; /* Set the average wheel velocity and turning radius of Create. - * \param vel is in m/s + * \param velocity is in m/s bounded between [-0.5, 0.5] * \param radius in meters. + * Special cases: drive straight = CREATE_2_STRAIGHT_RADIUS, + * turn in place counter-clockwise = CREATE_2_IN_PLACE_RADIUS, + * turn in place clockwise = -CREATE_2_IN_PLACE_RADIUS + * \return true if successful, false otherwise */ - //void driveRadius(const float& vel, const float& radius) const; + bool driveRadius(const float& velocity, const float& radius) const; - /* Set the velocities for the left and right wheels (m/s). + /* 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; - /* 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. * \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; /* Set the power to the side brush motor. * \param power is in the range [-1, 1] + * \return true if successful, false otherwise */ bool setSideMotor(const float& power); /* Set the power to the main brush motor. * \param power is in the range [-1, 1] + * \return true if successful, false otherwise */ bool setMainMotor(const float& power); /* Set the power to the vacuum motor. * \param power is in the range [0, 1] + * \return true if successful, false otherwise */ bool setVacuumMotor(const float& power); @@ -177,49 +183,65 @@ namespace create { * \param mainPower in the range [-1, 1] * \param sidePower in the range [-1, 1] * \param vacuumPower in the range [0, 1] + * \return true if successful, false otherwise */ bool setAllMotors(const float& mainPower, const float& sidePower, const float& vacuumPower); /* Set the blue "debris" LED on/off. + * \param enable + * \return true if successful, false otherwise */ bool enableDebrisLED(const bool& enable); /* Set the green "spot" LED on/off. + * \param enable + * \return true if successful, false otherwise */ bool enableSpotLED(const bool& enable); /* Set the green "dock" LED on/off. + * \param enable + * \return true if successful, false otherwise */ bool enableDockLED(const bool& enable); /* Set the orange "check Create" LED on/off. + * \param enable + * \return true if successful, false otherwise */ bool enableCheckRobotLED(const bool& enable); /* Set the center power LED. * \param power in range [0, 255] where 0 = green and 255 = red * \param intensity in range [0, 255] + * \return true if successful, false otherwise */ bool setPowerLED(const uint8_t& power, const uint8_t& intensity = 255); - // TODO - //void setScheduleLED(...); - /* Set the four 7-segment display digits from left to right. + * \param segments to enable (true) or disable (false). + * The size of segments should be less than 29. + * The ordering of segments is left to right, top to bottom for each digit: + * + * 0 7 14 21 + * |‾‾‾| |‾‾‾| |‾‾‾| |‾‾‾| + * 1 |___| 2 8 |___| 9 15 |___| 16 22 |___| 23 + * | 3 | | 10| | 17| | 24| + * 4 |___| 5 11|___| 12 18 |___| 19 25 |___| 26 + * 6 13 20 27 + * + * \return true if successful, false otherwise */ - // TODO - //void setDigits(uint8_t digit1, uint8_t digit2, - // uint8_t digit3, uint8_t digit4); - - // TODO - // pushButton(...); + //TODO (https://github.com/AutonomyLab/libcreate/issues/7) + //bool setDigits(const std::vector& segments) const; /* Set the four 7-segment display digits from left to right with ASCII codes. * Any code out side the accepted ascii ranges results in blank display. - * \param digit1 is left most digit with ascii range [32, 126] - * \param digit2 is second to left digit with ascii range [32, 126] - * \param digit3 is second to right digit with ascii range [32, 126] - * \param digit4 is right most digit with ascii range [32, 126] + * \param digit1 is left most digit with ascii range [32, 126] + * \param digit2 is second to left digit with ascii range [32, 126] + * \param digit3 is second to right digit with ascii range [32, 126] + * \param digit4 is right most digit with ascii range [32, 126] + * \return true if successful, false otherwise */ bool setDigitsASCII(const uint8_t& digit1, const uint8_t& digit2, const uint8_t& digit3, const uint8_t& digit4) const; @@ -231,6 +253,7 @@ namespace create { * \param notes is a sequence of notes. Each note is in the range [31, 127]. * Anything outside this range is considered a rest note. * \param durations for each note in fractions of a second from the range [0, 4) + * \return true if successful, false otherwise */ bool defineSong(const uint8_t& songNumber, const uint8_t& songLength, @@ -239,12 +262,11 @@ namespace create { /* Play a previously created song. * This command will not work if a song was not already defined with the specified song number. + * \param songNumber is one of four stored songs in the range [0, 4] + * \return true if successful, false otherwise */ bool playSong(const uint8_t& songNumber) const; - // TODO - //void registerCallback(...); - /* True if a left or right wheeldrop is detected. */ bool isWheeldrop() const; @@ -265,12 +287,19 @@ namespace create { */ bool isCliff() const; - //TODO - //bool isVirtualWall() const; + /* True if there is a virtual wall signal is being received. + */ + bool isVirtualWall() const; - //TODO + //TODO (https://github.com/AutonomyLab/libcreate/issues/8) //bool isWheelOvercurrent() const; + //TODO (https://github.com/AutonomyLab/libcreate/issues/8) + //bool isMainBrushOvercurrent() const; + + //TODO (https://github.com/AutonomyLab/libcreate/issues/8) + //bool isSideBrushOvercurrent() const; + /* Get level of the dirt detect sensor. * \return value in range [0, 255] */ @@ -292,55 +321,97 @@ namespace create { */ uint8_t getIRRight() const; + /* Get state of 'clean' button ('play' button on Create 1). + */ bool isCleanButtonPressed() const; + + /* Not supported by any firmware! + */ bool isClockButtonPressed() const; + + /* Not supported by any firmware! + */ bool isScheduleButtonPressed() const; + + /* Get state of 'day' button. + */ bool isDayButtonPressed() const; + + /* Get state of 'hour' button. + */ bool isHourButtonPressed() const; + + /* Get state of 'min' button. + */ bool isMinButtonPressed() const; + + /* Get state of 'dock' button ('advance' button on Create 1). + */ bool isDockButtonPressed() const; + + /* Get state of 'spot' button. + */ bool isSpotButtonPressed() const; /* Get battery voltage. - * \return value in millivolts + * \return value in volts */ - uint16_t getVoltage() const; + float getVoltage() const; /* Get current flowing in/out of battery. - * \return value in milliamps + * A positive current implies Create is charging. + * \return value in amps */ - uint16_t getCurrent() const; + float getCurrent() const; /* Get the temperature of battery. * \return value in Celsius */ - uint8_t getTemperature() const; + int8_t getTemperature() const; /* Get battery's remaining charge. - * \return value in milliamp-hours + * \return value in amp-hours */ - uint16_t getBatteryCharge() const; + float getBatteryCharge() const; /* Get estimated battery charge capacity. - * \return in milliamp-hours + * \return in amp-hours */ - uint16_t getBatteryCapacity() const; + float getBatteryCapacity() const; - bool isIRDetectLeft() const; - bool isIRDetectFrontLeft() const; - bool isIRDetectCenterLeft() const; - bool isIRDetectRight() const; - bool isIRDetectFrontRight() const; - bool isIRDetectCenterRight() const; + /* Return true if farthest left light sensor detects an obstacle, false otherwise. + */ + bool isLightBumperLeft() const; - uint16_t getDistLeft() const; - uint16_t getDistFrontLeft() const; - uint16_t getDistCenterLeft() const; - uint16_t getDistRight() const; - uint16_t getDistFrontRight() const; - uint16_t getDistCenterRight() const; + /* Return true if front left light sensor detects an obstacle, false otherwise. + */ + bool isLightBumperFrontLeft() const; - /* Return true if Create is moving forward. + /* Return true if center left light sensor detects an obstacle, false otherwise. + */ + bool isLightBumperCenterLeft() const; + + /* Return true if farthest right light sensor detects an obstacle, false otherwise. + */ + bool isLightBumperRight() const; + + /* Return true if front right light sensor detects an obstacle, false otherwise. + */ + bool isLightBumperFrontRight() const; + + /* Return true if center right light sensor detects an obstacle, false otherwise. + */ + bool isLightBumperCenterRight() const; + + //TODO (https://github.com/AutonomyLab/libcreate/issues/3) + //uint16_t getDistLeft() const; + //uint16_t getDistFrontLeft() const; + //uint16_t getDistCenterLeft() const; + //uint16_t getDistRight() const; + //uint16_t getDistFrontRight() const; + //uint16_t getDistCenterRight() const; + + /* Return true if Create is moving forward, false otherwise. */ bool isMovingForward() const; @@ -352,7 +423,7 @@ namespace create { */ create::CreateMode getMode() const; - /* Get the estimated position of Create based on it's wheel encoders. + /* Get the estimated position of Create based on wheel encoders. */ const create::Pose& getPose() const; diff --git a/include/create/types.h b/include/create/types.h index 3fcb3b1..9e65c08 100644 --- a/include/create/types.h +++ b/include/create/types.h @@ -80,7 +80,7 @@ namespace create { ID_UNUSED_2 = 32, ID_UNUSED_3 = 33, ID_CHARGE_SOURCE = 34, - ID_IO_MODE = 35, + ID_OI_MODE = 35, ID_SONG_NUM = 36, ID_PLAYING = 37, ID_NUM_STREAM_PACKETS = 38, @@ -156,7 +156,8 @@ namespace create { MODE_OFF = OC_POWER, MODE_PASSIVE = OC_START, MODE_SAFE = OC_SAFE, - MODE_FULL = OC_FULL + MODE_FULL = OC_FULL, + MODE_UNAVAILABLE = -1 }; enum CleanMode { diff --git a/include/create/util.h b/include/create/util.h index f20b710..e91d959 100644 --- a/include/create/util.h +++ b/include/create/util.h @@ -46,6 +46,9 @@ namespace create { 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 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 f9ef69d..b0cc473 100644 --- a/src/create.cpp +++ b/src/create.cpp @@ -66,10 +66,10 @@ namespace create { float dt = (curTime - prevOnDataTime) / 1000000.0; float deltaDist, deltaX, deltaY, deltaYaw; if (model == CREATE_1) { - deltaDist = GET_DATA(ID_DISTANCE) / 1000.0; //mm -> m - deltaYaw = GET_DATA(ID_ANGLE) * util::PI / 180.0; // D2R - deltaX = deltaDist * cos(pose.yaw); - deltaY = deltaDist * sin(pose.yaw); + 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) ); } else if (model == CREATE_2) { // Get cumulative ticks (wraps around at 65535) @@ -188,16 +188,16 @@ namespace create { return serial->send(cmd, 4); } - /*void Create::driveRadius(const float& vel, const float& radius) const { + bool 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); int16_t radius_mm = roundf(radius * 1000); - BOUND(vel_mm, -500, 500); + BOUND(vel_mm, -util::CREATE_2_MAX_VEL * 1000, util::CREATE_2_MAX_VEL * 1000); - // Consider special cases for radius + // Bound radius if not a special case if (radius_mm != 32768 && radius_mm != 32767 && radius_mm != -1 && radius_mm != 1) { - BOUND(radius_mm, -2000, 2000); + BOUND(radius_mm, -util::CREATE_2_MAX_RADIUS, util::CREATE_2_MAX_RADIUS); } uint8_t cmd[5] = { OC_DRIVE, @@ -207,9 +207,8 @@ namespace create { radius_mm & 0xff }; - serial->send(cmd, 5); + return serial->send(cmd, 5); } - */ bool Create::driveWheels(const float& leftVel, const float& rightVel) const { int16_t leftCmd = roundf(leftVel * 1000); @@ -226,17 +225,6 @@ 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, - rightPWM & 0xff, - leftPWM >> 8, - leftPWM & 0xff - }; - 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); @@ -432,6 +420,16 @@ namespace create { } } + bool Create::isVirtualWall() const { + if (data->isValidPacketID(ID_VIRTUAL_WALL)) { + return GET_DATA(ID_VIRTUAL_WALL); + } + else { + CERR("[create::Create] ", "Virtual Wall sensor not supported!"); + return false; + } + } + uint8_t Create::getDirtDetect() const { if (data->isValidPacketID(ID_DIRT_DETECT)) { return GET_DATA(ID_DIRT_DETECT); @@ -495,8 +493,9 @@ namespace create { } } - // Not working + // Not supported by any 600 series firmware bool Create::isClockButtonPressed() const { + CERR("[create::Create] ", "Clock button is not supported!"); if (data->isValidPacketID(ID_BUTTONS)) { return (GET_DATA(ID_BUTTONS) & 0x80) != 0; } @@ -506,8 +505,9 @@ namespace create { } } - // Not working + // Not supported by any 600 series firmware bool Create::isScheduleButtonPressed() const { + CERR("[create::Create] ", "Schedule button is not supported!"); if (data->isValidPacketID(ID_BUTTONS)) { return (GET_DATA(ID_BUTTONS) & 0x40) != 0; } @@ -567,9 +567,9 @@ namespace create { } } - uint16_t Create::getVoltage() const { + float Create::getVoltage() const { if (data->isValidPacketID(ID_VOLTAGE)) { - return GET_DATA(ID_VOLTAGE); + return (GET_DATA(ID_VOLTAGE) / 1000.0); } else { CERR("[create::Create] ", "Voltage sensor not supported!"); @@ -577,9 +577,9 @@ namespace create { } } - uint16_t Create::getCurrent() const { + float Create::getCurrent() const { if (data->isValidPacketID(ID_VOLTAGE)) { - return GET_DATA(ID_CURRENT); + return (((int16_t)GET_DATA(ID_CURRENT)) / 1000.0); } else { CERR("[create::Create] ", "Current sensor not supported!"); @@ -587,9 +587,9 @@ namespace create { } } - uint8_t Create::getTemperature() const { + int8_t Create::getTemperature() const { if (data->isValidPacketID(ID_TEMP)) { - return GET_DATA(ID_TEMP); + return (int8_t) GET_DATA(ID_TEMP); } else { CERR("[create::Create] ", "Temperature sensor not supported!"); @@ -597,9 +597,9 @@ namespace create { } } - uint16_t Create::getBatteryCharge() const { + float Create::getBatteryCharge() const { if (data->isValidPacketID(ID_CHARGE)) { - return GET_DATA(ID_CHARGE); + return (GET_DATA(ID_CHARGE) / 1000.0); } else { CERR("[create::Create] ", "Battery charge not supported!"); @@ -607,9 +607,9 @@ namespace create { } } - uint16_t Create::getBatteryCapacity() const { + float Create::getBatteryCapacity() const { if (data->isValidPacketID(ID_CAPACITY)) { - return GET_DATA(ID_CAPACITY); + return (GET_DATA(ID_CAPACITY) / 1000.0); } else { CERR("[create::Create] ", "Battery capacity not supported!"); @@ -617,7 +617,7 @@ namespace create { } } - bool Create::isIRDetectLeft() const { + bool Create::isLightBumperLeft() const { if (data->isValidPacketID(ID_LIGHT)) { return (GET_DATA(ID_LIGHT) & 0x01) != 0; } @@ -627,7 +627,7 @@ namespace create { } } - bool Create::isIRDetectFrontLeft() const { + bool Create::isLightBumperFrontLeft() const { if (data->isValidPacketID(ID_LIGHT)) { return (GET_DATA(ID_LIGHT) & 0x02) != 0; } @@ -637,7 +637,7 @@ namespace create { } } - bool Create::isIRDetectCenterLeft() const { + bool Create::isLightBumperCenterLeft() const { if (data->isValidPacketID(ID_LIGHT)) { return (GET_DATA(ID_LIGHT) & 0x04) != 0; } @@ -647,7 +647,7 @@ namespace create { } } - bool Create::isIRDetectCenterRight() const { + bool Create::isLightBumperCenterRight() const { if (data->isValidPacketID(ID_LIGHT)) { return (GET_DATA(ID_LIGHT) & 0x08) != 0; } @@ -657,7 +657,7 @@ namespace create { } } - bool Create::isIRDetectFrontRight() const { + bool Create::isLightBumperFrontRight() const { if (data->isValidPacketID(ID_LIGHT)) { return (GET_DATA(ID_LIGHT) & 0x10) != 0; } @@ -667,7 +667,7 @@ namespace create { } } - bool Create::isIRDetectRight() const { + bool Create::isLightBumperRight() const { if (data->isValidPacketID(ID_LIGHT)) { return (GET_DATA(ID_LIGHT) & 0x20) != 0; } @@ -687,6 +687,16 @@ namespace create { } } + create::CreateMode Create::getMode() const { + 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; + } + } + const Pose& Create::getPose() const { return pose; } diff --git a/src/data.cpp b/src/data.cpp index 28354a5..fe6dc29 100644 --- a/src/data.cpp +++ b/src/data.cpp @@ -24,13 +24,14 @@ namespace create { 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"); if (model == CREATE_1) { ADD_PACKET(ID_DISTANCE, 2, "distance"); ADD_PACKET(ID_ANGLE, 2, "angle"); } else if (model == CREATE_2) { - //ADD_PACKET(ID_VIRTUAL_WALL, 1, "virtual_wall"); //ADD_PACKET(ID_OVERCURRENTS, 1, "overcurrents"); ADD_PACKET(ID_DIRT_DETECT, 1, "dirt_detect"); //ADD_PACKET(ID_UNUSED_1, 1, "unused 1"); @@ -42,7 +43,6 @@ namespace create { //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_IO_MODE, 1, "oi_mode"); //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");