int DynamixelSimpleAPI::setMinMaxPositions(const int id, const int min, const int max)
{
int status = 0;
if (checkId(id) == true)
{
// Valid positions are in range [0:1023] for most servo series, and [0:4095] for high-end servo series
if ((min < 0) || (min > 4095) || (max < 0) || (max > 4095))
{
TRACE_ERROR(DAPI, "[#%i] Cannot set new min/max positions '%i/%i' for this servo: out of range\n", id, min, max);
}
else
{
int addr_min = getRegisterAddr(ct, REG_MIN_POSITION);
int addr_max = getRegisterAddr(ct, REG_MAX_POSITION);
// Write min value
dxl_write_word(id, addr_min, min);
if (dxl_print_error() == 0)
{
status = 1;
}
// Write max value
dxl_write_word(id, addr_max, max);
if (dxl_print_error() == 0)
{
status = 1;
}
}
}
return status;
}
bool DXLJointInterface::init()
{
if(!dxl_initialize(0, BAUD_NUM))
return false;
dxl_write_byte(200, 24, 1);
// sleep(1);
// dxl_ping(id());
// if(dxl_get_result() != COMM_RXSUCCESS)
// {
// ROS_ERROR("Could not find dynamixel with ID %d", id());
// return false;
// }
dxl_write_word(id(), REG_TORQUE_LIMIT, 1023);
dxl_write_word(id(), REG_CW_ANGLE_LIMIT, 0);
setControlType(CT_POSITION);
dxl_write_word(id(), REG_TORQUE_ENABLE, 1);
dxl_write_byte(id(), REG_P_GAIN, 2);
dxl_write_byte(id(), REG_I_GAIN, 0);
dxl_write_byte(id(), REG_D_GAIN, 0);
return true;
}
void DynamixelServo::setAngle(float ang, float velocity)
{
if (ang > max_angle_ || ang < min_angle_)
{
ROS_WARN_STREAM("Servo " << name_ << ": angle out of range (" << ang << ").Limits are: " << min_angle_ << "," << max_angle_);
ang=std::min(ang,max_angle_);
ang=std::max(ang,min_angle_);
}
if (velocity > max_speed_)
{
ROS_WARN_STREAM("Servo " << name_ << ": velocity out of range (" << velocity << ").Limit is: " << max_speed_);
velocity = max_speed_;
}
if (invert_)
ang = ang * -1.0;
int ticks = (int)(round( ang / rad_per_tick_ ));
int speed = (int)(round(velocity / rad_per_tick_));
if (speed==0) speed=1;
ticks += neutral_;
changeTorque(254);
dxl_write_word(id_,P_TORQUE_LIMIT_L,1023);
dxl_write_word(id_,P_GOAL_POSITION_L,ticks);
dxl_write_word(id_,P_GOAL_SPEED_L,speed);
}
void robo_ereto()
{
for(int i = 3;i<23;i++)
{
dxl_write_word(i, MOVING_SPEED, 100);
dxl_write_word(i, P_GOAL_POSITION_L, StandupPos[i-1]);
}
}
void curve(int id[], int length, int angle) {
int i;
for (i = 0; i < length; i +=2) {
dxl_write_word(id[i], GOAL_POSITION_L, angle);
}
for (i = 1; i < length; i +=2) {
dxl_write_word(id[i], GOAL_POSITION_L, 512);
}
}
void MotorInit(void){
dxl_initialize( 0, DEFAULT_BAUDNUM ); // Not using device index
//Wheel Mode
// dxl_write_word( 31, P_CW_ANGLE_LIMIT_L, 0 );
// dxl_write_word( 31, P_CCW_ANGLE_LIMIT_L, 0 );
//Set EEP Lock
dxl_write_word( 31, P_EEP_LOCK, 1 );
// Set goal speed
dxl_write_word( BROADCAST_ID, P_GOAL_SPEED_L, 0 );
dxl_write_byte( BROADCAST_ID, P_TORQUE_ENABLE, 0 );
_delay_ms(1000);
}
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 DXLJointInterface::relax()
{
if(controlType() == CT_TORQUE)
{
dxl_write_word(id(), REG_GOAL_TORQUE, 1);
sleep(3);
}
dxl_write_word(id(), REG_TORQUE_ENABLE, 0);
dxl_write_word(id(), REG_TORQUE_LIMIT, 1023);
m_currentVelocity = 0;
m_currentAcc = 0;
}
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");
}
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);
}
}
void robo_ereto()
{
for(int x=3; x<=8; x++)
dxl_write_word( x, 34, 1023); // Seta o torque dos braços no máximo
for(int i = 3;i<21;i++)
{
dxl_write_word(i, MOVING_SPEED, 60);
dxl_write_word(i, P_GOAL_POSITION_L, StandupPos[i-1]);
}
inverse_kinematic_right_leg(0, 0, 0, 100);
inverse_kinematic_left_leg(0, 0, 0, 100);
for(int x=3; x<=8; x++)
dxl_write_word( x, 34, 200); // Seta os braços com baixo torque
}
int DynamixelSimpleAPI::setGoalPosition(const int id, const int position, const int speed)
{
int status = 0;
if (checkId(id) == true)
{
// Set goal speed, and if success proceed to set goal position
if (setGoalSpeed(id, speed) != -1)
{
// Valid positions are in range [0:1023] for most servo series, and [0:4095] for high-end servo series
if ((position >= 0) && (position <= 4095))
{
int addr = getRegisterAddr(ct, REG_GOAL_POSITION);
dxl_write_word(id, addr, position);
if (dxl_print_error() == 0)
{
status = 1;
}
}
else
{
TRACE_ERROR(DAPI, "[#%i] Cannot set goal position '%i' for this servo: out of range\n", id, position);
}
}
}
return status;
}
void motor_set_speed_dir(char id, uint8_t percentage, char wise){
uint16_t v=0;
v = (uint16_t) percentage*1023ul/100ul; //convert percentage to 10 bit value
if (wise)
SET(v,10); //bit 10 is the direction bit 0 ccw, 1 cw
dxl_write_word(id, MOVING_SPEED_L, v); //set speed
}
void CloseGripper(float perc)
{
SetTorqueControl(7,1);
int bits1 = SetTorque( 7, perc);
dxl_write_word( 7, SERVO_PROG_SPEED , bits1 );
_delay_ms(30);
SetTorqueControl(8,1);
int bits2 = SetTorque( 8, perc);
dxl_write_word( 17, SERVO_PROG_SPEED , bits2 );
//printf("bits7: %d ",bits1);
//printf("bits17: %d",bits2);
desiredForce = perc;
doGripper = 1;
}
void OpenGripper(float perc)
{
float openedPosition = 1.1;
SetTorqueControl(7,0);
SetSpeedPerc(7,10);
int bits1 = SetPosition(7,(openedPosition * perc / 100.0) );
dxl_write_word( 7, SERVO_GOAL_POSITION_L, bits1);
_delay_ms(30);
SetTorqueControl(8,0);
SetSpeedPerc(17,10);
int bits2 = SetPosition(8,(openedPosition * perc / 100.0) );
dxl_write_word( 17, SERVO_GOAL_POSITION_L, bits2);
}
/**
* Initialize the servos
*/
void legsInit(void){
dxl_initialize( 0, DEFAULT_BAUDNUM ); // Not using device index
sei(); // Interrupt Enable
// Set speed of dynamixels
dxl_write_word( BROADCAST_ID, 32, 1000);
}
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 conAction()
{
if (condition == 1){
//Tunggu class move
EnableMove=0;
while(ready==0){}
EnableVision=0;
//Reset Posisi Kepala
dxl_write_word( 31, 30, 150);//32
dxl_write_word( 32, 30, 150);
printf("Saatnya Bangun depan");
//bangun_depan();
sleep(1);
//siapJalan();
//sleep(5);
EnableVision=1;
//move=1; //lanjutkan MOVE!
}else if (condition == 2){
//Tunggu class move
EnableMove=0;
while(ready==0){}
EnableVision=0;
//Reset Posisi Kepala
dxl_write_word( 31, 30, 150);//32
dxl_write_word( 32, 30, 150);
printf("Saatnya Bangun belakang");
//bangun_belakang();
//sleep(1);
//siapJalan();
//sleep(5);
EnableVision=1;
//move=1; //lanjutkan MOVE!
}
}
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_
}
void DXLJointInterface::setControlType(IJointInterface::ControlType ct)
{
IJointInterface::setControlType(ct);
dxl_write_word(id(), REG_CW_ANGLE_LIMIT, 0);
if(ct == CT_POSITION)
dxl_write_word(id(), REG_CCW_ANGLE_LIMIT, 4095);
else
dxl_write_word(id(), REG_CCW_ANGLE_LIMIT, 0);
if(ct == CT_TORQUE)
{
dxl_write_byte(id(), REG_TORQUE_CONTROL_EN, 1);
dxl_write_word(id(), REG_GOAL_TORQUE, 0);
}
else
dxl_write_byte(id(), REG_TORQUE_CONTROL_EN, 0);
dxl_write_word(id(), 24, 1); // torque enable
}
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 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 dynamixelApi_setGoalPos(int actuator, int pos) {
if(dynamixelApi_isWheelMode(actuator)) printf("WARNING: Position control in Dynamixel only possible in non-wheel model\n");
dxl_write_word(dyna.actuators[actuator], P_GOAL_POSITION_L, pos );
if(dyna.logPos) {
if(dyna.posLog==0) {
char name[100];
sprintf(name, "SetPos%li.log",time(NULL));
dyna.posLog = fopen(name,"w");
}
fprintf(dyna.posLog,"%f \t %i \t %i \t %i\n",getLocalTime(), actuator, pos, dynamixelApi_getPosition(actuator));
fflush(dyna.posLog);
}
checkForError(actuator,13);
}
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);
}
}
}
//mak237 added ccw limit function
int set_dynamixel_CCW_limit(short int motor_id, int CCW_limit)
{
dxl_write_word( motor_id, P_CCW_LIMIT_L, CCW_limit );
// mak237, not sure if this needs to be done as two seperate commands or just write the low with the total limit
/*
int CCW_high_limit = CCW_limit >> 8;
int CW_low_limit = CCW_limit & 0xFF;
dxl_write_word( motor_id, P_CCW_LIMIT_L, CCW_high_limit );
dxl_write_word( motor_id, P_CCW_LIMIT_H, CCW_low_limit );
*/
return 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;
}
// 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());
}
}
int DynamixelSimpleAPI::setMaxTorque(const int id, const int torque)
{
int status = 0;
if (checkId(id, false) == true)
{
// Valid torques are in range [0:1023]
if ((torque >= 0) && (torque <= 1023))
{
int addr = getRegisterAddr(ct, REG_MAX_TORQUE);
dxl_write_word(id, addr, torque);
if (dxl_print_error() == 0)
{
status = 1;
}
}
}
return status;
}
int DynamixelSimpleAPI::setGoalSpeed(const int id, const int speed)
{
int status = 0;
if (checkId(id) == true)
{
// Valid speeds are in range [0:1023] for "Join Mode", [0:2047] for "Wheel Mode"
if ((speed > -1) && (speed < 2048))
{
int addr = getRegisterAddr(ct, REG_GOAL_SPEED);
dxl_write_word(id, addr, speed);
if (dxl_print_error() == 0)
{
status = 1;
}
}
else
{
TRACE_ERROR(DAPI, "[#%i] Cannot set goal speed '%i' for this servo: out of range\n", id, speed);
}
}
return status;
}
