USV-PCIE7.hh

 
//
// Copyright (C) 2011-2021 Juan Padron and Yokokura, Yuki
// This program is free software;
// you can redistribute it and/or modify it under the terms of the BSD License.
// For details, see the License.txt file.
 
#ifndef USVPCIE7
#define USVPCIE7
 
#include <sys/mman.h>
#include <unistd.h>
#include <fcntl.h>
#include <cstdint>
#include <cmath>
#include <array>
 
// ARCS組込み用マクロ
#ifdef ARCS_IN
    // ARCSに組み込まれる場合
    #include "ARCSassert.hh"
    #include "ARCSeventlog.hh"
#else
    // ARCSに組み込まれない場合
    #define arcs_assert(a) (assert(a))
    #define PassedLog()
    #define EventLog(a)
    #define EventLogVar(a)
#endif
 
namespace ARCS {
template <size_t N>
class USV_PCIE7{
    public:
        enum ControlMode {
            ACCELERATION_CTRL,  
            CURRENT_CTRL        
        };
        
        USV_PCIE7(const unsigned long Addr)
            : ADDR_BASE(Addr), fd(0), Memptr(nullptr)
        {
            PassedLog();
            fd = open("/dev/mem", O_RDWR|O_SYNC);   // メモリ空間へのfile descriptor取得
            arcs_assert(fd != -1);                  // エラーがないか確認
            Memptr = (uint32_t*)mmap(nullptr, MEMMAP_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, fd, ADDR_BASE); //メモリ空間へのメモリーマップポインターを取得
            close(fd);
            arcs_assert(Memptr != MAP_FAILED);      //ちゃんとポインタ取得できたか確認
        }
 
        USV_PCIE7(USV_PCIE7&& right)
            : ADDR_BASE(right.ADDR_BASE), fd(0), Memptr(right.Memptr)
        {
            right.Memptr = nullptr;     // ムーブ元はヌルポにしておく
        }
 
        ~USV_PCIE7(){
            PassedLog();
            SetZeroCurrent();           // 念のためのゼロ電流指令
            munmap(Memptr, MEMMAP_SIZE);// メモリマッピング解除
        }
 
        void SetCurrent(const double Current, const size_t Axis){
            arcs_assert(1 <= Axis && Axis <= 7);        // 軸の範囲チェック
            UintAndFloat CurrentUnion;                  // float→uint32_t変換用共用体
            CurrentUnion.FloatValue = -(float)Current;  // 倍精度から単精度へキャストしてfloatとして共用体に格納，符号をエンコーダに合わせる
            Memptr[Axis - 1] = CurrentUnion.BinaryExpr; // uint32_tとして共用体から読み出して，q軸電流指令に対応したメモリに書き込み
        }
        
        void SetCurrent(const std::array<double, N>& Current){
            for(size_t i = 1; i <= N; ++i) SetCurrent(Current[i - 1], i);
        }
        
        void SetZeroCurrent(void){
            for(size_t i = 1; i <= N; ++i) SetCurrent(0, i);
        }
        
        double GetCurrent(const size_t Axis){
            arcs_assert(1 <= Axis && Axis <= 7);        // 軸の範囲チェック
            UintAndFloat CurrentUnion;                  // uint32_t→float変換用共用体
            CurrentUnion.BinaryExpr = Memptr[IDX_RDBASE + IDX_PER_AXIS*(Axis - 1) + IDX_GNRL];  // q軸電流に対応したメモリから読み込んで，uint32_tとして共用体に格納
            return (double)CurrentUnion.FloatValue;     // [A] 単精度から倍精度へキャストして返す
        }
        
        void GetCurrent(std::array<double, N>& Current){
            for(size_t i = 1; i <= N; ++i) Current[i - 1] = GetCurrent(i);
        }
        
        double GetVelocity(const size_t Axis){
            arcs_assert(1 <= Axis && Axis <= 7);        // 軸の範囲チェック
            UintAndFloat VelocityUnion;                 // uint32_t→float変換用共用体
            VelocityUnion.BinaryExpr = Memptr[IDX_RDBASE + IDX_PER_AXIS*(Axis - 1) + IDX_GNRL]; // 速度に対応したメモリから読み込んで，uint32_tとして共用体に格納
            return ((double)VelocityUnion.FloatValue)*RPM_TO_RAD;   // [rad/s] 単精度から倍精度へキャストしてRPM→rad/s換算して返す
        }
        
        void GetVelocity(std::array<double, N>& Velocity){
            for(size_t i = 1; i <= N; ++i) Velocity[i - 1] = GetVelocity(i);
        }
        
        double GetPosition(const size_t Axis){
            arcs_assert(1 <= Axis && Axis <= 7);        // 軸の範囲チェック
            return ((double)(ENC_MASK & Memptr[IDX_RDBASE + IDX_PER_AXIS*(Axis - 1) + IDX_POSL]))*PULSE_TO_RAD; // 位置に対応したメモリから読み込んで，マスクかけてpulse→rad換算して返す
        }
        
        void GetPosition(std::array<double, N>& Position){
            for(size_t i = 1; i <= N; ++i) Position[i - 1] = GetPosition(i);
        }
        
