include/robo_explorer_hardware.h

   1 #include <hardware_interface/joint_command_interface.h>
   2 #include <hardware_interface/joint_state_interface.h>
   3 #include <hardware_interface/robot_hw.h>
   4 #include "roboint/Inputs.h"
   5 #include "roboint/Motor.h"
   6
   7 // When to increase/decrease speed based on current wheel speed difference
   8 #define SPEED_CONTROL_THRESH 0.5
   9
  10 class RoboExplorer : public hardware_interface::RobotHW
  11 {
  12         public:
  13                 RoboExplorer(ros::NodeHandle nh) {
  14                         this->nh = nh;
  15                         pub_motor = nh.advertise<roboint::Motor>("ft/set_motor", 10);
  16                         sub_inputs = nh.subscribe("ft/get_inputs", 10, &RoboExplorer::cbInputs, this);
  17
  18                         // connect and register the joint state interface
  19                         hardware_interface::JointStateHandle state_handle_left("left_wheel_joint", &pos[0], &vel[0], &eff[0]);
  20                         jnt_state_interface.registerHandle(state_handle_left);
  21                         hardware_interface::JointStateHandle state_handle_right("right_wheel_joint", &pos[1], &vel[1], &eff[1]);
  22                         jnt_state_interface.registerHandle(state_handle_right);
  23                         registerInterface(&jnt_state_interface);
  24
  25                         // connect and register the joint velocity interface
  26                         hardware_interface::JointHandle joint_handle_left(state_handle_left, &cmd[0]);
  27                         jnt_velocity_interface.registerHandle(joint_handle_left);
  28                         hardware_interface::JointHandle joint_handle_right(state_handle_right, &cmd[1]);
  29                         jnt_velocity_interface.registerHandle(joint_handle_right);
  30                         registerInterface(&jnt_velocity_interface);
  31                 }
  32
  33                 // Converts pos_encoder from wheels to wheel angles
  34                 void read(ros::Duration period) {
  35                         static double pos_last[2];
  36
  37                         // gear ration 1:2, 8 ticks/revolution, 2*8=16
  38                         pos[0] = pos_encoder[0] * 2*M_PI/16;
  39                         pos[1] = pos_encoder[1] * 2*M_PI/16;
  40                         vel[0] = (pos[0] - pos_last[0])/period.toSec();
  41                         vel[1] = (pos[1] - pos_last[1])/period.toSec();
  42
  43                         std::copy(std::begin(pos), std::end(pos), std::begin(pos_last));
  44                 }
  45
  46                 // Writes current velocity command to hardware
  47                 void write() {
  48                         static double speed_l = 0;
  49                         static double speed_r = 0;
  50                         double wish_speed_left = cmd[0];
  51                         double wish_speed_right = cmd[1];
  52                         roboint::Motor msg;
  53
  54                         if (wish_speed_left == 0) {
  55                                 speed_l = 0;
  56                         } else if (vel[0] - wish_speed_left < -SPEED_CONTROL_THRESH) {
  57                                 speed_l++;
  58                         } else if (vel[0] - wish_speed_left > SPEED_CONTROL_THRESH) {
  59                                 speed_l--;
  60                         }
  61
  62                         if (wish_speed_right == 0) {
  63                                 speed_r = 0;
  64                         } else if (vel[1] - wish_speed_right < -SPEED_CONTROL_THRESH) {
  65                                 speed_r++;
  66                         } else if (vel[1] - wish_speed_right > SPEED_CONTROL_THRESH) {
  67                                 speed_r--;
  68                         }
  69
  70                         // check limits
  71                         if (speed_l < -7) speed_l = -7;
  72                         else if (speed_l > 7) speed_l = 7;
  73                         if (speed_r < -7) speed_r = -7;
  74                         else if (speed_r > 7) speed_r = 7;
  75
  76                         msg.num = 0;
  77                         msg.speed = round(speed_l);
  78                         pub_motor.publish(msg);
  79
  80                         msg.num = 1;
  81                         msg.speed = round(speed_r);
  82                         pub_motor.publish(msg);
  83                 }
  84
  85                 // Reads current input state and increases pos_encoder on change
  86                 void cbInputs(const roboint::Inputs::ConstPtr& msg) {
  87                         static std::array<uint8_t, 8> input_last;
  88
  89                         if (msg->input[0] != input_last[0]) { // left input changed
  90                                 // outputs have direction information
  91                                 if (msg->output[0] > 0) pos_encoder[0]++;
  92                                 else if (msg->output[1] > 0) pos_encoder[0]--;
  93                         }
  94                         if (msg->input[1] != input_last[1]) { // right input changed
  95                                 // outputs have direction information
  96                                 if (msg->output[2] > 0) pos_encoder[1]++;
  97                                 else if (msg->output[3] > 0) pos_encoder[1]--;
  98                         }
  99
 100                         std::copy(std::begin(msg->input), std::end(msg->input), std::begin(input_last));
 101                 }
 102
 103         private:
 104                 hardware_interface::JointStateInterface jnt_state_interface;
 105                 hardware_interface::VelocityJointInterface jnt_velocity_interface;
 106                 ros::NodeHandle nh;
 107                 ros::Publisher pub_motor;
 108                 ros::Subscriber sub_inputs;
 109                 double cmd[2];
 110                 double pos[2];
 111                 double vel[2];
 112                 double eff[2];
 113                 int64_t pos_encoder[2] = {0, 0};
 114 };