p_var_q.cpp
Go to the documentation of this file.
00001 /*
00002 * Software License Agreement (BSD License) 
00003 *
00004 * Copyright (c) 2011, Yaskawa America, Inc.
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 Yaskawa America, Inc., 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 #include "p_var_q.h"
00033 #include "controller.h"
00034 #include "joint_data.h"
00035 #include "joint_motion_handler.h"
00036 #include "ros_conversion.h"
00037 #include "log_wrapper.h"
00038 
00039 using motoman::controller::Controller;
00040 using motoman::joint_motion_handler;
00041 using motoman::ros_conversion;
00042 using industrial::joint_data;
00043 
00044 
00045 namespace motoman
00046 {
00047 namespace p_var_q
00048 {
00049 
00050 
00051 PVarQ::PVarQ()
00052 {       
00053   // TODO: Should check constants such as Q_SIZE, MOTION_POINTER & BUFFER_POINTER for
00054   // consitency
00055   
00056   // Checking class constants for consistancy
00057   STATIC_ASSERT(QSIZE_ > PT_LOOK_AHEAD_, QSIZE_NOT_GREATER_THAN_LOOK_AHEAD);
00058   STATIC_ASSERT(MOTION_POINTER_ > QSIZE_, MOTION_PONTER_NOT_GREATER_THAN_QSIZE);
00059   STATIC_ASSERT(BUFFER_POINTER_ > QSIZE_, BUFFER_PONTER_NOT_GREATER_THAN_QSIZE);
00060   STATIC_ASSERT(MIN_BUF_START_POINTER_ > QSIZE_, MIN_BUFF_START_PONTER_NOT_GREATER_THAN_QSIZE);
00061   
00062 }
00063 
00064 PVarQ::~PVarQ(void)
00065 {
00066 }
00067 
00068 
00069 void PVarQ::init(industrial::joint_data::JointData & point, double velocity_percent)
00070 {
00071   // Reseting the position buffer.  This should make it eaiser to catch any bugs on the
00072   // motoman side.
00073   industrial::joint_data::JointData empty;
00074   for(int i = 0; i < this->posVarQueueSize(); i++)
00075   {
00076     this->setPosition(i, empty, 0);
00077   }
00078   
00079   // Seed the intial point - this is required because upon startup, the indexes are zero and
00080   // therefore the intial point never gets set.
00081   setPosition(0, point, this->TEMP_getVelocityPercent());
00082   
00083   // Set the minium buffer size
00084   Controller::setInteger(MIN_BUF_START_POINTER_, PT_LOOK_AHEAD_);
00085 }
00086 
00087 void PVarQ::addPoint(industrial::joint_data::JointData & joints, double velocity_percent)
00088 {
00089 
00090   // Wait until buffer is not full
00091   
00092   LOG_DEBUG("Checking for buffer full");
00093   while(this->bufferFull()) {
00094       mpTaskDelay(this->BUFFER_POLL_TICK_DELAY_);
00095   };
00096   LOG_DEBUG("Buffer ready to accept new points");
00097   
00098   setNextPosition(joints, this->TEMP_getVelocityPercent());
00099   incBufferIndex();
00100         
00101 }
00102 
00103 int PVarQ::bufferSize()
00104 {
00105   int motionIdx = this->getMotionIndex();
00106   int bufferIdx = this->getBufferIndex();
00107   int rtn = 0;
00108   
00109   if (motionIdx > bufferIdx)
00110   {
00111     LOG_ERROR("Motion index: %d is greater than Buffer index: %d, returning empty buffer size", motionIdx, bufferIdx);
00112     rtn = 0;
00113   }
00114   else
00115   {
00116     rtn = bufferIdx - motionIdx;
00117   }
00118   
00119   return rtn;  
00120 }
00121     
00122 
00123   
00124 int PVarQ::getMotionPosIndex()
00125 {
00126   return (this->getMotionIndex() % this->posVarQueueSize());
00127 }
00128   
00129 int PVarQ::getBufferPosIndex()
00130 {
00131   return (this->getBufferIndex() % this->posVarQueueSize());
00132 }
00133 
00134 int PVarQ::getNextBufferPosIndex()
00135 {
00136   return ((this->getBufferIndex() + 1) % this->posVarQueueSize());
00137 }
00138     
00139 bool PVarQ::bufferFull()
00140 {
00141   bool rtn = false;
00142   int bufferSize = this->bufferSize();
00143   int maxBufferSize = this->maxBufferSize();
00144   if (bufferSize >= maxBufferSize)
00145   {
00146     rtn = true;
00147   }
00148   return rtn;
00149 }
00150 
00151 bool PVarQ::bufferEmpty()
00152 {
00153   bool rtn = false;
00154   if (this->bufferSize() <= 0)
00155   {
00156     LOG_DEBUG("Buffer is empty");
00157     rtn = true;
00158   }
00159   return rtn;
00160 }
00161 
00162 void PVarQ::incBufferIndex()
00163 {
00164   int bufferIdx = this->getBufferIndex();
00165   
00166   LOG_DEBUG("Incrementing buffer index from %d to %d", bufferIdx, bufferIdx + 1); 
00167   Controller::setInteger(BUFFER_POINTER_, bufferIdx + 1);
00168 }
00169 
00170 
00171 void PVarQ::setNextPosition(industrial::joint_data::JointData & point, double velocity_percent)
00172 {
00173   setPosition(this->getNextBufferPosIndex(), point, velocity_percent); 
00174 }
00175 
00176 
00177 void PVarQ::setPosition(int index, industrial::joint_data::JointData & point, 
00178     double velocity_percent)
00179 {
00180   const double VELOCITY_CONVERSION = 100.0;
00181   int convertedVelocity = 0;
00182   
00183   LOG_DEBUG("Setting joint position, index: %d", index);
00184 
00185   while (!Controller::setJointPositionVar(index, point)) {
00186     LOG_ERROR("Failed set position variable, index: %d, retrying...", index);
00187     mpTaskDelay(this->VAR_POLL_TICK_DELAY_);
00188   };
00189   
00190   convertedVelocity = (int)(velocity_percent * VELOCITY_CONVERSION);
00191   LOG_DEBUG("Converting percent velocity: %g to motoman integer value: %d", 
00192     velocity_percent, convertedVelocity);  
00193   LOG_DEBUG("Setting velocity, index: %d, value: %d", index, convertedVelocity);
00194   Controller::setInteger(index, convertedVelocity);
00195 }
00196 
00197 
00198 
00199 }//namespace p_var_q
00200 }//namespace motoman


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