void JointMotionHandler::startMotionJob(void)
{

  this->jobStarted = false;
  
  this->enableMotion();
  
  LOG_INFO("Starting motion job");
  while(mpStartJob(&job_start_data, &job_error) == ERROR)
  {
    LOG_ERROR("Failed to start job, error: %d, retrying...", job_error.err_no);
    mpTaskDelay(this->MP_POLL_TICK_DELAY);
  };
  
  LOG_DEBUG("Waiting for indexes to reset");
  // The INFORM job should reset the index counters 
  while( (pVarQ.getMotionPosIndex() != 0) && ( pVarQ.getBufferPosIndex() != 0))
  {
    LOG_ERROR("Waiting for indexes to reset, retying...");
    mpTaskDelay(this->MP_POLL_TICK_DELAY);
  };
  
  LOG_DEBUG("Reset indexes, motion: %d, buffer: %d", pVarQ.getMotionPosIndex(),
    pVarQ.getBufferPosIndex());  
  
  this->jobStarted = true;
}
void JointMotionHandler::disableMotion(void)
{
  LOG_INFO("Disabling motion");
  servo_power_data.sServoPower = OFF;
  while(mpSetServoPower(&servo_power_data, &servo_power_error) == ERROR)
  {
    LOG_ERROR("Failed to turn off servo power, error: %d, retrying...", servo_power_error.err_no);
    mpTaskDelay(this->MP_POLL_TICK_DELAY);
  };
  
  this->motionEnabled = false;
}
Beispiel #3
0
void Controller::enableMotion(void)
{
  
  LOG_INFO("Enabling motion");
  this->motionEnabled = false;

  servo_power_data.sServoPower = ON;
  while(mpSetServoPower(&servo_power_data, &servo_power_error) == MP_ERROR)
  {
    LOG_ERROR("Failed to turn on servo power, error: %d, retrying...", servo_power_error.err_no);
    mpTaskDelay(this->VAR_POLL_DELAY_);
  };
  
  hold_data.sHold = OFF;
  while(mpHold(&hold_data, &hold_error) == MP_ERROR)
  {
    LOG_ERROR("Failed to turn off hold, error: %d, retrying...", hold_error.err_no);
    mpTaskDelay(this->VAR_POLL_DELAY_);
  };
  
  this->motionEnabled = true;
}
Beispiel #4
0
void RosInitTask()
{
	Controller ros_controller;

	if(!Ros_Controller_Init(&ros_controller))
	{
		//set feedback signal to notify failure
		Ros_Controller_SetIOState(IO_FEEDBACK_FAILURE, TRUE);
		mpDeleteSelf;
		return;
	}

	ros_controller.tidConnectionSrv = mpCreateTask(MP_PRI_TIME_NORMAL, MP_STACK_SIZE, 
						(FUNCPTR)Ros_Controller_ConnectionServer_Start,
						(int)&ros_controller, 0, 0, 0, 0, 0, 0, 0, 0, 0);
		
#ifdef DX100
	// DX100 need to execute a SKILLSEND command prior to the WAIT in order for the 
	// incremental motion function to work.  These tasks monitor for those commands
	// This supports a maximum of two robots which should be assigned to slave id
	// MP_SL_ID1 and MP_SL_ID2 respectively.
	
	// first robot
	ros_controller.RosListenForSkillID[0] = mpCreateTask(MP_PRI_TIME_NORMAL, MP_STACK_SIZE, 
						(FUNCPTR)Ros_Controller_ListenForSkill,
						(int)&ros_controller, MP_SL_ID1, 0, 0, 0, 0, 0, 0, 0, 0);
	// if second robot
	if(ros_controller.numRobot > 1)
	{
		ros_controller.RosListenForSkillID[1] = mpCreateTask(MP_PRI_TIME_NORMAL, MP_STACK_SIZE, 
							(FUNCPTR)Ros_Controller_ListenForSkill,
							(int)&ros_controller, MP_SL_ID2, 0, 0, 0, 0, 0, 0, 0, 0);	
	}
	else
	{
		ros_controller.RosListenForSkillID[1] = INVALID_TASK;
	}
#endif
		
	// start loop to monitor controller state
	FOREVER
	{
		// Check controller status
		if (!Ros_Controller_StatusUpdate(&ros_controller))
			puts("Failed to update controller status.  Check robot parameters.");
	
		mpTaskDelay(CONTROLLER_STATUS_UPDATE_PERIOD);
	}
}
void JointMotionHandler::stopMotionJob(void)
{  
  LOG_INFO("Stopping motion job");
  this->disableMotion();
  
  // Reseting the job.
  job_reset_data.usJobLine = 1;  //Job lines start at 1
  while(mpSetCurJob(&job_reset_data, &job_reset_error) == ERROR)
  {
    LOG_ERROR("Failed to reset job, error: %d, retrying...", job_reset_error.err_no);
    mpTaskDelay(this->MP_POLL_TICK_DELAY);
  };
  
  this->jobStarted = false;
}	
Beispiel #6
0
void Controller::setInteger(int index, int value)
{
	MP_VAR_DATA data;
	
	data.usType = MP_RESTYPE_VAR_I;
	data.usIndex = index;
	data.ulValue = value;
	
	while (mpPutVarData ( &data, 1 ) == MP_ERROR) 
	{
        LOG_ERROR("Failed to set integer varaible, index: %d, value: %d, retrying...", 
            data.usIndex, data.ulValue);
        mpTaskDelay(VAR_POLL_DELAY_);
    }
}
Beispiel #7
0
void mpUsrRoot(int arg1, int arg2, int arg3, int arg4, int arg5, int arg6, int arg7, int arg8, int arg9, int arg10)
{

//#ifdef DX100
	mpTaskDelay(10000);  // 10 sec. delay to enable DX100 system to complete initialization
//#endif
	
	//Creates and starts a new task in a seperate thread of execution.
	//All arguments will be passed to the new task if the function
	//prototype will accept them.
	RosInitTaskID = mpCreateTask(MP_PRI_TIME_NORMAL, MP_STACK_SIZE, (FUNCPTR)RosInitTask,
						arg1, arg2, arg3, arg4, arg5, arg6, arg7, arg8, arg9, arg10);
									
	//Ends the initialization task.
	mpExitUsrRoot;
}
Beispiel #8
0
void Controller::stopMotionJob(char* job_name)
{  
  LOG_INFO("Stopping motion job");
  this->disableMotion();
  
  // delete task
  strcpy(job_delete_data.cJobName, job_name);
  
  while(mpDeleteJob(&job_delete_data, &job_error) == MP_ERROR)
  {
    LOG_ERROR("Failed to delete job, error: %d, retrying...", job_error.err_no);
    mpTaskDelay(this->VAR_POLL_DELAY_);
  };
  
  this->jobStarted = false;
}	
Beispiel #9
0
int Controller::getInteger(int index)
{
    
	MP_VAR_INFO info;
	LONG rtn;
	
	info.usType = MP_RESTYPE_VAR_I;
	info.usIndex = index;
	
	while (mpGetVarData ( &info, &rtn, 1 ) == MP_ERROR) 
	{
        LOG_ERROR("Failed to retreive integer variable index: %d, retrying...", info.usIndex);
        mpTaskDelay(VAR_POLL_DELAY_);
    }
    return rtn;
}
 void JointMotionHandler::motionInterface(JointMessage & jMsg)
 {
//
// Upon receiving...
// 1st point - initialize buffer, enable motion, start job, add point (increment buffer index), 
// Nth point - add point (increment buffer index)
// end of trajectory - wait until buffer size = 0, disable motion, stop job, reset buffer indicies
//motion stop - disable motion, stop job

  JointData joints;
  
   switch (jMsg.getSequence())
    {
      case SpecialSeqValues::END_TRAJECTORY:
         LOG_INFO("Received end trajectory command");
         while(!pVarQ.bufferEmpty())
         {
           LOG_DEBUG("Waiting for motion buffer to empty");
           mpTaskDelay(this->BUFFER_POLL_TICK_DELAY);
         };
         this->stopMotionJob();
           
         break;
         
      case SpecialSeqValues::STOP_TRAJECTORY:
         LOG_WARN("Received stop command, stopping motion immediately");
         this->stopMotionJob();
         break;
         
      default:
        
        joints.copyFrom(jMsg.getJoints());
        if (!(this->isJobStarted()))
        {
          //TODO: The velocity should be set from the message in the future.
          pVarQ.init(joints, 0.0);
          this->startMotionJob();
        }
        else
        {
          pVarQ.addPoint(joints, 0.0);
        }
    }
 }
