controller.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 * Copyright (c) 2011, Southwest Research Institute
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *      * Redistributions of source code must retain the above copyright
 *      notice, this list of conditions and the following disclaimer.
 *      * Redistributions in binary form must reproduce the above copyright
 *      notice, this list of conditions and the following disclaimer in the
 *      documentation and/or other materials provided with the distribution.
 *      * Neither the name of the Southwest Research Institute, nor the names
 *      of its contributors may be used to endorse or promote products derived
 *      from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef CONTROLLER_H
#define CONTROLLER_H

#include "motoPlus.h"
#include "ParameterExtraction.h"  // new motoPlus library
#include "joint_data.h"
#include "robot_status.h"

namespace motoman
{
namespace controller
{

// bitmask used to track which parameters have been initialized
typedef enum
{
  GP_GETNUMBEROFAXES        = 0x01,
  GP_GETPULSETORAD          = 0x02,
  GP_GETFBPULSECORRECTION   = 0x04,
  GP_GETQTYOFALLOWEDTASKS   = 0x08,
  GP_GETINTERPOLATIONPERIOD = 0x10,
  GP_GETMAXINCPERIPCYCLE    = 0x20,
  GP_GETGOVFORINCMOTION     = 0x40,
} param_func_t;

struct ctrl_grp_param_t
{
  int initialized;
  int num_axes;
  float pulses_per_rad[MAX_PULSE_AXES];
  FB_AXIS_CORRECTION pulse_correction[MAX_PULSE_AXES];
  int max_incr_per_cycle[MAX_PULSE_AXES];
  float incr_motion_limit;
  
  ctrl_grp_param_t() : initialized(0) {}  // default constructor to guarantee un-initialized at creation
};

struct sys_param_t
{
  int initialized;
  TASK_QTY_INFO tasks;
  UINT16 interp_period;
  
  sys_param_t() : initialized(0) {}  // default constructor to guarantee un-initialized at creation
};


#define MP_DRAM_DEV_DOS "MPRAM1:"  //This macro is supposed to be defined in motoPlus.h

//* Controller
class Controller
{
public:

 Controller();
 
 ~Controller();
 
 static bool initParameters(int ctrl_grp=0);
 
static int getInteger(int index);


static void setInteger(int index, int value);

static bool setJointPositionVar(int index, industrial::joint_data::JointData ros_joints);

 void setDigitalOut(int bit_offset, bool value);
 
 void waitDigitalIn(int bit_offset, bool wait_value);
 
 static bool getActJointPos(float* pos);
  
  
 bool getStatus(industrial::robot_status::RobotStatus & status);
 
bool isMotionEnabled(void) {return motionEnabled;};

bool isJobStarted(void) {return jobStarted;};
  
void enableMotion(void);

void disableMotion(void);

void startMotionJob(char* job_name);
        
void stopMotionJob(char* job_name);

void delayTicks(int ticks) { mpTaskDelay(ticks);};

 bool writeJob(char* path, char* job);
 
 bool loadJob(char* path, char * job);
 
 double getVelocityLimit()
    {return (double) this->getInteger(VELOCITY_LIMIT_INDEX);};


  static bool getNumRobotAxes(int ctrl_grp, int* numAxes);
  static bool getNumRobotAxes(int* numAxes) { return getNumRobotAxes(active_ctrl_grp_, numAxes); }

 static bool getPulsesPerRadian(int ctrl_grp, float* pulses_per_radian);
 static bool getPulsesPerRadian(float* pulses_per_radian) { return getPulsesPerRadian(active_ctrl_grp_, pulses_per_radian); }

 static bool getFBPulseCorrection(int ctrl_grp, FB_AXIS_CORRECTION* pulse_correction);
 static bool getFBPulseCorrection(FB_AXIS_CORRECTION* pulse_correction) { return getFBPulseCorrection(active_ctrl_grp_, pulse_correction); }
 
 static bool getNumTasks(int* num_normal_tasks, int* num_highPrio_tasks, int* num_out_files);

 static bool getInterpPeriod(UINT16* interp_period);

 static bool getMaxIncrPerCycle(int ctrl_grp, int* max_incr);
 static bool getMaxIncrPerCycle(int* max_incr) { return getMaxIncrPerCycle(active_ctrl_grp_, max_incr); }

 static bool getIncrMotionLimit(int ctrl_grp, float* limit);
 static bool getIncrMotionLimit(float* limit) { return getIncrMotionLimit(active_ctrl_grp_, limit); }

 static int getCtrlGroup() { return active_ctrl_grp_; }
 static void setCtrlGroup(int ctrl_grp) { active_ctrl_grp_ = ctrl_grp; }

protected:

static const int VELOCITY_LIMIT_INDEX = 94;
                
 static const int MP_ERROR = -1;
 
 static const int MP_OK = 0;

 static const int VAR_POLL_DELAY_ = 10; //ms
 static const int UNIV_IN_DATA_START_ = 10;
 static const int UNIV_OUT_DATA_START_ = 10010;
 static const int UNIV_IO_DATA_SIZE_ = 2048;
 
 // Servo power variables
MP_SERVO_POWER_SEND_DATA servo_power_data;
MP_STD_RSP_DATA servo_power_error;

// Job variables
MP_START_JOB_SEND_DATA job_start_data;
MP_DELETE_JOB_SEND_DATA job_delete_data;
MP_STD_RSP_DATA job_error;


// Hold variables
MP_HOLD_SEND_DATA hold_data;
MP_STD_RSP_DATA hold_error;
 
//TODO: motion and job flags are just internal state variables, we may
//want to make them query the appropriate motoplus API calls instead.
bool motionEnabled;
 
bool jobStarted;

 industrial::robot_status::TriState getInMotionStatus();
 
static int active_ctrl_grp_;
 
static bool is_valid_ctrl_grp(int ctrl_grp);
static bool is_bit_set(int i, param_func_t type) { return (i & type); }
static void set_bit(int* i, param_func_t type) { *i |= type; }
};



} //controller
} //motoman

#endif //CONTROLLER_H