NovintFalcon/haptics.cpp at main · ethan-soon/NovintFalcon · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
#include "haptics.h"
#include "mex.h"
#include <stdexcept>
#include <array>

HapticsClass *hapticsVar = nullptr;
static bool g_initFailed = false;

HapticsClass::HapticsClass() : m_inited(false), m_buttonApp(false)
{
    m_positionApp[0] = m_positionApp[1] = m_positionApp[2] = 0;
    m_forceApp[0]    = m_forceApp[1]    = m_forceApp[2]    = 0;
}

HapticsClass::~HapticsClass() { uninit(); }

void HapticsClass::init()
{
    m_device.setFalconComm<libnifalcon::FalconCommFTD2XX>();
    m_device.setFalconFirmware<libnifalcon::FalconFirmwareNovintSDK>();
    m_device.setFalconKinematic<libnifalcon::FalconKinematicStamper>();

    if (!m_device.open(0))
        throw std::runtime_error("Cannot open Falcon - is it plugged in?");

    if (!m_device.isFirmwareLoaded())
    {
        if (!m_device.setFirmwareFile("nvent_firmware.bin"))
            throw std::runtime_error("Cannot find nvent_firmware.bin");
        if (!m_device.loadFirmware((unsigned int)10))
            throw std::runtime_error("Firmware upload failed after 10 retries");
    }

    int attempts = 0;
    while (!m_device.isFirmwareLoaded() && attempts < 1000)
    {
        m_device.runIOLoop();
        attempts++;
    }
    if (!m_device.isFirmwareLoaded())
        throw std::runtime_error("Falcon firmware not responding after upload");

    m_inited = true;
}

void HapticsClass::uninit()
{
    if (m_inited)
    {
        m_device.close();
        m_inited = false;
    }
}

void HapticsClass::runLoop()
{
    std::array<double,3> f = {m_forceApp[0], m_forceApp[1], m_forceApp[2]};
    m_device.setForce(f);

    if (!m_device.runIOLoop())
        return;

    // Red = not homed, Green = homed
    auto fw = m_device.getFalconFirmware();
    if (fw->isHomed())
        fw->setLEDStatus(libnifalcon::FalconFirmware::GREEN_LED);
    else
        fw->setLEDStatus(libnifalcon::FalconFirmware::RED_LED);

    std::array<double,3> pos = m_device.getPosition();
    m_positionApp[0] = pos[0];
    m_positionApp[1] = pos[1];
    m_positionApp[2] = pos[2];
}

void HapticsClass::getPosition(double pos[3])
{
    pos[0] = m_positionApp[0];
    pos[1] = m_positionApp[1];
    pos[2] = m_positionApp[2];
}

bool HapticsClass::isButtonDown() { return m_buttonApp; }

void HapticsClass::setForce(double force[3])
{
    m_forceApp[0] = force[0];
    m_forceApp[1] = force[1];
    m_forceApp[2] = force[2];
}

void createHaptics()
{
    try {
        hapticsVar = new HapticsClass;
        hapticsVar->init();
    } catch (const std::exception& e) {
        g_initFailed = true;
        mexWarnMsgTxt(e.what());
    } catch (...) {
        g_initFailed = true;
        mexWarnMsgTxt("Haptics: Unknown error - running in simulation mode");
    }
}

void deleteHaptics()
{
    try { delete hapticsVar; hapticsVar = nullptr; } catch (...) {}
    g_initFailed = false;
}

void falconPlant(double force[3], double position[3])
{
    if (g_initFailed || hapticsVar == nullptr) {
        position[0] = position[1] = position[2] = 0;
        return;
    }
    hapticsVar->setForce(force);
    hapticsVar->runLoop();
    hapticsVar->getPosition(position);
}