create.h
Go to the documentation of this file.
00001 
00032 #ifndef CREATE_H
00033 #define CREATE_H
00034 
00035 #include <boost/shared_ptr.hpp>
00036 #include <boost/numeric/ublas/matrix.hpp>
00037 #include <string>
00038 #include <unistd.h>
00039 
00040 #include "create/serial_stream.h"
00041 #include "create/serial_query.h"
00042 #include "create/data.h"
00043 #include "create/types.h"
00044 #include "create/util.h"
00045 
00046 namespace create {
00047   class Create {
00048     private:
00049       typedef boost::numeric::ublas::matrix<float> Matrix;
00050 
00051       enum CreateLED {
00052         LED_DEBRIS = 1,
00053         LED_SPOT = 2,
00054         LED_DOCK = 4,
00055         LED_CHECK = 8
00056       };
00057 
00058       RobotModel model;
00059 
00060       uint8_t mainMotorPower;
00061       uint8_t sideMotorPower;
00062       uint8_t vacuumMotorPower;
00063 
00064       // LEDs
00065       uint8_t debrisLED;
00066       uint8_t spotLED;
00067       uint8_t dockLED;
00068       uint8_t checkLED;
00069       uint8_t powerLED;
00070       uint8_t powerLEDIntensity;
00071 
00072       CreateMode mode;
00073 
00074       create::Pose pose;
00075       create::Vel vel;
00076 
00077       uint32_t prevTicksLeft;
00078       uint32_t prevTicksRight;
00079       float totalLeftDist;
00080       float totalRightDist;
00081       bool firstOnData;
00082       util::timestamp_t prevOnDataTime;
00083 
00084       Matrix poseCovar;
00085 
00086       float measuredLeftVel;
00087       float measuredRightVel;
00088       float requestedLeftVel;
00089       float requestedRightVel;
00090 
00091       void init();
00092       // Add two matrices and handle overflow case
00093       Matrix addMatrices(const Matrix &A, const Matrix &B) const;
00094       void onData();
00095       bool updateLEDs();
00096 
00097     protected:
00098       boost::shared_ptr<create::Data> data;
00099       boost::shared_ptr<create::Serial> serial;
00100 
00101     public:
00109       Create(RobotModel model = RobotModel::CREATE_2);
00110 
00119       Create(const std::string& port, const int& baud, RobotModel model = RobotModel::CREATE_2);
00120 
00124       ~Create();
00125 
00133       bool connect(const std::string& port, const int& baud);
00134 
00140       inline bool connected() const { return serial->connected(); };
00141 
00145       void disconnect();
00146 
00152       bool setMode(const create::CreateMode& mode);
00153 
00159       bool clean(const create::CleanMode& mode = CLEAN_DEFAULT);
00160 
00166       bool dock() const;
00167 
00175       bool setDate(const create::DayOfWeek& day, const uint8_t& hour, const uint8_t& min) const;
00176 
00186       bool driveRadius(const float& velocity, const float& radius);
00187 
00194       bool driveWheels(const float& leftWheel, const float& rightWheel);
00195 
00202       bool driveWheelsPwm(const float& leftWheel, const float& rightWheel);
00203 
00210       bool drive(const float& xVel, const float& angularVel);
00211 
00217       bool setSideMotor(const float& power);
00218 
00224       bool setMainMotor(const float& power);
00225 
00231       bool setVacuumMotor(const float& power);
00232 
00240       bool setAllMotors(const float& mainPower, const float& sidePower, const float& vacuumPower);
00241 
00247       bool enableDebrisLED(const bool& enable);
00248 
00254       bool enableSpotLED(const bool& enable);
00255 
00261       bool enableDockLED(const bool& enable);
00262 
00268       bool enableCheckRobotLED(const bool& enable);
00269 
00276       bool setPowerLED(const uint8_t& power, const uint8_t& intensity = 255);
00277 
00297       bool setDigits(const std::vector<bool>& segments) const;
00298 
00308       bool setDigitsASCII(const uint8_t& digit1, const uint8_t& digit2,
00309                           const uint8_t& digit3, const uint8_t& digit4) const;
00310 
00321       bool defineSong(const uint8_t& songNumber,
00322                       const uint8_t& songLength,
00323                       const uint8_t* notes,
00324                       const float* durations) const;
00325 
00332       bool playSong(const uint8_t& songNumber) const;
00333 
00337       bool isWheeldrop() const;
00338 
00342       bool isLeftBumper() const;
00343 
00347       bool isRightBumper() const;
00348 
00352       bool isWall() const;
00353 
00357       bool isCliff() const;
00358 
00362       bool isVirtualWall() const;
00363 
00368       bool isWheelOvercurrent() const;
00369 
00374       bool isMainBrushOvercurrent() const;
00375 
00380       bool isSideBrushOvercurrent() const;
00381 
00386       uint8_t getDirtDetect() const;
00387 
00392       uint8_t getIROmni() const;
00393 
00399       uint8_t getIRLeft() const;
00400 
00405       uint8_t getIRRight() const;
00406 
00411       bool isCleanButtonPressed() const;
00412 
00416       bool isClockButtonPressed() const;
00417 
00421       bool isScheduleButtonPressed() const;
00422 
00427       bool isDayButtonPressed() const;
00428 
00433       bool isHourButtonPressed() const;
00434 
00439       bool isMinButtonPressed() const;
00440 
00445       bool isDockButtonPressed() const;
00446 
00451       bool isSpotButtonPressed() const;
00452 
00457       float getVoltage() const;
00458 
00464       float getCurrent() const;
00465 
00470       int8_t getTemperature() const;
00471 
00476       float getBatteryCharge() const;
00477 
00482       float getBatteryCapacity() const;
00483 
00487       bool isLightBumperLeft() const;
00488 
00492       bool isLightBumperFrontLeft() const;
00493 
00497       bool isLightBumperCenterLeft() const;
00498 
00502       bool isLightBumperRight() const;
00503 
00507       bool isLightBumperFrontRight() const;
00508 
00512       bool isLightBumperCenterRight() const;
00513 
00518       uint16_t getLightSignalLeft() const;
00519 
00524       uint16_t getLightSignalFrontLeft() const;
00525 
00530       uint16_t getLightSignalCenterLeft() const;
00531 
00536       uint16_t getLightSignalRight() const;
00537 
00542       uint16_t getLightSignalFrontRight() const;
00543 
00548       uint16_t getLightSignalCenterRight() const;
00549 
00553       bool isMovingForward() const;
00554 
00559       float getLeftWheelDistance() const;
00560 
00565       float getRightWheelDistance() const;
00566 
00571       float getMeasuredLeftWheelVel() const;
00572 
00577       float getMeasuredRightWheelVel() const;
00578 
00584       float getRequestedLeftWheelVel() const;
00585 
00591       float getRequestedRightWheelVel() const;
00592 
00597       create::ChargingState getChargingState() const;
00598 
00603       create::CreateMode getMode();
00604 
00609       create::Pose getPose() const;
00610 
00615       create::Vel getVel() const;
00616 
00623       uint64_t getNumCorruptPackets() const;
00624 
00629       uint64_t getTotalPackets() const;
00630   };  // end Create class
00631 
00632 }  // namespace create
00633 #endif  // CREATE_DRIVER_H


libcreate
Author(s): Jacob Perron
autogenerated on Thu Jun 6 2019 21:02:06