595 lines
19 KiB
C
595 lines
19 KiB
C
|
#include <string.h>
|
||
|
|
#include "h03_di_do.h"
|
||
|
|
|
||
|
|
|
||
|
|
/* Complete mapping table with function descriptions */
|
||
|
|
static const funin_map_entry_t funin_map1[] = {
|
||
|
|
{ SV630P_FunIN_1_S_ON, SV630P_DIFUNC1_FUNIN_1, "Servo enable"},
|
||
|
|
{ SV630P_FunIN_2_ALM_RST, SV630P_DIFUNC1_FUNIN_2, "Fault and warning reset"},
|
||
|
|
{SV630P_FunIN_3_GAIN_SEL, SV630P_DIFUNC1_FUNIN_3, "Gain selection"},
|
||
|
|
{ SV630P_FunIN_4_CMD_SEL, SV630P_DIFUNC1_FUNIN_4, "Main spindle command selection"},
|
||
|
|
{ SV630P_FunIN_5_DIR_SEL, SV630P_DIFUNC1_FUNIN_5, "Multi-speed DI direction selection"},
|
||
|
|
{ SV630P_FunIN_6_CMD1, SV630P_DIFUNC1_FUNIN_6, "Multi-step command selection 1"},
|
||
|
|
{ SV630P_FunIN_7_CMD2, SV630P_DIFUNC1_FUNIN_7, "Multi-step command selection 2"},
|
||
|
|
{ SV630P_FunIN_8_CMD3, SV630P_DIFUNC1_FUNIN_8, "Multi-step command selection 3"},
|
||
|
|
{ SV630P_FunIN_9_CMD4, SV630P_DIFUNC1_FUNIN_9, "Multi-step command selection 4"},
|
||
|
|
{ SV630P_FunIN_10_M1_SEL, SV630P_DIFUNC1_FUNIN_10, "Mode selection 1"},
|
||
|
|
{ SV630P_FunIN_11_M2_SEL, SV630P_DIFUNC1_FUNIN_11, "Mode selection 2"},
|
||
|
|
{ SV630P_FunIN_12_ZCLAMP, SV630P_DIFUNC1_FUNIN_12, "Zero clamp enable"},
|
||
|
|
{SV630P_FunIN_13_INHIBIT, SV630P_DIFUNC1_FUNIN_13, "Position command inhibit"},
|
||
|
|
{ SV630P_FunIN_14_P_OT, SV630P_DIFUNC1_FUNIN_14, "Positive overtravel switch"},
|
||
|
|
{ SV630P_FunIN_15_N_OT, SV630P_DIFUNC1_FUNIN_15, "Negative overtravel switch"},
|
||
|
|
};
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Find mask by DI function enum (FunIN)
|
||
|
|
* @param funin DI function enum
|
||
|
|
* @param[out] mask Output mask value
|
||
|
|
* @return 0 if found, -1 otherwise
|
||
|
|
*/
|
||
|
|
int find_mask1_by_funin(SV630P_DIDO_FunIN funin, SV630P_FuncTypeMask1 *mask)
|
||
|
|
{
|
||
|
|
for (uint8_t i = 0; i < FUNIN_MAP_SIZE(funin_map1); i++)
|
||
|
|
{
|
||
|
|
if (funin_map1[i].funin == funin)
|
||
|
|
{
|
||
|
|
*mask = funin_map1[i].mask;
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return -1;
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Find index in mapping table by mask
|
||
|
|
* @param mask Mask value
|
||
|
|
* @return Index if found, -1 otherwise
|
||
|
|
*/
|
||
|
|
int find_index_by_mask1(SV630P_FuncTypeMask1 mask)
|
||
|
|
{
|
||
|
|
for (uint8_t i = 0; i < FUNIN_MAP_SIZE(funin_map1); i++)
|
||
|
|
{
|
||
|
|
if (funin_map1[i].mask == mask)
|
||
|
|
{
|
||
|
|
return i;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return -1;
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Find function description by mask
|
||
|
|
* @param mask Mask value
|
||
|
|
* @return Description string if found, NULL otherwise
|
||
|
|
*/
|
||
|
|
char *find_funin_desc_by_mask1(SV630P_FuncTypeMask1 mask)
|
||
|
|
{
|
||
|
|
for (uint8_t i = 0; i < FUNIN_MAP_SIZE(funin_map1); i++)
|
||
|
|
{
|
||
|
|
if (funin_map1[i].mask == mask)
|
||
|
|
{
|
||
|
|
return (char *)funin_map1[i].desc;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return NULL;
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Read power-on DI function assignment 1
|
||
|
|
* @param dev Pointer to servo handle
|
||
|
|
* @param funin Output DI function enum
|
||
|
|
* @return SV630P_OK on success, error code otherwise
|
||
|
|
*/
|
||
|
|
int sv630p_h03_00_read_power_on_di_func_assign1(sv630p_handle_t *dev, SV630P_DIDO_FunIN *funin)
|
||
|
|
{
|
||
|
|
uint16_t value = 0;
|
||
|
|
int index = -1;
|
||
|
|
|
||
|
|
SV630P_ARGS_CHECK(dev);
|
||
|
|
if (dev->ops->read_holding_regs(dev, REG_H03_00_POWER_ON_DI1, 1, &value) == SV630P_OK)
|
||
|
|
{
|
||
|
|
index = find_index_by_mask1(value);
|
||
|
|
if (index >= 0)
|
||
|
|
{
|
||
|
|
SV630_DEBUG(dev, "DI function: %s\n", funin_map1[index].desc);
|
||
|
|
*funin = funin_map1[index].funin;
|
||
|
|
return SV630P_OK;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
SV630_DEBUG(dev, "DI function: %s\n", "Unknown");
|
||
|
|
return SV630P_INDEX_ERR;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return SV630P_BUS_ERR;
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Write power-on DI function assignment 1
|
||
|
|
* @param dev Pointer to servo handle
|
||
|
|
* @param func DI function enum to write
|
||
|
|
* @return SV630P_OK on success, error code otherwise
|
||
|
|
*/
|
||
|
|
int sv630p_h03_00_write_power_on_di_func_assign1(sv630p_handle_t *dev, SV630P_DIDO_FunIN func)
|
||
|
|
{
|
||
|
|
SV630P_FuncTypeMask1 mask;
|
||
|
|
SV630P_ARGS_CHECK(dev);
|
||
|
|
|
||
|
|
if (find_mask1_by_funin(func, &mask) == 0)
|
||
|
|
return dev->ops->write_single_reg(dev, REG_H03_00_POWER_ON_DI1, mask);
|
||
|
|
else
|
||
|
|
return SV630P_INDEX_ERR;
|
||
|
|
}
|
||
|
|
|
||
|
|
// ====== SV630P_FuncTypeMask2 mapping table and function ======
|
||
|
|
static const funin_map_entry_t funin_map2[] = {
|
||
|
|
{ SV630P_FunIN_17_N_CL, SV630P_DIFUNC2_FUNIN_17, "Negative external torque limit"},
|
||
|
|
{ SV630P_FunIN_18_JOGCMD_PLUS, SV630P_DIFUNC2_FUNIN_18, "Jog command +"},
|
||
|
|
{SV630P_FunIN_19_JOGCMD_MINUS, SV630P_DIFUNC2_FUNIN_19, "Jog command -"},
|
||
|
|
{ SV630P_FunIN_20_POSSTEP, SV630P_DIFUNC2_FUNIN_20, "Step enable"},
|
||
|
|
{ SV630P_FunIN_21_HX1, SV630P_DIFUNC2_FUNIN_21, "Handwheel ratio signal 1"},
|
||
|
|
{ SV630P_FunIN_22_HX2, SV630P_DIFUNC2_FUNIN_22, "Handwheel ratio signal 2"},
|
||
|
|
{ SV630P_FunIN_23_HX_EN, SV630P_DIFUNC2_FUNIN_23, "Handwheel enable signal"},
|
||
|
|
{ SV630P_FunIN_24_GEAR_SEL, SV630P_DIFUNC2_FUNIN_24, "Electronic gear select"},
|
||
|
|
{ SV630P_FunIN_25_TOQDirSel, SV630P_DIFUNC2_FUNIN_25, "Torque command direction select"},
|
||
|
|
{ SV630P_FunIN_26_SPDDirSel, SV630P_DIFUNC2_FUNIN_26, "Speed command direction select"},
|
||
|
|
{ SV630P_FunIN_27_POSDirSel, SV630P_DIFUNC2_FUNIN_27, "Position command direction select"},
|
||
|
|
{ SV630P_FunIN_28_PosinSen, SV630P_DIFUNC2_FUNIN_28, "Multi-position command enable"},
|
||
|
|
{ SV630P_FunIN_29_XintFree, SV630P_DIFUNC2_FUNIN_29, "Interrupt fixed length state release"},
|
||
|
|
// SV630P_FunIN_30_xxx, // If you have FunIN.30, add here
|
||
|
|
{ SV630P_FunIN_31_HomeSwitch, SV630P_DIFUNC2_FUNIN_31, "Home switch"},
|
||
|
|
{ SV630P_FunIN_32_HomingStart, SV630P_DIFUNC2_FUNIN_32, "Homing enable"},
|
||
|
|
};
|
||
|
|
|
||
|
|
#define FUNIN_MAP2_SIZE(x) (sizeof(x) / sizeof(x[0]))
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Find mask by DI function enum (FunIN) for group 2
|
||
|
|
* @param funin DI function enum
|
||
|
|
* @param[out] mask Output mask value
|
||
|
|
* @return 0 if found, -1 otherwise
|
||
|
|
*/
|
||
|
|
int find_mask2_by_funin(SV630P_DIDO_FunIN funin, SV630P_FuncTypeMask2 *mask)
|
||
|
|
{
|
||
|
|
for (uint8_t i = 0; i < FUNIN_MAP2_SIZE(funin_map2); i++)
|
||
|
|
{
|
||
|
|
if (funin_map2[i].funin == funin)
|
||
|
|
{
|
||
|
|
*mask = (SV630P_FuncTypeMask2)funin_map2[i].mask;
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return -1;
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Find index in mapping table by mask for group 2
|
||
|
|
* @param mask Mask value
|
||
|
|
* @return Index if found, -1 otherwise
|
||
|
|
*/
|
||
|
|
int find_index_by_mask2(SV630P_FuncTypeMask2 mask)
|
||
|
|
{
|
||
|
|
for (uint8_t i = 0; i < FUNIN_MAP2_SIZE(funin_map2); i++)
|
||
|
|
{
|
||
|
|
if ((SV630P_FuncTypeMask2)funin_map2[i].mask == mask)
|
||
|
|
{
|
||
|
|
return i;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return -1;
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Find function description by mask for group 2
|
||
|
|
* @param mask Mask value
|
||
|
|
* @return Description string if found, NULL otherwise
|
||
|
|
*/
|
||
|
|
char *find_funin_desc_by_mask2(SV630P_FuncTypeMask2 mask)
|
||
|
|
{
|
||
|
|
for (uint8_t i = 0; i < FUNIN_MAP2_SIZE(funin_map2); i++)
|
||
|
|
{
|
||
|
|
if ((SV630P_FuncTypeMask2)funin_map2[i].mask == mask)
|
||
|
|
{
|
||
|
|
return (char *)funin_map2[i].desc;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return NULL;
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Read power-on DI function assignment 2
|
||
|
|
* @param dev Pointer to servo handle
|
||
|
|
* @param funin Output DI function enum
|
||
|
|
* @return SV630P_OK on success, error code otherwise
|
||
|
|
*/
|
||
|
|
int sv630p_h03_01_read_power_on_di_func_assign2(sv630p_handle_t *dev, SV630P_DIDO_FunIN *funin)
|
||
|
|
{
|
||
|
|
uint16_t value = 0;
|
||
|
|
int index = -1;
|
||
|
|
SV630P_ARGS_CHECK(dev);
|
||
|
|
|
||
|
|
if (dev->ops->read_holding_regs(dev, REG_H03_01_POWER_ON_DI2, 1, &value) == SV630P_OK)
|
||
|
|
{
|
||
|
|
index = find_index_by_mask2(value);
|
||
|
|
if (index >= 0)
|
||
|
|
{
|
||
|
|
SV630_DEBUG(dev, "DI function: %s\n", funin_map2[index].desc);
|
||
|
|
*funin = funin_map2[index].funin;
|
||
|
|
return SV630P_OK;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
SV630_DEBUG(dev, "DI function: %s\n", "Unknown");
|
||
|
|
return SV630P_INDEX_ERR;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return SV630P_BUS_ERR;
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Write power-on DI function assignment 2, 设置某一DI功能(FunIN.17~FunIN.32)重新上电后立即有效
|
||
|
|
* @param dev Pointer to servo handle
|
||
|
|
* @param func DI function enum to write
|
||
|
|
* @return SV630P_OK on success, error code otherwise
|
||
|
|
* @description: 再上电生效, 实时更改
|
||
|
|
*/
|
||
|
|
int sv630p_h03_01_write_power_on_di_func_assign2(sv630p_handle_t *dev, SV630P_DIDO_FunIN func)
|
||
|
|
{
|
||
|
|
SV630P_FuncTypeMask2 mask;
|
||
|
|
SV630P_ARGS_CHECK(dev);
|
||
|
|
|
||
|
|
if (find_mask2_by_funin(func, &mask) == 0)
|
||
|
|
return dev->ops->write_single_reg(dev, REG_H03_01_POWER_ON_DI2, mask);
|
||
|
|
else
|
||
|
|
return SV630P_INDEX_ERR;
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief DI1端子功能选择
|
||
|
|
* @param dev
|
||
|
|
* @param type
|
||
|
|
* @return
|
||
|
|
* note:实时生效
|
||
|
|
*/
|
||
|
|
int sv630p_h03_02_read_di1_function_selection(sv630p_handle_t *dev, DiFunctionType *type)
|
||
|
|
{
|
||
|
|
SV630P_ARGS_CHECK(dev);
|
||
|
|
return dev->ops->read_holding_regs(dev, REG_H03_02_DI1_FUNC, 1, (uint16_t *)type);
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief DI1端子功能选择
|
||
|
|
* @param dev
|
||
|
|
* @param type - 设置硬件DI1端子对应的DI功能
|
||
|
|
*/
|
||
|
|
int sv630p_h03_02_write_di1_function_selection(sv630p_handle_t *dev, DiFunctionType type)
|
||
|
|
{
|
||
|
|
SV630P_ARGS_CHECK(dev);
|
||
|
|
return dev->ops->write_single_reg(dev, REG_H03_02_DI1_FUNC, type);
|
||
|
|
}
|
||
|
|
|
||
|
|
int sv630p_h03_03_read_di1_logic(sv630p_handle_t *dev, DI_LogicType *logic)
|
||
|
|
{
|
||
|
|
SV630P_ARGS_CHECK(dev);
|
||
|
|
return dev->ops->read_holding_regs(dev, REG_H03_03_DI1_LOGIC, 1, (uint16_t *)logic);
|
||
|
|
}
|
||
|
|
|
||
|
|
int sv630p_h03_03_write_di1_logic(sv630p_handle_t *dev, DI_LogicType logic)
|
||
|
|
{
|
||
|
|
SV630P_ARGS_CHECK(dev);
|
||
|
|
return dev->ops->write_single_reg(dev, REG_H03_03_DI1_LOGIC, (uint16_t)logic);
|
||
|
|
}
|
||
|
|
|
||
|
|
int sv630p_h03_04_read_di2_function(sv630p_handle_t *dev, DiFunctionType *type)
|
||
|
|
{
|
||
|
|
SV630P_ARGS_CHECK(dev);
|
||
|
|
return dev->ops->read_holding_regs(dev, REG_H03_04_DI2_FUNC, 1, (uint16_t *)type);
|
||
|
|
}
|
||
|
|
|
||
|
|
int sv630p_h03_04_write_di2_function(sv630p_handle_t *dev, DiFunctionType type)
|
||
|
|
{
|
||
|
|
SV630P_ARGS_CHECK(dev);
|
||
|
|
return dev->ops->write_single_reg(dev, REG_H03_04_DI2_FUNC, type);
|
||
|
|
}
|
||
|
|
|
||
|
|
int sv630p_h03_05_read_di2_logic(sv630p_handle_t *dev, DI_LogicType *logic)
|
||
|
|
{
|
||
|
|
SV630P_ARGS_CHECK(dev);
|
||
|
|
return dev->ops->read_holding_regs(dev, REG_H03_05_DI2_LOGIC, 1, (uint16_t *)logic);
|
||
|
|
}
|
||
|
|
|
||
|
|
int sv630p_h03_05_write_di2_logic(sv630p_handle_t *dev, DI_LogicType logic)
|
||
|
|
{
|
||
|
|
SV630P_ARGS_CHECK(dev);
|
||
|
|
return dev->ops->write_single_reg(dev, REG_H03_05_DI2_LOGIC, (uint16_t)logic);
|
||
|
|
}
|
||
|
|
|
||
|
|
int sv630p_h03_06_read_di3_function(sv630p_handle_t *dev, DiFunctionType *type)
|
||
|
|
{
|
||
|
|
SV630P_ARGS_CHECK(dev);
|
||
|
|
return dev->ops->read_holding_regs(dev, REG_H03_06_DI3_FUNC, 1, (uint16_t *)type);
|
||
|
|
}
|
||
|
|
|
||
|
|
int sv630p_h03_06_write_di3_function(sv630p_handle_t *dev, DiFunctionType type)
|
||
|
|
{
|
||
|
|
SV630P_ARGS_CHECK(dev);
|
||
|
|
return dev->ops->write_single_reg(dev, REG_H03_06_DI3_FUNC, type);
|
||
|
|
}
|
||
|
|
|
||
|
|
int sv630p_h03_07_read_di3_logic(sv630p_handle_t *dev, DI_LogicType *logic)
|
||
|
|
{
|
||
|
|
SV630P_ARGS_CHECK(dev);
|
||
|
|
return dev->ops->read_holding_regs(dev, REG_H03_07_DI3_LOGIC, 1, (uint16_t *)logic);
|
||
|
|
}
|
||
|
|
|
||
|
|
int sv630p_h03_07_write_di3_logic(sv630p_handle_t *dev, DI_LogicType logic)
|
||
|
|
{
|
||
|
|
SV630P_ARGS_CHECK(dev);
|
||
|
|
return dev->ops->write_single_reg(dev, REG_H03_07_DI3_LOGIC, (uint16_t)logic);
|
||
|
|
}
|
||
|
|
|
||
|
|
int sv630p_h03_08_read_di4_function(sv630p_handle_t *dev, DiFunctionType *type)
|
||
|
|
{
|
||
|
|
SV630P_ARGS_CHECK(dev);
|
||
|
|
return dev->ops->read_holding_regs(dev, REG_H03_08_DI4_FUNC, 1, (uint16_t *)type);
|
||
|
|
}
|
||
|
|
|
||
|
|
int sv630p_h03_08_write_di4_function(sv630p_handle_t *dev, DiFunctionType type)
|
||
|
|
{
|
||
|
|
SV630P_ARGS_CHECK(dev);
|
||
|
|
return dev->ops->write_single_reg(dev, REG_H03_08_DI4_FUNC, type);
|
||
|
|
}
|
||
|
|
|
||
|
|
int sv630p_h03_09_read_di4_logic(sv630p_handle_t *dev, DI_LogicType *logic)
|
||
|
|
{
|
||
|
|
SV630P_ARGS_CHECK(dev);
|
||
|
|
return dev->ops->read_holding_regs(dev, REG_H03_09_DI4_LOGIC, 1, (uint16_t *)logic);
|
||
|
|
}
|
||
|
|
|
||
|
|
int sv630p_h03_09_write_di4_logic(sv630p_handle_t *dev, DI_LogicType logic)
|
||
|
|
{
|
||
|
|
SV630P_ARGS_CHECK(dev);
|
||
|
|
return dev->ops->write_single_reg(dev, REG_H03_09_DI4_LOGIC, (uint16_t)logic);
|
||
|
|
}
|
||
|
|
|
||
|
|
int sv630p_h03_10_read_di5_function(sv630p_handle_t *dev, DiFunctionType *type)
|
||
|
|
{
|
||
|
|
SV630P_ARGS_CHECK(dev);
|
||
|
|
return dev->ops->read_holding_regs(dev, REG_H03_10_DI5_FUNC, 1, (uint16_t *)type);
|
||
|
|
}
|
||
|
|
|
||
|
|
int sv630p_h03_10_write_di5_function(sv630p_handle_t *dev, DiFunctionType type)
|
||
|
|
{
|
||
|
|
SV630P_ARGS_CHECK(dev);
|
||
|
|
return dev->ops->write_single_reg(dev, REG_H03_10_DI5_FUNC, type);
|
||
|
|
}
|
||
|
|
|
||
|
|
int sv630p_h03_11_read_di5_logic(sv630p_handle_t *dev, DI_LogicType *logic)
|
||
|
|
{
|
||
|
|
SV630P_ARGS_CHECK(dev);
|
||
|
|
return dev->ops->read_holding_regs(dev, REG_H03_11_DI5_LOGIC, 1, (uint16_t *)logic);
|
||
|
|
}
|
||
|
|
|
||
|
|
int sv630p_h03_11_write_di5_logic(sv630p_handle_t *dev, DI_LogicType logic)
|
||
|
|
{
|
||
|
|
SV630P_ARGS_CHECK(dev);
|
||
|
|
return dev->ops->write_single_reg(dev, REG_H03_11_DI5_LOGIC, (uint16_t)logic);
|
||
|
|
}
|
||
|
|
|
||
|
|
int sv630p_h03_16_read_di8_function(sv630p_handle_t *dev, DiFunctionType *type)
|
||
|
|
{
|
||
|
|
SV630P_ARGS_CHECK(dev);
|
||
|
|
return dev->ops->read_holding_regs(dev, REG_H03_16_DI8_FUNC, 1, (uint16_t *)type);
|
||
|
|
}
|
||
|
|
|
||
|
|
int sv630p_h03_16_write_di8_function(sv630p_handle_t *dev, DiFunctionType type)
|
||
|
|
{
|
||
|
|
SV630P_ARGS_CHECK(dev);
|
||
|
|
return dev->ops->write_single_reg(dev, REG_H03_16_DI8_FUNC, type);
|
||
|
|
}
|
||
|
|
|
||
|
|
int sv630p_h03_17_read_di8_logic(sv630p_handle_t *dev, DI_LogicType *logic)
|
||
|
|
{
|
||
|
|
SV630P_ARGS_CHECK(dev);
|
||
|
|
return dev->ops->read_holding_regs(dev, REG_H03_17_DI8_LOGIC, 1, (uint16_t *)logic);
|
||
|
|
}
|
||
|
|
|
||
|
|
int sv630p_h03_17_write_di8_logic(sv630p_handle_t *dev, DI_LogicType logic)
|
||
|
|
{
|
||
|
|
SV630P_ARGS_CHECK(dev);
|
||
|
|
return dev->ops->write_single_reg(dev, REG_H03_17_DI8_LOGIC, (uint16_t)logic);
|
||
|
|
}
|
||
|
|
|
||
|
|
int sv630p_h03_18_read_di9_function(sv630p_handle_t *dev, DiFunctionType *type)
|
||
|
|
{
|
||
|
|
SV630P_ARGS_CHECK(dev);
|
||
|
|
return dev->ops->read_holding_regs(dev, REG_H03_18_DI9_FUNC, 1, (uint16_t *)type);
|
||
|
|
}
|
||
|
|
|
||
|
|
int sv630p_h03_18_write_di9_function(sv630p_handle_t *dev, DiFunctionType type)
|
||
|
|
{
|
||
|
|
SV630P_ARGS_CHECK(dev);
|
||
|
|
return dev->ops->write_single_reg(dev, REG_H03_18_DI9_FUNC, type);
|
||
|
|
}
|
||
|
|
|
||
|
|
int sv630p_h03_19_read_di9_logic(sv630p_handle_t *dev, DI_LogicType *logic)
|
||
|
|
{
|
||
|
|
SV630P_ARGS_CHECK(dev);
|
||
|
|
return dev->ops->read_holding_regs(dev, REG_H03_19_DI9_LOGIC, 1, (uint16_t *)logic);
|
||
|
|
}
|
||
|
|
|
||
|
|
int sv630p_h03_19_write_di9_logic(sv630p_handle_t *dev, DI_LogicType logic)
|
||
|
|
{
|
||
|
|
SV630P_ARGS_CHECK(dev);
|
||
|
|
return dev->ops->write_single_reg(dev, REG_H03_19_DI9_LOGIC, (uint16_t)logic);
|
||
|
|
}
|
||
|
|
|
||
|
|
// ====== SV630P_FuncTypeMask3 mapping table and function ======
|
||
|
|
static const funin_map_entry_t funin_map3[] = {
|
||
|
|
{ SV630P_FunIN_33_XintInhibit, SV630P_DIFUNC3_FUNIN_33, "Interrupt fixed length prohibit"},
|
||
|
|
{ SV630P_FunIN_34_EmergencyStop, SV630P_DIFUNC3_FUNIN_34, "Emergency stop"},
|
||
|
|
{ SV630P_FunIN_35_ClrPosErr, SV630P_DIFUNC3_FUNIN_35, "Clear position error"},
|
||
|
|
{ SV630P_FunIN_36_V_LmtSel, SV630P_DIFUNC3_FUNIN_36, "Internal speed limit source"},
|
||
|
|
{ SV630P_FunIN_37_PulseInhibit, SV630P_DIFUNC3_FUNIN_37, "Pulse command inhibit"},
|
||
|
|
{ SV630P_FunIN_38_MultiBlockTrig, SV630P_DIFUNC3_FUNIN_38, "Multi-block trigger"},
|
||
|
|
{ SV630P_FunIN_39_MultiBlockWr, SV630P_DIFUNC3_FUNIN_39, "Multi-block write"},
|
||
|
|
{SV630P_FunIN_40_ClrCmdOkAndArrOk, SV630P_DIFUNC3_FUNIN_40, "Clear command and position complete"},
|
||
|
|
{ SV630P_FunIN_41_HomeRecord, SV630P_DIFUNC3_FUNIN_41, "Set current position as home"},
|
||
|
|
// Add more if FunIN.42~FunIN.47 are defined in your SV630P_DIDO_FunIN
|
||
|
|
};
|
||
|
|
|
||
|
|
#define FUNIN_MAP3_SIZE(x) (sizeof(x) / sizeof(x[0]))
|
||
|
|
|
||
|
|
int find_mask3_by_funin(SV630P_DIDO_FunIN funin, SV630P_FuncTypeMask3 *mask)
|
||
|
|
{
|
||
|
|
for (uint8_t i = 0; i < FUNIN_MAP3_SIZE(funin_map3); i++)
|
||
|
|
{
|
||
|
|
if (funin_map3[i].funin == funin)
|
||
|
|
{
|
||
|
|
*mask = (SV630P_FuncTypeMask3)funin_map3[i].mask;
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return -1;
|
||
|
|
}
|
||
|
|
|
||
|
|
int find_index_by_mask3(SV630P_FuncTypeMask3 mask)
|
||
|
|
{
|
||
|
|
for (uint8_t i = 0; i < FUNIN_MAP3_SIZE(funin_map3); i++)
|
||
|
|
{
|
||
|
|
if ((SV630P_FuncTypeMask3)funin_map3[i].mask == mask)
|
||
|
|
{
|
||
|
|
return i;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return -1;
|
||
|
|
}
|
||
|
|
|
||
|
|
char *find_funin_desc_by_mask3(SV630P_FuncTypeMask3 mask)
|
||
|
|
{
|
||
|
|
for (uint8_t i = 0; i < FUNIN_MAP3_SIZE(funin_map3); i++)
|
||
|
|
{
|
||
|
|
if ((SV630P_FuncTypeMask3)funin_map3[i].mask == mask)
|
||
|
|
{
|
||
|
|
return (char *)funin_map3[i].desc;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return NULL;
|
||
|
|
}
|
||
|
|
|
||
|
|
int sv630p_h03_34_read_power_on_di_function(sv630p_handle_t *dev, SV630P_DIDO_FunIN *funin)
|
||
|
|
{
|
||
|
|
uint16_t value = 0;
|
||
|
|
int index = -1;
|
||
|
|
SV630P_ARGS_CHECK(dev);
|
||
|
|
if (dev->ops->read_holding_regs(dev, REG_H03_34_POWER_ON_DI3, 1, &value) == SV630P_OK)
|
||
|
|
{
|
||
|
|
index = find_index_by_mask3(value);
|
||
|
|
if (index >= 0)
|
||
|
|
{
|
||
|
|
SV630_DEBUG(dev, "DI function: %s\n", funin_map3[index].desc);
|
||
|
|
*funin = funin_map3[index].funin;
|
||
|
|
return SV630P_OK;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
SV630_DEBUG(dev, "DI function: %s\n", "Unknown");
|
||
|
|
return SV630P_INDEX_ERR;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return SV630P_BUS_ERR;
|
||
|
|
}
|
||
|
|
|
||
|
|
int sv630p_h03_34_write_power_on_di_function(sv630p_handle_t *dev, SV630P_DIDO_FunIN func)
|
||
|
|
{
|
||
|
|
SV630P_FuncTypeMask3 mask;
|
||
|
|
SV630P_ARGS_CHECK(dev);
|
||
|
|
|
||
|
|
if (find_mask3_by_funin(func, &mask) == 0)
|
||
|
|
return dev->ops->write_single_reg(dev, REG_H03_34_POWER_ON_DI3, mask);
|
||
|
|
else
|
||
|
|
return SV630P_INDEX_ERR;
|
||
|
|
}
|
||
|
|
|
||
|
|
// ====== SV630P_FuncTypeMask4 mapping table and function ======
|
||
|
|
static const funin_map_entry_t funin_map4[] = {
|
||
|
|
// Add mapping for FunIN.49~FunIN.63 if defined in SV630P_DIDO_FunIN
|
||
|
|
// {SV630P_FunIN_49_xxx, SV630P_DIFUNC4_FUNIN_49, "FunIN.49 description"},
|
||
|
|
{SV630P_FunIN_33_XintInhibit, SV630P_DIFUNC3_FUNIN_33, "Interrupt fixed length prohibit"}, // for build error, not find define
|
||
|
|
};
|
||
|
|
|
||
|
|
#define FUNIN_MAP4_SIZE(x) (sizeof(x) / sizeof(x[0]))
|
||
|
|
|
||
|
|
int find_mask4_by_funin(SV630P_DIDO_FunIN funin, SV630P_FuncTypeMask4 *mask)
|
||
|
|
{
|
||
|
|
for (uint8_t i = 0; i < FUNIN_MAP4_SIZE(funin_map4); i++)
|
||
|
|
{
|
||
|
|
if (funin_map4[i].funin == funin)
|
||
|
|
{
|
||
|
|
*mask = (SV630P_FuncTypeMask4)funin_map4[i].mask;
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return -1;
|
||
|
|
}
|
||
|
|
|
||
|
|
int find_index_by_mask4(SV630P_FuncTypeMask4 mask)
|
||
|
|
{
|
||
|
|
for (uint8_t i = 0; i < FUNIN_MAP4_SIZE(funin_map4); i++)
|
||
|
|
{
|
||
|
|
if ((SV630P_FuncTypeMask4)funin_map4[i].mask == mask)
|
||
|
|
{
|
||
|
|
return i;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return -1;
|
||
|
|
}
|
||
|
|
|
||
|
|
char *find_funin_desc_by_mask4(SV630P_FuncTypeMask4 mask)
|
||
|
|
{
|
||
|
|
for (uint8_t i = 0; i < FUNIN_MAP4_SIZE(funin_map4); i++)
|
||
|
|
{
|
||
|
|
if ((SV630P_FuncTypeMask4)funin_map4[i].mask == mask)
|
||
|
|
{
|
||
|
|
return (char *)funin_map4[i].desc;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return NULL;
|
||
|
|
}
|
||
|
|
|
||
|
|
int sv630p_h03_35_read_power_on_di_function(sv630p_handle_t *dev, SV630P_DIDO_FunIN *funin)
|
||
|
|
{
|
||
|
|
uint16_t value = 0;
|
||
|
|
int index = -1;
|
||
|
|
SV630P_ARGS_CHECK(dev);
|
||
|
|
if (dev->ops->read_holding_regs(dev, REG_H03_35_POWER_ON_DI4, 1, &value) == SV630P_OK)
|
||
|
|
{
|
||
|
|
index = find_index_by_mask4(value);
|
||
|
|
if (index >= 0)
|
||
|
|
{
|
||
|
|
SV630_DEBUG(dev, "DI function: %s\n", funin_map4[index].desc);
|
||
|
|
*funin = funin_map4[index].funin;
|
||
|
|
return SV630P_OK;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
SV630_DEBUG(dev, "DI function: %s\n", "Unknown");
|
||
|
|
return SV630P_INDEX_ERR;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return SV630P_BUS_ERR;
|
||
|
|
}
|
||
|
|
|
||
|
|
int sv630p_h03_35_write_power_on_di_function(sv630p_handle_t *dev, SV630P_DIDO_FunIN func)
|
||
|
|
{
|
||
|
|
SV630P_FuncTypeMask4 mask;
|
||
|
|
SV630P_ARGS_CHECK(dev);
|
||
|
|
|
||
|
|
if (find_mask4_by_funin(func, &mask) == 0)
|
||
|
|
return dev->ops->write_single_reg(dev, REG_H03_35_POWER_ON_DI4, mask);
|
||
|
|
else
|
||
|
|
return SV630P_INDEX_ERR;
|
||
|
|
}
|
||
|
|
|
||
|
|
|