Fix phys axes..

README.md

@@ -333,8 +333,8 @@ Each YAML file explicitly sets `epics.motorRecord.enable: true` and
 The YAML loader also creates the standard axis diagnostic/control PVs using the
 names X, Y, Z, Rx, Ry, Rz and L1–L6 (mm for translations/legs, deg for rotations).
 The YAML loader configures motor-record support through ecmccfg. Use the
-coordinate motor records for commanding axes 1–6. Do not command leg axes 7–12
-independently while the transform is active.
+coordinate motor records for commanding axes 1–6. Direct leg moves on axes 7–12
+require the master/slave state machine to select the legs' internal trajectories.
 Axis IDs 1–12 must be free. No axis PLC may overwrite these external setpoints or
 encoder feeds.
 
@@ -343,7 +343,10 @@ coordinate trajectory positions and leg encoder positions from that cycle, then
 writes values consumed by the next axis cycle. This introduces pipeline latency;
 coordinate feedback also includes the inverse/plant/forward round trip.
 
-The ideal leg plant copies the computed stroke to the external leg encoder.
+The ideal leg plant copies each leg axis's selected trajectory position
+(`axN.setpos`) to its external encoder. This follows IK during coordinate moves
+and the internal leg trajectory during direct leg moves. The feedback is consumed
+on a subsequent axis cycle.
 The forward solver reads the **axis encoder values**, not the commanded pose, and
 writes its resulting six coordinates to the external coordinate encoders.
 A disabled virtual leg does not model a stopped physical actuator: the ideal plant

cfg/axis_coordinate.yaml

@@ -30,7 +30,7 @@ encoder:
 trajectory:
   source: 0
   axis:
-    velocity: 5
+    velocity: 1
     acceleration: 10
     deceleration: 10
 

cfg/axis_leg.yaml

@@ -31,7 +31,7 @@ encoder:
 trajectory:
   source: 1
   axis:
-    velocity: 5
+    velocity: 1
     acceleration: 10
     deceleration: 10
 

src/logic.cpp

@@ -8,6 +8,7 @@ struct HexapodLogic : ecmcCpp::LogicBase {
   std::array<double, 6> pose_actual{};
   std::array<double, 6> leg_set{};
   std::array<double, 6> leg_actual{};
+  std::array<double, 6> leg_trajectory{};
   std::array<double, 6> geometry_home{};
   // 1 invalid/stroke, 2 FK, 3 host write, 4 axis error
   int32_t fault{0};
@@ -40,6 +41,7 @@ struct HexapodLogic : ecmcCpp::LogicBase {
       // C++ item bindings use asyn names, rather than PLC object paths.
       ecmc.input("ax"+std::to_string(coordFirst+i)+".setpos",pose_set[i]);
       ecmc.input("ax"+std::to_string(legFirst+i)+".actpos",leg_actual[i]);
+      ecmc.input("ax"+std::to_string(legFirst+i)+".setpos",leg_trajectory[i]);
     }
     epics.readOnlyArray("hex.pose_set",pose_set)
          .readOnlyArray("hex.pose_actual",pose_actual)
@@ -53,7 +55,7 @@ struct HexapodLogic : ecmcCpp::LogicBase {
   void run() override {
     if(fault) return; // latched; reload/restart simulation after fixing cause
     for(int i=0;i<6;++i) if(ecmcCpp::axisHasError(coordFirst+i) || ecmcCpp::axisHasError(legFirst+i)) { fault=4; return; }
-    if(!hexapod::finite(pose_set)) { fault=1; return; }
+    if(!hexapod::finite(pose_set) || !hexapod::finite(leg_trajectory)) { fault=1; return; }
     auto lengths=model.inverse(pose_set);
     if(!model.withinStroke(lengths)) { fault=1; return; }
     // Leg axis zero = its individual length at nominal home (not total length).
@@ -65,10 +67,11 @@ struct HexapodLogic : ecmcCpp::LogicBase {
     pose_actual=fk.pose;
     for(int i=0;i<6;++i) leg_set[i]=lengths[i]-geometry_home[i];
     for(int i=0;i<6;++i) {
-      // Unit-gain plant: publish previous command as next encoder sample.
+      // Ideal plant follows the axis's selected trajectory, including direct
+      // leg moves when the state machine selects its internal source.
       // Sources are configured before enabling axes in cfg/axes.cmd.
       int rc=ecmcCpp::axisSetExternalSetpointPos(legFirst+i,leg_set[i]);
-      rc|=ecmcCpp::axisSetExternalEncoderPos(legFirst+i,leg_set[i]);
+      rc|=ecmcCpp::axisSetExternalEncoderPos(legFirst+i,leg_trajectory[i]);
       rc|=ecmcCpp::axisSetExternalEncoderPos(coordFirst+i,pose_actual[i]);
       if(rc) { fault=3; return; }
     }

tests/test_logic.cpp

@@ -15,9 +15,21 @@ int main() {
   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");