From 9b3caf511cae6f8258996ce76b1c5a632efac2d8 Mon Sep 17 00:00:00 2001 From: Leonetienne Date: Sun, 14 Dec 2025 19:59:51 +0100 Subject: [PATCH] made this work with clean mcu firmware --- MotionCore/Arm.cpp | 21 +-- MotionCore/ArmSegment.cpp | 53 ++------ MotionCore/ArmSegment.h | 13 +- MotionCore/MainLoop.cpp | 75 ++++------- MotionCore/MainLoop.h | 2 + MotionCore/MotionCore.vcxproj | 2 + MotionCore/MotionCore.vcxproj.filters | 6 + MotionCore/Motor.cpp | 44 ++++--- MotionCore/Motor.h | 16 +-- MotionCore/OSerialBus.cpp | 21 +-- MotionCore/OSerialBus.h | 8 +- MotionCore/vkb_controller.h | 83 ++++++++++++ MotionCore/vkb_controllerh.cpp | 176 ++++++++++++++++++++++++++ 13 files changed, 366 insertions(+), 154 deletions(-) create mode 100755 MotionCore/vkb_controller.h create mode 100755 MotionCore/vkb_controllerh.cpp diff --git a/MotionCore/Arm.cpp b/MotionCore/Arm.cpp index 55b48f9..454b55a 100755 --- a/MotionCore/Arm.cpp +++ b/MotionCore/Arm.cpp @@ -1,5 +1,11 @@ #include "Arm.h" #include +#include +#include + +#undef min +#undef max +#undef abs Arm::Arm(const std::wstring& comPort) noexcept: oSerialBus(comPort), @@ -9,9 +15,8 @@ Arm::Arm(const std::wstring& comPort) noexcept: Vector3d::Up, 100.0, //mm 0, - 186.0, - 10.0, - 270.0, + -135.0, + 45.0, 0.0, nullptr ), @@ -21,9 +26,8 @@ Arm::Arm(const std::wstring& comPort) noexcept: Vector3d::Forward, 72.0, //mm 1, - 71.0, - 0.0, - 181.0, + -38.0, + 85.0, 0.0, &j0 ), @@ -33,9 +37,8 @@ Arm::Arm(const std::wstring& comPort) noexcept: Vector3d::Forward, 95.0, //mm 2, - 171.0, - 0.0, - 280.0, + -123, + 57.0, 0.0, &j1 ) diff --git a/MotionCore/ArmSegment.cpp b/MotionCore/ArmSegment.cpp index 800e275..fca0295 100755 --- a/MotionCore/ArmSegment.cpp +++ b/MotionCore/ArmSegment.cpp @@ -9,23 +9,21 @@ ArmSegment::ArmSegment( const Vector3d& longitudinalAxis, const double length, const uint8_t motorIndex, - const double mechanicalToVirtualAngleOffset, - const double mechanicalMinAngle, - const double mechanicalMaxAngle, + const double safeMinAngle, + const double safeMaxAngle, const double homeAngle, ArmSegment* parent ) noexcept : rotationAxis(rotationAxis), longitudinalAxis(longitudinalAxis), length(length), - mechanicalToVirtualAngleOffset(mechanicalToVirtualAngleOffset), homeAngle(homeAngle), motor( motorIndex, serialBus, - virtualToMechanicalAngle(homeAngle), - mechanicalMinAngle, - mechanicalMaxAngle, + homeAngle, + safeMinAngle, + safeMaxAngle, 1.0 ), parent(parent) @@ -35,50 +33,35 @@ ArmSegment::ArmSegment( bool ArmSegment::goHome(const std::optional speed) noexcept { - return motor.moveTo( - virtualToMechanicalAngle(homeAngle), - speed - ); + return motor.moveTo(homeAngle, speed); } bool ArmSegment::goHomeImmediate() noexcept { - return motor.moveToImmediate( - virtualToMechanicalAngle(homeAngle) - ); + return motor.moveToImmediate(homeAngle); } bool ArmSegment::moveTo(const double theta, const std::optional speed) noexcept { - return motor.moveTo( - virtualToMechanicalAngle(theta), - speed - ); + return motor.moveTo(theta, speed); } bool ArmSegment::moveToImmediate(const double theta) noexcept { - return motor.moveToImmediate( - virtualToMechanicalAngle(theta) - ); + return motor.moveToImmediate(theta); } bool ArmSegment::moveBy(const double theta, const std::optional speed) noexcept { - return motor.moveBy( - virtualToMechanicalAngle(theta), - speed - ); + return motor.moveBy(theta, speed); } bool ArmSegment::moveByImmediate(const double theta) noexcept { - return motor.moveByImmediate( - virtualToMechanicalAngle(theta) - ); + return motor.moveByImmediate(theta); } bool ArmSegment::stopMoving() noexcept @@ -123,7 +106,7 @@ Vector3d ArmSegment::getGlobalEndpoint() const Vector3d p(0.0, 0.0, 0.0); for (const ArmSegment* seg : chain) { - r *= Matrix3x3::fromAxisAngle(seg->rotationAxis, mechanicalToVirtualAngle(seg->motor.getCurrentPosition())); + r *= Matrix3x3::fromAxisAngle(seg->rotationAxis, seg->motor.getCurrentPosition()); Vector3d d = seg->longitudinalAxis * seg->length; // link offset in local frame p = p + r * d; @@ -147,15 +130,5 @@ Vector3d ArmSegment::getGlobalAngle() const Vector3d ArmSegment::getLocalAngle() const { - return rotationAxis * mechanicalToVirtualAngle(motor.getCurrentPosition()); -} - -double ArmSegment::virtualToMechanicalAngle(const double virtualAngle) const noexcept -{ - return virtualAngle + mechanicalToVirtualAngleOffset; -} - -double ArmSegment::mechanicalToVirtualAngle(const double hardwareAngle) const noexcept -{ - return hardwareAngle - mechanicalToVirtualAngleOffset; + return rotationAxis * motor.getCurrentPosition(); } diff --git a/MotionCore/ArmSegment.h b/MotionCore/ArmSegment.h index f7c2be8..01ba0ac 100755 --- a/MotionCore/ArmSegment.h +++ b/MotionCore/ArmSegment.h @@ -14,9 +14,8 @@ public: const Vector3d& longitudinalAxis, const double length, const uint8_t motorIndex, - const double mechanicalToVirtualAngleOffset, // Mechanical angle at which this segment is neutral to its parent (assuming 0 degrees in virtual space) - const double mechanicalMinAngle, - const double mechanicalMaxAngle, + const double safeMinAngle, + const double safeMaxAngle, const double homeAngle, ArmSegment* parent // May be nullptr ) noexcept; @@ -46,8 +45,8 @@ public: void update(const double frametime); void setSpeed(const double speed) noexcept { motor.setSpeed(speed); }; - double getMinAngle() const noexcept { return mechanicalToVirtualAngle(motor.getMechanicalMinPosition()); }; - double getMaxAngle() const noexcept { return mechanicalToVirtualAngle(motor.getMechanicalMaxPosition()); }; + double getMinAngle() const noexcept { return motor.getSafeMinPos(); }; + double getMaxAngle() const noexcept { return motor.getSafeMaxPos(); }; double getHomeAngle() const noexcept { return homeAngle; }; double getSpeed() const noexcept { return motor.getSpeed(); }; ArmSegment* getParent() { return parent; }; @@ -59,14 +58,10 @@ public: Vector3d getLocalAngle() const; private: - double virtualToMechanicalAngle(const double virtualAngle) const noexcept; - double mechanicalToVirtualAngle(const double hardwareAngle) const noexcept; - Motor motor; Vector3d rotationAxis; Vector3d longitudinalAxis; double length; - double mechanicalToVirtualAngleOffset; double homeAngle; ArmSegment* parent; }; diff --git a/MotionCore/MainLoop.cpp b/MotionCore/MainLoop.cpp index 9b0e315..584330f 100755 --- a/MotionCore/MainLoop.cpp +++ b/MotionCore/MainLoop.cpp @@ -1,3 +1,4 @@ +#define WIN32_LEAN_AND_MEAN #include "MainLoop.h" #include #include @@ -6,23 +7,25 @@ #include #include -// Bullshit macros from windows lib -#undef max -#undef min - -MainLoop::MainLoop() noexcept: +MainLoop::MainLoop() noexcept : arm(L"COM4"), electronicsBB( Vector3d(100, 0, -20), - Vector3d(65, 30, 75) - //Vector3d(1000, 20, 1000) - ) + //Vector3d(65, 30, 75) + Vector3d(1000, 30, 1000) + ), + leftVKB(L" VKBsim Gladiator EVO L ") { if (!arm.assumeHomePoseImmediate()) { std::cerr << "Unable to move arm to home!" << std::endl; exit(-1); } + if (!leftVKB.connect()) { + std::cerr << "Unable to connect to left VKB controller!" << std::endl; + exit(-1); + } + SetProcessDPIAware(); } @@ -43,8 +46,6 @@ void MainLoop::run() { static std::chrono::steady_clock::time_point lastBegin = std::chrono::high_resolution_clock::now(); - - const Vector3d armPos(0, 500, 2560); while (isRunning) { @@ -54,52 +55,30 @@ void MainLoop::run() const double frametime = (begin - lastBegin).count() * 10e-7; // milliseconds - POINT p; - if (GetCursorPos(&p)) - { - Vector3d cursor( - p.x, - -p.y + 1440.0, - 0.0 - ); - - // Vector from arm to cursor - Vector3d toCursor = cursor - armPos; - double vx = std::max(toCursor.x, 0.0); - double vy = toCursor.y; - double vz = toCursor.z; // will be negative (0 - 2560) - - // Horizontal distance (XZ plane) - double horiz = std::sqrt(vx * vx + vz * vz); - - // Yaw (Y): rotation in XZ plane - double yawRad = std::atan2(vx, vz); - double yawDeg = (yawRad * 180.0 / std::numbers::pi) - 150; - - // Pitch (X): rotation in vertical plane - double pitchRad = std::atan2(vy, horiz) * 3.3 - 1.2; - double pitchDegTotal = (pitchRad * 180.0 / std::numbers::pi); - double pitchDegj1 = 90 - (pitchDegTotal / 2.0) - 45; - double pitchDegj2 = -(pitchDegTotal / 2.0) + 45; - - std::cout << arm.getJ2().getGlobalEndpoint() << std::endl; - - // Send yaw/pitch to your arm here... - // arm.setYaw(yawRad); - // arm.setPitch(pitchRad); - arm.getJ0().moveTo(yawDeg, std::nullopt); - arm.getJ1().moveTo(pitchDegj1, std::nullopt); - arm.getJ2().moveTo(pitchDegj2, std::nullopt); - } if (GetAsyncKeyState(VK_ESCAPE)) { isRunning = false; } + constexpr double speed = 3000; + VkbInputs vkbi = leftVKB.get(); + + const double moveJ0By = -vkbi.roll * speed * frametime; + const double moveJ1By = -vkbi.pitch * speed * frametime; + const double moveJ2By = -vkbi.yaw * speed * frametime; + + arm.getJ0().moveBy(moveJ0By, std::nullopt); + 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; + arm.update(frametime); + // Kill if endeffector enters electronics area if (electronicsBB.doesIntersect(arm.getJ2().getGlobalEndpoint())) { - isRunning = false; + //isRunning = false; } lastBegin = std::chrono::high_resolution_clock::now(); diff --git a/MotionCore/MainLoop.h b/MotionCore/MainLoop.h index 2147bff..b373188 100755 --- a/MotionCore/MainLoop.h +++ b/MotionCore/MainLoop.h @@ -1,6 +1,7 @@ #pragma once #include "Arm.h" #include "AxisAlignedBoundingBox.h" +#include "vkb_controller.h" // Singleton-class, use MainLoop::getInstance() to get the only instance of the runtime class MainLoop @@ -19,4 +20,5 @@ private: Arm arm; AxisAlignedBoundingBox electronicsBB; + VKBSimController leftVKB; }; diff --git a/MotionCore/MotionCore.vcxproj b/MotionCore/MotionCore.vcxproj index 8c50d41..d35ab9e 100755 --- a/MotionCore/MotionCore.vcxproj +++ b/MotionCore/MotionCore.vcxproj @@ -152,6 +152,7 @@ + @@ -162,6 +163,7 @@ + diff --git a/MotionCore/MotionCore.vcxproj.filters b/MotionCore/MotionCore.vcxproj.filters index bbe2c0c..e50c934 100755 --- a/MotionCore/MotionCore.vcxproj.filters +++ b/MotionCore/MotionCore.vcxproj.filters @@ -42,6 +42,9 @@ Quelldateien + + Quelldateien + @@ -68,5 +71,8 @@ Headerdateien + + Headerdateien + \ No newline at end of file diff --git a/MotionCore/Motor.cpp b/MotionCore/Motor.cpp index 6b1a20d..ba96937 100755 --- a/MotionCore/Motor.cpp +++ b/MotionCore/Motor.cpp @@ -1,6 +1,7 @@ #include "Motor.h" #include #include +#include constexpr double RAMPUP_REDUCTION_FAC = 0.1; constexpr double RAMPUP_RECOVERY_SPEED = 0.1; @@ -11,8 +12,8 @@ Motor::Motor( const uint8_t index, OSerialBus& serialBus, const double initialPosition, - const double mechanicalMinPosition, - const double mechanicalMaxPosition, + const double safeMinPos, + const double safeMaxPos, const double initialSpeed ) noexcept: index(index), @@ -20,8 +21,8 @@ Motor::Motor( currentPosition(initialPosition), targetPosition(currentPosition), moveStartPosition(currentPosition), - mechanicalMinPosition(mechanicalMinPosition), - mechanicalMaxPosition(mechanicalMaxPosition) + safeMinPos(safeMinPos), + safeMaxPos(safeMaxPos) { setSpeed(initialSpeed); } @@ -32,8 +33,8 @@ Motor::Motor(Motor&& other) noexcept: currentPosition(other.currentPosition), targetPosition(other.targetPosition), moveStartPosition(other.moveStartPosition), - mechanicalMinPosition(other.mechanicalMinPosition), - mechanicalMaxPosition(other.mechanicalMaxPosition), + safeMinPos(other.safeMinPos), + safeMaxPos(other.safeMaxPos), targetSpeed(other.targetSpeed), speedLow(other.speedLow) { @@ -52,7 +53,7 @@ bool Motor::moveTo(const double theta, const std::optional speed) noexce setSpeed(speed.value()); } - if (!isPositionInMechanicalRange(theta)) { + if (!isPositionInRange(theta)) { return false; } @@ -69,7 +70,7 @@ bool Motor::moveToImmediate(const double theta) noexcept return false; } - if (!isPositionInMechanicalRange(theta)) { + if (!isPositionInRange(theta)) { return false; } @@ -96,7 +97,7 @@ bool Motor::moveBy(const double theta, const std::optional speed) noexce const double acuteTarget = currentPosition + theta; - if (!isPositionInMechanicalRange(acuteTarget)) { + if (!isPositionInRange(acuteTarget)) { return false; } @@ -115,7 +116,7 @@ bool Motor::moveByImmediate(const double theta) noexcept const double acuteTarget = currentPosition + theta; - if (!isPositionInMechanicalRange(acuteTarget)) { + if (!isPositionInRange(acuteTarget)) { return false; } @@ -189,14 +190,14 @@ void Motor::setSpeed(const double speed) noexcept this->speedLow = speed * RAMPUP_REDUCTION_FAC; } -bool Motor::isPositionInMechanicalRange(const double pos) const noexcept +bool Motor::isPositionInRange(const double pos) const noexcept { if (disabled) { std::cerr << "Trying to act on disabled motor object!" << std::endl; return false; } - return pos >= mechanicalMinPosition && pos <= mechanicalMaxPosition; + return pos >= safeMinPos && pos <= safeMaxPos; } void Motor::handleMovement(const double frametime) @@ -218,7 +219,7 @@ void Motor::handleMovement(const double frametime) step = (remaining > 0 ? 1.0 : -1.0) * std::abs(maxStep); } - const double nextPositionStep = std::clamp(currentPosition + step, mechanicalMinPosition, mechanicalMaxPosition); + const double nextPositionStep = std::clamp(currentPosition + step, safeMinPos, safeMaxPos); if (sendMACommand(nextPositionStep)) { currentPosition = nextPositionStep; @@ -227,9 +228,20 @@ void Motor::handleMovement(const double frametime) bool Motor::sendMACommand(const double targetPos) noexcept { - char buffer[32]; - snprintf(buffer, sizeof(buffer), "MA %d %d", index, (int)targetPos); - return serialBus.sendl(buffer); + std::array buffer; // 5-byte packets + + buffer[0] = 0x00; // Instruction: Instruction to MOVE ABSOLUTE + + // Send motor index as u16 + buffer[1] = 0x00; // Motor index MSB: Won't have more than 255 motors + buffer[2] = index; // Motor index LSB, take as-is + + // Send motor degrees as i16 + const std::int16_t deciDegrees = (std::int16_t)(10.0 * targetPos); + buffer[3] = (deciDegrees >> 8) & 0xFF; // Motor degree MSB + buffer[4] = deciDegrees & 0xFF; // Motor degree LSB + + return serialBus.send(buffer.data(), buffer.size()); } double Motor::calculateCurrentSpeed() const noexcept diff --git a/MotionCore/Motor.h b/MotionCore/Motor.h index c7ff2f7..a860f7b 100755 --- a/MotionCore/Motor.h +++ b/MotionCore/Motor.h @@ -9,8 +9,8 @@ public: const uint8_t index, OSerialBus& serialBus, const double initialPosition, - const double mechanicalMinPosition, - const double mechanicalMaxPosition, + const double safeMinPos, + const double safeMaxPos, const double initialSpeed = 0.5 ) noexcept; @@ -38,10 +38,10 @@ public: double getTargetPosition() const noexcept { return targetPosition; }; double getSpeed() const noexcept { return targetSpeed; }; void setSpeed(const double speedTarget) noexcept; - double getMechanicalMinPosition() const noexcept { return mechanicalMinPosition; }; - double getMechanicalMaxPosition() const noexcept { return mechanicalMaxPosition; }; + double getSafeMinPos() const noexcept { return safeMinPos; }; + double getSafeMaxPos() const noexcept { return safeMaxPos; }; double getPaused() const noexcept { return paused; }; - bool isPositionInMechanicalRange(const double pos) const noexcept; + bool isPositionInRange(const double pos) const noexcept; private: // Update-handler for moving @@ -53,15 +53,15 @@ private: double getCurrentMovementProgress() const noexcept; bool isCurrentlyMoving() const noexcept; - const uint8_t index; + const std::uint8_t index; OSerialBus& serialBus; double currentPosition; // we are here double moveStartPosition; // last move instruction started here double targetPosition; // movement should end here double targetSpeed; // Motor should ideally move at this speed double speedLow; // End- and startpoint of ramp-up phase - const double mechanicalMinPosition; - const double mechanicalMaxPosition; + double safeMinPos; + double safeMaxPos; bool disabled = false; bool paused = false; }; diff --git a/MotionCore/OSerialBus.cpp b/MotionCore/OSerialBus.cpp index f3d6e58..dce1386 100755 --- a/MotionCore/OSerialBus.cpp +++ b/MotionCore/OSerialBus.cpp @@ -81,11 +81,10 @@ bool OSerialBus::close() noexcept return CloseHandle(serialHandle); } -bool OSerialBus::send(const std::string& line) noexcept +bool OSerialBus::send(const std::uint8_t* data, std::size_t length) noexcept { // Important: this relies on chartype of line being 1 byte long! - const char* data = line.data(); - std::size_t remaining = line.size(); + std::size_t remaining = length; while (remaining > 0) { DWORD bytesWritten = 0; @@ -104,22 +103,6 @@ bool OSerialBus::send(const std::string& line) noexcept return true; } -bool OSerialBus::sendl(const std::string& line) noexcept -{ - if (!send(line)) { - return false; - } - return send("\n"); -} - -OSerialBus& OSerialBus::operator<<(const std::string& in) -{ - if (!send(in)) { - throw std::runtime_error("Failed to write to COM port!"); - } - return *this; -} - std::wstring OSerialBus::normalizePort(const std::wstring& port) noexcept { // If port string already includes prefix, return as-is diff --git a/MotionCore/OSerialBus.h b/MotionCore/OSerialBus.h index 90932cc..13ed32c 100755 --- a/MotionCore/OSerialBus.h +++ b/MotionCore/OSerialBus.h @@ -1,4 +1,5 @@ #pragma once +#define WIN32_LEAN_AND_MEAN #include #include @@ -13,15 +14,12 @@ public: bool isOpen() const noexcept; bool close() noexcept; - bool send(const std::string& line) noexcept; - bool sendl(const std::string& line) noexcept; + bool send(const std::uint8_t* data, std::size_t length) noexcept; + int getBaudRate() const noexcept { return baudRate; }; const std::wstring& getComPort() const noexcept { return comPort; }; - // Alias for send() - OSerialBus& operator<<(const std::string& in); - private: static std::wstring normalizePort(const std::wstring& port) noexcept; diff --git a/MotionCore/vkb_controller.h b/MotionCore/vkb_controller.h new file mode 100755 index 0000000..2fb9f8d --- /dev/null +++ b/MotionCore/vkb_controller.h @@ -0,0 +1,83 @@ +#pragma once +#define DIRECTINPUT_VERSION 0x0800 +#define WIN32_LEAN_AND_MEAN +#include +#include + +// Frame snapshot +struct VkbInputs { + bool connected = false; + + // raw DI + int x = 0, y = 0, z = 0, rx = 0, ry = 0, rz = 0, s0 = 0, s1 = 0, pov0_deg = -1; + unsigned char btn[128]{}; + + // normalized [-1..1] with deadzones/smoothing applied + float roll = 0.f; // X + float pitch = 0.f; // Y + float yaw = 0.f; // Rz + float thumb_hor = 0.f; // Ry + float thumb_ver = 0.f; // Rx + float wheel = 0.f; // Z + + // named buttons + bool main_trigger_pull = false; // 0 + bool aux_trigger_pull = false; // 21 + bool aux_trigger_push = false; // 20 +}; + +// Per-axis deadzone config (0..0.99). Default 0.05. +struct Deadzones { + float roll = 0.05f, pitch = 0.05f, yaw = 0.05f, thumb_hor = 0.05f, thumb_ver = 0.05f, wheel = 0.05f; +}; + +// Per-axis smoothing alpha (0..1]. 1.0 = no smoothing, lower = smoother. +struct Smoothing { + float roll = 1.f, pitch = 1.f, yaw = 1.f, thumb_hor = 1.f, thumb_ver = 1.f, wheel = 1.f; +}; + +class VKBSimController { +public: + explicit VKBSimController(const wchar_t* exactName); + ~VKBSimController(); + + bool connect(); // (re)connect to exact-name device + void close(); + VkbInputs get(); // poll one frame; auto re-acquire + const wchar_t* name() const { return name_; } + + // Deadzone config + void setDeadzones(const Deadzones& dz); + void setDeadzoneAll(float v); + Deadzones getDeadzones() const { return dz_; } + + // Smoothing config + void enableSmoothing(bool on); + bool smoothingEnabled() const { return smooth_on_; } + void setSmoothing(const Smoothing& s); + void setSmoothingAll(float alpha); + Smoothing getSmoothing() const { return sm_alpha_; } + +private: + const wchar_t* name_; + IDirectInputDevice8* dev_ = nullptr; + + // shared DI + bool ensure_di_(); + void set_axis_ranges_(); + + // helpers + static float norm_raw_(int v); // [-32768..32767] -> [-1..1] + static float apply_dz_(float v, float dz); // dz in [0..0.99] + static float clamp01_(float v) { return v < 0.f ? 0.f : (v > 1.f ? 1.f : v); } + static Deadzones clampDz_(Deadzones d); + static Smoothing clampSm_(Smoothing s); + + // configs/state + Deadzones dz_{}; + bool smooth_on_ = false; + Smoothing sm_alpha_{}; // per-axis alpha in (0..1] + bool sm_inited_ = false; // init filter on first good frame + // smoothed state + float sm_roll_ = 0, sm_pitch_ = 0, sm_yaw_ = 0, sm_th_ = 0, sm_tv_ = 0, sm_wheel_ = 0; +}; diff --git a/MotionCore/vkb_controllerh.cpp b/MotionCore/vkb_controllerh.cpp new file mode 100755 index 0000000..5238bbc --- /dev/null +++ b/MotionCore/vkb_controllerh.cpp @@ -0,0 +1,176 @@ +#define DIRECTINPUT_VERSION 0x0800 +#include "vkb_controller.h" + +#pragma comment(lib,"dinput8.lib") +#pragma comment(lib,"dxguid.lib") +#pragma comment(lib,"user32.lib") + +// shared DirectInput across instances +static IDirectInput8* g_di = nullptr; + +// transient for EnumDevices +static const wchar_t* s_want = nullptr; +static IDirectInputDevice8* s_found = nullptr; + +static BOOL CALLBACK enum_cb(const DIDEVICEINSTANCE* inst, VOID*) { + if (!s_want) return DIENUM_STOP; + if (wcscmp(inst->tszInstanceName, s_want) == 0 || wcscmp(inst->tszProductName, s_want) == 0) { + if (SUCCEEDED(g_di->CreateDevice(inst->guidInstance, &s_found, nullptr))) + return DIENUM_STOP; + } + return DIENUM_CONTINUE; +} + +static BOOL CALLBACK axes_cb(const DIDEVICEOBJECTINSTANCE* doi, VOID* ctx) { + auto dev = reinterpret_cast(ctx); + if ((doi->dwType & DIDFT_AXIS) && dev) { + DIPROPRANGE r{}; r.diph.dwSize = sizeof r; r.diph.dwHeaderSize = sizeof(DIPROPHEADER); + r.diph.dwObj = doi->dwType; r.diph.dwHow = DIPH_BYID; r.lMin = -32768; r.lMax = 32767; + dev->SetProperty(DIPROP_RANGE, &r.diph); + } + return DIENUM_CONTINUE; +} + +VKBSimController::VKBSimController(const wchar_t* exactName) : name_(exactName) {} +VKBSimController::~VKBSimController() { close(); } + +bool VKBSimController::ensure_di_() { + if (g_di) return true; + return SUCCEEDED(DirectInput8Create(GetModuleHandle(nullptr), DIRECTINPUT_VERSION, + IID_IDirectInput8, (VOID**)&g_di, nullptr)); +} + +bool VKBSimController::connect() { + close(); + if (!ensure_di_()) return false; + + s_want = name_; s_found = nullptr; + g_di->EnumDevices(DI8DEVCLASS_GAMECTRL, enum_cb, nullptr, DIEDFL_ATTACHEDONLY); + if (!s_found) return false; + dev_ = s_found; + + if (FAILED(dev_->SetDataFormat(&c_dfDIJoystick2))) { close(); return false; } + HWND w = GetConsoleWindow(); if (!w) w = GetDesktopWindow(); + if (FAILED(dev_->SetCooperativeLevel(w, DISCL_NONEXCLUSIVE | DISCL_BACKGROUND))) { close(); return false; } + + set_axis_ranges_(); + if (FAILED(dev_->Acquire())) { close(); return false; } + + // reset smoothing state on (re)connect + sm_inited_ = false; + return true; +} + +void VKBSimController::set_axis_ranges_() { + if (!dev_) return; + dev_->EnumObjects(axes_cb, dev_, DIDFT_AXIS); +} + +void VKBSimController::close() { + if (dev_) { dev_->Unacquire(); dev_->Release(); dev_ = nullptr; } +} + +float VKBSimController::norm_raw_(int v) { + float d = (v >= 0) ? 32767.f : 32768.f; + float f = v / d; + if (f > 1.f) f = 1.f; else if (f < -1.f) f = -1.f; + return f; +} + +float VKBSimController::apply_dz_(float v, float dz) { + dz = dz < 0.f ? 0.f : (dz > 0.99f ? 0.99f : dz); + float a = v >= 0.f ? v : -v; + if (a <= dz) return 0.f; + float sign = v >= 0.f ? 1.f : -1.f; + float t = (a - dz) / (1.f - dz); + if (t > 1.f) t = 1.f; + return sign * t; +} + +Deadzones VKBSimController::clampDz_(Deadzones d) { + auto c = [](float v) { return v < 0.f ? 0.f : (v > 0.99f ? 0.99f : v); }; + d.roll = c(d.roll); d.pitch = c(d.pitch); d.yaw = c(d.yaw); + d.thumb_hor = c(d.thumb_hor); d.thumb_ver = c(d.thumb_ver); d.wheel = c(d.wheel); + return d; +} + +Smoothing VKBSimController::clampSm_(Smoothing s) { + auto c = [](float v) { return v <= 0.f ? 1.f : (v > 1.f ? 1.f : v); }; // clamp into (0..1] + s.roll = c(s.roll); s.pitch = c(s.pitch); s.yaw = c(s.yaw); + s.thumb_hor = c(s.thumb_hor); s.thumb_ver = c(s.thumb_ver); s.wheel = c(s.wheel); + return s; +} + +void VKBSimController::setDeadzones(const Deadzones& dz) { dz_ = clampDz_(dz); } +void VKBSimController::setDeadzoneAll(float v) { + v = v < 0.f ? 0.f : (v > 0.99f ? 0.99f : v); + Deadzones d{}; d.roll = d.pitch = d.yaw = d.thumb_hor = d.thumb_ver = d.wheel = v; dz_ = d; +} + +void VKBSimController::enableSmoothing(bool on) { + smooth_on_ = on; + if (!smooth_on_) sm_inited_ = false; +} +void VKBSimController::setSmoothing(const Smoothing& s) { sm_alpha_ = clampSm_(s); } +void VKBSimController::setSmoothingAll(float alpha) { + alpha = alpha <= 0.f ? 1.f : (alpha > 1.f ? 1.f : alpha); + Smoothing s{}; s.roll = s.pitch = s.yaw = s.thumb_hor = s.thumb_ver = s.wheel = alpha; sm_alpha_ = s; +} + +VkbInputs VKBSimController::get() { + VkbInputs out{}; + if (!dev_) return out; + + if (FAILED(dev_->Poll())) { + HRESULT hr = dev_->Acquire(); + while (hr == DIERR_INPUTLOST) hr = dev_->Acquire(); + } + + DIJOYSTATE2 js{}; + if (FAILED(dev_->GetDeviceState(sizeof(js), &js))) return out; + out.connected = true; + + // raw + out.x = js.lX; out.y = js.lY; out.z = js.lZ; + out.rx = js.lRx; out.ry = js.lRy; out.rz = js.lRz; + out.s0 = js.rglSlider[0]; out.s1 = js.rglSlider[1]; + out.pov0_deg = (js.rgdwPOV[0] >= 0) ? int(js.rgdwPOV[0] / 100) : -1; + for (int i = 0; i < 128; i++) out.btn[i] = (js.rgbButtons[i] & 0x80) ? 1 : 0; + + // normalized + deadzones + float nx = apply_dz_(norm_raw_(out.x), dz_.roll); + float ny = apply_dz_(norm_raw_(out.y), dz_.pitch); + float nz = apply_dz_(norm_raw_(out.z), dz_.wheel); + float nrx = apply_dz_(norm_raw_(out.rx), dz_.thumb_ver); + float nry = apply_dz_(norm_raw_(out.ry), dz_.thumb_hor); + float nrz = apply_dz_(norm_raw_(out.rz), dz_.yaw); + + // EMA smoothing + if (!smooth_on_) { + out.roll = nx; out.pitch = ny; out.yaw = nrz; + out.thumb_hor = nry; out.thumb_ver = nrx; out.wheel = nz; + } + else { + if (!sm_inited_) { + sm_roll_ = nx; sm_pitch_ = ny; sm_yaw_ = nrz; sm_th_ = nry; sm_tv_ = nrx; sm_wheel_ = nz; + sm_inited_ = true; + } + else { + sm_roll_ += sm_alpha_.roll * (nx - sm_roll_); + sm_pitch_ += sm_alpha_.pitch * (ny - sm_pitch_); + sm_yaw_ += sm_alpha_.yaw * (nrz - sm_yaw_); + sm_th_ += sm_alpha_.thumb_hor * (nry - sm_th_); + sm_tv_ += sm_alpha_.thumb_ver * (nrx - sm_tv_); + sm_wheel_ += sm_alpha_.wheel * (nz - sm_wheel_); + } + out.roll = sm_roll_; out.pitch = sm_pitch_; out.yaw = sm_yaw_; + out.thumb_hor = sm_th_; out.thumb_ver = sm_tv_; out.wheel = sm_wheel_; + } + + // named buttons + out.main_trigger_pull = out.btn[0] != 0; + out.aux_trigger_pull = out.btn[21] != 0; + out.aux_trigger_push = out.btn[20] != 0; + + return out; +}