zynq_7010/zynq_7010_code/MotorConfig.cpp

#include "MotorConfig.h"
#include <sys/stat.h>
#include <fstream>
#include <sstream>
#include <iomanip>

using namespace std;
MotorConfig::MotorConfig()
{
    initconfigfile();
}

MotorConfig::~MotorConfig()
{
}

std::string MotorConfig::GetParams(bool bzouzhi, MTBDType mttype)
{
    std::string j_str;
    json j;
    if (mttype == MTBDType::MT_TMC)
    {
        std::ifstream i(bzouzhi ? MT_TMC216_ZOU_PATH : MT_TMC216_CUO_PATH);
        i >> j;
        j_str = j.dump();
    }
    else
    {
        std::ifstream i(bzouzhi ? MT_DRV888_ZOU_PATH : MT_DRV888_CUO_PATH);
        i >> j;
        j_str = j.dump();
    }
    return j_str;
}

void MotorConfig::SetParams(bool bzouzhi, MTBDType mttype, MotorSpeedParamEx &param)
{
    std::string path;
    auto params = GetMotorSpeedParams(bzouzhi, mttype);
    for (int i = 0; i < params.size(); i++)
    {
        if (params[i].dpi == param.dpi &&
            params[i].colormode == param.colormode &&
            params[i].speed == param.speed)
        {
            params[i] = param;
        }
    }

    json j = json::array();

    for (int i = 0; i < params.size(); i++)
    {
        json t_j;
        to_json(params[i], t_j);
        j.push_back(t_j);
    }

    if (mttype == MTBDType::MT_TMC)
        path = bzouzhi ? MT_TMC216_ZOU_PATH : MT_TMC216_CUO_PATH;
    else
        path = bzouzhi ? MT_DRV888_ZOU_PATH : MT_DRV888_CUO_PATH;

    ofstream ofs(path);
    ofs << std::setw(4) << j << std::endl;
}

std::vector<MotorSpeedParamEx> MotorConfig::GetMotorSpeedParams(bool bzouzhi, MTBDType mttype)
{
    std::vector<MotorSpeedParamEx> ret;
    if (mttype == MTBDType::MT_TMC)
    {
        std::ifstream i(bzouzhi ? MT_TMC216_ZOU_PATH : MT_TMC216_CUO_PATH);
        json j;
        i >> j;
        for (json::iterator it = j.begin(); it != j.end(); ++it)
        {
            auto tmv = it.value();
            MotorSpeedParamEx param;
            from_json(tmv, param);
            ret.push_back(param);
        }
    }
    else
    {
        std::ifstream i(bzouzhi ? MT_DRV888_ZOU_PATH : MT_DRV888_CUO_PATH);
        json j;
        i >> j;
        for (json::iterator it = j.begin(); it != j.end(); ++it)
        {
            auto tmv = it.value();
            MotorSpeedParamEx param;
            from_json(tmv, param);
            ret.push_back(param);
        }
    }
    return ret;
}

void MotorConfig::initconfigfile()
{
    struct stat buff;
    for (int s = 0; s < m_jsonpaths.size(); s++)
    {
        if (stat(m_jsonpaths[s].c_str(), &buff) != 0) //不存在
        {
            {
                json j = json::array();
                for (int i = 1; i < 6; i++)//speed mode
                {
                    MotorSpeedParamEx param;
                    for (int k = 1; k < 4; k++) //dpi
                    {
                        double ratio = k == 1 ? 1 : (k == 2 ? 1.51 : 2.81);
                        param.mt_param.finalPeriod = s >= 2 ? 228000 / 4 *ratio: 228000*ratio; // s < 2 drv888
                        param.mt_param.Fmin = s >= 2 ? 1607750 / 4 : 1607750;
                        param.mt_param.stepnum = 30;
                        param.mt_param.a = 200;
                        param.mt_param.offset = 9;
                        param.mt_param.finalDelay = 2500;
                        param.speed = i; // 0 gray 1 color
                        param.dpi = k;   // 1 200dpi 2 300dpi 3 600dpi  
                        param.mt_param.acceleration_time=0;
                        for (int t = 0; t < 2; t++)//color mode
                        {
                            param.colormode = t;
                            if(t==1&& k== 1) param.sp = 2570;//200 color
                            else if(t==0&& k== 1) param.sp = 8150;//200 gray
                            else if(t==1&& k== 2) param.sp = 1719;//300 color
                            else if(t==0&& k== 2) param.sp = 5452;//300 color
                            else if(t==1&& k== 3) param.sp = 1700;//600 gray
                            else if(t==0&& k== 3) param.sp = 4850;//600 color

                            json t_j;
                            to_json(param, t_j);
                            j.push_back(t_j);
                        }
                    }
                }
                ofstream ofs(m_jsonpaths[s]);
                ofs << std::setw(4) << j << std::endl;
            }
        }
    }
}

