leonetienne/MotionCore
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

made this work with clean mcu firmware

Browse Source
This commit is contained in:
Leonetienne
2025-12-14 19:59:51 +01:00
parent ca05180fd2
commit 9b3caf511c
13 changed files with 366 additions and 154 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MotionCore/Arm.cpp
+12 -9
View File
@@ -1,5 +1,11 @@
#include "Arm.h"
#include <iostream>
#include <numbers>
#include <algorithm>
#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
)
MotionCore/ArmSegment.cpp
+13 -40
View File
@@ -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<double> 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<double> 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<double> 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();
}
MotionCore/ArmSegment.h
+4 -9
View File
@@ -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;
};
MotionCore/MainLoop.cpp
+27 -48
View File
@@ -1,3 +1,4 @@
#define WIN32_LEAN_AND_MEAN
#include "MainLoop.h"
#include <iostream>
#include <thread>
@@ -6,23 +7,25 @@
#include <algorithm>
#include <numbers>
// 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();
}
@@ -44,8 +47,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)
{
std::chrono::steady_clock::time_point begin =
@@ -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();
MotionCore/MainLoop.h
+2
View File
@@ -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;
};
MotionCore/MotionCore.vcxproj
+2
View File
@@ -152,6 +152,7 @@
<ClCompile Include="Motor.cpp" />
<ClCompile Include="OSerialBus.cpp" />
<ClCompile Include="Vector3d.cpp" />
<ClCompile Include="vkb_controllerh.cpp" />
</ItemGroup>
<ItemGroup>
<ClInclude Include="AxisAlignedBoundingBox.h" />
@@ -162,6 +163,7 @@
<ClInclude Include="Motor.h" />
<ClInclude Include="OSerialBus.h" />
<ClInclude Include="Vector3d.h" />
<ClInclude Include="vkb_controller.h" />
</ItemGroup>
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
<ImportGroup Label="ExtensionTargets">
MotionCore/MotionCore.vcxproj.filters
+6
View File
@@ -42,6 +42,9 @@
<ClCompile Include="Matrix3x3.cpp">
<Filter>Quelldateien</Filter>
</ClCompile>
<ClCompile Include="vkb_controllerh.cpp">
<Filter>Quelldateien</Filter>
</ClCompile>
</ItemGroup>
<ItemGroup>
<ClInclude Include="MainLoop.h">
@@ -68,5 +71,8 @@
<ClInclude Include="Matrix3x3.h">
<Filter>Headerdateien</Filter>
</ClInclude>
<ClInclude Include="vkb_controller.h">
<Filter>Headerdateien</Filter>
</ClInclude>
</ItemGroup>
</Project>
MotionCore/Motor.cpp
+28 -16
View File
@@ -1,6 +1,7 @@
#include "Motor.h"
#include <iostream>
#include <algorithm>
#include <array>
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<double> 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<double> 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<std::uint8_t, 5> 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
MotionCore/Motor.h
+8 -8
View File
@@ -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;
};
MotionCore/OSerialBus.cpp
+2 -19
View File
@@ -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
MotionCore/OSerialBus.h
+3 -5
View File
@@ -1,4 +1,5 @@
#pragma once
#define WIN32_LEAN_AND_MEAN
#include <windows.h>
#include <string>
@@ -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;
MotionCore/vkb_controller.h
Executable
+83
View File
@@ -0,0 +1,83 @@
#pragma once
#define DIRECTINPUT_VERSION 0x0800
#define WIN32_LEAN_AND_MEAN
#include <windows.h>
#include <dinput.h>
// 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;
};
MotionCore/vkb_controllerh.cpp
Executable
+176
View File
@@ -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<IDirectInputDevice8*>(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;
}