public PrgSts Disable() { var disableDone = new PrgSts(); while (disableDone.State == Status.Processing) { switch (disableDone.Step) { case 0: // data received event handler for COM1 _comPort.DataReceived += new SerialDataReceivedEventHandler(DataReceived); try { // Open the port for communications _comPort.Open(); disableDone.State = Status.Processing; disableDone.Step = 1; } catch (Exception e) { disableDone.Step = 30; disableDone.Message = "DisableEpos Port Failed to Open"; } break; // Disable Epos command case 1: _disVoltSts = SendData(FormatWriteMessage(0x6040, 0x00), _disVoltSts); if (_disVoltSts.State == Status.FinishedOk) { disableDone.Step = 20; } else if (_disVoltSts.State == Status.Failed || _disVoltSts.State == Status.TimedOut) { disableDone.Step = 30; disableDone.Message = "Error Writing the DisableEpos command"; } break; // Report Disable Epos successful case 20: disableDone.Step = 0; disableDone.State = Status.FinishedOk; disableDone.Message = "Epos is sucessfully Disabled"; // close com port when done _comPort.Close(); break; // Report Disable Epos failed case 30: disableDone.Step = 0; disableDone.State = Status.Failed; _comPort.Close(); break; } } return(disableDone); }
/* SetSpeed sets the Epos CalculatedSpeed by writing the Velocity Mode RPM * The function takes a the CalculatedSpeed in mm/min as a parameter and converts it into the RPM * The resulting RPM is used to writing the Velocity Mode RPM setting * Step numbers and their function: * 0. Initialise variables used for communication * Attempty to open COM port; if the COM port is busy, report InitFail (jump to Step 30) * Attach serial event handler * 1. Write the CalculatedSpeed via the Velocity Mode RPM * 20. Report that the CalculatedSpeed has been set successfully * 30. Report that the CalculatedSpeed setting command has failed */ public PrgSts SetSpeed(double speed) { var prgSetSpeed = new PrgSts(); var setSpdSts = new RwSts(); // calculate the RPM as a function of speed (speed is in mm/s) Int32 rpm = (Int32)(((speed * 81) / (Math.PI * 84)) * 60); while (prgSetSpeed.State == Status.Processing) { switch (prgSetSpeed.Step) { case 0: // data received event handler for COM1 _comPort.DataReceived += new SerialDataReceivedEventHandler(DataReceived); try { // Open the port for communications _comPort.Open(); prgSetSpeed.State = Status.Processing; prgSetSpeed.Step = 1; } catch (Exception e) { prgSetSpeed.Step = 30; prgSetSpeed.Message = "SetSpeed Port Failed to Open/n" + e.Message; } break; // Set speed Epos command case 1: setSpdSts = SendData(FormatWriteMessage(0x206B, rpm), setSpdSts); if (setSpdSts.State == Status.FinishedOk) { prgSetSpeed.Step = 20; } else if (setSpdSts.State == Status.Failed || setSpdSts.State == Status.TimedOut) { prgSetSpeed.Step = 30; prgSetSpeed.Message = "Error Writing the SetSpeed command"; } break; // Report Disable Epos successful case 20: prgSetSpeed.Step = 0; prgSetSpeed.State = Status.FinishedOk; prgSetSpeed.Message = "Motor speed sucessfully set"; // close com port when done _comPort.Close(); break; // Report Disable Epos failed case 30: prgSetSpeed.Step = 0; prgSetSpeed.State = Status.Failed; _comPort.Close(); break; } } return(prgSetSpeed); }
/* Generic format for writing a value to the motor * This function takes the pased byte aray containing a set of instructions that writes a value to the drive * The communications protocol in the drive requires the parts of the message to be sent in a specific order. * Each time a message is sent the drive responds with a code (either 'O' or 'F') identifying OKEn or Failed * The order of the message structure is as follows: * Step 1 = Send op code byte -> stored in byte 0 of the byte array * Step 2 = Wait for received event triggered, * if not recieved in 1 second exit under fault * Step 3 = Check for "O" / 'F' response * Step 4 = Send the write value request -> stored in bytes 1-11 of array (See FormatWriteMessage for byte definition) * Step 5 = Wait for received event triggered; * if not recieved in 1 second exit under fault * Step 6 = Check for "O" / 'F' response * Step 7 = Send "O" for acknowledgement -> stored in byte 12 of the byte array * Step 8 = Wait for the received event triggered * if not received in 1 second exit under fault * Step 9 = Receive a response from the Epos * Step 10 = Send an "O" for acknowledgement -> stored in byte 12 of the byte array * -------- * Step 20 = Data sent correctly * Step 30 = Communication failure with the Epos * Step 40 = Time Out in receiving a response from the */ private RwSts SendData(byte[] rs232Set, RwSts send) { // buffer that stores the data received back from the EPOS if (_comPort.IsOpen) { byte[] RS232buffer = new byte[_comPort.BytesToRead]; switch (send.Step) { // initialise variables used for communication case 0: send.Step = 1; send.State = Status.Processing; break; case 1: // send the first byte in the array, representing the op code _comPort.Write(rs232Set, 0, 1); // Store curent ticks + 1 second for timeout on recieve _endTicks = Environment.TickCount + 1000; // Go to next step send.Step = 2; break; case 2: // Wait for event handler data recieved if (_isDataReceived) { send.Step = 3; } if (Environment.TickCount > _endTicks) { // No response within 1 second send.Step = 40; } break; case 3: // Read the response from the EPOS _comPort.Read(RS232buffer, 0, _comPort.BytesToRead); _isDataReceived = false; send.Step = 4; break; case 4: // Write request to read (bytes 1->11 are part of the request) _comPort.Write(rs232Set, 1, 11); // Store curent ticks + 1 second for timeout on recieve _endTicks = Environment.TickCount + 1000; send.Step = 5; break; case 5: // Wait for event handler data recieved if (_isDataReceived) { send.Step = 6; } if (Environment.TickCount > _endTicks) { // No response within 1 second send.Step = 40; } break; case 6: // Read the response from the EPOS _comPort.Read(RS232buffer, 0, _comPort.BytesToRead); _isDataReceived = false; // Check if the response back is an 'O' character to say OPCode was received OKEn if (RS232buffer[0] == 79) { send.Step = 7; } else { // Error was received send.Step = 30; } break; case 7: // send the 12th byte in the array, representing an acknowledgement (character 'O') _comPort.Write(rs232Set, 12, 1); // Store curent ticks + 1 second for timeout on recieve _endTicks = Environment.TickCount + 1000; // Go to next step send.Step = 8; break; case 8: // Wait for event handler data recieved if (_isDataReceived) { send.Step = 9; } if (Environment.TickCount > _endTicks) { // No response within 1 second send.Step = 40; } break; case 9: // Read the response from the EPOS _comPort.Read(RS232buffer, 0, _comPort.BytesToRead); _isDataReceived = false; send.Step = 10; break; case 10: // send the 12th byte in the array, representing an acknowledgement (character 'O') _comPort.Write(rs232Set, 12, 1); // Operation finished sucessfully send.Step = 20; break; // Operation Finished sucessfully case 20: send.Step = 0; send.State = Status.FinishedOk; send.Message = "Operation Finished Sucessfully"; break; // Communications error received case 30: send.Step = 0; send.State = Status.Failed; send.Message = "Communication Error Received"; break; // Operation timed out case 40: send.Step = 0; send.State = Status.TimedOut; send.Message = "Operation Timed Out"; break; } } return(send); }
/* * The function performs the following: * Reads the status of the Epos * At any time, the motor can be * Switch On Disable * Ready To Switch On * SwitchedOn * Operation Enabled * Fault Mode * QuickStop Active * * The purpose of the EnableEposFunction function is to get the motor in the Operation Enabled state * In order to do that the motor performs the following steps * Reads the status * If the Motor is in Fault Mode, reset the Fault * If QuickStop is enabled, turn QuickStop off * Read the status * If the motor is Switch On Disabled, perform a "ShutDown" function (note: this is not a mistake, the terminilogy is used in the manual) * Read the status * If the motor is Ready to Switch On, or SwitchedOn then perform a 'SwitchOnAndEnableVoltage' command * The motor is now Enabled and ready to receive commands * * Set the operation mode to Velocity (OpMode = -2) * Set the Maximum Velocity to ... * Set the Maximum Acceleration to... * * Step numbers and their function: * 0. Initialise variables used for communication * Attempty to open COM port; if the COM port is busy, report InitFail (jump to Step 30) * Attach serial event handler * 1. Read Epos Status Word and decode Epos State * Based on the status, perform the following operations; see the SDS flowchart page 36 * 2. Send ShutDown command * 3. Send SwitchOnAndEnVoltage command * 4. Send SetOpNo command * 5. Send SetMaxVelocity command * 6. Send SetMaxAcceleration command; report InitDone (jump to Step 20) * 7. Send DisableVoltage command * 8. Send ResetFault command * 20. Report that the Epos has been Enabled successfully * 30. Report that the Epos Enable failed * * The index values used for Writing/Reading a value in the Epos (See SDS, page 28) * StatusWord - 0x6041 * ControlWord - 0x6040 * SetOpNumber - 0x6060 * SetMaxVelocity - 0x607F * SetMaxAcc - 0x60C5 */ public PrgSts Enable() { var initDone = new PrgSts(); var readWordSts = new RwSts(); var shutDownSts = new RwSts(); var resVolSts = new RwSts(); var setVelSts = new RwSts(); var swOnSts = new RwSts(); var setOpSts = new RwSts(); var setAccSts = new RwSts(); var disVolSts = new RwSts(); while (initDone.State == Status.Processing) { switch (initDone.Step) { // initialise variables case 0: // data received event handler for COM1 if (!_isComEventSet) { _isComEventSet = true; _comPort.DataReceived += new SerialDataReceivedEventHandler(DataReceived); } try { // Open the port for communications _comPort.Open(); initDone.State = Status.Processing; initDone.Step = 1; } catch (Exception e) { initDone.Step = 30; initDone.Message = "EnableEpos Port Failed to Open"; } break; /* Decode Epos Status * Request the Epos the controlword, by using a formatted array (See FormatReadMessage function) * When the ControlWord has been received, decode it and return the state */ case 1: // Read the StatusWord readWordSts = ReceiveData(FormatReadMessage(0x6041, 0x00), readWordSts); // RWState is 2 when Reading the StatusWord was successful if (readWordSts.State == Status.FinishedOk) { // Decode the Status Word // The flowchart on how StatusWord is interpreted can be found in the SDS, page 36 switch (_statusWord) { // Epos is Disabled - Perform a 'ShutDown' command case 0x140: initDone.Step = 2; break; // Epos is Ready - Perform a 'SwitchOn and EnableVoltage' command case 0x121: initDone.Step = 3; initDone.Message = "1) Epos is Ready - Perform a 'SwitchOn and EnableVoltage' command"; break; // Epos is Switched On - Perform a 'SwichOnAndEnableVoltage' command case 0x123: initDone.Step = 3; break; // Epos is Enabled - Perform a 'SetOperationNumber' command case 0x137: initDone.Step = 4; break; // QuickStop is on - Reset QuickStop by performing a DisableVoltage command case 0x117: initDone.Step = 7; break; // Epos is in Fault Mode - Perform a 'ResetFault' command case 0x108: initDone.Step = 8; initDone.Message = "1) Epos is in Fault Mode - Perform a 'ResetFault' command"; break; } } // if the Read Function timed out or a communication error occured, report that the initialisation failed else if (readWordSts.State == Status.Failed || readWordSts.State == Status.TimedOut) { initDone.Step = 30; initDone.Message = "Error Reading the StatusWord"; } break; // Shutdown command case 2: shutDownSts = SendData(FormatWriteMessage(0x6040, 0x06), shutDownSts); if (shutDownSts.State == Status.FinishedOk) { initDone.Step = 3; } else if (shutDownSts.State == Status.Failed || shutDownSts.State == Status.TimedOut) { initDone.Step = 30; initDone.Message = "Error Writing the ShutDown command"; } break; // Switch On and Enable Voltage command case 3: swOnSts = SendData(FormatWriteMessage(0x6040, 0x0F), swOnSts); if (swOnSts.State == Status.FinishedOk) { initDone.Step = 4; } else if (swOnSts.State == Status.Failed || swOnSts.State == Status.TimedOut) { initDone.Step = 30; initDone.Message = "Error Writing the SwitchOnAndEnableVoltage command"; } break; // Set Operation Number in Epos case 4: setOpSts = SendData(FormatWriteMessage(0x6060, -2), setOpSts); if (setOpSts.State == Status.FinishedOk) { initDone.Step = 5; } else if (setOpSts.State == Status.Failed || setOpSts.State == Status.TimedOut) { initDone.Step = 30; initDone.Message = "Error Writing the SetOperationNumber command"; } break; // Set Maximum Velocity command case 5: setVelSts = SendData(FormatWriteMessage(0x607F, 5000), setVelSts); if (setVelSts.State == Status.FinishedOk) { initDone.Step = 6; } else if (setVelSts.State == Status.Failed || setVelSts.State == Status.TimedOut) { initDone.Step = 30; initDone.Message = "Error Writing the SetMaximumVelocity command"; } break; // Set Maximum Acceleration command // Value changed from 4000 to 1000 by SJH 9th February 2015 case 6: setAccSts = SendData(FormatWriteMessage(0x60C5, 1000), setAccSts); if (setAccSts.State == Status.FinishedOk) { initDone.Step = 20; } else if (setAccSts.State == Status.Failed || setAccSts.State == Status.TimedOut) { initDone.Step = 30; initDone.Message = "Error Writing the SetMaximumAcceleration command"; } break; // DisableVoltage command case 7: disVolSts = SendData(FormatWriteMessage(0x6040, 0x00), disVolSts); if (disVolSts.State == Status.FinishedOk) { initDone.Step = 1; } else if (disVolSts.State == Status.Failed || disVolSts.State == Status.TimedOut) { initDone.Step = 30; initDone.Message = "Error Writing the DisableVoltage command"; } break; // Reset Voltage command case 8: resVolSts = SendData(FormatWriteMessage(0x6040, 0x80), resVolSts); if (resVolSts.State == Status.FinishedOk) { initDone.Step = 1; } else if (resVolSts.State == Status.Failed || resVolSts.State == Status.TimedOut) { initDone.Step = 30; initDone.Message = "Error Writing the ResetFault command"; } break; // Report Initialisation sucessful case 20: initDone.Step = 0; initDone.State = Status.FinishedOk; initDone.Message = "Epos is sucessfully initialised"; // close com port when done _comPort.Close(); break; // report Initialisation Failed case 30: initDone.Step = 0; initDone.State = Status.Failed; _comPort.Close(); break; } } return(initDone); }
private RwSts ReceiveData(byte[] rs232Set, RwSts receive) { // dynamicly re-sizeable array based on the amount of bytes in the serial receive buffer var rs232Buff = new byte[_comPort.BytesToRead]; switch (receive.Step) { // initialise variables used for communication case 0: receive.State = Status.Processing; receive.Step = 1; break; case 1: //send 1st byte to EPOS2, this is the OPCode (0x10 = Read, 0X11 = Write) _comPort.Write(rs232Set, 0, 1); // Store curent ticks + 1 second for timeout on recieve _endTicks = Environment.TickCount + 1000; // Go to next step receive.Step = 2; break; case 2: // Wait for event handler data recieved if (_isDataReceived) { receive.Step = 3; } if (Environment.TickCount > _endTicks) { // No response within 1 second receive.Step = 40; } break; case 3: // Read the response from the EPOS _comPort.Read(rs232Buff, 0, _comPort.BytesToRead); // Reset received flag _isDataReceived = false; receive.Step = 4; break; case 4: // Write request string to read the software version _comPort.Write(rs232Set, 1, 7); // Store curent ticks + 1 second for timeout on recieve _endTicks = Environment.TickCount + 1000; receive.Step = 5; break; case 5: //Wait for event handler to say data recieved if (_isDataReceived) { receive.Step = 6; } if (Environment.TickCount > _endTicks) { // No response within 1 second receive.Step = 40; } break; case 6: // receive data from buffer _comPort.Read(rs232Buff, 0, _comPort.BytesToRead); _isDataReceived = false; // Check if the response back is an 'O' character to say data request was received OKEn if (rs232Buff[0] == 79) { receive.Step = 7; } else { // error in reading response from the Epos receive.Step = 30; } break; case 7: // write acknowledgement _comPort.Write(rs232Set, 8, 1); // Store curent ticks + 1 second for timeout on recieve _endTicks = Environment.TickCount + 1000; receive.Step = 8; break; case 8: //Wait for event handler data recieved if (_isDataReceived) { receive.Step = 9; } if (Environment.TickCount > _endTicks) { // No response within 1 second receive.Step = 40; } break; case 9: // receive data from the buffer _comPort.Read(rs232Buff, 0, _comPort.BytesToRead); // reset data received flag try { // interpret the reponse from the controller -> extract the 5th and 6th byte and convert to a word _statusWord = (ushort)((((ushort)(rs232Buff[6] * 256)) + ((ushort)rs232Buff[5]) & 0x417F)); } catch (Exception e) { // exception error handling _systemStatus = e.ToString(); // Control word could not be interpreted receive.Step = 30; break; } _isDataReceived = false; receive.Step = 10; break; case 10: // write acknowledgement (character "O") _comPort.Write(rs232Set, 8, 1); // Report successful finish receive.Step = 20; break; // Operation Finished sucessfully case 20: receive.Step = 0; receive.State = Status.FinishedOk; receive.Message = "Operation Finished Sucessfully"; break; // Communications error received case 30: receive.Step = 0; receive.State = Status.Failed; receive.Message = "Communication Error Received"; break; // Operation timed out case 40: receive.Step = 0; receive.State = Status.TimedOut; receive.Message = "Operation Timed Out"; break; } return(receive); }