Phantom omini设备开发流程

     最近在忙着做毕业设计，我的毕业设计是做力觉临场感的，所以在力反馈设备Phantom Omini，由于整个设备是国外的国内的资料很少，我是14年拿到这个设备的但是真的是在开发是在16年了，中间有很多事没来得及进行，现在我把我的一个开发流程记录一下，也算是给后面需要使用这个设备的开发人员留下一点资料。

      力反馈设备Phantom Omini可以称之为六自由度机械臂，他有六个关节，其中三个关节有电机所以可以提供一个力觉的反馈，它的开发SDK提供了很多的例程，基本上都可以使用，在使用SDK时要注意配置好各个依赖的头文件和.lib库。在正式开发时，我们需要在下面这个Geomagic Touch Setup里面去设置设备的名字，当设置好名字后按下Pairing这个按钮如果出现succussful这个提示说明对设备初始化成功了，在按下Pairing这个按钮一定要不要忘了按设备后面的一个小按钮，只有这样才能初始化成功。

      在初始化后我们可以利用设备提供的上位机进行测试。

       

       在这个上位机软件上就可以测试设备末端点的位姿和各个关节的力了。

       好了下面我们开始在vs2010里面对设备进行开发，在vs2010里面进行开发我已经默认你已经将vs2010针对Phantom Omini这个设备的库和头文件包含好了。在vs2010里面开发程序我们首先需要了解设备的SDK，整个开发都是基于SDK的，所以熟悉它对开发的进度有很大的帮助，下面的代码的功能是：

1、对两台Phantom Omini设备进行初始

hHD2 = hdInitDevice("PHANToM_05");

hHD1 = hdInitDevice("Default_PHANToM")；

2、设置回调函数

整个回调函数会在一个单独的线程中进行执行，在Phantom Omini设备的开发过程中所有的设备状态读取、设备设置都需要在这个线程中执行，都是通过回调函数进行的。

hGravityWell = hdScheduleAsynchronous( SchedulerCallback, 0, HD_MAX_SCHEDULER_PRIORITY);SchedulerCallback就是回调函数。

3、设备使能

hdEnable(HD_FORCE_OUTPUT);

//hdMakeCurrentDevice(hHD1);

hdEnable(HD_FORCE_OUTPUT);

4、开启调度线程

这个线程开启以后除非结束线程，线程会一直执行，并且周期在1000hz、15000hz等周期内循环执行，当然也可以选择只是执行一次，在回调函数中HD_CALLBACK_CONTINUE这个宏可以设置。

hdStartScheduler();

5、打印两个设备的每个关节的关节角然后将关节角写入txt文档中进行保存

  1 /*****************************************************************************

Module Name:

  HelloHapticDevice.c

Description: 

  This application creat the device's position and angle,at the moment we just
  can use one device.
  
Auther     : qiuheng

Data       : 2016.12.19

*******************************************************************************/
#ifdef  _WIN64
#pragma warning (disable:4996)
#endif

#include <stdio.h>
#include <assert.h>

#if defined(WIN32)
# include <windows.h>
# include <conio.h>
#else
# include "conio.h"
# include <string.h>
#endif

#include <HD/hd.h>
#include <HDU/hduError.h>
#include <HDU/hduVector.h>

#include<stdio.h>
#include<string.h>

void mainLoop(void);

HDCallbackCode HDCALLBACK gravityWellCallback(void *data);
HDCallbackCode HDCALLBACK gravityWellCallback_1(void *data);
HDCallbackCode HDCALLBACK SchedulerCallback(void *data);
HDCallbackCode HDCALLBACK SchedulerCallback_1(void *data);
/* Data Structure. */
struct Sensable{
    hduVector3Dd jointAngles;
    hduVector3Dd position;
    hduVector3Dd jointTorques; 
} Haptic1, Haptic2;

float tau1[3];
float tau2[3];
float tau3[3];
float tau4[3];
double position_[3];
hduVector3Dd joint[3];
HDSchedulerHandle gCallbackHandle = 0;
HHD hHD1;
HHD hHD2;

//Haptic1 is slave robot   ===>Default_PHANToM
//Haptic2 is master robot  ===>PHANToM_05

