create.cpp
Go to the documentation of this file.
1 #include <iostream>
2 #include <cmath>
3 #include <ctime>
4 #include <memory>
5 #include <assert.h>
6 
7 #include "create/create.h"
8 
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))
12 
13 namespace create {
14 
15  namespace ublas = boost::numeric::ublas;
16 
17  void Create::init(bool install_signal_handler) {
18  mainMotorPower = 0;
19  sideMotorPower = 0;
20  vacuumMotorPower = 0;
21  debrisLED = 0;
22  spotLED = 0;
23  dockLED = 0;
24  checkLED = 0;
25  powerLED = 0;
27  prevTicksLeft = 0;
28  prevTicksRight = 0;
29  totalLeftDist = 0.0;
30  totalRightDist = 0.0;
31  firstOnData = true;
32  mode = MODE_OFF;
33  pose.x = 0;
34  pose.y = 0;
35  pose.yaw = 0;
36  pose.covariance = std::vector<float>(9, 0.0);
37  vel.x = 0;
38  vel.y = 0;
39  vel.yaw = 0;
40  vel.covariance = std::vector<float>(9, 0.0);
41  poseCovar = Matrix(3, 3, 0.0);
42  requestedLeftVel = 0;
44  dtHistoryLength = 100;
45  modeReportWorkaround = false;
46  data = std::shared_ptr<Data>(new Data(model.getVersion()));
47  if (model.getVersion() == V_1) {
48  serial = std::make_shared<SerialQuery>(data, install_signal_handler);
49  } else {
50  serial = std::make_shared<SerialStream>(
51  data, create::util::STREAM_HEADER, install_signal_handler);
52  }
53  }
54 
55  Create::Create(RobotModel m, bool install_signal_handler) : model(m) {
56  init(install_signal_handler);
57  }
58 
59  Create::Create(const std::string& dev, const int& baud, RobotModel m, bool install_signal_handler)
60  : model(m)
61  {
62  init(install_signal_handler);
63  serial->connect(dev, baud);
64  }
65 
67  disconnect();
68  }
69 
70  Create::Matrix Create::addMatrices(const Matrix &A, const Matrix &B) const {
71  size_t rows = A.size1();
72  size_t cols = A.size2();
73 
74  assert(rows == B.size1());
75  assert(cols == B.size2());
76 
77  Matrix C(rows, cols);
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);
82  if (util::willFloatOverflow(a, b)) {
83  // If overflow, set to float min or max depending on direction of overflow
84  C(i, j) = (a < 0.0) ? std::numeric_limits<float>::min() : std::numeric_limits<float>::max();
85  }
86  else {
87  C(i, j) = a + b;
88  }
89  }
90  }
91  return C;
92  }
93 
94  void Create::onData() {
95  if (firstOnData) {
96  if (model.getVersion() >= V_3) {
97  // Initialize tick counts
100  }
101  prevOnDataTime = std::chrono::steady_clock::now();
102  firstOnData = false;
103  }
104 
105  // Get current time
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;
109  float deltaX = 0.0f;
110  float deltaY = 0.0f;
111  float deltaYaw = 0.0f;
112  float leftWheelDist = 0.0f;
113  float rightWheelDist = 0.0f;
114  float wheelDistDiff = 0.0f;
115 
116  // Protocol versions 1 and 2 use distance and angle fields for odometry
117  int16_t angleField = 0;
118  if (model.getVersion() <= V_2) {
119  // This is a standards compliant way of doing unsigned to signed conversion
120  uint16_t distanceRaw = GET_DATA(ID_DISTANCE);
121  int16_t distance;
122  std::memcpy(&distance, &distanceRaw, sizeof(distance));
123  deltaDist = distance / 1000.0; // mm -> m
124 
125  // Angle is processed differently in versions 1 and 2
126  uint16_t angleRaw = GET_DATA(ID_ANGLE);
127  std::memcpy(&angleField, &angleRaw, sizeof(angleField));
128  }
129 
130  if (model.getVersion() == V_1) {
131  wheelDistDiff = 2.0 * angleField / 1000.0;
132  leftWheelDist = deltaDist - (wheelDistDiff / 2.0);
133  rightWheelDist = deltaDist + (wheelDistDiff / 2.0);
134  deltaYaw = wheelDistDiff / model.getAxleLength();
135  } else if (model.getVersion() == V_2) {
136  /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
137  * Certain older Creates have major problems with odometry *
138  * http://answers.ros.org/question/31935/createroomba-odometry/ *
139  * *
140  * All Creates have an issue with rounding of the angle field, which causes *
141  * major errors to accumulate in the odometry yaw. *
142  * http://wiki.tekkotsu.org/index.php/Create_Odometry_Bug *
143  * https://github.com/AutonomyLab/create_autonomy/issues/28 *
144  * *
145  * TODO: Consider using velocity command as substitute for pose estimation *
146  * to mitigate both of these problems. *
147  * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
148  deltaYaw = angleField * (util::PI / 180.0); // D2R
149  wheelDistDiff = model.getAxleLength() * deltaYaw;
150  leftWheelDist = deltaDist - (wheelDistDiff / 2.0);
151  rightWheelDist = deltaDist + (wheelDistDiff / 2.0);
152  } else if (model.getVersion() >= V_3) {
153  // Get cumulative ticks (wraps around at 65535)
154  uint16_t totalTicksLeft = GET_DATA(ID_LEFT_ENC);
155  uint16_t totalTicksRight = GET_DATA(ID_RIGHT_ENC);
156  // Compute ticks since last update
157  int ticksLeft = totalTicksLeft - prevTicksLeft;
158  int ticksRight = totalTicksRight - prevTicksRight;
159  prevTicksLeft = totalTicksLeft;
160  prevTicksRight = totalTicksRight;
161 
162  // Handle wrap around
163  if (std::abs(ticksLeft) > 0.9 * util::V_3_MAX_ENCODER_TICKS) {
164  ticksLeft = (ticksLeft % util::V_3_MAX_ENCODER_TICKS) + 1;
165  }
166  if (std::abs(ticksRight) > 0.9 * util::V_3_MAX_ENCODER_TICKS) {
167  ticksRight = (ticksRight % util::V_3_MAX_ENCODER_TICKS) + 1;
168  }
169 
170  // Compute distance travelled by each wheel
171  leftWheelDist = (ticksLeft / util::V_3_TICKS_PER_REV)
173  rightWheelDist = (ticksRight / util::V_3_TICKS_PER_REV)
175  deltaDist = (rightWheelDist + leftWheelDist) / 2.0;
176 
177  wheelDistDiff = rightWheelDist - leftWheelDist;
178  deltaYaw = wheelDistDiff / model.getAxleLength();
179  }
180 
181  // determine average dt over window
182  dtHistory.push_front(dt);
183 
184  if (dtHistory.size() > dtHistoryLength){
185  dtHistory.pop_back();
186  }
187 
188  float dtHistorySum = 0;
189  for (auto it = dtHistory.cbegin(); it != dtHistory.cend(); ++it)
190  {
191  dtHistorySum += *it;
192  }
193  auto dtAverage = dtHistorySum / dtHistory.size();
194 
195  measuredLeftVel = leftWheelDist / dtAverage;
196  measuredRightVel = rightWheelDist / dtAverage;
197 
198  // Moving straight
199  if (fabs(wheelDistDiff) < util::EPS) {
200  deltaX = deltaDist * cos(pose.yaw);
201  deltaY = deltaDist * sin(pose.yaw);
202  } else {
203  float turnRadius = (model.getAxleLength() / 2.0) * (leftWheelDist + rightWheelDist) / wheelDistDiff;
204  deltaX = turnRadius * (sin(pose.yaw + deltaYaw) - sin(pose.yaw));
205  deltaY = -turnRadius * (cos(pose.yaw + deltaYaw) - cos(pose.yaw));
206  }
207 
208  totalLeftDist += leftWheelDist;
209  totalRightDist += rightWheelDist;
210 
211  if (fabs(dtAverage) > util::EPS) {
212  vel.x = deltaDist / dtAverage;
213  vel.y = 0.0;
214  vel.yaw = deltaYaw / dtAverage;
215  } else {
216  vel.x = 0.0;
217  vel.y = 0.0;
218  vel.yaw = 0.0;
219  }
220 
221  // Update covariances
222  // Ref: "Introduction to Autonomous Mobile Robots" (Siegwart 2004, page 189)
223  float kr = 1.0; // TODO: Perform experiments to find these nondeterministic parameters
224  float kl = 1.0;
225  float cosYawAndHalfDelta = cos(pose.yaw + (deltaYaw / 2.0)); // deltaX?
226  float sinYawAndHalfDelta = sin(pose.yaw + (deltaYaw / 2.0)); // deltaY?
227  float distOverTwoWB = deltaDist / (model.getAxleLength() * 2.0);
228 
229  Matrix invCovar(2, 2);
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);
234 
235  Matrix Finc(3, 2);
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);
240  Finc(2, 0) = (1.0 / model.getAxleLength());
241  Finc(2, 1) = (-1.0 / model.getAxleLength());
242  Matrix FincT = boost::numeric::ublas::trans(Finc);
243 
244  Matrix Fp(3, 3);
245  Fp(0, 0) = 1.0;
246  Fp(0, 1) = 0.0;
247  Fp(0, 2) = (-deltaDist) * sinYawAndHalfDelta;
248  Fp(1, 0) = 0.0;
249  Fp(1, 1) = 1.0;
250  Fp(1, 2) = deltaDist * cosYawAndHalfDelta;
251  Fp(2, 0) = 0.0;
252  Fp(2, 1) = 0.0;
253  Fp(2, 2) = 1.0;
254  Matrix FpT = boost::numeric::ublas::trans(Fp);
255 
256  Matrix velCovar = ublas::prod(invCovar, FincT);
257  velCovar = ublas::prod(Finc, velCovar);
258 
259  vel.covariance[0] = velCovar(0, 0);
260  vel.covariance[1] = velCovar(0, 1);
261  vel.covariance[2] = velCovar(0, 2);
262  vel.covariance[3] = velCovar(1, 0);
263  vel.covariance[4] = velCovar(1, 1);
264  vel.covariance[5] = velCovar(1, 2);
265  vel.covariance[6] = velCovar(2, 0);
266  vel.covariance[7] = velCovar(2, 1);
267  vel.covariance[8] = velCovar(2, 2);
268 
269  Matrix poseCovarTmp = ublas::prod(poseCovar, FpT);
270  poseCovarTmp = ublas::prod(Fp, poseCovarTmp);
271  poseCovar = addMatrices(poseCovarTmp, velCovar);
272 
273  pose.covariance[0] = poseCovar(0, 0);
274  pose.covariance[1] = poseCovar(0, 1);
275  pose.covariance[2] = poseCovar(0, 2);
276  pose.covariance[3] = poseCovar(1, 0);
277  pose.covariance[4] = poseCovar(1, 1);
278  pose.covariance[5] = poseCovar(1, 2);
279  pose.covariance[6] = poseCovar(2, 0);
280  pose.covariance[7] = poseCovar(2, 1);
281  pose.covariance[8] = poseCovar(2, 2);
282 
283  // Update pose
284  pose.x += deltaX;
285  pose.y += deltaY;
286  pose.yaw = util::normalizeAngle(pose.yaw + deltaYaw);
287 
288  prevOnDataTime = curTime;
289 
290  // Make user registered callbacks, if any
291  // TODO
292  }
293 
294  bool Create::connect(const std::string& port, const int& baud) {
295  bool timeout = false;
296  time_t start, now;
297  float maxWait = 30; // seconds
298  float retryInterval = 5; //seconds
299  time(&start);
300  while (!serial->connect(port, baud, std::bind(&Create::onData, this)) && !timeout) {
301  time(&now);
302  if (difftime(now, start) > maxWait) {
303  timeout = true;
304  CERR("[create::Create] ", "failed to connect over serial: timeout");
305  }
306  else {
307  usleep(retryInterval * 1000000);
308  COUT("[create::Create] ", "retrying to establish serial connection...");
309  }
310  }
311 
312  return !timeout;
313  }
314 
316  serial->disconnect();
317  firstOnData = true;
318  }
319 
320  //void Create::reset() {
321  // serial->sendOpcode(OC_RESET);
322  // serial->reset(); // better
323  // TODO : Should we request reading packets again?
324  //}
325 
326  bool Create::setMode(const CreateMode& mode) {
327  if (model.getVersion() == V_1){
328  // Switch to safe mode (required for compatibility with V_1)
329  if (!(serial->sendOpcode(OC_START) && serial->sendOpcode(OC_CONTROL))) return false;
330  }
331  bool ret = false;
332  switch (mode) {
333  case MODE_OFF:
334  if (model.getVersion() == V_2) {
335  CERR("[create::Create] ", "protocol version 2 does not support turning robot off");
336  ret = false;
337  } else {
338  ret = serial->sendOpcode(OC_POWER);
339  }
340  break;
341  case MODE_PASSIVE:
342  ret = serial->sendOpcode(OC_START);
343  break;
344  case MODE_SAFE:
345  if (model.getVersion() > V_1) {
346  ret = serial->sendOpcode(OC_SAFE);
347  }
348  break;
349  case MODE_FULL:
350  ret = serial->sendOpcode(OC_FULL);
351  break;
352  default:
353  CERR("[create::Create] ", "cannot set robot to mode '" << mode << "'");
354  ret = false;
355  }
356  if (ret) {
357  this->mode = mode;
358  }
359  return ret;
360  }
361 
362  bool Create::clean(const CleanMode& mode) {
363  return serial->sendOpcode((Opcode) mode);
364  }
365 
366  bool Create::dock() const {
367  return serial->sendOpcode(OC_DOCK);
368  }
369 
370  bool Create::setDate(const DayOfWeek& day, const uint8_t& hour, const uint8_t& min) const {
371  if (day < 0 || day > 6 ||
372  hour > 23 ||
373  min > 59)
374  return false;
375 
376  uint8_t cmd[4] = { OC_DATE, static_cast<uint8_t>(day), hour, min };
377  return serial->send(cmd, 4);
378  }
379 
380  bool Create::driveRadius(const float& vel, const float& radius) {
381  // Bound velocity
382  float boundedVel = BOUND_CONST(vel, -model.getMaxVelocity(), model.getMaxVelocity());
383 
384  // Expects each parameter as two bytes each and in millimeters
385  int16_t vel_mm = roundf(boundedVel * 1000);
386  int16_t radius_mm = roundf(radius * 1000);
387 
388  // Bound radius if not a special case
389  if (radius_mm != -32768 && radius_mm != 32767 &&
390  radius_mm != -1 && radius_mm != 1) {
391  BOUND(radius_mm, -util::MAX_RADIUS * 1000, util::MAX_RADIUS * 1000);
392  }
393 
394  uint8_t cmd[5] = { OC_DRIVE,
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)
399  };
400 
401  return serial->send(cmd, 5);
402  }
403 
404  bool Create::driveWheels(const float& leftVel, const float& rightVel) {
405  const float boundedLeftVel = BOUND_CONST(leftVel, -model.getMaxVelocity(), model.getMaxVelocity());
406  const float boundedRightVel = BOUND_CONST(rightVel, -model.getMaxVelocity(), model.getMaxVelocity());
407  requestedLeftVel = boundedLeftVel;
408  requestedRightVel = boundedRightVel;
409  if (model.getVersion() > V_1) {
410  int16_t leftCmd = roundf(boundedLeftVel * 1000);
411  int16_t rightCmd = roundf(boundedRightVel * 1000);
412 
413  uint8_t cmd[5] = { OC_DRIVE_DIRECT,
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)
418  };
419  return serial->send(cmd, 5);
420  } else {
421  float radius;
422  // Prevent divide by zero when driving straight
423  if (boundedLeftVel != boundedRightVel) {
424  radius = -((model.getAxleLength() / 2.0) * (boundedLeftVel + boundedRightVel)) /
425  (boundedLeftVel - boundedRightVel);
426  } else {
427  radius = util::STRAIGHT_RADIUS;
428  }
429 
430  float vel;
431  // Fix signs for spin in place
432  if (boundedLeftVel == -boundedRightVel || std::abs(roundf(radius * 1000)) <= 1) {
433  radius = util::IN_PLACE_RADIUS;
434  vel = boundedRightVel;
435  } else {
436  vel = (std::abs(boundedLeftVel) + std::abs(boundedRightVel)) / 2.0 * ((boundedLeftVel + boundedRightVel) > 0 ? 1.0 : -1.0);
437  }
438 
439  // Radius turns have a maximum radius of 2.0 meters
440  // When the radius is > 2 but <= 10, use a 2 meter radius
441  // When it is > 10, drive straight
442  // TODO: alternate between these 2 meter radius and straight to
443  // fake a larger radius
444  if (radius > 10.0) {
445  radius = util::STRAIGHT_RADIUS;
446  }
447 
448  return driveRadius(vel, radius);
449  }
450  }
451 
452  bool Create::driveWheelsPwm(const float& leftWheel, const float& rightWheel)
453  {
454  static const int16_t PWM_COUNTS = 255;
455 
456  if (leftWheel < -1.0 || leftWheel > 1.0 ||
457  rightWheel < -1.0 || rightWheel > 1.0)
458  return false;
459 
460  int16_t leftPwm = roundf(leftWheel * PWM_COUNTS);
461  int16_t rightPwm = roundf(rightWheel * PWM_COUNTS);
462 
463  uint8_t cmd[5] = { OC_DRIVE_PWM,
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)
468  };
469 
470  return serial->send(cmd, 5);
471  }
472 
473  bool Create::drive(const float& xVel, const float& angularVel) {
474  // Compute left and right wheel velocities
475  float leftVel = xVel - ((model.getAxleLength() / 2.0) * angularVel);
476  float rightVel = xVel + ((model.getAxleLength() / 2.0) * angularVel);
477  return driveWheels(leftVel, rightVel);
478  }
479 
480  bool Create::setAllMotors(const float& main, const float& side, const float& vacuum) {
481  if (main < -1.0 || main > 1.0 ||
482  side < -1.0 || side > 1.0 ||
483  vacuum < -1.0 || vacuum > 1.0)
484  return false;
485 
486  mainMotorPower = roundf(main * 127);
487  sideMotorPower = roundf(side * 127);
488  vacuumMotorPower = roundf(vacuum * 127);
489 
490  if (model.getVersion() == V_1) {
491  uint8_t cmd[2] = { OC_MOTORS,
492  static_cast<uint8_t>((side != 0.0 ? 1 : 0) |
493  (vacuum != 0.0 ? 2 : 0) |
494  (main != 0.0 ? 4 : 0))
495  };
496  return serial->send(cmd, 2);
497  }
498 
499  uint8_t cmd[4] = { OC_MOTORS_PWM,
503  };
504 
505  return serial->send(cmd, 4);
506  }
507 
508  bool Create::setMainMotor(const float& main) {
509  return setAllMotors(main, static_cast<float>(sideMotorPower) / 127.0, static_cast<float>(vacuumMotorPower) / 127.0);
510  }
511 
512  bool Create::setSideMotor(const float& side) {
513  return setAllMotors(static_cast<float>(mainMotorPower) / 127.0, side, static_cast<float>(vacuumMotorPower) / 127.0);
514  }
515 
516  bool Create::setVacuumMotor(const float& vacuum) {
517  return setAllMotors(static_cast<float>(mainMotorPower) / 127.0, static_cast<float>(sideMotorPower) / 127.0, vacuum);
518  }
519 
521  uint8_t LEDByte = debrisLED + spotLED + dockLED + checkLED;
522  uint8_t cmd[4] = { OC_LEDS,
523  LEDByte,
524  powerLED,
526  };
527 
528  return serial->send(cmd, 4);
529  }
530 
531  bool Create::enableDebrisLED(const bool& enable) {
532  if (enable)
534  else
535  debrisLED = 0;
536  return updateLEDs();
537  }
538 
539  bool Create::enableSpotLED(const bool& enable) {
540  if (enable)
541  spotLED = LED_SPOT;
542  else
543  spotLED = 0;
544  return updateLEDs();
545  }
546 
547  bool Create::enableDockLED(const bool& enable) {
548  if (enable)
549  dockLED = LED_DOCK;
550  else
551  dockLED = 0;
552  return updateLEDs();
553  }
554 
555  bool Create::enableCheckRobotLED(const bool& enable) {
556  if (enable)
558  else
559  checkLED = 0;
560  return updateLEDs();
561  }
562 
563  bool Create::setPowerLED(const uint8_t& power, const uint8_t& intensity) {
564  powerLED = power;
565  powerLEDIntensity = intensity;
566  return updateLEDs();
567  }
568 
569  //void Create::setDigits(uint8_t digit1, uint8_t digit2,
570  // uint8_t digit3, uint8_t digit4) {
571  //}
572 
573  bool Create::setDigitsASCII(const uint8_t& digit1, const uint8_t& digit2,
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)
579  return false;
580 
581  uint8_t cmd[5] = { OC_DIGIT_LEDS_ASCII,
582  digit1,
583  digit2,
584  digit3,
585  digit4
586  };
587 
588  return serial->send(cmd, 5);
589  }
590 
591  bool Create::defineSong(const uint8_t& songNumber,
592  const uint8_t& songLength,
593  const uint8_t* notes,
594  const float* durations) const {
595  int i, j;
596  uint8_t duration;
597  std::vector<uint8_t> cmd(2 * songLength + 3);
598  cmd[0] = OC_SONG;
599  cmd[1] = songNumber;
600  cmd[2] = songLength;
601  j = 0;
602  for (i = 3; i < 2 * songLength + 3; i = i + 2) {
603  if (durations[j] < 0 || durations[j] >= 4)
604  return false;
605  duration = durations[j] * 64;
606  cmd[i] = notes[j];
607  cmd[i + 1] = duration;
608  j++;
609  }
610 
611  return serial->send(cmd.data(), cmd.size());
612  }
613 
614  bool Create::playSong(const uint8_t& songNumber) const {
615  if (songNumber > 4)
616  return false;
617  uint8_t cmd[2] = { OC_PLAY, songNumber };
618  return serial->send(cmd, 2);
619  }
620 
621  void Create::setDtHistoryLength(const uint8_t& dtHistoryLength) {
622  this->dtHistoryLength = dtHistoryLength;
623  }
624 
625  bool Create::isWheeldrop() const {
626  if (data->isValidPacketID(ID_BUMP_WHEELDROP)) {
627  return (GET_DATA(ID_BUMP_WHEELDROP) & 0x0C) != 0;
628  }
629  else {
630  CERR("[create::Create] ", "Wheeldrop sensor not supported!");
631  return false;
632  }
633  }
634 
635  bool Create::isLeftWheeldrop() const {
636  if (data->isValidPacketID(ID_BUMP_WHEELDROP)) {
637  return (GET_DATA(ID_BUMP_WHEELDROP) & 0x08) != 0;
638  }
639  else {
640  CERR("[create::Create] ", "Wheeldrop sensor not supported!");
641  return false;
642  }
643  }
644 
646  if (data->isValidPacketID(ID_BUMP_WHEELDROP)) {
647  return (GET_DATA(ID_BUMP_WHEELDROP) & 0x04) != 0;
648  }
649  else {
650  CERR("[create::Create] ", "Wheeldrop sensor not supported!");
651  return false;
652  }
653  }
654 
655  bool Create::isLeftBumper() const {
656  if (data->isValidPacketID(ID_BUMP_WHEELDROP)) {
657  return (GET_DATA(ID_BUMP_WHEELDROP) & 0x02) != 0;
658  }
659  else {
660  CERR("[create::Create] ", "Left bumper not supported!");
661  return false;
662  }
663  }
664 
665  bool Create::isRightBumper() const {
666  if (data->isValidPacketID(ID_BUMP_WHEELDROP)) {
667  return (GET_DATA(ID_BUMP_WHEELDROP) & 0x01) != 0;
668  }
669  else {
670  CERR("[create::Create] ", "Right bumper not supported!");
671  return false;
672  }
673  }
674 
675  bool Create::isWall() const {
676  if (data->isValidPacketID(ID_WALL)) {
677  return GET_DATA(ID_WALL) == 1;
678  }
679  else {
680  CERR("[create::Create] ", "Wall sensor not supported!");
681  return false;
682  }
683  }
684 
685  bool Create::isCliff() const {
686  if (data->isValidPacketID(ID_CLIFF_LEFT) &&
687  data->isValidPacketID(ID_CLIFF_FRONT_LEFT) &&
688  data->isValidPacketID(ID_CLIFF_FRONT_RIGHT) &&
689  data->isValidPacketID(ID_CLIFF_RIGHT)) {
690  return GET_DATA(ID_CLIFF_LEFT) == 1 ||
693  GET_DATA(ID_CLIFF_RIGHT) == 1;
694  }
695  else {
696  CERR("[create::Create] ", "Cliff sensors not supported!");
697  return false;
698  }
699  }
700 
701  bool Create::isCliffLeft() const {
702  if (data->isValidPacketID(ID_CLIFF_LEFT)) {
703  return GET_DATA(ID_CLIFF_LEFT) == 1;
704  }
705  else {
706  CERR("[create::Create] ", "Left cliff sensors not supported!");
707  return false;
708  }
709  }
710 
712  if (data->isValidPacketID(ID_CLIFF_FRONT_LEFT)) {
713  return GET_DATA(ID_CLIFF_FRONT_LEFT) == 1;
714  }
715  else {
716  CERR("[create::Create] ", "Front left cliff sensors not supported!");
717  return false;
718  }
719  }
720 
721  bool Create::isCliffRight() const {
722  if (data->isValidPacketID(ID_CLIFF_RIGHT)) {
723  return GET_DATA(ID_CLIFF_RIGHT) == 1;
724  }
725  else {
726  CERR("[create::Create] ", "Rightt cliff sensors not supported!");
727  return false;
728  }
729  }
730 
732  if (data->isValidPacketID(ID_CLIFF_FRONT_RIGHT)) {
733  return GET_DATA(ID_CLIFF_FRONT_RIGHT) == 1;
734  }
735  else {
736  CERR("[create::Create] ", "Front right cliff sensors not supported!");
737  return false;
738  }
739  }
740 
741  bool Create::isVirtualWall() const {
742  if (data->isValidPacketID(ID_VIRTUAL_WALL)) {
743  return GET_DATA(ID_VIRTUAL_WALL);
744  }
745  else {
746  CERR("[create::Create] ", "Virtual Wall sensor not supported!");
747  return false;
748  }
749  }
750 
751  uint8_t Create::getDirtDetect() const {
752  if (data->isValidPacketID(ID_DIRT_DETECT_LEFT)) {
754  }
755  else {
756  CERR("[create::Create] ", "Dirt detector not supported!");
757  return -1;
758  }
759  }
760 
761  uint8_t Create::getIROmni() const {
762  if (data->isValidPacketID(ID_IR_OMNI)) {
763  return GET_DATA(ID_IR_OMNI);
764  }
765  else {
766  CERR("[create::Create] ", "Omni IR sensor not supported!");
767  return -1;
768  }
769  }
770 
771  uint8_t Create::getIRLeft() const {
772  if (data->isValidPacketID(ID_IR_LEFT)) {
773  return GET_DATA(ID_IR_LEFT);
774  }
775  else {
776  CERR("[create::Create] ", "Left IR sensor not supported!");
777  return -1;
778  }
779  }
780 
781  uint8_t Create::getIRRight() const {
782  if (data->isValidPacketID(ID_IR_RIGHT)) {
783  return GET_DATA(ID_IR_RIGHT);
784  }
785  else {
786  CERR("[create::Create] ", "Right IR sensor not supported!");
787  return -1;
788  }
789  }
790 
792  if (data->isValidPacketID(ID_CHARGE_STATE)) {
793  uint8_t chargeState = GET_DATA(ID_CHARGE_STATE);
794  assert(chargeState <= 5);
795  return (ChargingState) chargeState;
796  }
797  else {
798  CERR("[create::Create] ", "Charging state not supported!");
799  return CHARGE_FAULT;
800  }
801  }
802 
804  if (data->isValidPacketID(ID_BUTTONS)) {
805  return (GET_DATA(ID_BUTTONS) & 0x01) != 0;
806  }
807  else {
808  CERR("[create::Create] ", "Buttons not supported!");
809  return false;
810  }
811  }
812 
813  // Not supported by any 600 series firmware
815  CERR("[create::Create] ", "Clock button is not supported!");
816  if (data->isValidPacketID(ID_BUTTONS)) {
817  return (GET_DATA(ID_BUTTONS) & 0x80) != 0;
818  }
819  else {
820  CERR("[create::Create] ", "Buttons not supported!");
821  return false;
822  }
823  }
824 
825  // Not supported by any 600 series firmware
827  CERR("[create::Create] ", "Schedule button is not supported!");
828  if (data->isValidPacketID(ID_BUTTONS)) {
829  return (GET_DATA(ID_BUTTONS) & 0x40) != 0;
830  }
831  else {
832  CERR("[create::Create] ", "Buttons not supported!");
833  return false;
834  }
835  }
836 
838  if (data->isValidPacketID(ID_BUTTONS)) {
839  return (GET_DATA(ID_BUTTONS) & 0x20) != 0;
840  }
841  else {
842  CERR("[create::Create] ", "Buttons not supported!");
843  return false;
844  }
845  }
846 
848  if (data->isValidPacketID(ID_BUTTONS)) {
849  return (GET_DATA(ID_BUTTONS) & 0x10) != 0;
850  }
851  else {
852  CERR("[create::Create] ", "Buttons not supported!");
853  return false;
854  }
855  }
856 
858  if (data->isValidPacketID(ID_BUTTONS)) {
859  return (GET_DATA(ID_BUTTONS) & 0x08) != 0;
860  }
861  else {
862  CERR("[create::Create] ", "Buttons not supported!");
863  return false;
864  }
865  }
866 
868  if (data->isValidPacketID(ID_BUTTONS)) {
869  return (GET_DATA(ID_BUTTONS) & 0x04) != 0;
870  }
871  else {
872  CERR("[create::Create] ", "Buttons not supported!");
873  return false;
874  }
875  }
876 
878  if (data->isValidPacketID(ID_BUTTONS)) {
879  return (GET_DATA(ID_BUTTONS) & 0x02) != 0;
880  }
881  else {
882  CERR("[create::Create] ", "Buttons not supported!");
883  return false;
884  }
885  }
886 
887  float Create::getVoltage() const {
888  if (data->isValidPacketID(ID_VOLTAGE)) {
889  return (GET_DATA(ID_VOLTAGE) / 1000.0);
890  }
891  else {
892  CERR("[create::Create] ", "Voltage sensor not supported!");
893  return 0;
894  }
895  }
896 
897  float Create::getCurrent() const {
898  if (data->isValidPacketID(ID_VOLTAGE)) {
899  return (((int16_t)GET_DATA(ID_CURRENT)) / 1000.0);
900  }
901  else {
902  CERR("[create::Create] ", "Current sensor not supported!");
903  return 0;
904  }
905  }
906 
907  int8_t Create::getTemperature() const {
908  if (data->isValidPacketID(ID_TEMP)) {
909  return (int8_t) GET_DATA(ID_TEMP);
910  }
911  else {
912  CERR("[create::Create] ", "Temperature sensor not supported!");
913  return 0;
914  }
915  }
916 
917  float Create::getBatteryCharge() const {
918  if (data->isValidPacketID(ID_CHARGE)) {
919  return (GET_DATA(ID_CHARGE) / 1000.0);
920  }
921  else {
922  CERR("[create::Create] ", "Battery charge not supported!");
923  return 0;
924  }
925  }
926 
928  if (data->isValidPacketID(ID_CAPACITY)) {
929  return (GET_DATA(ID_CAPACITY) / 1000.0);
930  }
931  else {
932  CERR("[create::Create] ", "Battery capacity not supported!");
933  return 0;
934  }
935  }
936 
938  if (data->isValidPacketID(ID_LIGHT)) {
939  return (GET_DATA(ID_LIGHT) & 0x01) != 0;
940  }
941  else {
942  CERR("[create::Create] ", "Light sensors not supported!");
943  return false;
944  }
945  }
946 
948  if (data->isValidPacketID(ID_LIGHT)) {
949  return (GET_DATA(ID_LIGHT) & 0x02) != 0;
950  }
951  else {
952  CERR("[create::Create] ", "Light sensors not supported!");
953  return false;
954  }
955  }
956 
958  if (data->isValidPacketID(ID_LIGHT)) {
959  return (GET_DATA(ID_LIGHT) & 0x04) != 0;
960  }
961  else {
962  CERR("[create::Create] ", "Light sensors not supported!");
963  return false;
964  }
965  }
966 
968  if (data->isValidPacketID(ID_LIGHT)) {
969  return (GET_DATA(ID_LIGHT) & 0x08) != 0;
970  }
971  else {
972  CERR("[create::Create] ", "Light sensors not supported!");
973  return false;
974  }
975  }
976 
978  if (data->isValidPacketID(ID_LIGHT)) {
979  return (GET_DATA(ID_LIGHT) & 0x10) != 0;
980  }
981  else {
982  CERR("[create::Create] ", "Light sensors not supported!");
983  return false;
984  }
985  }
986 
988  if (data->isValidPacketID(ID_LIGHT)) {
989  return (GET_DATA(ID_LIGHT) & 0x20) != 0;
990  }
991  else {
992  CERR("[create::Create] ", "Light sensors not supported!");
993  return false;
994  }
995  }
996 
997  uint16_t Create::getLightSignalLeft() const {
998  if (data->isValidPacketID(ID_LIGHT_LEFT)) {
999  return GET_DATA(ID_LIGHT_LEFT);
1000  }
1001  else {
1002  CERR("[create::Create] ", "Light sensors not supported!");
1003  return 0;
1004  }
1005  }
1006 
1008  if (data->isValidPacketID(ID_LIGHT_FRONT_LEFT)) {
1009  return GET_DATA(ID_LIGHT_FRONT_LEFT);
1010  }
1011  else {
1012  CERR("[create::Create] ", "Light sensors not supported!");
1013  return 0;
1014  }
1015  }
1016 
1018  if (data->isValidPacketID(ID_LIGHT_CENTER_LEFT)) {
1020  }
1021  else {
1022  CERR("[create::Create] ", "Light sensors not supported!");
1023  return 0;
1024  }
1025  }
1026 
1027  uint16_t Create::getLightSignalRight() const {
1028  if (data->isValidPacketID(ID_LIGHT_RIGHT)) {
1029  return GET_DATA(ID_LIGHT_RIGHT);
1030  }
1031  else {
1032  CERR("[create::Create] ", "Light sensors not supported!");
1033  return 0;
1034  }
1035  }
1036 
1038  if (data->isValidPacketID(ID_LIGHT_FRONT_RIGHT)) {
1040  }
1041  else {
1042  CERR("[create::Create] ", "Light sensors not supported!");
1043  return 0;
1044  }
1045  }
1046 
1048  if (data->isValidPacketID(ID_LIGHT_CENTER_RIGHT)) {
1050  }
1051  else {
1052  CERR("[create::Create] ", "Light sensors not supported!");
1053  return 0;
1054  }
1055  }
1056 
1058  if (data->isValidPacketID(ID_STASIS)) {
1059  return GET_DATA(ID_STASIS) == 1;
1060  }
1061  else {
1062  CERR("[create::Create] ", "Stasis sensor not supported!");
1063  return false;
1064  }
1065  }
1066 
1068  if (data->isValidPacketID(ID_OVERCURRENTS)) {
1069  return (GET_DATA(ID_OVERCURRENTS) & 0x01) != 0;
1070  }
1071  else {
1072  CERR("[create::Create] ", "Overcurrent sensor not supported!");
1073  return false;
1074  }
1075  }
1076 
1078  if (data->isValidPacketID(ID_OVERCURRENTS)) {
1079  return (GET_DATA(ID_OVERCURRENTS) & 0x04) != 0;
1080  }
1081  else {
1082  CERR("[create::Create] ", "Overcurrent sensor not supported!");
1083  return false;
1084  }
1085  }
1086 
1088  if (data->isValidPacketID(ID_OVERCURRENTS)) {
1089  return (GET_DATA(ID_OVERCURRENTS) & 0x18) != 0;
1090  }
1091  else {
1092  CERR("[create::Create] ", "Overcurrent sensor not supported!");
1093  return false;
1094  }
1095  }
1096 
1098  return totalLeftDist;
1099  }
1100 
1102  return totalRightDist;
1103  }
1104 
1106  return measuredLeftVel;
1107  }
1108 
1110  return measuredRightVel;
1111  }
1112 
1114  return requestedLeftVel;
1115  }
1116 
1118  return requestedRightVel;
1119  }
1120 
1121  void Create::setModeReportWorkaround(const bool& enable) {
1122  modeReportWorkaround = enable;
1123  }
1124 
1126  return modeReportWorkaround;
1127  }
1128 
1130  if (data->isValidPacketID(ID_OI_MODE)) {
1131  if (modeReportWorkaround) {
1133  } else {
1135  }
1136  }
1137 
1138  return mode;
1139  }
1140 
1142  return pose;
1143  }
1144 
1146  return vel;
1147  }
1148 
1149  uint64_t Create::getNumCorruptPackets() const {
1150  return serial->getNumCorruptPackets();
1151  }
1152 
1153  uint64_t Create::getTotalPackets() const {
1154  return serial->getTotalPackets();
1155  }
1156 
1157 } // end namespace
create::Create::measuredLeftVel
float measuredLeftVel
Definition: create.h:90
create::Create::getLeftWheelDistance
float getLeftWheelDistance() const
Get the total distance the left wheel has moved.
Definition: create.cpp:1097
create::Create::isSpotButtonPressed
bool isSpotButtonPressed() const
Get state of 'spot' button.
Definition: create.cpp:877
create::Create::updateLEDs
bool updateLEDs()
Definition: create.cpp:520
create::Create::~Create
~Create()
Attempts to disconnect from serial.
Definition: create.cpp:66
create::Create::isHourButtonPressed
bool isHourButtonPressed() const
Get state of 'hour' button.
Definition: create.cpp:847
create::Create::isDockButtonPressed
bool isDockButtonPressed() const
Get state of 'dock' button ('advance' button on Create 1).
Definition: create.cpp:867
create::Create::setAllMotors
bool setAllMotors(const float &mainPower, const float &sidePower, const float &vacuumPower)
Set the power of all motors.
Definition: create.cpp:480
create::OC_DRIVE
@ OC_DRIVE
Definition: types.h:169
create::Create::setMode
bool setMode(const create::CreateMode &mode)
Change Create mode.
Definition: create.cpp:326
create::ID_DISTANCE
@ ID_DISTANCE
Definition: types.h:113
create::Create::requestedRightVel
float requestedRightVel
Definition: create.h:93
create::Create::checkLED
uint8_t checkLED
Definition: create.h:70
create::ID_LIGHT_LEFT
@ ID_LIGHT_LEFT
Definition: types.h:140
create::Create::mainMotorPower
uint8_t mainMotorPower
Definition: create.h:62
create::Create::isCliffFrontLeft
bool isCliffFrontLeft() const
Definition: create.cpp:711
create::ID_CLIFF_LEFT
@ ID_CLIFF_LEFT
Definition: types.h:101
create::Create::powerLED
uint8_t powerLED
Definition: create.h:71
create::Create::getRequestedLeftWheelVel
float getRequestedLeftWheelVel() const
Get the requested velocity of the left wheel. This value is bounded at the maximum velocity of the ro...
Definition: create.cpp:1113
create::V_3
@ V_3
Definition: types.h:44
create::OC_FULL
@ OC_FULL
Definition: types.h:161
create::Create::isMainBrushOvercurrent
bool isMainBrushOvercurrent() const
Definition: create.cpp:1077
create::Create::vel
create::Vel vel
Definition: create.h:77
create::Create::data
std::shared_ptr< create::Data > data
Definition: create.h:106
create::ID_CLIFF_FRONT_RIGHT
@ ID_CLIFF_FRONT_RIGHT
Definition: types.h:103
create::util::STRAIGHT_RADIUS
static const float STRAIGHT_RADIUS
Definition: util.h:47
create::Create::driveWheelsPwm
bool driveWheelsPwm(const float &leftWheel, const float &rightWheel)
Set the direct for the left and right wheels.
Definition: create.cpp:452
create::ID_CLIFF_RIGHT
@ ID_CLIFF_RIGHT
Definition: types.h:104
create::Create::getLightSignalFrontLeft
uint16_t getLightSignalFrontLeft() const
Get the signal strength from the front-left light sensor.
Definition: create.cpp:1007
create::Create::dtHistory
std::deque< float > dtHistory
Definition: create.h:85
create::Create::isLightBumperRight
bool isLightBumperRight() const
Definition: create.cpp:987
create::Create::getIROmni
uint8_t getIROmni() const
Get value of 8-bit IR character currently being received by omnidirectional sensor.
Definition: create.cpp:761
create::Create::setModeReportWorkaround
void setModeReportWorkaround(const bool &enable)
Enable or disable the mode reporting workaround. Some Roomba 6xx robots incorrectly report the OI mod...
Definition: create.cpp:1121
create::Create::Create
Create(RobotModel model=RobotModel::CREATE_2, bool install_signal_handler=true)
Default constructor.
Definition: create.cpp:55
create::Create::setDtHistoryLength
void setDtHistoryLength(const uint8_t &dtHistoryLength)
Set dtHistoryLength parameter. Used to configure the size of the buffer for calculating average time ...
Definition: create.cpp:621
create::ID_RIGHT_ENC
@ ID_RIGHT_ENC
Definition: types.h:138
create::Create::measuredRightVel
float measuredRightVel
Definition: create.h:91
create::CleanMode
CleanMode
Definition: types.h:210
create::MODE_SAFE
@ MODE_SAFE
Definition: types.h:205
create::Create::modeReportWorkaround
bool modeReportWorkaround
Definition: create.h:103
create::Create::isCliff
bool isCliff() const
Definition: create.cpp:685
create::Create::addMatrices
Matrix addMatrices(const Matrix &A, const Matrix &B) const
Definition: create.cpp:70
create::ID_VIRTUAL_WALL
@ ID_VIRTUAL_WALL
Definition: types.h:105
create::ID_LEFT_ENC
@ ID_LEFT_ENC
Definition: types.h:137
create::Create::Matrix
boost::numeric::ublas::matrix< float > Matrix
Definition: create.h:51
create::Create::pose
create::Pose pose
Definition: create.h:76
create.h
BOUND_CONST
#define BOUND_CONST(val, min, max)
Definition: create.cpp:10
create::ID_DIRT_DETECT_LEFT
@ ID_DIRT_DETECT_LEFT
Definition: types.h:107
create::Create::LED_DEBRIS
@ LED_DEBRIS
Definition: create.h:54
create::Create::firstOnData
bool firstOnData
Definition: create.h:83
create::Create::powerLEDIntensity
uint8_t powerLEDIntensity
Definition: create.h:72
create::OC_DRIVE_DIRECT
@ OC_DRIVE_DIRECT
Definition: types.h:170
create::Pose::yaw
float yaw
Definition: types.h:278
create::Create::setMainMotor
bool setMainMotor(const float &power)
Set the power to the main brush motor.
Definition: create.cpp:508
create::Create::isClockButtonPressed
bool isClockButtonPressed() const
Not supported by any firmware!
Definition: create.cpp:814
create::Create::isDayButtonPressed
bool isDayButtonPressed() const
Get state of 'day' button.
Definition: create.cpp:837
create::ID_LIGHT_FRONT_RIGHT
@ ID_LIGHT_FRONT_RIGHT
Definition: types.h:144
create::RobotModel::getWheelDiameter
float getWheelDiameter() const
Definition: types.cpp:43
create::Create::isRightWheeldrop
bool isRightWheeldrop() const
Definition: create.cpp:645
create::Create::isWall
bool isWall() const
Definition: create.cpp:675
create::Create::isCliffLeft
bool isCliffLeft() const
Definition: create.cpp:701
CERR
#define CERR(prefix, msg)
Definition: util.h:38
create::OC_MOTORS_PWM
@ OC_MOTORS_PWM
Definition: types.h:173
create::CreateMode
CreateMode
Definition: types.h:202
create::Create::setDate
bool setDate(const create::DayOfWeek &day, const uint8_t &hour, const uint8_t &min) const
Sets the internal clock of Create.
Definition: create.cpp:370
create::Create::clean
bool clean(const create::CleanMode &mode=CLEAN_DEFAULT)
Starts a cleaning mode. Changes mode to MODE_PASSIVE.
Definition: create.cpp:362
create::OC_DATE
@ OC_DATE
Definition: types.h:168
create::ID_LIGHT_RIGHT
@ ID_LIGHT_RIGHT
Definition: types.h:145
create::ID_IR_RIGHT
@ ID_IR_RIGHT
Definition: types.h:111
create::Create::totalLeftDist
float totalLeftDist
Definition: create.h:81
create::Create::getLightSignalCenterLeft
uint16_t getLightSignalCenterLeft() const
Get the signal strength from the center-left light sensor.
Definition: create.cpp:1017
create::ID_OI_MODE
@ ID_OI_MODE
Definition: types.h:129
create::Create::enableDockLED
bool enableDockLED(const bool &enable)
Set the green "dock" LED on/off.
Definition: create.cpp:547
create::Create::serial
std::shared_ptr< create::Serial > serial
Definition: create.h:107
create::Create::LED_CHECK
@ LED_CHECK
Definition: create.h:57
create::RobotModel::getMaxVelocity
float getMaxVelocity() const
Definition: types.cpp:39
create::Create::isSideBrushOvercurrent
bool isSideBrushOvercurrent() const
Definition: create.cpp:1067
create::Pose
Represents a robot pose.
Definition: types.h:275
create::Create::prevTicksRight
uint32_t prevTicksRight
Definition: create.h:80
create::Create::enableSpotLED
bool enableSpotLED(const bool &enable)
Set the green "spot" LED on/off.
Definition: create.cpp:539
create::Create::getPose
create::Pose getPose() const
Get the estimated pose of Create based on wheel encoders.
Definition: create.cpp:1141
create::util::STREAM_HEADER
static const uint8_t STREAM_HEADER
Definition: util.h:43
create::Create::spotLED
uint8_t spotLED
Definition: create.h:68
create::Create::LED_SPOT
@ LED_SPOT
Definition: create.h:55
create::Create::driveWheels
bool driveWheels(const float &leftWheel, const float &rightWheel)
Set the velocities for the left and right wheels.
Definition: create.cpp:404
create::Create::prevTicksLeft
uint32_t prevTicksLeft
Definition: create.h:79
create::Create::getTemperature
int8_t getTemperature() const
Get the temperature of battery.
Definition: create.cpp:907
create::ID_CAPACITY
@ ID_CAPACITY
Definition: types.h:120
create::ID_LIGHT
@ ID_LIGHT
Definition: types.h:139
create::ID_CLIFF_FRONT_LEFT
@ ID_CLIFF_FRONT_LEFT
Definition: types.h:102
create::Create::getLightSignalRight
uint16_t getLightSignalRight() const
Get the signal strength from the right light sensor.
Definition: create.cpp:1027
create::Create::getMeasuredRightWheelVel
float getMeasuredRightWheelVel() const
Get the measured velocity of the right wheel.
Definition: create.cpp:1109
create::MODE_PASSIVE
@ MODE_PASSIVE
Definition: types.h:204
create::Create::isLightBumperCenterLeft
bool isLightBumperCenterLeft() const
Definition: create.cpp:957
BOUND
#define BOUND(val, min, max)
Definition: create.cpp:11
create::RobotModel::getVersion
ProtocolVersion getVersion() const
Definition: types.cpp:27
create::ID_OVERCURRENTS
@ ID_OVERCURRENTS
Definition: types.h:106
COUT
#define COUT(prefix, msg)
Definition: util.h:37
create::Create::setVacuumMotor
bool setVacuumMotor(const float &power)
Set the power to the vacuum motor.
Definition: create.cpp:516
create::DayOfWeek
DayOfWeek
Definition: types.h:225
create::Create::getLightSignalFrontRight
uint16_t getLightSignalFrontRight() const
Get the signal strength from the front-right light sensor.
Definition: create.cpp:1037
create::ID_ANGLE
@ ID_ANGLE
Definition: types.h:114
create::Create::drive
bool drive(const float &xVel, const float &angularVel)
Set the forward and angular velocity of Create.
Definition: create.cpp:473
create::Create::dock
bool dock() const
Starts the docking behaviour. Changes mode to MODE_PASSIVE.
Definition: create.cpp:366
create::util::normalizeAngle
float normalizeAngle(const float &angle)
Definition: util.h:53
create::ID_CHARGE_STATE
@ ID_CHARGE_STATE
Definition: types.h:115
create::Create::enableCheckRobotLED
bool enableCheckRobotLED(const bool &enable)
Set the orange "check Create" LED on/off.
Definition: create.cpp:555
create::OC_DOCK
@ OC_DOCK
Definition: types.h:165
create::Create::isLightBumperFrontRight
bool isLightBumperFrontRight() const
Definition: create.cpp:977
create::Create::isCliffRight
bool isCliffRight() const
Definition: create.cpp:721
create::ID_BUMP_WHEELDROP
@ ID_BUMP_WHEELDROP
Definition: types.h:99
create::Create::getLightSignalCenterRight
uint16_t getLightSignalCenterRight() const
Get the signal strength from the center-right light sensor.
Definition: create.cpp:1047
create::V_1
@ V_1
Definition: types.h:42
create::Create::getTotalPackets
uint64_t getTotalPackets() const
Get the total number of serial packets received (including corrupt packets) since first connecting to...
Definition: create.cpp:1153
create::CHARGE_FAULT
@ CHARGE_FAULT
Definition: types.h:222
create::Create::getModeReportWorkaround
bool getModeReportWorkaround() const
Definition: create.cpp:1125
create::Create::isMinButtonPressed
bool isMinButtonPressed() const
Get state of 'min' button.
Definition: create.cpp:857
create::Create::isWheelOvercurrent
bool isWheelOvercurrent() const
Definition: create.cpp:1087
create::Create::onData
void onData()
Definition: create.cpp:94
create::Create::dtHistoryLength
uint8_t dtHistoryLength
Definition: create.h:86
create::OC_SONG
@ OC_SONG
Definition: types.h:179
create::Create::debrisLED
uint8_t debrisLED
Definition: create.h:67
create::ChargingState
ChargingState
Definition: types.h:216
create::OC_PLAY
@ OC_PLAY
Definition: types.h:180
create::Create::getNumCorruptPackets
uint64_t getNumCorruptPackets() const
Get the number of corrupt serial packets since first connecting to Create. This value is ideally zero...
Definition: create.cpp:1149
create::Create::setPowerLED
bool setPowerLED(const uint8_t &power, const uint8_t &intensity=255)
Set the center power LED.
Definition: create.cpp:563
create::V_2
@ V_2
Definition: types.h:43
create::OC_POWER
@ OC_POWER
Definition: types.h:166
create::util::MAX_RADIUS
static const float MAX_RADIUS
Definition: util.h:46
create::Create::getRequestedRightWheelVel
float getRequestedRightWheelVel() const
Get the requested velocity of the right wheel. This value is bounded at the maximum velocity of the r...
Definition: create.cpp:1117
create::Create::getVoltage
float getVoltage() const
Get battery voltage.
Definition: create.cpp:887
create::Create::enableDebrisLED
bool enableDebrisLED(const bool &enable)
Set the blue "debris" LED on/off.
Definition: create.cpp:531
create::OC_START
@ OC_START
Definition: types.h:155
create::Create::getCurrent
float getCurrent() const
Get current flowing in/out of battery. A positive current implies Create is charging.
Definition: create.cpp:897
create::MODE_OFF
@ MODE_OFF
Definition: types.h:203
create::ID_IR_OMNI
@ ID_IR_OMNI
Definition: types.h:109
create::Create::isLeftBumper
bool isLeftBumper() const
Definition: create.cpp:655
create::ID_LIGHT_CENTER_RIGHT
@ ID_LIGHT_CENTER_RIGHT
Definition: types.h:143
create::util::PI
static const float PI
Definition: util.h:49
create::util::willFloatOverflow
bool willFloatOverflow(const float a, const float b)
Definition: util.h:60
create::Create::isRightBumper
bool isRightBumper() const
Definition: create.cpp:665
create::ID_CURRENT
@ ID_CURRENT
Definition: types.h:117
create::Create::model
RobotModel model
Definition: create.h:60
create::Create::isLightBumperLeft
bool isLightBumperLeft() const
Definition: create.cpp:937
create::util::V_3_TICKS_PER_REV
static const float V_3_TICKS_PER_REV
Definition: util.h:44
create::Create::getRightWheelDistance
float getRightWheelDistance() const
Get the total distance the right wheel has moved.
Definition: create.cpp:1101
create::ID_LIGHT_CENTER_LEFT
@ ID_LIGHT_CENTER_LEFT
Definition: types.h:142
create::Create::isMovingForward
bool isMovingForward() const
Definition: create.cpp:1057
create::Create::mode
CreateMode mode
Definition: create.h:74
create::Create::setDigitsASCII
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...
Definition: create.cpp:573
create::ID_BUTTONS
@ ID_BUTTONS
Definition: types.h:112
create::Data
Definition: data.h:43
create::Create::getBatteryCharge
float getBatteryCharge() const
Get battery's remaining charge.
Definition: create.cpp:917
create::MODE_FULL
@ MODE_FULL
Definition: types.h:206
create::Create::isCleanButtonPressed
bool isCleanButtonPressed() const
Get state of 'clean' button ('play' button on Create 1).
Definition: create.cpp:803
create::Create::isLightBumperCenterRight
bool isLightBumperCenterRight() const
Definition: create.cpp:967
create::ID_TEMP
@ ID_TEMP
Definition: types.h:118
create::ID_STASIS
@ ID_STASIS
Definition: types.h:150
create::Create::isLightBumperFrontLeft
bool isLightBumperFrontLeft() const
Definition: create.cpp:947
create::Create::LED_DOCK
@ LED_DOCK
Definition: create.h:56
create::Create::poseCovar
Matrix poseCovar
Definition: create.h:88
create::ID_CHARGE
@ ID_CHARGE
Definition: types.h:119
create::Create::getVel
create::Vel getVel() const
Get the estimated velocity of Create based on wheel encoders.
Definition: create.cpp:1145
create::Create::getIRRight
uint8_t getIRRight() const
Get value of 8-bit IR character currently being received by right sensor.
Definition: create.cpp:781
create::OC_MOTORS
@ OC_MOTORS
Definition: types.h:172
create::OC_LEDS
@ OC_LEDS
Definition: types.h:174
create::RobotModel::getAxleLength
float getAxleLength() const
Definition: types.cpp:31
GET_DATA
#define GET_DATA(id)
Definition: create.cpp:9
create::Create::disconnect
void disconnect()
Disconnect from serial.
Definition: create.cpp:315
create::Create::driveRadius
bool driveRadius(const float &velocity, const float &radius)
Set the average wheel velocity and turning radius of Create.
Definition: create.cpp:380
create::Create::getDirtDetect
uint8_t getDirtDetect() const
Get level of the dirt detect sensor.
Definition: create.cpp:751
create::Pose::covariance
std::vector< float > covariance
3x3 covariance matrix in row-major order.
Definition: types.h:283
main
int main(int argc, char **argv)
Definition: battery_level.cpp:35
create::Create::prevOnDataTime
std::chrono::time_point< std::chrono::steady_clock > prevOnDataTime
Definition: create.h:84
create::ID_VOLTAGE
@ ID_VOLTAGE
Definition: types.h:116
create::ID_LIGHT_FRONT_LEFT
@ ID_LIGHT_FRONT_LEFT
Definition: types.h:141
create::Create::defineSong
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.
Definition: create.cpp:591
create::Pose::y
float y
Definition: types.h:277
create::util::V_3_MAX_ENCODER_TICKS
static const uint32_t V_3_MAX_ENCODER_TICKS
Definition: util.h:45
create::Create::getMode
create::CreateMode getMode()
Get the current mode reported by Create.
Definition: create.cpp:1129
create::Create::totalRightDist
float totalRightDist
Definition: create.h:82
create::OC_CONTROL
@ OC_CONTROL
Definition: types.h:159
create::Create::connect
bool connect(const std::string &port, const int &baud)
Make a serial connection to Create.
Definition: create.cpp:294
create::Create::getMeasuredLeftWheelVel
float getMeasuredLeftWheelVel() const
Get the measured velocity of the left wheel.
Definition: create.cpp:1105
create::Create::isLeftWheeldrop
bool isLeftWheeldrop() const
Definition: create.cpp:635
create::Create::isCliffFrontRight
bool isCliffFrontRight() const
Definition: create.cpp:731
create::Create::sideMotorPower
uint8_t sideMotorPower
Definition: create.h:63
create::util::IN_PLACE_RADIUS
static const float IN_PLACE_RADIUS
Definition: util.h:48
create::util::EPS
static const float EPS
Definition: util.h:51
create::ID_WALL
@ ID_WALL
Definition: types.h:100
create::Create::vacuumMotorPower
uint8_t vacuumMotorPower
Definition: create.h:64
create::Opcode
Opcode
Definition: types.h:154
create::Create::getChargingState
create::ChargingState getChargingState() const
Get the current charging state.
Definition: create.cpp:791
create::Create::getIRLeft
uint8_t getIRLeft() const
Get value of 8-bit IR character currently being received by left sensor.
Definition: create.cpp:771
create::Create::isScheduleButtonPressed
bool isScheduleButtonPressed() const
Not supported by any firmware!
Definition: create.cpp:826
create::Create::requestedLeftVel
float requestedLeftVel
Definition: create.h:92
create
Definition: create.h:48
create::Create::isWheeldrop
bool isWheeldrop() const
Definition: create.cpp:625
create::Create::playSong
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...
Definition: create.cpp:614
create::Pose::x
float x
Definition: types.h:276
create::Create::init
void init(bool install_signal_handler)
Definition: create.cpp:17
create::RobotModel
Definition: types.h:48
create::Create::setSideMotor
bool setSideMotor(const float &power)
Set the power to the side brush motor.
Definition: create.cpp:512
create::Create::isVirtualWall
bool isVirtualWall() const
Definition: create.cpp:741
create::OC_SAFE
@ OC_SAFE
Definition: types.h:160
create::OC_DIGIT_LEDS_ASCII
@ OC_DIGIT_LEDS_ASCII
Definition: types.h:178
create::ID_IR_LEFT
@ ID_IR_LEFT
Definition: types.h:110
create::Create::dockLED
uint8_t dockLED
Definition: create.h:69
create::OC_DRIVE_PWM
@ OC_DRIVE_PWM
Definition: types.h:171
create::Create::getBatteryCapacity
float getBatteryCapacity() const
Get estimated battery charge capacity.
Definition: create.cpp:927
create::Create::getLightSignalLeft
uint16_t getLightSignalLeft() const
Get the signal strength from the left light sensor.
Definition: create.cpp:997


libcreate
Author(s): Jacob Perron
autogenerated on Wed May 24 2023 02:24:57