void SAVE_POSITION() { FILE *fp; fp = fopen("POSITION.txt", "w"); POSITION a; JOINTS b; READ_POSITION(&a, &b); fprintf(fp, "XYZOAT = \n\n"); fprintf(fp, "%g %g %g %g %g %g\n\n", a.X,a.Y,a.Z,a.O,a.A,a.T); fprintf(fp, "JOINTS = \n\n"); fprintf(fp, "%g %g %g %g %g %g\n", b.j1,b.j2,b.j3,b.j4,b.j5,b.j6); }
IOReturn IOSCSITape::ReadPosition(SCSI_ReadPositionShortForm *readPos, bool vendor) { SCSITaskIdentifier task = NULL; IOReturn status = kIOReturnError; SCSITaskStatus taskStatus = kSCSITaskStatus_DeviceNotResponding; IOMemoryDescriptor * dataBuffer = NULL; UInt8 readPosData[20] = { 0 }; dataBuffer = IOMemoryDescriptor::withAddress(&readPosData, sizeof(readPosData), kIODirectionIn); require((dataBuffer != 0), ErrorExit); task = GetSCSITask(); require((task != 0), ErrorExit); if (READ_POSITION(task, dataBuffer, (vendor ? kSCSIReadPositionServiceAction_ShortFormVendorSpecific : kSCSIReadPositionServiceAction_ShortFormBlockID), 0x0, 0x00) == true) { taskStatus = DoSCSICommand(task, SCSI_NOMOTION_TIMEOUT); } if (taskStatus == kSCSITaskStatus_GOOD) { readPos->flags = readPosData[0]; readPos->partitionNumber = readPosData[1]; readPos->firstLogicalObjectLocation = (readPosData[4] << 24) | (readPosData[5] << 16) | (readPosData[6] << 8) | readPosData[7]; readPos->lastLogicalObjectLocation = (readPosData[8] << 24) | (readPosData[9] << 16) | (readPosData[10] << 8) | readPosData[11]; readPos->logicalObjectsInObjectBuffer = (readPosData[13] << 16) | (readPosData[14] << 8) | readPosData[15]; readPos->bytesInObjectBuffer = (readPosData[16] << 24) | (readPosData[17] << 16) | (readPosData[18] << 8) | readPosData[19]; status = kIOReturnSuccess; } ReleaseSCSITask(task); dataBuffer->release(); ErrorExit: return status; }