Skip to content

Commit

Permalink
Merge branch 'szx' into dev
Browse files Browse the repository at this point in the history
  • Loading branch information
@py0330
py0330 committed Jun 14, 2016
2 parents a43de9e + 0d4b7e8 commit cfb21da
Show file tree
Hide file tree
Showing 7 changed files with 123 additions and 81 deletions.
43 changes: 21 additions & 22 deletions demo/demo_Aris_Server/demo_Server/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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<std::string, std::string> &params, aris::core::Msg &msg)->void
auto testParse(const aris::server::ControlServer &cs, const std::string &cmd, const std::map<std::string, std::string> &params, aris::core::Msg &msg_out)->void
{
aris::server::GaitParamBase param;

Expand All @@ -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 &param_in)->int
{
Expand Down
24 changes: 12 additions & 12 deletions doc/Robot_III.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -491,27 +491,27 @@
<all abbreviation="a"/>
<first abbreviation="f"/>
<second abbreviation="s"/>
<motor abbreviation="m" default="0"/>
<physical_motor abbreviation="p" default="0"/>
<slave_motor abbreviation="c" default="0"/>
<motion_id abbreviation="m" default="0"/>
<physical_id abbreviation="p" default="0"/>
<slave_id abbreviation="c" default="0"/>
<leg abbreviation="l" default="0"/>
</en>
<ds default_child_type="param" default="all">
<all abbreviation="a"/>
<first abbreviation="f"/>
<second abbreviation="s"/>
<motor abbreviation="m" default="0"/>
<physical_motor abbreviation="p" default="0"/>
<slave_motor abbreviation="c" default="0"/>
<motion_id abbreviation="m" default="0"/>
<physical_id abbreviation="p" default="0"/>
<slave_id abbreviation="c" default="0"/>
<leg abbreviation="l" default="0"/>
</ds>
<hm default_child_type="param" default="all">
<all abbreviation="a"/>
<first abbreviation="f"/>
<second abbreviation="s"/>
<motor abbreviation="m" default="0"/>
<physical_motor abbreviation="p" default="0"/>
<slave_motor abbreviation="c" default="0"/>
<motion_id abbreviation="m" default="0"/>
<physical_id abbreviation="p" default="0"/>
<slave_id abbreviation="c" default="0"/>
<leg abbreviation="l" default="0"/>
</hm>
<rc default="rc_param">
Expand Down Expand Up @@ -539,9 +539,9 @@
</wk>
<test default_child_type="param" default="all">
<all abbreviation="a"/>
<motor abbreviation="m" default="0"/>
<physical_motor abbreviation="p" default="0"/>
<slave_motor abbreviation="c" default="0"/>
<motion_id abbreviation="m" default="0"/>
<physical_id abbreviation="p" default="0"/>
<slave_id abbreviation="c" default="0"/>
</test>
</Commands>
</Server>
Expand Down
39 changes: 22 additions & 17 deletions src/aris_control/aris_control_motion.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -96,7 +89,7 @@ namespace aris
{
/*state is RUNNING, now change it to desired mode*/
pFather->writePdoIndex(MODEOPERATION, 0x00, static_cast<std::uint8_t>(mode));
return Motion::EXECUTING;
return Motion::MODE_CHANGE;
}

if (motorState == 0x0060 || motorState ==0x0040)
Expand Down Expand Up @@ -182,7 +175,7 @@ namespace aris

if(modeRead != Motion::HOME_MODE){
pFather->writePdoIndex(MODEOPERATION, 0x00, static_cast<std::uint8_t>(Motion::HOME_MODE));
return Motion::NOT_READY;
return Motion::MODE_CHANGE;
}
else if(motorState == 0x0400){
//homing procedure is interrupted or not started
Expand Down Expand Up @@ -217,7 +210,7 @@ namespace aris
pFather->writePdoIndex(CONTROLWORD, 0x00, static_cast<std::uint16_t>(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)
{
Expand All @@ -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
{
Expand All @@ -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
{
Expand All @@ -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
{
Expand Down Expand Up @@ -392,14 +397,14 @@ namespace aris
}

}

auto Motion::logData(const Slave::TxType &tx_data, const Slave::RxType &rx_data, std::fstream &file)->void
{
auto &rx_motiondata=static_cast<const RxType &>(rx_data);
auto &tx_motiondata=static_cast<const TxType &>(tx_data);
file<<rx_motiondata.feedback_pos<<" ";
file<<tx_motiondata.target_pos;
}

auto Motion::maxPos()->double { return imp_->max_pos; }
auto Motion::minPos()->double { return imp_->min_pos; }
auto Motion::maxVel()->double { return imp_->max_vel; }
Expand Down
15 changes: 8 additions & 7 deletions src/aris_control/aris_control_motion.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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;
Expand All @@ -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;
Expand Down
29 changes: 27 additions & 2 deletions src/aris_server/aris_server.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@ namespace aris

auto tg()->void;
auto checkError()->int;
auto checkMotionStatus()->void;
auto executeCmd()->int;
auto enable()->int;
auto disable()->int;
Expand Down Expand Up @@ -262,6 +263,7 @@ namespace aris
{
// 检查是否出错 //
if (checkError())return;
//checkMotionStatus();

// 查看是否有新cmd //
if (msg_pipe_.recvInRT(aris::core::MsgRT::instance[0]) > 0)
Expand Down Expand Up @@ -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<aris::control::RxMotionData&>(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<aris::control::RxMotionData&>(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<aris::dynamic::PlanParamBase *>(cmd_queue_[current_cmd_]);
Expand Down Expand Up @@ -473,7 +498,7 @@ namespace aris
for (std::size_t i = 0; i<model_->motionPool().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<aris::control::TxMotionData&>(controller_->txDataPool().at(sla_id));
tx_motion_data.cmd = aris::control::Motion::RUN;
Expand All @@ -483,7 +508,7 @@ namespace aris
}

// 检查电机命令是否超限 //
for (std::size_t i = 0; i<model_->motionPool().size(); ++i)
for (std::size_t i = 0; i<model_->motionPool().size()&&param->active_motor_[i]; ++i)
{
std::size_t abs_id = model_->motionPool().at(i).absID();
std::size_t sla_id = model_->motionPool().at(i).slaID();
Expand Down
4 changes: 2 additions & 2 deletions test/test_control/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
}
Loading

0 comments on commit cfb21da

Please sign in to comment.