馬達模組使用
===

FPGA
---
#### 要問冠綸怎麼定義，窩不知道ಥ﹏ಥ，好嘛，對不起辣。

C++
---
如總覽篇所說，若需要個別的控制每一個模組，而不使用四足的專案，那麼只需要執行NodeCore，以及前部分fpga-server的執行檔，和你自己撰寫的模組控制檔案。

模組控制檔案範例如下，以控制第一模組為例：

#include "NodeHandler.hpp"
#include <boost/thread.hpp>
#include "std_msg.hpp"
#include "geometry_msg.hpp"
#include "motor_msg.hpp"
#include "utils.hpp"
motor_msg::MotorModule motor_msg;
inline void motor_cb(std::shared_ptr<motor_msg::MotorModule> msg)
{
    motor_msg = *msg;
}
int main(int argc, char* argv[])
{
    std::string master_ip = "127.0.0.1";
    std::string local_ip = "127.0.0.1";
    uint32_t port = 9999;
    double freq = 1000;

    core::ArgParse aps(argc, argv);
    aps.get("MasterIP", master_ip);
    aps.get("MasterPort", port);
    aps.get("LocalIP", local_ip);
    aps.get("Frequency", freq);

    boost::asio::io_context timer_io(1);
    motor_msg::MotorModule pub_msg;
    pub_msg.header.seq = 0;
    pub_msg.header.timestamp = 0;

    core::NodeHandler nh(master_ip, port, local_ip);
    core::Subscriber motor_sub = nh.subscriber("/fpga/state1");
    core::Publisher motor_pub = nh.publisher("/fpga/command1");
    boost::thread node_service(boost::bind(&boost::asio::io_service::run, &core::node_ios));

    uint sleep_us = 1000000 / freq; 

    while(1)
    {
        boost::asio::deadline_timer timer(timer_io, boost::posix_time::microseconds(sleep_us));
        
        motor_sub.spinOnce(&motor_cb); // get the data of motor state
        /* ... your operation, for example change absolute position to {pi, pi}*/
        pub_msg.angle[0] = pi;
        pub_msg.angle[1] = pi;
        pub_msg.state = true;
        pub_msg.pid = {1, 0, 0, 1, 0, 0};
        
        motor_pub.publish(pub_msg); // send the command data for motor command
        
        timer.wait();
    }
    return 0;
}

訊息的可讀與可控定義如下，兩顆馬達的順序如前定義，狀態<font face="monospace" color="green">**true**</font>為可以運動的狀態：

class MotorModule
{
    public:
        double angle[2] = {0, 0};
        double twist[2] = {0, 0};
        double current[2] = {0, 0};
        double torque[2] = {0, 0};
        bool state[2] = {false, false};
        double pid[6] = {0, 0, 0, 0, 0, 0}; // pidpid
};