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; }