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
}
Exemple #2
0
bool ControlUtils::syncWriteWord(int8_t addr, const std::vector<int> &joints, const std::vector<int16_t> &val)
{
  if (joints.size() != val.size())
    return false;
  
  if (joints.empty())
    return true;

  // Make syncwrite packet
  dxl_set_txpacket_id(BROADCAST_ID);
  dxl_set_txpacket_instruction(INST_SYNC_WRITE);
  dxl_set_txpacket_parameter(0, addr);
  dxl_set_txpacket_parameter(1, 2);
  for (size_t i = 0; i < joints.size(); i++) {
    dxl_set_txpacket_parameter(2+3*i, _id[joints[i]]);
    
    dxl_set_txpacket_parameter(2+3*i+1, dxl_get_lowbyte(val[i]));
    dxl_set_txpacket_parameter(2+3*i+2, dxl_get_highbyte(val[i]));
  }
  dxl_set_txpacket_length((2+1)*joints.size()+4);

  dxl_txrx_packet();
  if(dxl_get_result() == COMM_RXSUCCESS)
    return true;
  else
    return false; 
}
Exemple #3
0
//------------------------------------------------------------------------------
// 
//  Data Format for SYNC write:
//  			ID		0xFE
//			Length		((L+1)*N )+4  	L: Data Length per RX-28, N: Number of motors
//			Instruction	0x83
//			Start address to write data	parameter 1
//			Length of data to write		parameter 2 = Number of motors
//			
//			First ID of RX-28		parameter 3
//			First data of the 1st RX-28
//			..
//			Lth data of the 1st RX-28 
//
//     #define ID				(2)
//     #define LENGTH				(3)
//     #define INSTRUCTION			(4)
//     #define ERRBIT				(4)
//     #define PARAMETER			(5)
//     #define DEFAULT_BAUDNUMBER		(1)
//
int16_t MOTOR_SendPositionCommandsToMotors(void)
{
  int16_t i;
  int16_t numJointsSent = 0;
  
  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, 4); // Write 4 bytes
  
  for( i=0; i<MOTOR_NJOINTS; i++ )
  {
      if ( !joint[i].bPositionSet )
      {
          // Don't send a joint value if we haven't got a command
          continue;
      }
      
	  dxl_set_txpacket_parameter(2+(5*numJointsSent), i);
	  dxl_set_txpacket_parameter(2+(5*numJointsSent)+1, dxl_get_lowbyte(joint[i].pos));
	  dxl_set_txpacket_parameter(2+(5*numJointsSent)+2, dxl_get_highbyte(joint[i].pos));
	  dxl_set_txpacket_parameter(2+(5*numJointsSent)+3, dxl_get_lowbyte(joint[i].speed));
	  dxl_set_txpacket_parameter(2+(5*numJointsSent)+4, dxl_get_highbyte(joint[i].speed));
      
      numJointsSent++;
  }
  
  dxl_set_txpacket_length((5*numJointsSent)+4);
  
  dxl_txrx_packet();
  return MOTOR_SUCCESS;
}
void dxl_sync_write_send()
{
	while(giBusUsing); // needs to be done before touching the TX buffer as it is used until the end of RX.	
	
    gbInstructionPacket[LENGTH] = gbSyncNbParam + 2;
    
	dxl_txrx_packet();
}
void dxl_ping( int id )
{
	while(giBusUsing);

	gbInstructionPacket[ID] = (unsigned char)id;
	gbInstructionPacket[INSTRUCTION] = INST_PING;
	gbInstructionPacket[LENGTH] = 2;
	
	dxl_txrx_packet();
}
void dxl_write_byte( int id, int address, int value )
{
	while(giBusUsing);

	gbInstructionPacket[ID] = (unsigned char)id;
	gbInstructionPacket[INSTRUCTION] = INST_WRITE;
	gbInstructionPacket[PARAMETER] = (unsigned char)address;
	gbInstructionPacket[PARAMETER+1] = (unsigned char)value;
	gbInstructionPacket[LENGTH] = 4;
	
	dxl_txrx_packet();
}
void dxl_write_word( int id, int address, int value )
{
	while(giBusUsing);

	gbInstructionPacket[ID] = (unsigned char)id;
	gbInstructionPacket[INSTRUCTION] = INST_WRITE;
	gbInstructionPacket[PARAMETER] = (unsigned char)address;
	gbInstructionPacket[PARAMETER+1] = (unsigned char)dxl_get_lowbyte(value);
	gbInstructionPacket[PARAMETER+2] = (unsigned char)dxl_get_highbyte(value);
	gbInstructionPacket[LENGTH] = 5;
	
	dxl_txrx_packet();
}
Exemple #8
0
//##############################################################################
uint8_t dxl_recover(uint8_t id, HaViMo2_Region_Buffer_t* hvm2rb)
{
    if (hvm2rb==0)
        return 0xFF;

    while (giBusUsing);
	hvm2rb->valid=0;
    uint8_t i;
    for (i=0; i<15; i++)
    {
        gbInstructionPacket[ID] = id;
        gbInstructionPacket[INSTRUCTION] = INST_READ;
        gbInstructionPacket[PARAMETER] = ((i+1)*16);
        gbInstructionPacket[PARAMETER+1] = 16;
        gbInstructionPacket[LENGTH] = 4;

        dxl_txrx_packet();

        if (gbStatusPacket[LENGTH]==(16+2))
        {
        	hvm2rb->valid++;
        }
        else
        {
            return hvm2rb->valid;
        }

        hvm2rb->rb[i].Index=gbStatusPacket[PARAMETER];
        hvm2rb->rb[i].Color=gbStatusPacket[PARAMETER+1];
        hvm2rb->rb[i].NumPix=(
                (uint16_t)gbStatusPacket[PARAMETER+2]+
                ((uint16_t)gbStatusPacket[PARAMETER+3]<<8));
        hvm2rb->rb[i].SumX=
                (
                    ((uint32_t)gbStatusPacket[PARAMETER+4]+
                    ((uint32_t)gbStatusPacket[PARAMETER+5]<<8)+
                    ((uint32_t)gbStatusPacket[PARAMETER+6]<<16))
                );
        hvm2rb->rb[i].SumY=
                (
                    ((uint32_t)gbStatusPacket[PARAMETER+8]+
                    ((uint32_t)gbStatusPacket[PARAMETER+9]<<8)+
                    ((uint32_t)gbStatusPacket[PARAMETER+10]<<16))
                );
        hvm2rb->rb[i].MaxX=gbStatusPacket[PARAMETER+12];
        hvm2rb->rb[i].MinX=gbStatusPacket[PARAMETER+13];
        hvm2rb->rb[i].MaxY=gbStatusPacket[PARAMETER+14];
        hvm2rb->rb[i].MinY=gbStatusPacket[PARAMETER+15];
    }
    return hvm2rb->valid;
}
int dxl_read_word( int id, int address )
{
	while(giBusUsing);

	gbInstructionPacket[ID] = (unsigned char)id;
	gbInstructionPacket[INSTRUCTION] = INST_READ;
	gbInstructionPacket[PARAMETER] = (unsigned char)address;
	gbInstructionPacket[PARAMETER+1] = 2;
	gbInstructionPacket[LENGTH] = 4;
	
	dxl_txrx_packet();

	return dxl_makeword((int)gbStatusPacket[PARAMETER], (int)gbStatusPacket[PARAMETER+1]);
}
Exemple #10
0
void SendServoTargetPos(uint8_t ServoNo, int16_t targetPos,uint16_t targetSpeed)
{
	//printf("setting servo %d pos to %d @ %d\n", ServoNo, targetPos, targetSpeed);
	dxl_set_txpacket_id(ServoNo);
	dxl_set_txpacket_instruction(INST_WRITE);
	dxl_set_txpacket_parameter(0, P_GOAL_POSITION_L);
	dxl_set_txpacket_parameter(1, LO_BYTE(targetPos));
	dxl_set_txpacket_parameter(2, HI_BYTE(targetPos));
	dxl_set_txpacket_parameter(3, LO_BYTE(targetSpeed));
	dxl_set_txpacket_parameter(4, HI_BYTE(targetSpeed));
	dxl_set_txpacket_length(7);

	dxl_txrx_packet();
}
Exemple #11
0
int dxl_read_byte( int id, int address )
{
	while(giBusUsing);
//	TxDString("E");

	gbInstructionPacket[ID] = (unsigned char)id;
	gbInstructionPacket[INSTRUCTION] = INST_READ;
	gbInstructionPacket[PARAMETER] = (unsigned char)address;
	gbInstructionPacket[PARAMETER+1] = 1;
	gbInstructionPacket[LENGTH] = 4;
	
	dxl_txrx_packet();

	return (int)gbStatusPacket[PARAMETER];
}
//--------------------------------------------------------------------
//[SetRegOnAllServos] Function that is called to set the state of one
//  register in all of the servos, like Torque on...
//--------------------------------------------------------------------
void SetRegOnAllServos(uint8_t bReg, uint8_t bVal)
{
    dxl_set_txpacket_id(AX_ID_BROADCAST);
    dxl_set_txpacket_instruction(AX_CMD_SYNC_WRITE);

    dxl_set_txpacket_parameter(0, bReg);    // which register
    dxl_set_txpacket_parameter(1, 1);       // 1 byte
    dxl_set_txpacket_length(2*NUMSERVOS+3);
    
    for (byte i = 0; i < NUMSERVOS; i++) {
        dxl_set_txpacket_parameter(2+i*2, (cPinTable[i]));       // 1 byte
        dxl_set_txpacket_parameter(3+i*2, bVal);       // 1 byte
    }
    dxl_txrx_packet();
    //return dxl_get_result();   // don't care for now
}
Exemple #13
0
// Function to write data into the control table of the Dynamixel actuator
// Length N+3 (N is the number of data to be written)
// Instruction 0x03
// Parameter1 Starting address of the location where the data is to be written
// Parameter2 1st data to be written
// Parameter3 2nd data to be written, etc.
// In this case we only have a 1-byte parameter
int dxl_write_byte( int id, int address, int value )
{
	// wait for the bus to be free
	while(giBusUsing);

	// create a WRITE instruction packet and send
	gbInstructionPacket[ID] = (unsigned char)id;
	gbInstructionPacket[INSTRUCTION] = INST_WRITE;
	gbInstructionPacket[PARAMETER] = (unsigned char)address;
	gbInstructionPacket[PARAMETER+1] = (unsigned char)value;
	gbInstructionPacket[LENGTH] = 4;
	
	dxl_txrx_packet();
	
	return gbCommStatus;
}
Exemple #14
0
// Read data from the control table of a Dynamixel device
// Length 0x04, Instruction 0x02
// Parameter1 Starting address of the location where the data is to be read
// Parameter2 Length of the data to be read (one byte in this case)
int dxl_read_byte( int id, int address )
{
	// wait for the bus to be free
	while(giBusUsing);

	// create a READ instruction packet and send
	gbInstructionPacket[ID] = (unsigned char)id;
	gbInstructionPacket[INSTRUCTION] = INST_READ;
	gbInstructionPacket[PARAMETER] = (unsigned char)address;
	gbInstructionPacket[PARAMETER+1] = 1;
	gbInstructionPacket[LENGTH] = 4;
	
	dxl_txrx_packet();

	return (int)gbStatusPacket[PARAMETER];
}
Exemple #15
0
// Read data from the control table of a Dynamixel device
// Length 0x04, Instruction 0x02
// Parameter1 Starting address of the location where the data is to be read
// Parameter2 Length of the data to be read (2 bytes in this case)
int dxl_read_word( int id, int address )
{
	// wait for the bus to be free
	while(giBusUsing);

	// create a READ instruction packet and send
	gbInstructionPacket[ID] = (unsigned char)id;
	gbInstructionPacket[INSTRUCTION] = INST_READ;
	gbInstructionPacket[PARAMETER] = (unsigned char)address;
	gbInstructionPacket[PARAMETER+1] = 2;
	gbInstructionPacket[LENGTH] = 4;
	
	dxl_txrx_packet();

	// combine the 2 bytes into a word and return
	return dxl_makeword((int)gbStatusPacket[PARAMETER], (int)gbStatusPacket[PARAMETER+1]);
}
Exemple #16
0
void dxl_SetPosition(int ServoID, int Position, int Speed)
    //    Sets the target position and speed of the specified servo
	////Made by Martin S. Mason(Professor @Mt. San Antonio College)
{
    while(giBusUsing);

    gbInstructionPacket[ID] = (unsigned char)ServoID;
    gbInstructionPacket[INSTRUCTION] = INST_WRITE;
    gbInstructionPacket[PARAMETER] = (unsigned char)30;
    gbInstructionPacket[PARAMETER+1] = (unsigned char)dxl_get_lowbyte(Position);
    gbInstructionPacket[PARAMETER+2] = (unsigned char)dxl_get_highbyte(Position);
    gbInstructionPacket[PARAMETER+3] = (unsigned char)dxl_get_lowbyte(Speed);
    gbInstructionPacket[PARAMETER+4] = (unsigned char)dxl_get_highbyte(Speed);
    gbInstructionPacket[LENGTH] = 7;

    dxl_txrx_packet();
}
Exemple #17
0
// Function for controlling several Dynamixel actuators at the same time. 
// The communication time decreases by using the Sync Write instruction 
// since many instructions can be transmitted by a single instruction. 
// However, you can use this instruction only when the lengths and addresses 
// of the control table to be written to are the same. 
// The broadcast ID (0xFE) needs to be used for transmitting.
// ID: 0xFE
// Length: (L + 1) * N + 4 (L: Data length for each Dynamixel actuator, N: The number of Dynamixel actuators)
// Instruction: 0x83
// Parameter1 Starting address of the location where the data is to be written
// Parameter2 The length of the data to be written (L)
// Parameter3 The ID of the 1st Dynamixel actuator
// Parameter4 The 1st data for the 1st Dynamixel actuator
// Parameter5 The 2nd data for the 1st Dynamixel actuator
// ParameterL+4 The ID of the 2nd Dynamixel actuator
// ParameterL+5 The 1st data for the 2nd Dynamixel actuator
// ParameterL+6 The 2nd data for the 2nd Dynamixel actuator
// …
// NOTE: this function only allows 2 bytes of data per actuator
int dxl_sync_write_word( int NUM_ACTUATOR, int address, const uint8 ids[], int16 values[] )
{
	int i = 0;

	// wait for the bus to be free
	while(giBusUsing);

	// check how many actuators are to be broadcast to
	if (NUM_ACTUATOR == 0) {
		// nothing to do, return
		return 0;
	} else if (NUM_ACTUATOR == 1) {
		// easy, we can use dxl_write_word for a single actuator
		dxl_write_word( ids[0], address, values[0] );
		return 0;
	}
	
	// Multiple values, create sync write packet
	// ID is broadcast id
	dxl_set_txpacket_id(BROADCAST_ID);
	// Instruction is sync write
	dxl_set_txpacket_instruction(INST_SYNC_WRITE);
	// Starting address where to write to
	dxl_set_txpacket_parameter(0, address);
	// Length of data to be written (each word = 2 bytes)
	dxl_set_txpacket_parameter(1, 2);
	// Loop over the active Dynamixel id's  
	for( i=0; i<NUM_ACTUATOR; i++ )
	{
		// retrieve the id and value for each actuator and add to packet
		dxl_set_txpacket_parameter(2+3*i, ids[i]);
		dxl_set_txpacket_parameter(2+3*i+1, dxl_get_lowbyte(values[i]));
		dxl_set_txpacket_parameter(2+3*i+2, dxl_get_highbyte(values[i]));
	}
	
	// total length is as per formula above with L=2
	dxl_set_txpacket_length((2+1)*NUM_ACTUATOR + 4);
	
	// all done, send the packet
	dxl_txrx_packet();
	
	// there is no status packet return, so return the CommStatus
	return gbCommStatus;
}
Exemple #18
0
// Function setting goal and speed for all Dynamixel actuators at the same time  
// Uses the Sync Write instruction (also see dxl_sync_write_word) 
// Inputs:	NUM_ACTUATOR - number of Dynamixel servos
//			ids - array of Dynamixel ids to write to
//			goal - array of goal positions
//			speed - array of moving speeds
//Returns:	commStatus
int dxl_set_goal_speed( int NUM_ACTUATOR, const uint8 ids[], uint16 goal[], uint16 speed[] )
{
	int i = 0;

	// wait for the bus to be free
	while(giBusUsing);

	// check how many actuators are to be broadcast to
	if (NUM_ACTUATOR == 0) {
		// nothing to do, return
		return 0;
	} 
	
	// Multiple values, create sync write packet
	// ID is broadcast id
	dxl_set_txpacket_id(BROADCAST_ID);
	// Instruction is sync write
	dxl_set_txpacket_instruction(INST_SYNC_WRITE);
	// Starting address where to write to
	dxl_set_txpacket_parameter(0, DXL_GOAL_POSITION_L);
	// Length of data to be written (2 words = 4 bytes)
	dxl_set_txpacket_parameter(1, 4);
	// Loop over the active Dynamixel id's  
	for( i=0; i<NUM_ACTUATOR; i++ )
	{
		// retrieve the id and value for each actuator and add to packet
		dxl_set_txpacket_parameter(2+5*i, ids[i]);
		dxl_set_txpacket_parameter(2+5*i+1, dxl_get_lowbyte(goal[i]));
		dxl_set_txpacket_parameter(2+5*i+2, dxl_get_highbyte(goal[i]));
		dxl_set_txpacket_parameter(2+5*i+3, dxl_get_lowbyte(speed[i]));
		dxl_set_txpacket_parameter(2+5*i+4, dxl_get_highbyte(speed[i]));
	}
	
	// total length is as per formula above with L=4
	dxl_set_txpacket_length((4+1)*NUM_ACTUATOR + 4);
	
	// all done, send the packet
	dxl_txrx_packet();
	
	// there is no status packet return, so return the CommStatus
	return gbCommStatus;
}
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);
	}
}
Exemple #20
0
// Ping a Dynamixel device
int dxl_ping( int id )
{
	// wait for the bus to be free
	while(giBusUsing);

	// create a PING instruction packet and send
	gbInstructionPacket[ID] = (unsigned char)id;
	gbInstructionPacket[INSTRUCTION] = INST_PING;
	gbInstructionPacket[LENGTH] = 2;
	
	dxl_txrx_packet();
	
	if (gbCommStatus == COMM_RXSUCCESS)
	{
		// return the error code
		return (int)gbStatusPacket[ERRBIT];
	// check if servo exists (via timeout)
	} else if( gbCommStatus == COMM_RXTIMEOUT )
	{
		return -1;
	} else {
		return 0;
	}
}
Exemple #21
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;
}
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);
	}
}
void DXLJointInterface::process()
{
	switch(controlType())
	{
		case CT_POSITION:
		{
			int goalTicks = m_goalPosition / TICKS_TO_RAD;
			dxl_write_word(id(), REG_GOAL_POSITION, goalTicks + m_tickOffset);
		}
			break;
		case CT_VELOCITY:
		{
			int velTicks = m_goalSpeed / VEL_TO_RAD_PER_S;
			if(velTicks < 0)
			{
				velTicks = (1 << 10) | (-velTicks);
			}
			dxl_write_word(id(), REG_MOVING_SPEED, velTicks);
		}
			break;
		case CT_TORQUE:
		{
			int torqueTicks = m_goalTorque / TORQUE_TO_NM;
			if(torqueTicks < 0)
			{
				torqueTicks = (1 << 10) | (-torqueTicks);
			}
			if(torqueTicks == 0)
				torqueTicks = 1;
			dxl_write_word(id(), REG_GOAL_TORQUE, torqueTicks);
		}
			break;
	}

// 	int torqueTicks = 1023.0 * (m_torqueLimit / 4.4); // at 12V
// 	if(torqueTicks > 1023)
// 		torqueTicks = 1023;
// 	printf("torque: %4d\n", torqueTicks);
//
// 	dxl_write_word(id(), 34, torqueTicks);

	dxl_set_txpacket_id(id());
	dxl_set_txpacket_instruction(INST_READ);
	dxl_set_txpacket_length(2 + 2);
	dxl_set_txpacket_parameter(0, 36); // start address: present pos
	dxl_set_txpacket_parameter(1, 6);  // #bytes

	dxl_txrx_packet();

	if(dxl_get_rxpacket_length() != 6+2)
	{
		fprintf(stderr, "DXL: Got malformed read answer\n");
		return;
	}

	int currentTicks = (dxl_get_rxpacket_parameter(1) << 8) | dxl_get_rxpacket_parameter(0);
	m_currentPosition = TICKS_TO_RAD * (currentTicks - m_tickOffset);

	double lastVel = m_currentVelocity;
	int velTicks = (dxl_get_rxpacket_parameter(3) << 8) | dxl_get_rxpacket_parameter(2);
	if(velTicks & (1 << 10))
		velTicks = -(velTicks & 1023);
	m_currentVelocity = VEL_TO_RAD_PER_S * velTicks;

	int loadTicks = (dxl_get_rxpacket_parameter(5) << 8) | dxl_get_rxpacket_parameter(4);
	if(loadTicks & (1 << 10))
		loadTicks = -(loadTicks & 1023);
	m_currentLoad = (1.0 / 1023.0) * loadTicks;

	int current = dxl_read_word(id(), 68) - 2048;
	m_currentCurrent = 0.0045 * current;

	m_currentAcc = (m_currentVelocity - lastVel) / 0.02;
}
Exemple #25
0
void Dynamixel::txrxPacket(void){
	dxl_txrx_packet();
}
Exemple #26
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;
}