SpidarSystem.hpp

Go to the documentation of this file.

#ifndef SPIDAR_SYSTEM_HPP
#define SPIDAR_SYSTEM_HPP

#include <cstdint>

namespace Spidar
{
namespace Core
{
template <class T>
class SpidarSystem
{
public:
    typedef T TraitsType;   

    static const size_t MOTOR_COUNT = TraitsType::MOTOR_COUNT;          

    typedef typename TraitsType::SpecType           SpecType;           
    typedef typename TraitsType::ControllerType     ControllerType;     

    typedef typename TraitsType::ValueType          ValueType;          
    typedef typename TraitsType::VectorType         VectorType;         
    typedef typename TraitsType::QuaternionType     QuaternionType;     
    typedef typename TraitsType::PoseType           PoseType;           

    typedef typename TraitsType::ModelType                  ModelType;                  
    typedef typename TraitsType::PoseEstimationType         PoseEstimationType;         
    typedef typename TraitsType::ForceDecompositionType     ForceDecompositionType;     

    typedef typename TraitsType::LogDataType        LogDataType;        
    typedef typename TraitsType::DataLoggerType     DataLoggerType;     
    typedef typename TraitsType::TimerType          TimerType;          
    typedef typename TraitsType::ThreadType         ThreadType;         

    SpidarSystem(void);
    ~SpidarSystem(void);

    double deltaTime(void) const { return timer_.avgDeltaTime(); }

    ValueType gripRadius(void) const { return estimator_.gripRadius(); }

    ValueType cascadeGain(void) const { return cascadeGain_; }

    VectorType targetForce(void) const { return targetForce_; }

    VectorType targetTorque(void) const { return targetTorque_; }

    VectorType resultantForce(void) const { return resultantForce_; }

    VectorType resultantTorque(void) const { return resultantTorque_; }

    bool initialize(const SpecType& spec);
    bool terminate(void);

    void setHaptics(bool enable);
    void setCascadeGain(ValueType gain);

    void start(void);
    void stop(void);
    void calibrate(void);

    void gripPose(VectorType& position, QuaternionType& rotation,
        VectorType& velocity, VectorType& angularVelocity) const;

    bool setForce(const VectorType& force, ValueType forceK, ValueType forceB,
        const VectorType& torque, ValueType torqueK, ValueType torqueB,
        bool lerp, bool cascade = false); 
    
    void encoderCount(int32_t(&count)[MOTOR_COUNT]) const;

    uint32_t gpioValue(void) const;

    void startLog(void);

private:
    void majorLoop(void);

    static void threadFunc(void* param);

    void outputLog(const std::string& filename);
    
    //
    ControllerType          controller_;    
    ModelType               model_;         

    PoseEstimationType      estimator_;     
    ForceDecompositionType  decomposer_;    

    DataLoggerType          dataLogger_;    
    TimerType               timer_;         
    ThreadType*             thread_;        

    bool    initialized_;       
    bool    haptics_;           
    bool    cascadeControl_;    
    bool    kill_;              
    bool    calibrate_;         
    bool    clearEncoder_;      

    int32_t         encoderCount_[MOTOR_COUNT];     

    ValueType       cascadeGain_;       
    ValueType       cascadeSpringK_;    

    ValueType       springKToPulseRatio_[MOTOR_COUNT];

    uint16_t        cascadeRatioKB_;    

    int16_t         velocityGain_[MOTOR_COUNT];     

    uint16_t        velocityFilterGain_;            

    uint16_t        forceFilterGain_;               

    uint16_t        pulseWidthLimit_[MOTOR_COUNT];  

    VectorType      targetForce_;           
    VectorType      targetTorque_;          

