41 lines
2.2 KiB
C++
41 lines
2.2 KiB
C++
#include "../src/logic.cpp"
|
|
#include <iostream>
|
|
#include <stdexcept>
|
|
#include <array>
|
|
std::array<double,13> setpoint{},encoder{};
|
|
int badAxis=0,writeFailure=0;
|
|
int32_t error(int32_t axis) { return axis==badAxis; }
|
|
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 value,const char* message) { if(!value) throw std::runtime_error(message); }
|
|
int main() {
|
|
ecmcCppLogicHostServices host{};
|
|
host.get_axis_error=error; host.set_axis_ext_set_pos=putSet; host.set_axis_ext_act_pos=putEnc;
|
|
ecmcCpp::setHostServices(&host);
|
|
HexapodLogic logic;
|
|
logic.run(); check(logic.fault==0,"home cycle");
|
|
logic.pose_set={5,-4,8,2,-3,4}; logic.run(); check(logic.fault==0,"IK cycle");
|
|
for(int i=0;i<6;++i) logic.leg_trajectory[i]=setpoint[i+7];
|
|
logic.run(); check(logic.fault==0,"leg trajectory cycle");
|
|
for(int i=0;i<6;++i) logic.leg_actual[i]=encoder[i+7];
|
|
logic.run(); check(logic.fault==0,"FK cycle");
|
|
for(int i=0;i<6;++i) check(std::abs(encoder[i+1]-logic.pose_set[i])<1e-5,"coordinate encoder FK");
|
|
HexapodLogic direct;
|
|
direct.leg_trajectory={1,2,3,4,5,6};
|
|
direct.run(); check(direct.fault==0,"direct leg move cycle");
|
|
for(int i=0;i<6;++i) {
|
|
check(encoder[i+7]==direct.leg_trajectory[i],"leg encoder follows selected trajectory");
|
|
check(setpoint[i+7]==0,"IK remains separate from direct leg trajectory");
|
|
direct.leg_actual[i]=encoder[i+7];
|
|
}
|
|
direct.run(); check(direct.fault==0,"direct leg FK cycle");
|
|
check(std::abs(direct.pose_actual[2])>0.01,"direct leg motion updates coordinate feedback");
|
|
auto held=setpoint; logic.pose_set[2]=1000; logic.run();
|
|
check(logic.fault==1 && setpoint==held,"stroke fails before writes");
|
|
logic.pose_set={}; logic.run(); check(logic.fault==1 && setpoint==held,"fault latched");
|
|
HexapodLogic fkFail; fkFail.leg_actual[0]=-1000; fkFail.run(); check(fkFail.fault==2,"FK error");
|
|
HexapodLogic axisFail; badAxis=8; axisFail.run(); check(axisFail.fault==4,"axis error"); badAxis=0;
|
|
HexapodLogic hostFail; writeFailure=1; hostFail.run(); check(hostFail.fault==3,"host error");
|
|
std::cout<<"PASS: 12-axis adapter, encoder feedback, latched stroke/FK/axis/host faults\n";
|
|
}
|