47 lines
3.0 KiB
C++
47 lines
3.0 KiB
C++
#include "../src/logic.cpp"
|
|
#include <iostream>
|
|
#include <stdexcept>
|
|
std::array<double,13> setpoint{},encoder{};
|
|
int badAxis=0,writeFailure=0;
|
|
int32_t error(int32_t axis) { return axis==badAxis; }
|
|
int32_t errorId(int32_t axis) { return axis==badAxis?85017:0; }
|
|
int32_t putSet(int32_t axis,double value) { setpoint.at(axis)=value; return writeFailure; }
|
|
int32_t putEnc(int32_t axis,double value) { encoder.at(axis)=value; return writeFailure; }
|
|
void check(bool ok,const char* msg) { if(!ok) throw std::runtime_error(msg); }
|
|
int main() {
|
|
ecmcCppLogicHostServices host{};
|
|
host.get_axis_error=error; host.get_axis_error_id=errorId; host.set_axis_ext_set_pos=putSet; host.set_axis_ext_act_pos=putEnc;
|
|
ecmcCpp::setHostServices(&host);
|
|
ArmLogic logic; logic.run(); check(!logic.fault,"zero axes represent home");
|
|
// Verify real binding descriptors rather than relying on the mock to resolve names.
|
|
check(logic.ecmc.count()==18,"18 input bindings");
|
|
check(std::string(logic.ecmc.data()[0].itemName)=="ax1.setpos","coordinate asyn path");
|
|
check(std::string(logic.ecmc.data()[1].itemName)=="ax7.actpos","joint asyn path");
|
|
logic.pose_set={5,-4,8,2,-3,4}; logic.run(); check(!logic.fault,"IK cycle");
|
|
for(int i=0;i<6;++i) logic.joint_trajectory[i]=setpoint[i+7];
|
|
logic.run();
|
|
for(int i=0;i<6;++i) logic.joint_actual[i]=encoder[i+7];
|
|
logic.run(); check(!logic.fault,"FK cycle");
|
|
for(int i=0;i<6;++i) check(std::abs(encoder[i+1]-logic.pose_set[i])<1e-4,"coordinate FK feedback");
|
|
ArmLogic direct; direct.joint_trajectory={1,2,3,4,5,6}; direct.run();
|
|
for(int i=0;i<6;++i) { check(encoder[i+7]==i+1,"direct joint ideal plant"); direct.joint_actual[i]=encoder[i+7]; }
|
|
direct.run(); check(!direct.fault&&std::abs(direct.pose_actual[0])>.01,"direct joint FK");
|
|
const auto held=setpoint; logic.pose_set[0]=10000; logic.run(); check(logic.fault==2&&setpoint==held,"IK failure holds outputs");
|
|
logic.pose_set={}; logic.run(); check(logic.fault==2,"latched fault");
|
|
// An IK fault must stop new commands without freezing the simulation plant.
|
|
logic.joint_trajectory[0]+=0.5;
|
|
logic.run();
|
|
check(logic.fault==2&&setpoint==held,"fault inhibits new IK commands");
|
|
check(encoder[7]==logic.joint_trajectory[0],"plant runs after IK fault");
|
|
ArmLogic limits; limits.joint_trajectory[0]=200; limits.run(); check(limits.fault==1,"joint limit");
|
|
ArmLogic axis; badAxis=1; axis.joint_trajectory[0]=2; axis.run();
|
|
check(axis.fault==4&&axis.fault_axis==1&&axis.fault_axis_error==85017,"axis stall diagnostics");
|
|
check(encoder[7]==2,"plant runs on axis fault cycle");
|
|
const auto stopped=setpoint;
|
|
axis.joint_trajectory[0]=3; axis.joint_actual[0]=2; axis.run();
|
|
check(encoder[7]==3&&std::abs(axis.pose_actual[1])>.01,"plant and FK run while fault latched");
|
|
check(setpoint==stopped,"axis fault inhibits new IK commands"); badAxis=0;
|
|
ArmLogic write; writeFailure=1; write.run(); check(write.fault==3,"host failure");
|
|
std::cout<<"PASS: input bindings, IK/FK pipeline, direct joint plant, latched faults\n";
|
|
}
|