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;
}
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;
}
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;
}
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_);
}
}
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;
}
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;
}
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);
}
}
}
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;
}
//-----------------------------------------------------------------------
// 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;
}
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_);
}
}