振镜运动和DMA功能例程

振镜运动和DMA功能

// ScanMotionPro.cpp : 定义控制台应用程序的入口点。
//

#include "stdafx.h"
#include <math.h>

// 该例程仅用于功能演示，请保证安全的情况下使用

// 测试功能：振镜运动例程，A口必须接带振镜功能的GNM403模块
// 测试平台：网络型运动控制器
// 测试环境：Windows
// 测试流程：
//           （1）初始化控制器
//           （2）压振镜数据
//           （3）启动振镜运动
//           （4）运动完成，关闭控制器
// 注意事项：
//           （1）本例程使用的“例程专用.xml”、“例程专用.cfg”，仅用于本例程
//           （2）实际使用时，需要使用MotionStudio生成网络配置xml
//           （3）实际使用时，必须确认网络上接了带振镜功能的GNM403模块！！！
//           （4）！！！A口必须接带振镜功能的GNM403模块！！！
//           （4）！！！A口必须接带振镜功能的GNM403模块！！！
//           （4）！！！A口必须接带振镜功能的GNM403模块！！！
//           （4）！！！A口必须接带振镜功能的GNM403模块！！！
//           （4）！！！A口必须接带振镜功能的GNM403模块！！！
//           （4）！！！A口必须接带振镜功能的GNM403模块！！！

// 加载固高运动控制库头文件
#include "gxn.h"
// 动态加载固高运动控制gxn.lib库
#pragma comment(lib,"gxn.lib")

#define PI                             (3.14159265358979323846)

/**
 * @brief 指令出错打印函数
 * @param command 打印信息字符串
 * @param error 错误码
 * @return 错误码
*/
short CommandHandler(char* command, short error)
{
    printf("%s = %d\n", command, error);
    getchar();

    return error;
}

/**
 * @brief 初始化运动控制器（开卡 + 初始化网络拓扑 + 初始化轴）
 * @param core 需要初始化的核号，从1开始
 * @param axis 需要初始化的轴起始索引，从1开始
 * @param axisCount 需要初始化的轴数量，从起始索引axis开始计数，必须大于0
 * @return 0表示初始化成功，非0表示初始化失败
*/
short InitMc(short core,short axis,short axisCount)
{
    short rtn;
    short overTime;
    long status;

    // 打开运动控制器
    rtn = GTN_OpenCard(CHANNEL_PCIE,NULL,NULL);
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_OpenCard",rtn);
    }
    printf("Open Card Success !\n");

    // 初始化网络
    // 注意：（1）“例程专用.xml”仅用于本例程
    //       （2）实际使用时，需要使用MotionStudio生成对应的网络配置文件
    // overTime：网络初始化超时时间，单位：秒
    overTime = 120;
    rtn = GTN_NetInit(NET_INIT_MODE_XML_STRICT,"例程专用.xml",overTime,&status);
    if ( 0 != rtn )
    {
        printf("status = %d\n",status);
        return CommandHandler("GTN_NetInit",rtn);
    }
    printf("Init Net Success !\n");

    // 加载配置文件到控制器
    // 注意：（1）“例程专用.cfg”仅用于本例程
    //       （2）实际使用时，需要使用MotionStudio生成对应的配置文件
    rtn = GTN_LoadConfig(core,"例程专用.cfg");
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_LoadConfig(\"例程专用.cfg\")",rtn);
    }

    // 清除轴状态
    rtn = GTN_ClrSts(core,axis,axisCount);
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_ClrSts",rtn);
    }
    printf("Init Mc Config Success !\n");

    return rtn;
}

