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