Axis Configuration complete code
This page contains the complete code of Axis Configuration.
In AxisConfiguration.cpp, your Axis Configuration code should be as follows:
Copy
#include "RT_Project_01.h"
#include "AxisConfiguration.h"
using namespace std;
VOID PrintDeviceInformation(int Index)
{
int Resolution = 0;
KsError nRet = errNoError;
SlaveStatus Sts = { 0 };
nRet = GetAxisByIndex(Index, &Sts, &Resolution, NULL, NULL);
if (nRet != errNoError)
RtPrintf("GetAxisByIndex failed: %x\n", nRet);
RtPrintf("Device Index: %d, Device Name: %s, Vendor ID: 0x%x, Product Code: 0x%x, Revision Number: 0x%x, Serial Number: 0x%x\n",
Index, Sts.Name, Sts.VendorId, Sts.ProductCode, Sts.RevisionNumber, Sts.SerialNumber);
RtPrintf("Slave ID: %d, Physical Address: %d, Alias Address: %d\n", Sts.SlaveId, Sts.PhysAddress, Sts.AliasAddress);
RtPrintf("Input Length: %d, Output Length: %d, Variable Index Offset: 0x%x\n", Sts.InputLength, Sts.OutputLength, Sts.VariableIndexOffset);
RtPrintf("EtherCAT State: %d, Cycle Time: %d\n", Sts.State, Sts.CycleTime);
RtPrintf("Resolution: %d\n", Resolution);
RtPrintf("\n");
}
VOID PrintAxisInformation(int Index)
{
BOOL HomeAbsSwitch = FALSE;
BOOL LimitSwitchPos = FALSE;
BOOL LimitSwitchNeg = FALSE;
BOOL Simulation = FALSE;
BOOL CommunicationReady = FALSE;
BOOL ReadyForPowerOn = FALSE;
BOOL PowerOn = FALSE;
BOOL IsHomed = FALSE;
BOOL AxisWarning = FALSE;
KsError nRet = errNoError;
nRet = GetAxisInfo(Index, &HomeAbsSwitch, &LimitSwitchPos, &LimitSwitchNeg, &Simulation, &CommunicationReady, &ReadyForPowerOn, &PowerOn, &IsHomed, &AxisWarning);
if (nRet != errNoError)
RtPrintf("GetAxisInfo failed: %x\n", nRet);
RtPrintf("Switches:\n");
RtPrintf("Home Switch: %d, Positive Limit Switch: %d, Negative Limit Switch: %d\n\n", HomeAbsSwitch, LimitSwitchPos, LimitSwitchNeg);
RtPrintf("States:\n");
RtPrintf("States: Simulation: %d, Communication Ready: %d, Ready for Power On: %d, Power On: %d, Is Homed: %d, Axis Warning: %d\n",
Simulation, CommunicationReady, ReadyForPowerOn, PowerOn, IsHomed, AxisWarning);
RtPrintf("\n");
}
VOID PrintMotionStates(int Index)
{
AxisState axisState = axisOffline;
KsError nRet = errNoError;
McMotionState motionState = mcHalted;
McDirection axisDirection = mcPositiveDirection;
string axisStates[10] = { "axisOffline", "axisCommunicationError", "axisMotionError", "axisDisabled", "axisLocked", "axisStandStill",
"axisHoming", "axisDiscreteMotion", "axisContinuousMotion", "axisSynchronizedMotion" };
string motionStates[4] = { "mcConstantVelocity", "mcAccelerating", "mcDecelrating", "mcHalted" };
string axisDirections[4] = { "mcPositiveDirection", "mcShortestWay", "mcNegativeDirection", "mcCurrentDirection" };
//Get the state of an axis.
nRet = GetAxisState(Index, &axisState);
if (nRet != errNoError)
RtPrintf("GetAxisState failed: %x\n", nRet);
//Reset nRet to errNoError if it has received an error.
nRet = errNoError;
//Get the motion state of an axis.
nRet = GetAxisMotionState(Index, &motionState, &axisDirection);
if (nRet != errNoError)
RtPrintf("GetAxisState failed: %x\n", nRet);
/*Use c_str() to convert a string object to a C_style string.
You can see the states in text instead of seeing them in number.*/
for (int i = 0; i < 10; i++)
{
if (axisState == i)
RtPrintf("Axis %d state: %s\n", Index, axisStates[i].c_str());
if (motionState == i)
RtPrintf("Axis %d motion state: %s\n", Index, motionStates[i].c_str());
if (axisDirection == i)
RtPrintf("Axis %d direction: %s\n", Index, axisDirections[i].c_str());
}
RtPrintf("\n");
}
VOID GetAvailableControlMode(int Index)
{
RtPrintf("Get the available control modes.\n");
int Modes = 0;
char trueFalse[] = "false";
KsError nRet = errNoError;
//Use a string array to store the list of control modes.
string controlMode[10] = { "modeManual", "modeDirectPos", "modeDirectVel",
"modeDirectTor", "modePidVel", "modePidTor", "modeMasterIntPos",
"modeMasterIntVel", "modeMasterIntTor", "modeSlaveInt" };
//Get the available control modes from an axis.
nRet = GetAxisAvailableControlModes(Index, &Modes);
/*Print the control modes in decimal format. The output is 67. The bits of Modes are 01000011.
Since we use accessPos in SystemInitialization.h, modeManual, modeDirectPos, and modeMasterIntPos are true.
modeSlaveint is false because it is not supported yet.
The output number varies between access modes.*/
RtPrintf("Control Modes: %d\n", Modes);
/*KINGSTAR offers 10 control modes.
To check what mode available to the axis, we use bitwise shift.*/
for (int i = 0; i < 10; i++)
{
//Reset trueFalse to "false."
strcpy(trueFalse, "false");
if (Modes & (1 << i))
strcpy(trueFalse, "true"); //Set trueFalse to "true."
//Print the control modes with flags.
/*Use c_str() to convert a string object to a C-style string.
You can see the available control modes in text instead of seeing them in number.*/
RtPrintf("Control mode: %s is %s\n", controlMode[i].c_str(), trueFalse);
}
RtPrintf("\n");
}
VOID SetControlMode(int Index)
{
RtPrintf("Set a control mode.\n");
string controlMode[10] = { "modeManual", "modeDirectPos", "modeDirectVel",
"modeDirectTor", "modePidVel", "modePidTor", "modeMasterIntPos",
"modeMasterIntVel", "modeMasterIntTor", "modeSlaveInt" };
//Set the mode depending on your access mode and needs.
McControlMode Mode = modeMasterIntPos;
KsCommandStatus status = WaitForCommand(5, TRUE, SetAxisControlMode(Index, Mode));
if (status.Error)
RtPrintf("SetAxisControlMode failed: %d\n", status.ErrorId);
else if (status.Done)
{
for (int i = 0; i < 10; i++)
{
if (Mode == i)
{
RtPrintf("The current control mode: %s\n\n", controlMode[i].c_str());
break;
}
}
}
}
VOID MotionProfile(int Index)
{
RtPrintf("Create a motion profile.\n");
McProfileSettings Motion =
{
MINIMUM_ERROR, //MinimumFollowingError
MAXIMUM_ERROR, //MaximumFollwingError
MAXIMUM_VELOCITY, //MaximumVelocity
MAXIMUM_ACCELERATION, //Acceleration
MAXIMUM_DECELERATION, //Deceleration
MAXIMUM_JERK, //Jerk
MAXIMUM_JOLT //Jolt
};
//MotionProfileType: profileUnitPerSecond.
KsError nRet = SetAxisMotionProfile(Index, MOTION_PROFILE_TYPE, Motion);
if (nRet != errNoError)
RtPrintf("SetAxisMotionProfile failed: %x\n", nRet);
RtPrintf("\n");
}
VOID PositionUnitConversion(int Index)
{
RtPrintf("Convert the unit to degrees.\n");
int Resolution = 0;
KsError nRet = errNoError;
SlaveStatus Sts = { 0 };
//Get the information from an axis.
nRet = GetAxisByIndex(Index, &Sts, &Resolution, NULL, NULL);
if (nRet != errNoError)
RtPrintf("GetAxisByIndex failed: %x\n", nRet);
//Set a conversion ratio.
nRet = SetAxisCountsPerUnit(Index, Resolution, UNITS_PER_REVOLUTION, FALSE);
if (nRet != errNoError)
RtPrintf("SetAxisCountsPerUnit failed: %x\n", nRet);
//Enable the unit conversion.
nRet = EnableAxisUnitConversion(Index, TRUE);
if (nRet != errNoError)
RtPrintf("EnableAxisUnitConversion failed: %x\n", nRet);
RtPrintf("\n");
}
VOID SoftLimitSwitch(int Index)
{
RtPrintf("Set software limit switches.\n");
//Enable positive software limit switch.
KsCommandStatus status = SetAxisParameter(Index, mcEnableLimitPositive, ENABLE_SOFTLIMIT_POSITIVE, mcImmediately);
if (status.Error)
RtPrintf("Failed to set mcEnableLimitPositive: %d\n", status.ErrorId);
//Enable negative software limit switch.
status = SetAxisParameter(Index, mcEnableLimitNegative, ENABLE_SOFTLIMIT_NEGATIVE, mcImmediately);
if (status.Error)
RtPrintf("Failed to set mcEnableLimitNegative: %d\n", status.ErrorId);
//Set the positive switch's position to 15000.
status = SetAxisParameter(Index, mcSoftLimitPositive, SOFT_LIMIT_POSITIVE, mcImmediately);
if (status.Error)
RtPrintf("Failed to set mcSoftLimitPositive: %d\n", status.ErrorId);
//Set the negative switch's position to -15000.
status = SetAxisParameter(Index, mcSoftLimitNegative, SOFT_LIMIT_NEGATIVE, mcImmediately);
if (status.Error)
RtPrintf("Failed to set mcSoftLimitNegative: %d\n", status.ErrorId);
RtPrintf("\n");
}
VOID LimitSensor(int Index)
{
RtPrintf("Set limit sensors.\n");
KsError nRet = SetAxisPositiveLimitSwitch
(
Index, //The index of an axis.
MAX_SENSOR_TYPE, //TRUE: axis. FALSE: I/O module.
MAX_SENSOR_INDEX, //Which axis or I/O module contains the sensor.
MAX_SENSOR_OFFSET, //The offset in bit of the sensor digital input in the module input variables.
MAX_SENSOR_INVERT, //Invert the sensor value.
MAX_SENSOR_ENABLE //TRUE: enable a positive limit switch. FALSE: disable a positive limit switch.
);
if (nRet != errNoError)
RtPrintf("SetAxisPositiveLimitSwitch failed: %x\n", nRet);
nRet = SetAxisNegativeLimitSwitch
(
Index, //The index of an axis.
MIN_SENSOR_TYPE, //TRUE: axis. FALSE: I/O module.
MIN_SENSOR_INDEX, //Which axis or I/O module contains the sensor.
MIN_SENSOR_OFFSET, //The offset in bit of the sensor digital input in the module input variables.
MIN_SENSOR_INVERT, //Invert the sensor value.
MIN_SENSOR_ENABLE //TRUE: enable a negative limit switch. FALSE: disable a megative limit switch.
);
if (nRet != errNoError)
RtPrintf("SetAxisNegativeLimitSwitch failed: %x\n", nRet);
RtPrintf("\n");
}
VOID LimitDirection(int Index)
{
RtPrintf("Limit the direction of an axis.\n");
//Reset an axis.
KsCommandStatus status = ResetAxis(Index);
if (status.Error)
RtPrintf("ResetAxis failed: %d\n", status.ErrorId);
Sleep(30);
//Enable an axis and only allow it to move in negative direction.
PowerAxis(
Index, //The index of an axis.
TRUE, //TRUE to enable the axis, FALSE to disable the axis.
FALSE, //Forbid the axis to move in positive direction.
TRUE //Allow the axis to move in negative direction.
);
if (status.Error)
RtPrintf("PowerAxis failed: %d\n", status.ErrorId);
Sleep(30);
RtPrintf("\n");
}
VOID UpdatePID(int Index)
{
RtPrintf("PID tuning.\n");
KsCommandStatus status = { 0 };
KsError nRet = errNoError;
McPidSettings myPid =
{
KP, //Proportional gain.
KI, //Integral gain.
KI_LIMIT_PERCENT, //The maximum integral error in percent of the maximum output.
KD, //Derivative gain.
KV, //Velocity feedforward gain.
KAA, //Acceleration feedforward gain.
KAD, //Deceleration feedforward gain.
KJ, //Jerk feedforward gain.
REDUCED_GAIN_DELAY, //Determine when the axis starts using RgFactor after finishing the move. Unit: second.
REDUCED_GAIN_FACTOR, //KP, KI, and KD multiplied by this factor after ReducedGainsDelay.
KI_STOPPED_ONLY, /*Use KI gain only after the move is done, in case you want to avoid KI during the move
to avoid overshooting but have it after to remove the steady-state error.
By default, it's TRUE.*/
KD_USE_INTERNAL_ENCODER, //Use the internal position encoder for KD. By default, it's FALSE.
MINIMUM_OUTPUT, //The minimum output of the PID. The minimum torque in % or minimum velocity.
MAXIMUM_OUTPUT //The maximum output of the PID. The maximum torque in % or maximum velocity.
};
//Apply the PID settings to an axis.
nRet = SetAxisVelocityPid(Index, myPid);
if (nRet != errNoError)
RtPrintf("SetAxisVelocityPid failed: %x\n", nRet);
//Set FeedBackDelay to 3 cycles.
status = SetAxisParameter(Index, mcFeedbackDelay, FEEDBACK_DELAY, mcImmediately);
if (status.Error)
RtPrintf("Failed to set mcFeedbackDelay: %d\n", status.ErrorId);
RtPrintf("\n");
}