public PrgSts Enable()
{
PrgSts prgHomeFesto = new PrgSts();
//evUpdatePhase(MachinePhase.Homing);
// data received event handler for Com2
if (!_comEventSet)
{
_comEventSet = true;
_comPort.DataReceived += new SerialDataReceivedEventHandler(DataReceived);
}
try
{
// Open the port for communications
_comPort.Open();
//if (_debugFesto)
{
Console.WriteLine("0) " + _comPort.PortName + "<> PORT OPEN");
}
// reset communication variables
prgHomeFesto.State = Status.Processing;
}
catch (Exception e)
{
prgHomeFesto.Message = "0) Port Failed to Open";
prgHomeFesto.State = Status.Failed;
//if (MainProg.bDebugFesto)
{
Console.WriteLine(_comPort.PortName + " - " + _message + " - " + e.ToString());
}
}
return(prgHomeFesto);
}
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);
}
/*
* 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);
}
