Merge pull request #31 from schemborerik/feat-wheels-pwm-measured-vel

Feat wheels pwm measured vel

include/create/create.h

@@ -83,6 +83,8 @@ namespace create {
 
       Matrix poseCovar;
 
+      float measuredLeftVel;
+      float measuredRightVel;
       float requestedLeftVel;
       float requestedRightVel;
 
@@ -191,6 +193,14 @@ namespace create {
        */
       bool driveWheels(const float& leftWheel, const float& rightWheel);
 
+      /**
+       * \brief Set the direct  for the left and right wheels.
+       * \param leftWheel pwm in the range [-1, 1]
+       * \param rightWheel pwm in the range [-1, 1]
+       * \return true if successful, false otherwise
+       */
+      bool driveWheelsPwm(const float& leftWheel, const float& rightWheel);
+
       /**
        * \brief Set the forward and angular velocity of Create.
        * \param xVel in m/s
@@ -552,6 +562,18 @@ namespace create {
        */
       float getRightWheelDistance() const;
 
+      /**
+       * \brief Get the measured velocity of the left wheel.
+       * \return velocity in m/s
+       */
+      float getMeasuredLeftWheelVel() const;
+
+      /**
+       * \brief Get the measured velocity of the right wheel.
+       * \return velocity in m/s
+       */
+      float getMeasuredRightWheelVel() const;
+
       /**
        * \brief Get the requested velocity of the left wheel.
        * This value is bounded at the maximum velocity of the robot model.

src/create.cpp

@@ -170,6 +170,9 @@ namespace create {
       deltaYaw = wheelDistDiff / model.getAxleLength();
     }
 
+    measuredLeftVel = leftWheelDist / dt;
+    measuredRightVel = rightWheelDist / dt;
+
     // Moving straight
     if (fabs(wheelDistDiff) < util::EPS) {
       deltaX = deltaDist * cos(pose.yaw);
@@ -424,6 +427,27 @@ namespace create {
     }
   }
 
+  bool Create::driveWheelsPwm(const float& leftWheel, const float& rightWheel)
+  {
+    static const int16_t PWM_COUNTS = 255;
+
+    if (leftWheel < -1.0 || leftWheel > 1.0 ||
+        rightWheel < -1.0 || rightWheel > 1.0)
+      return false;
+
+    int16_t leftPwm = roundf(leftWheel * PWM_COUNTS);
+    int16_t rightPwm = roundf(rightWheel * PWM_COUNTS);
+
+    uint8_t cmd[5] = { OC_DRIVE_PWM,
+                        rightPwm >> 8,
+                        rightPwm & 0xff,
+                        leftPwm >> 8,
+                        leftPwm & 0xff
+                       };
+
+    return serial->send(cmd, 5);
+  }
+
   bool Create::drive(const float& xVel, const float& angularVel) {
     // Compute left and right wheel velocities
     float leftVel = xVel - ((model.getAxleLength() / 2.0) * angularVel);
@@ -954,6 +978,14 @@ namespace create {
     return totalRightDist;
   }
 
+  float Create::getMeasuredLeftWheelVel() const {
+    return measuredLeftVel;
+  }
+
+  float Create::getMeasuredRightWheelVel() const {
+    return measuredRightVel;
+  }
+
   float Create::getRequestedLeftWheelVel() const {
     return requestedLeftVel;
   }