public static bool TryReadSerialPort(ObjectClasses.ISerialPortIo serialPort1, out byte ReadByte)
{
try
{
ReadByte = Convert.ToByte(serialPort1.ReadByte());
return true;
}
catch (System.IO.IOException)
{
DataComms.CreateThreadInstance("serialPort1.ReadByte - IOException");
ReadByte = 0;
return false;
}
catch (UnauthorizedAccessException e)
{
DataComms.CreateThreadInstance("serialPort1.ReadByte - UnauthorizedAccessException");
ReadByte = 0;
return false;
}
catch (Exception)
{
DataComms.CreateThreadInstance("serialPort1.ReadByte - Exception");
ReadByte = 0;
return false;
}
}
//This command only works for VP-Drives (Embedded Controller)
public static bool SetMotorPosition(ObjectClasses.ISerialPortIo serialPort1, int PositionRefVal)
{
string HexString = PositionRefVal.ToString("X");
Encoding ascii = Encoding.ASCII;
byte[] asciiBytes = ascii.GetBytes(PositionRefVal.ToString()); //Converts the string of ASCII characters to a byte array where each member is the decimal representation of the ASCII value
byte[] RSSCommand = new byte[27];
RSSCommand[0] = 0x05; //ENQ Header
RSSCommand[1] = 0x30; //Station ID = 0;
RSSCommand[2] = 0x30; //Station ID = 0;
RSSCommand[3] = 0x43; //C
RSSCommand[4] = 0x50; //P
RSSCommand[5] = 0x52; //R
RSSCommand[6] = 0x30; //Direction
RSSCommand[7] = 0x30; //1
RSSCommand[8] = 0x30; //2
RSSCommand[9] = 0x30; //3
RSSCommand[10] = 0x32; //4
RSSCommand[11] = 0x30; //5
RSSCommand[12] = 0x30; //6
RSSCommand[13] = 0x30; //Speed - 1
RSSCommand[14] = 0x35; //2
RSSCommand[15] = 0x30; //3
RSSCommand[16] = 0x30; //4
RSSCommand[17] = 0x30; //5
RSSCommand[18] = 0x32; //Accel Time - 1
RSSCommand[19] = 0x30; //2
RSSCommand[20] = 0x30; //3
RSSCommand[21] = 0x30; //4
RSSCommand[22] = 0x31; //Decel Time - 1
RSSCommand[23] = 0x30; //2
RSSCommand[24] = 0x30; //3
RSSCommand[25] = 0x30; //4
RSSCommand[26] = 0x04; //EOT
/*return*/ TransmitCommand(serialPort1, RSSCommand, 27);
System.Threading.Thread.Sleep(100);
while (serialPort1.BytesToRead != 0)
{
string text = Convert.ToString(serialPort1.ReadByte().ToString("X"));
System.Diagnostics.Debug.WriteLine(Convert.ToString(text));
}
//serialPort1.Close();
return true;
}
public static bool SetMotorSpeed(ObjectClasses.ISerialPortIo serialPort1, int SpeedRefVal, byte DirectionByte)
{
Encoding ascii = Encoding.ASCII;
byte[] asciiBytes = ascii.GetBytes(SpeedRefVal.ToString()); //Converts the string of ASCII characters to a byte array where each member is the decimal representation of the ASCII value
byte[] RSSCommand = new byte[25];
RSSCommand[0] = 0x05; //ENQ Header
RSSCommand[1] = 0x30; //Station ID = 0;
RSSCommand[2] = 0x30; //Station ID = 0;
RSSCommand[3] = 0x43; //C
RSSCommand[4] = 0x4A; //J
RSSCommand[5] = 0x52; //R
RSSCommand[6] = DirectionByte; //Dir
RSSCommand[7] = 0x30; //Speed 1000.0
RSSCommand[8] = 0x30; //
RSSCommand[9] = 0x30; //
RSSCommand[10] = 0x30; //
RSSCommand[11] = 0x30; //
RSSCommand[12] = 0x30; //0
RSSCommand[13] = 0x36; //6
RSSCommand[14] = 0x30; //0
RSSCommand[15] = 0x30; //0
RSSCommand[16] = 0x30; //0
RSSCommand[17] = 0x36; //6
RSSCommand[18] = 0x30; //0
RSSCommand[19] = 0x30; //0
RSSCommand[20] = 0x39; //9
RSSCommand[21] = 0x39; //9
RSSCommand[22] = 0x39; //9
RSSCommand[23] = 0x39; //9
RSSCommand[24] = 0x04; //EOT
for (int ii = 0; ii < asciiBytes.Length; ii++)
{
RSSCommand[10 - ii] = asciiBytes[asciiBytes.Length - 1 - ii];
}
/*return*/ TransmitCommand(serialPort1, RSSCommand, 25);
System.Threading.Thread.Sleep(100);
while (serialPort1.BytesToRead != 0)
{
string text = Convert.ToString(serialPort1.ReadByte().ToString("X"));
System.Diagnostics.Debug.WriteLine(Convert.ToString(text));
}
//serialPort1.Close();
return true;
}
public static objServoResult ReadFeedback(ObjectClasses.ISerialPortIo serialPort1, double[] FeedbackParams, bool ReadSuccess, byte currentByte, byte[] streamPacket, byte[] currentSpeed, byte[] currentPulse, byte[] currentTorque, byte[] currentContactStatus, ref int ErrorCode, int TimeOutCounter, objServoResult RetVal, StringBuilder sb, string ByteString)
{
GlobalVariables.SerialPortBusyLeft = true;
//objServoResult RetVal = new objServoResult();
FeedbackParams[0] = 0;
FeedbackParams[1] = 0;
FeedbackParams[2] = 0;
FeedbackParams[3] = 0;
currentByte = 0;
ReadSuccess = false;
TimeOutCounter = 0;
ErrorCode = 0;
sb.Clear();
//System.Diagnostics.Debug.WriteLine("Bytes in Buffer before empty: " + serialPort1.BytesToRead.ToString());
ByteString = serialPort1.ReadExisting();
//System.Diagnostics.Debug.WriteLine("Bytes in Buffer before empty: " + serialPort1.BytesToRead.ToString());
//SerialPortFunctions.TryDiscardInputBuffer(serialPort1);
//SerialPortFunctions.TryDiscardOutputBuffer(serialPort1);
//System.Threading.Thread.Sleep(5);
//if (SerialPortFunctions.TryDiscardInputBuffer(serialPort1) == false) { RetVal.ErrorCode = 1; RetVal.IsSuccess = false; return RetVal; }
//if (SerialPortFunctions.TryDiscardOutputBuffer(serialPort1) == false) { RetVal.ErrorCode = 2; RetVal.IsSuccess = false; return RetVal; }
do
{
if (currentByte != 0x06) //only read another byte if you don't already have a packet header, I.E. you only read here the first time through the do loop
{
if (serialPort1.BytesToRead != 0) //Check to make sure there are bytes in the buffer
{
//System.Diagnostics.Debug.WriteLine("Timeout Counter: " + TimeOutCounter);
if (SerialPortFunctions.TryReadSerialPort(serialPort1, out currentByte) == false)
{
RetVal.IsSuccess = false;
RetVal.ErrorCode = 4;
return RetVal;
}
}
else
TimeOutCounter++;
if (TimeOutCounter >= 60000)
{
System.Diagnostics.Debug.WriteLine("Timeout Counter: " + TimeOutCounter + ", Bytes to Read: " + serialPort1.BytesToRead);
RetVal.ErrorCode = 3;
RetVal.IsSuccess = false;
return RetVal;
}
}
if (currentByte == 0x06) //Initiate Low Duty Cycle command success | Packet header
{
for (int k = 0; k < 72; k++) //56 is the old number of bytes excl contact status
{
currentByte = Convert.ToByte(serialPort1.ReadByte());
streamPacket[k] = currentByte;
}
try
{
currentSpeed[0] = Convert.ToByte(streamPacket[7].ToString("X2"));
currentSpeed[1] = Convert.ToByte(streamPacket[8].ToString("X2"));
currentSpeed[2] = Convert.ToByte(streamPacket[9].ToString("X2"));
currentSpeed[3] = Convert.ToByte(streamPacket[10].ToString("X2"));
currentSpeed[4] = Convert.ToByte(streamPacket[11].ToString("X2"));
currentSpeed[5] = Convert.ToByte(streamPacket[12].ToString("X2"));
currentSpeed[6] = Convert.ToByte(streamPacket[13].ToString("X2"));
currentSpeed[7] = Convert.ToByte(streamPacket[14].ToString("X2"));
currentPulse[0] = Convert.ToByte(streamPacket[23].ToString("X2"));
//currentPulse[1] = Convert.ToByte(streamPacket[24].ToString("X2"));
currentPulse[1] = SerialMotorControl.ConvertToByte(streamPacket[24]);
currentPulse[2] = Convert.ToByte(streamPacket[25].ToString("X2"));
currentPulse[3] = Convert.ToByte(streamPacket[26].ToString("X2"));
currentPulse[4] = Convert.ToByte(streamPacket[27].ToString("X2"));
currentPulse[5] = Convert.ToByte(streamPacket[28].ToString("X2"));
currentPulse[6] = Convert.ToByte(streamPacket[29].ToString("X2"));
currentPulse[7] = Convert.ToByte(streamPacket[30].ToString("X2"));
currentTorque[0] = Convert.ToByte(streamPacket[39].ToString("X2"));
currentTorque[1] = Convert.ToByte(streamPacket[40].ToString("X2"));
currentTorque[2] = Convert.ToByte(streamPacket[41].ToString("X2"));
currentTorque[3] = Convert.ToByte(streamPacket[42].ToString("X2"));
currentTorque[4] = Convert.ToByte(streamPacket[43].ToString("X2"));
currentTorque[5] = Convert.ToByte(streamPacket[44].ToString("X2"));
currentTorque[6] = Convert.ToByte(streamPacket[45].ToString("X2"));
currentTorque[7] = Convert.ToByte(streamPacket[46].ToString("X2"));
currentContactStatus[7] = Convert.ToByte(streamPacket[62].ToString("X2"));
}
catch (FormatException e)
{
ReadSuccess = true;
FeedbackParams[0] = 0;
FeedbackParams[1] = 0;
FeedbackParams[2] = 0;
FeedbackParams[3] = 0;
RetVal.ErrorCode = 4;
RetVal.IsSuccess = false;
return RetVal;
}
finally
{
}
//StringBuilder sb = new StringBuilder();
foreach (byte byteval in currentSpeed)
{
Char charval = Convert.ToChar(Int32.Parse(byteval.ToString(), System.Globalization.NumberStyles.HexNumber));
sb.Append(Convert.ToString(charval));
}
FeedbackParams[0] = ConvertToInt32(sb);
sb.Clear();
foreach (byte byteval in currentPulse)
{
Char charval = Convert.ToChar(Int32.Parse(byteval.ToString(), System.Globalization.NumberStyles.HexNumber));
sb.Append(Convert.ToString(charval));
}
FeedbackParams[1] = ConvertToInt32(sb) - Convert.ToInt32((12000 * CalibrationSettings.Default.GearRatio) * (Math.Floor(ConvertToInt32(sb) / (12000 * CalibrationSettings.Default.GearRatio))));
sb.Clear();
foreach (byte byteval in currentTorque)
{
Char charval = Convert.ToChar(Int32.Parse(byteval.ToString(), System.Globalization.NumberStyles.HexNumber));
sb.Append(Convert.ToString(charval));
}
FeedbackParams[2] = -ConvertToInt32(sb);
sb.Clear();
Char charval1 = Convert.ToChar(Int32.Parse(currentContactStatus[7].ToString(), System.Globalization.NumberStyles.HexNumber));
sb.Append(Convert.ToString(charval1));
FeedbackParams[3] = ConvertToInt32(sb);
ReadSuccess = true;
}
} while (ReadSuccess == false);
RetVal.IsSuccess = true;
GlobalVariables.SerialPortBusyLeft = false;
return RetVal;
}