Beispiel #11
0
void Controller::startMotionJob(char* job_name)
{

  this->jobStarted = false;
  
  this->enableMotion();
  
  // Set up job variables
  job_start_data.sTaskNo = 0;
  strcpy(job_start_data.cJobName, job_name);
 
  
  LOG_INFO("Starting motion job");
  while(mpStartJob(&job_start_data, &job_error) == ERROR)
  {
    LOG_ERROR("Failed to start job, error: %d, retrying...", job_error.err_no);
    mpTaskDelay(this->VAR_POLL_DELAY_);
  }; 
  
  this->jobStarted = true;
}
Beispiel #12
0
//-----------------------------------------------------------------------
// Send state (robot position) as long as there is an active connection
//-----------------------------------------------------------------------
void Ros_StateServer_SendState(Controller* controller)
{
	BOOL bHasConnections = TRUE;
	int groupNo;
	SimpleMsg sendMsg;
	int msgSize;
	
	printf("Starting State Server Send State task\r\n");
	
	while(bHasConnections)
	{
		// Send position for each control group
		for(groupNo=0; groupNo < controller->numGroup; groupNo++)
		{
			msgSize = Ros_SimpleMsg_JointFeedback(controller->ctrlGroups[groupNo], &sendMsg);
			if(msgSize > 0)
			{
				bHasConnections = Ros_StateServer_SendMsgToAllClient(controller, &sendMsg, msgSize);
			}
		}

		// Send controller/robot status
		if(bHasConnections)
		{
			msgSize = Ros_Controller_StatusToMsg(controller, &sendMsg);
			if(msgSize > 0)
			{
				bHasConnections = Ros_StateServer_SendMsgToAllClient(controller, &sendMsg, msgSize);
			}
		}
		mpTaskDelay(STATE_UPDATE_MIN_PERIOD);
	}
	
	// Terminate this task
	controller->tidStateSendState = INVALID_TASK;
	Ros_Controller_SetIOState(IO_FEEDBACK_STATESERVERCONNECTED, FALSE);
	printf("State Server Send State task was terminated\r\n");
	mpDeleteSelf;
}
Beispiel #13
0
 void Controller::waitDigitalIn(int bit_offset, bool wait_value)
 {
   LOG_DEBUG("Waiting for digital in, Bit offset: %d, Wait value: %d", bit_offset, wait_value);
   if ( (bit_offset < this->UNIV_IO_DATA_SIZE_) && 
       ( bit_offset > 0) )
  { 
    MP_IO_INFO info;
    info.ulAddr = this->UNIV_IN_DATA_START_ + bit_offset;
    
    USHORT readValue;
    do
    {
      readValue = !wait_value;  
      //TODO: The return result of mpReadIO is not checked
      mpReadIO (&info, &readValue, 1);
      mpTaskDelay(VAR_POLL_DELAY_);
    } while ( ((bool)readValue) != wait_value);
  }
  else
  {
    LOG_ERROR("Bit offset: %d, is greater than size: %d", bit_offset, this->UNIV_IO_DATA_SIZE_);
  }
 }