got forward kinematics working reliably

MotionCore/Arm.cpp

@@ -12,8 +12,8 @@ Arm::Arm(const std::wstring& comPort) noexcept:
 	j0(
 		oSerialBus,
 		Vector3d::Y,
-		Vector3d::Up,
+		Vector3d::Forward,
-		100.0, //mm
+		0.0, //mm
 		0,
 		-135.0,
 		45.0,
@@ -22,8 +22,8 @@ Arm::Arm(const std::wstring& comPort) noexcept:
 	),
 	j1(
 		oSerialBus,
-		Vector3d::X,
+		-Vector3d::X,
-		Vector3d::Forward,
+		Vector3d::Up,
 		72.0, //mm
 		1,
 		-38.0,
@@ -33,8 +33,8 @@ Arm::Arm(const std::wstring& comPort) noexcept:
 	),
 	j2(
 		oSerialBus,
-		Vector3d::X,
+		-Vector3d::X,
-		Vector3d::Forward,
+		Vector3d::Up,
 		95.0, //mm
 		2,
 		-123,

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());
+		r *= Matrix3x3::fromAxisAngle(seg->rotationAxis, seg->motor.getCurrentPosition());  // LOCAL axis
-
+		p += r * (seg->longitudinalAxis * seg->length);
-		Vector3d d = seg->longitudinalAxis * seg->length;  // link offset in local frame
-		p = p + r * d;
 	}
 
 	return p;

MotionCore/MainLoop.cpp

@@ -1,18 +1,16 @@
-#define WIN32_LEAN_AND_MEAN
 #include "MainLoop.h"
 #include <iostream>
 #include <thread>
 #include <chrono>
-#include <Windows.h>
 #include <algorithm>
 #include <numbers>
+#include "yz_plotter_win.h"
 
 MainLoop::MainLoop() noexcept :
     arm(L"COM4"),
     electronicsBB(
-        Vector3d(100, 0, -20),
+        Vector3d(-125, -45, -20),
-        //Vector3d(65, 30, 75)
+        Vector3d(65, 20, 120)
-        Vector3d(1000, 30, 1000)
     ),
     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;
+        const Vector3d ef = arm.getJ2().getGlobalEndpoint();
-        std::cout << arm.getJ2().getGlobalEndpoint() << std::endl;
+        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();

MotionCore/MotionCore.vcxproj

@@ -164,6 +164,7 @@
     <ClInclude Include="OSerialBus.h" />
     <ClInclude Include="Vector3d.h" />
     <ClInclude Include="vkb_controller.h" />
+    <ClInclude Include="yz_plotter_win.h" />
   </ItemGroup>
   <Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
   <ImportGroup Label="ExtensionTargets">

MotionCore/MotionCore.vcxproj.filters

@@ -74,5 +74,8 @@
     <ClInclude Include="vkb_controller.h">
       <Filter>Headerdateien</Filter>
     </ClInclude>
+    <ClInclude Include="yz_plotter_win.h">
+      <Filter>Headerdateien</Filter>
+    </ClInclude>
   </ItemGroup>
 </Project>

MotionCore/main.cpp

@@ -1,4 +1,3 @@
-
 #include "MainLoop.h"
 
 int main()

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 <windows.h>
+#ifdef min
+#undef min
+#endif
+#ifdef max
+#undef max
+#endif
+
+#include <string>
+#include <vector>
+#include <algorithm>
+#include <cmath>
+#include <sstream>
+#include <iomanip>
+
+/// 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<std::string> 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);
+}
+*/