joint_motion_handler.cpp
Go to the documentation of this file.
00001 /*
00002 * Software License Agreement (BSD License) 
00003 *
00004 * Copyright (c) 2011, Southwest Research Institute
00005 * All rights reserved.
00006 *
00007 * Redistribution and use in source and binary forms, with or without
00008 * modification, are permitted provided that the following conditions are met:
00009 *
00010 *       * Redistributions of source code must retain the above copyright
00011 *       notice, this list of conditions and the following disclaimer.
00012 *       * Redistributions in binary form must reproduce the above copyright
00013 *       notice, this list of conditions and the following disclaimer in the
00014 *       documentation and/or other materials provided with the distribution.
00015 *       * Neither the name of the Southwest Research Institute, nor the names 
00016 *       of its contributors may be used to endorse or promote products derived
00017 *       from this software without specific prior written permission.
00018 *
00019 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00020 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00021 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00022 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00023 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00024 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00025 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00026 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00027 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00028 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00029 * POSSIBILITY OF SUCH DAMAGE.
00030 */ 
00031 
00032 
00033 #include "joint_motion_handler.h"
00034 #include "joint_message.h"
00035 #include "log_wrapper.h"
00036 
00037 
00038 using namespace industrial::joint_message;
00039 using namespace industrial::joint_data;
00040 using namespace industrial::simple_message;
00041 
00042 namespace motoman
00043 {
00044 namespace joint_motion_handler
00045 {
00046 
00047 #define JOBNAME "PVARQ20VV"
00048 
00049 JointMotionHandler::JointMotionHandler()
00050 {       
00051   // Set up job variables
00052   // start task
00053   job_start_data.sTaskNo = 0;
00054   strcpy(job_start_data.cJobName, JOBNAME);
00055  
00056   // reset task
00057   strcpy(job_reset_data.cJobName, JOBNAME);
00058   
00059   this->jobStarted = false;
00060   this->motionEnabled = false;
00061 }
00062 
00063 JointMotionHandler::~JointMotionHandler()
00064 {
00065   this->disableMotion();
00066   this->stopMotionJob();
00067 }
00068 
00069 bool JointMotionHandler::init(industrial::smpl_msg_connection::SmplMsgConnection* connection)
00070 {
00071   return this->init(StandardMsgTypes::JOINT_POSITION, connection);
00072 }
00073 
00074 bool JointMotionHandler::internalCB(industrial::simple_message::SimpleMessage & in)
00075 {
00076   bool rtn = false;
00077   JointMessage jMsg;
00078   SimpleMessage reply;
00079 
00080     if (jMsg.init(in))
00081     {
00082         this->motionInterface(jMsg);
00083     }
00084     else
00085     {
00086     LOG_ERROR("Failed to initialize joint message");
00087     rtn = false;
00088     }
00089 
00090     // Send response if requested
00091     if (CommTypes::SERVICE_REQUEST == in.getCommType())
00092         if (jMsg.toReply(reply, ReplyTypes::SUCCESS))
00093         {
00094             if(this->getConnection()->sendMsg(reply))
00095             {
00096                 LOG_INFO("Joint ack sent");
00097                 rtn = true;
00098             }
00099             else
00100             {
00101                 LOG_ERROR("Failed to send joint ack");
00102                 rtn = false;
00103             }
00104         }
00105         else
00106         {
00107             LOG_ERROR("Failed to generate joint ack message");
00108             rtn = false;
00109         }
00110     
00111   return rtn;
00112 }
00113 
00114 
00115  void JointMotionHandler::motionInterface(JointMessage & jMsg)
00116  {
00117 //
00118 // Upon receiving...
00119 // 1st point - initialize buffer, enable motion, start job, add point (increment buffer index), 
00120 // Nth point - add point (increment buffer index)
00121 // end of trajectory - wait until buffer size = 0, disable motion, stop job, reset buffer indicies
00122 //motion stop - disable motion, stop job
00123 
00124   JointData joints;
00125   
00126    switch (jMsg.getSequence())
00127     {
00128       case SpecialSeqValues::END_TRAJECTORY:
00129          LOG_INFO("Received end trajectory command");
00130          while(!pVarQ.bufferEmpty())
00131          {
00132            LOG_DEBUG("Waiting for motion buffer to empty");
00133            mpTaskDelay(this->BUFFER_POLL_TICK_DELAY);
00134          };
00135          this->stopMotionJob();
00136            
00137          break;
00138          
00139       case SpecialSeqValues::STOP_TRAJECTORY:
00140          LOG_WARN("Received stop command, stopping motion immediately");
00141          this->stopMotionJob();
00142          break;
00143          
00144       default:
00145         
00146         joints.copyFrom(jMsg.getJoints());
00147         if (!(this->isJobStarted()))
00148         {
00149           //TODO: The velocity should be set from the message in the future.
00150           pVarQ.init(joints, 0.0);
00151           this->startMotionJob();
00152         }
00153         else
00154         {
00155           pVarQ.addPoint(joints, 0.0);
00156         }
00157     }
00158  }
00159  
00160  
00161 void JointMotionHandler::enableMotion(void)
00162 {
00163   
00164   LOG_INFO("Enabling motion");
00165   this->motionEnabled = false;
00166 
00167   servo_power_data.sServoPower = ON;
00168   while(mpSetServoPower(&servo_power_data, &servo_power_error) == ERROR)
00169   {
00170     LOG_ERROR("Failed to turn on servo power, error: %d, retrying...", servo_power_error.err_no);
00171     mpTaskDelay(this->MP_POLL_TICK_DELAY);
00172   };
00173   
00174   hold_data.sHold = OFF;
00175   while(mpHold(&hold_data, &hold_error) == ERROR)
00176   {
00177     LOG_ERROR("Failed to turn off hold, error: %d, retrying...", hold_error.err_no);
00178     mpTaskDelay(this->MP_POLL_TICK_DELAY);
00179   };
00180   
00181   this->motionEnabled = true;
00182 }
00183 
00184 
00185 void JointMotionHandler::disableMotion(void)
00186 {
00187   LOG_INFO("Disabling motion");
00188   servo_power_data.sServoPower = OFF;
00189   while(mpSetServoPower(&servo_power_data, &servo_power_error) == ERROR)
00190   {
00191     LOG_ERROR("Failed to turn off servo power, error: %d, retrying...", servo_power_error.err_no);
00192     mpTaskDelay(this->MP_POLL_TICK_DELAY);
00193   };
00194   
00195   this->motionEnabled = false;
00196 }
00197 
00198 void JointMotionHandler::startMotionJob(void)
00199 {
00200 
00201   this->jobStarted = false;
00202   
00203   this->enableMotion();
00204   
00205   LOG_INFO("Starting motion job");
00206   while(mpStartJob(&job_start_data, &job_error) == ERROR)
00207   {
00208     LOG_ERROR("Failed to start job, error: %d, retrying...", job_error.err_no);
00209     mpTaskDelay(this->MP_POLL_TICK_DELAY);
00210   };
00211   
00212   LOG_DEBUG("Waiting for indexes to reset");
00213   // The INFORM job should reset the index counters 
00214   while( (pVarQ.getMotionPosIndex() != 0) && ( pVarQ.getBufferPosIndex() != 0))
00215   {
00216     LOG_ERROR("Waiting for indexes to reset, retying...");
00217     mpTaskDelay(this->MP_POLL_TICK_DELAY);
00218   };
00219   
00220   LOG_DEBUG("Reset indexes, motion: %d, buffer: %d", pVarQ.getMotionPosIndex(),
00221     pVarQ.getBufferPosIndex());  
00222   
00223   this->jobStarted = true;
00224 }
00225 void JointMotionHandler::stopMotionJob(void)
00226 {  
00227   LOG_INFO("Stopping motion job");
00228   this->disableMotion();
00229   
00230   // Reseting the job.
00231   job_reset_data.usJobLine = 1;  //Job lines start at 1
00232   while(mpSetCurJob(&job_reset_data, &job_reset_error) == ERROR)
00233   {
00234     LOG_ERROR("Failed to reset job, error: %d, retrying...", job_reset_error.err_no);
00235     mpTaskDelay(this->MP_POLL_TICK_DELAY);
00236   };
00237   
00238   this->jobStarted = false;
00239 }       
00240 
00241 }//namespace joint_motion_handler
00242 }//namespace industrial
00243 
00244 


dx100
Author(s): Shaun Edwards (Southwest Research Institute)
autogenerated on Mon Oct 6 2014 02:25:33