void firmware_init(){ // Initialize firmware dxl_initialize(0,1); //initialize dynamixel communication serial_initialize(57600); //initialize serial communication //initialize sensors sensor_init(SENSOR_FRONT,SENSOR_DISTANCE); sensor_init(SENSOR_FRONTLEFT,SENSOR_IR); sensor_init(SENSOR_FRONTRIGHT,SENSOR_IR); sensor_init(SENSOR_BACKLEFT,SENSOR_IR); sensor_init(SENSOR_BACKRIGHT,SENSOR_IR); //initialize I/O io_init(); io_set_interrupt(BTN_START, &reset_state); //assign callback function when start button is pressed // Activate general interrupts sei(); // Set motors to wheel mode motor_set_mode(254, MOTOR_WHEEL_MODE); // Set serial communication through ZigBee serial_set_zigbee(); }
void Dynamixel::begin(int baud) { //TxDString("[DXL]start begin\r\n"); afio_remap(AFIO_REMAP_USART1);//USART1 -> DXL afio_cfg_debug_ports(AFIO_DEBUG_FULL_SWJ_NO_NJRST); #ifdef BOARD_CM900 //Engineering version case gpio_set_mode(PORT_ENABLE_TXD, PIN_ENABLE_TXD, GPIO_OUTPUT_PP); gpio_set_mode(PORT_ENABLE_RXD, PIN_ENABLE_RXD, GPIO_OUTPUT_PP); gpio_write_bit(PORT_ENABLE_TXD, PIN_ENABLE_TXD, 0 );// TX Disable gpio_write_bit(PORT_ENABLE_RXD, PIN_ENABLE_RXD, 1 );// RX Enable #else gpio_set_mode(PORT_TXRX_DIRECTION, PIN_TXRX_DIRECTION, GPIO_OUTPUT_PP); gpio_write_bit(PORT_TXRX_DIRECTION, PIN_TXRX_DIRECTION, 0 );// RX Enable #endif timer_set_mode(TIMER2, TIMER_CH1, TIMER_OUTPUT_COMPARE); timer_pause(TIMER2); uint16 ovf = timer_get_reload(TIMER2); timer_set_count(TIMER2, min(0, ovf)); timer_set_reload(TIMER2, 30000);//set overflow ovf = timer_get_reload(TIMER2); timer_set_compare(TIMER2, TIMER_CH1, min(1000, ovf)); timer_attach_interrupt(TIMER2, TIMER_CH1, TIM2_IRQHandler); timer_generate_update(TIMER2); timer_resume(TIMER2); dxl_initialize(0, baud); }
bool DXLJointInterface::init() { if(!dxl_initialize(0, BAUD_NUM)) return false; dxl_write_byte(200, 24, 1); // sleep(1); // dxl_ping(id()); // if(dxl_get_result() != COMM_RXSUCCESS) // { // ROS_ERROR("Could not find dynamixel with ID %d", id()); // return false; // } dxl_write_word(id(), REG_TORQUE_LIMIT, 1023); dxl_write_word(id(), REG_CW_ANGLE_LIMIT, 0); setControlType(CT_POSITION); dxl_write_word(id(), REG_TORQUE_ENABLE, 1); dxl_write_byte(id(), REG_P_GAIN, 2); dxl_write_byte(id(), REG_I_GAIN, 0); dxl_write_byte(id(), REG_D_GAIN, 0); return true; }
/** * Initialize the servos */ void legsInit(void){ dxl_initialize( 0, DEFAULT_BAUDNUM ); // Not using device index sei(); // Interrupt Enable // Set speed of dynamixels dxl_write_word( BROADCAST_ID, 32, 1000); }
void Dynamixel::begin(int baud) { //TxDString("[DXL]start begin\r\n"); afio_remap(AFIO_REMAP_USART1);//USART1 -> DXL afio_cfg_debug_ports(AFIO_DEBUG_FULL_SWJ_NO_NJRST); #ifdef BOARD_CM900 //Engineering version case gpio_set_mode(PORT_ENABLE_TXD, PIN_ENABLE_TXD, GPIO_OUTPUT_PP); gpio_set_mode(PORT_ENABLE_RXD, PIN_ENABLE_RXD, GPIO_OUTPUT_PP); gpio_write_bit(PORT_ENABLE_TXD, PIN_ENABLE_TXD, 0 );// TX Disable gpio_write_bit(PORT_ENABLE_RXD, PIN_ENABLE_RXD, 1 );// RX Enable #else gpio_set_mode(PORT_TXRX_DIRECTION, PIN_TXRX_DIRECTION, GPIO_OUTPUT_PP); gpio_write_bit(PORT_TXRX_DIRECTION, PIN_TXRX_DIRECTION, 0 );// RX Enable #endif /* timer_set_mode(TIMER2, TIMER_CH1, TIMER_OUTPUT_COMPARE); timer_pause(TIMER2); uint16 ovf = timer_get_reload(TIMER2); timer_set_count(TIMER2, min(0, ovf)); timer_set_reload(TIMER2, 10000);//set overflow ovf = timer_get_reload(TIMER2); timer_set_compare(TIMER2, TIMER_CH1, min(1000, ovf)); timer_attach_interrupt(TIMER2, TIMER_CH1, TIM2_IRQHandler); timer_generate_update(TIMER2); //timer_resume(TIMER2);*/ /* * Timer Configuation for Dynamixel bus * 2013-04-03 ROBOTIS Changed it as the below codes * Dynamixel bus used timer 2(channel 1) to check timeout for receiving data from the bus. * So, don't use time 2(channel 1) in other parts. * */ // Pause the timer while we're configuring it timer.pause(); // Set up period timer.setPeriod(1); // in microseconds // Set up an interrupt on channel 1 timer.setMode(TIMER_CH1,TIMER_OUTPUT_COMPARE); timer.setCompare(TIMER_CH1, 1); // Interrupt 1 count after each update timer.attachInterrupt(TIMER_CH1,TIM2_IRQHandler); // Refresh the timer's count, prescale, and overflow timer.refresh(); // Start the timer counting //timer.resume(); dxl_initialize(0, baud); }
int open_USB2Dyn(const int baudnum,const int deviceIndex){ if( dxl_initialize(deviceIndex, baudnum) == 0 ) { printf( "Failed to open USB2Dynamixel!\n" ); return 0; } else printf( "Succeed to open USB2Dynamixel!\n" ); return 1; }
void MotorInit(void){ dxl_initialize( 0, DEFAULT_BAUDNUM ); // Not using device index //Wheel Mode // dxl_write_word( 31, P_CW_ANGLE_LIMIT_L, 0 ); // dxl_write_word( 31, P_CCW_ANGLE_LIMIT_L, 0 ); //Set EEP Lock dxl_write_word( 31, P_EEP_LOCK, 1 ); // Set goal speed dxl_write_word( BROADCAST_ID, P_GOAL_SPEED_L, 0 ); dxl_write_byte( BROADCAST_ID, P_TORQUE_ENABLE, 0 ); _delay_ms(1000); }
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; }
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; }
int open_dxl(int deviceIndex, int baudnum) { //int baudnum = DEFAULT_BAUDNUM; //printf( "\n\nRead/Write example for Linux\n\n" ); ///////// Open USB2Dynamixel //////////// if( dxl_initialize(deviceIndex, baudnum) == 0 ) { //printf( "Failed to open USB2Dynamixel via /dev/ttyUSB%d!\n",deviceIndex ); return 0; } else //printf( "Succeeded in opening /dev/ttyUSB2%d\n", deviceIndex); return 1; }
Dxl::Dxl(unsigned int device_index, unsigned int baudrate) : device_index(device_index), baudrate(baudrate) { if(!dxl_initialize(device_index, baudrate)) { // Error std::stringstream ss; ss << "dxl_initialize() failed."<< std::endl; } else { std::cout << "Connection to DXL device is ok." << std::endl; } }
int main(void) { //int Value = 0; DDRC = 0x7F; PORTC = 0x7D; serial_initialize(57600); // USART Initialize //serial_initialize(250000); // USART Initialize dxl_initialize( 0, 7 ); // Not using device index sei(); SetUpServos(); SetConstants(); SetKConstants(); printf("ARMS CONTROL - Left \r" ); printf("Ver1.0 - Last Moddification 23-03-15\r" ); while (1) { if(cmdReady) { if(!execCommand()) printf("Unknown command\r"); } if(doLoop) { executeControl(); count = count + 100; } if(doGripper) { executeGripper(); } _delay_ms(3); Leds(); } return 1; }
ControlUtils::ControlUtils() { vec_set(joints, 0., TOTAL_JOINTS); vec_set(jointsd, 0., TOTAL_JOINTS); vec_set(joints_d, 0., TOTAL_JOINTS); if( dxl_initialize(0, 1) == 0 ) { printf( "Failed to open USB2Dynamixel!\n" ); printf( "Press Enter key to terminate...\n" ); getchar(); exit(-1); } else printf( "Succeed to open USB2Dynamixel!\n" ); _legIdx = 0; }
int main(){ //portInit(); serial_initialize(57600); dxl_initialize( 0, 1 ); // init with baud = 1 Mbps ADCInit(); sei(); //Enables global interrupts unsigned int distanceLeft, distanceRight, front, movingLeft, movingRight; signed int speedLeft, speedRight; while(1) { //printf("%d %d\n\n",getSensorValue(3),getSensorValue(4)); //_delay_ms(1000); // Get sensor reading (in cm) front = DMSDistance(getSensorValue(1)); distanceLeft = IRValue(getSensorValue(3)); distanceRight = IRValue(getSensorValue(4)); // Calculating the required speed speedRight = (int)((front-40*distanceLeft)); speedLeft = (int)((front-40*distanceRight)); movingLeft = dxl_read_byte( 1, 38 ); movingRight = dxl_read_byte(2,38); if(movingLeft < 15 && movingRight < 15){ wheel(1,0); wheel(2,-20); _delay_ms(1000); }else{ wheel(1,-speedRight); wheel(2,speedLeft); } printf("%d %d - %d\n\n",speedLeft,speedRight,getSensorValue(1)); //_delay_ms(1000); // Making the wheels spin } return 0; }
bool dynamixelApi_setup(int baud, void (*msgHandler)(char*, char, char)) { EventManager_registerTopic(DYNAMIXEL_ERROR_EVENT); int res = dxl_initialize(); if (res == 1) { ase_printf("SUCCESS: USB2Dynamixel opened!\n"); dynamixelApi_setBaud(baud); dyna.msgHandler = msgHandler; dyna.logPos = false; EventManager_registerTopic(DYNAMIXEL_ROTATION_DONE_EVENT); dyna.updateTimer = TimerManager_createPeriodicTimer(100, 0, updateStates); //10hz updates return true; } else { char* errorType = "Failed to open USB2Dynamixel!"; ase_printf("ERROR: %s \n",errorType); Event_t event; event.val_prt = errorType; EventManager_publish(DYNAMIXEL_ERROR_EVENT, &event); return false; } }
void MotorInit(void) { dxl_initialize( 0, DEFAULT_BAUDNUM ); // Not using device index //Wheel Mode dxl_write_word( RIGHT_MOTOR, P_CW_ANGLE_LIMIT_L, 0 ); dxl_write_word( RIGHT_MOTOR, P_CW_ANGLE_LIMIT_H, 0 ); dxl_write_word( RIGHT_MOTOR, P_CCW_ANGLE_LIMIT_L, 0 ); dxl_write_word( RIGHT_MOTOR, P_CCW_ANGLE_LIMIT_H, 0 ); dxl_write_word( LEFT_MOTOR, P_CW_ANGLE_LIMIT_L, 0 ); dxl_write_word( LEFT_MOTOR, P_CW_ANGLE_LIMIT_H, 0 ); dxl_write_word( LEFT_MOTOR, P_CCW_ANGLE_LIMIT_L, 0 ); dxl_write_word( LEFT_MOTOR, P_CCW_ANGLE_LIMIT_H, 0 ); //Set Torque dxl_write_word( RIGHT_MOTOR, 24, 1 ); dxl_write_word( LEFT_MOTOR, 24, 1 ); //Set EEP Lock dxl_write_word( RIGHT_MOTOR, P_EEP_LOCK, 1 ); dxl_write_word( LEFT_MOTOR, P_EEP_LOCK, 1 ); // Set goal speed dxl_write_word( BROADCAST_ID, P_GOAL_SPEED_L, 0 ); _delay_ms(1000); }
BBAzimuthalReflex::BBAzimuthalReflex() : _neck_motor(nullptr), _lock(true), _old_ILD(0) { try { //Initialize DXL framework if(dxl_initialize(0,1)==1) { //Retrieve motor ID and configuration file path if any int MOT_ID_NECK_AZIMUTH; std::string cfg_path; ConfigSingleton::GetInstance()->get_as<int>("MOT_ID_NECK_AZIMUTH",MOT_ID_NECK_AZIMUTH); ConfigSingleton::GetInstance()->get_as<std::string>("MOT_CFG_PATH",cfg_path); ConfigSingleton::GetInstance()->get_as<lbfloat>("DSP_SAMPLE_FREQUENCY",_sample_freq); ConfigSingleton::GetInstance()->get_as<lbfloat>("ILD_STEP_THRESHOLD",_step_threshold); //Declare motor _neck_motor = new Mx28(MOT_ID_NECK_AZIMUTH); //Initialize it //_neck_motor->configure(cfg_path.c_str()); _neck_motor->get_value(MX28_CW_ANGLE_LIMIT, _cw_angle_limit); _neck_motor->get_value(MX28_CCW_ANGLE_LIMIT, _ccw_angle_limit); init(); } else { std::cerr << "Failed to connect to motors." << std::endl; std::cerr << "Leaving." << std::endl; exit(100); } } catch(std::out_of_range& e) { std::cerr << "Could not initialize BBAzimuthalReflex properly. Object might be ill-formed." << std::endl; } }
int main(int argc, char *argv[]) { signal(SIGINT, signal_callback_handler); if (dxl_initialize(0,4) == 0) { printf( "Failed to open USB2Dynamixel!\n" ); return -1; } while (true) { for (int i = 1 ; i <= 13 ; i++) { if (i >= 1 && i <= 8) { int pos_spring = dxl_read_word( 0x64 + i, 0x24 ); int result_spring = dxl_get_result(); if (result_spring != 1) pos_spring = -1; int pos = dxl_read_word( i, 0x24 ); int result = dxl_get_result(); if (result != 1) pos = -1; printf("%d:%d,%d:%d,", i, pos, 0x64+i, pos_spring); } else { int pos = dxl_read_word( i, 0x24 ); int result = dxl_get_result(); if (result != 1) pos = -1; printf("%d:%d,", i, pos); } } printf("\n"); } return EXIT_SUCCESS; }
int main(int argc, char *argv[]) { // Checks command line arguments if (argc != 2) { printf("\nYou must specify an id!\n"); printf("Usage: %s id\n\n", argv[0]); exit(EXIT_SUCCESS); } // Get the id of the servo or spring from // the command line int id = atoi(argv[1]); // This is for handling CTRL+C nicely signal(SIGINT, signal_callback_handler); // Initialize the communication with the // Dynamixel network bus at 250000bps if (dxl_initialize(4,8) == 0) { printf( "Failed to open USB2Dynamixel!\n" ); return -1; } while (true) { int pos = dxl_read_word( id, 0x24 ); int result = dxl_get_result(); if (result != 1) pos = -1; printf("%d:%d\n", id, pos); } return EXIT_SUCCESS; }
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; }
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; }
CSerialController::CSerialController(std::string port1, int baud1, std::string port2, int baud2, float timeout1, float timeout2, double rate, ros::NodeHandle nh, std::string dmxPort) : CQboduinoDriver(port1, baud1, port2, baud2, timeout1, timeout2), rate_(rate), nh_(nh), dmxPort_(dmxPort) { //Check parameters in ROS_PARAM and start controllers //Advertise the test service qboTestService_=nh_.advertiseService("/qbo_arduqbo/test_service", &CSerialController::qboTestService, this); //Check for controllers that are governed by the head board if(boards_.count("head")==1) { //Controlled servos if(nh_.hasParam("controlledservos")) { std::map< std::string, XmlRpc::XmlRpcValue >::iterator it; std::map< std::string, XmlRpc::XmlRpcValue > value; XmlRpc::MyXmlRpcValue servos; nh_.getParam("controlledservos", servos); ROS_ASSERT(servos.getType() == XmlRpc::XmlRpcValue::TypeStruct); value=servos; for(it=value.begin();it!=value.end();it++) { ROS_ASSERT((*it).second.getType() == XmlRpc::XmlRpcValue::TypeStruct); servosList_[(*it).first]=new ControledServo((*it).first,this); servosNamesList_.push_back((*it).first); //Set servos parameters servosList_[(*it).first]->setParams((*it).second); servosList_[(*it).first]->setAngle(0); ROS_INFO_STREAM("Servo pseudocontrolado " << (*it).first << " started"); } } //Un-controlled servos if(nh_.hasParam("uncontrolledservos")) { std::map< std::string, XmlRpc::XmlRpcValue >::iterator it; std::map< std::string, XmlRpc::XmlRpcValue > value; XmlRpc::MyXmlRpcValue servos; nh_.getParam("uncontrolledservos", servos); ROS_ASSERT(servos.getType() == XmlRpc::XmlRpcValue::TypeStruct); value=servos; for(it=value.begin();it!=value.end();it++) { ROS_ASSERT((*it).second.getType() == XmlRpc::XmlRpcValue::TypeStruct); servosList_[(*it).first]=new CServo((*it).first,this); servosNamesList_.push_back((*it).first); //Set servos parameters servosList_[(*it).first]->setParams((*it).second); servosList_[(*it).first]->setAngle(0); ROS_INFO_STREAM("Servo " << (*it).first << " started"); } } if(nh_.hasParam("dynamixelservo")) { //Start dynamixel port if( dxl_initialize(const_cast<char *>(dmxPort_.c_str()), 1) == 0 ) { ROS_ERROR( "Failed to open dynamixel controller!\n" ); } else { ROS_INFO( "Succeed to open dynamixel controller!\n" ); std::map< std::string, XmlRpc::XmlRpcValue >::iterator it; std::map< std::string, XmlRpc::XmlRpcValue > value; XmlRpc::MyXmlRpcValue servos; nh_.getParam("dynamixelservo", servos); ROS_ASSERT(servos.getType() == XmlRpc::XmlRpcValue::TypeStruct); value=servos; for(it=value.begin();it!=value.end();it++) { ROS_ASSERT((*it).second.getType() == XmlRpc::XmlRpcValue::TypeStruct); servosList_[(*it).first]=new DynamixelServo(nh,(*it).first,this); servosNamesList_.push_back((*it).first); //Set servos parameters ((DynamixelServo*)servosList_[(*it).first])->changeTorque(254); servosList_[(*it).first]->setParams((*it).second); servosList_[(*it).first]->setAngle(0,0.3); ROS_INFO_STREAM("Dynamixel " << (*it).first << " started"); } } } //Check for controllers if(nh_.hasParam("controllers")) { std::map< std::string, XmlRpc::XmlRpcValue >::iterator it; std::map< std::string, XmlRpc::XmlRpcValue > value; XmlRpc::MyXmlRpcValue controllers; nh_.getParam("controllers", controllers); ROS_ASSERT(controllers.getType() == XmlRpc::XmlRpcValue::TypeStruct); value=controllers; for(it=value.begin();it!=value.end();it++) { ROS_ASSERT((*it).second.getType() == XmlRpc::XmlRpcValue::TypeStruct); XmlRpc::XmlRpcValue controller_params=(*it).second; if(controller_params.hasMember("type")) { std::string type=controller_params["type"]; //Joint controller if(type.compare("joint_controller")==0) { ROS_INFO("joint_controller started"); controllersList_.push_back(new CJointController((*it).first, this, nh, servosList_)); } //Mouth controller else if(type.compare("mouth_controller")==0) { ROS_INFO("mouth_controller started"); controllersList_.push_back(new CMouthController((*it).first, this, nh)); } else if(type.compare("nose_controller")==0) { ROS_INFO("nose_controller started"); controllersList_.push_back(new CNoseController((*it).first, this, nh)); } else if(type.compare("mics_controller")==0) { ROS_INFO("mics_controller started"); controllersList_.push_back(new CMicsController((*it).first, this, nh)); } } } } //The following lines setup the joint state publisher at the given rate std::string topic; nh_.param("joint_states_topic", topic, std::string("joint_states")); joint_pub_ = nh_.advertise<sensor_msgs::JointState>(topic, 1); timer_=nh_.createTimer(ros::Duration(1/rate_),&CSerialController::timerCallback,this); } //Check for controllers that are governed by the base board if(boards_.count("base")==1) { if(nh_.hasParam("controllers")) { std::map< std::string, XmlRpc::XmlRpcValue >::iterator it; std::map< std::string, XmlRpc::XmlRpcValue > value; XmlRpc::MyXmlRpcValue controllers; nh_.getParam("controllers", controllers); ROS_ASSERT(controllers.getType() == XmlRpc::XmlRpcValue::TypeStruct); value=controllers; for(it=value.begin();it!=value.end();it++) { ROS_ASSERT((*it).second.getType() == XmlRpc::XmlRpcValue::TypeStruct); XmlRpc::XmlRpcValue controller_params=(*it).second; if(controller_params.hasMember("type")) { std::string type=controller_params["type"]; //Base controller (speed and odometry) if(type.compare("base_controller")==0) { ROS_INFO("base_controller started"); controllersList_.push_back(new CBaseController((*it).first, this, nh)); } //Battery controller else if(type.compare("battery_controller")==0) { ROS_INFO("battery_controller started"); controllersList_.push_back(new CBatteryController((*it).first, this, nh)); } //Sensors controller (I2C sonar sensors and ADC readings) else if(type.compare("sensors_controller")==0) { ROS_INFO("sensors_controller started"); sensorsController_ = new CSrf10Controller((*it).first, this, nh); controllersList_.push_back(sensorsController_); } //Back LCD controller else if(type.compare("lcd_controller")==0) { ROS_INFO("lcd_controller started"); controllersList_.push_back(new CLCDController((*it).first, this, nh)); ipTimer_=nh_.createTimer(ros::Duration(5),&CSerialController::ipTimerCallback,this); } //IMU board controller else if(type.compare("imu_controller")==0) { ROS_INFO("imu_controller started"); controllersList_.push_back(new CImuController((*it).first, this, nh)); } else if(type.compare("irs_controller")==0) { ROS_INFO("irs_controller started"); controllersList_.push_back(new CInfraRedsController((*it).first, this, nh)); } } } } } }
int main() { int baudnum = 34; int deviceIndex = 0; 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; jointMode(4,180); jointMode(5,180); /* int i; for (i=4;i<=5;i++) { dxl_ping(i); printf("%i",i); int result = dxl_get_result( ); if( result == COMM_TXSUCCESS ) { printf("TX+"); } else if( result == COMM_RXSUCCESS ) { printf("RX+"); int j; for (j=0;j<=8;j++) { int value; result=dxl_get_result( ); if (result == COMM_RXSUCCESS) { value=dxl_read_byte(i,j); printf("%i->%i\n",j,value); } else printf("Error reading byte\n"); } } else if( result == COMM_TXFAIL ) { printf("TX-"); } else if( result == COMM_RXFAIL) { printf("RX-"); } else if( result == COMM_TXERROR ) { printf("TX-"); } else if( result == COMM_RXWAITING ) { printf("W?"); } printf("\n"); } */ } // Close device dxl_terminate(); printf( "Press Enter key to terminate...\n" ); getchar(); return 0; }
int main() { int i; int baudnum = DEFAULT_BAUDNUM; int GoalPos[2] = {0, 1023}; //int GoalPos[2] = {0, 4095}; // for Ex series int index = 0; int deviceIndex = 0; int Moving, PresentPos; int CommStatus; system("/home/humanoid/Documents/visao/bin/Debug/visao"); system("echo 123456 | sudo -S chmod 777 /dev/ttyUSB*");//USB //system("./home/humanoidfei/Documents/visao/bin/Debug/visao"); //id_t PID = getpid(); char string[50]; sprintf(string,"echo 123456 | sudo -S renice -20 -p %d", getpid()); system(string); // char command[50]; // sprintf(command, "echo 123456 | sudo -S renice 0 -p %d", getProcIdByName("pulseaudio")); // system(command); //int PID = popen("pidof pulseaudio","r"); //system("echo 123456 | sudo -S renice 0 1533"); //setpriority(PRIO_PROCESS, getpid(), -20); printf( "\n\nRx-28 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 for(i = 1;i<21;i++) dxl_write_word( i, P_GOAL_POSITION_L, GoalPos[index] ); dxl_write_word( 3, 32, 0);//usleep(1000000); if( index == 0 ) index = 1; else index = 0; /* do { // Read present position //for(i=1;i<11;i++) PresentPos = dxl_read_word( i, P_PRESENT_POSITION_L ); //usleep(1000000); CommStatus = dxl_get_result(); if( CommStatus == COMM_RXSUCCESS ) { printf( "%d %d\n",GoalPos[index], PresentPos ); PrintErrorCode(); } else { PrintCommStatus(CommStatus); break; } // Check moving done //for(i=1;i<11;i++) Moving = dxl_read_byte( i, 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; }
// High level initialization - specific robot settings for Bioloid void dxl_init(int baudnum) { int commStatus = 0, errorStatus = 0; // now prepare the Dynamixel servos // first initialize the bus dxl_initialize( 0, baudnum ); // wait 0.1s _delay_ms(100); // Next check the hardware configuration is valid for (int i=0; i<NUM_AX12_SERVOS; i++) { // ping each servo in turn errorStatus = dxl_ping(AX12_IDS[i]); if (errorStatus == -1) { printf("\nHardware Configuration Failure at Dynamixel ID %i.\n", AX12_IDS[i]); dxl_terminate(); return; } } // set alarm LED and shutdown to prevent overheat/overload commStatus = dxl_write_byte(BROADCAST_ID, DXL_ALARM_LED, 36); if(commStatus != COMM_RXSUCCESS) { printf("\nDXL_ALARM_LED Broadcast - "); dxl_printCommStatus(dxl_get_result()); } commStatus = dxl_write_byte(BROADCAST_ID, DXL_ALARM_SHUTDOWN, 36); if(commStatus != COMM_RXSUCCESS) { printf("\nDXL_ALARM_LED Broadcast - "); dxl_printCommStatus(dxl_get_result()); } // now set temperature and voltage limits commStatus = dxl_write_byte(BROADCAST_ID, DXL_TEMPERATURE_LIMIT, 70); if(commStatus != COMM_RXSUCCESS) { printf("\nDXL_TEMPERATURE_LIMIT Broadcast - "); dxl_printCommStatus(dxl_get_result()); } commStatus = dxl_write_byte(BROADCAST_ID, DXL_LOW_VOLTAGE_LIMIT, 70); if(commStatus != COMM_RXSUCCESS) { printf("\nDXL_LOW_VOLTAGE_LIMIT Broadcast - "); dxl_printCommStatus(dxl_get_result()); } // set a 2-point compliance margin (equals 0.58 deg) commStatus = dxl_write_byte(BROADCAST_ID, DXL_CW_COMPLIANCE_MARGIN, 2); if(commStatus != COMM_RXSUCCESS) { printf("\nDXL_CW_COMPLIANCE_MARGIN Broadcast - "); dxl_printCommStatus(dxl_get_result()); } commStatus = dxl_write_byte(BROADCAST_ID, DXL_CCW_COMPLIANCE_MARGIN, 2); if(commStatus != COMM_RXSUCCESS) { printf("\nDXL_CCW_COMPLIANCE_MARGIN Broadcast - "); dxl_printCommStatus(dxl_get_result()); } _delay_ms(100); // and enable torque to keep positions commStatus = dxl_write_byte(BROADCAST_ID, DXL_TORQUE_ENABLE, 1); if(commStatus != COMM_RXSUCCESS) { printf("\nDXL_TORQUE_ENABLE Broadcast - "); dxl_printCommStatus(dxl_get_result()); } _delay_ms(50); }
int main(){ int devNumber = 0; int baudNumber = 1; //For 1 Mbps baud printf("Running StandSit. Press ESC to terminate.\n"); if(!(dxl_initialize(devNumber, baudNumber))){ printf("Failed to open USB Interface. :(\n"); return 0; } else{ printf("USB Interfaced Opened! :D\n"); } // if(SITTING_POS != dxl_read_byte(BROADCAST_ID, P_PRESENT_POSITION_L)){ // // printf("Going to initial sitting position.\n"); // dxl_write_word(BROADCAST_ID, P_PRESENT_POSITION_L, SITTING_POS); // while(1 == dxl_read_byte(BROADCAST_ID, P_MOVING)); // } int loopCount = 0; printf("Going into main loop...\n"); while(1){ printf("Loop time! Round %d\n", loopCount++); // if(getchar() == 0x1b) // break; if(STANDING_POS == dxl_read_byte(BROADCAST_ID, P_PRESENT_POSITION_L)){ printf("Sitting down...\n"); dxl_write_word(BROADCAST_ID, P_PRESENT_POSITION_L, SITTING_POS); } else if(SITTING_POS == dxl_read_byte(BROADCAST_ID, P_PRESENT_POSITION_L)){ printf("Standing up...\n"); dxl_write_word(BROADCAST_ID, P_PRESENT_POSITION_L, STANDING_POS); } else{ int CommStatus = dxl_get_result(); switch(CommStatus) { case COMM_TXFAIL: printf("COMM_TXFAIL: Failed transmit instruction packet!\n"); break; case COMM_TXERROR: printf("COMM_TXERROR: Incorrect instruction packet!\n"); break; case COMM_RXFAIL: printf("COMM_RXFAIL: Failed get status packet from device!\n"); break; case COMM_RXWAITING: printf("COMM_RXWAITING: Now recieving status packet!\n"); break; case COMM_RXTIMEOUT: printf("COMM_RXTIMEOUT: There is no status packet!\n"); break; case COMM_RXCORRUPT: printf("COMM_RXCORRUPT: Incorrect status packet!\n"); break; default: printf("This is unknown error code!\n"); break; } } //while(1 == dxl_read_byte(BROADCAST_ID, P_MOVING)); sleep(1); } dxl_terminate(); printf("Shutting down...\n"); return 0; }
int main() { double CoMavg; double sumns = 0; unsigned char dc1[2]; unsigned char dc2[2]; double arrayX[] = { -31.6190, -31.9288, -32.0040, -32.3204, -32.5042, -16.5338, -16.5530, -16.7062, -16.7277, -16.7474, 0.2290,0.1433,-0.1915,-0.4104,-0.6338,16.3727,16.4583,16.5425,16.6221,16.7002,32.8839,32.6383,32.3759,32.1024, 31.8116}; //{-20.0480,-22.4280,-20.0480,-22.4280,-20.0480,-14.0480,-16.4280,-14.0480,-16.4280,-14.0480,-8.0480,-10.4280,-8.0480,-10.4280,-8.0480,-2.0480,-4.4280,-2.0480,-4.4280,-2.0480,3.9520,1.5720,3.9520,1.5720,3.9520,9.9520,7.5720,9.9520,7.5720,9.9520,15.9520,13.5720,15.9520,13.5720,15.9520,21.9520,19.5720,21.9520, 19.5720, 21.9520}; // {4.76, 2.38, 4.76, 2.38, 4.76, 10.76, 8.38, 10.76, 8.38, 10.76, 16.76, 14.38, 16.76, 14.38, 16.76, 22.76,20.38,22.76, 20.38, 22.76,28.76, 26.38,28.76, 26.38, 28.76, 34.76, 32.38, 34.76, 32.38, 34.76, 40.76, 38.38, 40.76, 38.38, 40.76, 46.76, 44.38, 46.76, 44.38, 46.76}; //double arrayY[] = { 12,6,0,-6,-12,12,6,0,-6,-12,12,6,0,-6,-12,12,6,0,-6,-12,12,6,0,-6,-12,12,6,0,-6,-12,12,6,0,-6,-12,12,6,0,-6,-12}; //{27, 21, 15, 9, 3, 27, 21, 15, 9, 3, 27, 21, 15, 9, 3, 27, 21, 15, 9, 3, 27, 21, 15, 9, 3, 27, 21, 15, 9, 3, 27, 21, 15, 9, 3, 27, 21, 15, 9, 3}; double weight[] = {0.5241, 2.2774, 0, 0.3845, 1.4690, 0.0059, 0,3.7938, 0,2.6875,323.1828,238.0930,0,0,0,7.7829,0,1.6664,0,0,0.8394,0.8289,0,1.2467,0.5905}; //dynamixel initialization stuff int baudnum = 1; int limits[2] = {0, 1024}; int index = 0; int deviceIndex = 0; int Moving, PresentPos; int CommStatus; int ns = 20; double CoMhist[ns]; if( dxl_initialize(deviceIndex, baudnum) == 0 ) { printf( "Failed to open USB2Dynamixel!\n" ); } int turnValue = 512; dxl_write_word( MOTOR, 32, 75); //sets the speed for turning dxl_write_word( MOTOR, P_GOAL_POSITION_L, turnValue ); TakkTile takktile = TakkTile(); takktile.startSampling(200,dc2); double baseline[40]; std::cout<< "calibrating..." <<std::endl; if(takktile.calibrate(baseline,10)==0) { std::cout<< "error!" <<std::endl; goto exit; } std::cout<<"done calibrating"<<std::endl; for (int i=0; i<100000; i++) { //args instead of 5 if (i > ns) sumns = sumns - CoMhist[i%ns]; Data data; if(takktile.getData(data)==0){ std::cout<< "error :(" <<std::endl; } double sumx=0; // double sumy=0; double sum=0; for (int j = 0; j < 25; j++) {//change to 40 for array sumx += (double)(data.pressures[j]-baseline[j])*arrayX[j]*weight[j]; // sumy += (double)(data.pressures[i]-baseline[i])*arrayY[i]*weight[i]; sum += (double)(data.pressures[j]-baseline[j])*weight[j]; } double CoMx = (double)(sumx)/(double)(sum); // double CoMy = (double)(sumy)/(double)(sum); CoMhist[i % ns] = CoMx; sumns = sumns + CoMhist[i%ns]; CoMavg = (sumns / ns) + 1; if (i > 500){ Moving = dxl_read_byte( DEFAULT_ID, P_MOVING ); std::cout<<CoMx<<","<<CoMavg<<","<<turnValue<<"; "; //if (Moving){} //else{ if(turnValue > 700) { turnValue = 700;} else if (turnValue < 350) { turnValue = 350;} else turnValue = turnValue - (int)(CoMavg/2);//plus for arry //std::cout<<turnValue<<std::endl; } dxl_write_word( MOTOR, P_GOAL_POSITION_L, turnValue ); //} // do { // Read present position //PresentPos = dxl_read_word( MOTOR, P_PRESENT_POSITION_L ); //CommStatus = dxl_get_result(); //if( CommStatus == COMM_RXSUCCESS ) { // printf( "%d %d\n",turnValue, PresentPos ); //PrintErrorCode(); // } else { //PrintCommStatus(CommStatus); // std::cout<< "comm error" <<std::endl; // break; //} // Check moving done // Moving = dxl_read_byte( MOTOR, P_MOVING ); //CommStatus = dxl_get_result(); //if( CommStatus == COMM_RXSUCCESS ) { // //PrintErrorCode(); //} else { // std::cout<< "comm error 2" <<std::endl; //PrintCommStatus(CommStatus); // break; //} // } while(Moving == 1); // PresentPos = dxl_read_word( MOTOR, P_PRESENT_POSITION_L ); // std::cout<< CoMx << " " << CoMy <<std::endl; }//time how long this takes // /for (int i = 0; i<100000000;i++); exit: //note: doesn't quit very well takktile.stopSampling(dc1); takktile.TakkClose(); dxl_terminate(); return 0; };
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 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; }
DynamixelInterface::DynamixelInterface() { int status=dxl_initialize(DEVICEINDEX,BAUDNUM) == 0; // cout << status << endl; ok=DXL_ComError::isOK(status); }