std::vector<MotorSpeedParamEx> MotorConfig::GetMotorSpeedParams(const std::string &json_str)
{
    std::vector<MotorSpeedParamEx> ret;
    if (json_str.length())
    {
        json j(json_str);
        for (json::iterator it = j.begin(); it != j.end(); ++it)
        {
            auto tmv = it.value();
            MotorSpeedParamEx param;
            from_json(tmv, param);
            ret.push_back(param);
        }
    }
    return ret;
}


MotorSpeedParamEx MotorConfig::GetMotorSpeedParam(bool bzouzhi,MTBDType mttype,int speed,int colormmode,int dpi)
{
    MotorSpeedParamEx retparam;
    auto exparams= GetMotorSpeedParams(bzouzhi,mttype);
    for(auto &item:exparams)
    {
        if(item.colormode==colormmode && 
            item.dpi==dpi&&
            item.speed==speed)
            {
                retparam = item;
                break;
            }
    }
    return retparam;
}
void MotorConfig::to_json(MotorSpeedParamEx &param, json &j)
{
    j = json{
        {"A", param.mt_param.a},
        {"FINALDELAY", param.mt_param.finalDelay},
        {"FINALPERIOD", param.mt_param.finalPeriod},
        {"FMIN", param.mt_param.Fmin},
        {"OFFSET", param.mt_param.offset},
        {"STEPNUM", param.mt_param.stepnum},
        {"DPI", param.dpi},
        {"COLORMODE", param.colormode},
        {"ACCELERATIONTIME",param.mt_param.acceleration_time},
        {"SPEED", param.speed},
        {"SP",param.sp}};
}

void MotorConfig::from_json(json &j, MotorSpeedParamEx &param)
{

    if(j.contains("A"))
        j.at("A").get_to(param.mt_param.a);
    else
        param.mt_param.a = 200;

    if(j.contains("FINALDELAY"))
        j.at("FINALDELAY").get_to(param.mt_param.finalDelay);
    else
        param.mt_param.finalDelay=2500;

    if(j.contains("FINALPERIOD"))
        j.at("FINALPERIOD").get_to(param.mt_param.finalPeriod);
    else
       param.mt_param.finalPeriod = 455000;

    if(j.contains("FMIN")) 
        j.at("FMIN").get_to(param.mt_param.Fmin);
    else
        param.mt_param.Fmin = 1655000;

    if(j.contains("OFFSET"))
        j.at("OFFSET").get_to(param.mt_param.offset);
    else
        param.mt_param.offset=7;

    if(j.contains("STEPNUM"))
        j.at("STEPNUM").get_to(param.mt_param.stepnum);
    else
        param.mt_param.stepnum=30;

    if(j.contains("DPI"))
        j.at("DPI").get_to(param.dpi);
    else
        param.dpi = 1;

    if(j.contains("COLORMODE"))
        j.at("COLORMODE").get_to(param.colormode);
    else
        param.colormode=1;

    if(j.contains("ACCELERATIONTIME"))
        j.at("ACCELERATIONTIME").get_to(param.mt_param.acceleration_time);
    else
        param.mt_param.acceleration_time=0;

    if(j.contains("SPEED"))
        j.at("SPEED").get_to(param.speed);
    else
        param.speed=4;

    if(j.contains("SP"))
        j.at("SP").get_to(param.sp);
    else
        param.sp=1800;
    
}