p_var_q.h
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 #ifndef __p_var_q_h
00033 #define __p_var_q_h
00034 
00035 #include "motoPlus.h"
00036 #include "controller.h"
00037 #include "joint_data.h"
00038 
00039 /*
00040 Point variable queue
00041 
00042 The point variable queue class encapsulates the point variable buffer
00043 used to buffer robot motion points.  The robot controller requires a
00044 point buffer in order to do it's own path interpolation and 
00045 ensure smooth motion between points.  This buffer exists between the
00046 motoplus application and the INFORM motion program.  (An INFORM motion
00047 program is used because it can perform smooth motion through several
00048 points, motoplus cannot).  The following section describes the structure
00049 and function of the point variable queue.
00050 
00051 Variables:
00052 
00053 Position variables (joint type)
00054 (NOTE: The max number of P variables is limited by the controller)
00055 P000 - PXXX(see QSIZE) units in pulses
00056 
00057 Velocity variables (integer type)
00058 When specifying a VJ speed in percentage, the controller calculates the 
00059 time it takes for each motor to travel that its distance (in pulse) at 
00060 specified speed. After it finds the motor that will take the longest time, 
00061 it will adjust down the speed of all the other motors so they will take the 
00062 same time.
00063 
00064 I000 - IXXX(see QSIZE) (percent 0.01%-100% -> 1-10000)
00065 
00066 Buffer Management
00067 The following variables are used to manage the buffer.  Two variables are used.
00068 The current Motion pointer is the index of the point currently being exectuted as
00069 part of the MOVJ (NOTE: Depending on how the controller performs it's look ahead,
00070 this may not be the point that it is currently executing motion on)  
00071 The current buffer pointer is the last point was populated with a valid joint point.
00072 
00073 NOTE: These variables are hard-codes in INFORM.  If their indexes change than the
00074 INFORM program must also be updated.
00075 
00076 IXXX(MOTION_POINTER) index of current point being executed in MOVJ
00077 IXXX(BUFFER_POINTER) index of the last populated point.
00078 
00079 Given these two variables the following can be determined.
00080 
00081 BUFFER_SIZE = BUFFER_POINTER - MOTION_POINTER
00082 
00083 POSITION VARIABLE INEXES:
00084 MOTION_POS_INDEX        = MOTION_POINTER % QSIZE
00085 BUFFER_POS_INDEX        = BUFFER_POINTER % QSIZE
00086 NEXT_BUFFER_POS_INDEX   = (BUFFER_POINTER + 1) % QSIZE
00087 
00088 MAX_BUFFER_SIZE <= QSIZE - 1 (or less, if desired)
00089 
00090 The INFORM program should only execute MOVJ on those points when the
00091 BUFFER_POINTER >=  MOTION_POINTER.
00092 
00093 TODO: WHAT TO DO WITH INTEGER ROLL OVERS ON MOTION/BUFFER POINTERS.
00094 
00095 */
00096 
00097 
00098 namespace motoman
00099 {
00100 namespace p_var_q
00101 {
00102 
00103 // Utility macro for some compile time checking.
00104 #define STATIC_ASSERT(COND,MSG) typedef char static_assertion_##MSG[(COND)?1:-1]
00105 
00106 using motoman::controller::Controller;
00107 
00108 class PVarQ
00109 // Holds data and functions for executing position variable queue motion
00110 {
00111   public:
00112     PVarQ();
00113     ~PVarQ(void);
00114     
00115     
00123     void init(industrial::joint_data::JointData & point, double velocity_percent);
00124     
00130     void addPoint(industrial::joint_data::JointData & joints, double velocity_percent);
00131     
00138     int bufferSize();
00139     
00140     
00147     int posVarQueueSize() {return QSIZE_;};
00148     
00149     
00156     int maxBufferSize() {return (posVarQueueSize() - PT_LOOK_AHEAD_);};
00157     
00158     
00164     int getMotionIndex()
00165     {return Controller::getInteger(MOTION_POINTER_);};
00166 
00172     int getBufferIndex()
00173     {return Controller::getInteger(BUFFER_POINTER_);};
00174   
00180     int getMotionPosIndex() ;
00181   
00182   
00188     int getBufferPosIndex();
00189     
00195     int getNextBufferPosIndex();
00196     
00202     bool bufferFull();
00203     
00204         
00210     bool bufferEmpty();
00211     
00212     
00218     double TEMP_getVelocityPercent()
00219     {return (double) Controller::getInteger(TEMP_VELOCITY_POINTER);};
00220   
00221   
00222                 
00223   protected:
00224 
00225         static const int VAR_POLL_TICK_DELAY_ = 10;
00226         
00230         static const int BUFFER_POLL_TICK_DELAY_ = 100;
00231         
00236     static const int QSIZE_ = 20;
00237     
00242     static const int PT_LOOK_AHEAD_ = 5;
00243     
00250     static const int MOTION_POINTER_ = 91;
00251     
00258     static const int BUFFER_POINTER_ = 92;
00259     
00260     
00267     static const int MIN_BUF_START_POINTER_ = 93;
00268     
00273     static const int TEMP_VELOCITY_POINTER = 94;
00274     
00275         
00280     void incBufferIndex();
00281    
00289     void setNextPosition(industrial::joint_data::JointData & point, double velocity_percent);
00290     
00299     void setPosition(int index, industrial::joint_data::JointData & point, 
00300     double velocity_percent);
00301                         
00302 };
00303 
00304 
00305 }//namespace p_var_q
00306 }//namespace motoman
00307 
00308 
00309 
00310 #endif


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