void RobotCom::_float()
{
CMsg mOut;
mOut.WriteMessageType(CONTROL_MODE);
mOut.WriteInt(FLOAT);
sendMessage(mOut);
}
// get_type = GET_CURTIME (gv.curTime),
// GET_JPOS (gv.q), GET_JVEL (gv.dq), GET_TORQ (gv.tau),
// GET_IPOS (gv.x)
void RobotCom::getStatus( UiToServoMessageType get_type, float *arg )
{
if( get_type != GET_CURTIME && get_type != GET_JPOS
&& get_type != GET_JVEL && get_type != GET_TORQ
&& get_type != GET_IPOS )
return;
CMsg mOut;
mOut.Init();
mOut.WriteMessageType( get_type );
mOut.WriteInt( GUI );
sendMessage(mOut);
short expectedMesgType;
int numOfData;
switch( get_type )
{
case GET_CURTIME:
expectedMesgType = CURTIME_DATA;
numOfData = 1;
break;
case GET_JPOS:
expectedMesgType = JPOS_DATA;
numOfData = 6;
break;
case GET_JVEL:
expectedMesgType = JVEL_DATA;
numOfData = 6;
break;
case GET_TORQ:
expectedMesgType = TORQ_DATA;
numOfData = 6;
break;
case GET_IPOS:
expectedMesgType = IPOS_DATA;
numOfData = 7;
break;
}
for(;;)
{
if( IsDataAvailable() )
{
CMappedMsg mIn = GetMsg();
short mesgType = mIn.ReadMessageType();
//printf("Type = %d\n", mesgType );
if( mesgType == expectedMesgType )
{
mIn.ReadFloat( arg, numOfData );
return;
}
}
}
}
void RobotCom::controlGripper( float voltage )
{
if( voltage < -10.0 || 10.0 < voltage ) return;
CMsg mOut;
mOut.Init();
mOut.WriteMessageType(GRIPPER);
mOut.WriteInt( (int)(voltage * 4096.0 / 10.0) );
sendMessage(mOut);
}
void RobotCom::control( ControlMode mode, float *arg, int numArgs )
{
CMsg mOut;
mOut.Init();
mOut.WriteMessageType(CONTROL_MODE);
mOut.WriteInt(mode);
if( numArgs > 0 )
mOut.WriteFloat( arg, numArgs );
sendMessage(mOut);
}
// get_type = GET_CURTIME (gv.curTime),
// GET_JPOS (gv.q), GET_JVEL (gv.dq), GET_TORQ (gv.tau),
// GET_IPOS (gv.x)
void RobotCom::getStatus( UiToServoMessageType get_type, float *arg )
{
CMsg mOut;
mOut.Init();
mOut.WriteMessageType( get_type );
mOut.WriteInt( GUI );
sendMessage(mOut);
short expectedMesgType = NO_MSSG;
switch( get_type )
{
case GET_CURTIME:
expectedMesgType = CURTIME_DATA;
break;
case GET_JPOS:
expectedMesgType = JPOS_DATA;
break;
case GET_JVEL:
expectedMesgType = JVEL_DATA;
break;
case GET_TORQ:
expectedMesgType = TORQ_DATA;
break;
case GET_IPOS:
expectedMesgType = IPOS_DATA;
break;
default:
std::cout<<"Warning: RobotCom::getStatus: Unsupported messagetype provided!"<<std::endl;
return;
break;
}
for(;;)
{
if( IsDataAvailable() )
{
CMappedMsg mIn = GetMsg();
short mesgType = mIn.ReadMessageType();
int numData = mIn.ReadInt();
if( mesgType == expectedMesgType )
{
mIn.ReadFloat( arg, numData );
return;
}
}
}
}
void RobotCom::jointControl( ControlMode mode, float q0, float q1, float q2, float q3, float q4, float q5)
{
if( mode != NJMOVE && mode != JMOVE
&& mode != NJGOTO && mode != JGOTO
&& mode != NJTRACK && mode != JTRACK )
return;
CMsg mOut;
mOut.Init();
mOut.WriteMessageType(CONTROL_MODE);
mOut.WriteInt(mode);
mOut.WriteFloat(q0);
mOut.WriteFloat(q1);
mOut.WriteFloat(q2);
mOut.WriteFloat(q3);
mOut.WriteFloat(q4);
mOut.WriteFloat(q5);
sendMessage(mOut);
}
void RobotCom::jointControl( ControlMode mode, float q0, float q1, float q2, float q3, float q4, float q5)
{
if( mode != NJMOVE && mode != JMOVE
&& mode != NJGOTO && mode != JGOTO
&& mode != NJTRACK && mode != JTRACK )
{
std::cout<<"Warning: RobotCom::jointControl: Unsupported messagetype provided!"<<std::endl;
return;
}
CMsg mOut;
mOut.Init();
mOut.WriteMessageType(CONTROL_MODE);
mOut.WriteInt(mode);
mOut.WriteFloat(q0);
mOut.WriteFloat(q1);
mOut.WriteFloat(q2);
mOut.WriteFloat(q3);
mOut.WriteFloat(q4);
mOut.WriteFloat(q5);
sendMessage(mOut);
}
