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