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
}
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;
}
//------------------------------------------------------------------------------
//
// 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();
}
//##############################################################################
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]);
}
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();
}
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
}
// 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;
}
// 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];
}
// 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]);
}
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();
}
// 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;
}
// 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);
}
}
// 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;
}
}
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;
}
void Dynamixel::txrxPacket(void){
dxl_txrx_packet();
}
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;
}