public void sendmsg(SendVar sendVar, int num)//给机械臂蓝牙串口发送控制指令
{
string strAngle = "";
string strtimes = "";
string msg = "";
if (sendVar.Times < 100)
{
strtimes = "T00" + sendVar.Times.ToString() + "!\r\n";
}
if (sendVar.Times >= 100 && sendVar.Times < 1000)
{
strtimes = "T0" + sendVar.Times.ToString() + "!\r\n";
}
if (sendVar.Times >= 1000)
{
strtimes = "T" + sendVar.Times.ToString() + "!\r\n";
}
if (sendVar.Angle < 1000)
{
strAngle = "P0" + sendVar.Angle.ToString();
}
if (sendVar.Angle >= 1000)
{
strAngle = "P" + sendVar.Angle.ToString();
}
msg = "{#00" + sendVar.Id.ToString() + strAngle + strtimes + "}";
form.SendMsg(msg, num);
}
public void Joint3Run(double angle, int num, int times)
{
SendVar sendVar = new SendVar();
sendVar.Id = 3;
sendVar.Angle = (int)(angle * 7.8) + 1500;
sendVar.Times = times;
msg.sendmsg(sendVar, num);
Thread.Sleep(sendVar.Times);
}
/**************3号舵机*****************/
public void Joint3Run(double angle, int num, ref double lastAngle)
{
SendVar sendVar = new SendVar();
sendVar.Id = 3;
sendVar.Angle = (int)(angle * 7.8) + 1500;
sendVar.Times = (int)Math.Abs(angle - lastAngle) * 50;
msg.sendmsg(sendVar, num);
lastAngle = angle;
Thread.Sleep(sendVar.Times);
}
public void Joint0Run(double angle, int num, int times)
{
SendVar sendVar = new SendVar();
double temp;
sendVar.Id = 0;
temp = angle * 7.8;
sendVar.Angle = (int)temp + 1500;
sendVar.Times = times;
msg.sendmsg(sendVar, num);
Thread.Sleep(sendVar.Times);
}