/**
 * @brief 压入振镜激光相关数据
 * @param core 核号，从1开始
 * @param scanCrd 振镜坐标系号，从1开始
 * @param pListInfo 指令流信息结构体指针
 * @return 0表示压数据成功，非0表示压数据失败
*/
short PushScanLaserPrmData(short core,short scanCrd,TListInfo *pListInfo)
{
    short rtn;

    // 配置激光开关光延时
    double laserOnDelay;               // 激光开光延时
    double laserOffDelay;              // 激光关光延时

    laserOnDelay = 0;
    laserOffDelay = 50;
    ++ pListInfo->segNum;
    rtn = GTN_SetScanLaserDelayPro(core,scanCrd,laserOnDelay,laserOffDelay,pListInfo);
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_SetScanLaserDelayPro",rtn);
    }

    TLaserParameterPro laserPrmPro;    // 激光参数结构体
    TLaserDutyRatioModeParameterPro *pDuty;
    double duty;
    memset(&laserPrmPro,0,sizeof(laserPrmPro));
    pDuty = &(laserPrmPro.laserPrm.dutyRatioModePrm);

    // 设置振镜激光为占空比模式，输出频率：20kHz，占空比：50%
    laserPrmPro.laserMode = 0;         // 激光输出模式：占空比模式
    pDuty->minDutyRatio = 0;           // 激光输出占空比最小值：0%
    pDuty->maxDutyRatio = 100;         // 激光输出占空比最大值：100%
    pDuty->frequency = 20;             // 激光输出频率：20kHz

    ++ pListInfo->segNum;
    rtn = GTN_SetScanLaserPrmPro(core,scanCrd,&laserPrmPro,pListInfo);
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_SetScanLaserPrmPro",rtn);
    }

    duty = 50;                         // 激光输出占空比：50%

    ++ pListInfo->segNum;
    rtn = GTN_SetScanLaserPowerPro(core,scanCrd,duty,pListInfo);
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_SetScanLaserPowerPro",rtn);
    }

    printf("Push Scan Laser Data Success !\n");

    return rtn;
}

/**
 * @brief 压入振镜运动数据
 * @param core 核号，从1开始
 * @param scanCrd 振镜坐标系号，从1开始
 * @param pListInfo 指令流信息结构体指针
 * @return 0表示压数据成功，非0表示压数据失败
*/
short PushScanMotionData(short core,short scanCrd,TListInfo *pListInfo)
{
    short rtn;

    // 画5个同心32边形，买条边激光开关间隔
    short count;                       // 同心多边形个数
    short polygonCount;                // 多边形边的数量
    double radiusStep;                 // 半径步长
    count = 5;
    polygonCount = 32;
    radiusStep = 5000;

    TScanLinearMotionPro linearMotion; // 振镜运动参数结构体
    TVelModePro *pVelMode;
    memset(&linearMotion,0,sizeof(linearMotion));

    pVelMode = &(linearMotion.motionPrm.velMode);
    pVelMode->vel = 1500;

    short i,j;
    double radius;
    double theta;
    double thetaStep;
    radius = 0;
    theta = 0;
    thetaStep = 2*PI/polygonCount;
    for (i=0;i<count;++i)
    {
        radius += radiusStep;
        linearMotion.pos[0] = radius;
        linearMotion.pos[1] = 0;
        ++ pListInfo->segNum;
        rtn = GTN_ScanLinearPro(core,scanCrd,SCAN_MOTION_MODE_JUMP,&linearMotion,pListInfo);
        if ( 0 != rtn )
        {
            return CommandHandler("GTN_ScanLinearPro",rtn);
        }

        theta = 0;
        for (j=0;j<polygonCount;++j)
        {
            theta += thetaStep;
            linearMotion.pos[0] = radius*cos(theta);
            linearMotion.pos[1] = radius*sin(theta);
            ++ pListInfo->segNum;
            if ( 0 == (j%2) )
            {
                rtn = GTN_ScanLinearPro(core,scanCrd,SCAN_MOTION_MODE_MARK,&linearMotion,pListInfo);
            }
            else
            {
                rtn = GTN_ScanLinearPro(core,scanCrd,SCAN_MOTION_MODE_JUMP,&linearMotion,pListInfo);
            }
            if ( 0 != rtn )
            {
                return CommandHandler("GTN_ScanLinearPro",rtn);
            }
        }
    }

    linearMotion.pos[0] = 0;
    linearMotion.pos[1] = 0;
    ++ pListInfo->segNum;
    rtn = GTN_ScanLinearPro(core,scanCrd,SCAN_MOTION_MODE_JUMP,&linearMotion,pListInfo);
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_ScanLinearPro",rtn);
    }

    printf("Push Scan Motion Data Success !\n");

    return rtn;
}

