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; }
/** * Compliance Margin are the areas where output torque is 0. * Compliance Slope are the areas where output torque is reduced when they are getting close to Goal Position. * slope [1,254] * margin [0,254] */ void dynamixelApi_setCompliance(int actuator, char margin, char slope) { dxl_write_byte(dyna.actuators[actuator], P_CW_COMPLIANCE_MARGIN, margin); checkForError(actuator,9); dxl_write_byte(dyna.actuators[actuator], P_CCW_COMPLIANCE_MARGIN, margin); checkForError(actuator,10); dxl_write_byte(dyna.actuators[actuator], P_CW_COMPLIANCE_SLOPE, slope); checkForError(actuator,11); dxl_write_byte(dyna.actuators[actuator], P_CCW_COMPLIANCE_SLOPE, slope); checkForError(actuator,12); }
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); } } }
int DynamixelSimpleAPI::setLed(const int id, int value, const int color) { int status = 0; if (checkId(id) == true) { // Normalize value if (value >= 1) { (servoSerie == SERVO_XL) ? value = color : value = 1; } else { value = 0; } // Execute command int addr = getRegisterAddr(ct, REG_LED); if (addr >= 0) { dxl_write_byte(id, addr, value); // Check for error if (dxl_print_error() == 0) { status = 1; } } } return status; }
int DynamixelSimpleAPI::changeBaudRate(const int id, const int baudnum) { int status = 0; if (checkId(id) == true) { // Valid baudnums are in range [0:254] if ((baudnum >= 0) && (baudnum <= 254)) { int addr = getRegisterAddr(ct, REG_BAUD_RATE); dxl_write_byte(id, addr, baudnum); if (dxl_print_error() == 0) { status = 1; } } else { TRACE_ERROR(DAPI, "[#%i] Cannot set new baudnum '%i' for this servo: out of range\n", id, baudnum); } } return status; }
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; } }
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 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 }
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 DynamixelInterface::sendByte(int id,int address,unsigned char byte) { if (!DXL2USB.isOk()) throw DXL_ComError(-1,id,__FILE__,__LINE__); int retry=DynamixelInterface::RETRIES; int status=0; do { dxl_write_byte(id,address,byte); 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,id,__FILE__,__LINE__); }
//-------------------------------------------------------------------- // 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 }
int DynamixelSimpleAPI::setTorqueEnabled(const int id, int torque) { int status = 0; if (checkId(id) == true) { int addr = getRegisterAddr(ct, REG_TORQUE_ENABLE); // Valid torque are in range [0:1] if (torque != 0) { torque = 1; } dxl_write_byte(id, addr, torque); if (dxl_print_error() == 0) { status = 1; } } return status; }
int DynamixelSimpleAPI::changeId(const int old_id, const int new_id) { int status = 0; // Check 'old' ID if (checkId(old_id, false) == true) { // Check 'new' ID // Valid IDs are in range [0:maxId] if ((new_id >= 0) && (new_id <= maxId)) { // If the ping get a response, then we already have a servo on the new id dxl_ping(new_id); if (dxl_get_com_status() == COMM_RXSUCCESS) { TRACE_ERROR(DAPI, "[#%i] Cannot set new ID '%i' for this servo: already in use\n", new_id); } else { int addr = getRegisterAddr(ct, REG_ID); dxl_write_byte(old_id, addr, new_id); if (dxl_print_error() == 0) { status = 1; } } } else { TRACE_ERROR(DAPI, "[#%i] Cannot set new ID '%i' for this servo: out of range\n", new_id); } } return status; }
void Dynamixel::writeByte(int id, int address, int value ) { dxl_write_byte( id, address, value ); }
//============================================================================================ //-------------------------------------------------------------------------------------------- void levantar_de_costas() { for(int x=3; x<=8; x++) dxl_write_word( x, 34, 1023); // Inicia os braços com alto torque dxl_write_word(12, MOVING_SPEED, 150); dxl_write_word(12, P_GOAL_POSITION_L, StandupPos[11]-271);//310 dxl_write_word(18, MOVING_SPEED, 150); dxl_write_word(18, P_GOAL_POSITION_L, StandupPos[17]-264);//228 //dxl_write_word(3, MOVING_SPEED, 150); dxl_write_byte(3, TORQUE_ENABLE, 0); //dxl_write_word(6, MOVING_SPEED, 150); dxl_write_byte(6, TORQUE_ENABLE, 0); dxl_write_word(5, MOVING_SPEED, 150); dxl_write_word(5, P_GOAL_POSITION_L,StandupPos[4]-360);//10 dxl_write_word(8, MOVING_SPEED, 150); dxl_write_word(8, P_GOAL_POSITION_L,StandupPos[7]+340);//1010 dxl_write_word(10, MOVING_SPEED, 150); dxl_write_word(10, P_GOAL_POSITION_L, StandupPos[9]+168);//700 dxl_write_word(16, MOVING_SPEED, 150); dxl_write_word(16, P_GOAL_POSITION_L, StandupPos[15]-163);//324 dxl_write_word(20, MOVING_SPEED, 150); dxl_write_word(20, P_GOAL_POSITION_L, StandupPos[19]-159);//60 dxl_write_word(14, MOVING_SPEED, 150); dxl_write_word(14, P_GOAL_POSITION_L, StandupPos[13]+141);//947 espera_mov_levantar(); //dxl_write_word(20, MOVING_SPEED, 100); dxl_write_byte(20, TORQUE_ENABLE, 0); //dxl_write_word(14, MOVING_SPEED, 100); dxl_write_byte(14, TORQUE_ENABLE, 0); espera_mov_levantar(); dxl_write_word(3, MOVING_SPEED, 480); dxl_write_word(6, MOVING_SPEED, 480); dxl_write_word(3, P_GOAL_POSITION_L, StandupPos[2]+77);//459 dxl_write_word(6, P_GOAL_POSITION_L, StandupPos[5]-42);//600 dxl_write_word(10, MOVING_SPEED, 200); dxl_write_word(10, P_GOAL_POSITION_L, StandupPos[9]-95);//437 dxl_write_word(16, MOVING_SPEED, 200); dxl_write_word(16, P_GOAL_POSITION_L, StandupPos[15]+105);//592 espera_mov_levantar(); dxl_write_word(5, MOVING_SPEED, 700); dxl_write_word(5, P_GOAL_POSITION_L, StandupPos[4]-350);//-----------------------xxx --0002 dxl_write_word(8, MOVING_SPEED, 700); dxl_write_word(8, P_GOAL_POSITION_L, StandupPos[7]+350);//--------------------xxx --1020 dxl_write_word(3, MOVING_SPEED, 480); dxl_write_word(6, MOVING_SPEED, 480); dxl_write_word(3, P_GOAL_POSITION_L, StandupPos[2]+47);//429 dxl_write_word(6, P_GOAL_POSITION_L, StandupPos[5]-12);//630 dxl_write_word(12, MOVING_SPEED, 200); dxl_write_word(12, P_GOAL_POSITION_L, StandupPos[11]-311);//270 dxl_write_word(18, MOVING_SPEED, 200); dxl_write_word(18, P_GOAL_POSITION_L, StandupPos[17]-304);//188 espera_mov_levantar(); //----------------------------------------------------------------- // ---- Abre as pernas ------------------------- dxl_write_word(5, MOVING_SPEED, 100); dxl_write_word(5, P_GOAL_POSITION_L, StandupPos[4]-276);//97 dxl_write_word(8, MOVING_SPEED, 100); dxl_write_word(8, P_GOAL_POSITION_L, StandupPos[7]+248);//917 dxl_write_word(11, MOVING_SPEED, 95); dxl_write_word(11, P_GOAL_POSITION_L, StandupPos[10]+286);//793 dxl_write_word(17, MOVING_SPEED, 92); dxl_write_word(17, P_GOAL_POSITION_L, StandupPos[16]-289);//243 dxl_write_word(10, MOVING_SPEED, 100); dxl_write_word(10, P_GOAL_POSITION_L, StandupPos[9]+139);//671 dxl_write_word(16, MOVING_SPEED, 100); dxl_write_word(16, P_GOAL_POSITION_L, StandupPos[15]-113);//374 espera_mov_levantar(); //---- Empurra o corpo para ficar sentado ----------------- dxl_write_word(5, MOVING_SPEED, 100); dxl_write_word(5, P_GOAL_POSITION_L, StandupPos[4]-159); //214 dxl_write_word(8, MOVING_SPEED, 100); dxl_write_word(8, P_GOAL_POSITION_L, StandupPos[7]+159); //778+50 dxl_write_word(14, MOVING_SPEED, 100); dxl_write_word(14, P_GOAL_POSITION_L, StandupPos[13]-118); //688 dxl_write_word(20, MOVING_SPEED, 100); dxl_write_word(20, P_GOAL_POSITION_L, StandupPos[19]+108); //327 espera_mov_levantar(); //----- Puxa os braços para traz ------------------------- dxl_write_word(13, MOVING_SPEED, 150); dxl_write_word(13, P_GOAL_POSITION_L, StandupPos[12]+392); //904 dxl_write_word(19, MOVING_SPEED, 150); dxl_write_word(19, P_GOAL_POSITION_L, StandupPos[18]- 188); //324 dxl_write_word(5, MOVING_SPEED, 200); dxl_write_word(5, P_GOAL_POSITION_L, StandupPos[4]-350); //------------------------xx 0002 dxl_write_word(8, MOVING_SPEED, 200); dxl_write_word(8, P_GOAL_POSITION_L, StandupPos[7]+350); //------------------------xx 1020 usleep(100000); dxl_write_word(3, MOVING_SPEED, 400); dxl_write_word(3, P_GOAL_POSITION_L,StandupPos[2]+480); dxl_write_word(6, MOVING_SPEED, 400); dxl_write_word(6, P_GOAL_POSITION_L,StandupPos[5]-480); espera_mov_levantar(); //---- Traz os braço para frente--------------------------- dxl_write_word(5, MOVING_SPEED, 700); dxl_write_word(5, P_GOAL_POSITION_L,StandupPos[4] + 100); dxl_write_word(8, MOVING_SPEED, 700); dxl_write_word(8, P_GOAL_POSITION_L, StandupPos[7] - 100); usleep(80000); //espera_mov_levantar(); //---- Gira as pernas ------------------------------------- dxl_write_word(9, MOVING_SPEED, 200); dxl_write_word(9, P_GOAL_POSITION_L,StandupPos[8] + 75); dxl_write_word(15, MOVING_SPEED, 200); dxl_write_word(15, P_GOAL_POSITION_L, StandupPos[14] - 75); espera_mov_levantar(); dxl_write_word(16, MOVING_SPEED, 200); dxl_write_word(16, P_GOAL_POSITION_L,StandupPos[15]); dxl_write_word(10, MOVING_SPEED, 200); dxl_write_word(10, P_GOAL_POSITION_L, StandupPos[9]); espera_mov_levantar(); /* dxl_write_word(9, MOVING_SPEED, 10); dxl_write_word(9, P_GOAL_POSITION_L,StandupPos[8]); dxl_write_word(15, MOVING_SPEED, 10); dxl_write_word(15, P_GOAL_POSITION_L, StandupPos[14]); dxl_write_word(11, MOVING_SPEED, 100); dxl_write_word(11, P_GOAL_POSITION_L, StandupPos[10]); dxl_write_word(17, MOVING_SPEED, 100); dxl_write_word(17, P_GOAL_POSITION_L, StandupPos[16]); dxl_write_word(13, MOVING_SPEED, 20); dxl_write_word(13, P_GOAL_POSITION_L, StandupPos[12]); dxl_write_word(19, MOVING_SPEED, 20); dxl_write_word(19, P_GOAL_POSITION_L, StandupPos[18]); */ inverse_kinematic_right_leg(0, 0, 0, 250); inverse_kinematic_left_leg(0, 0, 0, 250); //espera_mov_levantar(); inclina_plano(LEG_RIGHT, 0, 150, 0); inclina_plano(LEG_LEFT, 0, 150, 0); espera_mov_levantar(); dxl_write_word(9, MOVING_SPEED, 100); dxl_write_word(9, P_GOAL_POSITION_L,StandupPos[8]); dxl_write_word(15, MOVING_SPEED, 100); dxl_write_word(15, P_GOAL_POSITION_L, StandupPos[14]); espera_mov_levantar(); //---------------levantar de frente------------------------ /* dxl_write_word(5, MOVING_SPEED, 300); dxl_write_word(5, P_GOAL_POSITION_L, StandupPos[4]- 270); dxl_write_word(8, MOVING_SPEED, 300); dxl_write_word(8, P_GOAL_POSITION_L, StandupPos[7] + 270); dxl_write_word(3, MOVING_SPEED, 400); dxl_write_word(3, P_GOAL_POSITION_L,StandupPos[2]+550); dxl_write_word(6, MOVING_SPEED, 400); dxl_write_word(6, P_GOAL_POSITION_L,StandupPos[5]-550); */ dxl_write_word(10, MOVING_SPEED, 500); dxl_write_word(10, P_GOAL_POSITION_L, StandupPos[9] + 341); dxl_write_word(12, MOVING_SPEED, 500); dxl_write_word(12, P_GOAL_POSITION_L, StandupPos[11] - 426); dxl_write_word(14, MOVING_SPEED, 500); dxl_write_word(14, P_GOAL_POSITION_L, StandupPos[13] - 200); dxl_write_word(16, MOVING_SPEED, 500); dxl_write_word(16, P_GOAL_POSITION_L, StandupPos[15] - 337); dxl_write_word(18, MOVING_SPEED, 500); dxl_write_word(18, P_GOAL_POSITION_L, StandupPos[17] - 424); dxl_write_word(20, MOVING_SPEED, 500); dxl_write_word(20, P_GOAL_POSITION_L, StandupPos[19] + 182); espera_mov_levantar(); dxl_write_word(5, MOVING_SPEED, 200); dxl_write_word(5, P_GOAL_POSITION_L, StandupPos[4]+ 50); dxl_write_word(8, MOVING_SPEED, 200); dxl_write_word(8, P_GOAL_POSITION_L, StandupPos[7] - 50); espera_mov_levantar(); dxl_write_word(5, MOVING_SPEED, 200); dxl_write_word(5, P_GOAL_POSITION_L, StandupPos[4]+ 160); dxl_write_word(8, MOVING_SPEED, 200); dxl_write_word(8, P_GOAL_POSITION_L, StandupPos[7] - 160); dxl_write_word(3, MOVING_SPEED, 420); dxl_write_word(3, P_GOAL_POSITION_L,StandupPos[2]+0); dxl_write_word(6, MOVING_SPEED, 420); dxl_write_word(6, P_GOAL_POSITION_L,StandupPos[5]-0); espera_mov_levantar(); dxl_write_word(5, MOVING_SPEED, 400); dxl_write_word(5, P_GOAL_POSITION_L, StandupPos[4]- 60); dxl_write_word(8, MOVING_SPEED, 400); dxl_write_word(8, P_GOAL_POSITION_L, StandupPos[7] + 60); espera_mov_levantar(); inverse_kinematic_right_leg(0, 0, 0, 200); inverse_kinematic_left_leg(0, 0, 0, 200); espera_mov_levantar(); /*teste*/ for(int x=3; x<=8; x++) dxl_write_word( x, 34, 200); // Inicia os braços com baixo torque }
bool DynamixelServo::servoTorqueEnable(qbo_arduqbo::TorqueEnable::Request &req, qbo_arduqbo::TorqueEnable::Response &res) { dxl_write_byte( id_, P_TORQUE_ENABLE, req.torque_enable ); }
void DynamixelServo::changeTorque(int torque) { dxl_write_byte( id_, P_CW_COMPILANCE_SLOPE, torque ); dxl_write_byte( id_, P_CCW_COMPILANCE_SLOPE, torque ); //dxl_write_byte( id_, P_LIMIT_TEMPERATURE, 99 ); }
// mak237, added torque mode switch command // torque_mode = 0 for joint mode, 1 for torque mode int torque_control_toggle(short int motor_id, int torque_mode) { dxl_write_byte( motor_id, P_TORQUE_ENABLE, torque_mode ); return 0; }
void DXLJointInterface::setPValue(int value) { dxl_write_byte(id(), REG_P_GAIN, value); }
int main(void) { // local variables int sensor_flag, command_flag, comm_status, sensor_process_flag, obstacle_flag, wait_flag; unsigned long wait_timer = 0; unsigned long wait_time = 0; uint8 motion_flag = 0; // TIMING: unsigned long timer1, timer2, timer3, timer4; // Initialization Routines led_init(); // switches all 6 LEDs on serial_init(57600); // serial port at 57600 baud buzzer_init(); // enable buzzer melodies button_init(); // enable push buttons on CM-510 delay_ms(200); // wait 0.2s led_off(ALL_LED); // and switch them back off // initialize the clock clock_init(); wait_flag = 0; // enable interrupts sei(); // print welcome message printf("\nBioloid C Control V0.8\n"); printf("Press the START button on the CM-510 to continue.\n"); // reset the start button variable, something triggers the interrupt on start-up start_button_pressed = FALSE; // initialize motion pages motionPageInit(); // Wait for the START Button before going any further while(!start_button_pressed) { // PLAY LED is flashing at 5Hz led_toggle(LED_PLAY); delay_ms(200); } // Now turn LED solid led_on(LED_PLAY); // and reset the start button variable start_button_pressed = FALSE; // perform high level initialization of Dynamixel bus and servos dxl_init(DEFAULT_BAUDNUMBER); // assume initial pose executeMotion(COMMAND_BALANCE_MP); // set the walk state walk_setWalkState(0); obstacle_flag = 0; // initialize the PID controller for balancing #ifdef ACCEL_AND_ULTRASONIC pid_init(); setupGyroKalman(); #endif // initialize the ADC and take default readings delay_ms(4000); // wait 4s for gyros to stabilize adc_init(); sensor_process_flag = 0; sensor_flag = 0; // print out default sensor values #ifdef GYRO_AND_DMS_ONLY printf("\nBattery = %imV, Gyro X, Y Center = %i %i ", adc_battery_val, adc_gyrox_center, adc_gyroy_center); #endif #ifdef ACCEL_AND_ULTRASONIC printf("\nBattery, Gyro X, Y Accel X, Y Center = %imV %i %i %i %i", adc_battery_val, adc_gyrox_center, adc_gyroy_center, adc_accelx_center, adc_accely_center); #endif // write out the command prompt printf( "\nReady for command.\n> "); // TIMING: timer4 = micros(); // main command loop (takes 28us when idle) // keeps executing unless we encounter a major alarm while( !major_alarm ) { // Check if we received a new command command_flag = serialReceiveCommand(); // takes 4ms if new command (largely because of printf) // TIMING: timer1 = micros() - timer4; // see if we are in a wait command if ( bioloid_command == COMMAND_WAIT_MILLISECONDS || bioloid_command == COMMAND_WAIT_SECONDS ) { // first look if we should continue waiting if ( wait_flag == 1 ) { // check timer if ( millis() - wait_timer > wait_time ) { // wait time is finished - reset the wait flag, timer and time wait_flag = 0; wait_timer = 0; wait_time = 0; // read next command from command sequence if ( flag_motion_sequence == 1 ) { bioloid_command = command_sequence_buffer[command_sequence_counter][0]; next_motion_page = command_sequence_buffer[command_sequence_counter][1]; // update pointer if there are more commands left if ( command_sequence_counter < command_sequence_length ) { command_sequence_counter++; } else { // sequence is finished, reset all sequence related variables flag_motion_sequence = 0; command_sequence_counter = 0; command_sequence_length = 0; bioloid_command = COMMAND_STOP; } } } } if ( command_flag == 1 && wait_flag == 0 ) { // wait command has only just been received, calculate wait time wait_timer = millis(); command_flag = 0; wait_flag = 1; wait_time = next_motion_page; } else if ( command_flag == 1 && wait_flag == 1 ) { // wait command is still in progress but new command has been received // check for STOP, otherwise ignore if ( bioloid_command == COMMAND_STOP ) { // reset the wait flag, timer and time wait_flag = 0; wait_timer = 0; wait_time = 0; } } } // check if start button has been pressed and we need to do emergency stop if ( start_button_pressed && bioloid_command != COMMAND_STOP ) { // disable torque & reset current command comm_status = dxl_write_byte(BROADCAST_ID, DXL_TORQUE_ENABLE, 0); last_bioloid_command = bioloid_command; bioloid_command = COMMAND_STOP; command_flag = 1; // and reset the start button variable start_button_pressed = FALSE; } else if ( start_button_pressed && bioloid_command == COMMAND_STOP ) { // we are resuming from an emergency stop, restore last command bioloid_command = last_bioloid_command; last_bioloid_command = COMMAND_STOP; command_flag = 1; // and reset the start button variable start_button_pressed = FALSE; } // Check if we need to read the sensors sensor_flag = adc_readSensors(); // takes 0.6ms for gyro/accel and 0.9ms including DMS/ultrasonic (156us per channel) if ( sensor_flag == 1 ) { // new sensor data - process and update command flag if necessary sensor_process_flag = adc_processSensorData(); if ( command_flag == 0 && sensor_process_flag == 1 ) { // robot has slipped, front/back getup command has been issued command_flag = 1; } else if ( sensor_process_flag == 2 ) { // if the sensor process flag = 2 it means low voltage emergency stop major_alarm = TRUE; } } // obstacle avoidance for walking if ( walk_getWalkState() != 0 ) { // currently very basic - turn left until path is clear obstacle_flag = walk_avoidObstacle(obstacle_flag); if ( command_flag == 0 && (obstacle_flag == 1 || obstacle_flag == -1) ) { command_flag = 1; } } // set the new command global variable if( command_flag == 1 ) { new_command = TRUE; command_flag = 0; // if we are coming out of BAL command, reset joint offsets if( last_bioloid_command == COMMAND_BALANCE && bioloid_command != COMMAND_BALANCE ) { for (uint8 i=0; i<NUM_AX12_SERVOS; i++) { joint_offset[i] = 0; } } } // TEST printf("\n Command %i, New %i, MP %i, Next MP %i ", bioloid_command, new_command, current_motion_page, next_motion_page); // TIMING: timer2 = micros() - timer4 - timer1; #ifdef ACCEL_AND_ULTRASONIC // static balancing if ( bioloid_command == COMMAND_BALANCE && major_alarm != TRUE ) { // static balancing uses Kalman Filter or PID controller depending on availability of accelerometer // first make sure the PID is turned on if ( pid_getMode() != AUTOMATIC ) { pid_setMode(AUTOMATIC); } staticRobotBalance(); } else if ( pid_getMode() == 1 ) { pid_setMode(MANUAL); } #endif // execute motion steps if ( major_alarm != TRUE ) { motion_flag = executeMotionSequence(); // takes 2.1ms when executing a step during walking or 3.3ms if unpacking a new motion page } // TIMING: timer3 = micros() - timer4 - timer1 - timer2; // TIMING: printf("Timer 1 = %lu, 2 = %lu, 3 = %lu, SFlag = %i\n", timer1, timer2, timer3, sensor_flag); // TIMING: timer4 = micros(); } // end of main command loop }
// High level initialization - specific robot settings for Bioloid void dxl_init(int baudnum) { int commStatus = 0, errorStatus = 0; // now prepare the Dynamixel servos // first initialize the bus dxl_initialize( 0, baudnum ); // wait 0.1s _delay_ms(100); // Next check the hardware configuration is valid for (int i=0; i<NUM_AX12_SERVOS; i++) { // ping each servo in turn errorStatus = dxl_ping(AX12_IDS[i]); if (errorStatus == -1) { printf("\nHardware Configuration Failure at Dynamixel ID %i.\n", AX12_IDS[i]); dxl_terminate(); return; } } // set alarm LED and shutdown to prevent overheat/overload commStatus = dxl_write_byte(BROADCAST_ID, DXL_ALARM_LED, 36); if(commStatus != COMM_RXSUCCESS) { printf("\nDXL_ALARM_LED Broadcast - "); dxl_printCommStatus(dxl_get_result()); } commStatus = dxl_write_byte(BROADCAST_ID, DXL_ALARM_SHUTDOWN, 36); if(commStatus != COMM_RXSUCCESS) { printf("\nDXL_ALARM_LED Broadcast - "); dxl_printCommStatus(dxl_get_result()); } // now set temperature and voltage limits commStatus = dxl_write_byte(BROADCAST_ID, DXL_TEMPERATURE_LIMIT, 70); if(commStatus != COMM_RXSUCCESS) { printf("\nDXL_TEMPERATURE_LIMIT Broadcast - "); dxl_printCommStatus(dxl_get_result()); } commStatus = dxl_write_byte(BROADCAST_ID, DXL_LOW_VOLTAGE_LIMIT, 70); if(commStatus != COMM_RXSUCCESS) { printf("\nDXL_LOW_VOLTAGE_LIMIT Broadcast - "); dxl_printCommStatus(dxl_get_result()); } // set a 2-point compliance margin (equals 0.58 deg) commStatus = dxl_write_byte(BROADCAST_ID, DXL_CW_COMPLIANCE_MARGIN, 2); if(commStatus != COMM_RXSUCCESS) { printf("\nDXL_CW_COMPLIANCE_MARGIN Broadcast - "); dxl_printCommStatus(dxl_get_result()); } commStatus = dxl_write_byte(BROADCAST_ID, DXL_CCW_COMPLIANCE_MARGIN, 2); if(commStatus != COMM_RXSUCCESS) { printf("\nDXL_CCW_COMPLIANCE_MARGIN Broadcast - "); dxl_printCommStatus(dxl_get_result()); } _delay_ms(100); // and enable torque to keep positions commStatus = dxl_write_byte(BROADCAST_ID, DXL_TORQUE_ENABLE, 1); if(commStatus != COMM_RXSUCCESS) { printf("\nDXL_TORQUE_ENABLE Broadcast - "); dxl_printCommStatus(dxl_get_result()); } _delay_ms(50); }
//-------------------------------------------------------------------- //Init //-------------------------------------------------------------------- void ServoDriver::Init(void) { // First lets get the actual servo positions for all of our servos... g_fServosFree = true; // We will duplicate and expand on the functionality of the bioloid readPose, // function. In our loop we will set the IDs to that of our table, so they // will be in the same order. Plus we will verify we have all of the servos // If we care configured such that none of the servos in our loop have id #1 and // we find that servo #1 and only one other is not found, we will assume that // servo did the renumber to 1 problem and we will renumber it to the missing one. // But again only if 1 servo is missing. // Note: We are not saving the read positions, but the make sure servos are on will... bioloid.poseSize(NUMSERVOS); // Method in this version uint16_t w; int count_missing = 0; int missing_servo = -1; bool servo_1_in_table = false; for (int i = 0; i < NUMSERVOS; i++) { // Set the id bioloid.setId(i, cPinTable[i]); if (cPinTable[i] == 1) servo_1_in_table = true; // Now try to get it's position w = ax12GetRegister(cPinTable[i], AX_PRESENT_POSITION_L, 2); if (w == 0xffff) { // Try a second time to make sure. delay(25); w = ax12GetRegister(cPinTable[i], AX_PRESENT_POSITION_L, 2); if (w == 0xffff) { // We have a failure printf("Servo(%d): %d not found", i, cPinTable[i]); if (++count_missing == 1) missing_servo = cPinTable[i]; } } delay(25); } // Now see if we should try to recover from a potential servo that renumbered itself back to 1. if (count_missing) printf("ERROR: Servo driver init: %d servos missing\n", count_missing); if ((count_missing == 1) && !servo_1_in_table) { if (dxl_read_word(1, AX_PRESENT_POSITION_L) != 0xffff) { printf("Servo recovery: Servo 1 found - setting id to %d\n", missing_servo); dxl_write_byte(1, AX_ID, missing_servo); } } #ifdef USB2AX_REG_VOLTAGE ax12SetRegister(AX_ID_DEVICE, USB2AX_REG_VOLTAGE, cPinTable[FIRSTFEMURPIN]); #endif g_fAXSpeedControl = false; #ifdef OPT_GPPLAYER _fGPEnabled = true; // assume we support it. #endif }
void dynamixelApi_setLed(int actuator, bool led) { dxl_write_byte(dyna.actuators[actuator], P_LED, led); checkForError(actuator,8); }
int main() { int baudnum = 1; //int GoalPos[2] = {0, 1023}; int GoalPos[2] = {300, 700}; int GoalSpeed[2] = {20,20}; //int GoalPos[2] = {0, 4095}; // for Ex series int index = 0; int deviceIndex = 1; int Moving, PresentPos, PresentSpeed; 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] ); dxl_write_word( DEFAULT_ID, P_GOAL_SPEED_L, GoalSpeed[index] ); dxl_write_byte( DEFAULT_ID, P_CW_COMPILANCE_SLOPE, 254 ); dxl_write_byte( DEFAULT_ID, P_CCW_COMPILANCE_SLOPE, 254 ); do { // Read present position PresentPos = dxl_read_word( DEFAULT_ID, P_PRESENT_POSITION_L ); PresentSpeed = dxl_read_word( DEFAULT_ID, P_PRESENT_SPEED_L ); CommStatus = dxl_get_result(); if( CommStatus == COMM_RXSUCCESS ) { printf( "%d %d -- %d %d\n",GoalPos[index], PresentPos, GoalSpeed[index], PresentSpeed ); 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; } //sleep(1); }while(Moving == 1); } // Close device dxl_terminate(); printf( "Press Enter key to terminate...\n" ); getchar(); return 0; }
int main(void){ int log = 0; //Start Switch // DDRA = 0x00; // PORTA = 0x12; //Start PORT A for switch and IR sensors DDRA = 0xFC; PORTA = 0xFE; //LED Initial DDRC = 0x7F; PORTC = 0x7E; DDRD = 0x70; PORTD = 0x11; MotorInit(); initSerial(); char * readData = NULL; int isFinish = 0; sensorInit(); if (isCaptureMode ==1) dxl_write_byte( BROADCAST_ID, P_TORQUE_ENABLE, 0 ); while(1){ sensorTest(0); sensorTest(1); sensorTest(2); setMode(); if( checkSerialRead() > 0 ){ readData = getReadBuffer(); if( readData != NULL ){ // printf( "readData=%s\n", &readData[0] ); split( &readData[0] ); switch( serCmd[0] ){ case EVT_ACTION: ServoControl( serCmd[1] ); // setSpeedTest( serCmd[1] ); sendAck(1); break; case EVT_START_MOTION: startMotion( serCmd[1], serCmd[2] ); PORTC = ~(1 << (LED_MAX - 2)); sendAck(1); break; case EVT_STOP_MOTION: stopMotion(); sendAck(1); break; case EVT_FORCE_MOTION: forceMotion( serCmd[1], serCmd[2] ); break; case EVT_GET_NOW_ANGLE: getAngle(); break; case EVT_SET_ANGLE: setAngle(); case EVT_GET_ACT_ANGLE: if( serCmd[1] >= ACT_MAX ){ sendAck(0); }else{ sendActAngle(serCmd[1]); } break; case EVT_GET_LOAD: getLoad(); // printf( "%d\n", movingTime ); break; case EVT_GET_VOLTAGE: getVoltage(); break; case EVT_TORQUE_DISABLE: dxl_write_byte( BROADCAST_ID, P_TORQUE_ENABLE, 0 ); break; case EVT_WATCH_DOG: watchDogCnt = 0; break; case EVT_MOTION_EDIT: break; case 999: // printf( "finish\n"); sendAck(999); isFinish = 1; break; default: sendAck(0); } if( isFinish > 0 ){ MotorControl( 0, 0 ); break; } memset( readData, 0x00, SERIAL_BUFFER_SIZE ); } } memset( &serCmd[0], 0x00, sizeof(int) * SERIAL_BUFFER_SIZE ); if (~PINA & SW_START ) { if (log == 1) printf( "main() 0\n"); if( iStart > 0 ){ iStart = 0; PORTC = LED_BAT|LED_TxD|LED_RxD|LED_AUX|LED_MANAGE|LED_PROGRAM|LED_PLAY; if (isCaptureMode != 1) ServoControl( 0 ); } }else{ if( iStart == 0 ){ PORTC &= ~LED_PLAY; iStart = 1; } if( modeWait <= 0 ){ if (log == 1) printf( "main() 1\n"); setModeAction(); if (isMovetest == 1) { moveTest(); } else { move(); } }else{ if (log == 1) printf( "main() 2\n"); modeWait -= MAIN_DELAY; } } if (sensorValue[0] == 0 && sensorValueOld[0] != sensorValue[0]) { if (log == 1) printf( "### main() sensorValue[0] == 0\n"); PORTC |= LED_PROGRAM; //edit }else if (sensorValueOld[0] != sensorValue[0]){ if (log == 1) printf( "### main() sensorValue[0] == 1\n"); PORTC &= ~LED_PROGRAM; //edit } if (sensorValue[1] == 0 && sensorValueOld[1] != sensorValue[1]) { if (log == 1) printf( "### main() sensorValue[1] == 0\n"); PORTC |= LED_MANAGE; //mon }else if (sensorValueOld[1] != sensorValue[1]){ if (log == 1) printf( "### main() sensorValue[1] == 1\n"); PORTC &= ~LED_MANAGE; //mon } if (sensorValue[2] == 0 && sensorValueOld[2] != sensorValue[2]) { if (log == 1) printf( "### main() sensorValue[2] == 0\n"); PORTC |= LED_AUX; //AUX }else if (sensorValueOld[2] != sensorValue[2]){ if (log == 1) printf( "### main() sensorValue[2] == 1\n"); PORTC &= ~LED_AUX; //AUX } sensorValueOld[0] = sensorValue[0]; sensorValueOld[1] = sensorValue[1]; sensorValueOld[2] = sensorValue[2]; // walk pattern LED // brinkLED(); _delay_ms(MAIN_DELAY); watchDogCnt++; caputureCount1++; if (caputureCount1 == 25){ if (isCaptureMode == 1) getAngle(); caputureCount1 = 0; } } }
void Dynamixel::havCap(uint8_t id) { dxl_write_byte(id, 0, 0); }
void dynamixelApi_setTorqueEnable(int actuator, bool enable) { dxl_write_byte(dyna.actuators[actuator], P_TORQUE_ENABLE, 0); checkForError(actuator,7); }
int DynamixelSimpleAPI::setSetting(const int id, const int reg_name, const int reg_value, int reg_type, const int device) { int status = 0; if (checkId(id) == true) { // Device detection const int (*cctt)[8] = getRegisterTable(device); if (cctt == NULL) { // Using default control table from this SimpleAPI isntance cctt = ct; } if (cctt) { // Find register's informations (addr, size...) RegisterInfos infos = {-1, -1, -1, -1, -1, -1, -1, -1, -1}; if (getRegisterInfos(cctt, reg_name, infos) == 1) { // Check if we have permission to write into this register if (infos.reg_access_mode == READ_WRITE) { // Check value if (reg_value >= infos.reg_value_min && reg_value <= infos.reg_value_max) { // Write value if (infos.reg_size == 1) { dxl_write_byte(id, infos.reg_addr, reg_value); } else if (infos.reg_size == 2) { dxl_write_word(id, infos.reg_addr, reg_value); } // Check for error if (dxl_print_error() == 0) { status = 1; } } else { TRACE_ERROR(DAPI, "[#%i] setSetting(reg %i / %s) [VALUE ERROR] (min: %i / max: %i)\n", id, reg_name, getRegisterNameTxt(reg_name).c_str(), infos.reg_value_min, infos.reg_value_max); } } else { TRACE_ERROR(DAPI, "[#%i] setSetting(reg %i / %s) [REGISTER ACCESS ERROR]\n", id, reg_name, getRegisterNameTxt(reg_name).c_str()); } } else { TRACE_ERROR(DAPI, "[#%i] setSetting(reg %i / %s) [REGISTER NAME ERROR]\n", id, reg_name, getRegisterNameTxt(reg_name).c_str()); } } else { TRACE_ERROR(DAPI, "[#%i] setSetting(reg %i / %s) [CONTROL TABLE ERROR]\n", id, reg_name, getRegisterNameTxt(reg_name).c_str()); } } else { TRACE_ERROR(DAPI, "[#%i] setSetting(reg %i / %s) [DEVICE ID ERROR]\n", id, reg_name, getRegisterNameTxt(reg_name).c_str()); } return status; }