//ID 80, SetPTPJointParams
static void SetPTPJointParams(PTPJointParams ptpJointParams)
{
Program AllFunctions = new Program();
AllFunctions.ConnectToServer();
byte[] DATA_TO_SEND = new byte[38];
//info of the functon
Message message = new Message();
// set ID 80 INFO
message.id = 80;
message.rw = 1;
message.isQueued = 0;
message.paramsLen = 34;
//Start set functions
//Header
DATA_TO_SEND[0] = 170; //0xAA
DATA_TO_SEND[1] = 170; //0xAA
// // Params
DATA_TO_SEND[2] = message.paramsLen; //Len
DATA_TO_SEND[3] = message.id; //ID
DATA_TO_SEND[4] = 0; //Ctrl
byte[] tempByte = new byte[16];
////////////////////
// CHECK IF NULL
///////////////////
Buffer.BlockCopy(ptpJointParams.velocity, 0, tempByte, 0, tempByte.Length);//velocity Parameters
for (int i = 0; i <= 15; i++)
{
DATA_TO_SEND[5 + i] = tempByte[i];
}
Buffer.BlockCopy(ptpJointParams.acceleration, 0, tempByte, 0, tempByte.Length);//acceleration Parameters
for (int i = 0; i <= 15; i++)
{
DATA_TO_SEND[21 + i] = tempByte[i];
}
DATA_TO_SEND[37] = message.paramsLen;
//Send instruction
string hexString = string.Empty;
hexString = ToHexString(DATA_TO_SEND);
AllFunctions.Send(hexString);
}
static void Main(string[] args)
{
Program AllFunctions = new Program();
//connect to server
AllFunctions.ConnectToServer();
/* ********************************************************** */
//Please Write Code Below
/* ********************************************************** */
PTPJointParams ptpJointParams = new PTPJointParams();
JOGCmd jogCmd = new JOGCmd();
PTPCoordinateParams ptpCoordinateParams = new PTPCoordinateParams();
PTPCmd ptpCmd = new PTPCmd();
//Set up the velocity of inverse kinematics
ptpCoordinateParams.xyzVelocity = 100;
SetPTPCoordinateParams(ptpCoordinateParams);
//Set up the velocity of forward kinematics
ptpJointParams.velocity[0] = 50F;
ptpJointParams.velocity[1] = 50F;
ptpJointParams.velocity[2] = 50F;
SetPTPJointParams(ptpJointParams);
//Use inverse kinematics, move to coordinate（-270, 140, 95）
ptpCmd.ptpMode = 1;
ptpCmd.x = -270F;
ptpCmd.y = 140F;
ptpCmd.z = 95F;
SetPTPCmd(ptpCmd);
//Turn on the suction cup
SetEndEffectorSuctionCup(true);
//Use forward kinematics, move to coordinate（130, 45, 56.9）
ptpCmd.ptpMode = 3;
ptpCmd.x = 130F;
ptpCmd.y = 45F;
ptpCmd.z = 56.9F;
SetPTPCmd(ptpCmd);
//Turn off the suction cup and release the object
SetEndEffectorSuctionCup(false);
/* ********************************************************** */
//Please Write Code Above
/* ********************************************************** */
Console.WriteLine("Program End");
}
