joint_motion_handler.h
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 #ifndef JOINT_MOTION_HANDLER_H
00034 #define JOINT_MOTION_HANDLER_H
00035 
00036 #include "motoPlus.h"
00037 #include "message_handler.h"
00038 #include "joint_message.h"
00039 #include "p_var_q.h"
00040 
00041 namespace motoman
00042 {
00043 namespace joint_motion_handler
00044 {
00045 
00049 //* MessageHandler
00050 
00058 /*
00059 Motoplus Functional Specs:
00060 
00061 Upon receiving...
00062 1st point - enable motion, start job, add point (increment buffer index), 
00063 Nth point - add point (increment buffer index)
00064 end of trajectory - wait until buffer size = 0, disable motion, stop job, reset buffer indicies
00065 motion stop - disable motion, stop job
00066 
00067 INFORM Functional Specs:
00068 
00069 start job - reset buffer index
00070 non-empty buffer - execute motion, increment motion pointer
00071 empty buffer - do not execute any motion (wait for points)
00072 end of position variables (jump to beginning position)
00073  
00074 TODO: WHAT TO DO IF THE END OF TRAJECTORY FLAG HAS BEEN RECEIVED AND
00075 THEN A MOTION CANCEL HAS BEEN RECEIVED.  MIGHT WANT TO CONSIDER USING
00076 AN EXTRA VARIABLE ON THE CONTROLLER FOR THE INFORM JOB TO MONITOR
00077 */
00078 
00079 
00080 class JointMotionHandler : public industrial::message_handler::MessageHandler
00081 {
00082 
00083 public:
00084 
00085 JointMotionHandler(void);
00086 ~JointMotionHandler(void);
00087 
00095 bool init(industrial::smpl_msg_connection::SmplMsgConnection* connection);
00096 
00105 bool init(int msg_type, industrial::smpl_msg_connection::SmplMsgConnection* connection)
00106 { return MessageHandler::init(msg_type, connection);};
00107 
00108 
00109 private:
00110 
00111 
00112 // Servo power variables
00113 MP_SERVO_POWER_SEND_DATA servo_power_data;
00114 MP_STD_RSP_DATA servo_power_error;
00115 
00116 // Job variables
00117 MP_START_JOB_SEND_DATA job_start_data;
00118 MP_CUR_JOB_SEND_DATA job_reset_data;
00119 MP_STD_RSP_DATA job_reset_error;
00120 MP_STD_RSP_DATA job_error;
00121 
00122 
00123 // Hold variables
00124 MP_HOLD_SEND_DATA hold_data;
00125 MP_STD_RSP_DATA hold_error;
00126         
00127 motoman::p_var_q::PVarQ pVarQ; 
00128  
00132  static const int MP_POLL_TICK_DELAY = 10;
00133  
00137  static const int BUFFER_POLL_TICK_DELAY = 1000;
00138  
00139  //TODO: motion and job flags are just internal state variables, we may
00140  //want to make them query the appropriate motoplus API calls instead.
00141  //This should just require rewriting the access functions below.
00145  bool motionEnabled;
00146  
00150  bool jobStarted;
00151  
00158 bool isMotionEnabled(void) {return motionEnabled;};
00159 
00166 bool isJobStarted(void) {return jobStarted;};
00167  
00175  bool internalCB(industrial::simple_message::SimpleMessage & in);
00176  
00177  
00184  void motionInterface(industrial::joint_message::JointMessage & jMsg);
00185  
00186  
00192 void enableMotion(void);
00193 
00199 void disableMotion(void);
00200 
00206 void startMotionJob(void);
00207         
00212 void stopMotionJob(void);
00213         
00214 };
00215 
00216 }//joint_motion_handler
00217 }//industrial
00218 
00219 
00220 #endif /* JOINT_MOTION_HANDLER_H */


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