0
$\begingroup$

I have an mbot ranger which uses the 180 Optical Encoder Motor (https://support.makeblock.com/hc/en-us/articles/12376087604887-About-180-Optical-Encoder-Motor-for-mBot-Ranger-). My final goal is to publish the odom data in ROS. However, I can see that the distances calculated using the encoders are incorrect. It's fine for forward and backward movement but when turns occur, the distance calculated go out of control. I'm not exactly sure what am I doing wrong and would be grateful for your help.

I use the MeEncoderOnBoard class (https://github.com/Makeblock-official/Makeblock-Libraries/blob/master/src/MeEncoderOnBoard.cpp) to calculate the values. My code is as follows,

  • Get the current position of both wheels from the encoder
getCurPos();
  • Convert to radians and get the linear distance
return (degrees*PI/180)*constants::WHEEL_RADIUS ; // r = 0.0198
  • The below code is run every 120 ms to calculate the x, y and theta values,
    OdomData odom_data_current;
    odom_data_current.linear_distance_left_wheel = RobotDriver::Instance().GetTotalLinearDistance(enums::WHEEL_TYPE::LEFT_WHEEL);
    odom_data_current.linear_distance_right_wheel = -1*RobotDriver::Instance().GetTotalLinearDistance(enums::WHEEL_TYPE::RIGHT_WHEEL);
    odom_data_current.time = makeblock_ros::GetRosNodeHandle().now();
    odom_data_current.linear_distance_total = (odom_data_current.linear_distance_left_wheel + odom_data_current.linear_distance_right_wheel) / 2;
    odom_data_current.angular_distance_total = ((-1*odom_data_current.linear_distance_left_wheel) + odom_data_current.linear_distance_right_wheel) / robot_structure::WHEEL_DISTANCE_BETWEEN_CENTER;
    
    ros::Duration delta_t = odom_data_current.time - odom_data_last_.time;
    float delta_linear_l = odom_data_current.linear_distance_left_wheel - odom_data_last_.linear_distance_left_wheel;
    float delta_linear_r = odom_data_current.linear_distance_right_wheel - odom_data_last_.linear_distance_right_wheel;
    float delta_linear = (delta_linear_l + delta_linear_r)/2;
    float delta_orientation = ((-1*delta_linear_l) + delta_linear_r) / (robot_structure::WHEEL_DISTANCE_BETWEEN_CENTER);
    
    // According to the following link, delta_orientation should be divided by 2 -> https://medium.com/@nahmed3536/wheel-odometry-model-for-differential-drive-robotics-91b85a012299
    float theta = total_orientation_ + (delta_orientation/2);
    float delta_linear_x = delta_linear * cos(theta);
    float delta_linear_y = delta_linear * sin(theta);
    
    float delta_linear_velocity = delta_linear / delta_t.toSec();
    float delta_linear_velocity_x = delta_linear_x / delta_t.toSec();
    float delta_linear_velocity_y = delta_linear_y / delta_t.toSec();

    float delta_linear_velocity_l = delta_linear_l / delta_t.toSec();
    float delta_linear_velocity_r = delta_linear_r / delta_t.toSec();
    float delta_angular_velocity = delta_orientation / delta_t.toSec();
    
    total_orientation_ += delta_orientation;
    total_x_ += delta_linear_x;
    total_y_ += delta_linear_y;

    Logger::Instance().LogInfo("Total Linear Distance: Left-> " + String(odom_data_current.linear_distance_left_wheel) + ", Right->" + String(odom_data_current.linear_distance_right_wheel));
    Logger::Instance().LogInfo("Encoder Speed: Left-> " + String(RobotDriver::Instance().GetWheel(enums::WHEEL_TYPE::LEFT_WHEEL).GetSpeed()) + ", Right->" + String(RobotDriver::Instance().GetWheel(enums::WHEEL_TYPE::RIGHT_WHEEL).GetSpeed()));
    Logger::Instance().LogInfo("1. Total x: " + String(total_x_) + ", y: " + String(total_y_) + ", Theta: " + String(total_orientation_));
    Logger::Instance().LogInfo("2. Overall x: " + String(odom_data_current.linear_distance_total* cos(odom_data_current.angular_distance_total)) + ", y: " + String(odom_data_current.linear_distance_total* sin(odom_data_current.angular_distance_total)) + ", Theta: " + String(odom_data_current.angular_distance_total));

I've tried with two forulas where total_x_ is the summation of small movements and odom_data_current.linear_distance_total where this is calculated from the total movement of the robot. Both give incorrect values when run on the robot. I think its an issue with the encoders but I am not sure. May be my forumlas are incorrect.

The PWM, pulse and gear ratio is set as follows,

    TCCR1A = _BV(WGM10);//PIN12
    TCCR1B = _BV(CS11) | _BV(CS10) | _BV(WGM12);
    TCCR2A = _BV(WGM21) | _BV(WGM20);//PIN8
    TCCR2B = _BV(CS22);
    wheel_encoder_left.setPulse(9); // 9
    wheel_encoder_right.setPulse(9);
    wheel_encoder_left.setRatio(39.6); // 39.6
    wheel_encoder_right.setRatio(39.6);
$\endgroup$

0

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.