From 88246ce278109fb42e8526619a6332ccf77facda Mon Sep 17 00:00:00 2001 From: szxwpp <1039194607@qq.com> Date: Sun, 12 Jun 2016 17:10:37 +0800 Subject: [PATCH 1/2] test control --- test/test_control/main.cpp | 4 +- test/test_control/test_control_motion.cpp | 50 ++++++++++++++--------- 2 files changed, 33 insertions(+), 21 deletions(-) diff --git a/test/test_control/main.cpp b/test/test_control/main.cpp index b133aad9..290f8752 100644 --- a/test/test_control/main.cpp +++ b/test/test_control/main.cpp @@ -7,11 +7,11 @@ int main(int argc, char *argv[]) { - test_control_ethercat(); + test_control_ethercat(); test_control_motion(); std::cout << "test_control finished, press any key to continue" << std::endl; std::cin.get(); return 0; -} \ No newline at end of file +} diff --git a/test/test_control/test_control_motion.cpp b/test/test_control/test_control_motion.cpp index c5b1cb7e..3eb88c39 100644 --- a/test/test_control/test_control_motion.cpp +++ b/test/test_control/test_control_motion.cpp @@ -1,6 +1,16 @@ #include #include +#ifdef UNIX +#include "rtdk.h" +#include "unistd.h" +#endif + +#ifdef WIN32 +#define rt_printf printf +#endif + + #include "test_control_motion.h" @@ -38,6 +48,11 @@ const char xml_file[] = " " " " " " +" " +" " +" " +" " +" " " " " " " " @@ -75,6 +90,12 @@ const char xml_file[] = " " " " " " +" " +" " +" " +" " +" " +" " " " " " " " @@ -92,7 +113,7 @@ const char xml_file[] = " " " " " " -" " +" " " " " " " " @@ -128,22 +149,16 @@ const char xml_file[] = " " " " " " -" " -" " -" " -" " +" " +" " +" " +" " " " " " " " ""; - -#ifdef WIN32 -#define rt_printf printf -#endif - - -#define SlaveNumber 18 +#define SlaveNumber 5 enum { MAX_MOTOR_NUM = 100 }; //for enable, disable, and home struct BasicFunctionParam @@ -234,8 +249,8 @@ BasicFunctionParam decode(const std::string input) std::fill_n(bfParam.active_motor, SlaveNumber, false); for (auto &i : params) { - if (i.first != "slave_motor") - std::cout << "the first param must be 'c', it means the slave id." << std::endl; + if (i.first != "physical_motor") + std::cout << "the first param must be 'p', it means the physic id." << std::endl; else { if (stoi(i.second)>SlaveNumber - 1 || stoi(i.second)<0) { throw std::runtime_error("the second param is invalid"); @@ -266,6 +281,7 @@ void tg() rt_printf("command finished, cmd_count: %d\n", cmd_count); cmd_success = true; } + if (cmd_count % 1000 == 0 && !cmd_success) rt_printf("executing command: %d\n", cmd_count); } @@ -306,10 +322,6 @@ void tg() cmd_count++; } - - - - void test_control_motion() { aris::core::XmlDocument xml_doc; @@ -343,4 +355,4 @@ void test_control_motion() controller.stop(); return; -} \ No newline at end of file +} From c30a8d2dee7b68e995af074717298d0a2305a928 Mon Sep 17 00:00:00 2001 From: szxwpp <1039194607@qq.com> Date: Mon, 13 Jun 2016 18:32:26 +0800 Subject: [PATCH 2/2] modify xml to match the server demo --- demo/demo_Aris_Server/demo_Server/main.cpp | 43 +++++++++++----------- doc/Robot_III.xml | 24 ++++++------ src/aris_control/aris_control_motion.cpp | 39 +++++++++++--------- src/aris_control/aris_control_motion.h | 15 ++++---- src/aris_server/aris_server.cpp | 29 ++++++++++++++- 5 files changed, 90 insertions(+), 60 deletions(-) diff --git a/demo/demo_Aris_Server/demo_Server/main.cpp b/demo/demo_Aris_Server/demo_Server/main.cpp index c95deda8..adb80885 100644 --- a/demo/demo_Aris_Server/demo_Server/main.cpp +++ b/demo/demo_Aris_Server/demo_Server/main.cpp @@ -35,14 +35,14 @@ auto basicParse(const aris::server::ControlServer &cs, const std::string &cmd, c std::fill_n(param.active_motor_ + 15, 3, true); } else if (i.first == "motion_id") - { + { std::size_t id{ std::stoul(i.second) }; if (id < 0 || id > cs.model().motionPool().size())throw std::runtime_error("invalid param in basic parse func in param \"" + i.first + "\""); std::fill(param.active_motor_, param.active_motor_ + 18, false); param.active_motor_[cs.model().motionAtAbs(id).absID()] = true; } else if (i.first == "physical_id") - { + { std::size_t id{ std::stoul(i.second) }; if (id < 0 || id > cs.model().motionPool().size())throw std::runtime_error("invalid param in basic parse func in param \"" + i.first + "\""); std::fill(param.active_motor_, param.active_motor_ + 18, false); @@ -69,7 +69,7 @@ auto basicParse(const aris::server::ControlServer &cs, const std::string &cmd, c msg_out.copyStruct(param); } -auto testParse(const aris::server::ControlServer &cs, const std::string &cmd, const std::map ¶ms, aris::core::Msg &msg)->void +auto testParse(const aris::server::ControlServer &cs, const std::string &cmd, const std::map ¶ms, aris::core::Msg &msg_out)->void { aris::server::GaitParamBase param; @@ -79,32 +79,31 @@ auto testParse(const aris::server::ControlServer &cs, const std::string &cmd, co { std::fill_n(param.active_motor_, 18, true); } - else if (i.first == "motor") + else if (i.first == "motion_id") { - int id = { stoi(i.second) }; - if (id<0 || id>17)throw std::runtime_error("invalid param in basicParse func"); - - std::fill_n(param.active_motor_, 18, false); - param.active_motor_[id] = true; + std::size_t id{ std::stoul(i.second) }; + if (id < 0 || id > cs.model().motionPool().size())throw std::runtime_error("invalid param in basic parse func in param \"" + i.first + "\""); + std::fill(param.active_motor_, param.active_motor_ + 18, false); + param.active_motor_[cs.model().motionAtAbs(id).absID()] = true; } - else if (i.first == "physical_motor") + else if (i.first == "physical_id") { - int id = { stoi(i.second) }; - if (id<0 || id>17)throw std::runtime_error("invalid param in basicParse func"); - - std::fill_n(param.active_motor_, 18, false); - param.active_motor_[id] = true; + std::size_t id{ std::stoul(i.second) }; + if (id < 0 || id > cs.model().motionPool().size())throw std::runtime_error("invalid param in basic parse func in param \"" + i.first + "\""); + std::fill(param.active_motor_, param.active_motor_ + 18, false); + param.active_motor_[cs.model().motionAtPhy(id).absID()] = true; } - else if (i.first == "slave_motor") + else if (i.first == "slave_id") { - int id = { stoi(i.second) }; - if (id<0 || id>17)throw std::runtime_error("invalid param in basicParse func"); - - std::fill_n(param.active_motor_, 18, false); - param.active_motor_[id] = true; + std::size_t id{ std::stoul(i.second) }; + if (id < 0 || id > cs.controller().slavePool().size())throw std::runtime_error("invalid param in basic parse func in param \"" + i.first + "\""); + std::fill(param.active_motor_, param.active_motor_ + 18, false); + if (cs.model().motionAtSla(id).absID() >= cs.model().motionPool().size())throw std::runtime_error("invalid param in basic parse func in param \"" + i.first + "\", this slave is not motion"); + param.active_motor_[cs.model().motionAtSla(id).absID()] = true; } } - msg.copyStruct(param); + + msg_out.copyStruct(param); } auto testGait(aris::dynamic::Model &model, const aris::dynamic::PlanParamBase ¶m_in)->int { diff --git a/doc/Robot_III.xml b/doc/Robot_III.xml index 4d4d480f..215e4bea 100644 --- a/doc/Robot_III.xml +++ b/doc/Robot_III.xml @@ -491,27 +491,27 @@ - - - + + + - - - + + + - - - + + + @@ -539,9 +539,9 @@ - - - + + + diff --git a/src/aris_control/aris_control_motion.cpp b/src/aris_control/aris_control_motion.cpp index e7ab804c..b3a5a5e7 100644 --- a/src/aris_control/aris_control_motion.cpp +++ b/src/aris_control/aris_control_motion.cpp @@ -42,20 +42,13 @@ namespace aris VELOCITYOFFSET=0x60B1, TORQUEOFFSET=0x60B2, MAXTORQUE=0x6072, - }; enum SDO_Entry_Index { - POSITIONLIMIT=0x607D, HOMEOFFSET=0x607C, }; - enum SDO_Entry_Subindex - { - MINPOSLIM=0x01, - MAXPOSLIM=0x02, - }; Imp(Motion *mot) :pFather(mot) {} ~Imp() = default; @@ -96,7 +89,7 @@ namespace aris { /*state is RUNNING, now change it to desired mode*/ pFather->writePdoIndex(MODEOPERATION, 0x00, static_cast(mode)); - return Motion::EXECUTING; + return Motion::MODE_CHANGE; } if (motorState == 0x0060 || motorState ==0x0040) @@ -182,7 +175,7 @@ namespace aris if(modeRead != Motion::HOME_MODE){ pFather->writePdoIndex(MODEOPERATION, 0x00, static_cast(Motion::HOME_MODE)); - return Motion::NOT_READY; + return Motion::MODE_CHANGE; } else if(motorState == 0x0400){ //homing procedure is interrupted or not started @@ -217,7 +210,7 @@ namespace aris pFather->writePdoIndex(CONTROLWORD, 0x00, static_cast(0x0100)); rt_printf("%s\n","homing error occurred, the motor is halting!"); home_period=0; - return Motion::EXE_ERROR; + return Motion::HOME_ERROR; } else if(motorState == 0x1400) { @@ -243,9 +236,13 @@ namespace aris std::uint8_t modeRead; pFather->readPdoIndex(MODEOPERATIONDIS, 0x00,modeRead); - if (motorState != 0x0027 || modeRead != Motion::POSITION) + if (motorState != 0x0027 ) { - return Motion::EXE_ERROR; + return Motion::ENABLE_ERROR; + } + else if(modeRead != Motion::POSITION) + { + return Motion::MODE_ERROR; } else { @@ -266,9 +263,13 @@ namespace aris std::uint8_t modeRead; pFather->readPdoIndex(MODEOPERATIONDIS, 0x00,modeRead); - if (motorState != 0x0027 || modeRead != Motion::VELOCITY) + if (motorState != 0x0027 ) { - return Motion::EXE_ERROR; + return Motion::ENABLE_ERROR; + } + else if(modeRead != Motion::VELOCITY) + { + return Motion::MODE_ERROR; } else { @@ -288,9 +289,13 @@ namespace aris std::uint8_t modeRead; pFather->readPdoIndex(MODEOPERATIONDIS, 0x00,modeRead); - if (motorState != 0x0027 || modeRead != Motion::TORQUE) //need running and cur mode + if (motorState != 0x0027 ) + { + return Motion::ENABLE_ERROR; + } + else if(modeRead != Motion::TORQUE) { - return Motion::EXE_ERROR; + return Motion::MODE_ERROR; } else { @@ -392,7 +397,6 @@ namespace aris } } - auto Motion::logData(const Slave::TxType &tx_data, const Slave::RxType &rx_data, std::fstream &file)->void { auto &rx_motiondata=static_cast(rx_data); @@ -400,6 +404,7 @@ namespace aris file<double { return imp_->max_pos; } auto Motion::minPos()->double { return imp_->min_pos; } auto Motion::maxVel()->double { return imp_->max_vel; } diff --git a/src/aris_control/aris_control_motion.h b/src/aris_control/aris_control_motion.h index 5a610731..3b30389f 100644 --- a/src/aris_control/aris_control_motion.h +++ b/src/aris_control/aris_control_motion.h @@ -40,11 +40,15 @@ namespace aris SUCCESS=0, EXECUTING=1, EXE_FAULT=2,//the fault accure during executing ,can reset - NOT_START=3, - NOT_READY=4, + MODE_CHANGE=3,//changing mode + NOT_START=4, + CMD_ERROR=-1,//all motor should disable when the error accure - EXE_ERROR=-2,//all motor should halt when the error accure during executing + HOME_ERROR=-2,//all motor should halt when the error accure during executing + ENABLE_ERROR=-3,//motor change to disable when run + MODE_ERROR=-4,//motor change to wrong mode when run }; + enum Cmd { IDLE = 0, @@ -66,10 +70,6 @@ namespace aris virtual auto type() const->const std::string&{ return Type(); } Motion(Object &father, std::size_t id, const aris::core::XmlElement &xml_ele); - auto txTypeSize()const->std::size_t override{ return sizeof(TxMotionData); } - auto rxTypeSize()const->std::size_t override{ return sizeof(RxMotionData); } - virtual auto logData(const Slave::TxType &tx_data, const Slave::RxType &rx_data, std::fstream &file)->void override; - auto maxPos()->double; auto minPos()->double; auto maxVel()->double; @@ -78,6 +78,7 @@ namespace aris protected: virtual auto readUpdate()->void override; virtual auto writeUpdate()->void override; + virtual auto logData(const Slave::TxType &tx_data, const Slave::RxType &rx_data, std::fstream &file)->void override; private: class Imp; diff --git a/src/aris_server/aris_server.cpp b/src/aris_server/aris_server.cpp index ebc5ae14..03ce6c2d 100644 --- a/src/aris_server/aris_server.cpp +++ b/src/aris_server/aris_server.cpp @@ -32,6 +32,7 @@ namespace aris auto tg()->void; auto checkError()->int; + auto checkMotionStatus()->void; auto executeCmd()->int; auto enable()->int; auto disable()->int; @@ -262,6 +263,7 @@ namespace aris { // 检查是否出错 // if (checkError())return; + //checkMotionStatus(); // 查看是否有新cmd // if (msg_pipe_.recvInRT(aris::core::MsgRT::instance[0]) > 0) @@ -331,6 +333,29 @@ namespace aris return fault_count; } + auto ControlServer::Imp::checkMotionStatus()->void + { + for (auto &motion : model_->motionPool()) + { + auto &rx_motion_data=static_cast(controller_->rxDataPool().at(motion.slaID())); + if(rx_motion_data.fault_warning==0) + continue; + else + { + rt_printf("Motor %d (sla id) has wrong status:%d\n", motion.slaID(), rx_motion_data.fault_warning); + rt_printf("The status using ABS sequence are:\n"); + for (auto &motion : model_->motionPool()) + { + auto &rx_data=static_cast(controller_->rxDataPool().at(motion.slaID())); + rt_printf("%d\t", rx_data.fault_warning); + } + rt_printf("\n"); + return; + } + } + + } + auto ControlServer::Imp::executeCmd()->int { aris::dynamic::PlanParamBase *param = reinterpret_cast(cmd_queue_[current_cmd_]); @@ -473,7 +498,7 @@ namespace aris for (std::size_t i = 0; imotionPool().size(); ++i) { std::size_t sla_id = model_->motionPool().at(i).slaID(); - if (param->active_motor_[sla_id]) + if (param->active_motor_[i]) { auto &tx_motion_data = static_cast(controller_->txDataPool().at(sla_id)); tx_motion_data.cmd = aris::control::Motion::RUN; @@ -483,7 +508,7 @@ namespace aris } // 检查电机命令是否超限 // - for (std::size_t i = 0; imotionPool().size(); ++i) + for (std::size_t i = 0; imotionPool().size()&¶m->active_motor_[i]; ++i) { std::size_t abs_id = model_->motionPool().at(i).absID(); std::size_t sla_id = model_->motionPool().at(i).slaID();