void haptic_fire(unsigned f) { if(!haptic_ok) return; dhdSetForce(f*10,f*10,f*10,DHD); dhdSleep(0.05); dhdSetForce(0,0,0,DHD); return; }
void haptic_force(double forces[AXES]) { int i; if(!haptic_ok) return; if(forces == NULL) return; if(abs(forces[i]) > 222) { dhdSetForce(0,0,0,DHD); return; } dhdSetForce(forces[0],forces[1],forces[2],DHD); return; }
void haptic_init() { int i,major, minor, release, revision; dhdGetAPIVersion (&major, &minor, &release, &revision); Com_Printf ("Force Dimension - %d.%d.%d.%d\n", major, minor, release, revision); Com_Printf ("(C) 2010 Force Dimension\n"); Com_Printf ("All Rights Reserved.\n\n"); //Com_Printf ("%s device detected\n\n", dhdGetSystemName()); if(haptic_ok) return; //axes = (double*)malloc(AXES*sizeof(double)); buf = (haptic_t*)malloc(sizeof(haptic_t)); { buf->fmove = 0; for(i=0;i<AXES;i++) buf->views[i] = 0; for(i=0;i<BTN_CNT;i++) buf->buttons[i] = 0; } for(i=0;i<AXES;i++) axes[i] = 0; //haptic_ok = !(dhdOpen() < 0); i = dhdOpen(); if(i >= 0) haptic_ok = true; Com_Printf("%s\r\n",dhdErrorGetLastStr()); if(!haptic_ok) return; //dhdReset(DHD); // dhdWaitForReset(0,DHD); dhdSetGravityCompensation(DHD_ON,DHD); dhdSetForce(0,0,0,DHD); return; }
void haptic_stuff(cvar_t sens) { int i; if(!haptic_ok) return; dhdGetPosition(&axes[0],&axes[1],&axes[2],DHD); if(axes[0] == 0 && axes[1] == 0 && axes[2] == 0) return; for(i=0;i<BTN_CNT;i++) buf->buttons[i] = (char)(haptic_btn() == 1<<i); if(cnt > 0) cnt -= 1; if(buf->buttons[0] && cnt < 5) cnt += (rand()%5)+3; //haptic_print(axes,"sts"); // 0 -> down // 1 -> right -> 3 buf->views[0] = (float)axes[2] * -15.0 * sens.value; buf->views[1] = (float)axes[1]; //buf->views[0] = (float)axes[2]; //buf->views[1] = (float)axes[1]; haptic_move(buf->views,(sens.value)*15.0); double Y = axes[0]*100; if(Y < 1.0 && Y > -1.0) Y = 0; else Y *= -32.0; buf->fmove = ClampChar(Y); haptic_joystick(axes,cnt); //haptic_force(axes); dhdSetForce(axes[0],axes[1],axes[2],DHD); return; }
//=========================================================================== int cDeltaDevice::command(int a_command, void* a_data) { // temp variables int result = CHAI_MSG_OK; double x=0.0,y=0.0,z=0.0; // check if device is open if (m_systemReady) { switch (a_command) { // read position of delta device case CHAI_CMD_GET_POS_3D: { // read position from device dhdGetPosition(&x, &y, &z, m_deviceID); cVector3d* position = (cVector3d *) a_data; // Convert from m to mm position->mul(1000.0); position->set(x, y, z); } break; // read normalized position of the delta device case CHAI_CMD_GET_POS_NORM_3D: { // read position from device dhdGetPosition(&x, &y, &z, m_deviceID); // normalize position cVector3d* position = (cVector3d *) a_data; position->set(x, y, z); position->div(m_halfSizeWorkspace); } break; // read orientation angles of delta wrist case CHAI_CMD_GET_ROT_ANGLES: { cVector3d* angles = (cVector3d *) a_data; angles->set(0, 0, 0); dhdGetOrientationRad(&angles->x, &angles->y, &angles->z, m_deviceID); } break; // read orientation matrix of wrist case CHAI_CMD_GET_ROT_MATRIX: { cMatrix3d frame; frame.identity(); switch (m_deviceType) { // delta devices case (DHD_DEVICE_3DOF): case (DHD_DEVICE_6DOF): case (DHD_DEVICE_6DOF_500): { // read angles cVector3d angles; angles.set(0,0,0); dhdGetOrientationRad(&angles.x, &angles.y, &angles.z, m_deviceID); // compute rotation matrix angles.mul(1.5); frame.rotate(cVector3d(1,0,0), angles.x); frame.rotate(cVector3d(0,1,0), angles.y); frame.rotate(cVector3d(0,0,1), angles.z); } break; // omega devices case (DHD_DEVICE_OMEGA): case (DHD_DEVICE_OMEGA3): case (DHD_DEVICE_OMEGA33): case (DHD_DEVICE_OMEGA331): { // read rotation matrix double rot[3][3]; rot[0][0] = 1.0; rot[0][1] = 0.0; rot[0][2] = 0.0; rot[1][0] = 0.0; rot[1][1] = 1.0; rot[1][2] = 0.0; rot[2][0] = 0.0; rot[2][1] = 0.0; rot[2][2] = 1.0; dhdGetOrientationFrame(rot, m_deviceID); cMatrix3d result; frame.m[0][0] = rot[0][0]; frame.m[0][1] = rot[0][1]; frame.m[0][2] = rot[0][2]; frame.m[1][0] = rot[1][0]; frame.m[1][1] = rot[1][1]; frame.m[1][2] = rot[1][2]; frame.m[2][0] = rot[2][0]; frame.m[2][1] = rot[2][1]; frame.m[2][2] = rot[2][2]; } break; } // return result cMatrix3d* matrix = (cMatrix3d *) a_data; *matrix = frame; } break; // set force to device case CHAI_CMD_SET_FORCE_3D: { cVector3d* force = (cVector3d *) a_data; dhdSetForce(force->x, force->y, force->z, m_deviceID); } break; // set torque to device case CHAI_CMD_SET_TORQUE_3D: { cVector3d* torque = (cVector3d *) a_data; dhdSetTorque(torque->x, torque->y, torque->z, m_deviceID); } break; // read user switch from wrist case CHAI_CMD_GET_SWITCH_0: { int* result = (int *) a_data; *result = getUserSwitch(m_deviceID); } break; // read user switch from wrist case CHAI_CMD_GET_SWITCH_MASK: { int* result = (int *) a_data; // Force the result to be 0 or 1, since bit 0 should carry button 0's value *result = getUserSwitch(m_deviceID) ? 1 : 0; } break; // read scale factor from normalized coords to mm case CHAI_CMD_GET_NORMALIZED_SCALE_FACTOR: { double* scale = (double*)a_data; // Multiply by m_halfSizeWorkspace to get meters back *scale = m_halfSizeWorkspace; // Then multiply by 1000 to get millimeters *scale *= 1000.0; result = CHAI_MSG_OK; } break; // function is not implemented for delta devices default: result = CHAI_MSG_NOT_IMPLEMENTED; } } else { result = CHAI_MSG_SYSTEM_NOT_READY; } return (result); }