diff --git a/MotionCore/Arm.cpp b/MotionCore/Arm.cpp index 454b55a..f087ed3 100755 --- a/MotionCore/Arm.cpp +++ b/MotionCore/Arm.cpp @@ -12,8 +12,8 @@ Arm::Arm(const std::wstring& comPort) noexcept: j0( oSerialBus, Vector3d::Y, - Vector3d::Up, - 100.0, //mm + Vector3d::Forward, + 0.0, //mm 0, -135.0, 45.0, @@ -22,8 +22,8 @@ Arm::Arm(const std::wstring& comPort) noexcept: ), j1( oSerialBus, - Vector3d::X, - Vector3d::Forward, + -Vector3d::X, + Vector3d::Up, 72.0, //mm 1, -38.0, @@ -33,8 +33,8 @@ Arm::Arm(const std::wstring& comPort) noexcept: ), j2( oSerialBus, - Vector3d::X, - Vector3d::Forward, + -Vector3d::X, + Vector3d::Up, 95.0, //mm 2, -123, diff --git a/MotionCore/ArmSegment.cpp b/MotionCore/ArmSegment.cpp index fca0295..6c2470f 100755 --- a/MotionCore/ArmSegment.cpp +++ b/MotionCore/ArmSegment.cpp @@ -106,10 +106,8 @@ Vector3d ArmSegment::getGlobalEndpoint() const Vector3d p(0.0, 0.0, 0.0); for (const ArmSegment* seg : chain) { - r *= Matrix3x3::fromAxisAngle(seg->rotationAxis, seg->motor.getCurrentPosition()); - - Vector3d d = seg->longitudinalAxis * seg->length; // link offset in local frame - p = p + r * d; + r *= Matrix3x3::fromAxisAngle(seg->rotationAxis, seg->motor.getCurrentPosition()); // LOCAL axis + p += r * (seg->longitudinalAxis * seg->length); } return p; diff --git a/MotionCore/MainLoop.cpp b/MotionCore/MainLoop.cpp index 584330f..855e090 100755 --- a/MotionCore/MainLoop.cpp +++ b/MotionCore/MainLoop.cpp @@ -1,18 +1,16 @@ -#define WIN32_LEAN_AND_MEAN #include "MainLoop.h" #include #include #include -#include #include #include +#include "yz_plotter_win.h" MainLoop::MainLoop() noexcept : arm(L"COM4"), electronicsBB( - Vector3d(100, 0, -20), - //Vector3d(65, 30, 75) - Vector3d(1000, 30, 1000) + Vector3d(-125, -45, -20), + Vector3d(65, 20, 120) ), leftVKB(L" VKBsim Gladiator EVO L ") { @@ -25,8 +23,6 @@ MainLoop::MainLoop() noexcept : std::cerr << "Unable to connect to left VKB controller!" << std::endl; exit(-1); } - - SetProcessDPIAware(); } MainLoop& MainLoop::getInstance() @@ -46,7 +42,9 @@ void MainLoop::run() { static std::chrono::steady_clock::time_point lastBegin = std::chrono::high_resolution_clock::now(); - + + //YZConsolePlotter plot(-200, 200, 20, 2); + while (isRunning) { std::chrono::steady_clock::time_point begin = @@ -71,14 +69,16 @@ void MainLoop::run() arm.getJ1().moveBy(moveJ1By, std::nullopt); arm.getJ2().moveBy(moveJ2By, std::nullopt); - //std::cout << "DJ0: " << moveJ0By << " DJ1: " << moveJ1By << " DJ2: " << moveJ2By << std::endl; - std::cout << arm.getJ2().getGlobalEndpoint() << std::endl; - + const Vector3d ef = arm.getJ2().getGlobalEndpoint(); + std::cout << ef << std::endl; + //plot.draw(ef); + arm.update(frametime); // Kill if endeffector enters electronics area if (electronicsBB.doesIntersect(arm.getJ2().getGlobalEndpoint())) { - //isRunning = false; + std::cout << "Terminating because of AABB violation..." << std::endl; + isRunning = false; } lastBegin = std::chrono::high_resolution_clock::now(); diff --git a/MotionCore/MotionCore.vcxproj b/MotionCore/MotionCore.vcxproj index d35ab9e..21f4c08 100755 --- a/MotionCore/MotionCore.vcxproj +++ b/MotionCore/MotionCore.vcxproj @@ -164,6 +164,7 @@ + diff --git a/MotionCore/MotionCore.vcxproj.filters b/MotionCore/MotionCore.vcxproj.filters index e50c934..f178124 100755 --- a/MotionCore/MotionCore.vcxproj.filters +++ b/MotionCore/MotionCore.vcxproj.filters @@ -74,5 +74,8 @@ Headerdateien + + Headerdateien + \ No newline at end of file diff --git a/MotionCore/main.cpp b/MotionCore/main.cpp index 1f5521a..189e3bf 100755 --- a/MotionCore/main.cpp +++ b/MotionCore/main.cpp @@ -1,4 +1,3 @@ - #include "MainLoop.h" int main() diff --git a/MotionCore/yz_plotter_win.h b/MotionCore/yz_plotter_win.h new file mode 100755 index 0000000..5d84875 --- /dev/null +++ b/MotionCore/yz_plotter_win.h @@ -0,0 +1,293 @@ +// Externally sourced 2D plotter for data visualization + +// yz_plotter_win.h +#pragma once + +// Stop Windows from defining min/max macros (and nuke them if already defined) +#ifndef NOMINMAX +#define NOMINMAX +#endif +#include +#ifdef min +#undef min +#endif +#ifdef max +#undef max +#endif + +#include +#include +#include +#include +#include +#include + +/// Compute a step (units per pixel) that yields about `targetCells` pixels +/// across the given range [minVal..maxVal]. For [-100..100] span=200: +/// targetCells=21 -> step ~10, targetCells=11 -> step ~20, etc. +inline int stepForTargetCells(int minVal, int maxVal, int targetCells) +{ + targetCells = std::max(2, targetCells); + const int span = maxVal - minVal; + const double raw = (double)span / (double)(targetCells - 1); + return std::max(1, (int)std::ceil(raw)); +} + +/// Flicker-free terminal plotter (Windows console): +/// - Vertical axis: Y (height) +/// - Horizontal axis: Z (forward/back) +/// - Point drawn as "**" +/// - Axes drawn as "||" and "--" +/// +/// Range stays fixed (e.g. -100..100) while "pixel count" is controlled by `step`. +class YZConsolePlotter { +public: + // step = units per pixel cell (bigger step => fewer pixels) + // cellW = characters per pixel horizontally (2 recommended for Windows fonts) + explicit YZConsolePlotter(int minVal = -100, int maxVal = 100, int step = 10, int cellW = 2) + : minV_(minVal), maxV_(maxVal), step_(step), cellW_(cellW) + { + normalizeParams_(); + recomputeGrid_(); + + hOut_ = GetStdHandle(STD_OUTPUT_HANDLE); + consoleOK_ = (hOut_ != INVALID_HANDLE_VALUE && hOut_ != nullptr); + + if (consoleOK_) { + CONSOLE_SCREEN_BUFFER_INFO csbi{}; + if (!GetConsoleScreenBufferInfo(hOut_, &csbi)) { + consoleOK_ = false; + } + else { + anchor_ = csbi.dwCursorPosition; + + // Hide cursor (optional) + CONSOLE_CURSOR_INFO cci{}; + if (GetConsoleCursorInfo(hOut_, &cci)) { + savedCursor_ = cci; + cci.bVisible = FALSE; + SetConsoleCursorInfo(hOut_, &cci); + cursorSaved_ = true; + } + } + } + + // First draw to "claim" the region + draw(Vector3d{ 0, 0, 0 }); + } + + // Convenience: specify desired pixel count instead of step + static YZConsolePlotter withTargetCells(int minVal, int maxVal, int targetCells, int cellW = 2) + { + return YZConsolePlotter(minVal, maxVal, stepForTargetCells(minVal, maxVal, targetCells), cellW); + } + + ~YZConsolePlotter() { + if (consoleOK_ && cursorSaved_) { + SetConsoleCursorInfo(hOut_, &savedCursor_); + } + } + + void setTitle(std::string t) { title_ = std::move(t); } + void setCompact(bool on) { compact_ = on; } + + // If you want to relocate the plot to "here" (current cursor position) + void resetAnchorHere() { + if (!consoleOK_) return; + CONSOLE_SCREEN_BUFFER_INFO csbi{}; + if (GetConsoleScreenBufferInfo(hOut_, &csbi)) { + anchor_ = csbi.dwCursorPosition; + } + } + + // Main call: draw latest point + void draw(const Vector3d& v) { + const std::string frame = render_(v); + writeAtAnchor_(frame); + } + +private: + int minV_{ -100 }, maxV_{ 100 }, step_{ 10 }; + int cellW_{ 2 }; + + int span_{ 200 }; + int cols_{ 21 }, rows_{ 21 }; + int rowChars_{ 0 }; + + int originC_{ 0 }, originR_{ 0 }; + + HANDLE hOut_{ nullptr }; + COORD anchor_{ 0, 0 }; + bool consoleOK_{ false }; + + bool cursorSaved_{ false }; + CONSOLE_CURSOR_INFO savedCursor_{}; + + bool compact_{ true }; + std::string title_{ "YZ Plot (Y=height vertical, Z=forward horizontal)" }; + + // For clean overwrite: fixed line width + int labelW_{ 6 }; // " 100 " etc + int lineLen_{ 0 }; // labelW_ + rowChars_ + +private: + void normalizeParams_() + { + if (step_ <= 0) step_ = 10; + if (cellW_ <= 0) cellW_ = 2; + if (minV_ >= maxV_) { minV_ = -100; maxV_ = 100; } + } + + void recomputeGrid_() + { + span_ = maxV_ - minV_; + cols_ = span_ / step_ + 1; + rows_ = span_ / step_ + 1; + rowChars_ = cols_ * cellW_; + + originC_ = toCol_(0); + originR_ = toRow_(0); + + lineLen_ = labelW_ + rowChars_; + } + + int clampI_(double a) const { + int ai = (int)std::lround(a); + return std::max(minV_, std::min(maxV_, ai)); + } + + int toCol_(int zVal) const { return (zVal - minV_) / step_; } + int toRow_(int yVal) const { return (maxV_ - yVal) / step_; } // +Y upwards + + void putCell2_(std::string& row, int c, char a, char b) const + { + const int i = c * cellW_; + if (i < 0 || i >= (int)row.size()) return; + row[i] = a; + if (cellW_ >= 2 && i + 1 < (int)row.size()) row[i + 1] = b; + } + + void appendLinePadded_(std::ostringstream& out, const std::string& line) const + { + if ((int)line.size() >= lineLen_) { + out << line.substr(0, (size_t)lineLen_) << "\n"; + } + else { + out << line; + out << std::string((size_t)(lineLen_ - (int)line.size()), ' '); + out << "\n"; + } + } + + void writeAtAnchor_(const std::string& s) + { + if (!consoleOK_) { + return; + } + + SetConsoleCursorPosition(hOut_, anchor_); + DWORD written = 0; + WriteConsoleA(hOut_, s.c_str(), (DWORD)s.size(), &written, nullptr); + } + + std::string render_(const Vector3d& v) const + { + const int yC = clampI_(v.y); + const int zC = clampI_(v.z); + + const int starC = toCol_(zC); + const int starR = toRow_(yC); + + std::vector grid(rows_, std::string((size_t)rowChars_, ' ')); + + // axes + if (originR_ >= 0 && originR_ < rows_) { + for (int c = 0; c < cols_; ++c) putCell2_(grid[originR_], c, '-', '-'); + } + if (originC_ >= 0 && originC_ < cols_) { + for (int r = 0; r < rows_; ++r) putCell2_(grid[r], originC_, '|', '|'); + } + if (originR_ >= 0 && originR_ < rows_ && originC_ >= 0 && originC_ < cols_) { + putCell2_(grid[originR_], originC_, '+', '+'); + } + + // point + if (starR >= 0 && starR < rows_ && starC >= 0 && starC < cols_) { + putCell2_(grid[starR], starC, '*', '*'); + } + + std::ostringstream out; + + // Header (kept short; padded to overwrite cleanly) + if (!compact_) { + appendLinePadded_(out, title_); + { + std::ostringstream h; + h << std::fixed << std::setprecision(1) + << "raw x=" << std::setw(7) << v.x + << " y=" << std::setw(7) << v.y + << " z=" << std::setw(7) << v.z; + appendLinePadded_(out, h.str()); + } + { + std::ostringstream h; + h << "range[" << minV_ << "," << maxV_ << "] step=" << step_ + << " grid=" << cols_ << "x" << rows_ + << " cellW=" << cellW_; + appendLinePadded_(out, h.str()); + } + appendLinePadded_(out, ""); // blank + } + else { + std::ostringstream h; + h << std::fixed << std::setprecision(0) + << "Y=" << std::setw(5) << v.y + << " Z=" << std::setw(5) << v.z + << " (clamp Y=" << std::setw(4) << yC + << " Z=" << std::setw(4) << zC << ")"; + appendLinePadded_(out, h.str()); + } + + // Grid with sparse Y labels + const int labelStride = step_ * (compact_ ? 20 : 10); + + for (int r = 0; r < rows_; ++r) { + const int yVal = maxV_ - r * step_; + + std::ostringstream line; + if (labelStride > 0 && (yVal % labelStride) == 0) { + line << std::setw(5) << yVal << " "; + } + else { + line << " "; + } + + line << grid[r]; + appendLinePadded_(out, line.str()); + } + + // Extra lines to obliterate previous longer output + appendLinePadded_(out, ""); + appendLinePadded_(out, ""); + + return out.str(); + } +}; + +/* +USAGE: + +// Few pixels over same -100..100 space: +YZConsolePlotter plot(-100, 100, 20, 2); // 11x11 pixels + +// Or pick pixels and auto-compute step: +auto plot = YZConsolePlotter::withTargetCells(-100, 100, 21, 2); // ~21x21 pixels + +plot.setCompact(true); + +while (...) { + Vector3d ef = arm.getGlobalEndpoint(); + plot.draw(ef); + Sleep(16); +} +*/