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