Revision history [back]

However, the robot now turns left and right towards the set goal instead of going straight.

I look into your code and it looks like you have some code smells there, e.g.

// Check if we need to turn 
  if (cmdVel.angular.z != 0.0) {

    // Turn left
    if (cmdVel.angular.z > 0.0) {
      pwmLeftReq = -PWM_TURN;
      pwmRightReq = PWM_TURN;
    }
    // Turn right    
    else {
      pwmLeftReq = PWM_TURN;
      pwmRightReq = -PWM_TURN;
    }
  }
  // Go straight
  else {...}

You are checking cmdVel.angular.z != 0.0, when checking cmdVel.angular.z < SOME_MIN_DIFF would be better for uC. I am not so sure with your PWM control too - did you check it on the real robot?

// Publish tick counts to topics
leftPub.publish( &left_wheel_tick_count );
rightPub.publish( &right_wheel_tick_count );

Are these for sure synchronized? Your comment with

However, the robot now turns left and right towards the set goal instead of going straight.

seems connected to this.

However, the robot now turns left and right towards the set goal instead of going straight.

I look into your code and it looks like you have some code smells there, e.g.

// Check if we need to turn 
  if (cmdVel.angular.z != 0.0) {

    // Turn left
    if (cmdVel.angular.z > 0.0) {
      pwmLeftReq = -PWM_TURN;
      pwmRightReq = PWM_TURN;
    }
    // Turn right    
    else {
      pwmLeftReq = PWM_TURN;
      pwmRightReq = -PWM_TURN;
    }
  }
  // Go straight
  else {...}

You are checking cmdVel.angular.z != 0.0, when checking cmdVel.angular.z < SOME_MIN_DIFF would be better for uC. I am not so sure with your PWM control too - did you check it on the real robot?

// Publish tick counts to topics
leftPub.publish( &left_wheel_tick_count );
rightPub.publish( &right_wheel_tick_count );

Are these for sure synchronized? Your comment with

However, the robot now turns left and right towards the set goal instead of going straight.

seems to be connected to this.