Go to the documentation of this file.
9 #define GET_DATA(id) (data->getPacket(id)->getData())
10 #define BOUND_CONST(val,min,max) (val<min?min:(val>max?max:val))
11 #define BOUND(val,min,max) (val = BOUND_CONST(val,min,max))
15 namespace ublas = boost::numeric::ublas;
48 serial = std::make_shared<SerialQuery>(
data, install_signal_handler);
50 serial = std::make_shared<SerialStream>(
56 init(install_signal_handler);
62 init(install_signal_handler);
63 serial->connect(dev, baud);
71 size_t rows = A.size1();
72 size_t cols = A.size2();
74 assert(rows == B.size1());
75 assert(cols == B.size2());
78 for (
size_t i = 0u; i < rows; i++) {
79 for (
size_t j = 0u; j < cols; j++) {
80 const float a = A(i, j);
81 const float b = B(i, j);
84 C(i, j) = (a < 0.0) ? std::numeric_limits<float>::min() : std::numeric_limits<float>::max();
106 auto curTime = std::chrono::steady_clock::now();
107 float dt =
static_cast<std::chrono::duration<float>
>(curTime -
prevOnDataTime).count();
108 float deltaDist = 0.0f;
111 float deltaYaw = 0.0f;
112 float leftWheelDist = 0.0f;
113 float rightWheelDist = 0.0f;
114 float wheelDistDiff = 0.0f;
117 int16_t angleField = 0;
122 std::memcpy(&distance, &distanceRaw,
sizeof(distance));
123 deltaDist = distance / 1000.0;
127 std::memcpy(&angleField, &angleRaw,
sizeof(angleField));
131 wheelDistDiff = 2.0 * angleField / 1000.0;
132 leftWheelDist = deltaDist - (wheelDistDiff / 2.0);
133 rightWheelDist = deltaDist + (wheelDistDiff / 2.0);
148 deltaYaw = angleField * (
util::PI / 180.0);
150 leftWheelDist = deltaDist - (wheelDistDiff / 2.0);
151 rightWheelDist = deltaDist + (wheelDistDiff / 2.0);
175 deltaDist = (rightWheelDist + leftWheelDist) / 2.0;
177 wheelDistDiff = rightWheelDist - leftWheelDist;
188 float dtHistorySum = 0;
193 auto dtAverage = dtHistorySum /
dtHistory.size();
200 deltaX = deltaDist * cos(
pose.
yaw);
201 deltaY = deltaDist * sin(
pose.
yaw);
203 float turnRadius = (
model.
getAxleLength() / 2.0) * (leftWheelDist + rightWheelDist) / wheelDistDiff;
205 deltaY = -turnRadius * (cos(
pose.
yaw + deltaYaw) - cos(
pose.
yaw));
212 vel.
x = deltaDist / dtAverage;
214 vel.
yaw = deltaYaw / dtAverage;
225 float cosYawAndHalfDelta = cos(
pose.
yaw + (deltaYaw / 2.0));
226 float sinYawAndHalfDelta = sin(
pose.
yaw + (deltaYaw / 2.0));
230 invCovar(0, 0) = kr * fabs(rightWheelDist);
231 invCovar(0, 1) = 0.0;
232 invCovar(1, 0) = 0.0;
233 invCovar(1, 1) = kl * fabs(leftWheelDist);
236 Finc(0, 0) = (cosYawAndHalfDelta / 2.0) - (distOverTwoWB * sinYawAndHalfDelta);
237 Finc(0, 1) = (cosYawAndHalfDelta / 2.0) + (distOverTwoWB * sinYawAndHalfDelta);
238 Finc(1, 0) = (sinYawAndHalfDelta / 2.0) + (distOverTwoWB * cosYawAndHalfDelta);
239 Finc(1, 1) = (sinYawAndHalfDelta / 2.0) - (distOverTwoWB * cosYawAndHalfDelta);
242 Matrix FincT = boost::numeric::ublas::trans(Finc);
247 Fp(0, 2) = (-deltaDist) * sinYawAndHalfDelta;
250 Fp(1, 2) = deltaDist * cosYawAndHalfDelta;
254 Matrix FpT = boost::numeric::ublas::trans(Fp);
256 Matrix velCovar = ublas::prod(invCovar, FincT);
257 velCovar = ublas::prod(Finc, velCovar);
270 poseCovarTmp = ublas::prod(Fp, poseCovarTmp);
295 bool timeout =
false;
298 float retryInterval = 5;
302 if (difftime(now, start) > maxWait) {
304 CERR(
"[create::Create] ",
"failed to connect over serial: timeout");
307 usleep(retryInterval * 1000000);
308 COUT(
"[create::Create] ",
"retrying to establish serial connection...");
335 CERR(
"[create::Create] ",
"protocol version 2 does not support turning robot off");
353 CERR(
"[create::Create] ",
"cannot set robot to mode '" <<
mode <<
"'");
371 if (day < 0 || day > 6 ||
376 uint8_t cmd[4] = {
OC_DATE,
static_cast<uint8_t
>(day), hour, min };
377 return serial->send(cmd, 4);
385 int16_t vel_mm = roundf(boundedVel * 1000);
386 int16_t radius_mm = roundf(radius * 1000);
389 if (radius_mm != -32768 && radius_mm != 32767 &&
390 radius_mm != -1 && radius_mm != 1) {
395 static_cast<uint8_t
>(vel_mm >> 8),
396 static_cast<uint8_t
>(vel_mm & 0xff),
397 static_cast<uint8_t
>(radius_mm >> 8),
398 static_cast<uint8_t
>(radius_mm & 0xff)
401 return serial->send(cmd, 5);
410 int16_t leftCmd = roundf(boundedLeftVel * 1000);
411 int16_t rightCmd = roundf(boundedRightVel * 1000);
414 static_cast<uint8_t
>(rightCmd >> 8),
415 static_cast<uint8_t
>(rightCmd & 0xff),
416 static_cast<uint8_t
>(leftCmd >> 8),
417 static_cast<uint8_t
>(leftCmd & 0xff)
419 return serial->send(cmd, 5);
423 if (boundedLeftVel != boundedRightVel) {
425 (boundedLeftVel - boundedRightVel);
432 if (boundedLeftVel == -boundedRightVel || std::abs(roundf(radius * 1000)) <= 1) {
434 vel = boundedRightVel;
436 vel = (std::abs(boundedLeftVel) + std::abs(boundedRightVel)) / 2.0 * ((boundedLeftVel + boundedRightVel) > 0 ? 1.0 : -1.0);
454 static const int16_t PWM_COUNTS = 255;
456 if (leftWheel < -1.0 || leftWheel > 1.0 ||
457 rightWheel < -1.0 || rightWheel > 1.0)
460 int16_t leftPwm = roundf(leftWheel * PWM_COUNTS);
461 int16_t rightPwm = roundf(rightWheel * PWM_COUNTS);
464 static_cast<uint8_t
>(rightPwm >> 8),
465 static_cast<uint8_t
>(rightPwm & 0xff),
466 static_cast<uint8_t
>(leftPwm >> 8),
467 static_cast<uint8_t
>(leftPwm & 0xff)
470 return serial->send(cmd, 5);
481 if (main < -1.0 || main > 1.0 ||
482 side < -1.0 || side > 1.0 ||
483 vacuum < -1.0 || vacuum > 1.0)
492 static_cast<uint8_t
>((side != 0.0 ? 1 : 0) |
493 (vacuum != 0.0 ? 2 : 0) |
494 (
main != 0.0 ? 4 : 0))
496 return serial->send(cmd, 2);
505 return serial->send(cmd, 4);
528 return serial->send(cmd, 4);
574 const uint8_t& digit3,
const uint8_t& digit4)
const {
575 if (digit1 < 32 || digit1 > 126 ||
576 digit2 < 32 || digit2 > 126 ||
577 digit3 < 32 || digit3 > 126 ||
578 digit4 < 32 || digit4 > 126)
588 return serial->send(cmd, 5);
592 const uint8_t& songLength,
593 const uint8_t* notes,
594 const float* durations)
const {
597 std::vector<uint8_t> cmd(2 * songLength + 3);
602 for (i = 3; i < 2 * songLength + 3; i = i + 2) {
603 if (durations[j] < 0 || durations[j] >= 4)
605 duration = durations[j] * 64;
607 cmd[i + 1] = duration;
611 return serial->send(cmd.data(), cmd.size());
617 uint8_t cmd[2] = {
OC_PLAY, songNumber };
618 return serial->send(cmd, 2);
630 CERR(
"[create::Create] ",
"Wheeldrop sensor not supported!");
640 CERR(
"[create::Create] ",
"Wheeldrop sensor not supported!");
650 CERR(
"[create::Create] ",
"Wheeldrop sensor not supported!");
660 CERR(
"[create::Create] ",
"Left bumper not supported!");
670 CERR(
"[create::Create] ",
"Right bumper not supported!");
680 CERR(
"[create::Create] ",
"Wall sensor not supported!");
696 CERR(
"[create::Create] ",
"Cliff sensors not supported!");
706 CERR(
"[create::Create] ",
"Left cliff sensors not supported!");
716 CERR(
"[create::Create] ",
"Front left cliff sensors not supported!");
726 CERR(
"[create::Create] ",
"Rightt cliff sensors not supported!");
736 CERR(
"[create::Create] ",
"Front right cliff sensors not supported!");
746 CERR(
"[create::Create] ",
"Virtual Wall sensor not supported!");
756 CERR(
"[create::Create] ",
"Dirt detector not supported!");
766 CERR(
"[create::Create] ",
"Omni IR sensor not supported!");
776 CERR(
"[create::Create] ",
"Left IR sensor not supported!");
786 CERR(
"[create::Create] ",
"Right IR sensor not supported!");
794 assert(chargeState <= 5);
798 CERR(
"[create::Create] ",
"Charging state not supported!");
808 CERR(
"[create::Create] ",
"Buttons not supported!");
815 CERR(
"[create::Create] ",
"Clock button is not supported!");
820 CERR(
"[create::Create] ",
"Buttons not supported!");
827 CERR(
"[create::Create] ",
"Schedule button is not supported!");
832 CERR(
"[create::Create] ",
"Buttons not supported!");
842 CERR(
"[create::Create] ",
"Buttons not supported!");
852 CERR(
"[create::Create] ",
"Buttons not supported!");
862 CERR(
"[create::Create] ",
"Buttons not supported!");
872 CERR(
"[create::Create] ",
"Buttons not supported!");
882 CERR(
"[create::Create] ",
"Buttons not supported!");
892 CERR(
"[create::Create] ",
"Voltage sensor not supported!");
902 CERR(
"[create::Create] ",
"Current sensor not supported!");
912 CERR(
"[create::Create] ",
"Temperature sensor not supported!");
922 CERR(
"[create::Create] ",
"Battery charge not supported!");
932 CERR(
"[create::Create] ",
"Battery capacity not supported!");
942 CERR(
"[create::Create] ",
"Light sensors not supported!");
952 CERR(
"[create::Create] ",
"Light sensors not supported!");
962 CERR(
"[create::Create] ",
"Light sensors not supported!");
972 CERR(
"[create::Create] ",
"Light sensors not supported!");
982 CERR(
"[create::Create] ",
"Light sensors not supported!");
992 CERR(
"[create::Create] ",
"Light sensors not supported!");
1002 CERR(
"[create::Create] ",
"Light sensors not supported!");
1012 CERR(
"[create::Create] ",
"Light sensors not supported!");
1022 CERR(
"[create::Create] ",
"Light sensors not supported!");
1032 CERR(
"[create::Create] ",
"Light sensors not supported!");
1042 CERR(
"[create::Create] ",
"Light sensors not supported!");
1052 CERR(
"[create::Create] ",
"Light sensors not supported!");
1062 CERR(
"[create::Create] ",
"Stasis sensor not supported!");
1072 CERR(
"[create::Create] ",
"Overcurrent sensor not supported!");
1082 CERR(
"[create::Create] ",
"Overcurrent sensor not supported!");
1092 CERR(
"[create::Create] ",
"Overcurrent sensor not supported!");
1150 return serial->getNumCorruptPackets();
1154 return serial->getTotalPackets();
float getLeftWheelDistance() const
Get the total distance the left wheel has moved.
bool isSpotButtonPressed() const
Get state of 'spot' button.
~Create()
Attempts to disconnect from serial.
bool isHourButtonPressed() const
Get state of 'hour' button.
bool isDockButtonPressed() const
Get state of 'dock' button ('advance' button on Create 1).
bool setAllMotors(const float &mainPower, const float &sidePower, const float &vacuumPower)
Set the power of all motors.
bool setMode(const create::CreateMode &mode)
Change Create mode.
bool isCliffFrontLeft() const
float getRequestedLeftWheelVel() const
Get the requested velocity of the left wheel. This value is bounded at the maximum velocity of the ro...
bool isMainBrushOvercurrent() const
std::shared_ptr< create::Data > data
static const float STRAIGHT_RADIUS
bool driveWheelsPwm(const float &leftWheel, const float &rightWheel)
Set the direct for the left and right wheels.
uint16_t getLightSignalFrontLeft() const
Get the signal strength from the front-left light sensor.
std::deque< float > dtHistory
bool isLightBumperRight() const
uint8_t getIROmni() const
Get value of 8-bit IR character currently being received by omnidirectional sensor.
void setModeReportWorkaround(const bool &enable)
Enable or disable the mode reporting workaround. Some Roomba 6xx robots incorrectly report the OI mod...
Create(RobotModel model=RobotModel::CREATE_2, bool install_signal_handler=true)
Default constructor.
void setDtHistoryLength(const uint8_t &dtHistoryLength)
Set dtHistoryLength parameter. Used to configure the size of the buffer for calculating average time ...
bool modeReportWorkaround
Matrix addMatrices(const Matrix &A, const Matrix &B) const
boost::numeric::ublas::matrix< float > Matrix
#define BOUND_CONST(val, min, max)
uint8_t powerLEDIntensity
bool setMainMotor(const float &power)
Set the power to the main brush motor.
bool isClockButtonPressed() const
Not supported by any firmware!
bool isDayButtonPressed() const
Get state of 'day' button.
float getWheelDiameter() const
bool isRightWheeldrop() const
#define CERR(prefix, msg)
bool setDate(const create::DayOfWeek &day, const uint8_t &hour, const uint8_t &min) const
Sets the internal clock of Create.
bool clean(const create::CleanMode &mode=CLEAN_DEFAULT)
Starts a cleaning mode. Changes mode to MODE_PASSIVE.
uint16_t getLightSignalCenterLeft() const
Get the signal strength from the center-left light sensor.
bool enableDockLED(const bool &enable)
Set the green "dock" LED on/off.
std::shared_ptr< create::Serial > serial
float getMaxVelocity() const
bool isSideBrushOvercurrent() const
bool enableSpotLED(const bool &enable)
Set the green "spot" LED on/off.
create::Pose getPose() const
Get the estimated pose of Create based on wheel encoders.
static const uint8_t STREAM_HEADER
bool driveWheels(const float &leftWheel, const float &rightWheel)
Set the velocities for the left and right wheels.
int8_t getTemperature() const
Get the temperature of battery.
uint16_t getLightSignalRight() const
Get the signal strength from the right light sensor.
float getMeasuredRightWheelVel() const
Get the measured velocity of the right wheel.
bool isLightBumperCenterLeft() const
#define BOUND(val, min, max)
ProtocolVersion getVersion() const
#define COUT(prefix, msg)
bool setVacuumMotor(const float &power)
Set the power to the vacuum motor.
uint16_t getLightSignalFrontRight() const
Get the signal strength from the front-right light sensor.
bool drive(const float &xVel, const float &angularVel)
Set the forward and angular velocity of Create.
bool dock() const
Starts the docking behaviour. Changes mode to MODE_PASSIVE.
float normalizeAngle(const float &angle)
bool enableCheckRobotLED(const bool &enable)
Set the orange "check Create" LED on/off.
bool isLightBumperFrontRight() const
bool isCliffRight() const
uint16_t getLightSignalCenterRight() const
Get the signal strength from the center-right light sensor.
uint64_t getTotalPackets() const
Get the total number of serial packets received (including corrupt packets) since first connecting to...
bool getModeReportWorkaround() const
bool isMinButtonPressed() const
Get state of 'min' button.
bool isWheelOvercurrent() const
uint64_t getNumCorruptPackets() const
Get the number of corrupt serial packets since first connecting to Create. This value is ideally zero...
bool setPowerLED(const uint8_t &power, const uint8_t &intensity=255)
Set the center power LED.
static const float MAX_RADIUS
float getRequestedRightWheelVel() const
Get the requested velocity of the right wheel. This value is bounded at the maximum velocity of the r...
float getVoltage() const
Get battery voltage.
bool enableDebrisLED(const bool &enable)
Set the blue "debris" LED on/off.
float getCurrent() const
Get current flowing in/out of battery. A positive current implies Create is charging.
bool isLeftBumper() const
bool willFloatOverflow(const float a, const float b)
bool isRightBumper() const
bool isLightBumperLeft() const
static const float V_3_TICKS_PER_REV
float getRightWheelDistance() const
Get the total distance the right wheel has moved.
bool isMovingForward() const
bool setDigitsASCII(const uint8_t &digit1, const uint8_t &digit2, const uint8_t &digit3, const uint8_t &digit4) const
Set the four 7-segment display digits from left to right with ASCII codes. Any code out side the acce...
float getBatteryCharge() const
Get battery's remaining charge.
bool isCleanButtonPressed() const
Get state of 'clean' button ('play' button on Create 1).
bool isLightBumperCenterRight() const
bool isLightBumperFrontLeft() const
create::Vel getVel() const
Get the estimated velocity of Create based on wheel encoders.
uint8_t getIRRight() const
Get value of 8-bit IR character currently being received by right sensor.
float getAxleLength() const
void disconnect()
Disconnect from serial.
bool driveRadius(const float &velocity, const float &radius)
Set the average wheel velocity and turning radius of Create.
uint8_t getDirtDetect() const
Get level of the dirt detect sensor.
std::vector< float > covariance
3x3 covariance matrix in row-major order.
int main(int argc, char **argv)
std::chrono::time_point< std::chrono::steady_clock > prevOnDataTime
bool defineSong(const uint8_t &songNumber, const uint8_t &songLength, const uint8_t *notes, const float *durations) const
Defines a song from the provided notes and labels it with a song number.
static const uint32_t V_3_MAX_ENCODER_TICKS
create::CreateMode getMode()
Get the current mode reported by Create.
bool connect(const std::string &port, const int &baud)
Make a serial connection to Create.
float getMeasuredLeftWheelVel() const
Get the measured velocity of the left wheel.
bool isLeftWheeldrop() const
bool isCliffFrontRight() const
static const float IN_PLACE_RADIUS
create::ChargingState getChargingState() const
Get the current charging state.
uint8_t getIRLeft() const
Get value of 8-bit IR character currently being received by left sensor.
bool isScheduleButtonPressed() const
Not supported by any firmware!
bool playSong(const uint8_t &songNumber) const
Play a previously created song. This command will not work if a song was not already defined with the...
void init(bool install_signal_handler)
bool setSideMotor(const float &power)
Set the power to the side brush motor.
bool isVirtualWall() const
float getBatteryCapacity() const
Get estimated battery charge capacity.
uint16_t getLightSignalLeft() const
Get the signal strength from the left light sensor.
libcreate
Author(s): Jacob Perron
autogenerated on Wed May 24 2023 02:24:57