Gearbox Motor Controller Design - Differential Drive

edit

I decided to write my own Pidgets motor controller code because I was unable to get the Phidget code working properly and found no support. The problem is that wheel speeds did not match the navigation velocity requests by a factor of 5.

The attached code is a proof of concept. I am not requesting a code rewrite. I am hoping for comments that will tell me what approach is fraught with trouble and where I clearly don’t understand basic concepts or leverage existing ROS libraries. My code is absolutely disgusting in places, however, it functions well enough that the robot gets close to the target before exhibiting bizarre behavior. I need to calibrate the pid controllers and recalibrate the navigation stack. I don’t want to attempt that on a faulty design.

1. The chunk of code that determines the sign for linear speed, reverse vs. forward, is most embarrassing. I suspect I can replace it with single line that leverages the quaternion library. I can use some help.
2. As an alternative, my son suggested I transform navigation speed targets from the map coordinate system to linear x and angular z speeds in a robot coordinate system. That means I only need to deal with the linear x to get linear speed and its sign to determine if it is going forward or backward. My understanding of quaternions is too weak to implement that transformation.
3. I found motor driver code snippets in Lentin Joseph’s Mastering ROS book. It helped me combine linear and angular speed components. I’m uneasy about my implementation. It tests out fine on the floor.
4. Are there other nicely written examples or white papers that I missed in my searchers? For example, the Turtlebot3 OpenCR controller code didn’t appear to be applicable because I am using a gearhead motor, not a Dynamixel.
5. The differential_drive package looks tempting but I see support stopped with groovy. Are there standard motor control packages that I’ve missed?

Odometry aside: Currently, I can create maps by joy-sticking around and using odometry from the Phidgets motor encoders. I’ve been able to use the ROS navigation tuning guides to calibrate. Measured distances and velocities match odom messages. This means a lot is working.

Phidget controller aside: I didn’t understand how to use the motor control parameters for the motor_control_hc node in the Phidgets package. At one point I hacked the controller by multiplying the requested velocity by 5. I had moderate navigation success after that. This helped me understand the problem.

void setMotorPower ()
{
    float wheel0_speed = 0;
    float wheel1_speed = 0;

    // *** Compute the current wheel speeds ***
    // First compute the Robot's linear and angular speeds
    float xspeed = odom_.twist.twist.linear.x;
    float yspeed = odom_.twist.twist.linear.y;
    float linear_speed = sqrt (xspeed * xspeed + yspeed * yspeed);

    float angular_speed = odom_.twist.twist.angular.z;

    // Now compute wheel speed by summing linear and rotational elements.
    // It makes a difference if the robot is backing up or going forward.
    // Direction is determined ...