public objServoResult TryStopMotor(ISerialPortIo serialPort)
{
//Stop Motor - Ignore E-Stop (10010 - 18)
//Stop Motor - With E-Stop (10110 - 22)
//if (SerialMotorControl.WriteSingleRegister(236, 22, serialPort) == false)
if (SerialMotorControl.WriteSingleRegister(236, 17, serialPort) == false)
{
ServoResult.IsSuccess = false;
ServoResult.ErrorMessage = "Stop Motor Failed.";
ServoResult.ErrorCode = -1;
}
else
{
ServoResult.IsSuccess = true;
ServoResult.ErrorMessage = "";
ServoResult.ErrorCode = 0;
}
if (ServoResult.IsSuccess == false)
{
frmMessageDialog.DisplayBriefMessage("Stop Motor Failed: Check cable connections.");
}
return ServoResult;
}
public objServoResult SetMotorForce(int SelectedForce, ISerialPortIo serialPort)
{
ServoResult.IsSuccess = true; //Explicitrly Set the IsSuccess to true
double SelectedTorque = SelectedForce * CalibrationSettings.Default.SprocketRadius;
int torqueRefVal = GlobalFunctions.ConvertToTorqueRefVal(SelectedTorque);
//thisLock blocks the UI thread - Bad!!!
lock (thisLock)
{
System.Diagnostics.Debug.WriteLine("SetMotorTorque Lock Owner: " + Thread.CurrentThread.ManagedThreadId);
//Set Motor Torque Forward
if (SerialMotorControl.WriteSingleRegister(166, (UInt16)torqueRefVal, serialPort) == false)
{
ServoResult.IsSuccess = false;
}
//Set Motor Torque Reverse
if (SerialMotorControl.WriteSingleRegister(168, (UInt16)torqueRefVal, serialPort) == false)
{
ServoResult.IsSuccess = false;
}
}
return ServoResult;
}
public Bootloader(ISerialPortIo port, byte yPaddingByte, ChipData chipData, ushort endOfReadableEEPROM, int bytesPerEEPROMAddress)
{
sp = port;
m_yPaddingByte = yPaddingByte;
mChipData = chipData;
mEndOfReadableEEPROM = endOfReadableEEPROM;
mBytesPerEEPROMAddress = bytesPerEEPROMAddress;
}
public PrinterConnection(string com_port)
{
var portThreadSync = new object();
_safeSerialPort = new SafeSerialPort(com_port, portThreadSync)
{
Parity = Parity.None,
StopBits = StopBits.One,
Handshake = Handshake.None
};
InitConnection();
}
public override objServoResult ReadFeedBack_OnDataReceived2(ISerialPortIo serialPort)
{
if (GlobalVariables.leftSerialPort.BytesToRead == 0)
{
//Result.IsSuccess = false;
//Result.ErrorCode = 15;
Result.IsSuccess = true;
GlobalVariables.SerialPortBusyLeft = false;
return Result;
}
GlobalVariables.SerialPortBusyLeft = true;
int bytesToRead = GlobalVariables.leftSerialPort.BytesToRead;
byte[] buffer = new byte[bytesToRead];
int readBytes = 0;
try
{
readBytes = GlobalVariables.leftSerialPort.Read(buffer, 0, GlobalVariables.leftSerialPort.BytesToRead);
}
catch (Exception Ex)
{
//MessageBox.Show(Ex.Message + ": BytesToRead: + " + bytesToRead.ToString());
Result.IsSuccess = true;
GlobalVariables.SerialPortBusyLeft = false;
return Result;
}
if (readBytes == 15)
{
byte contactStatus = buffer[6];
var bit = (contactStatus & (1 << 3 - 1)) != 0;
if (bit == false)
{
Result.IsSuccess = false;
Result.ErrorCode = 5;
Result.ErrorMessage = "Emergency Stop Activated!";
GlobalVariables.SerialPortBusyLeft = false;
return Result;
}
int PulseLow = (buffer[8] + buffer[7] * 256);
int PulseHigh = ((buffer[10] + buffer[9] * 256) * 65536);
UInt32 PulseTotal = (UInt32)(PulseLow + PulseHigh);
//if (PulseTotal < 0)
// throw new Exception();
FeedbackPulse = PulseTotal - Convert.ToUInt32((12000 * CalibrationSettings.Default.GearRatio) * (Math.Floor(PulseTotal / (12000 * CalibrationSettings.Default.GearRatio))));
FeedbackAngle1 = CalculateAngle();
Int16 Speed = (Int16)(buffer[4] + buffer[3] * 256);
FeedbackSpeed = Speed / (CalibrationSettings.Default.GearRatio);
Int16 Torque = (Int16)(buffer[12] + buffer[11] * 256);
FeedbackTorque = Torque * CalibrationSettings.Default.RatedMotorTorque * CalibrationSettings.Default.GearRatio / 100 / 10;
//FeedbackPower = Math.Abs(FeedbackTorque * FeedbackSpeed * 2 * Math.PI / 60);
Result.IsSuccess = true;
//DataSampleCalculations();
}
else
{
Result.IsSuccess = true;
GlobalVariables.SerialPortBusyLeft = false;
return Result;
//GlobalVariables.SerialPortBusy = false;
//System.Diagnostics.Debug.WriteLine("Bytes to read (ReadFeedBack_OnDataReceived2): " + readBytes);
//Result.IsSuccess = false;
//Result.ErrorMessage = "Incorrect Number of Bytes in Buffer";
//Result.ErrorCode = 16;
}
GlobalVariables.SerialPortBusyLeft = false;
return Result;
}
private void ReadWrite(ISerialPortIo serialPort)
{
while (keepThreadGoing)
{
lock (thisLock)
{
System.Diagnostics.Debug.WriteLine("Lock on Read Serial Data, Thread Owner: " + Thread.CurrentThread.ManagedThreadId);
bool result = SerialMotorControl.ReadRegisters(serialPort);
System.Diagnostics.Debug.WriteLine("Lock Owner: " + 0);
}
Thread.Sleep(15); //Allow time for the UI thread to update the UI
}
}
public void StartServoDataStreamThread(ISerialPortIo serialPort)
{
keepThreadGoing = true;
startReadWrite = new Thread(() => ReadWrite(serialPort));
startReadWrite.Start();
}
public void TryDiscardOutBuffer(ISerialPortIo serialPort)
{
lock (thisLock)
{
serialPort.DiscardOutBuffer();
}
}
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;
}
}
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 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 objServoResult InitiateMotorForce(int SelectedSpeed, int SelectedForce, Enumerators.Direction SelectedDirection, ISerialPortIo serialPort)
{
int SpeedRefVal = Convert.ToInt32(Math.Round(Convert.ToDouble(SelectedSpeed) * CalibrationSettings.Default.GearRatio, 0));
double SelectedTorque = SelectedForce * CalibrationSettings.Default.SprocketRadius;
int TorqueRefVal = GlobalFunctions.ConvertToTorqueRefVal(SelectedTorque);
ServoResult.IsSuccess = true; //Explicitrly Set the IsSuccess to true
if (SelectedDirection == Enumerators.Direction.None)
{
return ServoResult;
}
else if (SelectedDirection == Enumerators.Direction.Forward)
{
GlobalVariables.leftSerialPort.MotorDirection = 1; //For Serial Port Faking (Forward)
GlobalVariables.rightSerialPort.MotorDirection = 1; //For Serial Port Faking (Forward)
if (SerialMotorControl.WriteSingleRegister(238, 17, serialPort) == false) //10001 / 17 (Old value was 31)
if (SerialMotorControl.WriteSingleRegister(238, 17, serialPort) == false)
ServoResult.IsSuccess = false;
}
else if (SelectedDirection == Enumerators.Direction.Backward)
{
GlobalVariables.leftSerialPort.MotorDirection = 2; //For Serial Port Faking (Reverse)
GlobalVariables.rightSerialPort.MotorDirection = 2; //For Serial Port Faking (Reverse)
if (SerialMotorControl.WriteSingleRegister(238, 16, serialPort) == false) //10000 / 16 (Old value was 30)
if (SerialMotorControl.WriteSingleRegister(238, 16, serialPort) == false)
ServoResult.IsSuccess = false;
}
//Set Motor Torque Forward
if (ServoResult.IsSuccess == true)
if (SerialMotorControl.WriteSingleRegister(166, (UInt16)TorqueRefVal, serialPort) == false)
if (SerialMotorControl.WriteSingleRegister(166, (UInt16)TorqueRefVal, serialPort) == false)
ServoResult.IsSuccess = false;
//Set Motor Torque Reverse
if (ServoResult.IsSuccess == true)
if (SerialMotorControl.WriteSingleRegister(168, (UInt16)TorqueRefVal, serialPort) == false)
if (SerialMotorControl.WriteSingleRegister(168, (UInt16)TorqueRefVal, serialPort) == false)
ServoResult.IsSuccess = false;
//Set Speed (Speed Command 7)
//if (ServoResult.IsSuccess == true)
// if (SerialMotorControl.WriteSingleRegister(292, (UInt16)SpeedRefVal, serialPort) == false)
// if (SerialMotorControl.WriteSingleRegister(292, (UInt16)SpeedRefVal, serialPort) == false)
// ServoResult.IsSuccess = false;
//Set Speed (Speed Command 1)
if (ServoResult.IsSuccess == true)
if (SerialMotorControl.WriteSingleRegister(280, (UInt16)SpeedRefVal, serialPort) == false)
if (SerialMotorControl.WriteSingleRegister(280, (UInt16)SpeedRefVal, serialPort) == false)
ServoResult.IsSuccess = false;
//Start Motor - Ignore E-Stop (10000 - 16)
//start Motor - With E-stop (10100 - 20)
//if (ServoResult.IsSuccess == true)
// if (SerialMotorControl.WriteSingleRegister(236, 20, serialPort) == false)
// if (SerialMotorControl.WriteSingleRegister(236, 20, serialPort) == false)
// ServoResult.IsSuccess = false;
//Start Motor - (11100 - 28)
if (ServoResult.IsSuccess == true)
if (SerialMotorControl.WriteSingleRegister(236, 28, serialPort) == false)
if (SerialMotorControl.WriteSingleRegister(236, 28, serialPort) == false)
ServoResult.IsSuccess = false;
if (ServoResult.IsSuccess == false)
{
frmMessageDialog.DisplayBriefMessage("Communications error: Check cable connections.");
}
if (ServoResult.IsSuccess == false)
ServoResult.ErrorMessage = "Error Occurred with WriteSingleRegister in InitiateMotor";
return ServoResult;
}
public static bool WriteSingleRegister(UInt16 address, UInt16 value, ISerialPortIo port)
{
int timeOutCounter = 0;
byte[] AddressBytes = BitConverter.GetBytes(address);
byte[] ValueBytes = BitConverter.GetBytes(value);
byte[] Command = new byte[8];
Command[0] = 0x00; //Node ID
Command[1] = 0x06; //Function
Command[2] = AddressBytes[1]; //Starting Address Hi
Command[3] = AddressBytes[0]; //Strating Address Low
Command[4] = ValueBytes[1]; //Quantity of Registers Hi
Command[5] = ValueBytes[0]; //Quantity of Registers Low
Command[6] = 0x00; //CRC Hi
Command[7] = 0x00; //CRC Low
byte[] array = BitConverter.GetBytes(ModRTU_CRC(Command, 6));
Command[6] = array[0]; //CRC Hi
Command[7] = array[1]; //CRC Low
//bool result = TransmitCommand(GlobalVariables.serialPort, Command, 8);
port.DiscardInBuffer();
port.DiscardOutBuffer();
port.Write(Command, 0, 8);
port.Flush();
while (port.BytesToWrite != 0)
{
//System.Diagnostics.Debug.WriteLine("Waiting to Write Bites: " + GlobalVariables.serialPort.BytesToWrite);
}
while (true)
{
if (timeOutCounter >= 1000000)
{
System.Diagnostics.Debug.WriteLine("failed");
return false;
}
if (port.BytesToRead >= 8)
{
System.Diagnostics.Debug.WriteLine(timeOutCounter++);
break;
}
timeOutCounter++;
}
//while (GlobalVariables.serialPort.BytesToRead < 8)
//{
// timeOutCounter++;
// if (timeOutCounter == 1000)
// {
// return false;
// }
//}
byte[] receiveBuffer = new byte[port.BytesToRead];
try
{
int readBytes = port.Read(receiveBuffer, 0, port.BytesToRead);
System.Diagnostics.Debug.WriteLine("Read Bytes: " + readBytes);
}
catch (Exception)
{
return false;
}
return true;
}
public static bool ReadRegisters(ISerialPortIo serialPort)
{
byte[] AddressBytes1 = BitConverter.GetBytes(2); //Current Speed At Address 2
byte[] AddressBytes2 = BitConverter.GetBytes(36); //Input Contact Status At Address 36
byte[] AddressBytes3 = BitConverter.GetBytes(6); //Single -Turn Data L At Address 46
byte[] AddressBytes4 = BitConverter.GetBytes(8); //Single -Turn Data H At Address 48
byte[] AddressBytes5 = BitConverter.GetBytes(22); //Current Torque At Address 22
//byte[] AddressBytes6 = BitConverter.GetBytes(38); //Output Contact Status At Address 38
byte[] Command = new byte[15];
Command[0] = 0x00; //Node ID
Command[1] = 0x6A; //Function
Command[2] = 0x0A; //Byte Count
Command[3] = AddressBytes1[1]; //Starting Address Hi
Command[4] = AddressBytes1[0]; //Strating Address Low
Command[5] = AddressBytes2[1]; //Starting Address Hi
Command[6] = AddressBytes2[0]; //Strating Address Low
Command[7] = AddressBytes3[1]; //Starting Address Hi
Command[8] = AddressBytes3[0]; //Strating Address Low
Command[9] = AddressBytes4[1]; //Starting Address Hi
Command[10] = AddressBytes4[0]; //Strating Address Low
Command[11] = AddressBytes5[1]; //Starting Address Hi
Command[12] = AddressBytes5[0]; //Strating Address Low
Command[13] = 0x00; //CRC Hi
Command[14] = 0x00; //CRC Low
byte[] array = BitConverter.GetBytes(ModRTU_CRC(Command, 13));
Command[13] = array[0]; //CRC Hi
Command[14] = array[1]; //CRC Low
serialPort.DiscardInBuffer();
serialPort.DiscardOutBuffer();
bool result = serialPort.WriteWaitResponse(Command, 0, 15, 15);
System.Diagnostics.Debug.WriteLine("ReadRegisters Thread Id: " + System.Threading.Thread.CurrentThread.ManagedThreadId);
return result;
}
public virtual objServoResult ReadFeedBack_OnDataReceived2(ISerialPortIo serialPort)
{
//if (GlobalVariables.leftSerialPort.BytesToRead == 0)
//{
// //Result.IsSuccess = false;
// //Result.ErrorCode = 15;
// Result.IsSuccess = true;
// GlobalVariables.SerialPortBusyLeft = false;
// return Result;
//}
//GlobalVariables.SerialPortBusyLeft = true;
int bytesToRead = serialPort.BytesToRead;
byte[] buffer = new byte[bytesToRead];
int readBytes = 0;
try
{
readBytes = serialPort.Read(buffer, 0, serialPort.BytesToRead);
}
catch (Exception Ex)
{
Result.IsSuccess = true;
//GlobalVariables.SerialPortBusyLeft = false;
return Result;
}
if (readBytes == 15)
{
System.Diagnostics.Debug.WriteLine(buffer[6]);
byte Address36_Low = buffer[6];
//var bit = (contactStatus & (1 << 3 - 1)) != 0;
var CWBit = (Address36_Low & (1 << 6)) != 0;
var CCWBit = (Address36_Low & (1 << 7)) != 0;
byte Address36_Hi = buffer[5];
var EmgStopBit = (Address36_Hi & (1)) != 0;
if (EmgStopBit == true)
{
EmgStop_Activated = true;
Result.IsSuccess = false;
Result.ErrorCode = 5;
Result.ErrorMessage = "Emergency Stop Activated!";
////GlobalVariables.SerialPortBusyLeft = false;
return Result;
}
if (CWBit == true)
CW_Activated = true;
if (CCWBit == true)
CCW_Activated = true;
//if (EmgStopBit == false)
//{
// Result.IsSuccess = false;
// Result.ErrorCode = 5;
// Result.ErrorMessage = "Emergency Stop Activated!";
// //GlobalVariables.SerialPortBusyLeft = false;
// return Result;
//}
UInt32 PulseLow = (UInt32)(buffer[8] + buffer[7] * 256);
UInt32 PulseHigh = (UInt32)((buffer[10] + buffer[9] * 256) * 65536);
UInt32 PulseTotal = (UInt32)(PulseLow + PulseHigh);
if (PulseTotal < 0)
throw new Exception();
FeedbackPulse = PulseTotal - Convert.ToUInt32((12000 * CalibrationSettings.Default.GearRatio) * (Math.Floor(PulseTotal / (12000 * CalibrationSettings.Default.GearRatio))));
FeedbackPulseAbs = PulseTotal; //Absolute Feedback Pulse Position
FeedbackAngle1 = CalculateAngle();
Int16 Speed = (Int16)(buffer[4] + buffer[3] * 256);
FeedbackSpeed = Speed / (CalibrationSettings.Default.GearRatio);
Int16 Torque = (Int16)(buffer[12] + buffer[11] * 256);
FeedbackTorque = Torque * CalibrationSettings.Default.RatedMotorTorque * CalibrationSettings.Default.GearRatio / 100 / 10;
FeedbackPower = Math.Abs(FeedbackTorque * FeedbackSpeed * 2 * Math.PI / 60);
FeedbackDistance = CalculateDistance();
Result.IsSuccess = true;
DataSampleCalculations();
}
else
{
Result.IsSuccess = true;
//GlobalVariables.SerialPortBusyLeft = false;
return Result;
//GlobalVariables.SerialPortBusy = false;
//System.Diagnostics.Debug.WriteLine("Bytes to read (ReadFeedBack_OnDataReceived2): " + readBytes);
//Result.IsSuccess = false;
//Result.ErrorMessage = "Incorrect Number of Bytes in Buffer";
//Result.ErrorCode = 16;
}
//GlobalVariables.SerialPortBusyLeft = false;
return Result;
}
