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