diff --git a/CMakeLists.txt b/CMakeLists.txt index 9df560a..40aff92 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -55,5 +55,12 @@ target_sources(dractest PUBLIC ${COMMON_FILES}) target_link_libraries(dractest Amp1394 OpenGL::GL OpenGL::GLU glfw GLEW::GLEW ${CMAKE_DL_LIBS}) set_property(TARGET dractest PROPERTY CXX_STANDARD 20) +add_executable(dractune dractune.cpp) +target_sources(dractune PUBLIC ${COMMON_FILES}) +target_link_libraries(dractune Amp1394 OpenGL::GL OpenGL::GLU glfw GLEW::GLEW ${CMAKE_DL_LIBS}) +set_property(TARGET dractune PROPERTY CXX_STANDARD 20) + + install(TARGETS dractweak DESTINATION bin) -install(TARGETS dractest DESTINATION bin) \ No newline at end of file +install(TARGETS dractest DESTINATION bin) +install(TARGETS dractune DESTINATION bin) \ No newline at end of file diff --git a/dractest.cpp b/dractest.cpp index bf88455..e36b667 100644 --- a/dractest.cpp +++ b/dractest.cpp @@ -347,7 +347,7 @@ void test_all() { const std::lock_guard lock(port_mutex); board->SetPowerEnable(1); } - sleep(500); + sleep(1000); test_48v(); // test_safety_chain(); test_adc_zero(); @@ -572,6 +572,7 @@ int main(int, char**) std::string portDescription = ethfw ? "udp" : "fw"; std::stringstream().swap(ss_console); Port = PortFactory(portDescription.c_str(), ss_console); + sleep(500, false); if (Port && Port->IsOK()) { board = new AmpIO(board_id); Port->AddBoard(board); @@ -931,4 +932,4 @@ int main(int, char**) glfwTerminate(); return 0; -} \ No newline at end of file +} diff --git a/dractune.cpp b/dractune.cpp new file mode 100644 index 0000000..441639d --- /dev/null +++ b/dractune.cpp @@ -0,0 +1,470 @@ +#define IMGUI_IMPL_OPENGL_LOADER_GLEW +#include +#include "PortFactory.h" +#include "AmpIO.h" +#include "Amp1394Time.h" +#include "EthBasePort.h" +#include +#include +#include +#include "CRC.h" +#include "imgui.h" +#include "imgui_impl_glfw.h" +#include "imgui_impl_opengl3.h" +#include "imgui_stdlib.h" +#include +#if defined(IMGUI_IMPL_OPENGL_ES2) +#include +#endif +#include // Will drag system OpenGL headers +#include +#include +#include +#include +#include +#include +#include "implot.h" + + +const std::string version = "1.0.0"; +const auto mv_bit_to_volt = 1.31255e-3; +int command_wait_time = 20; + +std::stringstream ss_console; + +std::mutex port_mutex; +std::binary_semaphore rt_rw_sem{0}; +bool testing = false; + +void sleep(int t, bool wait_for_rt=true) { + if (wait_for_rt) + rt_rw_sem.acquire(); + std::this_thread::sleep_for(std::chrono::milliseconds(t)); + if (wait_for_rt) + rt_rw_sem.acquire(); +} + +void pass(std::string s="") { + std::cout << std::setw(13) << "\U0001f7e2 Pass" << std::setw(30) << s << std::endl; +} + +void fail(std::string s="") { + std::cout << std::setw(13) << "\U0001F4A9 Fail" << std::setw(30) << s << std::endl; +} + +void test(std::string s) { + std::cout << std::setw(20) << s; +} + + +const auto red = ImVec4(1.0f, 0.0f, 0.0f, 1.0f); +const auto green = ImVec4(0.0f, 1.0f, 0.0f, 1.0f); +const auto white = ImVec4(1.0f, 1.0f, 1.0f, 1.0f); +ImU32 cell_bg_pass; +ImU32 cell_bg_fail; +ImU32 cell_bg_in_progress; +ImU32 cell_bg_untested; + + + +static AmpIO* board; +static BasePort* Port; +static int board_id = 0; +static bool board_connected = false; +const int num_axes = 10; +int motor_current_read[num_axes]; +float motor_voltage_read[num_axes]; +int amp_fault_codes[num_axes]; +float mv = 12.0; +const float amps_to_bits[10] = {4800.0, 4800.0, 16000.0, 16000.0, 16000.0, 16000.0, 16000.0, 16000.0, 16000.0, 16000.0}; +const float internal_res_hr = 0.40; +const float internal_res_lr = 0.46; +const float internal_resistance[10] = {internal_res_hr,internal_res_hr,internal_res_lr,internal_res_lr,internal_res_lr,internal_res_lr,internal_res_lr,internal_res_lr,internal_res_lr,internal_res_lr}; +const char* channel_names[10] = {"M1", "M2", "M3", "M4", "M5", "M6", "M7", "B1", "B2", "B3"}; +const char* amp_fault_text[16] = {"-", "ADC saturated", "Current deviation", "HW overcurrent", "HW overtemp", "Undefined", "Undefined", "Undefined", "Undefined", "Undefined", "Undefined", "Undefined", "Undefined", "Undefined", "Undefined", "Undefined"}; +static std::string BoardSN; +static std::string BoardSNRead; + +int tuning_pulse_width = 100; +std::vector current_history; +std::vector current_history_t; + +int tuning_axis_index = 0; +int kp, ki; +int ilim, olim; + + + +void read_quadlet_threadsafe(unsigned char board_id, nodeaddr_t address, quadlet_t &data) { + const std::lock_guard lock(port_mutex); + Port->ReadQuadlet(board_id, address, data); +} + +void write_quadlet_threadsafe(unsigned char board_id, nodeaddr_t address, quadlet_t data) { + const std::lock_guard lock(port_mutex); + Port->WriteQuadlet(board_id, address, data); +} + +void update_all_boards() { + static int consecutive_failures = 0; + while (true) { + if (board_connected){ + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + const std::lock_guard lock(port_mutex); + if (Port && Port->IsOK()) { + auto w_result = Port->WriteAllBoards(); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + auto r_result = Port->ReadAllBoards(); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + + if (w_result == false || r_result == false) { + consecutive_failures++; + std::cerr << "eth fail" << std::endl; + + if (consecutive_failures > 200) { + board_connected = false; + std::cerr << "Board disconnected" << std::endl; + } + } else { + consecutive_failures = 0; + } + rt_rw_sem.release(); + } + } else { + consecutive_failures = 0; + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } +} + + +bool collect_cb(quadlet_t* data, short num_avail) { + for (int i = 0; i < num_avail; i++) { + quadlet_t value = data[i]; + bool type = ((value&0x80000000)>>31); + bool timer_overflow = ((value&0x40000000)>>30); + int timer = ((value&0x3FFF0000)>>16); + int data = (value&0x0000FFFF); + if (type == 1) { + current_history.push_back((data - 0x8000) / amps_to_bits[tuning_axis_index]); + current_history_t.push_back(1.0f / 80e3 * current_history.size()); + // printf("collect %d \n", num_avail); + } + // printf("sz %d \n", current_history.size()); + // printf("type %d, ovf %d, timer %d, data %d\n", type, timer_overflow, timer, data); + } + // if (num_avail != 0) printf("dat col cb %d buffer %d\n", num_avail, current_history.size()); + if (current_history.size() > tuning_pulse_width && board->IsCollecting()) { + board->DataCollectionStop(); + board->SetMotorCurrent(tuning_axis_index, 0x8000); + } + // return num_avail != 0 && board->IsCollecting(); + return true; +} + + +static void glfw_error_callback(int error, const char* description) +{ + fprintf(stderr, "Glfw Error %d: %s\n", error, description); +} + +int main(int, char**) +{ + current_history.reserve(65536); + current_history_t.reserve(65536); + + // Setup window + glfwSetErrorCallback(glfw_error_callback); + if (!glfwInit()) + return 1; + + // Decide GL+GLSL versions +#if defined(IMGUI_IMPL_OPENGL_ES2) + // GL ES 2.0 + GLSL 100 + const char* glsl_version = "#version 100"; + glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 2); + glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 0); + glfwWindowHint(GLFW_CLIENT_API, GLFW_OPENGL_ES_API); +#elif defined(__APPLE__) + // GL 3.2 + GLSL 150 + const char* glsl_version = "#version 150"; + glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3); + glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 2); + glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE); // 3.2+ only + glfwWindowHint(GLFW_OPENGL_FORWARD_COMPAT, GL_TRUE); // Required on Mac +#else + // GL 3.0 + GLSL 130 + const char* glsl_version = "#version 130"; + glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3); + glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 0); + //glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE); // 3.2+ only + //glfwWindowHint(GLFW_OPENGL_FORWARD_COMPAT, GL_TRUE); // 3.0+ only +#endif + + // Create window with graphics context + GLFWwindow* window = glfwCreateWindow(1280, 720, "dRAC factory test", NULL, NULL); + if (window == NULL) + return 1; + glfwMakeContextCurrent(window); + glfwSwapInterval(1); // Enable vsync + + // Setup Dear ImGui context + IMGUI_CHECKVERSION(); + ImGui::CreateContext(); + ImPlot::CreateContext(); + ImGuiIO& io = ImGui::GetIO(); (void)io; + //io.ConfigFlags |= ImGuiConfigFlags_NavEnableKeyboard; // Enable Keyboard Controls + //io.ConfigFlags |= ImGuiConfigFlags_NavEnableGamepad; // Enable Gamepad Controls + + // Setup Dear ImGui style + ImGui::StyleColorsDark(); + //ImGui::StyleColorsLight(); + + // Setup Platform/Renderer backends + ImGui_ImplGlfw_InitForOpenGL(window, true); + ImGui_ImplOpenGL3_Init(glsl_version); + + // Load Fonts + // - If no fonts are loaded, dear imgui will use the default font. You can also load multiple fonts and use ImGui::PushFont()/PopFont() to select them. + // - AddFontFromFileTTF() will return the ImFont* so you can store it if you need to select the font among multiple. + // - If the file cannot be loaded, the function will return NULL. Please handle those errors in your application (e.g. use an assertion, or display an error and quit). + // - The fonts will be rasterized at a given size (w/ oversampling) and stored into a texture when calling ImFontAtlas::Build()/GetTexDataAsXXXX(), which ImGui_ImplXXXX_NewFrame below will call. + // - Read 'docs/FONTS.md' for more instructions and details. + // - Remember that in C/C++ if you want to include a backslash \ in a string literal you need to write a double backslash \\ ! + //io.Fonts->AddFontDefault(); + //io.Fonts->AddFontFromFileTTF("../../misc/fonts/Roboto-Medium.ttf", 16.0f); + //io.Fonts->AddFontFromFileTTF("../../misc/fonts/Cousine-Regular.ttf", 15.0f); + //io.Fonts->AddFontFromFileTTF("../../misc/fonts/DroidSans.ttf", 16.0f); + //io.Fonts->AddFontFromFileTTF("../../misc/fonts/ProggyTiny.ttf", 10.0f); + //ImFont* font = io.Fonts->AddFontFromFileTTF("c:\\Windows\\Fonts\\ArialUni.ttf", 18.0f, NULL, io.Fonts->GetGlyphRangesJapanese()); + //IM_ASSERT(font != NULL); + // ImGuiIO& io = ImGui::GetIO(); + // io.Fonts->AddFontFromFileTTF("/usr/share/fonts/truetype/noto/NotoMono-Regular.ttf", 16.0f); + // io.Fonts->AddFontFromFileTTF("FiraMono-Medium.ttf", 16.0f); + + // Our state + bool show_demo_window = false; + bool show_another_window = true; + ImVec4 clear_color = ImVec4(0.45f, 0.55f, 0.60f, 1.00f); + cell_bg_pass = ImGui::GetColorU32(ImVec4(0.0f, 0.3f, 0.0f, 0.7f)); + cell_bg_fail = ImGui::GetColorU32(ImVec4(0.5f, 0.0f, 0.0f, 0.7f)); + cell_bg_in_progress = ImGui::GetColorU32(ImVec4(0.3f, 0.3f, 0.0f, 0.5f)); + cell_bg_untested = ImGui::GetColorU32(ImVec4(1.0f, 1.0f, 1.0f, 0.1f)); + std::thread(update_all_boards).detach(); + + // Main loop + while (!glfwWindowShouldClose(window)) + { + // Poll and handle events (inputs, window resize, etc.) + // You can read the io.WantCaptureMouse, io.WantCaptureKeyboard flags to tell if dear imgui wants to use your inputs. + // - When io.WantCaptureMouse is true, do not dispatch mouse input data to your main application, or clear/overwrite your copy of the mouse data. + // - When io.WantCaptureKeyboard is true, do not dispatch keyboard input data to your main application, or clear/overwrite your copy of the keyboard data. + // Generally you may always pass all inputs to dear imgui, and hide them from your application based on those two flags. + glfwPollEvents(); + + // Start the Dear ImGui frame + ImGui_ImplOpenGL3_NewFrame(); + ImGui_ImplGlfw_NewFrame(); + ImGui::NewFrame(); + int width, height; + glfwGetWindowSize(window, &width, &height); + ImGui::SetNextWindowSize(ImVec2(width, height)); // ensures ImGui fits the GLFW window + ImGui::SetNextWindowPos(ImVec2(0, 0)); + + ImGui::Begin("dRAC factory test"); + ImGui::Text("This program tests the dRAC boards with special termination plug. Do not run the test with robot connected. You may damage the robot."); + ImGui::PushItemWidth(100); + // ImGui::InputInt("Command to read delay", &command_wait_time, 10, 100); + // ImGui::SameLine(); + // ImGui::InputInt("kp", &kp); + // ImGui::SameLine(); + // ImGui::InputInt("ki", &ki); + static int ethfw = 1; + ImGui::InputInt("Board number", &board_id); ImGui::SameLine(); + ImGui::RadioButton("Firewire", ðfw, 0); ImGui::SameLine(); + ImGui::RadioButton("Ethernet", ðfw, 1); ImGui::SameLine(); + + if (ImGui::Button("Connect", ImVec2(300, 0))) { + const std::lock_guard lock(port_mutex); + board_connected = false; + BasePort::ProtocolType protocol = BasePort::PROTOCOL_SEQ_RW; + std::string portDescription = ethfw ? "udp" : "fw"; + std::stringstream().swap(ss_console); + Port = PortFactory(portDescription.c_str(), ss_console); + sleep(500, false); + if (Port && Port->IsOK()) { + board = new AmpIO(board_id); + Port->AddBoard(board); + board_connected = (Port && Port->GetNumOfNodes() > 0); + board_connected = (Port && Port->GetNumOfNodes() > 0); + BoardSN.clear(); + BoardSNRead.clear(); + + if (board_connected) { + // const std::lock_guard lock(port_mutex); + sleep(10, false); + BoardSNRead = board->GetQLASerialNumber(0); + BoardSN = BoardSNRead; + board->WriteWatchdogPeriodInSeconds(0.1); + Port->WriteQuadlet(board_id, 0xB100, 0b11111110); // bypass safety, except the watchdog + Port->WriteAllBoards(); + // for (int i = 0; i < num_axes; i++){ + // board->WriteCurrentKpRaw(i, kp); + // board->WriteCurrentKiRaw(i, ki); + // board->WriteCurrentITermLimitRaw(i, 100); + // } + } + } + } + ImGui::SameLine(); + if (board_connected) { + ImGui::TextColored(green, "Connected"); + } else { + ImGui::TextColored(red, "Not connected"); + } + // ImGui::SameLine(); + + ImGui::Separator(); + + + // EthBasePort *ethPort = dynamic_cast(Port); + if (board_connected) { + ImGui::Separator(); + ImGui::BeginTable("rt monitor", 11); + ImGui::TableNextColumn(); + ImGui::Text("Channel"); + for (int axis_index = 0; axis_index < 10; axis_index++) { + ImGui::TableNextColumn(); + ImGui::Text(channel_names[axis_index]); + } + ImGui::TableNextColumn(); + ImGui::Text("Power"); + for (int axis_index = 0; axis_index < 10; axis_index++) { + ImGui::TableNextColumn(); + auto axis_en = board->GetAmpEnable(axis_index); + ImGui::TextColored(axis_en ? green : white, axis_en ? "On" : "Off"); + } + ImGui::TableNextColumn(); + ImGui::Text("Current"); + for (int axis_index = 0; axis_index < 10; axis_index++) { + motor_current_read[axis_index] = board->GetMotorCurrent(axis_index); + ImGui::TableNextColumn(); + // ImGui::Text(channel_names[axis_index]); + ImGui::Text("0x%04X", motor_current_read[axis_index]); + ImGui::Text("% 1.03f A", (motor_current_read[axis_index] - 0x8000) / amps_to_bits[axis_index]); + } + ImGui::TableNextColumn(); + ImGui::Text("Voltage"); + for (int axis_index = 0; axis_index < 10; axis_index++) { + motor_voltage_read[axis_index] = board->GetMotorVoltageRatio(axis_index); + ImGui::TableNextColumn(); + ImGui::Text("% 2.03f V", mv * motor_voltage_read[axis_index]); + } + ImGui::TableNextColumn(); + ImGui::Text("Fault"); + ImGui::SameLine(); + ImGui::TextDisabled("(?)"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Faults will disable the corresponding channel. Faults are sticky and can be cleared when enabling the channel. \nHW overcurrent and overtemp faults will affect the other channel on the same chip.\nIf ADC satuation, check AD4008 and INA240. If HW overcurrent or overtemp, check DRV8432."); + + for (int axis_index = 0; axis_index < 10; axis_index++) { + amp_fault_codes[axis_index] = board->GetAmpFaultCode(axis_index); + ImGui::TableNextColumn(); + if (amp_fault_codes[axis_index] == 0) { + // ImGui::TableSetBgColor(ImGuiTableBgTarget_CellBg, cell_bg_pass); + } else { + ImGui::TableSetBgColor(ImGuiTableBgTarget_CellBg, cell_bg_fail); + } + ImGui::Text(amp_fault_text[amp_fault_codes[axis_index]]); + } + ImGui::EndTable(); + ImGui::PopItemWidth(); + ImGui::PushItemWidth(200); + + + ImGui::Separator(); + + if (ImGui::Button("board enable")) { + const std::lock_guard lock(port_mutex); + board->SetPowerEnable(true); + } + + ImGui::InputInt("Tuning axis", &tuning_axis_index); + ImGui::SameLine(); + ImGui::Text(channel_names[tuning_axis_index]); + + + + if (ImGui::Button("read")) { + const std::lock_guard lock(port_mutex); + kp = board->ReadCurrentKpRaw(tuning_axis_index); + ki = board->ReadCurrentKiRaw(tuning_axis_index); + ilim = board->ReadCurrentITermLimitRaw(tuning_axis_index); + olim = board->ReadDutyCycleLimit(tuning_axis_index); + } + + if (ImGui::Button("enable")) { + const std::lock_guard lock(port_mutex); + board->SetAmpEnable(tuning_axis_index, true); + } + + ImGui::InputInt("tuning pulse width", &tuning_pulse_width, 1, 10); + ImGui::DragInt("kp", &kp, 1, 0, 0x3ffff); + ImGui::DragInt("ki", &ki, 1, 0, 0x3ffff); + ImGui::InputInt("i term lim", &ilim, 1, 10); + ImGui::InputInt("duty cycle lim", &olim, 1, 10); + if (ImGui::Button("write")) { + const std::lock_guard lock(port_mutex); + Port->WriteQuadlet(board_id, 0x9000 | (tuning_axis_index+1) << 4 | 0x0c, tuning_pulse_width); + board->WriteCurrentKpRaw(tuning_axis_index, kp); + board->WriteCurrentKiRaw(tuning_axis_index, ki); + board->WriteCurrentITermLimitRaw(tuning_axis_index, ilim); + board->WriteDutyCycleLimit(tuning_axis_index, olim); + + } + if (ImGui::Button("pulse 0.1A")) { + const std::lock_guard lock(port_mutex); + current_history.clear(); + current_history_t.clear(); + board->DataCollectionStart(tuning_axis_index + 1, collect_cb); + board->SetMotorCurrent(tuning_axis_index, 0.1 * amps_to_bits[tuning_axis_index] + 0x8000); + } + ImGui::Text(board->IsCollecting() ? "Collecting" : "Not collecting"); + // current_history_t.push_back(0.1); + // current_history.push_back(0.1); + static ImPlotFlags flags = ImPlotFlags_NoLegend; + static ImPlotAxisFlags xflags = ImPlotAxisFlags_AutoFit|ImPlotAxisFlags_NoGridLines; + static ImPlotAxisFlags yflags = ImPlotAxisFlags_AutoFit|ImPlotAxisFlags_NoGridLines; + if (ImPlot::BeginPlot("##Tuning","t","I",ImVec2(-1,-1),flags,xflags,yflags)) { + ImPlot::PlotLine("I measured", current_history_t.data(), current_history.data(), tuning_pulse_width + 200); + ImPlot::EndPlot(); + } + } + ImGui::Text(ss_console.str().c_str()); + + + + ImGui::End(); + + // Rendering + ImGui::Render(); + int display_w, display_h; + glfwGetFramebufferSize(window, &display_w, &display_h); + glViewport(0, 0, display_w, display_h); + glClearColor(clear_color.x * clear_color.w, clear_color.y * clear_color.w, clear_color.z * clear_color.w, clear_color.w); + glClear(GL_COLOR_BUFFER_BIT); + ImGui_ImplOpenGL3_RenderDrawData(ImGui::GetDrawData()); + + glfwSwapBuffers(window); + } + + // Cleanup + ImGui_ImplOpenGL3_Shutdown(); + ImGui_ImplGlfw_Shutdown(); + ImGui::DestroyContext(); + + glfwDestroyWindow(window); + glfwTerminate(); + + return 0; +}