private void GoMove(int id)
{
byte[] cmd = { 0xFA, 0xAF, (byte)id, 1, 0, 0, 0, 0, 0, 0xED };
if (((txtMoveAngle.Text == null) || (txtMoveAngle.Text.Trim() == "")) ||
((txtMoveTime.Text == null) || (txtMoveTime.Text.Trim() == "")))
{
AppendLog(LocUtil.FindResource("ubt.msgGoMoveParameter"));
return;
}
try
{
byte angle = (byte)UTIL.GetInputInteger(txtMoveAngle.Text);
int timeMs = UTIL.GetInputInteger(txtMoveTime.Text);
/*
* byte time = (byte)(timeMs / 20);
* cmd[4] = angle;
* cmd[5] = cmd[7] = time;
*
* SendCommand(cmd, 1);
*
* if ((id != 0) && (robot.Available == 1))
* {
* byte[] buffer = robot.ReadAll();
* if (buffer[0] == (0xAA + id))
* {
* //txtAdjPreview.Text = angle.ToString();
* //txtAutoAdjAngle.Text = angle.ToString();
* SetCurrAngle(angle);
* AppendLog(String.Format(LocUtil.FindResource("ubt.msgGoMoveSuccess"), id, angle));
* }
* else
* {
* AppendLog(String.Format(LocUtil.FindResource("ubt.msgGoMoveFail"), id));
* }
* }
*/
int result = UBTGoMove(id, angle, timeMs);
switch (result)
{
case 0:
SetCurrAngle(angle);
AppendLog(String.Format(LocUtil.FindResource("ubt.msgGoMoveSuccess"), id, angle));
break;
case 1:
AppendLog(String.Format(LocUtil.FindResource("ubt.msgGoMoveFail"), id));
break;
}
}
catch (Exception ex)
{
AppendLog("\nERR: " + ex.Message);
}
}
private void GoRotate(int id)
{
byte[] cmd = { 0xFA, 0xAF, (byte)id, 1, 0, 0, 0, 0, 0, 0xED };
if (string.IsNullOrWhiteSpace(txtRotateSpeed.Text))
{
AppendLog(LocUtil.FindResource("ubt.msgGoRotateParameter"));
return;
}
try
{
int iDirection = cboRotateDirection.SelectedIndex;
int iSpeed = UTIL.GetInputInteger(txtRotateSpeed.Text);
if (iSpeed > 2000)
{
AppendLog(LocUtil.FindResource("ubt.msgGoRotateSpeed"));
return;
}
cmd[4] = (byte)(iDirection == 0 ? 0xFD : 0xFE);
cmd[6] = (byte)(iSpeed >> 8 & 0xFF);
cmd[7] = (byte)(iSpeed & 0xFF);
SendCommand(cmd, 1);
if ((id > 0) && (robot.Available == 1))
{
byte[] buffer = robot.ReadAll();
string action = (iSpeed == 0 ? LocUtil.FindResource("msgStop") : LocUtil.FindResource("ubt.msgStart"));
if (buffer[0] == (0xAA + id))
{
AppendLog(String.Format(LocUtil.FindResource("ubt.msgGoRotateSuccess"), id, action));
}
else
{
AppendLog(String.Format(LocUtil.FindResource("ubt.msgGoRotateFail"), id, action));
}
}
}
catch (Exception ex)
{
AppendLog("\nERR: " + ex.Message);
}
}
private void AutoAdjAngle(int id)
{
if (id == 0)
{
AppendLog(LocUtil.FindResource("ubt.msgAutoAdjustNoBroadcast"));
return;
}
if (!robot.isConnected)
{
AppendLog(LocUtil.FindResource("ubt.msgAutoAdjustMustConnect"));
return;
}
if ((txtAutoAdjAngle.Text == null) || (txtAutoAdjAngle.Text.Trim() == ""))
{
AppendLog(LocUtil.FindResource("ubt.msgAutoAdjustRequireAngle"));
return;
}
try
{
int iFixAngle = (byte)UTIL.GetInputInteger(txtAutoAdjAngle.Text);
if (iFixAngle > CONST.UBT.MAX_ANGLE)
{
AppendLog(String.Format(LocUtil.FindResource("ubt.msgAutoInvalidAngle"), iFixAngle, CONST.UBT.MAX_ANGLE));
return;
}
// Get Crrent Adj Angle
byte[] buffer;
UInt16 adj;
if (!UBTGetAdjAngle(id, out adj, out buffer))
{
AppendLog(LocUtil.FindResource("ubt.msgGetAdjustFail"));
return;
}
int adjValue = 0;
if ((adj >= 0x0000) && (adj <= 0x0130))
{
adjValue = adj;
}
else if ((adj >= 0xFED0) && (adj <= 0xFFFF))
{
adjValue = (adj - 65536);
}
else
{
AppendLog(string.Format(LocUtil.FindResource("ubt.msgCurrentAdjustInvalid"), adj));
return;
}
byte currAngle;
if (!UBTGetAngle(id, out currAngle, out buffer))
{
AppendLog(LocUtil.FindResource("ubt.msgGetAngleFail"));
return;
}
int delta = cboAutoAdjDelta.SelectedIndex;
int actualValue = currAngle * 3 + adjValue;
int actualAngle = actualValue / 3;
int actualDelta = actualValue % 3;
int newValue = iFixAngle * 3 + delta;
int newAdjValue = actualValue - newValue;
if (!UBTSetAdjAngle(id, newAdjValue))
{
AppendLog(LocUtil.FindResource("ubt.msgSetAdjustFail"));
return;
}
AppendLog(string.Format(LocUtil.FindResource("ubt.msgAutoAdjustComplete"), id, actualAngle, actualDelta, iFixAngle, delta));
System.Threading.Thread.Sleep(100);
GetAdjAngle(id);
}
catch (Exception ex)
{
AppendLog("\nERR: " + ex.Message);
}
}
