private void EnterUSBTTLSSB()
{
if (robot.currMode == RobotConnection.connMode.Network)
{
if (!MessageConfirm(LocUtil.FindResource("cb.msgConfirmEnterUSBTTL")))
{
return;
}
}
byte[] cmd = { 0xA9, 0x9A, 0x04, 0x07, 0x00, 0x01, 0x0C, 0xED };
SendCBCommand(cmd, 7);
if (robot.isConnected)
{
string action = LocUtil.FindResource("cb.msgEnterUSBTTLSSB");
if (robot.Available == 7)
{
byte[] result = robot.ReadAll();
if (result[4] == 0)
{
AppendLog(LocUtil.FindResource("msgSuccess") + action);
}
else
{
AppendLog(action + LocUtil.FindResource("msgFail"));
}
}
else
{
AppendLog(string.Format(LocUtil.FindResource("cb.msgUSBTTLNoReply"), action));
}
}
}
private void SetAdjAngle(int id)
{
int adjValue = (int)sliderAdjValue.Value;
if (!UBTSetAdjAngle(id, adjValue))
{
if (robot.Available != 1)
{
return;
}
if (id == 0)
{
return;
}
AppendLog(LocUtil.FindResource("ubt.msgSetAdjustFail"));
return;
}
AppendLog(LocUtil.FindResource("ubt.msgSetAdjustSuccess"));
if ((txtAdjPreview.Text == null) || (txtAdjPreview.Text.Trim() == ""))
{
return;
}
System.Threading.Thread.Sleep(500);
txtMoveAngle.Text = txtAdjPreview.Text;
txtMoveTime.Text = "1000";
GoMove(id);
}
// A9 9A 02 10 12 ED
private void CheckServoMode()
{
byte[] cmd = { 0xA9, 0x9A, 0x02, 0x10, 0x12, 0xED };
SendCBCommand(cmd, 7);
if (robot.Available == 7)
{
byte[] result = robot.ReadAll();
string servo;
switch (result[4])
{
case 1:
servo = LocUtil.FindResource("cb.msgServoUBTech");
break;
case 2:
servo = LocUtil.FindResource("cb.msgServoHaiLzd");
break;
case 3:
servo = LocUtil.FindResource("cb.msgServoFeeTech");
break;
default:
servo = LocUtil.FindResource("cb.msgServoError");
break;
}
AppendLog(String.Format(LocUtil.FindResource("cb.msgServoProtocol"), servo));
}
robot.ClearRxBuffer();
}
protected bool AllowExecution()
{
if (!inProgress)
{
return(true);
}
UpdateInfo(LocUtil.FindResource("base.msgWaitServoDetection"), UTIL.InfoType.alert);
return(false);
}
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);
}
}
int GetId(bool allowZero = false)
{
int id = GetIdValue(txtId.Text.Trim());
if ((id < 0) || (id > CONST.MAX_SERVO) || ((id == 0) && !allowZero))
{
UpdateInfo(LocUtil.FindResource("ubt.msgInvalidId"), UTIL.InfoType.error);
return(-1);
}
return(id);
}
// FC CF {id} 01 00 00 00 00 {sum} ED
private bool GetVersion(int id)
{
// byte[] cmd = { 0xFC, 0xCF, (byte)id, 0x01, 0, 0, 0, 0, 0, 0xED };
byte[] cmd = { 0xFC, 0xCF, (byte)id, 0x01, 0, 0, 0, 0, 0, 0xED };
SendCommand(cmd, 10);
if ((id > 0) && (robot.Available == 10))
{
byte[] buffer = robot.ReadAll();
string format = LocUtil.FindResource("ubt.msgShowVersion");
string result = String.Format(format,
buffer[4], buffer[5], buffer[6], buffer[7]);
AppendLog(result);
return(true);
}
return(false);
}
private void GetAngle(int id)
{
byte angle;
byte[] buffer;
if (!UBTGetAngle(id, out angle, out buffer))
{
return;
}
int iAngle = (buffer[4] << 8) | buffer[5];
int iActual = (buffer[6] << 8) | buffer[7];
string result = String.Format(LocUtil.FindResource("ubt.msgShowAngle"),
buffer[4], buffer[5], iAngle, buffer[6], buffer[7], iActual);
SetCurrAngle(buffer[7]);
AppendLog(result);
}
// A9 9A 02 0A 0C ED
private void CheckCommandMode()
{
byte[] cmd = { 0xA9, 0x9A, 0x02, 0x0A, 0x0C, 0xED };
SendCBCommand(cmd, 12);
if (robot.Available == 12)
{
byte[] result = robot.ReadAll();
String msg = "";
if (result[4] != 0)
{
msg += " V1";
}
if (result[5] != 0)
{
msg += " V2";
}
if (result[6] != 0)
{
msg += " BT";
}
if (result[7] != 0)
{
msg += " CB";
}
if (result[8] != 0)
{
msg += " SV";
}
if (result[9] != 0)
{
msg += " HaiLzd";
}
if (msg == "")
{
msg = LocUtil.FindResource("cb.msgNoCommandSupported");
}
else
{
msg = LocUtil.FindResource("cb.msgSupportedCommand") + msg;
}
AppendLog(msg);
}
robot.ClearRxBuffer();
}
private void SetLed(int id, bool mode)
{
byte[] cmd = { 0xFA, 0xAF, (byte)id, 0x04, (byte)(mode ? 0x00 : 0x01), 0, 0, 0, 0, 0xED };
SendCommand(cmd, 10);
if ((id != 0) && (robot.Available == 1))
{
byte[] buffer = robot.ReadAll();
string action = LocUtil.FindResource(mode ? "ubt.msgLedOn" : "ubt.msgLedOff");
if (buffer[0] == (0xAA + id))
{
AppendLog(String.Format(LocUtil.FindResource("ubt.msgSetLedSuccess"), id, action));
}
else
{
AppendLog(String.Format(LocUtil.FindResource("ubt.msgSetLedFail"), id, action));
}
}
}
private void ExitUSBTTL()
{
if (robot.currMode == RobotConnection.connMode.Network)
{
if (!MessageConfirm(LocUtil.FindResource("cb.msgConfirmQuitUSBTTL")))
{
return;
}
}
byte[] cmd = { 0xA9, 0x9A, 0x01, 0x06, 0x09 };
AppendLog("\n" + (robot.isConnected ? ">> " : "") + UTIL.GetByteString(cmd) + "\n");
robot.ClearRxBuffer();
if (robot.isConnected)
{
robot.SendCommand(cmd, cmd.Length, 0);
AppendLog(LocUtil.FindResource("cb.msgQuitUSBTTLSent"));
}
}
private void GetAdjAngle(int id)
{
UInt16 adjValue;
byte[] buffer;
if (!UBTGetAdjAngle(id, out adjValue, out buffer))
{
return;
}
if (id == 0)
{
return;
}
string adjMsg = "";
if (adjValue == 0)
{
adjMsg = LocUtil.FindResource("ubt.msgNoAdjust");
sliderAdjValue.Value = 0;
}
else if ((adjValue >= 0x0000) && (adjValue <= 0x0130))
{
adjMsg = string.Format(LocUtil.FindResource("ubt.msgPosAdjust"), adjValue);
sliderAdjValue.Value = adjValue;
}
else if ((adjValue >= 0xFED0) && (adjValue <= 0xFFFF))
{
adjMsg = string.Format(LocUtil.FindResource("ubt.msgNegAdjust"), (65536 - adjValue));
sliderAdjValue.Value = (adjValue - 65536);
}
else
{
adjMsg = LocUtil.FindResource("ubt.msgInvalidAdjust");
}
string result = String.Format(LocUtil.FindResource("ubt.msgShowAdjust"),
buffer[4], buffer[5], buffer[6], buffer[7], adjMsg);
AppendLog(result);
}
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 sliderAdjValue_ValueChanged(object sender, RoutedPropertyChangedEventArgs <double> e)
{
int n = (int)((Slider)sender).Value;
if (n == 0)
{
txtAdjMsg.Text = String.Format(LocUtil.FindResource("ubt.msgNoAdjust"));
}
else if (n > 0)
{
txtAdjMsg.Text = String.Format(LocUtil.FindResource("ubt.msgPosAdjust"), n);
}
else
{
txtAdjMsg.Text = String.Format(LocUtil.FindResource("ubt.msgNegAdjust"), -n);
}
if (n < 0)
{
n += 65536;
}
txtAdjAngle.Text = n.ToString("X4");
}
private void SetVersion()
{
if (robot.isConnected)
{
string version = GetVersion();
if (version == "")
{
lblVersion.Content = LocUtil.FindResource("cb.msgControllerNotFound");
lblVersion.Foreground = new SolidColorBrush(Colors.Red);
}
else
{
lblVersion.Content = version;
lblVersion.Foreground = new SolidColorBrush(Colors.Blue);
}
}
else
{
lblVersion.Content = LocUtil.FindResource("cb.msgPleaseConnectController");
lblVersion.Foreground = new SolidColorBrush(Colors.LightGray);
}
}
// FA AF {id} CD 00 {newId} 00 00 {sum} ED
private void ChangeId(int id, int newId)
{
string msg = String.Format(LocUtil.FindResource("ubt.msgConfirmChangeId"),
(id == 0 ? LocUtil.FindResource("ubt.msgAllServo") : LocUtil.FindResource("ubt.msgServoId") + id.ToString()), newId);
if (!MessageConfirm(msg))
{
return;
}
byte[] cmd = { 0xFA, 0xAF, (byte)id, 0xCD, 0, (byte)newId, 0, 0, 0, 0xED };
SendCommand(cmd, 10);
if ((id > 0) && (robot.Available == 10))
{
byte[] buffer = robot.ReadAll();
if (buffer[3] == 0xAA)
{
string result = String.Format(LocUtil.FindResource("ubt.msgChangeIdSuccess"), id, newId);
AppendLog(result);
txtId.Text = newId.ToString();
UpdateInfo(result, UTIL.InfoType.alert);
}
}
}
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);
}
}
static protected bool MessageConfirm(String msg)
{
MessageBoxResult result = MessageBox.Show(msg, LocUtil.FindResource("msgConfirm"), MessageBoxButton.YesNo, MessageBoxImage.Question);
return(result == MessageBoxResult.Yes);
}
public override void ExecuteCommand()
{
if (!AllowExecution())
{
return;
}
if (rbFreeInput.IsChecked == true)
{
ExecuteFreeInput();
}
else
{
int id = GetId((cbxSupportBroadcast.IsChecked == true));
if (id < 0)
{
return;
}
if (rbCheckVersion.IsChecked == true)
{
GetVersion(id);
}
else if (rbChangeId.IsChecked == true)
{
int newId = GetIdValue(txtNewId.Text.Trim());
if ((newId <= 0) || (newId > CONST.MAX_SERVO))
{
UpdateInfo(LocUtil.FindResource("ubt.msgInvalidNewId"), UTIL.InfoType.error);
return;
}
if (id == newId)
{
UpdateInfo(LocUtil.FindResource("ubt.msgNewIdSame"), UTIL.InfoType.error);
return;
}
ChangeId(id, newId);
}
else if (rbLedOn.IsChecked == true)
{
SetLed(id, true);
}
else if (rbLedOff.IsChecked == true)
{
SetLed(id, false);
}
else if (rbGetAngle.IsChecked == true)
{
GetAngle(id);
}
else if (rbMove.IsChecked == true)
{
GoMove(id);
}
else if (rbRotate.IsChecked == true)
{
GoRotate(id);
}
else if (rbGetAdjAngle.IsChecked == true)
{
GetAdjAngle(id);
}
else if (rbSetAdjAngle.IsChecked == true)
{
SetAdjAngle(id);
}
else if (rbAutoAdjAngle.IsChecked == true)
{
AutoAdjAngle(id);
}
}
}