    VectorType      resultantForce_;        
    VectorType      resultantTorque_;       

}; // end of class SpidarSystem.

template <class T>
SpidarSystem<T>::SpidarSystem(void) : thread_(nullptr), kill_(true), calibrate_(false), clearEncoder_(false),
initialized_(false), haptics_(false), cascadeControl_(false), cascadeGain_(1), cascadeSpringK_(0), cascadeRatioKB_(0)
{
}

template <class T>
SpidarSystem<T>::~SpidarSystem(void)
{
    terminate();
}

template <class T>
bool SpidarSystem<T>::initialize(const SpecType& spec)
{
    // すでに初期化済みの場合，一度終了処理を行う
    if (initialized_)
    {
        if (!terminate())   return false;
    }

    // 初期状態の設定
    kill_ = true;

    // コントローラの初期化
    if (!controller_.initialize(spec.serialNumber))     return false;

    // メンバの初期化
    if (!estimator_.initialize(spec, controller_.controlFrequency()))       return false;
    if (!decomposer_.initialize(spec))      return false;

    // コントローラ内速度補償ゲインの設定
    for (size_t i = 0; i < MOTOR_COUNT; ++i)
    {
        if (spec.speedConstant[i] == 0)
        {
            velocityGain_[i] = 0;
        }
        else
        {
            double dutyPerVoltage = controller_.maxPulseWidth() / spec.terminalVoltage[i];
            double pulsePerSecond = spec.encoderResolution[i] * spec.speedConstant[i] / 60;

            double value = controller_.cascadeFrequency() * dutyPerVoltage / pulsePerSecond;

            velocityGain_[i] = static_cast<int16_t>(-value * spec.encoderDirection[i]);
        }
    }

    velocityFilterGain_ = static_cast<uint16_t>(spec.minorLoopVelocityFilterGain);
    
    // コントローラ内提示力制限の設定
    for (size_t i = 0; i < MOTOR_COUNT; ++i)
    {
        double pulseWidthLimit = decomposer_.tensionToDuty(i) * decomposer_.maxTension(i) * controller_.maxPulseWidth();
        pulseWidthLimit_[i] = static_cast<uint16_t>(pulseWidthLimit);
    }

    // カスケード制御ゲインの設定
    const double PI = 3.14159265358979323846;

    for (size_t i = 0; i < MOTOR_COUNT; ++i)
    {
        double lengthPerCount = spec.pulleyRadius[i] * 2.0 * PI / spec.encoderResolution[i];

        double maxCurrent = spec.terminalVoltage[i] / spec.terminalResistance[i];

        double maxTension = spec.torqueConstant[i] / spec.pulleyRadius[i] * maxCurrent;

        double pulseWidthPerNewton = controller_.maxPulseWidth() / maxTension;

        springKToPulseRatio_[i] = static_cast<ValueType>(pulseWidthPerNewton * lengthPerCount * spec.encoderDirection[i]);
    }

    forceFilterGain_ = static_cast<uint16_t>(spec.minorLoopForceFilterGain);
    
    // システムの初期化完了
    initialized_ = true;

    return true;
}

template <class T>
bool SpidarSystem<T>::terminate(void)
{
    if (!initialized_)  return true;

    // 動作中の場合は停止
    if (!kill_) stop();

    // コントローラの終了
    if (!controller_.terminate())    return false;

    initialized_ = false;

    return true;
}

template <class T>
void SpidarSystem<T>::setHaptics(bool enable)
{
    haptics_ = enable;
}

template <class T>
void SpidarSystem<T>::setCascadeGain(ValueType gain)
{
    cascadeGain_ = gain;
}

template <class T>
void SpidarSystem<T>::start(void)
{
    if (!initialized_ || thread_)   return;

    kill_ = false;

    thread_ = new ThreadType(threadFunc, static_cast<void*>(this));
}

template <class T>
void SpidarSystem<T>::stop(void)
{
    if (!initialized_ || !thread_)  return;

    kill_ = true;

    thread_->join();

    delete thread_;

    thread_ = nullptr;

    outputLog("C:/SPIDAR/log.txt");
}

template <class T>
void SpidarSystem<T>::calibrate(void)
{
    if (!initialized_ || kill_)   return;

    clearEncoder_ = true;
    calibrate_ = true;
}

template <class T>
void SpidarSystem<T>::gripPose(VectorType& position, QuaternionType& rotation,
    VectorType& velocity, VectorType& angularVelocity) const
{
    if (!initialized_)  return;

    position = estimator_.pose().position;
    rotation = estimator_.pose().rotation;
    velocity = estimator_.pose().velocity;
    angularVelocity = estimator_.pose().angularVelocity;
}

template <class T>
bool SpidarSystem<T>::setForce(const VectorType& force, ValueType forceK, ValueType forceB,
    const VectorType& torque, ValueType torqueK, ValueType torqueB, bool lerp, bool cascade)
{
    if (!initialized_)    return false;

    model_.setParameter(forceK, forceB, torqueK, torqueB);
    model_.setOrigin(estimator_.pose(), force, torque, lerp);

    cascadeControl_ = cascade;
    cascadeSpringK_ = forceK;
    
    if (forceB == 0)    cascadeRatioKB_ = 0;
    else                cascadeRatioKB_ = static_cast<uint16_t>(forceK / forceB);

    return true;
}

template <class T>
void SpidarSystem<T>::encoderCount(int32_t(&count)[MOTOR_COUNT]) const
{
    if (!initialized_)  return;

    for (size_t i = 0; i < MOTOR_COUNT; ++i)    count[i] = encoderCount_[i];
}

template <class T>
uint32_t SpidarSystem<T>::gpioValue(void) const
{
    if (!initialized_)  return 0;

    return controller_.gpioValue();
}

//
//
// 以下プライベート関数
//
//

template <class T>
void SpidarSystem<T>::majorLoop(void)
{
    int32_t     count[8];
    double      duty[8];
 
    // force time limit
    size_t forceCount = 0;

    const size_t collisionThreshold = static_cast<const size_t>(1000 * decomposer_.maxDuration());

    const size_t maxCollisionCount = collisionThreshold + 1000;

    // log
    LogDataType log;

    // コントローラのオフライン設定
    controller_.writePulseWidthLimit(pulseWidthLimit_);
    controller_.writeVelocityGain(velocityGain_);
    controller_.writeVelocityFilterGain(velocityFilterGain_);
    controller_.writeForceFilterGain(forceFilterGain_);

    // エンコーダ値のクリア
    for (size_t i = 0; i < MOTOR_COUNT; ++i)    encoderCount_[i] = 0;

    // バネダンパモデルの初期化
    model_.clear();
    model_.setFrequency(static_cast<ValueType>(controller_.controlFrequency()), 60);

    // 最初のキャリブーション用設定
    calibrate_ = true;
    clearEncoder_ = true;
    size_t clearEncoderCount = 0;

    // コントローラの有効化
    controller_.enableMotorControl();

    // コントローラの制御周波数でタイマーを初期化
    timer_.initialize(controller_.controlFrequency());

    // main loop
    while (!kill_)
    {
        // 安全のため提示力の計算にはゼロクリアから始めるローカル変数を使用する
        VectorType force = VectorType(), torque = VectorType();

        resultantForce_ = targetForce_ = force;
        resultantTorque_ = targetTorque_ = torque;

        // キャリブレーション
        if (calibrate_)
        {
            if (clearEncoder_)
            {
                controller_.clearEncoderCount();
                controller_.readEncoderCount(count);
                clearEncoder_ = false;
            }
            else
            {
                if (clearEncoderCount < 10)
                {
                    controller_.readEncoderCount(count);
                    ++clearEncoderCount;
                }
                else
                {
                    clearEncoderCount = 0;
                    controller_.readEncoderCount(count);
                    estimator_.calibrate(count);
                    forceCount = 0;
                    calibrate_ = false;
                }
            }
        }
        else
        {
            // エンコーダ値の読み込み
            controller_.readEncoderCount(count);

            for (size_t i = 0; i < MOTOR_COUNT; ++i)    encoderCount_[i] = count[i];

            // グリップ位置，姿勢の更新
            estimator_.estimate(count);

            //
            // 提示力の計算
            //

            // 提示力の目標値をバネダンパモデルにより補間計算
            model_.calcModel(estimator_.pose(), force, torque);

            if (force.norm() != 0 || torque.norm() != 0)
            {
                // モータ出力の時間制限（安全のため）
                if (forceCount < maxCollisionCount)  ++forceCount;

                if (forceCount > collisionThreshold)
                {
                    float temp = 0.001f * (forceCount - collisionThreshold);

                    force *= (1.0f - temp);
                    torque *= (1.0f - temp);
                }
            }
            else
            {
                forceCount = 0;
            }

            // 提示力の目標値の記録
            targetForce_ = force;
            targetTorque_ = torque;

            //
            // 張力分配
            //  
            // 力覚提示OFFの場合：提示力の目標値は保存したまま，張力分配の入力はクリア
            if (haptics_ && forceCount != 0)    decomposer_.decompose(estimator_.wireVector(), force, torque);
            else                                decomposer_.decompose(estimator_.wireVector(), VectorType(), VectorType());

            // 各モータの張力を合成した実際の提示力を保存
            resultantForce_ = decomposer_.force();
            resultantTorque_ = decomposer_.torque();
        }

        // 張力をデューティー比に変換
        if (!haptics_ || forceCount == 0)
        {
            for (size_t i = 0; i<MOTOR_COUNT; ++i)
            {
                double temp = (decomposer_.minTension(i) - decomposer_.springTension(i)) * decomposer_.tensionToDuty(i);
                duty[i] = static_cast<ValueType>(temp);
            }
        }
        else
        {
            for (size_t i = 0; i<MOTOR_COUNT; ++i)
            {
                double temp = (decomposer_.wireTension(i) - decomposer_.springTension(i)) * decomposer_.tensionToDuty(i);
                duty[i] = static_cast<ValueType>(temp);
            }
        }

        // コントローラから出力
        if (!cascadeControl_)
        {
            controller_.writeDutyCycle(duty);
        }
        else
        {
            int16_t springK[8];

            for (size_t i = 0; i < MOTOR_COUNT; ++i)
            {
                springK[i] = static_cast<int32_t>(springKToPulseRatio_[i] * cascadeGain_ * cascadeSpringK_);
            }

            controller_.writeDutyCycle(duty, springK, cascadeRatioKB_);
        }

        // ログ
        if (dataLogger_.started())
        {
            //
            log.position = estimator_.pose().position;
            log.velocity = estimator_.pose().velocity;

            log.targetForce = targetForce_;
            log.resultantForce = resultantForce_;

            //
            log.rotation = estimator_.pose().rotation;
            log.angularVelocity = estimator_.pose().angularVelocity;
            log.targetTorque = targetTorque_;
            log.resultantTorque = resultantTorque_;

            for (size_t i = 0; i < MOTOR_COUNT; ++i)
            {
                log.tension[i] = decomposer_.wireTension(i);
            }
            dataLogger_.add(log);
        }

        // busy waiting
        timer_.wait();

    } // end of while (!kill)

    for (size_t i = 0; i<MOTOR_COUNT; ++i)  duty[i] = 0;

    controller_.writeDutyCycle(duty);

    // コントローラの無効化
    controller_.disableMotorControl();
}

template <class T>
void SpidarSystem<T>::threadFunc(void* param)
{
    SpidarSystem<T>* system = static_cast<SpidarSystem<T>*>(param);

    system->majorLoop();
}

template <class T>
void SpidarSystem<T>::startLog(void)
{
    dataLogger_.start();
}

template <class T>
void SpidarSystem<T>::outputLog(const std::string& filename)
{
    if (!kill_)    return;

    if (dataLogger_.started())    dataLogger_.output(filename);
}

} // end of namespace Core.
} // end of namespace Spidar.

#endif // SPIDAR_SYSTEM_HPP

// end of file.