void getServoStatus(void){ for( int i=0; i<NUM_ACTUATOR; i++ ){ if( mServoList[i] != NULL && mServoList[i]->id > 0 ){ mServoList[i]->PresentPosition = dxl_read_word( mServoList[i]->id, P_PRESENT_POSITION_L ); mServoList[i]->PresentLoad = dxl_read_word( mServoList[i]->id, P_PRESENT_LOAD_L ); printf( "%d, %d %d\n", mServoList[i]->id, mServoList[i]->PresentPosition, mServoList[i]->PresentLoad ); } } }
void getOperationType(const int servo_id,int *typeOperation,int *CommStatus){ int cw_val = dxl_read_word( servo_id, P_CW_LIMIT_L); int ccw_val = dxl_read_word( servo_id, P_CCW_LIMIT_L); if (cw_val+ccw_val==0) *typeOperation = WHEEL_TYPE; else if (cw_val<1024 && ccw_val<1024) *typeOperation = JOINT_TYPE; else *typeOperation = MULTITURN_TYPE; *CommStatus = dxl_get_result(); if(*CommStatus != COMM_RXSUCCESS) PrintCommStatus(*CommStatus); }
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); }
int DynamixelSimpleAPI::readCurrentLoad(const int id) { int value = -1; if (checkId(id, false) == true) { int addr = getRegisterAddr(ct, REG_CURRENT_LOAD); value = dxl_read_word(id, addr); if (dxl_print_error() == 0) { // Valid loads are in range [0:2047] if ((value < 0) || (value > 2047)) { value = -1; } } else { value = -1; } } return value; }
int DynamixelSimpleAPI::readCurrentSpeed(const int id) { int value = -1; if (checkId(id, false) == true) { int addr = getRegisterAddr(ct, REG_CURRENT_SPEED); value = dxl_read_word(id, addr); if (dxl_print_error() == 0) { // Valid speeds are in range [0:1023] for "Join Mode", [0:2047] for "Wheel Mode" if ((value < 0) || (value > 2047)) { value = -1; } } else { value = -1; } } return value; }
int DynamixelSimpleAPI::readCurrentPosition(const int id) { int value = -1; if (checkId(id, false) == true) { int addr = getRegisterAddr(ct, REG_CURRENT_POSITION); value = dxl_read_word(id, addr); if (dxl_print_error() == 0) { // Valid positions are in range [0:1023] for most servo series, and [0:4095] for high-end servo series if ((value < 0) || (value > 4095)) { value = -1; } } else { value = -1; } } return value; }
int DynamixelSimpleAPI::getMaxTorque(const int id) { int value = -1; if (checkId(id, false) == true) { int addr = getRegisterAddr(ct, REG_MAX_TORQUE); value = dxl_read_word(id, addr); if (dxl_print_error() == 0) { // Valid torques are in range [0:1023] if ((value < 0) || (value > 1023)) { value = -1; } } else { value = -1; } } return value; }
//=================================================================================================== // Setup //==================================================================================================== void setup() { // Lets initialize the Commander Serial.begin(); if (!command.begin("/dev/ttyXBEE", B38400)) { printf("Commander Begin failed\n"); return; } // Next initialize the Bioloid bioloid.poseSize = CNT_SERVOS; // Read in the current positions... printf("Before readPose\n"); bioloid.readPose(); printf("After readPose\n"); for (int i=1; i <= CNT_SERVOS; i++) { Serial.println(dxl_read_word(i, AX_PRESENT_POSITION_L), DEC); } // Start off to put arm to sleep... Serial.println("Kurt's Arm"); PutArmToSleep(); MSound(3, 60, 2000, 80, 2250, 100, 2500); }
void motor_wait_finish(char id, uint16_t goal_position) { uint16_t present_pos = 0, diff = MOTOR_MAX_DEVIATION + 1; uint8_t cycles_not_moving = 0; uint16_t timeout = 0; while (diff > MOTOR_MAX_DEVIATION && cycles_not_moving < MOTOR_MAX_CYCLES_WITHOUT_MOVING && timeout < MOTOR_MAX_TIMEOUT) { // Read current position present_pos = dxl_read_word(id, PRESENT_POSITION_L); // Wait to not overload serial motor bus _delay_ms(MOTOR_COMMAND_DELAY); timeout += MOTOR_COMMAND_DELAY; if (dxl_get_result() == COMM_RXSUCCESS) { if (present_pos > goal_position) diff = present_pos - goal_position; else diff = goal_position - present_pos; // Check if motor is not moving for a certain time if (diff <= MOTOR_MAX_DEVIATION) cycles_not_moving++; else cycles_not_moving = 0; } } }
int dynamixelApi_getLoad(int actuator) { int load = dxl_read_word( dyna.actuators[actuator], P_PRESENT_LOAD_L); checkForError(actuator,18); int sign = (load&&1<<10)?-1:1; int ratio = 0x03FF&&load; return sign * ratio; }
int dynamixelApi_getSpeed(int actuator) { int temp = dxl_read_word( dyna.actuators[actuator], P_PRESENT_SPEED_L ); checkForError(actuator,15); int sign = (temp&&1<<10)?-1:1; int ratio = 0x03FF&&temp; return sign * ratio; }
int read_data(char id, char which) { int value=dxl_read_word(id,which); //read requested data int result=dxl_get_result(); //check communication status if(result==COMM_RXSUCCESS) //communication succeded return value; //return read value PrintCommStatus(result); //communication failed, print error message return 5000; //return dummy value }
void getVoltage(void){ int tmp[SERVO_MAX] = {0}; for(int i=0; i<SERVO_MAX; i++ ){ tmp[i] = dxl_get_lowbyte(dxl_read_word( servoId[i], P_PRESENT_VOLTAGE )); } printf( "%d:%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n", EVT_GET_VOLTAGE, tmp[0],tmp[1],tmp[2],tmp[3],tmp[4],tmp[5],tmp[6],tmp[7],tmp[8],tmp[9],tmp[10],tmp[11] ); }
void getLoad(void){ int tmp[SERVO_MAX] = {0}; for(int i=0; i<SERVO_MAX; i++ ){ tmp[i] = dxl_read_word( servoId[i], P_PRESENT_LOAD_L ); } printf( "%d:%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n", EVT_GET_LOAD, tmp[0],tmp[1],tmp[2],tmp[3],tmp[4],tmp[5],tmp[6],tmp[7],tmp[8],tmp[9],tmp[10],tmp[11] ); }
int Dxl::getMovingSpeed(unsigned int device_id) const { const int speed = dxl_read_word(device_id, MOVING_SPEED_L); if(commRXIsOk()) { return speed; } return -1; }
int DynamixelSimpleAPI::getSetting(const int id, const int reg_name, int reg_type, const int device) { int value = -1; if (checkId(id) == true) { // Device detection const int (*cctt)[8] = getRegisterTable(device); if (cctt == NULL) { // Using default control table from this SimpleAPI isntance cctt = ct; } if (cctt) { // Find register's informations (addr, size...) RegisterInfos infos = {-1, -1, -1, -1, -1, -1, -1, -1, -1}; if (getRegisterInfos(cctt, reg_name, infos) == 1) { // Read value if (infos.reg_size == 1) { value = dxl_read_byte(id, infos.reg_addr); } else if (infos.reg_size == 2) { value = dxl_read_word(id, infos.reg_addr); } // Check value if (value < infos.reg_value_min && value > infos.reg_value_max) { value = -1; } } else { TRACE_ERROR(DAPI, "[#%i] getSetting(reg %i / %s) [REGISTER NAME ERROR]\n", id, reg_name, getRegisterNameTxt(reg_name).c_str()); } } else { TRACE_ERROR(DAPI, "[#%i] getSetting(reg %i / %s) [CONTROL TABLE ERROR]\n", id, reg_name, getRegisterNameTxt(reg_name).c_str()); } } else { TRACE_ERROR(DAPI, "[#%i] getSetting(reg %i / %s) [DEVICE ID ERROR]\n", id, reg_name, getRegisterNameTxt(reg_name).c_str()); } return value; }
int dynamixelApi_getPosition(int actuator) { long start = getLocalMsTime(); int presentPos = dxl_read_word( dyna.actuators[actuator], P_PRESENT_POSITION_L); long time = getLocalMsTime()-start; if(time>1) { // printf("time taken to comm = %i ms\n ",getLocalMsTime()-start); } checkForError(actuator,17); return presentPos; }
int Dxl::getCurrentPosition(unsigned int device_id) const { const int position = dxl_read_word(device_id, PRESENT_POSITION_L); if(commRXIsOk()) { return position; } return -1; }
// Read present position short int read_position(short int motor_id) { //short int read_position_code = P_PRESENT_POSITION_L; short int PresentPos; short int CommStatus; PresentPos = dxl_read_word( motor_id, P_PRESENT_POSITION_L ); CommStatus = dxl_get_result(); if( CommStatus != COMM_RXSUCCESS ) { PresentPos+=4096; // set bit to indicate fault //printf("comm err\n"); } return PresentPos; }
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; }
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_; }
void DynamixelSimpleAPI::getMinMaxPositions(const int id, int &min, int &max) { if (checkId(id, false) == true) { int addr_min = getRegisterAddr(ct, REG_MIN_POSITION); int addr_max = getRegisterAddr(ct, REG_MAX_POSITION); min = dxl_read_word(id, addr_min); if (dxl_print_error() == 0) { // Valid positions are in range [0:1023] for most servo series, and [0:4095] for high-end servo series if ((min < 0) || (min > 4095)) { min = -1; } } else { min = -1; } max = dxl_read_word(id, addr_max); if (dxl_print_error() == 0) { // Valid positions are in range [0:1023] for most servo series, and [0:4095] for high-end servo series if ((max < 0) || (max > 4095)) { max = -1; } } else { max = -1; } } }
void getAngle(){ int tmp[SERVO_MAX] = {0}; for(int i=0; i<SERVO_MAX; i++ ){ tmp[i] = dxl_read_word( servoId[i], P_PRESENT_POSITION_L ); } // printf( "%d:%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n", // EVT_GET_NOW_ANGLE, tmp[0],tmp[1],tmp[2],tmp[3],tmp[4],tmp[5],tmp[6],tmp[7],tmp[8],tmp[9],tmp[10],tmp[11] ); // Legs // printf( "Legs... %d:{%d, %d, %d, %d, %d, %d, %d, %d}\n", // EVT_GET_NOW_ANGLE, tmp[0],tmp[1],tmp[2],tmp[3],tmp[4],tmp[5],tmp[6],tmp[7] ); // Neck printf( "Neck... %d:{%d, %d, %d, %d, %d, %d, %d}\n", EVT_GET_NOW_ANGLE, tmp[8],tmp[9],tmp[10],tmp[11],tmp[12],tmp[13],tmp[14]); }
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 DynamixelSimpleAPI::readModelNumber(const int id) { int value = -1; if (checkId(id, false) == true) { int addr = getRegisterAddr(ct, REG_MODEL_NUMBER); value = dxl_read_word(id, addr); if (dxl_print_error() != 0) { value = -1; } } return value; }
bool ControlUtils::getJoints(double *a) { for (int i = 0; i < TOTAL_JOINTS; i++) { // Read present position ticks_from[i] = dxl_read_word(_id[i], ADDR_PRESENT_POSITION_L); int CommStatus = dxl_get_result(); if(CommStatus == COMM_RXSUCCESS) a[i] = tick2rad(ticks_from[i], i); else return false; //usleep(5000); } return true; }
bool ControlUtils::getLegJointsCircular(double a[TOTAL_JOINTS]) { int ctr = 0; while(ctr < 2) { ticks_from[_legIdx] = dxl_read_word(_id[_legIdx], ADDR_PRESENT_POSITION_L); int CommStatus = dxl_get_result(); if(CommStatus == COMM_RXSUCCESS) a[_legIdx] = tick2rad(ticks_from[_legIdx], _legIdx); else return false; _legIdx++; if (_legIdx > R_AAA) _legIdx = L_HZ; ctr++; } return true; }
bool dynamixelApi_connect(int actId) { dxl_ping(actId); if (dxl_get_result() == COMM_RXSUCCESS) { ase_printf("SUCCESS: Found Dynamixel with id = %i renamed to %i \n", actId, dyna.nActuators); int wValue = dxl_read_word(actId, P_MODEL_NUMBER_L); if (dxl_get_result() == COMM_RXSUCCESS) ase_printf("Model number:%d, ", wValue); int bValue = dxl_read_byte(actId, P_VERSION); if (dxl_get_result() == COMM_RXSUCCESS) ase_printf("Version:%d\n", bValue); dyna.actuators[dyna.nActuators] = actId; dynamixelApi_setWheelMode(dyna.nActuators, false); dyna.nActuators++; return true; } else { ase_printf("ERROR: Unable to connect to Dynamixel with id = %i\n", actId); return false; } }
int DynamixelInterface::readWord(int id,int address) { if (!DXL2USB.isOk())throw DXL_ComError(-1,id,__FILE__,__LINE__); int retry=DynamixelInterface::RETRIES; int status=0; int value=0; do { value=dxl_read_word(id,address); retry--; status=dxl_get_result(); // cout << status << endl; if (!DXL_ComError::isOK(status)) { // dxl_terminate(); usleep(10000); // dxl_initialize(DEVICEINDEX,BAUDNUM); } }while (!DXL_ComError::isOK(status) && retry>0); if (!DXL_ComError::isOK(status)) { unsigned char b=dxl_read_byte(id,DXL_ALARM_SHUTDOWN_BYTE); cout << "Alarm Shutdown Byte:"<<((int)b)<<endl; throw DXL_ComError(status,id,__FILE__,__LINE__); } return value; }