View source for Motor Control 6x Hack for Navigation

Correction - 5x hack

= Status =

Currently this is the most reliable Phidgets motor controller navigation solution.

= phidgets_cat =

The modified code is in the phidgets-cat package. "cat" is a reference to the port of the Phidgets motor controller to the 
Catkin build system.

  ~/catkin_ws_indigo/src/phidgets_cat

= 5x Hack =

These are quick code hacks performed on the motor control to experiment with fix so navigation would not got into a "Clearing costmap" 
followed by a "Rotate recovery behavior". See [[Navigation_Tuning#clearing_costmap]] for details.

These are the hacks made in the code.
* The incoming target speed Twist x was multiplied 5. The robot moved slowly to the navigation target. When it got close, its speed was
decreased. Since the speed was slow to start with, it ended up stopping instead of going slow. This triggered the recovery mode. See
the following link for details [[Navigation_Tuning#clearing_costmap]].
* The odometry input was disabled. It resulted in no movement. I was unable to find a set of parameters that enabled movement.
* Hard coded reverse vs. forward detection because using the parameters didn't work correctly.

This code is in the phidgets-cat package
  ~/catkin_ws_indigo/src/phidgets_cat/src/motor_control_hc_hack_5x.cpp

  diff motor_control_hc.cpp motor_control_hc_hack_5x.cpp
  
  285c285,286
  <         float x = m.linear.x;
  ---
  >         //float x = m.linear.x;
  >         float x = m.linear.x * 5;
  286a288
  >         // float y = m.angular.z * 5;
  311c313,315
  <         if (odometry_active) {
  ---
  >         // HACK: disable odometry feedback for PID
  >         if (false) {
  >         //        if (odometry_active) {
  474a479,486
  >         // HACK: hard code invert rotation and forward
  >             CPhidgetMotorControl_setVelocity (phid, 0,
  >                                             -(-forward_speed -
  >                                               (rotate*speed)));
  >             CPhidgetMotorControl_setVelocity (phid, 1,
  >                                             -(forward_speed -
  >                                               (rotate*speed)));
  >           /*
  480a493
  >           */
  566a580,581
  > 
  >     ROS_INFO ("***** EXPERIMENTAL - motor_control_hc_5x *****");

= Build =

Additions to <tt>~/catkin_ws_indigo/src/phidgets_cat/CMakeList.txt</tt>

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


= launch script =

cat motor_control_5x.launch 
  <!--
    Launcher for Phidgets differential drive wheel motor controller.
    -->
  <launch>
      <node pkg="phidgets_cat" type="motor_control_hc_hack_5x" name="motor_control_hc_hack_5x" respawn="true" output="screen">
          <!-- for now disable odometry feedback for PID by providing a bogus topic -->
          <param name="odometry" value="disable" />
      </node>
  </launch>

It is called from the <tt>nav.launch</tt> script.

= Execution =

The following are running separate windows
  roslaunch floor_hugger motor_control_hack.launch
  roslaunch floor_hugger odometry.launch

Then I ran this script test1.sh
  rostopic echo odom > odom.log&
  rostopic pub -1 /cmd_vel geometry_msgs/Twist '[0.37,0,0]' '[0,0,0]'
  rostopic pub -1 /cmd_vel geometry_msgs/Twist '[1,0,0]' '[0,0,0]'
  kill -9 %1

To confirm visually I also ran 

   rqt_plot /odom/twist/twist/linear/x

= Conclusion =

This demonstrated the slow speed was an issue. Speeds still not correct. I didn't expect them to be because 5x was an off-the-cuff pick.

When the navigation app publishes a linear x twist of 0.37 m/s, the wheels end up only
going 1/5 of that speed 0.14 to 0.17 m/s.
When we send it a 1 m/s - it goes .34 to .42 m/s. It is still running too slow.