public bool SendCommand(ServoType servoType, byte[] cmd)
{
SerialPort sp = registeredSerialPorts[servoType];
if (sp == null)
{
return(false);
}
EraseBuffer(servoType);
try
{
if (sp.IsOpen)
{
sp.Write(cmd, 0, cmd.Length);
}
else
{
sp.Open();
sp.Write(cmd, 0, cmd.Length);
}
}
catch
{
TextBoxStreamWriter.DefaultLog.WriteLine(" Serial Port Manager => Serial Transmission Problem");
//byte[] byteErr = null;
//byteError(cmd);
return(false);
}
return(true);
}
public override void OnLoad()
{
servo = PhidgetManager.Get <AdvancedServo>(false);
servo.Attach += servo_Attach;
servo.Detach += servo_Detach;
foreach (string servoType in Enum.GetNames(typeof(ServoServo.ServoType)))
{
//stop here
if (servoType.Equals(ServoServo.ServoType.USER_DEFINED.ToString()))
{
break;
}
TypeComboBox.Items.Add(servoType);
// Select the type we are given originally.
if (servoType == ServoType.ToString())
{
TypeComboBox.SelectedItem = servoType;
}
}
PositionSlider.Value = Position;
// Set the text (this doesn't get called if position was 0)
PositionSlider_ValueChanged(null, null);
SetAttached();
}
public bool ReceiveCommand(ServoType servoType, int receivedDataLength, out byte[] inCmd)
{
int counter = 0;
int bufferLength = receivedDataLength + 6;
inCmd = new byte[bufferLength];
SerialPort sp = registeredSerialPorts[servoType];
//while (sp.BytesToRead == 0) {
// Thread.Sleep(0);
// sp = registeredSerialPorts[servoType];
//}
if (sp == null)
{
return(false);
}
for (int i = 0; i < bufferLength; i++)
{
try
{
while (sp.BytesToRead == 0)
{
Thread.Sleep(0);
counter++;
if (counter > 50000)
{
return(false);
}
}
inCmd[i] = Convert.ToByte(sp.ReadByte());
if (i == 0)
{
if (inCmd[0] != 255)
{
i--;
}
}
}
catch
{
TextBoxStreamWriter.DefaultLog.WriteLine(" Serial Port Manager => Serial Reception Problem");
byte[] byteErr = null;
byteError(byteErr);
return(false);
}
}
if ((byteError != null) && (inCmd[4] != 0))
{
byteError(inCmd);
return(false);
}
return(true);
}
public void AddSerialPort(ServoType servoType, SerialPort serialPort)
{
if (!serialPorts.ContainsKey(serialPort.PortName))
{
serialPorts.Add(serialPort.PortName, serialPort);
}
if (!registeredSerialPorts.ContainsKey(servoType))
{
registeredSerialPorts.Add(servoType, serialPort);
}
}
public void AddSerialPort(ServoType servoType, string portCOM, int baudrate, Parity parity, int bits, StopBits stopBits)
{
if (!serialPorts.ContainsKey(portCOM))
{
SerialPort serialPort = new SerialPort(portCOM, baudrate, parity, bits, stopBits);
serialPorts.Add(portCOM, serialPort);
}
if (!registeredSerialPorts.ContainsKey(servoType))
{
registeredSerialPorts.Add(servoType, serialPorts[portCOM]);
}
}
public ServoMotor(string pwmId, ServoType type, int pwmPin)
{
PwmController pwmController = PwmController.FromId(pwmId);
pwmController.SetDesiredFrequency(50);
this.ConfigurePulseParameters(1.0, 2.0);
this.servo = pwmController.OpenPin(pwmPin);
this.type = type;
this.Position = 0;
}
public bool SendReceiveCommand(ServoType servoType, byte[] cmd, int dataLength, out byte[] inCmd)
{
inCmd = null;
if (!EraseBuffer(servoType))
{
return(false);
}
if (!SendCommand(servoType, cmd))
{
return(false);
}
if (!ReceiveCommand(servoType, dataLength, out inCmd))
{
return(false);
}
return(true);
}
private bool EraseBuffer(ServoType servoType)
{
SerialPort sp = registeredSerialPorts[servoType];
if (sp == null)
{
return(false);
}
try
{
sp.DiscardInBuffer();
}
catch
{
TextBoxStreamWriter.DefaultLog.WriteLine("Serial Port Manager => Erase Buffer: Serial Port is not Ready");
//byte[] byteErr = null;
//byteError(byteErr);
return(false);
}
return(true);
}
public void ConfigureAsContinuous(bool inverted)
{
this.type = ServoType.Continuous;
this.invertServo = inverted;
}
public void ConfigureAsPositional(bool inverted)
{
this.type = ServoType.Positional;
this.invertServo = inverted;
}
public void Move(ServoType servoType, int newIndex, int speed)
{
servoMotors[(int)servoType].GoPositionIndex(newIndex, speed);
}
