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;
}
}
}
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;
}
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();
}
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 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 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);
}
}
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
}
uint8_t PingDevice(uint8_t id) {
dxl_ping(id);
if(dxl_get_result() == COMM_RXSUCCESS) {
return 1;
} else {
printf("E:Device %d not connected\n\r",id);
return 0;
}
}
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
}
bool Dxl::commRXIsOk() const
{
const int status = dxl_get_result();
if(status == COMM_RXSUCCESS)
{
return true;
}
return false;
}
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;
}
}
void ServoDriver::IdleTime(void)
{
// Each time we call this set servos LED on or off...
g_iIdleServoNum++;
if (g_iIdleServoNum >= NUMSERVOS) {
g_iIdleServoNum = 0;
g_iIdleLedState = 1 - g_iIdleLedState;
}
dxl_write_byte((cPinTable[g_iIdleServoNum]), AX_LED, g_iIdleLedState);
dxl_get_result(); // don't care for now
}
bool ControlUtils::setByte(int8_t val, int8_t addr, int joint)
{
dxl_write_byte(_id[joint], addr, val);
if (dxl_get_result() == COMM_RXSUCCESS) {
return true;
}
else {
printf("faled to write byte to %d at %d\n", addr, joint);
return false;
}
}
bool ControlUtils::getByte(int8_t *val, int8_t addr, int joint)
{
int8_t tmp = dxl_read_byte(_id[joint], addr);
if(dxl_get_result() == COMM_RXSUCCESS) {
*val = tmp;
return true;
}
else {
printf("faled to get byte from %d at %d\n", addr, joint);
return false;
}
}
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;
}
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_
}
// 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(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 wheelMode(int id,int speed) {
// if (!wheel) {
dxl_write_word(id,P_CCW_ANGLE_LIMIT,0);
int result = dxl_get_result( );
if( result == COMM_TXSUCCESS ) printf("CCW Good\n");
dxl_write_word(id,P_CW_ANGLE_LIMIT,0);
if( result == COMM_TXSUCCESS ) printf("CW Good\n");
// wheel=1;
// joint=0;
// }
dxl_write_word(id,P_MOVING_SPEED,speed);
if( result == COMM_TXSUCCESS ) printf("Speed Good\n");
}
void DynamixelInterface::sendWord(int id,int address,int word) {
if (!DXL2USB.isOk()) throw DXL_ComError(-1,id,__FILE__,__LINE__);
int retry=DynamixelInterface::RETRIES;
int status=0;
do {
dxl_write_word(id,address,word);
retry--;
status=dxl_get_result();
cout << status << endl;
if (!DXL_ComError::isOK(status)) usleep(10000);
}while (!DXL_ComError::isOK(status) && retry>0);
if (!DXL_ComError::isOK(status)) throw DXL_ComError(status,0,__FILE__,__LINE__);
}
void straighten(int id[], int length) {
int i;
for (i = 0; i < length; i++) {
dxl_write_word(id[i], GOAL_POSITION_L, 512);
if (dxl_get_result() != COMM_RXSUCCESS) {
dxl_write_byte(id[i], LED, 1);
} else {
dxl_write_byte(id[i], LED, 0);
}
}
}
//--------------------------------------------------------------------
// Cleanup
//--------------------------------------------------------------------
void ServoDriver::Cleanup(void) {
// Do any cleanup that the driver may need.
printf("ServoDriver::Cleanup\n\r");
#ifdef USB2AX_REG_VOLTAGE
ax12SetRegister(AX_ID_DEVICE, USB2AX_REG_VOLTAGE, 0); // Turn off the voltage testing...
#endif
// Turn off all of the servo LEDS... Maybe use broadcast?
for (int iServo=0; iServo < NUMSERVOS; iServo++) {
dxl_write_byte((cPinTable[iServo]), AX_LED, 0);
dxl_get_result(); // don't care for now
}
bioloid.end(); // tell the driver to abort
}
static void checkForError(int actuator,int callerID) {
if( dxl_get_result() != COMM_RXSUCCESS ) {
ase_printf( "ERROR: Dynamixel #%i, ID=%i: ", callerID, dyna.actuators[actuator]);
char* errorType;
switch( dxl_get_result() ) {
case COMM_TXFAIL:
errorType = "COMM_TXFAIL";
break;
case COMM_TXERROR:
errorType = "COMM_TXERROR" ;
break;
case COMM_RXFAIL:
errorType = "COMM_RXFAIL";
break;
case COMM_RXWAITING:
errorType = "COMM_RXWAITING";
break;
case COMM_RXTIMEOUT:
errorType = "COMM_RXTIMEOUT";
break;
case COMM_RXCORRUPT:
errorType = "COMM_RXCORRUPT";
break;
default:
printf("Unknown error!!!\n");
break;
}
ase_printf(errorType);
ase_printf("\n");
Event_t event; event.val_prt = errorType;
EventManager_publish(DYNAMIXEL_ERROR_EVENT, &event);
}
}
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;
}
void jointMode(int id,int angle) {
int pos=((180+angle)*4095)/360;
// if (!joint) {
dxl_write_word(id,P_CCW_ANGLE_LIMIT,4095);
int result = dxl_get_result( );
if( result == COMM_TXSUCCESS ) printf("CCW Good\n");
dxl_write_word(id,P_CW_ANGLE_LIMIT,0);
if( result == COMM_TXSUCCESS ) printf("CW Good\n");
// joint=1;
// wheel=0;
// }
printf("Joint %d is at %d\n",id,pos);
dxl_write_word(id,P_GOAL_POSITION,pos);
if( result == COMM_TXSUCCESS ) printf("Position Good\n");
}
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;
}
unsigned char DynamixelInterface::readByte(int id,int address) {
if (!DXL2USB.isOk())throw DXL_ComError(-1,id,__FILE__,__LINE__);
int retry=DynamixelInterface::RETRIES;
int status=0;
unsigned char value=0;
do {
value=dxl_read_byte(id,address);
retry--;
status=dxl_get_result();
if (!DXL_ComError::isOK(status)) usleep(10000);
}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;
}
// initialize for walking - assume walk ready pose
void walk_init()
{
int commStatus = 0;
// reset walk state and command
walk_state = 0;
walk_command = 0;
// and get ready for walking!
current_motion_page = COMMAND_WALK_READY_MP;
executeMotion(current_motion_page);
// experimental - increase punch for walking
commStatus = dxl_write_word(BROADCAST_ID, DXL_PUNCH_L, 100);
if(commStatus != COMM_RXSUCCESS) {
printf("\nDXL_PUNCH Broadcast - ");
dxl_printCommStatus(dxl_get_result());
}
}
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;
}
