Subsystem Initialization complete code
This page contains the complete code of Subsystem Initialization.
In SystemInitialization.cpp, your Subsystem Initialization code should be as follows:
Copy
#include "RT_Project_01.h"
#include "SystemInitialization.h"
BOOL StartKingstar()
{
//Launch or connect to the KINGSTAR Subsystem process.
KsError nRet = errNoError;
nRet = Create(INSTANCE, 0);
if (nRet != errNoError)
{
RtPrintf("InitializeLink failed: %x\n", nRet);
return FALSE;
}
//Configure the Subsystem settings. We set it cycle500.
//You can find the corresponding cycle time in ksapi.h.
//Time unit: second.
/*cycle100 = 0.0001
cycle125 = 0.000125
cycle250 = 0.000250
cycle500 = 0.0005
cycle1000 = 0.001*/
nRet = SetCycleTime(CYCLE_TIME);
if (nRet != errNoError)
{
RtPrintf("SetCycleTime failed: %x\n", nRet);
WaitForCommand(5, FALSE, Stop());
Destroy();
return FALSE;
}
/*We use accessPos, which supports modeDirectPos, modeMasterIntPos, and modeManual.
By default, it uses modeMasterIntPos.*/
nRet = SetAxisAccessMode(ACCESS_MODE);
if (nRet != errNoError)
{
RtPrintf("SetAxisAccessMode failed: %x\n", nRet);
WaitForCommand(5, FALSE, Stop());
Destroy();
return FALSE;
}
//VERBOSITY is FALSE.
nRet = EnableServerLog(VERBOSITY);
if (nRet != errNoError)
{
RtPrintf("EnableServerLog failed: %x\n", nRet);
WaitForCommand(5, FALSE, Stop());
Destroy();
return FALSE;
}
//SERVO_INPUT is TRUE.
nRet = EnableAxisInput(SERVO_INPUT);
if (nRet != errNoError)
{
RtPrintf("EnableAxisInput failed: %x\n", nRet);
WaitForCommand(5, FALSE, Stop());
Destroy();
return FALSE;
}
//SERVO_OUTPUT is FALSE.
nRet = EnableAxisOutput(SERVO_OUTPUT);
if (nRet != errNoError)
{
RtPrintf("EnableAxisOutput failed: %x\n", nRet);
WaitForCommand(5, FALSE, Stop());
Destroy();
return FALSE;
}
//SECOND_ENCODER is FALSE.
nRet = EnableSecondEncoder(SECOND_ENCODER);
if (nRet != errNoError)
{
RtPrintf("EnableSecondEncoder failed: %x\n", nRet);
WaitForCommand(5, FALSE, Stop());
Destroy();
return FALSE;
}
//ACTUAL_VELOCITY is TRUE.
nRet = EnableActualVelocity(ACTUAL_VELOCITY);
if (nRet != errNoError)
{
RtPrintf("EnableActualVelocity failed: %x\n", nRet);
WaitForCommand(5, FALSE, Stop());
Destroy();
return FALSE;
}
//ACTUAL_TORQUE is TRUE.
nRet = EnableActualTorque(ACTUAL_TORQUE);
if (nRet != errNoError)
{
RtPrintf("EnableActualTorque failed: %x\n", nRet);
WaitForCommand(5, FALSE, Stop());
Destroy();
return FALSE;
}
//ACTUAL_CURRENT is TRUE.
nRet = EnableActualCurrent(ACTUAL_CURRENT);
if (nRet != errNoError)
{
RtPrintf("EnableActualCurrent failed: %x\n", nRet);
WaitForCommand(5, FALSE, Stop());
Destroy();
return FALSE;
}
//FOLLOWING_ERROR is TRUE.
nRet = EnableFollowingError(FOLLOWING_ERROR);
if (nRet != errNoError)
{
RtPrintf("EnableFollowingError failed: %x\n", nRet);
WaitForCommand(5, FALSE, Stop());
Destroy();
return FALSE;
}
//MAX_TORQUE is FALSE.
nRet = EnableMaxTorque(MAX_TORQUE);
if (nRet != errNoError)
{
RtPrintf("EnableMaxTorque failed: %x\n", nRet);
WaitForCommand(5, FALSE, Stop());
Destroy();
return FALSE;
}
//MAX_CURRENT is FALSE.
nRet = EnableMaxCurrent(MAX_CURRENT);
if (nRet != errNoError)
{
RtPrintf("EnableMaxCurrent failed: %x\n", nRet);
WaitForCommand(5, FALSE, Stop());
Destroy();
return FALSE;
}
/*Enable the control mode change through PDO. This way, you can change
the control mode while the drive is enabled, but when you change the mode,
the motor can't be running. Not all drives support this feature.*/
//SYNCHORNIZED_CONTROL_MODE is TRUE.
nRet = EnableSynchronizedControlMode(SYNCHORNIZED_CONTROL_MODE);
if (nRet != errNoError)
{
RtPrintf("EnableSynchronizedControlMode failed: %x\n", nRet);
WaitForCommand(5, FALSE, Stop());
Destroy();
return FALSE;
}
//AUTO_RESTART is TRUE.
nRet = EnableAutoRestart(AUTO_RESTART);
if (nRet != errNoError)
{
RtPrintf("EnableAutoRestart failed: %x\n", nRet);
WaitForCommand(5, FALSE, Stop());
Destroy();
return FALSE;
}
//AUTO_REPAIR is TRUE.
nRet = EnableAutoRepair(AUTO_REPAIR);
if (nRet != errNoError)
{
RtPrintf("EnableAutoRepair failed: %x\n", nRet);
WaitForCommand(5, FALSE, Stop());
Destroy();
return FALSE;
}
//AUTO_CONFIG is TRUE.
nRet = EnableAutoConfig(AUTO_CONFIG);
if (nRet != errNoError)
{
RtPrintf("EnableAutoConfig failed: %x\n", nRet);
WaitForCommand(5, FALSE, Stop());
Destroy();
return FALSE;
}
//ENABLE_DC is TRUE, DC_CHECK is TRUE, DC_MASTER_SHIFT is FALSE.
nRet = ConfigureDc(ENABLE_DC, DC_CHECK, DC_MASTER_SHIFT, 0);
if (nRet != errNoError)
{
RtPrintf("ConfigureDc failed: %x\n", nRet);
WaitForCommand(5, FALSE, Stop());
Destroy();
return FALSE;
}
//HOT_CONNECT is FALSE.
nRet = EnableHotConnect(HOT_CONNECT);
if (nRet != errNoError)
{
RtPrintf("EnableHotConnect failed: %x\n", nRet);
WaitForCommand(5, FALSE, Stop());
Destroy();
return FALSE;
}
//REDUNDANCY is FALSE.
nRet = EnableRedundancy(REDUNDANCY);
if (nRet != errNoError)
{
RtPrintf("EnableRedundancy failed: %x\n", nRet);
WaitForCommand(5, FALSE, Stop());
Destroy();
return FALSE;
}
//TOUCHPROBE is TRUE.
nRet = EnableTouchProbe(TOUCHPROBE);
if (nRet != errNoError)
{
RtPrintf("EnableTouchProbe failed: %x\n", nRet);
WaitForCommand(5, FALSE, Stop());
Destroy();
return FALSE;
}
//AXES_COUNT is two.
nRet = SetConfiguredAxesCount(AXES_COUNT);
if (nRet != errNoError)
{
RtPrintf("SetConfiguredAxesCount failed: %x\n", nRet);
WaitForCommand(5, FALSE, Stop());
Destroy();
return FALSE;
}
//IO_COUNT is three.
nRet = SetConfiguredIoCount(IO_COUNT);
if (nRet != errNoError)
{
RtPrintf("SetConfiguredIoCount failed: %x\n", nRet);
WaitForCommand(5, FALSE, Stop());
Destroy();
return FALSE;
}
//IO_INPUT_LENGTH is 16. IO_OUTPUT_LENGTH is 16.
SlaveStatus simulatedIo = { 0 };
simulatedIo.InputLength = IO_INPUT_LENGTH;
simulatedIo.OutputLength = IO_OUTPUT_LENGTH;
for (int i = 0; i < IO_COUNT; i++)
{
nRet = ConfigureIo(i, simulatedIo);
if (nRet != errNoError) {
RtPrintf("ConfigureIo failed: %x\n", nRet);
WaitForCommand(5, FALSE, Stop());
Destroy();
return FALSE;
}
}
//Start the EtherCAT network.
//This function is asynchronous so the network is not started yet when it returns.
/*Some hardware may require more than 30 seconds to start because DC check needs
some time to complete.*/
KsCommandStatus status = WaitForCommand(40, TRUE, Start());
if (status.Error)
{
RtPrintf("Start failed: %d\n", status.ErrorId);
WaitForCommand(5, FALSE, Stop());
Destroy();
return FALSE;
}
SubsystemStatus Subsystem = { ecatOffline, ecatOffline, 0, 0, 0, {ecatOffline}, {ecatOffline}, {axisOffline} };
GetStatus(&Subsystem, NULL);
if (Subsystem.State != ecatOP && Subsystem.State != ecatOffline)
{
RtPrintf("The KINGSTAR Subsystem in an unexpected state: %d\n", Subsystem.State);
return FALSE;
}
return TRUE;
}
BOOL StopKingstar()
{
BOOL Success = TRUE;
//Stop the EtherCAT network.
KsCommandStatus status = WaitForCommand(5, FALSE, Stop());
if (status.Error)
{
RtPrintf("Stop failed: %d\n", status.ErrorId);
Success = FALSE;
}
//Terminate the KINGSTAR Subsystem process.
KsError nRet = Destroy();
if (nRet != errNoError)
{
RtPrintf("DisposeLink failed: %x\n", nRet);
Success = FALSE;
}
return Success;
}