Exemplo n.º 1
0
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);
}
Exemplo n.º 3
0
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;
}
Exemplo n.º 8
0
//===================================================================================================
// 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);

}
Exemplo n.º 9
0
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;
}
Exemplo n.º 12
0
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
}
Exemplo n.º 13
0
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] );
}
Exemplo n.º 14
0
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] );
}
Exemplo n.º 15
0
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;
}
Exemplo n.º 18
0
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;
}
Exemplo n.º 19
0
// 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;
}
Exemplo n.º 20
0
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;
}
Exemplo n.º 21
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;
}
Exemplo n.º 22
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;
        }
    }
}
Exemplo n.º 24
0
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]);

}
Exemplo n.º 25
0
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;
}
Exemplo n.º 27
0
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;
}
Exemplo n.º 28
0
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;
	}
}
Exemplo n.º 30
0
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;
}