Motor Control 6x Hack for Navigation

From wikidb
Jump to: navigation, search

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 ~/catkin_ws_indigo/src/phidgets_cat/CMakeList.txt 

 # 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 

 <launch>
     <node pkg="phidgets_cat" type="motor_control_hc_hack_5x" name="motor_control_hc_hack_5x" respawn="true" output="screen">
         <param name="odometry" value="disable" />
     </node>
 </launch>

It is called from the nav.launch 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.

Retrieved from "http://zdome.net/wiki/index.php?title=Motor_Control_6x_Hack_for_Navigation&oldid=3204"