public static objServoResult TryConfigureDrive(ISerialPortIo serialPort)
{
objServoResult ServoResult = new objServoResult();
GlobalVariables.leftSerialPort.BaudRate = 57600;
if (SerialPortFunctions.TryOpenSerialPort(serialPort, "Drive Setup") == true)
{
//What is the code of the motor, ie 71 ????????????????????????????????????????????????????????????????????????? 90?
//MessageBox.Show("Motor code is missing");
if (!SerialMotorControl.WriteSingleRegister(70, 90, serialPort)) { ServoResult.IsSuccess = false; return ServoResult; }; //Set Menu PE-201 to 71 (Motor SE09M)
if (!SerialMotorControl.WriteSingleRegister(280, 0, serialPort)) { ServoResult.IsSuccess = false; return ServoResult; }; //Set Menu P3-00 to 0 (Speed Command 1)
if (!SerialMotorControl.WriteSingleRegister(282, 0, serialPort)) { ServoResult.IsSuccess = false; return ServoResult; }; //Set Menu P3-01 to 0 (Speed Command 2)
if (!SerialMotorControl.WriteSingleRegister(284, 0, serialPort)) { ServoResult.IsSuccess = false; return ServoResult; }; //Set Menu P3-02 to 0 (Speed Command 3)
if (!SerialMotorControl.WriteSingleRegister(286, 0, serialPort)) { ServoResult.IsSuccess = false; return ServoResult; }; //Set Menu P3-03 to 0 (Speed Command 4)
if (!SerialMotorControl.WriteSingleRegister(288, 0, serialPort)) { ServoResult.IsSuccess = false; return ServoResult; }; //Set Menu P3-04 to 0 (Speed Command 5)
if (!SerialMotorControl.WriteSingleRegister(290, 0, serialPort)) { ServoResult.IsSuccess = false; return ServoResult; }; //Set Menu P3-05 to 0 (Speed Command 6)
if (!SerialMotorControl.WriteSingleRegister(292, 0, serialPort)) { ServoResult.IsSuccess = false; return ServoResult; }; //Set Menu P3-06 to 0 (Speed Command 7)
if (!SerialMotorControl.WriteSingleRegister(86, 14, serialPort)) { ServoResult.IsSuccess = false; return ServoResult; }; //Set Menu P0-08 to 15 (Input EXT Set)
if (!SerialMotorControl.WriteSingleRegister(296, 2000, serialPort)) { ServoResult.IsSuccess = false; return ServoResult; }; //Set Menu PE- 609 to 4000 (Accell Time)
if (!SerialMotorControl.WriteSingleRegister(298, 2000, serialPort)) { ServoResult.IsSuccess = false; return ServoResult; }; //Set Menu PE-610 to 4000 (Decell Time)
if (!SerialMotorControl.WriteSingleRegister(236, 22, serialPort)) { ServoResult.IsSuccess = false; return ServoResult; }; //Set Menu PE-807 : External Input Logic 236
if (!SerialMotorControl.WriteSingleRegister(238, 31, serialPort)) { ServoResult.IsSuccess = false; return ServoResult; }; //Set Menu PE-807 : External Input Logic 238
}
else
{
ServoResult.IsSuccess = false;
return ServoResult;
}
if (SerialPortFunctions.TryCloseSerialPort(GlobalVariables.leftSerialPort, "Drive Setup") == false)
{
ServoResult.IsSuccess = false;
return ServoResult;
}
else
{
ServoResult.IsSuccess = true;
return ServoResult;
}
}
//Gear Ratio 5.588
public static objServoResult TryConfigureCpmDrive2()
{
objServoResult ServoResult = new objServoResult();
GlobalVariables.leftSerialPort.BaudRate = 9600;
if (SerialPortFunctions.TryOpenSerialPort(GlobalVariables.leftSerialPort, "Drive Setup") == true)
{
//Manually Set Menu PE-202 to 3 (Baud: 57600)
SerialMotorControl.WriteMenuDataIndividually(41, 3);
}
else
{
ServoResult.IsSuccess = false;
return ServoResult;
}
if (SerialPortFunctions.TryCloseSerialPort(GlobalVariables.leftSerialPort, "Drive Setup") == false)
{
ServoResult.IsSuccess = false;
return ServoResult;
}
GlobalVariables.leftSerialPort.BaudRate = 57600;
if (SerialPortFunctions.TryOpenSerialPort(GlobalVariables.leftSerialPort, "Drive Setup") == true)
{
//Set Menu PE-201 to 71 (Motor SE09M)
SerialMotorControl.WriteMenuDataIndividually(40, 63);
//Set Menu PE-605 to 0 (Speed Command 4)
SerialMotorControl.WriteMenuDataIndividually(124, 0);
//Set Menu PE-606 to 0 (Speed Command 5)
SerialMotorControl.WriteMenuDataIndividually(125, 0);
//Set Menu PE-607 to 0 (Speed Command 6)
SerialMotorControl.WriteMenuDataIndividually(126, 0);
//Set Menu PE-209 to 15 (Input EXT Set)
SerialMotorControl.WriteMenuDataIndividually(48, 15);
//Set Menu PE- 609 to 4000 (Accell Time)
SerialMotorControl.WriteMenuDataIndividually(128, 4000);
//Set Menu PE-610 to 4000 (Decell Time)
SerialMotorControl.WriteMenuDataIndividually(129, 2000);
//Set Menu PE-807 : 0 to On (Servo ON)
SerialMotorControl.WriteMenuDataIndividually_1Bit(17 * 32 + 0, 0x31);
//Set Menu PE-807 : 1 to Off (SPD1)
SerialMotorControl.WriteMenuDataIndividually_1Bit(17 * 32 + 1, 0x30);
//Set Menu PE-807 : 2 to Off (SPD2) - Set t0 0x31 to disable manual E-stop
SerialMotorControl.WriteMenuDataIndividually_1Bit(17 * 32 + 2, 0x31);
//Set Menu PE-807 : 3 to On (SPD3)
SerialMotorControl.WriteMenuDataIndividually_1Bit(17 * 32 + 3, 0x31);
//Set Menu PE-807 : 6 to Off (CCW LIM)
SerialMotorControl.WriteMenuDataIndividually_1Bit(17 * 32 + 6, 0x30);
//Set Menu PE-807 : 7 to Off (CW LIM)
SerialMotorControl.WriteMenuDataIndividually_1Bit(17 * 32 + 7, 0x30);
//Set Menu PE-807 : 8 to Off (Default) (TORQUE LIMIT)
SerialMotorControl.WriteMenuDataIndividually_1Bit(17 * 32 + 8, 0x30);
//Set Menu PE-807 : 9 to Off (E-Stop)
SerialMotorControl.WriteMenuDataIndividually_1Bit(17 * 32 + 9, 0x30);
CalibrationSettings.Default.GearRatio = 5.588;
CalibrationSettings.Default.MaxAllowableForce = Convert.ToInt32(120 / CalibrationSettings.Default.SprocketRadius);
CalibrationSettings.Default.MinAllowableTorue = 15;
CalibrationSettings.Default.MaxMotorTorque = 144;
CalibrationSettings.Default.Save();
}
else
{
ServoResult.IsSuccess = false;
return ServoResult;
}
if (SerialPortFunctions.TryCloseSerialPort(GlobalVariables.leftSerialPort, "Drive Setup") == false)
{
ServoResult.IsSuccess = false;
return ServoResult;
}
else
{
ServoResult.IsSuccess = true;
return ServoResult;
}
}
public static objServoResult EnableDrive(ISerialPortIo serialPort)
{
objServoResult ServoResult = new objServoResult();
if (SerialMotorControl.EnableDrive(serialPort) == true)
{
ServoResult.IsSuccess = true;
ServoResult.ErrorMessage = "";
ServoResult.ErrorCode = 0;
}
else
{
ServoResult.IsSuccess = false;
ServoResult.ErrorMessage = "Enable Drive Failed.";
ServoResult.ErrorCode = -3;
}
if (ServoResult.IsSuccess == false)
{
//frmCustomDialog frmObject_CustomDialog = new frmCustomDialog();
//frmObject_CustomDialog.lblDialogText.Text = "Enable Drive Failed: Check cable connections.";
//frmObject_CustomDialog.Show();
frmMessageDialog.DisplayBriefMessage("Enable Drive Failed: Check cable connections.");
}
return ServoResult;
}
public static objServoResult TrySetServoBaudRate(ISerialPortIo serialPort)
{
objServoResult ServoResult = new objServoResult();
serialPort.BaudRate = 9600;
if (SerialPortFunctions.TryOpenSerialPort(serialPort, "Drive Setup") == true)
{
//Manually Set Menu PE-202 to 3 (Baud: 57600)
SerialMotorControl.WriteSingleRegister(78, 3, serialPort);
}
else
{
ServoResult.IsSuccess = false;
return ServoResult;
}
if (SerialPortFunctions.TryCloseSerialPort(GlobalVariables.leftSerialPort, "Drive Setup") == false)
{
ServoResult.IsSuccess = false;
return ServoResult;
}
else
{
ServoResult.IsSuccess = true;
return ServoResult;
}
}
public static objServoResult DisableDrive(ObjectClasses.SerialPortIo serialPort)
{
objServoResult ServoResult = new objServoResult();
if (SerialMotorControl.DisableDrive(serialPort) == true)
{
ServoResult.IsSuccess = true;
ServoResult.ErrorMessage = "";
ServoResult.ErrorCode = 0;
}
else
{
ServoResult.IsSuccess = false;
ServoResult.ErrorMessage = "Disable Drive Failed.";
ServoResult.ErrorCode = -2;
}
if (ServoResult.IsSuccess == false)
{
frmMessageDialog.DisplayBriefMessage("Disable Drive Failed: Check cable connections.");
}
return ServoResult;
}
public static objServoResult ReadFeedback_OnDataReceived(System.IO.MemoryStream stream, 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.SerialPortBusy = true;
RetVal.IsSuccess = true;
//double[] FeedbackParams = new double[3];
FeedbackParams[0] = 0;
FeedbackParams[1] = 0;
FeedbackParams[2] = 0;
FeedbackParams[3] = 0;
currentByte = 0;
ReadSuccess = false;
ErrorCode = 1;
TimeOutCounter = 0;
sb.Clear();
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 (stream.Length != 0)
// //if (serialPort1.BytesToRead != 0) //Check to make sure there are bytes in the buffer
// {
// currentByte = (byte)stream.ReadByte();
// //if (SerialPortFunctions.TryReadSerialPort(serialPort1, out currentByte) == false)
// //{
// // RetVal.IsSuccess = false;
// // RetVal.ErrorCode = 4;
// // return RetVal;
// //}
// }
// else TimeOutCounter++;
// if (TimeOutCounter >= 30000)
// {
// RetVal.ErrorCode = 3; RetVal.IsSuccess = false;
// return RetVal;
// }
//}
if (true) //Initiate Low Duty Cycle command success | Packet header
{
stream.Position = 0;
for (int k = 0; k < 72; k++) //56 is the old number of bytes excl contact status
{
currentByte = (byte)stream.ReadByte();
//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
{
}
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))));
//Absolute Pulse Value
FeedbackParams[4] = ConvertToInt32(sb);
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);
//System.Diagnostics.Debug.WriteLine(streamPacket[59].ToString("X2"));
//if (FeedbackParams[3] == 0x0B)
//{
// MessageBox.Show("!");
//}
ReadSuccess = true;
}
} while (ReadSuccess == false);
RetVal.IsSuccess = true;
//if (SerialPortFunctions.TryDiscardInputBuffer(serialPort1) == false) { RetVal.ErrorCode = 1; RetVal.IsSuccess = false; return RetVal; }
//GlobalVariables.SerialPortBusy = false;
return RetVal;
}
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;
}
public objServoResult ReadFeedBack_OnDataReceived()
{
GlobalVariables.SerialPortBusyLeft = true;
while (GlobalVariables.leftSerialPort.BytesToRead != 0)
{
if (SerialReadInProgress == false)
{
if (CurrentByte != 0x06)
{
if (GlobalVariables.leftSerialPort.BytesToRead != 0)
{
CurrentByte = (byte)GlobalVariables.leftSerialPort.ReadByte();
Result.IsSuccess = true;
continue;
}
else TimeOutCounter++;
if (TimeOutCounter >= 30000)
{
Result.ErrorCode = 3;
Result.IsSuccess = false;
GlobalVariables.SerialPortBusyLeft = false;
return Result;
}
}
else
{
SerialReadInProgress = true;
Result.IsSuccess = true;
}
}
else
{
CurrentByte = (byte)GlobalVariables.leftSerialPort.ReadByte();
ByteStream.WriteByte(CurrentByte);
if (ByteStream.Length == 72)
{
Result = SerialMotorControl.ReadFeedback_OnDataReceived(ByteStream, FeedbackParams, ReadSuccess, currentByte1, streamArray, currentSpeed, currentPulse, currentTorque, currentContactStatus, ref ErrorCode, TimeOutCounter, ServoResult, sb, ByteString);
SerialReadInProgress = false;
ByteStream = new MemoryStream();
if ((new System.Collections.BitArray(new byte[] { currentContactStatus[7] }))[2] == false)
eStopCounter++;
else eStopCounter = 0;
if (Result.IsSuccess == false)
{
}
else if (eStopCounter >= 2) //The status of SPD2 is given by bit 3 of byte 8 of the contact statuc byte array
{
Result.IsSuccess = false;
Result.ErrorCode = 5;
Result.ErrorMessage = "Emergency Stop Activated!";
eStopCounter = 0;
}
else
{
FeedbackTorque = (FeedbackParams[2] / 100 / 100) * CalibrationSettings.Default.RatedMotorTorque * CalibrationSettings.Default.GearRatio;
if (FeedbackTorque >= 1000)
FeedbackTorque = previousTorque;
previousTorque = FeedbackTorque;
FeedbackSpeed = FeedbackParams[0] / (CalibrationSettings.Default.GearRatio * 10);
FeedbackPower = Math.Abs(FeedbackTorque * FeedbackSpeed * 2 * Math.PI / 60);
FeedbackPulse = FeedbackParams[1]; //Feedback Pulse Position in Current Cycle
FeedbackPulseAbs = FeedbackParams[4]; //Absolute Feedback Pulse Position
FeedbackContactStatus = FeedbackParams[3];
FeedbackAngle1 = CalculateAngle();
DataSampleCalculations();
//Clear the buffer if it starts to fill up
if (GlobalVariables.leftSerialPort.BytesToRead > 219)
{
if (BufferOverFlowEvent != null)
{
BufferOverFlowEventArgs args = new BufferOverFlowEventArgs(GlobalVariables.leftSerialPort.BytesToRead);
BufferOverFlowEvent(this, args);
}
GlobalVariables.leftSerialPort.DiscardInBuffer();
}
if (SerialReadCompleteEvent != null)
{
SerialReadCompleteEventArgs args = new SerialReadCompleteEventArgs(FeedbackTorque, FeedbackAngle1);
SerialReadCompleteEvent(this, args);
}
}
GlobalVariables.SerialPortBusyLeft = false;
return Result;
}
else
{
Result.IsSuccess = true;
}
}
}
GlobalVariables.SerialPortBusyLeft = false;
return Result;
}
//Read Current Feedback Parameters
public objServoResult ReadFeedBack()
{
Result = SerialMotorControl.ReadFeedback(serialPort, FeedbackParams, ReadSuccess, currentByte1, streamArray, currentSpeed, currentPulse, currentTorque, currentContactStatus, ref ErrorCode, TimeOutCounter, ServoResult, sb, ByteString);
if (Result.IsSuccess == false)
{
frmMessageDialog ErrorMessage = new frmMessageDialog();
ErrorMessage.lblDialogText.Text = "Error Occured. Error Code: \"" + Result.ErrorCode.ToString() + "\"";
ErrorMessage.Show();
return Result;
}
else if (FeedbackParams[3] == 0x0B)
{
Result.IsSuccess = false;
Result.ErrorCode = 5;
Result.ErrorMessage = "Emergency Stop Activated!";
return Result;
}
else
{
FeedbackTorque = (FeedbackParams[2] / 100 / 100) * CalibrationSettings.Default.RatedMotorTorque * CalibrationSettings.Default.GearRatio;
FeedbackSpeed = FeedbackParams[0] / (CalibrationSettings.Default.GearRatio * 10);
FeedbackPower = Math.Abs(FeedbackTorque * FeedbackSpeed * 2 * Math.PI / 60);
FeedbackPulse = FeedbackParams[1];
FeedbackContactStatus = FeedbackParams[3];
DataSampleCalculations();
return Result;
}
}
