motor.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009, CoroWare
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Stanford U. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include "ros/ros.h"
00031 #include "ssc32.hpp"
00032 #include "corobot_msgs/MotorCommand.h"
00033 #include "corobot_msgs/ServoPosition.h"
00034 #include <math.h>
00035 #include <errno.h>
00036 #include <sys/stat.h>
00037 #include <diagnostic_updater/diagnostic_updater.h>
00038 #include <diagnostic_updater/publisher.h>
00039 #include <corobot_diagnostics/diagnostics.h>
00040 
00041 using namespace Servo; 
00042 
00043 SSC32 m_ssc32;
00044 
00045 std::string SSC32_PORT;
00046 ros::Timer timer;
00047 
00048 // use for diagnostics purpose
00049 int ssc32Error = 0; 
00050 
00051 // Inform us on which servo has already been moved at least once.
00052 // The size has been set to 20 to make sure there is enough place for all the servos. 
00053 // We want to avoid dynamic memory control to be faster
00054 bool first_time_command[20] = {false};
00055 
00056 
00057 
00058 // stop the motors after the requested time
00059 // This is used only if some dc motors are connected to the ssc32 board
00060 void timerCallback(const ros::TimerEvent&)
00061 {
00062         bool ret;       
00063 
00064         ret = m_ssc32.SendMessage(1,0,0);
00065         if (ret)
00066                 ssc32Error = 0;
00067         else
00068                 ssc32Error = 2;
00069 
00070         ret = m_ssc32.SendMessage(0,0,0);
00071         if (ret)
00072                 ssc32Error = 0;
00073         else
00074                 ssc32Error = 2;
00075 }
00076 
00077 // set the motors requested speed. 
00078 // This is used only if some dc motors are connected to the ssc32 board
00079 void SetSpeedTopic(const corobot_msgs::MotorCommand::ConstPtr &msg)
00080 {
00081         ros::NodeHandle n;
00082         bool ret;
00083 
00084         ret = m_ssc32.SendMessage(1,(msg->leftSpeed*-5)+1500,(100-msg->acceleration)*10);
00085         if (ret)
00086                 ssc32Error = 0;
00087         else
00088                 ssc32Error = 2;
00089 
00090         m_ssc32.SendMessage(0,(msg->rightSpeed*-5)+1500,(100-msg->acceleration)*10);
00091         if (ret)
00092                 ssc32Error = 0;
00093         else
00094                 ssc32Error = 2;
00095 
00096         timer = n.createTimer(ros::Duration(msg->secondsDuration), timerCallback, true);
00097 }
00098 
00099 
00105 void setPositionCallback(const corobot_msgs::ServoPosition &msg)
00106 {
00107         //ideally, msg.position is between 0 and 150 degrees
00108         int amount =  (int) (msg.position / 180 * 1500) + 750;  
00109 
00110         // The first command should not contain any speed, so we are not sending any
00111         bool ok;
00112         if(first_time_command[msg.index] == false)
00113         {
00114                 ok = m_ssc32.SendMessage(msg.index,amount,-1);
00115                 first_time_command[msg.index] = true;
00116         }
00117         else
00118         {
00119                 ok = m_ssc32.SendMessage(msg.index,amount,2000);
00120         }
00121         
00122         if (!ok)
00123         {
00124                 ROS_ERROR("Could not set the servo motor number %d to the position %f",msg.index, msg.position);
00125                 ssc32Error = 2; 
00126         }
00127 }
00128 
00132 void ssc32_diagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat)
00133 {
00134         if (!ssc32Error)  
00135                 stat.summaryf(diagnostic_msgs::DiagnosticStatus::OK, "initialized");
00136         else if (ssc32Error == 1)
00137         {
00138                 stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "ssc32 cannot be initialized");
00139                 stat.addf("Recommendation",SSC32_ERROR_CONNECTION);
00140         }
00141         else if (ssc32Error == 2)
00142         {
00143                 stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "cannot move arm");
00144                 stat.addf("Recommendation", ERROR_MOVING_ARM);
00145         }
00146 }
00147 
00148 int main(int argc, char **argv)
00149 {
00150         ros::init(argc, argv, "ssc32_node");
00151         ros::NodeHandle n("~");
00152 
00153 
00154         //Subscribe to topics
00155         ros::Subscriber velocity=n.subscribe<corobot_msgs::MotorCommand>("/ssc32_velocity",1000, SetSpeedTopic);
00156         ros::Subscriber position_sub = n.subscribe("/setPositionServo",100, &setPositionCallback);
00157 
00158 
00159         //create an updater that will send information on the diagnostics topics
00160         diagnostic_updater::Updater updater;
00161         updater.setHardwareIDf("SSC32");
00162         //function that will be executed with updater.update()
00163         updater.add("SSC32", ssc32_diagnostic); 
00164 
00165         std::string ssc32_port_;
00166 
00167         n.param<std::string>("ssc32_port", ssc32_port_, "/dev/ttyS1");
00168 
00169         if(!n.getParam("ssc32_port", ssc32_port_))
00170                 ROS_INFO("can not get parameter ssc32_port!");
00171 
00172         SSC32_PORT = ssc32_port_;
00173 
00174         // Let's give permission to do anything on the ssc32 port, or else we won't be able to use it. Commented because the node would need to be executed in super user. 
00175         /*char mode[] = "0777";
00176         int i;
00177         i = strtol(mode, 0, 8);
00178         if (chmod (SSC32_PORT.c_str(),i) < 0)
00179         {
00180                 ROS_ERROR("%s: error in chmod(%s, %s) - %d (%s)\n", argv[0], SSC32_PORT.c_str(), mode, errno, strerror(errno));
00181         }
00182         */
00183         //Let's now initialize the ssc32 and connect to it
00184         m_ssc32.initSerial();
00185 
00186         if(m_ssc32.startSerial(SSC32_PORT))
00187         {
00188           ROS_INFO("Success!!! Connect to serial port");
00189           ssc32Error = 0;
00190         }
00191         else
00192           ssc32Error = 1; 
00193 
00194         //main ros function
00195         while (ros::ok())
00196         {
00197                 ros::spinOnce();
00198                 // Diagnostic 
00199                 updater.update();
00200         }
00201 
00202         m_ssc32.stopSerial();
00203         
00204         return 0;
00205 }


corobot_ssc32
Author(s):
autogenerated on Wed Aug 26 2015 11:09:37