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;
}
float haptic_axis(char axis, float m)
{
double xyz[AXES];
if(!haptic_ok)
return 0;
dhdGetPosition(&xyz[0],&xyz[1],&xyz[2],DHD);
return (float)(xyz[axis%AXES] * m);
}
void haptic_getpos(double *arr)
{
int i;
if(!haptic_ok)
return;
if(arr == NULL)
return;
if(dhdGetTime() - lt < RI)
{
for(i=0;i<AXES;i++)
arr[i] = 0;
return;
}else
lt = dhdGetTime();
dhdGetPosition(&arr[0],&arr[1],&arr[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);
}
