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