GC24001/User/axis_app.c

#include "board.h"
#include "io_app.h"
#include "mv_port.h"
#include "user_app.h"
#include "variable.h"
#include <string.h>

#define i_F_XOring      0
#define i_F_YOring      1
#define i_F_ZOring      2
#define i_F_UOring      3

#define X_Origin X00
#define Y_Origin X01

typedef struct
{
    unsigned short no;
    unsigned short speed;
    int position;
    int dst_position;
    unsigned short aac;
    unsigned short ddc;
    unsigned short max_acc;
    unsigned short max_dec;
    unsigned short min_speed;
    unsigned short max_speed;
    unsigned short time_uint;
    unsigned short speed_unit;
} motion_parameter_t;

/**
 * 轴初始化，包括参数初始化，回掉函数设置
 *
 * @author lxz
 *
 * @param void
 */
void axis_app_init(void)
{
   hw_io_infomation_t info;
//轴分配与初始化
#if AXIS_NUMBER > 0
    if (MotorParam->XCycleLength == 0)
        MotorParam->XCycleLength = 1000;
    if (MotorParam->XCyclePulse == 0)
        MotorParam->XCyclePulse = 10000;

    MV_AxisInit(X_AXIS, PHY_AXIS_1);
    MV_SetParam(X_AXIS, MV_PARAM_CLOCK, 6000000);
    MV_SetRatio(X_AXIS,
                MotorParam->XCyclePulse,
                MotorParam->XCycleLength, 1);
    MV_EnableRatio(X_AXIS);

    MV_SetParam(X_AXIS, MV_PARAM_HOME_SPACE, 50);
    MV_SetParam(X_AXIS, MV_PARAM_HOME_OFFSET, 0);

    MV_SetParam(X_AXIS, MV_PARAM_START_SPEED, 1000);
    MV_SetAccDec(X_AXIS, 1, 200, 200);
    MV_SetParam(X_AXIS, AXIS_HOME_LOW_SPEED, 8000);
    MV_SetParam(X_AXIS, AXIS_HOME_FIND_SPEED, 1000);

    MV_SetParam(X_AXIS, MV_PARAM_MIN_STEP, 1);
    MV_SetParam(X_AXIS, MV_PARAM_PERIOD, 1);
    MV_SetParam(X_AXIS, MV_PARAM_AAC_LMT, 5000);
    MV_SetParam(X_AXIS, MV_PARAM_ACC_LMT, 30000);
    MV_SetParam(X_AXIS, MV_PARAM_DEC_LMT, 30000);

    if (MotorParam->XDir > 0)
    {
        MotorParam->XDir = 1;
    }
    MV_SetParam(X_AXIS,
                MV_PARAM_DIR_LEVEL,
                MotorParam->XDir);

    if (hw_io_get_input_pin_infomation(i_F_XOring, &info))
    {
        MV_SetHomeSgn(X_AXIS,
                      info.io_register,
                      info.pin_mask,
                      info.valid_value, 5);
    }

#endif

#if AXIS_NUMBER > 1
 //   if (MotorParam->YCycleLength == 0)
        MotorParam->YCycleLength = 1000;
//    if (MotorParam->YCyclePulse == 0)
        MotorParam->YCyclePulse = 10000;

    MV_AxisInit(Y_AXIS, PHY_AXIS_2);
    MV_SetParam(Y_AXIS, MV_PARAM_CLOCK, 6000000);

    MV_SetRatio(Y_AXIS,
                MotorParam->YCyclePulse,
                MotorParam->YCycleLength, 1);
    MV_EnableRatio(Y_AXIS);

    MV_SetParam(Y_AXIS, MV_PARAM_HOME_SPACE, 50);
    MV_SetParam(Y_AXIS, MV_PARAM_HOME_OFFSET, 0);

    MV_SetParam(Y_AXIS, MV_PARAM_START_SPEED, 1000);
    MV_SetAccDec(Y_AXIS, 1, 200, 200);
    MV_SetParam(Y_AXIS, AXIS_HOME_LOW_SPEED, 8000);
    MV_SetParam(Y_AXIS, AXIS_HOME_FIND_SPEED, 1000);
    MV_SetParam(Y_AXIS, MV_PARAM_MIN_STEP, 1);
    MV_SetParam(Y_AXIS, MV_PARAM_PERIOD, 1);
    MV_SetParam(Y_AXIS, MV_PARAM_AAC_LMT, 5000);
    MV_SetParam(Y_AXIS, MV_PARAM_ACC_LMT, 30000);
    MV_SetParam(Y_AXIS, MV_PARAM_DEC_LMT, 30000);

    if (MotorParam->YDir > 0)
    {
        MotorParam->YDir = 1;
    }
    MV_SetParam(Y_AXIS,
                MV_PARAM_DIR_LEVEL,
                MotorParam->YDir);
    if (hw_io_get_input_pin_infomation(i_F_YOring, &info))
    {
        MV_SetHomeSgn(Y_AXIS,
                      info.io_register,
                      info.pin_mask,
                      info.valid_value, 5);
    }

#endif

#if AXIS_NUMBER > 2
    if (MotorParam->ZCycleLength == 0)
        MotorParam->ZCycleLength = 500;
    if (MotorParam->ZCyclePulse == 0)
        MotorParam->ZCyclePulse = 5000;

    MV_AxisInit(Z_AXIS, PHY_AXIS_1);
    MV_SetParam(Z_AXIS, MV_PARAM_CLOCK, 6000000);

    MV_SetRatio(Z_AXIS,
                MotorParam->ZCyclePulse,
                MotorParam->ZCycleLength, 10);
    MV_EnableRatio(Z_AXIS);

    MV_SetParam(Z_AXIS, MV_PARAM_HOME_SPACE, 50);
    MV_SetParam(Z_AXIS, MV_PARAM_HOME_OFFSET, 0);

    MV_SetParam(Z_AXIS, MV_PARAM_START_SPEED, 1000);
    MV_SetAccDec(Z_AXIS, 1, 200, 200);

    MV_SetParam(Z_AXIS, MV_PARAM_MIN_STEP, 1);
    MV_SetParam(Z_AXIS, MV_PARAM_PERIOD, 1);
    MV_SetParam(Z_AXIS, MV_PARAM_AAC_LMT, 5000);
    MV_SetParam(Z_AXIS, MV_PARAM_ACC_LMT, 30000);
    MV_SetParam(Z_AXIS, MV_PARAM_DEC_LMT, 30000);

    if (MotorParam->ZDir > 0)
    {
        MotorParam->ZDir = 1;
    }

    MV_SetParam(Z_AXIS,
                MV_PARAM_DIR_LEVEL,
                MotorParam->ZDir);

    if (hw_io_get_input_pin_infomation(i_F_ZOring, &info))
    {
        MV_SetHomeSgn(Z_AXIS,
                      info.io_register,
                      info.pin_mask,
                      info.valid_value, 5);
    }

#endif

#if AXIS_NUMBER > 3
    if (MotorParam->UCyclePulse == 0)
        MotorParam->UCyclePulse = 5000;
    if (MotorParam->UCycleLength == 0)
        MotorParam->UCycleLength = 10000;

    MV_AxisInit(U_AXIS, PHY_AXIS_4);
    MV_SetParam(U_AXIS, MV_PARAM_CLOCK, 6000000); //设置频率

    MV_SetRatio(U_AXIS,
                MotorParam->UCyclePulse,
                MotorParam->UCycleLength, 10); //设置齿轮比
    MV_EnableRatio(U_AXIS);

    MV_SetParam(U_AXIS, MV_PARAM_HOME_SPACE, 50);
    MV_SetParam(U_AXIS, MV_PARAM_HOME_OFFSET, 0); //设置原点偏移

    //设置启动速度
    MV_SetParam(U_AXIS, MV_PARAM_START_SPEED, 1000);
    MV_SetAccDec(U_AXIS, 1, 200, 200); //设置加减速时间

    MV_SetParam(U_AXIS, MV_PARAM_MIN_STEP, 1); //设置最小加速步
    MV_SetParam(U_AXIS, MV_PARAM_PERIOD, 1);   //设置加减速周期
    //伺服采用的是定加加速S曲线加减速
    MV_SetParam(U_AXIS, MV_PARAM_AAC_LMT, 5000);
    MV_SetParam(U_AXIS, MV_PARAM_ACC_LMT, 30000);
    MV_SetParam(U_AXIS, MV_PARAM_DEC_LMT, 30000);

    if (MotorParam->UDir > 0)
    {
        MotorParam->UDir = 1;
    }
    MV_SetParam(U_AXIS,
                MV_PARAM_ENABLE_SGN_LEVEL,
                MotorParam->UDir);
    if (hw_io_get_input_pin_infomation(i_F_UOring, &info))
    { //设置原点引脚
        MV_SetHomeSgn(U_AXIS,
                      info.io_register,
                      info.pin_mask,
                      info.valid_value, 5);
    }

    //设置方向

#endif

#if INTERP_TASK_NUMBER
    float X_JGRatio =  (float)MotorParam->XCyclePulse / (float)MotorParam->XCycleLength;
    float Y_JGRatio =  (float)MotorParam->YCyclePulse / (float)MotorParam->YCycleLength;
    float Z_JGRatio =  (float)MotorParam->ZCyclePulse / (float)MotorParam->ZCycleLength;

//插补任务初始化
    MV_InterpInit(0, MV_GetParam(X_AXIS, AXIS_CLOCK));
//插补虚拟轴分配
    MV_InterpSetAxis(0, X_AXIS, X_AXIS, X_JGRatio , 1); //X_JGRatio);
    MV_InterpSetAxis(0, Y_AXIS, Y_AXIS, Y_JGRatio , 1); //Y_JGRatio);
    MV_InterpSetAxis(0, Z_AXIS, Z_AXIS, Z_JGRatio , 1); //Z_JGRatio);
#endif
}

/**
 * 轴应用函数
 *
 * @author lxz
 *
 * @param void
 */
void axis_app_run(void)
{

    //更新回零信号
    MV_UpdateHomeSgn(X_AXIS, X_Origin, 0);
    MV_UpdateHomeSgn(Y_AXIS, Y_Origin, 0);

    //更新报警信息
    //MV_UpdateAlarmSgn(X_AXIS, iServoError, 0, 0);

    MV_IdleTask();
}