        int32_t GetNumOfRotation(const size_t Axis){
            arcs_assert(1 <= Axis && Axis <= 7);        // 軸の範囲チェック
            UintAndInt NumOfRotUnion;                   // uint32_t→int32_t変換用共用体
            NumOfRotUnion.UnsignedExpr = Memptr[IDX_RDBASE + IDX_PER_AXIS*(Axis - 1) + IDX_POSH];   // 回転の数に対応したメモリから読み込む
            return NumOfRotUnion.SignedExpr;            // 符号付きで返す
        }
        
        double GetFullPosition(const size_t Axis){
            arcs_assert(1 <= Axis && Axis <= 7);        // 軸の範囲チェック
            return (double)GetNumOfRotation(Axis)*2.0*M_PI + GetPosition(Axis); // 「回転の数の分のrad」と「0～2π」を足して返す
        }
        
        void GetFullPosition(std::array<double, N>& Position){
            for(size_t i = 1; i <= N; ++i) Position[i - 1] = GetFullPosition(i);
        }
        
        double GetTorque(const size_t Axis){
            arcs_assert(1 <= Axis && Axis <= 7);        // 軸の範囲チェック
            UintAndFloat TorqueUnion;                   // uint32_t→float変換用共用体
            TorqueUnion.BinaryExpr = Memptr[IDX_RDBASE + IDX_PER_AXIS*(Axis - 1) + IDX_TRQ];    // トルクに対応したメモリから読み込んで，uint32_tとして共用体に格納
            return ((double)TorqueUnion.FloatValue);    // [Nm] 単精度から倍精度へキャストして返す
        }
        
        void GetTorque(std::array<double, N>& Torque){
            for(size_t i = 1; i <= N; ++i) Torque[i - 1] = GetTorque(i);
        }
        
        uint32_t GetCounter(const size_t Axis){
            arcs_assert(1 <= Axis && Axis <= 7);        // 軸の範囲チェック
            return Memptr[IDX_CNTBASE + Axis - 1];      // ユニネットデータ同期用カウント値を読み出して返す
        }
 
        void TurnServosOn(void){
            // 書き込み値は無視され，書き込むとコマンドが全軸に同時に単純に発行される。
            Memptr[IDX_SERVO_ON] = 0x01;
            sleep(2);   // サーボON状態が落ち着くまで2秒待機
        }
 
        void TurnServosOff(void){
            // 書き込み値は無視され，書き込むとコマンドが全軸に同時に単純に発行される。
            Memptr[IDX_SERVO_OFF] = 0x01;
        }
 
        void SetControlMode(ControlMode Ctrl){
            // 書き込み値は無視され，書き込むとコマンドが全軸に同時に単純に発行される。
            switch(Ctrl){
                case ControlMode::ACCELERATION_CTRL:
                    Memptr[IDX_ACCCTRL] = 0x01; // 加速度制御モード
                    break;
                case ControlMode::CURRENT_CTRL:
                    Memptr[IDX_CURCTRL] = 0x01; // 電流制御モード
                    break;
                default:
                    break;
            }
        }
 
        void ClearErrorStatus(void){
            // 書き込み値は無視され，書き込むとコマンドが全軸に同時に単純に発行される。
            Memptr[IDX_ERRCLR] = 0x01;
        }
 
        void CalibrateZeroTorque(void){
            // 書き込み値は無視され，書き込むとコマンドが全軸に同時に単純に発行される。
            Memptr[IDX_ZEROCAL] = 0x01;
        }
 
    private:
        USV_PCIE7(const USV_PCIE7&) = delete;                   
        const USV_PCIE7& operator=(const USV_PCIE7&) = delete;  
        static constexpr unsigned int MEMMAP_SIZE = 0x120;      
        static constexpr size_t IDX_RDBASE = 32;                
        static constexpr size_t IDX_PER_AXIS = 4;               
        static constexpr size_t IDX_POSH = 0;                   
        static constexpr size_t IDX_POSL = 1;                   
        static constexpr size_t IDX_TRQ  = 2;                   
        static constexpr size_t IDX_GNRL = 3;                   
        static constexpr size_t IDX_CNTBASE   = 60;             
        static constexpr size_t IDX_SERVO_ON  = 20;             
        static constexpr size_t IDX_SERVO_OFF = 21;             
        static constexpr size_t IDX_ACCCTRL   = 22;             
        static constexpr size_t IDX_CURCTRL   = 23;             
        static constexpr size_t IDX_ERRCLR    = 24;             
        static constexpr size_t IDX_ZEROCAL   = 25;             
        static constexpr double RPM_TO_RAD   = 2.0*M_PI/60.0;       
        static constexpr double PULSE_TO_RAD = 2.0*M_PI/1048576.0;  
        static constexpr uint32_t ENC_MASK = 0x000FFFFF;        
        const unsigned long ADDR_BASE;                          
        int fd;                                                 
        uint32_t* Memptr;                                       
        
        union UintAndFloat {
            uint32_t BinaryExpr;    
            float FloatValue;       
        };
        
        union UintAndInt {
            uint32_t UnsignedExpr;  
            int32_t  SignedExpr;    
        };
        
};
}
 
#endif