Set close to zero velocity to zero. Fixes #92

[jviereck]

Printing the velocity values while the robot is at rest, I was getting these readings:

motor_drivers[i].motor1->velocity = 0.000000
motor_drivers[i].motor2->velocity = 0.000000

motor_drivers[i].motor1->velocity = -0.051133
motor_drivers[i].motor2->velocity = -0.051133

motor_drivers[i].motor1->velocity = 0.000000
motor_drivers[i].motor2->velocity = -0.051133

motor_drivers[i].motor1->velocity = 0.000000
motor_drivers[i].motor2->velocity = 0.000000

motor_drivers[i].motor1->velocity = -0.051133
motor_drivers[i].motor2->velocity = -0.051133

motor_drivers[i].motor1->velocity = 0.000000
motor_drivers[i].motor2->velocity = -0.051133

motor_drivers[i].motor1->velocity = 0.000000
motor_drivers[i].motor2->velocity = 0.000000

motor_drivers[i].motor1->velocity = -0.051133
motor_drivers[i].motor2->velocity = -0.051133

motor_drivers[i].motor1->velocity = 0.000000
motor_drivers[i].motor2->velocity = -0.051133

motor_drivers[i].motor1->velocity = 0.000000
motor_drivers[i].motor2->velocity = 0.000000

Based on this, I set the velocity to zero if it's below 0.052 on both motors.

[jviereck]

\cc @wxmerkt

[thomasfla]

Thanks for this PR.

Alternatively, the fix could be done before the packet interpretation: https://github.com/open-dynamic-robot-initiative/master-board/blob/1c81a2acbfd44f1c25f5ebe23487057b9e6cf7f8/sdk/master_board_sdk/src/master_board_interface.cpp#L362 by comparing sensor_packet.dual_motor_driver_sensor_packets[i].velocity[x] with +1 and -1.

if (sensor_packet.dual_motor_driver_sensor_packets[i].velocity[x] == 1 || 
    sensor_packet.dual_motor_driver_sensor_packets[i].velocity[x] == -1) 
{
    sensor_packet.dual_motor_driver_sensor_packets[i].velocity[x]  = 0;
}

This way, we don't have to hard-code the threshold, and it will automatically adapt to a any future change of velocity resolution.

[wxmerkt]

Thanks @jviereck! I truncate on the output side after the 9:1 if it's below 0.006 which translates to 0.054 - so this does the same job :-).

[jviereck]

Alternatively, the fix could be done before the packet interpretation:

Thanks for the suggestion @thomasfla . I've updated the PR accordingly.

Can people take a second look and if this is good I will merge the PR.

[thomasfla]

Thanks!

[jviereck]

Does it solve your issue?

Yes. I tested it on the real solo12 and it removes the steady-state velocity-off-by-one problem reported initially by @wxmerkt .