Phantom omini设备开发流程

Posted 2020-08-24 FREE

     最近在忙着做毕业设计，我的毕业设计是做力觉临场感的，所以在力反馈设备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 /*****************************************************************************

  2 
  3 Module Name:
  4 
  5   HelloHapticDevice.c
  6 
  7 Description: 
  8 
  9   This application creat the device\'s position and angle,at the moment we just
 10   can use one device.
 11   
 12 Auther     : qiuheng
 13 
 14 Data       : 2016.12.19
 15 
 16 *******************************************************************************/
 17 #ifdef  _WIN64
 18 #pragma warning (disable:4996)
 19 #endif
 20 
 21 #include <stdio.h>
 22 #include <assert.h>
 23 
 24 #if defined(WIN32)
 25 # include <windows.h>
 26 # include <conio.h>
 27 #else
 28 # include "conio.h"
 29 # include <string.h>
 30 #endif
 31 
 32 #include <HD/hd.h>
 33 #include <HDU/hduError.h>
 34 #include <HDU/hduVector.h>
 35 
 36 #include<stdio.h>
 37 #include<string.h>
 38 
 39 void mainLoop(void);
 40 
 41 HDCallbackCode HDCALLBACK gravityWellCallback(void *data);
 42 HDCallbackCode HDCALLBACK gravityWellCallback_1(void *data);
 43 HDCallbackCode HDCALLBACK SchedulerCallback(void *data);
 44 HDCallbackCode HDCALLBACK SchedulerCallback_1(void *data);
 45 /* Data Structure. */
 46 struct Sensable{
 47     hduVector3Dd jointAngles;
 48     hduVector3Dd position;
 49     hduVector3Dd jointTorques; 
 50 } Haptic1, Haptic2;
 51 
 52 float tau1[3];
 53 float tau2[3];
 54 float tau3[3];
 55 float tau4[3];
 56 double position_[3];
 57 hduVector3Dd joint[3];
 58 HDSchedulerHandle gCallbackHandle = 0;
 59 HHD hHD1;
 60 HHD hHD2;
 61 
 62 //Haptic1 is slave robot   ===>Default_PHANToM
 63 //Haptic2 is master robot  ===>PHANToM_05
 64 
 65 /*******************************************************************************
 66  Main function.
 67  Initializes the device, starts the schedule, creates a schedule callback
 68  to handle gravity well forces, waits for the user to press a button, exits
 69  the application.
 70 *******************************************************************************/
 71 int main(int argc, char* argv[])
 72 {    
 73     HDErrorInfo error;
 74     HDSchedulerHandle   hGravityWell;
 75     HDSchedulerHandle   hGravityWell_1;
 76     //ofstream ofile;               //定义输出文件
 77     FILE *fp1;
 78     FILE *fp2;
 79     FILE *fp3;
 80     FILE *fp4;
 81     int i=0;
 82     int j=0;
 83     /* Initialize the device, must be done before attempting to call any hd 
 84        functions. Passing in HD_DEFAULT_DEVICE causes the default device to be 
 85        initialized.
 86      */
 87 
 88       hHD2 = hdInitDevice("PHANToM_05");
 89         //Sleep(100);
 90         hHD1 = hdInitDevice("Default_PHANToM");
 91 
 92     hGravityWell = hdScheduleAsynchronous(
 93             SchedulerCallback, 0, HD_MAX_SCHEDULER_PRIORITY);
 94     
 95     hdEnable(HD_FORCE_OUTPUT);
 96     //hdMakeCurrentDevice(hHD1);
 97     hdEnable(HD_FORCE_OUTPUT);
 98     
 99     hdStartScheduler();
103     /* Wait until the user presses a key.  Meanwhile, the scheduler
104        runs and applies forces to the device. */
105     //printf("Feel around for the gravity well...\\n");
106     //printf("Press any key to quit.\\n\\n");
107 
108     fp1=fopen("D:/毕业设计程序/data/Haptic1_position.txt","a+");
109     fp2=fopen("D:/毕业设计程序/data/Haptic2_position.txt","a+");
110     fp3=fopen("D:/毕业设计程序/data/Haptic1_jointAngles.txt","a+");
111     fp4=fopen("D:/毕业设计程序/data/Haptic2_jointAngles.txt","a+");
112     while (!_kbhit())
113     {
114         
115        for(i=0;i<3;i++)
116          {
117              //position_[i]=Haptic1.position[i];
118              printf("Position_1=>%d:%lf\\n",i,Haptic1.position[i]);
119              tau3[j]=Haptic1.position[j];
120              fprintf(fp1,"Position_1[%d]:%lf\\n",j,tau3[j]);
121              fflush(fp1);
122          }
123     
124        for(j=0;j<3;j++)
125          {
126             printf("Angles_1[%d]:%lf\\n",j,Haptic1.jointAngles[j]);
127             tau1[j]=Haptic1.jointAngles[j];
128             fprintf(fp3,"Angles_1[%d]:%lf\\n",j,tau1[j]);
129          }
130     
131        for(i=0;i<3;i++)
132          {
133              //position_[i]=Haptic1.position[i];
134              printf("Position_2=>%d:%lf\\n",i,Haptic2.position[i]);
135              tau4[j]=Haptic2.position[j];
136              fprintf(fp2,"Haptic2.position[%d]:%lf\\n",j,tau4[j]);
137              fflush(fp2);
138          }
139         
140        for(j=0;j<3;j++)
141          {
142             printf("Angles_2[%d]:%lf\\n",j,Haptic2.jointAngles[j]);
143             tau2[j]=Haptic2.jointAngles[j];
144             fprintf(fp4,"Angles_2[%d]:%lf\\n",j,tau2[j]);
145          }
146 
147         /* Periodically check if the gravity well callback has exited. */
148         if (!hdWaitForCompletion(hGravityWell, HD_WAIT_CHECK_STATUS))
149          {
150             fprintf(stderr, "Press any key to quit.\\n");     
151             getch();
152             break;
153          }
154     }
155 
156  // /* For cleanup, unschedule callback and stop the scheduler. */
157     hdStopScheduler();
158     hdUnschedule(hGravityWell);
159     //hdUnschedule(hGravityWell_1);
160     /* Disable the device1. */
161     hdDisableDevice(hHD1); 
162     fclose(fp1);
163     fclose(fp2);
164     fclose(fp3);
165     fclose(fp4);
166     /* Disable the device2. */
167     hdDisableDevice(hHD2);
168 
169     return 0;
170 }
179 /*=============================================================================*/
180 /* Functions for Phantom =========================================*/
181 
182 /******************************************************************
183  * Sets Torque To Haptic.
184  ******************************************************************/
185 
186 HDCallbackCode HDCALLBACK SchedulerCallback(void *pUserData)
187 {
188     float a;
189 
190     HDErrorInfo error;
191 
192     Haptic1.jointTorques[0] = 0;
193     Haptic1.jointTorques[1] = 0;
194     Haptic1.jointTorques[2] = 0;
195     
196     Haptic2.jointTorques[0] = 0;
197     Haptic2.jointTorques[1] = 0;
198     Haptic2.jointTorques[2] = 0;
199 
200     hdBeginFrame(hHD1);//-------------------------------------------
201     
202         //Read Haptic State
203     hdGetDoublev(HD_CURRENT_JOINT_ANGLES, Haptic1.jointAngles);
204     hdGetDoublev(HD_CURRENT_POSITION, Haptic1.position);
205 
206 
207     hdBeginFrame(hHD2);//-------------------------------------------
208         //Read Haptic State
209     hdGetDoublev(HD_CURRENT_JOINT_ANGLES, Haptic2.jointAngles);
210     hdGetDoublev(HD_CURRENT_POSITION, Haptic2.position);
211 
212         //Set Haptic torques (tau)
213     a=1000; //to convert from Nm to mNm
214     Haptic2.jointTorques[0] =250*3.5*(Haptic1.jointAngles[0]-Haptic2.jointAngles[0]);
215     Haptic2.jointTorques[1] =250*3.5*(Haptic1.jointAngles[1]-Haptic2.jointAngles[1]);
216     Haptic2.jointTorques[2] =250*1.5*(Haptic1.jointAngles[2]-Haptic2.jointAngles[2]);
217     hdEnable(HD_FORCE_OUTPUT);
218     hdSetDoublev(HD_CURRENT_JOINT_TORQUE,Haptic2.jointTorques);
219     hdEndFrame(hHD2);//-------------------------------------------------
220 
221     hdMakeCurrentDevice(hHD1);
222     //Set Haptic torques (tau)
223     a=1000; //to convert from Nm to mNm
224     Haptic1.jointTorques[0] = -250*3.5*(Haptic1.jointAngles[0]-Haptic2.jointAngles[0]);
225     Haptic1.jointTorques[1] = -250*6*(Haptic1.jointAngles[1]-Haptic2.jointAngles[1]);
226     Haptic1.jointTorques[2] = -250*3.5*(Haptic1.jointAngles[2]-Haptic2.jointAngles[2]);
227 
228     hdSetDoublev(HD_CURRENT_JOINT_TORQUE,Haptic1.jointTorques);
229     hdEndFrame(hHD1);//-------------------------------------------------
230 
231     return HD_CALLBACK_CONTINUE;
232     
233     if (HD_DEVICE_ERROR(error = hdGetError()))
234     {
235         hduPrintError(stderr, &error, "Error while commanding control values");
236         if (hduIsSchedulerError(&error))
237         {
238             return HD_CALLBACK_DONE;
239         }
240     }
241     
242     return HD_CALLBACK_CONTINUE;
243 }