private void getServoPositionComSerial(Servo.ServoPortEnum servoPort, EZ_B.Classes.GetServoValueResponse getServoResponse)
{
if (!_serialPort.IsOpen)
{
getServoResponse.Success = false;
getServoResponse.ErrorStr = "COM Port not open";
return;
}
_serialPort.DiscardInBuffer();
sendServoCommand(_controller.GetServoPosition(servoPort));
System.Threading.Thread.Sleep(100);
var ret = new byte[_serialPort.BytesToRead];
_serialPort.Read(ret, 0, ret.Length);
if (ret.Length != 14)
{
getServoResponse.ErrorStr = "Servo did not respond";
getServoResponse.Success = false;
return;
}
getServoResponse.Position = (int)EZ_B.Functions.RemapScalar(BitConverter.ToInt16(ret, 11), 1, 1000, Servo.SERVO_MIN, Servo.SERVO_MAX);
getServoResponse.Success = true;
}
private void Servo_OnServoGetPosition(Servo.ServoPortEnum servoPort, EZ_B.Classes.GetServoValueResponse getServoResponse)
{
if (getServoResponse.Success)
{
return;
}
if (!_customConfig.VirtualPorts.Contains(servoPort))
{
getServoResponse.ErrorStr = "No matching lewansoul servo specified";
getServoResponse.Success = false;
return;
}
Invokers.SetAppendText(tbLog, true, "Reading position from {0}", servoPort);
if (Convert.ToBoolean(_cf.STORAGE[ConfigTitles.USE_COM_PORT]))
{
getServoPositionComSerial(servoPort, getServoResponse);
}
if (!getServoResponse.Success && Convert.ToBoolean(_cf.STORAGE[ConfigTitles.USE_HARDWARE_UART]))
{
getServoPositionEZBUART(servoPort, getServoResponse);
}
}
private void getServoPositionEZBUART(Servo.ServoPortEnum servoPort, EZ_B.Classes.GetServoValueResponse getServoResponse)
{
if (!EZBManager.EZBs[0].IsConnected)
{
getServoResponse.Success = false;
getServoResponse.ErrorStr = "Not connected to EZ-B 0";
return;
}
if (!Convert.ToBoolean(_cf.STORAGE[ConfigTitles.USE_HARDWARE_UART]))
{
getServoResponse.ErrorStr = "This feature is only available when using the hardware uart";
getServoResponse.Success = false;
return;
}
initUART();
sendServoCommand(_controller.GetServoPosition(servoPort));
System.Threading.Thread.Sleep(100);
var ret = EZBManager.EZBs[0].Uart.UARTExpansionReadAvailable(Convert.ToInt16(_cf.STORAGE[ConfigTitles.HARDWARE_PORT]));
if (ret.Length != 14)
{
getServoResponse.ErrorStr = "Servo did not respond";
getServoResponse.Success = false;
return;
}
getServoResponse.Position = (int)EZ_B.Functions.RemapScalar(BitConverter.ToInt16(ret.Reverse().ToArray(), 0), 1, 1000, Servo.SERVO_MIN, Servo.SERVO_MAX);
getServoResponse.Success = true;
}
