Spidar.cpp

Go to the documentation of this file.

#include "Spidar.h"

#include "../controller/ControllerFtd2xx.hpp"
#include "../parser/SpecParserRapidJson.hpp"

#include "../core/DeviceSpec.hpp"
#include "../core/Pose.hpp"
#include "../core/WireVector.hpp"
#include "../core/SpringDamperModel.hpp"
#include "../core/PoseEstimation.hpp"
#include "../core/ForceDecomposition.hpp"
#include "../core/SpidarSystem.hpp"

#include "../math/Vector.hpp"
#include "../math/Quaternion.hpp"
#include "../math/Matrix.hpp"
#include "../math/LowPassFilter.hpp"
#include "../math/QuadraticProgramming.hpp"

#include "../win32/DataLogger.hpp"
#include "../win32/Timer.hpp"
#include "../win32/Thread.hpp"
#include "../win32/SpidarTraits.hpp"

#include <map>

namespace
{
#define SPIDAR_VERSION 0x16033101

typedef Spidar::SpecParser::SpecParserRapidJson     ParserType;
typedef Spidar::Controller::ControllerFtd2xx        ControllerType;

typedef SpidarTraits::SpecType                      SpecType;
typedef Spidar::Core::SpidarSystem<SpidarTraits>    SystemType;

typedef SystemType::ValueType       ValueType;
typedef SystemType::VectorType      VectorType;
typedef SystemType::QuaternionType  QuaternionType;

typedef std::pair<SystemType*, ControllerType*> SpidarPairType;

std::map<uint32_t, SpidarPairType> spidar;
}

SPIDAR_API BOOL __stdcall SpidarGetLibraryVersion(UINT& version)
{
    version = SPIDAR_VERSION;
    return TRUE;
}

SPIDAR_API BOOL __stdcall SpidarInitialize(LPCSTR fileName, UINT& serialNumber)
{
    SpecType spec;

    ParserType parser;

    std::string filename = fileName;

    if (!parser.parse(filename, spec))  return FALSE;

    if (spidar.count(spec.serialNumber) != 0 &&
        (spidar[spec.serialNumber].first != nullptr || spidar[spec.serialNumber].second != nullptr))   return FALSE;

    spidar[spec.serialNumber].first = new SystemType();

    if (!spidar[spec.serialNumber].first->initialize(spec))
    {
        delete spidar[spec.serialNumber].first;
        spidar[spec.serialNumber].first = nullptr;
        spidar.erase(spec.serialNumber);

        return FALSE;
    }

    serialNumber = spec.serialNumber;

    return TRUE;
}
SPIDAR_API BOOL __stdcall SpidarInitializeController(UINT serialNumber)
{
    if (spidar.count(serialNumber) != 0 &&
        (spidar[serialNumber].first != nullptr || spidar[serialNumber].second != nullptr))   return FALSE;

    spidar[serialNumber].second = new Spidar::Controller::ControllerFtd2xx();

    if (!spidar[serialNumber].second->initialize(serialNumber))
    {
        delete spidar[serialNumber].second;
        spidar[serialNumber].second = nullptr;
        spidar.erase(serialNumber);

        return FALSE;
    }

    return TRUE;
}
SPIDAR_API BOOL __stdcall SpidarTerminate(UINT serialNumber)
{
    BOOL retval = TRUE;

    if (spidar.count(serialNumber) != 0)
    {
        if (spidar[serialNumber].first != nullptr)
        {
            retval = spidar[serialNumber].first->terminate() ? TRUE : FALSE;

            delete spidar[serialNumber].first;
            spidar[serialNumber].first = nullptr;
            
        }
        else if (spidar[serialNumber].second != nullptr)
        {
            retval = spidar[serialNumber].second->terminate() ? TRUE : FALSE;

            delete spidar[serialNumber].second;
            spidar[serialNumber].second = nullptr;
        }
        spidar.erase(serialNumber);
    }
    return retval;
}

SPIDAR_API BOOL __stdcall SpidarCalibrate(UINT serialNumber)
{
    if (!spidar[serialNumber].first) return FALSE;

    spidar[serialNumber].first->calibrate();

    return TRUE;
}

SPIDAR_API BOOL __stdcall SpidarSetHaptics(UINT serialNumber, BOOL enable)
{
    if (!spidar[serialNumber].first) return FALSE;

    spidar[serialNumber].first->setHaptics(enable ? true : false);

    return TRUE;
}

SPIDAR_API BOOL __stdcall SpidarSetCascadeGain(UINT serialNumber, FLOAT gain)
{
    if (!spidar[serialNumber].first) return FALSE;

    spidar[serialNumber].first->setCascadeGain(gain);

    return TRUE;
}

SPIDAR_API BOOL __stdcall SpidarStart(UINT serialNumber)
{
    if (!spidar[serialNumber].first) return FALSE;

    spidar[serialNumber].first->start();

    return TRUE;
}

SPIDAR_API BOOL __stdcall SpidarStop(UINT serialNumber)
{
    if (!spidar[serialNumber].first) return FALSE;

    spidar[serialNumber].first->stop();

    return TRUE;
}

