channel.cpp
Go to the documentation of this file.
00001 
00026 #include "roboteq_driver/channel.h"
00027 #include "roboteq_driver/controller.h"
00028 
00029 #include "ros/ros.h"
00030 #include "roboteq_msgs/Feedback.h"
00031 #include "roboteq_msgs/Command.h"
00032 
00033 // Simple conversions between rad/s and RPM.
00034 #define TO_RPM(x) (double(x) * 60 / (2 * M_PI))
00035 #define FROM_RPM(x) (double(x) * (2 * M_PI) / 60)
00036 
00037 namespace roboteq {
00038 
00039 Channel::Channel(int channel_num, std::string ns, Controller* controller) :
00040   channel_num_(channel_num), nh_(ns), controller_(controller), max_rpm_(3500)
00041 {
00042   sub_cmd_ = nh_.subscribe("cmd", 1, &Channel::cmdCallback, this);
00043   pub_feedback_ = nh_.advertise<roboteq_msgs::Feedback>("feedback", 1);
00044 
00045   timer_init_ = nh_.createTimer(ros::Duration(1.0), &Channel::timerCallback, this);
00046 }
00047 
00048 void Channel::cmdCallback(const roboteq_msgs::Command& command) {
00049   // First convert the user's command from rad/s to RPM.
00050   float commanded_rpm = TO_RPM(command.commanded_velocity);
00051 
00052   // Now get the -1000 .. 1000 command as a proportion of the maximum RPM.
00053   int roboteq_command = int((commanded_rpm / max_rpm_) * 1000.0);
00054   ROS_DEBUG_STREAM("Sending command value of " << roboteq_command << " to motor driver.");
00055 
00056   // Write the command.
00057   controller_->command << "G" << channel_num_ << roboteq_command << controller_->send;
00058   controller_->flush();
00059 }
00060 
00061 void Channel::feedbackCallback(std::vector<std::string> fields) {
00062   roboteq_msgs::Feedback msg;
00063   msg.header.stamp = last_feedback_time_ = ros::Time::now();
00064 
00065   try {
00066     msg.motor_current = boost::lexical_cast<float>(fields[2]) / 10;
00067     msg.commanded_velocity = FROM_RPM(boost::lexical_cast<double>(fields[3]));
00068     msg.motor_power = boost::lexical_cast<float>(fields[4]) / 1000.0;
00069     msg.measured_velocity = FROM_RPM(boost::lexical_cast<double>(fields[5]));
00070     msg.measured_position = boost::lexical_cast<double>(fields[6]) * 2 * M_PI / 4096;
00071     msg.supply_voltage = boost::lexical_cast<float>(fields[7]) / 10.0;
00072     msg.supply_current = boost::lexical_cast<float>(fields[8]) / 10.0;
00073     msg.motor_temperature = boost::lexical_cast<int>(fields[9]) * 0.020153 - 4.1754;
00074     msg.channel_temperature = boost::lexical_cast<int>(fields[10]);
00075   } catch (std::bad_cast& e) {
00076     ROS_WARN("Failure parsing feedback data. Dropping message.");
00077     return;
00078   }
00079   pub_feedback_.publish(msg);
00080 }
00081 
00082 void Channel::timerCallback(const ros::TimerEvent&) {
00083   if (ros::Time::now() - last_feedback_time_ > ros::Duration(1.0)) {
00084     // Not receiving feedback, attempt to start it.
00085     controller_->setUserBool(channel_num_, 1);
00086     controller_->flush();
00087   }
00088 }
00089 
00090 }


roboteq_driver
Author(s):
autogenerated on Fri Feb 12 2016 00:10:33