int _tmain(int argc, _TCHAR* argv[])
{
    short rtn;                         // 指令返回值
    short core;                        // 需要执行例程的运动控制器核号
    short axis;                        // 需要初始化的轴起始索引号
    short axisCount;                   // 需要初始化的轴数量，从轴起始索引号开始算起

    // 初始化运动控制器
    // 开卡 + 初始化网络拓扑 + 初始化核1的1-8轴
    core = 1;
    axis = 1;
    axisCount = 8;
    rtn = InitMc(core,axis,axisCount);
    if ( 0 != rtn )
    {
        return rtn;
    }

    short scanCrd;                     // 振镜坐标系号
    short laserChannel;                // 振镜关联的激光通道号

    // 关联振镜和激光通道
    scanCrd = 1;                       // 第一套振镜
    laserChannel = 1;                  // 第一套激光
                                       // 注意：此处激光通道索引从1开始，0表示解绑。与其他激光指令范围不一致
    rtn = GTN_SetScanLaserLinkPro(core,scanCrd,laserChannel,0);
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_SetScanLaserLinkPro",rtn);
    }

    // 打开振镜DMA数据传输通道
    short dmaChannel;                  // dma通道号
    short threshold;                   // dma每次传输数据阈值

    dmaChannel = 1;                    // 第一路dma通道
    threshold = 200;                   // 一次dma传输200段数据
    rtn = GTN_ScanHsOn(core,scanCrd,dmaChannel,threshold);
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_ScanHsOn",rtn);
    }

    short list;                        // 振镜数据需要放入的指令流号
    TListInfo listInfo;                // 指令流信息结构体
    memset(&listInfo,0,sizeof(listInfo));

    list = 1;                          // 第一套指令流
    listInfo.list = list;

    // 振镜初始化
    rtn = GTN_ScanInitPro(core,scanCrd,0,&listInfo);
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_ScanInitPro",rtn);
    }

    // 清除数据
    rtn = GTN_ClearCommandListData(core,list,0);
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_ClearCommandListData",rtn);
    }

    // 压振镜激光数据
    rtn = PushScanLaserPrmData(core,scanCrd,&listInfo);
    if ( 0 != rtn )
    {
        return rtn;
    }

    // 压振镜运动数据
    rtn = PushScanMotionData(core,scanCrd,&listInfo);
    if ( 0 != rtn )
    {
        return rtn;
    }

    // 将剩余数据推入控制器中
    do 
    {
        rtn = GTN_CommandListDataEnd(core,list);
    } while ( 1 == rtn );
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_CommandListDataEnd",rtn);
    }

    printf("Push Scan All Data Success !\n");

    rtn = GTN_StartCommandList(core,list,0);
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_StartCommandList",rtn);
    }

    double pos[3];
    TScanStatusPro status;
    memset(&status,0,sizeof(status));
    do 
    {
        rtn = GTN_GetScanCrdPosPro(core,scanCrd,pos);
        rtn = GTN_GetScanStatusPro(core,scanCrd,&status);

        printf("Scan:%d x:%lf y:%lf run:%d empty:%d \r",
            scanCrd,pos[0],pos[1],status.run,status.fifoEmpty);

    } while ( 1 == status.run );

    if ( 1 == status.fifoEmpty )
    {
        printf("\nScan Motion Done !\n");
    }

    // 复位振镜
    rtn = GTN_StopCommandList(core,list,0);
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_StopCommandList",rtn);
    }

    // 关闭振镜DMA通道
    rtn = GTN_ScanHsOff(core,scanCrd);
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_ScanHsOff",rtn);
    }

    // 关闭控制器
    rtn = GTN_Close();
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_Close",rtn);
    }

    printf("Press Any Key To Exit !\n");
    getchar();

    return 0;
}