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; }