void ServoDynamixel::status()
{
std::lock_guard <std::mutex> lock(access);
std::cout << "Status(#" << servoId << ")" << std::endl;
std::cout << "> model : " << servoModel << std::endl;
std::cout << "> firmware : " << registerTableValues[gid(REG_FIRMWARE_VERSION)] << std::endl;
std::cout << "> baudrate : " << dxl_get_baudrate(registerTableValues[gid(REG_BAUD_RATE)]) << std::endl;
std::cout << ">> speed mode : " << speedMode << std::endl;
std::cout << ">> steps : " << steps << std::endl;
std::cout << ">> runningDegrees : " << runningDegrees << std::endl;
std::cout << "> torque enabled : " << registerTableValues[gid(REG_TORQUE_ENABLE)] << std::endl;
std::cout << "> max torque : " << registerTableValues[gid(REG_MAX_TORQUE)] << std::endl;
std::cout << "> torque limit : " << registerTableValues[gid(REG_TORQUE_LIMIT)] << std::endl;
std::cout << "> goal position : " << registerTableValues[gid(REG_GOAL_POSITION)] << std::endl;
std::cout << "> goal speed : " << registerTableValues[gid(REG_GOAL_SPEED)] << std::endl;
std::cout << "> current position: " << registerTableValues[gid(REG_CURRENT_POSITION)] << std::endl;
std::cout << "> current speed : " << registerTableValues[gid(REG_CURRENT_SPEED)] << std::endl;
std::cout << "> current load : " << registerTableValues[gid(REG_CURRENT_LOAD)] << std::endl;
std::cout << "> current voltage : " << registerTableValues[gid(REG_CURRENT_VOLTAGE)] << std::endl;
std::cout << "> current temp : " << registerTableValues[gid(REG_CURRENT_TEMPERATURE)] << std::endl;
std::cout << "> registered : " << registerTableValues[gid(REG_REGISTERED)] << std::endl;
std::cout << "> moving : " << registerTableValues[gid(REG_MOVING)] << std::endl;
std::cout << "> lock : " << registerTableValues[gid(REG_LOCK)] << std::endl;
std::cout << "> punch : " << registerTableValues[gid(REG_PUNCH)] << std::endl;
}
int SerialPort::checkBaudRate(const int baud)
{
int baudRate = 57600;
// Set default baudrates
if (servoDevices >= SERVO_HERKULEX)
{
// Default baudrate for HerkuleX devices
baudRate = 115200;
}
else if (servoDevices >= SERVO_DYNAMIXEL)
{
if (servoDevices >= SERVO_PRO)
{
// Default baudrate for Dynamixel PRO devices
baudRate = 57600;
}
else if (servoDevices >= SERVO_XL)
{
// Default baudrate for Dynamixel XL-320 devices
baudRate = 1000000;
}
else if (servoDevices >= SERVO_RX)
{
// Default baudrate for Dynamixel RX, EX and MX devices
baudRate = 57600;
}
else if (servoDevices >= SERVO_AX)
{
// Default baudrate for Dynamixel AX devices
baudRate = 1000000;
}
else if (servoDevices >= SERVO_DX)
{
// Default baudrate for Dynamixel DX devices
baudRate = 57600;
}
}
if (baud >= 0)
{
if (servoDevices >= SERVO_HERKULEX)
{
if (baud < 255)
{
// We have a 'baudnum' not a 'baudrate'
baudRate = hkx_get_baudrate(baud);
}
else if (baud >= 57600 && baud <= 1000000)
{
// We already have a 'valid' baudrate
baudRate = baud;
}
else
{
TRACE_ERROR(SERIAL, "Invalid baudrate '%i' bps for a HerkuleX device, using default baudrate value of: '%i' bps\n", baud, baudRate);
}
}
else if (servoDevices >= SERVO_DYNAMIXEL)
{
if (baud < 255)
{
// We have a 'baudnum' not a 'baudrate'
baudRate = dxl_get_baudrate(baud, servoDevices);
}
else if (baud >= 2400 && baud <= 10500000)
{
// We already have a 'valid' baudrate
baudRate = baud;
}
else
{
TRACE_ERROR(SERIAL, "Invalid baudrate '%i' bps for a Dynamixel device, using default baudrate value of: '%i' bps\n", baud, baudRate);
}
}
else
{
if (baud >= 2400 && baud <= 1000000)
{
// We already have a 'valid' baudrate
baudRate = baud;
}
else
{
TRACE_ERROR(SERIAL, "Invalid device class '%i' with baudrate '%i' bps, using default baudrate value of: '%i'bps\n", servoDevices, baud, baudRate);
}
}
}
else
{
TRACE_ERROR(SERIAL, "Invalid baudrate '%i' bps, using default baudrate value of: '%i' bps\n", baud, baudRate);
}
// Apply bandwith restriction depending on the adapter chip
if (serialDevice == SERIAL_USB2DYNAMIXEL || serialDevice == SERIAL_OTHER_FTDI)
{
if (baudRate > 4500000)
{
baudRate = 4500000;
TRACE_ERROR(SERIAL, "Invalid baudrate ('%i' > 4500000): too high for SERIAL_USB2DYNAMIXEL or or FTDI based device, using default baudrate value of: '1000000'\n", baud);
}
}
else if (serialDevice == SERIAL_USB2AX)
{
if (baudRate > 1000000)
{
baudRate = 1000000;
TRACE_ERROR(SERIAL, "Invalid baudrate ('%i' > 1000000): too high for USB2AX device, using default baudrate value of: '1000000'\n", baud);
}
}
else if (serialDevice == SERIAL_ZIG100)
{
if (baudRate > 115200)
{
baudRate = 115200;
TRACE_ERROR(SERIAL, "Invalid baudrate ('%i' > 115200): too high for ZIG-100/110A device, using fallback baudrate value of: '115200'\n", baud);
}
}
return baudRate;
}
int ServoDynamixel::getBaudRate()
{
std::lock_guard <std::mutex> lock(access);
return dxl_get_baudrate(registerTableValues[gid(REG_BAUD_RATE)], servoSerie);
}
int main(int argc, char *argv[])
{
std::cout << std::endl << "======== Smart Servo Framework Tester ========" << std::endl;
int start = 0, stop = 253;
// Argument(s) parsing
if (argc == 1)
{
std::cerr << "ex_advance_scanner: no argument! Using default [0:253] ID scanning range." << std::endl;
}
else
{
for (int i = 1; i < argc; i++)
{
//std::cout << "argv[" << i << "] = " << argv[i] << std::endl;
if (strncmp(argv[i], "-start", sizeof("-start")) == 0)
{
if (argv[i+1] != NULL)
{
start = std::atoi(argv[i+1]);
if (start < 0 || start > 252)
{
start = 0;
}
}
else
{
std::cerr << "-start: value out of boundaries" << std::endl;
}
}
else if (strncmp(argv[i], "-stop", sizeof("-stop")) == 0)
{
if (argv[i+1] != NULL)
{
stop = std::atoi(argv[i+1]);
if (stop < 1 || stop > 253)
{
stop = 253;
}
}
else
{
std::cerr << "-stop: value out of boundaries" << std::endl;
}
}
}
}
std::cout << std::endl << "======== Advance Scanner ========" << std::endl;
std::multimap <int, int> search_results;
// Initialize a Dynamixel "Simple API" instance
DynamixelSimpleAPI dxl;
// Try a new on all 'bandnums'
for (int i = start; i <= stop; i++)
{
// Initialize a serial link for the controller with serial port auto-detection
std::string deviceName = "auto";
if (dxl.connect(deviceName, i) == 0)
{
std::cerr << "> Failed to open a serial link for our SimpleAPI!" << std::endl;
break;
}
// Scan serial port for servos, store results inside a vector
std::vector <int> ids = dxl.servoScan();
// Add results to global search results
if (ids.size() > 0)
{
for (auto id: ids)
{
search_results.insert(std::pair<int, int>(dxl_get_baudrate(i), id));
}
}
// Close device(s)
dxl.disconnect();
}
std::cout << std::endl << "======== SEARCH RESULTS ========" << std::endl;
std::cout << "Search results:\n";
for (auto it: search_results)
{
std::cout << it.first << " => " << it.second << '\n';
}
std::cout << std::endl << "======== EXITING ========" << std::endl;
return EXIT_SUCCESS;
}
