标签：command tin etc vector color dstar oat llb code
最近在忙着做毕业设计，我的毕业设计是做力觉临场感的，所以在力反馈设备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 }
标签：command tin etc vector color dstar oat llb code
原文地址：http://www.cnblogs.com/qiuheng/p/6217500.html