/*******************************************************************************
 Main function.
 Initializes the device, starts the schedule, creates a schedule callback
 to handle gravity well forces, waits for the user to press a button, exits
 the application.
*******************************************************************************/
int main(int argc, char* argv[])
{    
    HDErrorInfo error;
    HDSchedulerHandle   hGravityWell;
    HDSchedulerHandle   hGravityWell_1;
    //ofstream ofile;               //定义输出文件
    FILE *fp1;
    FILE *fp2;
    FILE *fp3;
    FILE *fp4;
    int i=0;
    int j=0;
    /* Initialize the device, must be done before attempting to call any hd 
       functions. Passing in HD_DEFAULT_DEVICE causes the default device to be 
       initialized.
     */

      hHD2 = hdInitDevice("PHANToM_05");
        //Sleep(100);
        hHD1 = hdInitDevice("Default_PHANToM");

    hGravityWell = hdScheduleAsynchronous(
            SchedulerCallback, 0, HD_MAX_SCHEDULER_PRIORITY);
    
    hdEnable(HD_FORCE_OUTPUT);
    //hdMakeCurrentDevice(hHD1);
    hdEnable(HD_FORCE_OUTPUT);
    
    hdStartScheduler();
    /* Wait until the user presses a key.  Meanwhile, the scheduler
       runs and applies forces to the device. */
    //printf("Feel around for the gravity well...
");
    //printf("Press any key to quit.

");

    fp1=fopen("D:/毕业设计程序/data/Haptic1_position.txt","a+");
    fp2=fopen("D:/毕业设计程序/data/Haptic2_position.txt","a+");
    fp3=fopen("D:/毕业设计程序/data/Haptic1_jointAngles.txt","a+");
    fp4=fopen("D:/毕业设计程序/data/Haptic2_jointAngles.txt","a+");
    while (!_kbhit())
    {
        
       for(i=0;i<3;i++)
         {
             //position_[i]=Haptic1.position[i];
             printf("Position_1=>%d:%lf
",i,Haptic1.position[i]);
             tau3[j]=Haptic1.position[j];
             fprintf(fp1,"Position_1[%d]:%lf
",j,tau3[j]);
             fflush(fp1);
         }
    
       for(j=0;j<3;j++)
         {
            printf("Angles_1[%d]:%lf
",j,Haptic1.jointAngles[j]);
            tau1[j]=Haptic1.jointAngles[j];
            fprintf(fp3,"Angles_1[%d]:%lf
",j,tau1[j]);
         }
    
       for(i=0;i<3;i++)
         {
             //position_[i]=Haptic1.position[i];
             printf("Position_2=>%d:%lf
",i,Haptic2.position[i]);
             tau4[j]=Haptic2.position[j];
             fprintf(fp2,"Haptic2.position[%d]:%lf
",j,tau4[j]);
             fflush(fp2);
         }
        
       for(j=0;j<3;j++)
         {
            printf("Angles_2[%d]:%lf
",j,Haptic2.jointAngles[j]);
            tau2[j]=Haptic2.jointAngles[j];
            fprintf(fp4,"Angles_2[%d]:%lf
",j,tau2[j]);
         }

        /* Periodically check if the gravity well callback has exited. */
        if (!hdWaitForCompletion(hGravityWell, HD_WAIT_CHECK_STATUS))
         {
            fprintf(stderr, "Press any key to quit.
");     
            getch();
            break;
         }
    }

 // /* For cleanup, unschedule callback and stop the scheduler. */
    hdStopScheduler();
    hdUnschedule(hGravityWell);
    //hdUnschedule(hGravityWell_1);
    /* Disable the device1. */
    hdDisableDevice(hHD1); 
    fclose(fp1);
    fclose(fp2);
    fclose(fp3);
    fclose(fp4);
    /* Disable the device2. */
    hdDisableDevice(hHD2);

    return 0;
}
/*=============================================================================*/
/* Functions for Phantom =========================================*/

/******************************************************************
 * Sets Torque To Haptic.
 ******************************************************************/

HDCallbackCode HDCALLBACK SchedulerCallback(void *pUserData)
{
    float a;

    HDErrorInfo error;

    Haptic1.jointTorques[0] = 0;
    Haptic1.jointTorques[1] = 0;
    Haptic1.jointTorques[2] = 0;
    
    Haptic2.jointTorques[0] = 0;
    Haptic2.jointTorques[1] = 0;
    Haptic2.jointTorques[2] = 0;

    hdBeginFrame(hHD1);//-------------------------------------------
    
        //Read Haptic State
    hdGetDoublev(HD_CURRENT_JOINT_ANGLES, Haptic1.jointAngles);
    hdGetDoublev(HD_CURRENT_POSITION, Haptic1.position);


    hdBeginFrame(hHD2);//-------------------------------------------
        //Read Haptic State
    hdGetDoublev(HD_CURRENT_JOINT_ANGLES, Haptic2.jointAngles);
    hdGetDoublev(HD_CURRENT_POSITION, Haptic2.position);

        //Set Haptic torques (tau)
    a=1000; //to convert from Nm to mNm
    Haptic2.jointTorques[0] =250*3.5*(Haptic1.jointAngles[0]-Haptic2.jointAngles[0]);
    Haptic2.jointTorques[1] =250*3.5*(Haptic1.jointAngles[1]-Haptic2.jointAngles[1]);
    Haptic2.jointTorques[2] =250*1.5*(Haptic1.jointAngles[2]-Haptic2.jointAngles[2]);
    hdEnable(HD_FORCE_OUTPUT);
    hdSetDoublev(HD_CURRENT_JOINT_TORQUE,Haptic2.jointTorques);
    hdEndFrame(hHD2);//-------------------------------------------------

    hdMakeCurrentDevice(hHD1);
    //Set Haptic torques (tau)
    a=1000; //to convert from Nm to mNm
    Haptic1.jointTorques[0] = -250*3.5*(Haptic1.jointAngles[0]-Haptic2.jointAngles[0]);
    Haptic1.jointTorques[1] = -250*6*(Haptic1.jointAngles[1]-Haptic2.jointAngles[1]);
    Haptic1.jointTorques[2] = -250*3.5*(Haptic1.jointAngles[2]-Haptic2.jointAngles[2]);

    hdSetDoublev(HD_CURRENT_JOINT_TORQUE,Haptic1.jointTorques);
    hdEndFrame(hHD1);//-------------------------------------------------

    return HD_CALLBACK_CONTINUE;
    
    if (HD_DEVICE_ERROR(error = hdGetError()))
    {
        hduPrintError(stderr, &error, "Error while commanding control values");
        if (hduIsSchedulerError(&error))
        {
            return HD_CALLBACK_DONE;
        }
    }
    
    return HD_CALLBACK_CONTINUE;
}