uint8_t PingDevice(uint8_t id) {
dxl_ping(id);
if(dxl_get_result() == COMM_RXSUCCESS) {
return 1;
} else {
printf("E:Device %d not connected\n\r",id);
return 0;
}
}
std::vector <int> DynamixelSimpleAPI::servoScan(int start, int stop)
{
// Check start/stop boundaries
if (start < 0 || start > (maxId - 1))
start = 0;
if (stop < 1 || stop > maxId || stop < start)
stop = maxId;
TRACE_INFO(DAPI, "> Scanning for Dynamixel devices on '%s'... Range is [%i,%i]\n",
serialGetCurrentDevice().c_str(), start, stop);
// A vector of Dynamixel IDs found during the scan
std::vector <int> ids;
for (int id = start; id <= stop; id++)
{
PingResponse pingstats;
// If the ping gets a response, then we have found a servo
if (dxl_ping(id, &pingstats) == true)
{
setLed(id, 1, LED_GREEN);
ids.push_back(id);
TRACE_INFO(DAPI, "[#%i] Dynamixel servo found!\n", id);
TRACE_INFO(DAPI, "[#%i] model: '%i' (%s)\n", id, pingstats.model_number,
dxl_get_model_name(pingstats.model_number).c_str());
// Other informations, not printed by default:
TRACE_1(DAPI, "[#%i] firmware: '%i' \n", id, pingstats.firmware_version);
TRACE_1(DAPI, "[#%i] position: '%i' \n", id, readCurrentPosition(id));
TRACE_1(DAPI, "[#%i] speed: '%i' \n", id, readCurrentSpeed(id));
TRACE_1(DAPI, "[#%i] torque: '%i' \n", id, getTorqueEnabled(id));
TRACE_1(DAPI, "[#%i] load: '%i' \n", id, readCurrentLoad(id));
TRACE_1(DAPI, "[#%i] baudrate: '%i' \n", id, getSetting(id, REG_BAUD_RATE));
setLed(id, 0);
}
else
{
printf(".");
}
}
printf("\n");
return ids;
}
bool dynamixelApi_connect(int actId) {
dxl_ping(actId);
if (dxl_get_result() == COMM_RXSUCCESS) {
ase_printf("SUCCESS: Found Dynamixel with id = %i renamed to %i \n", actId, dyna.nActuators);
int wValue = dxl_read_word(actId, P_MODEL_NUMBER_L);
if (dxl_get_result() == COMM_RXSUCCESS)
ase_printf("Model number:%d, ", wValue);
int bValue = dxl_read_byte(actId, P_VERSION);
if (dxl_get_result() == COMM_RXSUCCESS)
ase_printf("Version:%d\n", bValue);
dyna.actuators[dyna.nActuators] = actId;
dynamixelApi_setWheelMode(dyna.nActuators, false);
dyna.nActuators++;
return true;
}
else {
ase_printf("ERROR: Unable to connect to Dynamixel with id = %i\n", actId);
return false;
}
}
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::ping(int id){
dxl_ping( id );
}
bool DynamixelSimpleAPI::ping(const int id, PingResponse *status)
{
return dxl_ping(id, status);
}
// 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);
}
