From 75dd293bfdc06607694517b010e4003c0f58645b Mon Sep 17 00:00:00 2001 From: jacobperron Date: Mon, 28 Mar 2016 19:17:35 -0700 Subject: [PATCH] * Rename 'isIRDetect*' functions to 'isLightBumper*' * Documentation / code cleanup * Add function 'driveRadius' * Add function 'isVirtualWall' --- examples/create_demo.cpp | 12 +-- include/create/create.h | 174 +++++++++++++++++++++++++++------------ include/create/util.h | 3 + src/create.cpp | 50 +++++------ src/data.cpp | 2 +- 5 files changed, 157 insertions(+), 84 deletions(-) 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 e1885b4..615da02 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,13 +321,36 @@ 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. @@ -307,7 +359,7 @@ namespace create { uint16_t getVoltage() const; /* Get current flowing in/out of battery. - * A positive current implies Create is charging. + * A positive current implies Create is charging. * \return value in milliamps */ int16_t getCurrent() const; @@ -327,21 +379,39 @@ namespace create { */ uint16_t 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; @@ -353,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/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 5cb9f72..5456751 100644 --- a/src/create.cpp +++ b/src/create.cpp @@ -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; } @@ -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; } diff --git a/src/data.cpp b/src/data.cpp index 28354a5..25616c9 100644 --- a/src/data.cpp +++ b/src/data.cpp @@ -24,13 +24,13 @@ 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"); 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");