Motor Controller Rewrite

References

• C++ Style Guide

Source Code

TBD - code is in flux

• CPhidgetMotorControl_setVelocity velocity parameter is 0 to 100 percent

Build

Additions to ~/catkin_ws_indigo/src/phidgets_cat/CMakeList.txt

 # motor_control_hc_nav
 add_executable(motor_control_hc_nav src/motor_control_hc_nav.cpp)
 target_link_libraries(motor_control_hc_nav 
   ${catkin_LIBRARIES}
   phidget21
 )

Launch Script

Launch script in floor_hugger/launch/motor_control_nav.launch

 <launch>
     <node pkg="phidgets_cat" type="motor_control_hc_nav" name="motor_control_hc_nav" respawn="true" output="screen">
        <param name="odometry" value="/odom" />
        <param name="wheelbase" value="305" />
        <param name="pid_proportional" value="15" />
        <param name="pid_integral" value="60" />
        <param name="pid_derivative" value="0" />
     </node>
 </launch>

Test Execution

Terminal 2

The motor_control_nav launch script executes the code associated with the motor_control_hc_nav.cpp code in the phidgets_cat package.

 roslaunch floor_hugger motor_control_nav.launch

Terminal 3

 roslaunch floor_hugger odometry.launch

Terminal 4

Take the place of the navigation stack by publishing a cmd_vel message of type Twist. It has linear.x value of 0.20 m/sec and an angular.z value of 0.1 radians/sec.

 roscd floor_hugger/test/
 python motors_go_full.py 0.20 0.1

 motors_go_full.py <linear.x> <angular.z>

See motors_go_full.py

Terminal 5

Monitor resulting motor status with one of the following

 rostopic echo odom
 rqt_plot /cmd_vel/linear/x /odom/twist/twist/linear/x /odom/twist/twist/linear/y

Results Without PID

Odometry is not steady. This is the results of the printout from:

   ROS_INFO ("linear x cmd_vel %.3f    odom %.3f", 
                     m.linear.x, odom.twist.twist.linear.x);

The requested linear.x was 0.37 m/sec. Notice that the resulting x speed varied from about 0.31 to 0.39 or roughly 20%. The disturbing results are periodic values of 0. They occur at rotation quadrant boundaries.

 [ INFO] [1498936142.530818061]: linear x cmd_vel -0.370    odom -0.324
 [ INFO] [1498936142.630888175]: linear x cmd_vel -0.370    odom -0.381
 [ INFO] [1498936142.730873989]: linear x cmd_vel -0.370    odom -0.378
 [ INFO] [1498936142.830848798]: linear x cmd_vel -0.370    odom 0.000
 [ INFO] [1498936142.930902163]: linear x cmd_vel -0.370    odom -0.360
 [ INFO] [1498936143.030857096]: linear x cmd_vel -0.370    odom -0.360
 [ INFO] [1498936143.130885123]: linear x cmd_vel -0.370    odom -0.319
 [ INFO] [1498936143.230823537]: linear x cmd_vel -0.370    odom -0.317
 [ INFO] [1498936143.330835216]: linear x cmd_vel -0.370    odom -0.388
 ...

The following proof-of-concept code that was used before replacing it with the ROS PID library. It just uses the P part of PID.

   // compute speed errors
   float linear_speed_error = target.linear.x - linear_speed;
   float angular_speed_error = target.angular.z - odom.twist.twist.angular.z;
 
   // motor power computed between -100 and 100 percent full power
   // as a simple proof-of-concept use only the P part of PID
   power_wheel0 = power_wheel0 + linear_speed_error * linear_velocity_proportional + 
                                 angular_speed_error * angular_velocity_proportional;
   if (power_wheel0 >  100) power_wheel0 =  100;
   if (power_wheel0 < -100) power_wheel0 = -100;

Resolution Investigations

• This appears to be an odometry (not motor controller) issue. See odometry.cpp in the Phidgets package.
• Include PID and tune. See Phidgets PID
• Cleanup direction (forward vs. reverse) code