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;
}
