Skip to content

Commit cfb21da

Browse files
py0330py0330
committed
Merge branch 'szx' into dev
2 parents a43de9e + 0d4b7e8 commit cfb21da

File tree

7 files changed

+123
-81
lines changed

7 files changed

+123
-81
lines changed

‎demo/demo_Aris_Server/demo_Server/main.cpp

Lines changed: 21 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -35,14 +35,14 @@ auto basicParse(const aris::server::ControlServer &cs, const std::string &cmd, c
3535
std::fill_n(param.active_motor_ + 15, 3, true);
3636
}
3737
else if (i.first == "motion_id")
38-
{
38+
{
3939
std::size_t id{ std::stoul(i.second) };
4040
if (id < 0 || id > cs.model().motionPool().size())throw std::runtime_error("invalid param in basic parse func in param \"" + i.first + "\"");
4141
std::fill(param.active_motor_, param.active_motor_ + 18, false);
4242
param.active_motor_[cs.model().motionAtAbs(id).absID()] = true;
4343
}
4444
else if (i.first == "physical_id")
45-
{
45+
{
4646
std::size_t id{ std::stoul(i.second) };
4747
if (id < 0 || id > cs.model().motionPool().size())throw std::runtime_error("invalid param in basic parse func in param \"" + i.first + "\"");
4848
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
6969
msg_out.copyStruct(param);
7070
}
7171

72-
auto testParse(const aris::server::ControlServer &cs, const std::string &cmd, const std::map<std::string, std::string> &params, aris::core::Msg &msg)->void
72+
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
7373
{
7474
aris::server::GaitParamBase param;
7575

@@ -79,32 +79,31 @@ auto testParse(const aris::server::ControlServer &cs, const std::string &cmd, co
7979
{
8080
std::fill_n(param.active_motor_, 18, true);
8181
}
82-
else if (i.first == "motor")
82+
else if (i.first == "motion_id")
8383
{
84-
int id = { stoi(i.second) };
85-
if (id<0 || id>17)throw std::runtime_error("invalid param in basicParse func");
86-
87-
std::fill_n(param.active_motor_, 18, false);
88-
param.active_motor_[id] = true;
84+
std::size_t id{ std::stoul(i.second) };
85+
if (id < 0 || id > cs.model().motionPool().size())throw std::runtime_error("invalid param in basic parse func in param \"" + i.first + "\"");
86+
std::fill(param.active_motor_, param.active_motor_ + 18, false);
87+
param.active_motor_[cs.model().motionAtAbs(id).absID()] = true;
8988
}
90-
else if (i.first == "physical_motor")
89+
else if (i.first == "physical_id")
9190
{
92-
int id = { stoi(i.second) };
93-
if (id<0 || id>17)throw std::runtime_error("invalid param in basicParse func");
94-
95-
std::fill_n(param.active_motor_, 18, false);
96-
param.active_motor_[id] = true;
91+
std::size_t id{ std::stoul(i.second) };
92+
if (id < 0 || id > cs.model().motionPool().size())throw std::runtime_error("invalid param in basic parse func in param \"" + i.first + "\"");
93+
std::fill(param.active_motor_, param.active_motor_ + 18, false);
94+
param.active_motor_[cs.model().motionAtPhy(id).absID()] = true;
9795
}
98-
else if (i.first == "slave_motor")
96+
else if (i.first == "slave_id")
9997
{
100-
int id = { stoi(i.second) };
101-
if (id<0 || id>17)throw std::runtime_error("invalid param in basicParse func");
102-
103-
std::fill_n(param.active_motor_, 18, false);
104-
param.active_motor_[id] = true;
98+
std::size_t id{ std::stoul(i.second) };
99+
if (id < 0 || id > cs.controller().slavePool().size())throw std::runtime_error("invalid param in basic parse func in param \"" + i.first + "\"");
100+
std::fill(param.active_motor_, param.active_motor_ + 18, false);
101+
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");
102+
param.active_motor_[cs.model().motionAtSla(id).absID()] = true;
105103
}
106104
}
107-
msg.copyStruct(param);
105+
106+
msg_out.copyStruct(param);
108107
}
109108
auto testGait(aris::dynamic::Model &model, const aris::dynamic::PlanParamBase &param_in)->int
110109
{

‎doc/Robot_III.xml

Lines changed: 12 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -491,27 +491,27 @@
491491
<all abbreviation="a"/>
492492
<first abbreviation="f"/>
493493
<second abbreviation="s"/>
494-
<motor abbreviation="m" default="0"/>
495-
<physical_motor abbreviation="p" default="0"/>
496-
<slave_motor abbreviation="c" default="0"/>
494+
<motion_id abbreviation="m" default="0"/>
495+
<physical_id abbreviation="p" default="0"/>
496+
<slave_id abbreviation="c" default="0"/>
497497
<leg abbreviation="l" default="0"/>
498498
</en>
499499
<ds default_child_type="param" default="all">
500500
<all abbreviation="a"/>
501501
<first abbreviation="f"/>
502502
<second abbreviation="s"/>
503-
<motor abbreviation="m" default="0"/>
504-
<physical_motor abbreviation="p" default="0"/>
505-
<slave_motor abbreviation="c" default="0"/>
503+
<motion_id abbreviation="m" default="0"/>
504+
<physical_id abbreviation="p" default="0"/>
505+
<slave_id abbreviation="c" default="0"/>
506506
<leg abbreviation="l" default="0"/>
507507
</ds>
508508
<hm default_child_type="param" default="all">
509509
<all abbreviation="a"/>
510510
<first abbreviation="f"/>
511511
<second abbreviation="s"/>
512-
<motor abbreviation="m" default="0"/>
513-
<physical_motor abbreviation="p" default="0"/>
514-
<slave_motor abbreviation="c" default="0"/>
512+
<motion_id abbreviation="m" default="0"/>
513+
<physical_id abbreviation="p" default="0"/>
514+
<slave_id abbreviation="c" default="0"/>
515515
<leg abbreviation="l" default="0"/>
516516
</hm>
517517
<rc default="rc_param">
@@ -539,9 +539,9 @@
539539
</wk>
540540
<test default_child_type="param" default="all">
541541
<all abbreviation="a"/>
542-
<motor abbreviation="m" default="0"/>
543-
<physical_motor abbreviation="p" default="0"/>
544-
<slave_motor abbreviation="c" default="0"/>
542+
<motion_id abbreviation="m" default="0"/>
543+
<physical_id abbreviation="p" default="0"/>
544+
<slave_id abbreviation="c" default="0"/>
545545
</test>
546546
</Commands>
547547
</Server>

‎src/aris_control/aris_control_motion.cpp

Lines changed: 22 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -42,20 +42,13 @@ namespace aris
4242
VELOCITYOFFSET=0x60B1,
4343
TORQUEOFFSET=0x60B2,
4444
MAXTORQUE=0x6072,
45-
4645
};
4746

4847
enum SDO_Entry_Index
4948
{
50-
POSITIONLIMIT=0x607D,
5149
HOMEOFFSET=0x607C,
5250
};
5351

54-
enum SDO_Entry_Subindex
55-
{
56-
MINPOSLIM=0x01,
57-
MAXPOSLIM=0x02,
58-
};
5952

6053
Imp(Motion *mot) :pFather(mot) {}
6154
~Imp() = default;
@@ -96,7 +89,7 @@ namespace aris
9689
{
9790
/*state is RUNNING, now change it to desired mode*/
9891
pFather->writePdoIndex(MODEOPERATION, 0x00, static_cast<std::uint8_t>(mode));
99-
return Motion::EXECUTING;
92+
return Motion::MODE_CHANGE;
10093
}
10194

10295
if (motorState == 0x0060 || motorState ==0x0040)
@@ -182,7 +175,7 @@ namespace aris
182175

183176
if(modeRead != Motion::HOME_MODE){
184177
pFather->writePdoIndex(MODEOPERATION, 0x00, static_cast<std::uint8_t>(Motion::HOME_MODE));
185-
return Motion::NOT_READY;
178+
return Motion::MODE_CHANGE;
186179
}
187180
else if(motorState == 0x0400){
188181
//homing procedure is interrupted or not started
@@ -217,7 +210,7 @@ namespace aris
217210
pFather->writePdoIndex(CONTROLWORD, 0x00, static_cast<std::uint16_t>(0x0100));
218211
rt_printf("%s\n","homing error occurred, the motor is halting!");
219212
home_period=0;
220-
return Motion::EXE_ERROR;
213+
return Motion::HOME_ERROR;
221214
}
222215
else if(motorState == 0x1400)
223216
{
@@ -243,9 +236,13 @@ namespace aris
243236

244237
std::uint8_t modeRead;
245238
pFather->readPdoIndex(MODEOPERATIONDIS, 0x00,modeRead);
246-
if (motorState != 0x0027 || modeRead != Motion::POSITION)
239+
if (motorState != 0x0027 )
247240
{
248-
return Motion::EXE_ERROR;
241+
return Motion::ENABLE_ERROR;
242+
}
243+
else if(modeRead != Motion::POSITION)
244+
{
245+
return Motion::MODE_ERROR;
249246
}
250247
else
251248
{
@@ -266,9 +263,13 @@ namespace aris
266263

267264
std::uint8_t modeRead;
268265
pFather->readPdoIndex(MODEOPERATIONDIS, 0x00,modeRead);
269-
if (motorState != 0x0027 || modeRead != Motion::VELOCITY)
266+
if (motorState != 0x0027 )
270267
{
271-
return Motion::EXE_ERROR;
268+
return Motion::ENABLE_ERROR;
269+
}
270+
else if(modeRead != Motion::VELOCITY)
271+
{
272+
return Motion::MODE_ERROR;
272273
}
273274
else
274275
{
@@ -288,9 +289,13 @@ namespace aris
288289

289290
std::uint8_t modeRead;
290291
pFather->readPdoIndex(MODEOPERATIONDIS, 0x00,modeRead);
291-
if (motorState != 0x0027 || modeRead != Motion::TORQUE) //need running and cur mode
292+
if (motorState != 0x0027 )
293+
{
294+
return Motion::ENABLE_ERROR;
295+
}
296+
else if(modeRead != Motion::TORQUE)
292297
{
293-
return Motion::EXE_ERROR;
298+
return Motion::MODE_ERROR;
294299
}
295300
else
296301
{
@@ -392,14 +397,14 @@ namespace aris
392397
}
393398

394399
}
395-
396400
auto Motion::logData(const Slave::TxType &tx_data, const Slave::RxType &rx_data, std::fstream &file)->void
397401
{
398402
auto &rx_motiondata=static_cast<const RxType &>(rx_data);
399403
auto &tx_motiondata=static_cast<const TxType &>(tx_data);
400404
file<<rx_motiondata.feedback_pos<<" ";
401405
file<<tx_motiondata.target_pos;
402406
}
407+
403408
auto Motion::maxPos()->double { return imp_->max_pos; }
404409
auto Motion::minPos()->double { return imp_->min_pos; }
405410
auto Motion::maxVel()->double { return imp_->max_vel; }

‎src/aris_control/aris_control_motion.h

Lines changed: 8 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -40,11 +40,15 @@ namespace aris
4040
SUCCESS=0,
4141
EXECUTING=1,
4242
EXE_FAULT=2,//the fault accure during executing ,can reset
43-
NOT_START=3,
44-
NOT_READY=4,
43+
MODE_CHANGE=3,//changing mode
44+
NOT_START=4,
45+
4546
CMD_ERROR=-1,//all motor should disable when the error accure
46-
EXE_ERROR=-2,//all motor should halt when the error accure during executing
47+
HOME_ERROR=-2,//all motor should halt when the error accure during executing
48+
ENABLE_ERROR=-3,//motor change to disable when run
49+
MODE_ERROR=-4,//motor change to wrong mode when run
4750
};
51+
4852
enum Cmd
4953
{
5054
IDLE = 0,
@@ -66,10 +70,6 @@ namespace aris
6670
virtual auto type() const->const std::string&{ return Type(); }
6771
Motion(Object &father, std::size_t id, const aris::core::XmlElement &xml_ele);
6872

69-
auto txTypeSize()const->std::size_t override{ return sizeof(TxMotionData); }
70-
auto rxTypeSize()const->std::size_t override{ return sizeof(RxMotionData); }
71-
virtual auto logData(const Slave::TxType &tx_data, const Slave::RxType &rx_data, std::fstream &file)->void override;
72-
7373
auto maxPos()->double;
7474
auto minPos()->double;
7575
auto maxVel()->double;
@@ -78,6 +78,7 @@ namespace aris
7878
protected:
7979
virtual auto readUpdate()->void override;
8080
virtual auto writeUpdate()->void override;
81+
virtual auto logData(const Slave::TxType &tx_data, const Slave::RxType &rx_data, std::fstream &file)->void override;
8182

8283
private:
8384
class Imp;

‎src/aris_server/aris_server.cpp

Lines changed: 27 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,7 @@ namespace aris
3232

3333
auto tg()->void;
3434
auto checkError()->int;
35+
auto checkMotionStatus()->void;
3536
auto executeCmd()->int;
3637
auto enable()->int;
3738
auto disable()->int;
@@ -262,6 +263,7 @@ namespace aris
262263
{
263264
// 检查是否出错 //
264265
if (checkError())return;
266+
//checkMotionStatus();
265267

266268
// 查看是否有新cmd //
267269
if (msg_pipe_.recvInRT(aris::core::MsgRT::instance[0]) > 0)
@@ -331,6 +333,29 @@ namespace aris
331333

332334
return fault_count;
333335
}
336+
auto ControlServer::Imp::checkMotionStatus()->void
337+
{
338+
for (auto &motion : model_->motionPool())
339+
{
340+
auto &rx_motion_data=static_cast<aris::control::RxMotionData&>(controller_->rxDataPool().at(motion.slaID()));
341+
if(rx_motion_data.fault_warning==0)
342+
continue;
343+
else
344+
{
345+
rt_printf("Motor %d (sla id) has wrong status:%d\n", motion.slaID(), rx_motion_data.fault_warning);
346+
rt_printf("The status using ABS sequence are:\n");
347+
for (auto &motion : model_->motionPool())
348+
{
349+
auto &rx_data=static_cast<aris::control::RxMotionData&>(controller_->rxDataPool().at(motion.slaID()));
350+
rt_printf("%d\t", rx_data.fault_warning);
351+
}
352+
rt_printf("\n");
353+
return;
354+
}
355+
}
356+
357+
}
358+
334359
auto ControlServer::Imp::executeCmd()->int
335360
{
336361
aris::dynamic::PlanParamBase *param = reinterpret_cast<aris::dynamic::PlanParamBase *>(cmd_queue_[current_cmd_]);
@@ -473,7 +498,7 @@ namespace aris
473498
for (std::size_t i = 0; i<model_->motionPool().size(); ++i)
474499
{
475500
std::size_t sla_id = model_->motionPool().at(i).slaID();
476-
if (param->active_motor_[sla_id])
501+
if (param->active_motor_[i])
477502
{
478503
auto &tx_motion_data = static_cast<aris::control::TxMotionData&>(controller_->txDataPool().at(sla_id));
479504
tx_motion_data.cmd = aris::control::Motion::RUN;
@@ -483,7 +508,7 @@ namespace aris
483508
}
484509

485510
// 检查电机命令是否超限 //
486-
for (std::size_t i = 0; i<model_->motionPool().size(); ++i)
511+
for (std::size_t i = 0; i<model_->motionPool().size()&&param->active_motor_[i]; ++i)
487512
{
488513
std::size_t abs_id = model_->motionPool().at(i).absID();
489514
std::size_t sla_id = model_->motionPool().at(i).slaID();

‎test/test_control/main.cpp

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -7,11 +7,11 @@
77

88
int main(int argc, char *argv[])
99
{
10-
test_control_ethercat();
10+
test_control_ethercat();
1111
test_control_motion();
1212

1313
std::cout << "test_control finished, press any key to continue" << std::endl;
1414
std::cin.get();
1515

1616
return 0;
17-
}
17+
}

0 commit comments

Comments
 (0)