SPIDAR_API BOOL __stdcall SpidarGetPose(UINT serialNumber, Vector3& position, Quaternion& rotation,
    Vector3& velocity, Vector3& angularVelocity)
{
    if (!spidar[serialNumber].first) return FALSE;

    VectorType pos;
    QuaternionType rot;
    VectorType vel;
    VectorType avel;

    spidar[serialNumber].first->gripPose(pos, rot, vel, avel);

    position.x = static_cast<float>(pos.x());
    position.y = static_cast<float>(pos.y());
    position.z = static_cast<float>(pos.z());

    rotation.x = static_cast<float>(rot.x);
    rotation.y = static_cast<float>(rot.y);
    rotation.z = static_cast<float>(rot.z);
    rotation.w = static_cast<float>(rot.w);

    velocity.x = static_cast<float>(vel.x());
    velocity.y = static_cast<float>(vel.y());
    velocity.z = static_cast<float>(vel.z());

    angularVelocity.x = static_cast<float>(avel.x());
    angularVelocity.y = static_cast<float>(avel.y());
    angularVelocity.z = static_cast<float>(avel.z());

    return TRUE;
}

SPIDAR_API BOOL __stdcall SpidarGetEncoderCount(UINT serialNumber, INT(&count)[8])
{
    if (!spidar[serialNumber].first) return FALSE;

    spidar[serialNumber].first->encoderCount(count);

    return TRUE;
}

SPIDAR_API BOOL __stdcall SpidarSetForce(UINT serialNumber, const Vector3& force, FLOAT forceK, FLOAT forceB,
    const Vector3& torque, FLOAT torqueK, FLOAT torqueB, BOOL lerp, BOOL cascade)
{
    if (!spidar[serialNumber].first) return FALSE;

    VectorType f, t;

    f.x() = force.x;
    f.y() = force.y;
    f.z() = force.z;

    t.x() = torque.x;
    t.y() = torque.y;
    t.z() = torque.z;

    spidar[serialNumber].first->setForce(f, forceK, forceB, t, torqueK, torqueB, lerp ? true : false, cascade ? true : false);

    return TRUE;
}

SPIDAR_API BOOL __stdcall SpidarEnableMotorControl(UINT serialNumber, BOOL enable)
{
    if (!spidar[serialNumber].second)  return FALSE;

    if (enable)
    {
        if (!spidar[serialNumber].second->enableMotorControl())    return FALSE;
    }
    else
    {
        if (!spidar[serialNumber].second->disableMotorControl())    return FALSE;
    }

    return TRUE;
}

SPIDAR_API BOOL __stdcall SpidarWritePulseWidth(UINT serialNumber, const UINT(&pulseWidth)[8])
{
    if (!spidar[serialNumber].second)  return FALSE;

    uint16_t width[SpidarTraits::MOTOR_COUNT];

    for (size_t i = 0; i < SpidarTraits::MOTOR_COUNT; ++i)
    {
        width[i] = pulseWidth[i];
    }
    if (!spidar[serialNumber].second->writePulseWidth(width))    return FALSE;

    return TRUE;
}

SPIDAR_API BOOL __stdcall SpidarGetMaxPulseWidth(UINT serialNumber, UINT& maxPulseWidth)
{
    if (!spidar[serialNumber].second)  return FALSE;

    maxPulseWidth = spidar[serialNumber].second->maxPulseWidth();

    return TRUE;
}

SPIDAR_API BOOL __stdcall SpidarReadEncoderCount(UINT serialNumber, INT(&count)[8])
{
    if (!spidar[serialNumber].second)  return FALSE;

    if (!spidar[serialNumber].second->readEncoderCount(count))  return FALSE;

    return TRUE;
}

SPIDAR_API BOOL __stdcall SpidarClearEncoderCount(UINT serialNumber)
{
    if (!spidar[serialNumber].second)  return FALSE;

    if (!spidar[serialNumber].second->clearEncoderCount())  return FALSE;

    return TRUE;
}

//
// その他
//

SPIDAR_API BOOL __stdcall SpidarGetGpioValue(UINT serialNumber, UINT& value)
{
    if (spidar[serialNumber].first)
    {
        value = spidar[serialNumber].first->gpioValue();
        return TRUE;
    }
    else if (spidar[serialNumber].second)
    {
        value = spidar[serialNumber].second->gpioValue();
        return TRUE;
    }

    return FALSE;
}

SPIDAR_API BOOL __stdcall SpidarGetDeltaTime(UINT serialNumber, FLOAT& deltaTime)
{
    if (!spidar[serialNumber].first) return FALSE;

    deltaTime = static_cast<FLOAT>(spidar[serialNumber].first->deltaTime());

    return TRUE;
}

SPIDAR_API BOOL __stdcall SpidarGetGripRadius(UINT serialNumber, FLOAT& gripRadius)
{
    if (!spidar[serialNumber].first) return FALSE;

    gripRadius = static_cast<FLOAT>(spidar[serialNumber].first->gripRadius());

    return TRUE;
}

SPIDAR_API BOOL __stdcall SpidarStartLog(UINT serialNumber)
{
    if (!spidar[serialNumber].first) return FALSE;

    spidar[serialNumber].first->startLog();

    return TRUE;
}

// end of file.