private void blurSlider_MouseLeftButtonUp(object sender, MouseButtonEventArgs e)
{
if (!isConnectted)
{
return;
}
UInt64 cmdIndex = 0;
Slider obj = (Slider)sender;
if (obj.Name == "sld")
{
JOGCommonParams jdParam;
jdParam.velocityRatio = (float)sld.Value;
jdParam.accelerationRatio = 100;
DobotDll.SetJOGCommonParams(ref jdParam, false, ref cmdIndex);
}
else if (obj.Name == "sld1" || obj.Name == "sldAcc") // playback
{
PTPCommonParams pbdParam;
pbdParam.velocityRatio = (float)sld1.Value;
pbdParam.accelerationRatio = (float)sldAcc.Value;
DobotDll.SetPTPCommonParams(ref pbdParam, false, ref cmdIndex);
}
}
private void placeCard(int x, int y, UInt64 cmdIndex)
{
moveDobot(x, y, 50, cmdIndex);
moveDobot(x, y, -70, cmdIndex);
DobotDll.SetEndEffectorSuctionCup(true, false, true, ref cmdIndex);
moveDobot(x, y, 50, cmdIndex);
}
private void AlarmTest()
{
int ret;
byte[] alarmsState = new byte[32];
UInt32 len = 32;
ret = DobotDll.GetAlarmsState(alarmsState, ref len, alarmsState.Length);
for (int i = 0; i < alarmsState.Length; i++)
{
byte alarm = alarmsState[i];
for (int j = 0; j < 8; j++)
{
if ((alarm & 0x01 << j) > 0)
{
int alarmIndex = i * 8 + j;
switch (alarmIndex)
{
case 0x00:
{ // reset
//Get Alarm status: reset
break;
}
/* other status*/
default:
break;
}
}
}
}
//DobotDll.ClearAllAlarmsState();
}
/// <summary>
/// StartDobot
/// </summary>
private void StartDobot()
{
StringBuilder fwType = new StringBuilder(60);
StringBuilder version = new StringBuilder(60);
int ret = DobotDll.ConnectDobot("", 115200, fwType, version);
// start connect
if (ret != (int)DobotConnect.DobotConnect_NoError)
{
Console.WriteLine("Connect error", Utils.MsgInfoType.Error);
return;
}
Console.WriteLine("Connect success", Utils.MsgInfoType.Info);
_isConnectted = true;
DobotDll.SetCmdTimeout(3000);
// Must set when sensor is not exist
//DobotDll.ResetPose(true, 45, 45);
// Get name
string deviceName = "Dobot Magician";
DobotDll.SetDeviceName(deviceName);
StringBuilder deviceSN = new StringBuilder(64);
DobotDll.GetDeviceName(deviceSN, 64);
SetParam();
}
private void PosTimer_Tick(object sender, EventArgs e)
{
if (!isConnectted)
{
return;
}
Pose pose = new Pose();
DobotDll.GetPose(ref pose);
tbJoint1Angle.Text = pose.jointAngle[0].ToString();
tbJoint2Angle.Text = pose.jointAngle[1].ToString();
tbJoint3Angle.Text = pose.jointAngle[2].ToString();
tbJoint4Angle.Text = pose.jointAngle[3].ToString();
if (sync.IsChecked == true)
{
isGrab.IsChecked = pose.isGrab == 1;
X.Text = pose.x.ToString();
Y.Text = pose.y.ToString();
Z.Text = pose.z.ToString();
rHead.Text = pose.rHead.ToString();
pauseTime.Text = "0";
isGripper.Text = pose.gripper.ToString();
}
}
private void PosTimer_Tick(object sender, System.Timers.ElapsedEventArgs e)
{
if (!isConnectted)
{
return;
}
DobotDll.GetPose(ref pose);
this.Dispatcher.BeginInvoke((Action) delegate()
{
tbJoint1Angle.Text = pose.jointAngle[0].ToString();
tbJoint2Angle.Text = pose.jointAngle[1].ToString();
tbJoint3Angle.Text = pose.jointAngle[2].ToString();
tbJoint4Angle.Text = pose.jointAngle[3].ToString();
if (sync.IsChecked == true)
{
X.Text = pose.x.ToString();
Y.Text = pose.y.ToString();
Z.Text = pose.z.ToString();
rHead.Text = pose.rHead.ToString();
pauseTime.Text = "0";
}
});
}
/// <summary>
/// ホーミング
/// </summary>
public void Homing()
{
HOMECmd homeCmd = new HOMECmd();
ulong cmdIndex = 0;
DobotDll.SetHOMECmd(ref homeCmd, false, ref cmdIndex);
}
private UInt64 arc(float x, float y, float z, float r, float x1, float y1, float z1, float r1)
{
UInt64 cmdIndex = 0;
ARCCmd arcCmd;
arcCmd.cirPoint_x = x;
arcCmd.cirPoint_y = y;
arcCmd.cirPoint_z = z;
arcCmd.cirPoint_r = r;
arcCmd.toPoint_x = x1;
arcCmd.toPoint_y = y1;
arcCmd.toPoint_z = z1;
arcCmd.toPoint_r = r1;
while (true)
{
int ret = DobotDll.SetARCCmd(ref arcCmd, true, ref cmdIndex);
if (ret == 0)
{
break;
}
}
return(cmdIndex);
}
//Überprüft alle 100ms die aktuelle Position, wenn ein Roboter verbunden ist
private void PosTimer_Tick(object sender, EventArgs e)
{
if (!isConnectted)
{
return;
}
DobotDll.GetPose(ref pose);
}
private void pickNewCard(UInt64 cmdIndex, WAITCmd wait)
{
moveDobot(220, 140, 50, cmdIndex);
moveDobot(220, 140, -55, cmdIndex);
DobotDll.SetEndEffectorSuctionCup(true, true, true, ref cmdIndex);
moveDobot(220, 140, 50, cmdIndex);
DobotDll.SetWAITCmd(ref wait, true, ref cmdIndex);
}
/// <summary>
/// StartDobot
/// </summary>
private void StartDobot()
{
int ret;
// start connect
if ((ret = DobotDll.ConnectDobot()) >= (int)DobotResult.DobotResult_Error_Min)
{
Msg("Connect error ,code:" + ret.ToString(), MsgInfoType.Error);
return;
}
isConnectted = true;
// reset and start
//DobotDll.ResetDobot();
DobotDll.SetCmdTimeout(500);
DobotDll.SetEndType(EndType.EndTypePump);
// Must set when sensor is not exist
InitialPose initialPose;
initialPose.joint2Angle = 45;
initialPose.joint3Angle = 45;
DobotDll.SetInitialPose(ref initialPose);
JogStaticParams jsParam;
jsParam.jointMaxVelocity = 15;
jsParam.jointMaxAcceleration = 50;
jsParam.servoMaxVelocity = 30;
jsParam.servoMaxAcceleration = 10;
jsParam.linearMaxVelocity = 40;
jsParam.linearMaxAcceleration = 40;
DobotDll.SetJogStaticParams(ref jsParam);
JogDynamicParams jdParam;
jdParam.velocityRatio = 30;
DobotDll.SetJogDynamicParams(ref jdParam);
PlaybackStaticParams pbsParam;
pbsParam.jointMaxVelocity = 200;
pbsParam.jointMaxAcceleration = 200;
pbsParam.servoMaxVelocity = 200;
pbsParam.servoMaxAcceleration = 200;
pbsParam.linearMaxVelocity = 800;
pbsParam.linearMaxAcceleration = 1000;
pbsParam.pauseTime = 100;
pbsParam.jumpHeight = 20;
DobotDll.SetPlaybackStaticParams(ref pbsParam);
PlaybackDynamicParams pbdParam;
pbdParam.velocityRatio = 30;
pbdParam.accelerationRatio = 30;
DobotDll.SetPlaybackDynamicParams(ref pbdParam);
}
/// <summary>
/// StartDobot
/// </summary>
private void StartDobot()
{
int ret = DobotDll.ConnectDobot("", 115200);
// start connect
if (ret != (int)DobotConnect.DobotConnect_NoError)
{
Console.WriteLine("Connect error", MsgInfoType.Error);
return;
}
Console.WriteLine("Connect success", MsgInfoType.Info);
isConnectted = true;
DobotDll.SetCmdTimeout(3000);
// Must set when sensor is not exist
DobotDll.ResetPose(true, 45, 45);
UInt64 cmdIndex = 0;
JOGJointParams jsParam;
jsParam.velocity = new float[] { 200, 200, 200, 200 };
jsParam.acceleration = new float[] { 200, 200, 200, 200 };
DobotDll.SetJOGJointParams(ref jsParam, false, ref cmdIndex);
JOGCommonParams jdParam;
jdParam.velocityRatio = 100;
jdParam.accelerationRatio = 100;
DobotDll.SetJOGCommonParams(ref jdParam, false, ref cmdIndex);
PTPJointParams pbsParam;
pbsParam.velocity = new float[] { 200, 200, 200, 200 };
pbsParam.acceleration = new float[] { 200, 200, 200, 200 };
DobotDll.SetPTPJointParams(ref pbsParam, false, ref cmdIndex);
PTPCoordinateParams cpbsParam;
cpbsParam.xyzVelocity = 100;
cpbsParam.xyzAcceleration = 100;
cpbsParam.rVelocity = 100;
cpbsParam.rAcceleration = 100;
DobotDll.SetPTPCoordinateParams(ref cpbsParam, false, ref cmdIndex);
PTPJumpParams pjp;
pjp.jumpHeight = 20;
pjp.zLimit = 100;
DobotDll.SetPTPJumpParams(ref pjp, false, ref cmdIndex);
PTPCommonParams pbdParam;
pbdParam.velocityRatio = 30;
pbdParam.accelerationRatio = 30;
DobotDll.SetPTPCommonParams(ref pbdParam, false, ref cmdIndex);
}
private void StartPeriodic()
{
//Startet Comand-Timer, wird alle 100ms aufgerungen
cmdTimer = new System.Threading.Timer(state => DobotDll.PeriodicTask(), null, 0, 100);
//Startet Positions-Timer, wird alle 100ms aufgerufen
posTimer = new System.Threading.Timer(state => Positionsbestimmung(), null, 0, 100);
}
/// <summary>
/// 作業を開始します
/// </summary>
public void WorkStart(double target_x, double target_y, SettingsObj obj)
{
// 作業をクリア
DobotDll.SetQueuedCmdClear();
// コマンド開始
DobotDll.SetQueuedCmdStartExec();
OnWorkEvent?.Invoke(this, "作業開始");
// 現在位置を取得
var pose = GetCurrentPose();
OnWorkEvent(this, $"現在位置を取得:");
OnWorkEvent?.Invoke(this, $"X:{pose.x}, Y:{pose.y}, Z:{pose.z}, R:{pose.rHead}");
// 対象の上部まで移動(Z座標は適当)
var cmdIndex = ptp((byte)2, (float)target_x, (float)target_y, (float)0.0, pose.rHead);
OnWorkEvent?.Invoke(this, "対象の上部まで移動:");
OnWorkEvent?.Invoke(this, $"X:{(float)target_x}, Y:{(float)target_y}, Z:{(float)0.0}, R:{pose.rHead}");
// 対象物のZ座標計算
var object_z = obj.PedestalZ + obj.ObjectHeight;
OnWorkEvent?.Invoke(this, "対象物のZ座標:");
OnWorkEvent?.Invoke(this, $"Z:{object_z}");
// 対象の位置まで下がる
cmdIndex = ptp((byte)2, (float)target_x, (float)target_y, (float)object_z, pose.rHead);
OnWorkEvent?.Invoke(this, "対象物までアームを降ろす:");
OnWorkEvent?.Invoke(this, $"X:{(float)target_x}, Y:{(float)target_y}, Z:{(float)object_z}, R:{pose.rHead}");
// 下がったらサクションカップONしてつかむ
DobotDll.SetEndEffectorSuctionCup(true, true, true, ref cmdIndex);
OnWorkEvent?.Invoke(this, "サクションカップでつかむ");
// いったん上に持ち上げる
cmdIndex = ptp((byte)2, (float)target_x, (float)target_y, (float)0.0, pose.rHead);
OnWorkEvent?.Invoke(this, $"持ち上げる:");
OnWorkEvent?.Invoke(this, $"X:{(float)target_x}, Y:{(float)target_y}, Z:{(float)0.0}, R:{pose.rHead}");
// 持っていく場所に移動
cmdIndex = ptp((byte)2, (float)obj.PlacePoseXCoordinate, (float)obj.PlacePoseYCoordinate, (float)obj.PlacePoseZCoordinate, pose.rHead);
OnWorkEvent?.Invoke(this, $"持っていく場所に移動:");
OnWorkEvent?.Invoke(this, $"X:{(float)obj.PlacePoseXCoordinate}, Y:{(float)obj.PlacePoseYCoordinate}, Z:{(float)obj.PlacePoseZCoordinate}, R:{pose.rHead}");
// 下がったらサクションカップOFFして放す
DobotDll.SetEndEffectorSuctionCup(false, false, false, ref cmdIndex);
OnWorkEvent?.Invoke(this, "サクションカップOFFして放す");
// 上がって終わり
cmdIndex = ptp((byte)2, (float)obj.PlacePoseXCoordinate, (float)obj.PlacePoseYCoordinate, (float)0.0, pose.rHead);
OnWorkEvent?.Invoke(this, "アームを上にあげる");
// コマンド終了
DobotDll.SetQueuedCmdStopExec();
OnWorkEvent?.Invoke(this, "作業終了");
}
private void btndobotbewegung_Click(object sender, EventArgs e)
{
//speichern der Position, die zum Anfahr-Zeitpunkt aktuelle ist
float xAkt = pose.x;
float yAkt = pose.y;
float zAkt = pose.z;
foreach (Linie l in Arbeitskopie)
{
// anzahl der koordinaten innerhalb einer linie erfassen und aktuelle_position-count setzen
int liniencount = l.Count;
int akt_pos = 0;
//abarbeiten aller linien und der beinhaltenden koordinaten
foreach (Koordinate k in l)
{
//ist die Koordinate ein Anfangspunkt einer linie, soll "gesprungen" werden (motionstyle = 0);
if (akt_pos == 0)
{
//angeben der koordinaten ( x,y,z,) die sich aus dem ausgangspunkt und der tatsächlichen koordinate zusammensetzt, z = const
pdbCmd.playbackPoint.motionStyle = 0; //springt zum anfangspunkt
pdbCmd.playbackPoint.x = xAkt + k.X;
pdbCmd.playbackPoint.y = yAkt + k.Y;
pdbCmd.playbackPoint.z = zAkt;
pdbCmd.playbackPoint.pauseTime = 0;
//übergeben des comand-obj (pdbCmd) an der Playbackbuffer, wird beim aufrufen des comand-timers erst wirklich ausgelöst
DobotDll.SetPlaybackBufferCmd(ref pdbCmd);
//0.5s warten, bis der roboter die bewegung ausgeführt hat, erst dann mit der schleifenabarbeitung fortsetzen
Thread.Sleep(500);
akt_pos++;
}
//Ist die Koordinate kein Anfangspunkt soll normal und liniar verfahren werden (motionstyle = 2)
else
{
//angeben der koordinaten ( x,y,z,) die sich aus dem ausgangspunkt und der tatsächlichen koordinate zusammensetzt, z = const
pdbCmd.playbackPoint.motionStyle = 2;
pdbCmd.playbackPoint.x = xAkt + k.X;
pdbCmd.playbackPoint.y = yAkt + k.Y;
pdbCmd.playbackPoint.z = zAkt;
//übergeben des comand-obj (pdbCmd) an der Playbackbuffer, wird beim aufrufen des comand-timers erst wirklich ausgelöst
DobotDll.SetPlaybackBufferCmd(ref pdbCmd);
//0.5s warten, bis der roboter die bewegung ausgeführt hat, erst dann mit der schleifenabarbeitung fortsetzen
Thread.Sleep(500);
akt_pos++;
}
}
}
//schließen der Form nach Abarbeiten aller Linien
this.Close();
}
private void moveDobot(int x, int y, int z, UInt64 cmdIndex)
{
PTPCmd ptpCmd;
ptpCmd.ptpMode = (byte)PTPMode.PTPMOVLXYZMode;
ptpCmd.x = x;
ptpCmd.y = y;
ptpCmd.z = z;
ptpCmd.rHead = 0;
DobotDll.SetPTPCmd(ref ptpCmd, true, ref cmdIndex);
}
/// <summary>
/// タイマーで定期的に現在位置を取得します
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void PosTimer_Tick(object sender, System.Timers.ElapsedEventArgs e)
{
if (!_isConnectted)
{
return;
}
DobotDll.GetPose(ref _pose);
// コンソール出力
Console.WriteLine($"x:{_pose.x}");
Console.WriteLine($"y:{_pose.y}");
Console.WriteLine($"z:{_pose.z}");
Console.WriteLine($"R:{_pose.rHead}");
}
private void EIOTest()
{
UInt64 cmdIndex = 0;
//EIO
IOMultiplexing iom;
iom.address = 2; //io index
iom.multiplex = (byte)IOFunction.IOFunctionDO; // ioType
DobotDll.SetIOMultiplexing(ref iom, false, ref cmdIndex);
IODO iod;
iod.address = 2;
iod.level = 1; // set io index 2 to open
DobotDll.SetIODO(ref iod, false, ref cmdIndex);
}
private void StartDobot()
{
StringBuilder fwType = new StringBuilder(60);
StringBuilder version = new StringBuilder(60);
int ret = DobotDll.ConnectDobot("", 115200, fwType, version);
// start connect
if (ret != (int)DobotConnect.DobotConnect_NoError)
{
Msg("Connect error", MsgInfoType.Error);
MessageBox.Show("Connect Error!");
return;
}
Msg("Connect success", MsgInfoType.Info);
MessageBox.Show("Connect Success!");
isConnectted = true;
DobotDll.SetCmdTimeout(3000);
// Must set when sensor is not exist
//DobotDll.ResetPose(true, 45, 45);
// Get name
string deviceName = "Dobot Magician";
DobotDll.SetDeviceName(deviceName);
StringBuilder deviceSN = new StringBuilder(64);
DobotDll.GetDeviceName(deviceSN, 64);
StartGetPose();
// SetParam();
//EIOTest();
//ARCTest();
// AlarmTest();
EndTypeParams e = new EndTypeParams();
DobotDll.GetEndEffectorParams(ref e);
MessageBox.Show(e.xBias.ToString() + " " + e.yBias.ToString() + " " + e.zBias.ToString());
}
/// <summary>
/// 初期パラメータ設定
/// </summary>
private void SetParam()
{
UInt64 cmdIndex = 0;
JOGJointParams jsParam;
jsParam.velocity = new float[] { 200, 200, 200, 200 };
jsParam.acceleration = new float[] { 200, 200, 200, 200 };
DobotDll.SetJOGJointParams(ref jsParam, false, ref cmdIndex);
JOGCommonParams jdParam;
jdParam.velocityRatio = 100;
jdParam.accelerationRatio = 100;
DobotDll.SetJOGCommonParams(ref jdParam, false, ref cmdIndex);
PTPJointParams pbsParam;
pbsParam.velocity = new float[] { 200, 200, 200, 200 };
pbsParam.acceleration = new float[] { 200, 200, 200, 200 };
DobotDll.SetPTPJointParams(ref pbsParam, false, ref cmdIndex);
PTPCoordinateParams cpbsParam;
cpbsParam.xyzVelocity = 100;
cpbsParam.xyzAcceleration = 100;
cpbsParam.rVelocity = 100;
cpbsParam.rAcceleration = 100;
DobotDll.SetPTPCoordinateParams(ref cpbsParam, false, ref cmdIndex);
PTPJumpParams pjp;
pjp.jumpHeight = 20;
pjp.zLimit = 100;
DobotDll.SetPTPJumpParams(ref pjp, false, ref cmdIndex);
PTPCommonParams pbdParam;
pbdParam.velocityRatio = 30;
pbdParam.accelerationRatio = 30;
DobotDll.SetPTPCommonParams(ref pbdParam, false, ref cmdIndex);
}
private UInt64 ptp(byte style, float x, float y, float z, float r)
{
PTPCmd pdbCmd;
UInt64 cmdIndex = 0;
pdbCmd.ptpMode = style;
pdbCmd.x = x;
pdbCmd.y = y;
pdbCmd.z = z;
pdbCmd.rHead = r;
while (true)
{
int ret = DobotDll.SetPTPCmd(ref pdbCmd, true, ref cmdIndex);
if (ret == 0)
{
break;
}
}
return(cmdIndex);
}
private UInt64 cp(byte mod, float x, float y, float z, float velocity)
{
CPCmd pdbCmd;
UInt64 cmdIndex = 0;
pdbCmd.cpMode = mod;
pdbCmd.x = x;
pdbCmd.y = y;
pdbCmd.z = z;
pdbCmd.velocity = velocity;
while (true)
{
int ret = DobotDll.SetCPCmd(ref pdbCmd, true, ref cmdIndex);
if (ret == 0)
{
break;
}
}
return(cmdIndex);
}
// event handle
private void ProcessEvt(object sender, EventArgs e)
{
if (!isConnectted)
{
return;
}
Button obj = (Button)sender;
String con = obj.Content.ToString();
switch (con)
{
case "SendPlaybackCmd":
{
PlaybackBufferCmd pdbCmd;
float x, y, z, r, gripper, pTime;
if (!float.TryParse(X.Text, out x) || !float.TryParse(Y.Text, out y) || !float.TryParse(Z.Text, out z) || !float.TryParse(rHead.Text, out r) ||
!float.TryParse(isGripper.Text, out gripper) || !float.TryParse(pauseTime.Text, out pTime))
{
Msg("Please input float formate", MsgInfoType.Error);
return;
}
Msg("", MsgInfoType.Info);
pdbCmd.playbackPoint.isGrab = isGrab.IsChecked == true ? (byte)1 : (byte)0;
pdbCmd.playbackPoint.motionStyle = (byte)modeStyle.SelectedIndex;
pdbCmd.playbackPoint.x = x;
pdbCmd.playbackPoint.y = y;
pdbCmd.playbackPoint.z = z;
pdbCmd.playbackPoint.rHead = r;
pdbCmd.playbackPoint.gripper = gripper;
pdbCmd.playbackPoint.pauseTime = pTime;
DobotDll.SetPlaybackBufferCmd(ref pdbCmd);
} break;
default:
break;
}
}
private void CheckBox_Unchecked(object sender, RoutedEventArgs e)
{
if (!isConnectted)
{
return;
}
CheckBox obj = (CheckBox)sender;
String con = obj.Content.ToString();
if (con == "Grab") // cancel grab
{
currentCmd.isJoint = isJoint;
currentCmd.cmd = JogCmd.JogRelease;
DobotDll.SetJogInstantCmd(ref currentCmd);
}
else if (con == "Laser") // release laser
{
currentCmd.isJoint = isJoint;
currentCmd.cmd = JogCmd.JogLaserOff;
DobotDll.SetJogInstantCmd(ref currentCmd);
}
}
private void CheckBox_Checked(object sender, RoutedEventArgs e)
{
if (!isConnectted)
{
return;
}
CheckBox obj = (CheckBox)sender;
String con = obj.Content.ToString();
if (con == "Grab") // grab
{
currentCmd.isJoint = isJoint;
currentCmd.cmd = JogCmd.JogGrab;
DobotDll.SetJogInstantCmd(ref currentCmd);
}
else if (con == "Laser") // Shutting
{
currentCmd.isJoint = isJoint;
currentCmd.cmd = JogCmd.JogLaserOn;
DobotDll.SetJogInstantCmd(ref currentCmd);
}
}
private void blurSlider_MouseLeftButtonUp(object sender, MouseButtonEventArgs e)
{
if (!isConnectted)
{
return;
}
Slider obj = (Slider)sender;
if (obj.Name == "sld")
{
JogDynamicParams jdParam;
jdParam.velocityRatio = (float)sld.Value;
DobotDll.SetJogDynamicParams(ref jdParam);
}
else if (obj.Name == "sld1" || obj.Name == "sldAcc") // playback
{
PlaybackDynamicParams pbdParam;
pbdParam.velocityRatio = (float)obj.Value;
pbdParam.accelerationRatio = (float)obj.Value;
DobotDll.SetPlaybackDynamicParams(ref pbdParam);
}
}
private void GetPose()
{
if (!isConnectted)
{
return;
}
DobotDll.GetPose(ref pose);
this.Dispatcher.BeginInvoke((Action) delegate()
{
//if (sync.IsChecked == true)
//{
Xtext.Text = pose.x.ToString();
Ytext.Text = pose.y.ToString();
Ztext.Text = pose.z.ToString();
J1text.Text = pose.jointAngle[0].ToString();
J2text.Text = pose.jointAngle[1].ToString();
J3text.Text = pose.jointAngle[2].ToString();
sdXtext.Text = (pose.x - sd_origin.X).ToString();
sdYtext.Text = (pose.y - sd_origin.Y).ToString();
sdZtext.Text = (pose.z - sd_origin.Z).ToString();
//rHead.Text = pose.rHead.ToString();
// pauseTime.Text = "0";
//}
var matrix = new Matrix3D();
matrix.Rotate(new System.Windows.Media.Media3D.Quaternion(new Vector3D(0, 1, 0), pose.jointAngle[0]));
matrix.Translate(new Vector3D((pose.y - sd_origin.Y), (pose.z - sd_origin.Z), (pose.x - sd_origin.X)));
MeshGeometryModel3D model = ModelGroup.Children[1] as MeshGeometryModel3D;
model.Transform = new MatrixTransform3D(matrix);
});
}
/// <summary>
/// Point To Point Move
/// </summary>
/// <param name="style"></param>
/// <param name="x"></param>
/// <param name="y"></param>
/// <param name="z"></param>
/// <param name="r"></param>
/// <returns></returns>
private UInt64 ptp(byte style, float x, float y, float z, float r)
{
PTPCmd pdbCmd;
UInt64 cmdIndex = 0;
pdbCmd.ptpMode = style;
pdbCmd.x = x;
pdbCmd.y = y;
pdbCmd.z = z;
pdbCmd.rHead = r;
while (true)
{
int ret = DobotDll.SetPTPCmd(ref pdbCmd, true, ref cmdIndex);
if (ret == 0)
{
break;
}
}
while (true)
{
UInt64 retIndex = 0;
int ind = DobotDll.GetQueuedCmdCurrentIndex(ref retIndex);
if (ind == 0 && cmdIndex <= retIndex)
{
break;
}
}
float waitTime = 500;
WAITCmd waitcmd;
waitcmd.timeout = (uint)waitTime;
DobotDll.SetWAITCmd(ref waitcmd, false, ref cmdIndex);
return(cmdIndex);
}
private void CheckBox_Unchecked(object sender, RoutedEventArgs e)
{
if (!isConnectted)
{
return;
}
CheckBox obj = (CheckBox)sender;
String con = obj.Content.ToString();
UInt64 cmdIndex = 0;
if (con == "Grab") // cancel grab
{
DobotDll.SetEndEffectorGripper(true, false, false, ref cmdIndex);
}
else if (con == "Laser") // release laser
{
DobotDll.SetEndEffectorLaser(false, false, false, ref cmdIndex);
}
else if (con == "SuctionCup")
{
DobotDll.SetEndEffectorSuctionCup(false, false, false, ref cmdIndex);
}
}
/// <summary>
/// StartDobot
/// </summary>
private void StartDobot()
{
int ret = DobotDll.ConnectDobot("", 115200);
// start connect
if (ret != (int)DobotConnect.DobotConnect_NoError)
{
Msg("Connect error", MsgInfoType.Error);
return;
}
Msg("Connect success", MsgInfoType.Info);
isConnectted = true;
DobotDll.SetCmdTimeout(3000);
// Must set when sensor is not exist
DobotDll.ResetPose(true, 45, 45);
// Get name
string deviceName = "Dobot Magician";
DobotDll.SetDeviceName(deviceName);
StringBuilder deviceSN = new StringBuilder(64);
DobotDll.GetDeviceName(deviceSN, 64);
UInt64 cmdIndex = 0;
JOGJointParams jsParam;
jsParam.velocity = new float[] { 200, 200, 200, 200 };
jsParam.acceleration = new float[] { 200, 200, 200, 200 };
DobotDll.SetJOGJointParams(ref jsParam, false, ref cmdIndex);
JOGCommonParams jdParam;
jdParam.velocityRatio = 100;
jdParam.accelerationRatio = 100;
DobotDll.SetJOGCommonParams(ref jdParam, false, ref cmdIndex);
PTPJointParams pbsParam;
pbsParam.velocity = new float[] { 200, 200, 200, 200 };
pbsParam.acceleration = new float[] { 200, 200, 200, 200 };
DobotDll.SetPTPJointParams(ref pbsParam, false, ref cmdIndex);
PTPCoordinateParams cpbsParam;
cpbsParam.xyzVelocity = 100;
cpbsParam.xyzAcceleration = 100;
cpbsParam.rVelocity = 100;
cpbsParam.rAcceleration = 100;
DobotDll.SetPTPCoordinateParams(ref cpbsParam, false, ref cmdIndex);
PTPJumpParams pjp;
pjp.jumpHeight = 20;
pjp.zLimit = 100;
DobotDll.SetPTPJumpParams(ref pjp, false, ref cmdIndex);
PTPCommonParams pbdParam;
pbdParam.velocityRatio = 30;
pbdParam.accelerationRatio = 30;
DobotDll.SetPTPCommonParams(ref pbdParam, false, ref cmdIndex);
//EIO
IOMultiplexing iom;
iom.address = 2; //io index
iom.multiplex = (byte)IOFunction.IOFunctionDO; // ioType
DobotDll.SetIOMultiplexing(ref iom, false, ref cmdIndex);
IODO iod;
iod.address = 2;
iod.level = 1; // set io index 2 to open
DobotDll.SetIODO(ref iod, false, ref cmdIndex);
// arc
//ptp((byte)1, 67.99f, 216.4f, -27.99f, 0);
//arc(154.92f, 182.41f, -62.89f, 0f, 216.07f, 85.54f, -8.34f, 0f);
}