void setMovement(int servo_id,const int angleMin,const int angleMax){
int GoalPos[2] = {angleMin,angleMax};
int index =0;
int Moving,CommStatus;
while(1){
// Write goal position
dxl_write_word( servo_id, P_GOAL_POSITION_L, GoalPos[index] );
do
{
// Check moving done
Moving = dxl_read_byte( servo_id, P_MOVING );
CommStatus = dxl_get_result();
if( CommStatus == COMM_RXSUCCESS )
{
if( Moving == 0 )
{
// Change goal position
if( index == 0 )
index = 1;
else
index = 0;
}
}
else
{
PrintCommStatus(CommStatus);
break;
}
}while(Moving == 1);
}
}
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);
}
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
}
void isMoving(const int servo_id,int *result,int *CommStatus){
*result = dxl_read_byte( servo_id, P_MOVING);
*CommStatus = dxl_get_result();
if(*CommStatus != COMM_RXSUCCESS)
PrintCommStatus(*CommStatus);
PrintErrorCode();
}
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 motor_set_position(char id, uint16_t motor_position, char blocking) {
dxl_write_word(id, GOAL_POSITION_L, motor_position); //set position
int CommStatus = dxl_get_result(); //check communication
if (CommStatus == COMM_RXSUCCESS) {
if (blocking == MOTOR_MOVE_BLOCKING) //block until position reached
motor_wait_finish(id, motor_position);
}
else
PrintCommStatus(CommStatus);//communication failed, print error code
}
void MotorControl(int id, int power) {
#ifndef _MOTOR_OFF_
int CommStatus = COMM_RXSUCCESS;
// printf( "%d %d\n", id, power );
dxl_write_word( id, P_GOAL_SPEED_L, power );
CommStatus = dxl_get_result();
if( CommStatus == COMM_RXSUCCESS )
PrintErrorCode();
else
PrintCommStatus(CommStatus);
#endif // _MOTOR_OFF_
}
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;
}
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 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);
}
}
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_;
}
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);
}
}
int main()
{
int baudnum = 34;
int GoalPos[2] = {0, 7095};
//int GoalPos[2] = {0, 4095}; // for Ex series
int index = 1;
int deviceIndex = 0;
int Moving, 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" );
while(1)
{
printf( "Press Enter key to continue!(press ESC and Enter to quit)\n" );
if(getchar() == 0x1b)
break;
// Write goal position
dxl_write_word( DEFAULT_ID, P_GOAL_POSITION_L, GoalPos[index] );
do
{
// Read present position
PresentPos = dxl_read_word( DEFAULT_ID, P_PRESENT_POSITION_L );
CommStatus = dxl_get_result();
if( CommStatus == COMM_RXSUCCESS )
{
printf( "%d %d\n",GoalPos[index], PresentPos );
PrintErrorCode();
}
else
{
PrintCommStatus(CommStatus);
break;
}
// Check moving done
Moving = dxl_read_byte( DEFAULT_ID, P_MOVING );
CommStatus = dxl_get_result();
if( CommStatus == COMM_RXSUCCESS )
{
if( Moving == 0 )
{
// Change goal position
if( index == 0 )
index = 1;
else
index = 0;
}
PrintErrorCode();
}
else
{
PrintCommStatus(CommStatus);
break;
}
}while(Moving == 1);
}
// Close device
dxl_terminate();
printf( "Press Enter key to terminate...\n" );
getchar();
return 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;
}
void getPosition(const int servo_id,int *pos,int *CommStatus){
*pos = dxl_read_word( servo_id, P_PRESENT_POSITION_L );
*CommStatus = dxl_get_result();
if(*CommStatus != COMM_RXSUCCESS)
PrintCommStatus(*CommStatus);
}
