void ReleaseBSP(BioAPI_UUID_PTR * uuid) { BioAPI_RETURN bioReturn; // If the device is attached (which is signified by hCurrentBiometric NOT being equal to // 0) then call BioAPI_ModuleDetach. if(gModuleHandle != 0) { bioReturn = BioAPI_ModuleDetach(gModuleHandle); if(BioAPI_OK != bioReturn) { PrintErrorCode(bioReturn); return; } gModuleHandle = 0; } // If the Module was loaded (which is signified by the uuid NOT being equal to 0) then call // BioAPI_ModuleUnload. if(*uuid != NULL) { bioReturn = BioAPI_ModuleUnload(*uuid, NULL/*(BioAPI_ModuleEventHandler)BiometricEventHandler*/, 0); if(BioAPI_OK != bioReturn) { PrintErrorCode(bioReturn); return; } free(*uuid); *uuid = NULL; } }
// // unregister a service as device-notification listener // void LSLib_UnregisterDevNotification(HDEVNOTIFY hSCSIIfNtf, HDEVNOTIFY hVolNtf) { if(UnregisterDeviceNotification(hSCSIIfNtf) == FALSE) { PrintErrorCode("[LDServ] Unregister SCSI Adapter Interface Notification fail... ", GetLastError()); } if(UnregisterDeviceNotification(hVolNtf) == FALSE) { PrintErrorCode("[LDServ] Unregister Logical Volume Notification fail... ", GetLastError()); } }
void motor_sync_move(const uint8_t size, const uint8_t * id, const uint16_t * position, const char blocking) { volatile int i, CommStatus; dxl_set_txpacket_id(MOTOR_BROADCAST_ID); //set broadcast id dxl_set_txpacket_instruction(INST_SYNC_WRITE); //set instruction type dxl_set_txpacket_parameter(0, GOAL_POSITION_L); //memory area to write dxl_set_txpacket_parameter(1, 2); //length of the data for(i=0;i<size;i++){ dxl_set_txpacket_parameter(2+(3*i), id[i]); //id dxl_set_txpacket_parameter(2+(3*i)+1, dxl_get_lowbyte(position[i]));//low byte dxl_set_txpacket_parameter(2+(3*i)+2, dxl_get_highbyte(position[i]));//high byte } dxl_set_txpacket_length((2+1)*size+4); //set packet length dxl_txrx_packet(); //transmit packet CommStatus = dxl_get_result(); //get transmission state if( CommStatus == COMM_RXSUCCESS ){ //transmission succeded PrintErrorCode(); //show potentiol motors error (overload, overheat,etc....) if (blocking == MOTOR_MOVE_BLOCKING) //blocking function requested { // Wait for finish of all motors for (i = 0; i < size; i++) motor_wait_finish(id[i], position[i]); } } else //communication failed PrintCommStatus(CommStatus); //show communication error }
void test() { bMoving = dxl_read_byte( id, MOVING ); CommStatus = dxl_get_result(); if( CommStatus == COMM_RXSUCCESS ) { if( bMoving == 0 ) { // Change goal position if( INDEX == 0 ) INDEX = 1; else INDEX = 0; // Write goal position dxl_write_word( id, GOAL_POSITION_L, GoalPos[INDEX] ); } PrintErrorCode(); // Read present position wPresentPos = dxl_read_word( id, PRESENT_POSITION_L ); TxDWord16(GoalPos[INDEX]); TxDString(" "); TxDWord16(wPresentPos); TxDByte_PC('\r'); TxDByte_PC('\n'); } else PrintCommStatus(CommStatus); }
void isMoving(const int servo_id,int *result,int *CommStatus){ *result = dxl_read_byte( servo_id, P_MOVING); *CommStatus = dxl_get_result(); if(*CommStatus != COMM_RXSUCCESS) PrintCommStatus(*CommStatus); PrintErrorCode(); }
// inserted by ILGU for alarm Event BOOLEAN LsBusCtlPlugInEx( ULONG SlotNo, ULONG MaxRequestBlocks, HANDLE hEvent, HANDLE hAlarmEvent) { HANDLE file; BOOLEAN result; ULONG bytes; ULONG bytesReturned; PBUSENUM_PLUGIN_HARDWARE_EX lanscsiPluginData; DebugPrint(3, ("[LanscsiLib]LanscsiPlugin: SlotNo. of the device to be enumerated: %d\n", SlotNo)); file = OpenBusInterface(); if(file == NULL) { return FALSE; } bytes = sizeof (BUSENUM_PLUGIN_HARDWARE_EX) + BUS_HARDWARE_IDS_LENGTH; lanscsiPluginData = HeapAlloc(GetProcessHeap(), HEAP_ZERO_MEMORY, bytes); lanscsiPluginData->Size = sizeof (BUSENUM_PLUGIN_HARDWARE_EX); lanscsiPluginData->SlotNo = SlotNo; lanscsiPluginData->MaxRequestBlocks = MaxRequestBlocks; lanscsiPluginData->phEvent = &hEvent; lanscsiPluginData->phAlarmEvent = &hAlarmEvent; DebugPrint(1, ("[LanscsiLib]LanscsiPlugin: hEvent 0x%x\n", hEvent)); DebugPrint(1, ("[LanscsiLib]LanscsiPlugin: hAlarmEvnet 0x%x\n", hAlarmEvent)); CopyMemory( lanscsiPluginData->HardwareIDs, BUS_HARDWARE_IDS, BUS_HARDWARE_IDS_LENGTH); if (!DeviceIoControl ( file, IOCTL_BUSENUM_PLUGIN_HARDWARE_EX, lanscsiPluginData, bytes, lanscsiPluginData, bytes, &bytesReturned, NULL)) { PrintErrorCode("[LanscsiLib]LanscsiPlugin: PlugIn failed ", GetLastError()); result = FALSE; } else { DebugPrint(1, ("[LanscsiLib]LanscsiPlugin: Succeeded.\n")); result = TRUE; } HeapFree(GetProcessHeap(), 0, lanscsiPluginData); CloseHandle(file); return result; }
int main() { int baudnum = 1; int deviceIndex = 0; int PresentPos; int CommStatus; printf( "\n\nRead/Write example for Linux\n\n" ); ///////// Open USB2Dynamixel //////////// if( dxl_initialize(deviceIndex, baudnum) == 0 ) { printf( "Failed to open USB2Dynamixel!\n" ); printf( "Press Enter key to terminate...\n" ); getchar(); return 0; } else printf( "Succeed to open USB2Dynamixel!\n" ); dxl_write_byte( DEFAULT_ID, P_TORQUE_ENABLE, 0 ); while(1) { printf( "Press Enter key to continue!(press ESC and Enter to quit)\n" ); if(getchar() == 0x1b) break; do { // Read present position PresentPos = dxl_read_word( DEFAULT_ID, P_PRESENT_POSITION_L ); CommStatus = dxl_get_result(); if( CommStatus == COMM_RXSUCCESS ) { printf( "Position: %d\n", PresentPos ); PrintErrorCode(); } else { PrintCommStatus(CommStatus); break; } } while(1); } // Close device dxl_terminate(); printf( "Press Enter key to terminate...\n" ); getchar(); return 0; }
void MotorControl(int id, int power) { #ifndef _MOTOR_OFF_ int CommStatus = COMM_RXSUCCESS; // printf( "%d %d\n", id, power ); dxl_write_word( id, P_GOAL_SPEED_L, power ); CommStatus = dxl_get_result(); if( CommStatus == COMM_RXSUCCESS ) PrintErrorCode(); else PrintCommStatus(CommStatus); #endif // _MOTOR_OFF_ }
int main(void) { /* System Clocks Configuration */ RCC_Configuration(); /* NVIC configuration */ NVIC_Configuration(); /* GPIO configuration */ GPIO_Configuration(); SysTick_Configuration(); Timer_Configuration(); dxl_initialize( 0, 1 ); USART_Configuration(USART_PC, Baudrate_PC); while(1) { bMoving = dxl_read_byte( id, P_MOVING ); CommStatus = dxl_get_result(); if( CommStatus == COMM_RXSUCCESS ) { if( bMoving == 0 ) { // Change goal position if( INDEX == 0 ) INDEX = 1; else INDEX = 0; // Write goal position dxl_write_word( id, P_GOAL_POSITION_L, GoalPos[INDEX] ); } PrintErrorCode(); // Read present position wPresentPos = dxl_read_word( id, P_PRESENT_POSITION_L ); TxDWord16(GoalPos[INDEX]); TxDString(" "); TxDWord16(wPresentPos); TxDByte_PC('\r'); TxDByte_PC('\n'); } else PrintCommStatus(CommStatus); } return 0; }
BioAPI_RETURN BiometricEventHandler(BioAPI_UUID *BSPUuid, void * AppNotifyCallbackCtx, BioAPI_DEVICE_ID DeviceID, uint32 Reserved, BioAPI_MODULE_EVENT EventType) { BioAPI_RETURN bioReturn = BioAPI_OK; BioAPI_VERSION Version; switch(EventType) { case BioAPI_NOTIFY_INSERT : if(gModuleHandle == 0) { Version.Major = BioAPI_MAJOR; Version.Minor = BioAPI_MINOR; bioReturn = BioAPI_ModuleAttach(BSPUuid, &Version, &BioAPIWinMemoryFuncs, DeviceID, 0,0,0, NULL, 0, NULL, &gModuleHandle); } break; case BioAPI_NOTIFY_REMOVE : if(gModuleHandle != 0) { bioReturn = BioAPI_ModuleDetach(gModuleHandle); gModuleHandle = 0; } break; case BioAPI_NOTIFY_FAULT : break; case BioAPI_NOTIFY_SOURCE_PRESENT : break; case BioAPI_NOTIFY_SOURCE_REMOVED : break; } if(bioReturn != BioAPI_OK) PrintErrorCode(bioReturn); return(bioReturn); }
BOOLEAN LsBusCtlQueryDvdStatus( ULONG SlotNo, PULONG pDvdStatus) { HANDLE file; BOOLEAN bResult; ULONG bytesReturned; BUSENUM_DVD_STATUS DVDSTATUS_IN; file = OpenBusInterface(); if(file == NULL) { return FALSE; } DVDSTATUS_IN.SlotNo = SlotNo; DVDSTATUS_IN.Size = sizeof(BUSENUM_DVD_STATUS); if (!DeviceIoControl( file, IOCTL_DVD_GET_STATUS, &DVDSTATUS_IN, sizeof(BUSENUM_DVD_STATUS), &DVDSTATUS_IN, sizeof(BUSENUM_DVD_STATUS), &bytesReturned, NULL)) { PrintErrorCode("[LanscsiLib]LanscsiQueryDvdStatus: failed ", GetLastError()); bResult = FALSE; } else { bResult = TRUE; *pDvdStatus = DVDSTATUS_IN.Status; } CloseHandle(file); return bResult; }
void MotorControl( int id, int power ){ int CommStatus; // printf( "%d %d\n", id, power ); // dxl_write_word( id, P_GOAL_SPEED_L, power ); if(1){ dxl_set_txpacket_id(BROADCAST_ID); dxl_set_txpacket_instruction(INST_SYNC_WRITE); dxl_set_txpacket_parameter(0, P_GOAL_SPEED_L); dxl_set_txpacket_parameter(1, 2); dxl_set_txpacket_parameter(2, id); dxl_set_txpacket_parameter(3, dxl_get_lowbyte(power)); dxl_set_txpacket_parameter(4, dxl_get_highbyte(power)); dxl_set_txpacket_length(4+3*1); dxl_txrx_packet(); CommStatus = dxl_get_result(); if( CommStatus == COMM_RXSUCCESS ) PrintErrorCode(); else PrintCommStatus(CommStatus); } }
BOOLEAN LsBusCtlQueryNodeAlive( ULONG SlotNo, PBOOL pbAdapterHasError) { HANDLE file; BOOLEAN bResult; ULONG bytesReturned; BUSENUM_NODE_ALIVE_IN nodeAliveIn; BUSENUM_NODE_ALIVE_OUT nodeAliveOut; file = OpenBusInterface(); if(file == NULL) { return FALSE; } nodeAliveIn.SlotNo = SlotNo; if (!DeviceIoControl( file, IOCTL_BUSENUM_QUERY_NODE_ALIVE, &nodeAliveIn, sizeof(BUSENUM_NODE_ALIVE_IN), &nodeAliveOut, sizeof(BUSENUM_NODE_ALIVE_OUT), &bytesReturned, NULL)) { PrintErrorCode("[LanscsiLib]LanscsiQueryNodeAlive: failed ", GetLastError()); bResult = FALSE; } else { bResult = nodeAliveOut.bAlive; *pbAdapterHasError = nodeAliveOut.bHasError; } CloseHandle(file); return bResult; }
BOOLEAN LsBusCtlEject( ULONG SlotNo) { HANDLE file; BOOLEAN result; ULONG bytesReturned; BUSENUM_EJECT_HARDWARE eject; DebugPrint(3, ("[LanscsiLib]LanscsiEject: SlotNo. of the device to be ejected: %d\n", SlotNo)); file = OpenBusInterface(); if(file == NULL) { return FALSE; } ZeroMemory(&eject, sizeof(BUSENUM_EJECT_HARDWARE)); eject.SlotNo = SlotNo; eject.Size = sizeof (eject); if (!DeviceIoControl ( file, IOCTL_BUSENUM_EJECT_HARDWARE, &eject, sizeof (eject), &eject, sizeof (eject), &bytesReturned, NULL)) { PrintErrorCode("[LanscsiLib]LanscsiEject: LanscsiEject failed ", GetLastError()); result = FALSE; } else result = TRUE; CloseHandle(file); return result; }
BOOLEAN LsBusCtlUnplug( ULONG SlotNo) { HANDLE file; BOOLEAN result; ULONG bytesReturned; BUSENUM_UNPLUG_HARDWARE unplug; DebugPrint(3, ("[LanscsiLib]LanscsiUnplug: SlotNo. of the device to be unpluged: %d\n", SlotNo)); file = OpenBusInterface(); if(file == NULL) { return FALSE; } ZeroMemory(&unplug, sizeof(BUSENUM_UNPLUG_HARDWARE)); unplug.SlotNo = SlotNo; unplug.Size = sizeof (unplug); if (!DeviceIoControl ( file, IOCTL_BUSENUM_UNPLUG_HARDWARE, &unplug, sizeof (unplug), &unplug, sizeof (unplug), &bytesReturned, NULL)) { PrintErrorCode("[LanscsiLib]LanscsiUnplug: LanscsiUnplug failed ", GetLastError()); result = FALSE; } else result = TRUE; CloseHandle(file); return result; }
float DynamixelServo::getAngle() { int pos = dxl_read_word( id_, P_PRESENT_POSITION_L ); int CommStatus = dxl_get_result(); if( CommStatus == COMM_RXSUCCESS ) { PrintErrorCode(); } else { PrintCommStatus(CommStatus); } qbo_arduqbo::motor_state motor_state; motor_state.header.stamp = ros::Time::now(); motor_state.id=id_; motor_state.goal=dxl_read_word( id_, P_GOAL_POSITION_L ); motor_state.position=pos; motor_state.error=motor_state.goal-pos; int speed=dxl_read_word( id_, P_PRESENT_SPEED_L ); if(speed>=1024) speed=1024-speed; motor_state.speed=speed; int load=dxl_read_word( id_, P_PRESENT_LOAD_L ); if(load>=1024) load=1024-load; motor_state.load=((float)load)/1024.0; motor_state.voltage=((float)dxl_read_byte(id_,P_PRESENT_VOLTAGE))/10.0; motor_state.temperature=dxl_read_byte( id_, P_PRESENT_TEMPERATURE ); motor_state.moving=(dxl_read_byte( id_, P_MOVING)==1); servo_state_pub_.publish(motor_state); float angle = (pos - neutral_) * rad_per_tick_; if (invert_) angle = angle * -1.0; angle_ = angle; ROS_DEBUG_STREAM("Recibed angle " << angle_ << " for servo " << name_ << " from the head board"); return angle_; }
BOOLEAN LsBusCtlAddTarget( PLANSCSI_ADD_TARGET_DATA AddTargetData) { HANDLE file; BOOLEAN result; ULONG bytesReturned; DebugPrint(1, ("[LanscsiLib]LanscsiAddTarget: SlotNo. of the device to add target: %d\n", AddTargetData->ulSlotNo)); // Sleep(1000*5); file = OpenBusInterface(); if(file == NULL) { return FALSE; } if (!DeviceIoControl ( file, IOCTL_LANSCSI_ADD_TARGET, AddTargetData, AddTargetData->ulSize, AddTargetData, AddTargetData->ulSize, &bytesReturned, NULL)) { PrintErrorCode("[LanscsiLib]LanscsiAddTarget: Add Target failed ", GetLastError()); result = FALSE; } else { result = TRUE; } CloseHandle(file); return result; }
int main(void) { #if 0 int id[NUM_ACTUATOR]; float phase[NUM_ACTUATOR]; float theta = 0; int AmpPos = 512; //int AmpPos = 2048; // for EX series int GoalPos; int i; int CommStatus; int isPress = 0; int isOn = 0; unsigned char ReceivedData; int Value; mServoList[0] = (stServo *)malloc(sizeof(stServo)); memset((void *)mServoList[0], 0x00, sizeof(stServo) ); mServoList[0]->id = 4; serial_initialize(57600); dxl_initialize( 0, DEFAULT_BAUDNUM ); // Not using device index sei(); // Interrupt Enable printf( "\n\nSyncWrite example for CM-700\n\n" ); #ifdef MODE_SYNC for( i=0; i<NUM_ACTUATOR; i++ ) { id[i] = i+2; phase[i] = 2*PI * (float)i / (float)NUM_ACTUATOR; } #else int wPresentPos = 512; #endif //Set EEP Lock dxl_write_word( BROADCAST_ID, P_EEP_LOCK, 1 ); // Set goal speed dxl_write_word( BROADCAST_ID, P_GOAL_SPEED_L, 0 ); // Set goal position dxl_write_word( BROADCAST_ID, P_GOAL_POSITION_L, AmpPos ); dxl_write_word( 4, P_TORQUE_LIMIT_L, 0); _delay_ms(1000); while(1) { if(~PIND & SW_START){ isPress = 1; }else{ if( isPress == 1 ){ if( isOn == 0 ){ isOn = 1; }else{ isOn = 0; } } isPress = 0; } // while( ReceivedData = getchar() != NULL ){ if(ReceivedData == 'u') Value++; else if(ReceivedData == 'd') Value--; printf("%d, %d\r\n", Value, ReceivedData); // } if( isOn ){ #ifdef MODE_SYNC // Make syncwrite packet dxl_set_txpacket_id(BROADCAST_ID); dxl_set_txpacket_instruction(INST_SYNC_WRITE); dxl_set_txpacket_parameter(0, P_GOAL_POSITION_L); dxl_set_txpacket_parameter(1, 2); for( i=0; i<NUM_ACTUATOR; i++ ) { dxl_set_txpacket_parameter(2+3*i, id[i]); GoalPos = (int)((sin(theta+phase[i]) + 1.0) * (float)AmpPos); printf( "%d ", GoalPos ); dxl_set_txpacket_parameter(2+3*i+1, dxl_get_lowbyte(GoalPos)); dxl_set_txpacket_parameter(2+3*i+2, dxl_get_highbyte(GoalPos)); } dxl_set_txpacket_length((2+1)*NUM_ACTUATOR+4); printf( "\n" ); dxl_txrx_packet(); CommStatus = dxl_get_result(); if( CommStatus == COMM_RXSUCCESS ) PrintErrorCode(); else PrintCommStatus(CommStatus); theta += STEP_THETA; if( theta > 2*PI ) theta -= 2*PI; #else wPresentPos = dxl_read_word( 4, P_PRESENT_POSITION_L ); printf( "%d\n", wPresentPos ); dxl_write_word( 2, P_GOAL_POSITION_L, wPresentPos ); dxl_write_word( 3, P_GOAL_POSITION_L, wPresentPos ); PrintErrorCode(); #endif } getServoStatus(); _delay_ms(CONTROL_PERIOD); } return 0; #endif #if 0 DDRC = 0x7F; PORTC = 0x7E; DDRD = 0x70; PORTD = 0x11; while (1) { if(~PIND & SW_START) PORTC = ~(LED_BAT|LED_TxD|LED_RxD|LED_AUX|LED_MANAGE|LED_PROGRAM|LED_PLAY); else PORTC = LED_BAT|LED_TxD|LED_RxD|LED_AUX|LED_MANAGE|LED_PROGRAM|LED_PLAY; } return 1; #endif while(isFinish == 0){ _delay_ms(500); getSerialData(); // ReceivedData = getchar(); //if(ReceivedData == 'u'){ //printf("%d\r\n", Value); //Value++; //}else if(ReceivedData == 'd'){ //printf("%d\r\n", Value); //Value--; //}else if(ReceivedData == 10 || ReceivedData == 13 ){ //printf("%s\r\n", "end"); //break; //} printf("%s\r\n", "Loop"); } printf("%s\r\n", "finish"); return 0; }
void ServoControl( int act ){ int i; int CommStatus = 0; if( act >= ACT_MAX ){ // printf( "act error: %d / %d\n", act, SERVO_MAX ); return; } //GetAngle int angle = 0; int diffMax = 0; int angleDiff[SERVO_MAX] = {0}; for(int i=0; i<SERVO_MAX; i++ ){ // if( motionFirst < 0 ){ angle = dxl_read_word( servoId[i], P_PRESENT_POSITION_L ); // }else{ // angle = angleList[motionFirst][i]; // } angleDiff[i] = angleList[act][i] - angle; if( angleDiff[i] < 0 ){ angleDiff[i] = angleDiff[i] * -1; } if( diffMax < angleDiff[i] ){ diffMax = angleDiff[i]; } } // motionFirst = act; int speed[SERVO_MAX] = {100}; for(int i=0; i<SERVO_MAX; i++ ){ speed[i] = (int)((float)(angleList[act][SERVO_MAX]) * ((float)angleDiff[i] / diffMax)); if( speed[i] == 0 ){ speed[i] = 1; } } // diffmaxTest[motionCount-1] = diffMax; // movingTime = ((float)CYCLE_TIME/VALUE_MAX) * ((float)VALUE_MAX / angleList[act][SERVO_MAX]) * diffMax; movingTime = diffMax * (float)(((VALUE_MAX*10)/angleList[act][SERVO_MAX])/2); if( movingTime < MAIN_DELAY ){ movingTime = MAIN_DELAY; } //Speed dxl_set_txpacket_id(BROADCAST_ID); dxl_set_txpacket_instruction(INST_SYNC_WRITE); dxl_set_txpacket_parameter(0, P_GOAL_SPEED_L); dxl_set_txpacket_parameter(1, 2); for( i=0; i<SERVO_MAX; i++ ){ dxl_set_txpacket_parameter(2+(3*i), servoId[i]); dxl_set_txpacket_parameter(3+(3*i), dxl_get_lowbyte(speed[i])); dxl_set_txpacket_parameter(4+(3*i), dxl_get_highbyte(speed[i])); } dxl_set_txpacket_length(4+3*SERVO_MAX); dxl_txrx_packet(); CommStatus = dxl_get_result(); if( CommStatus == COMM_RXSUCCESS ){ PrintErrorCode(); //Angle dxl_set_txpacket_id(BROADCAST_ID); dxl_set_txpacket_instruction(INST_SYNC_WRITE); dxl_set_txpacket_parameter(0, P_GOAL_POSITION_L); dxl_set_txpacket_parameter(1, 2); for( i=0; i<SERVO_MAX; i++ ){ dxl_set_txpacket_parameter(2+(3*i), servoId[i]); dxl_set_txpacket_parameter(3+(3*i), dxl_get_lowbyte(angleList[act][i])); dxl_set_txpacket_parameter(4+(3*i), dxl_get_highbyte(angleList[act][i])); } dxl_set_txpacket_length(4+3*SERVO_MAX); dxl_txrx_packet(); CommStatus = dxl_get_result(); if( CommStatus == COMM_RXSUCCESS ){ PrintErrorCode(); }else{ PrintCommStatus(CommStatus); } }else{ PrintCommStatus(CommStatus); } }
int main() { int baudnum = 34; int GoalPos[2] = {0, 7095}; //int GoalPos[2] = {0, 4095}; // for Ex series int index = 1; int deviceIndex = 0; int Moving, PresentPos; int CommStatus; printf( "\n\nRead/Write example for Linux\n\n" ); ///////// Open USB2Dynamixel //////////// if( dxl_initialize(deviceIndex, baudnum) == 0 ) { printf( "Failed to open USB2Dynamixel!\n" ); printf( "Press Enter key to terminate...\n" ); getchar(); return 0; } else printf( "Succeed to open USB2Dynamixel!\n" ); while(1) { printf( "Press Enter key to continue!(press ESC and Enter to quit)\n" ); if(getchar() == 0x1b) break; // Write goal position dxl_write_word( DEFAULT_ID, P_GOAL_POSITION_L, GoalPos[index] ); do { // Read present position PresentPos = dxl_read_word( DEFAULT_ID, P_PRESENT_POSITION_L ); CommStatus = dxl_get_result(); if( CommStatus == COMM_RXSUCCESS ) { printf( "%d %d\n",GoalPos[index], PresentPos ); PrintErrorCode(); } else { PrintCommStatus(CommStatus); break; } // Check moving done Moving = dxl_read_byte( DEFAULT_ID, P_MOVING ); CommStatus = dxl_get_result(); if( CommStatus == COMM_RXSUCCESS ) { if( Moving == 0 ) { // Change goal position if( index == 0 ) index = 1; else index = 0; } PrintErrorCode(); } else { PrintCommStatus(CommStatus); break; } }while(Moving == 1); } // Close device dxl_terminate(); printf( "Press Enter key to terminate...\n" ); getchar(); return 0; }
LRESULT CALLBACK WndProc (HWND hwnd, UINT iMsg, WPARAM wParam, LPARAM lParam) { HDC hdc; PAINTSTRUCT ps; RECT rc; HBRUSH hBrush, hBrushOld; HPEN hPen, hPenOld; LOGBRUSH lgBrush; int index; static BioAPI_UUID_PTR uuid; BioAPI_RETURN bioReturn; TCHAR szUserName[100]; BioAPI_VERSION Version; BioAPI_BIR_HANDLE EnrolledTemplate, CapturedTemplate, ProcessedBir; BioAPI_INPUT_BIR birEnroll, birCapture, InputBirProcessed; BioAPI_BIR_HEADER birHeader; BioAPI_FAR MaxFAR, AchievedFAR; BioAPI_BOOL bResponse; BioAPI_BOOL bPrecedence = BioAPI_TRUE; static BioAPI_BSP_SCHEMA * CurrSchema; switch (iMsg) { case WM_PAINT : hdc = BeginPaint(hwnd, &ps); GetClientRect(hwnd, &rc); lgBrush.lbStyle = BS_SOLID; lgBrush.lbColor = GetSysColor(COLOR_3DFACE); hBrush = CreateBrushIndirect(&lgBrush); hPen = CreatePen(PS_SOLID, 0, GetSysColor(COLOR_3DFACE)); hPenOld = SelectObject(hdc, hPen); hBrushOld = SelectObject(hdc, hBrush); Rectangle(hdc, rc.left, rc.top, rc.right, rc.bottom); SelectObject(hdc, hBrushOld); SelectObject(hdc, hPenOld); DeleteObject(hBrush); DeleteObject(hPen); EndPaint(hwnd, &ps); return 0; case WM_COMMAND : switch (HIWORD (wParam)) { case CBN_SELCHANGE : ReleaseBSP(&uuid); // Retrieve the index of the item selected index = SendMessage(hEnumTech, CB_GETCURSEL, 0, 0); // Retrieve a pointer to the uuid for the module CurrSchema = (BioAPI_BSP_SCHEMA *)SendMessage(hEnumTech, CB_GETITEMDATA, index, 0); uuid = (BioAPI_UUID *)malloc(sizeof(BioAPI_UUID)); if(uuid == NULL) { MessageBox(hwnd, TEXT("Unable to allocate memory to load the module identifier"), TEXT("BioAPI Sample"), MB_OK); return 0; } BioAPI_CopyUuid(uuid, CurrSchema->ModuleId); bioReturn = BioAPI_ModuleLoad(uuid, 0, NULL/*(BioAPI_ModuleEventHandler)BiometricEventHandler*/, 0); if(BioAPI_OK != bioReturn) { PrintErrorCode(bioReturn); free(uuid); uuid = NULL; return 0; } // wsprintf(szCurrHandle, TEXT("Current Handle: 0x%X"), hBtCurrent); // SetWindowText(hCurrHbt, szCurrHandle); Version.Major = BioAPI_MAJOR; Version.Minor = BioAPI_MINOR; bioReturn = BioAPI_ModuleAttach(uuid, &Version, &BioAPIWinMemoryFuncs, 0, 0,0,0, NULL, 0, NULL, &gModuleHandle); if(BioAPI_OK != bioReturn) { PrintErrorCode(bioReturn); BioAPI_ModuleUnload (uuid, NULL, 0); free(uuid); uuid = NULL; return 0; } break; case BN_CLICKED: switch (LOWORD (wParam)) { case IDOK : ReleaseBSP(&uuid); EndDialog(hwnd, 0); PostQuitMessage(0); break; case IDC_ENROLL : if(GetWindowText(hUserId, szUserName, 100)==0) MessageBox(hwnd, TEXT("Please specify a user id"), TEXT("Bad User Id"), MB_OK); else { bioReturn = BioAPI_Enroll(gModuleHandle, BioAPI_PURPOSE_ENROLL_FOR_VERIFICATION_ONLY, NULL, &EnrolledTemplate, NULL, -1, NULL); if(bioReturn != BioAPI_OK) { PrintErrorCode(bioReturn); return 0; } OutputToFile(szUserName, EnrolledTemplate); } break; case IDC_VERIFY : if(GetWindowText(hUserId, szUserName, 100)==0) MessageBox(hwnd, TEXT("Please specify a user id"), TEXT("Bad User Id"), MB_OK); else { if(InputFromFile(szUserName, &birEnroll) != BioAPI_OK) { MessageBox(hwnd, TEXT("User not enrolled"), TEXT("Bad User Id"), MB_OK); return 0; } // See if the BSP supports BioAPI_VerifyMatch by checking // the operations mask if(CurrSchema->Operations & BioAPI_VERIFYMATCH) { if((bioReturn = BioAPI_Capture(gModuleHandle, BioAPI_PURPOSE_VERIFY, &CapturedTemplate, -1, NULL)) != BioAPI_OK) { PrintErrorCode(bioReturn); GlobalFree(birEnroll.InputBIR.BIR); return 0; } if((bioReturn = BioAPI_GetHeaderFromHandle(gModuleHandle, CapturedTemplate, &birHeader)) != BioAPI_OK) { PrintErrorCode(bioReturn); GlobalFree(birEnroll.InputBIR.BIR); return 0; } if(birHeader.Type == BioAPI_BIR_DATA_TYPE_INTERMEDIATE) { birCapture.Form = BioAPI_BIR_HANDLE_INPUT; birCapture.InputBIR.BIRinBSP = &CapturedTemplate; if((bioReturn = BioAPI_Process(gModuleHandle, &birCapture, &ProcessedBir)) != BioAPI_OK) { PrintErrorCode(bioReturn); GlobalFree(birEnroll.InputBIR.BIR); return 0; } MaxFAR = 1; InputBirProcessed.Form = BioAPI_BIR_HANDLE_INPUT; InputBirProcessed.InputBIR.BIRinBSP = &ProcessedBir; } else { MaxFAR = 1; InputBirProcessed.Form = BioAPI_BIR_HANDLE_INPUT; InputBirProcessed.InputBIR.BIRinBSP = &CapturedTemplate; } bioReturn = BioAPI_VerifyMatch(gModuleHandle, &MaxFAR, NULL, &bPrecedence, &InputBirProcessed, &birEnroll, NULL, &bResponse, &AchievedFAR, NULL, NULL); } else // We simply call BioAPI_Verify { MaxFAR = 1; bioReturn = BioAPI_Verify(gModuleHandle, &MaxFAR, NULL, &bPrecedence, &birEnroll, NULL, &bResponse, &AchievedFAR, NULL, NULL, -1, NULL); } GlobalFree(birEnroll.InputBIR.BIR); if(bioReturn != BioAPI_OK) { PrintErrorCode(bioReturn); return 0; } if(bResponse == TRUE) MessageBox(hwnd, TEXT("Match"), TEXT("BioAPI"), MB_OK); else MessageBox(hwnd, TEXT("No Match"), TEXT("BioAPI"), MB_OK); } break; } break; } return 0 ; case WM_CLOSE : ReleaseBSP(&uuid); EndDialog(hwnd, 0); PostQuitMessage(0); return 0; case WM_DESTROY : ReleaseBSP(&uuid); EndDialog(hwnd, 0); PostQuitMessage(0); return 0 ; } return DefWindowProc (hwnd, iMsg, wParam, lParam) ; }
int main() { int id[NUM_ACTUATOR]; int baudnum = 1; int deviceIndex = 0; float phase[NUM_ACTUATOR]; float theta = 0; int AmpPos = 512; //int AmpPos = 2048; // for EX series int GoalPos; int i; int CommStatus; printf( "\n\nSyncWrite example for Linux\n\n" ); // Initialize id and phase for( i=0; i<NUM_ACTUATOR; i++ ) { id[i] = i+1; phase[i] = 2*PI * (float)i / (float)NUM_ACTUATOR; } ///////// Open USB2Dynamixel //////////// if( dxl_initialize(deviceIndex, baudnum) == 0 ) { printf( "Failed to open USB2Dynamixel!\n" ); printf( "Press Enter key to terminate...\n" ); getchar(); return 0; } else printf( "Succeed to open USB2Dynamixel!\n" ); // Set goal speed dxl_write_word( BROADCAST_ID, P_GOAL_SPEED_L, 0 ); // Set goal position dxl_write_word( BROADCAST_ID, P_GOAL_POSITION_L, AmpPos ); while(1) { printf( "Press Enter key to continue!(press ESC and Enter to quit)\n" ); if(getchar() == 0x1b) break; theta = 0; do { // Make syncwrite packet dxl_set_txpacket_id(BROADCAST_ID); dxl_set_txpacket_instruction(INST_SYNC_WRITE); dxl_set_txpacket_parameter(0, P_GOAL_POSITION_L); dxl_set_txpacket_parameter(1, 2); for( i=0; i<NUM_ACTUATOR; i++ ) { dxl_set_txpacket_parameter(2+3*i, id[i]); GoalPos = (int)((sin(theta+phase[i]) + 1.0) * (double)AmpPos); printf( "%d:%d ", id[i], GoalPos ); dxl_set_txpacket_parameter(2+3*i+1, dxl_get_lowbyte(GoalPos)); dxl_set_txpacket_parameter(2+3*i+2, dxl_get_highbyte(GoalPos)); } dxl_set_txpacket_length((2+1)*NUM_ACTUATOR+4); printf( "\n" ); dxl_txrx_packet(); CommStatus = dxl_get_result(); if( CommStatus == COMM_RXSUCCESS ) { PrintErrorCode(); } else { PrintCommStatus(CommStatus); break; } theta += STEP_THETA; usleep(CONTROL_PERIOD); }while(theta < 2*PI); } dxl_terminate(); printf( "Press Enter key to terminate...\n" ); getchar(); return 0; }
int WINAPI WinMain (HINSTANCE hInstance, HINSTANCE hPrevInstance, PSTR szCmdLine, int iCmdShow) { HWND hwnd ; MSG msg ; WNDCLASSEX wndclass ; BioAPI_RETURN bioReturn; BioAPI_VERSION bioVersion; BioAPI_BSP_SCHEMA * BspSchemaArray; BioAPI_BSP_SCHEMA * CurrSchema; int index; uint32 ArraySize, ElementsNeeded, NumElementsReturned, i; RegisterWindowsClass(hInstance, &wndclass); bioVersion.Major = BioAPI_MAJOR; bioVersion.Minor = BioAPI_MINOR; bioReturn = BioAPI_Init(&bioVersion, 0, NULL, 0, NULL); if(BioAPI_OK != bioReturn) { if(BioAPIERR_H_FRAMEWORK_INCOMPATIBLE_VERSION == bioReturn) { MessageBox(GetActiveWindow(), TEXT("This application is not compatible with the installed version of the BioAPI"), TEXT("BioAPI Error"), MB_OK | MB_ICONSTOP); } else { PrintErrorCode(bioReturn); } return 0; } gModuleHandle = 0; hwnd = CreateDialog (hInstance, szAppName, 0, NULL) ; ShowWindow (hwnd, iCmdShow) ; EnumChildWindows(hwnd, EnumChildProc, IDC_USERID); hUserId = g_hWnd; EnumChildWindows(hwnd, EnumChildProc, IDC_BIOTECH); hEnumTech = g_hWnd; EnumChildWindows(hwnd, EnumChildProc, IDC_CURRENTHBT); hCurrHbt = g_hWnd; bioReturn = BioAPI_EnumModules(NULL, 0, &ElementsNeeded, &NumElementsReturned); if(bioReturn != BioAPI_OK) { PrintErrorCode(bioReturn); return 0; } ArraySize = ElementsNeeded; BspSchemaArray = (BioAPI_BSP_SCHEMA *)malloc(ElementsNeeded * sizeof(BioAPI_BSP_SCHEMA)); if(BspSchemaArray == NULL) { MessageBox(GetActiveWindow(), TEXT("Unable to allocate BSP list"), TEXT("BioAPI Sample"), MB_OK); return 0; } bioReturn = BioAPI_EnumModules(BspSchemaArray, ArraySize, &ElementsNeeded, &NumElementsReturned); if(bioReturn != BioAPI_OK) { PrintErrorCode(bioReturn); free(BspSchemaArray); return 0; } CurrSchema = BspSchemaArray; for(i = 0; i < NumElementsReturned; i ++) { index = SendMessage(hEnumTech, CB_ADDSTRING, (WPARAM)0, (LPARAM)(CurrSchema->Description)); SendMessage(hEnumTech, CB_SETITEMDATA, (WPARAM)index, (LPARAM)CurrSchema); CurrSchema ++; } while (GetMessage (&msg, NULL, 0, 0)) { TranslateMessage (&msg) ; DispatchMessage (&msg) ; } free(BspSchemaArray); BioAPI_Terminate(); return msg.wParam ; }
int main() { BioAPI_RETURN bioReturn; BioAPI_VERSION bioVersion; BioAPI_BSP_SCHEMA *BspSchemaArray; BioAPI_BSP_SCHEMA *CurrSchema; int index; uint32 ArraySize, ElementsNeeded, NumElementsReturned, i; printf("Starting BioAPI Test Application\n"); bioVersion.Major = BioAPI_MAJOR; bioVersion.Minor = BioAPI_MINOR; printf("Major=%d Minor=%d\n", bioVersion.Major, bioVersion.Minor); bioReturn = BioAPI_Init(&bioVersion, 0, NULL, 0, NULL); if(BioAPI_OK != bioReturn) { if(BioAPIERR_H_FRAMEWORK_INCOMPATIBLE_VERSION == bioReturn) { printf("This application is not compatible with the installed version of the BioAPI\n"); } else { PrintErrorCode(bioReturn); } return 0; } bioReturn = BioAPI_EnumModules(NULL, 0, &ElementsNeeded, &NumElementsReturned); if(bioReturn != BioAPI_OK) { if (bioReturn == BioAPIERR_H_FRAMEWORK_FUNCTION_FAILED) { printf("No BSPs have been installed"); } else { PrintErrorCode(bioReturn); } return 0; } ArraySize = ElementsNeeded; BspSchemaArray = (BioAPI_BSP_SCHEMA *) malloc(ElementsNeeded * sizeof(BioAPI_BSP_SCHEMA)); if(BspSchemaArray == NULL) { printf("Unable to allocate BSP list\n"); return 0; } bioReturn = BioAPI_EnumModules(BspSchemaArray, ArraySize, &ElementsNeeded, &NumElementsReturned); if(bioReturn != BioAPI_OK) { free(BspSchemaArray); PrintErrorCode(bioReturn); return 0; } CurrSchema = BspSchemaArray; for(i = 0; i < NumElementsReturned; i++) { printf("BSP Index= %d\n", i); printf("BSP Name: %s\n", CurrSchema->BSPName); printf("Description: %s\n", CurrSchema->Description); printf("Vendor: %s\n", CurrSchema->Vendor); PrintModuleId(CurrSchema->ModuleId); printf("Device ID: 0x%.8x\n", CurrSchema->DeviceId); CurrSchema++; } free(BspSchemaArray); BioAPI_Terminate(); printf("Ending BioAPI Test Application\n"); }
int main(void) { int id[NUM_ACTUATOR]; float phase[NUM_ACTUATOR]; float theta = 0; int AmpPos = 512; //int AmpPos = 2048; // for EX series int GoalPos; int i; int CommStatus; serial_initialize(57600); dxl_initialize( 0, DEFAULT_BAUDNUM ); // Not using device index sei(); // Interrupt Enable printf( "\n\nSyncWrite example for CM-700\n\n" ); for( i=0; i<NUM_ACTUATOR; i++ ) { id[i] = i+1; phase[i] = 2*PI * (float)i / (float)NUM_ACTUATOR; } // Set goal speed dxl_write_word( BROADCAST_ID, P_GOAL_SPEED_L, 0 ); // Set goal position dxl_write_word( BROADCAST_ID, P_GOAL_POSITION_L, AmpPos ); _delay_ms(1000); while(1) { // Make syncwrite packet dxl_set_txpacket_id(BROADCAST_ID); dxl_set_txpacket_instruction(INST_SYNC_WRITE); dxl_set_txpacket_parameter(0, P_GOAL_POSITION_L); dxl_set_txpacket_parameter(1, 2); for( i=0; i<NUM_ACTUATOR; i++ ) { dxl_set_txpacket_parameter(2+3*i, id[i]); GoalPos = (int)((sin(theta+phase[i]) + 1.0) * (float)AmpPos); printf( "%d ", GoalPos ); dxl_set_txpacket_parameter(2+3*i+1, dxl_get_lowbyte(GoalPos)); dxl_set_txpacket_parameter(2+3*i+2, dxl_get_highbyte(GoalPos)); } dxl_set_txpacket_length((2+1)*NUM_ACTUATOR+4); printf( "\n" ); dxl_txrx_packet(); CommStatus = dxl_get_result(); if( CommStatus == COMM_RXSUCCESS ) PrintErrorCode(); else PrintCommStatus(CommStatus); theta += STEP_THETA; if( theta > 2*PI ) theta -= 2*PI; _delay_ms(CONTROL_PERIOD); } return 0; }
inline int RecvIt( SOCKET sock, PCHAR buf, int size ) { int iErrcode; int len = size, iReceived; WSAOVERLAPPED overlapped; WSABUF buffer[1]; DWORD dwFlag; DWORD dwRecvDataLen; WSAEVENT hEvent; BOOL bResult; // Overlapped event // hEvent = WSACreateEvent(); // // Receive Reply Header. // memset(&overlapped, 0, sizeof(WSAOVERLAPPED)); overlapped.hEvent = hEvent; iReceived = 0; while(iReceived < size) { if(size - iReceived >= 1024) buffer[0].len = 1024; else buffer[0].len = size - iReceived; buffer[0].buf = buf + iReceived; // Flag dwFlag = 0; iErrcode = WSARecv( sock, buffer, 1, &dwRecvDataLen, &dwFlag, &overlapped, NULL ); if(iErrcode == SOCKET_ERROR) { DWORD dwError = WSAGetLastError(); if(dwError == WSA_IO_PENDING) { DWORD dwFlags; dwError = WSAWaitForMultipleEvents( 1, &hEvent, TRUE, TIME_OUT, TRUE ); if(dwError != WSA_WAIT_EVENT_0) { PrintErrorCode(TEXT("[LanScsiCli]RecvIt: "), dwError); dwRecvDataLen = -1; TRACE("[LanScsiCli]RecvIt: Request %d, Received %d\n", size, iReceived ); goto Out; } // Get Result... bResult = WSAGetOverlappedResult( sock, &overlapped, &dwRecvDataLen, TRUE, &dwFlags ); if(bResult == FALSE) { PrintErrorCode(TEXT("[LanScsiCli]RecvIt: GetOverlappedResult Failed "), GetLastError()); dwRecvDataLen = SOCKET_ERROR; goto Out; } } else { PrintErrorCode(TEXT("[LanScsiCli]RecvIt: WSARecv Failed "), dwError); dwRecvDataLen = -1; goto Out; } } iReceived += dwRecvDataLen; WSAResetEvent(hEvent); } Out: WSACloseEvent(hEvent); return dwRecvDataLen; }