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


libcreate
Author(s): Jacob Perron
autogenerated on Sat Jan 2 2021 04:00:49