KITProstheticHandDriverExampleNoArmarX.cpp

Go to the documentation of this file.

#include <iostream>
#include <thread>
 
#include <QCoreApplication>
 
#include <RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.h>
 
[[maybe_unused]] static constexpr auto old = "DF:70:E8:81:DB:D6";
[[maybe_unused]] static constexpr auto dlr = "DF:A2:F5:48:E7:50";
[[maybe_unused]] static constexpr auto arches = "XX:XX:XX:XX:XX:XX";
 
int
main(int argc, char* argv[])
{
    for (int i = 0; i < argc; ++i)
    {
        if (std::string(argv[i]) == "-h" ||
            std::string(argv[i]) == "--print-options") // needed for ArmarX Doc generation
        {
            // TODO: print help
            return 0;
        }
    }
    QCoreApplication a(argc, argv);
    {
        BLEProthesisInterface iface{dlr};
        iface.verboseReceive(false);
        iface.verboseSend(true);
        std::size_t dcCounter = 0;
        while (iface.getState() != BLEProthesisInterface::State::Running)
        {
            std::cout << "Waiting for Running State: " << to_string(iface.getState()) << std::endl;
 
            if (iface.getState() == BLEProthesisInterface::State::Disconnected)
            {
                ++dcCounter;
            }
 
            if (dcCounter > 50)
            {
                return 1;
            }
 
            std::this_thread::sleep_for(std::chrono::milliseconds{100});
        }
 
        auto printAndSleep = [&](std::size_t n = 25)
        {
            for (std::size_t i2 = 0; i2 < n; ++i2)
            {
                std::cout << iface.getThumbPos() << "\t" << iface.getThumbPWM() << "\t"
                          << iface.getFingerPos() << "\t" << iface.getFingerPWM() << std::endl;
                std::this_thread::sleep_for(std::chrono::milliseconds{100});
            }
        };
 
        for (std::size_t i = 0; i < 4; ++i)
        {
            iface.sendGrasp(i % 2);
            printAndSleep();
        }
        iface.sendThumbPWM(200, 2999, 0);
        printAndSleep(1);
        iface.sendFingerPWM(200, 2999, 0);
        printAndSleep();
    }
    return 0;
}