MC_HOMING_PROCEDURES__MCHOMEABSOLUTESWITCHPOSITIVE
·67 min read
MC_HOME in MCHOMEABSOLUTESWITCHPOSITIVE模式
graph TD;
mc_state_Standstill-->mc_state_Homing
mc_state_Homing-->s.step_1
s.step_1-..->Axis.Public.TouchProbeRisingTrigger
s.step_1-->Axis.OTG.ctrl_mode=gt_Velocity
Axis.OTG.ctrl_mode=gt_Velocity-->Axis.Public.AxisMotionMode=mc_mode_csp
Axis.Public.TouchProbeRisingTrigger-->s.step_3
s.step_3-..->Home
Home-->s.Step_5
Home-->Axis.Public.HomingSignalFound=1
s.Step_5-->!Home
s.Step_5-->gt_Position
active_cmd.Completed-->s.Step_99
!Home-->s.Step_99
s.Step_99-->gt_Velocity
Axis.OTG.state=gt_Reached-->Axis.Public_HomingCompleted=1
- __MK_ComputeAxis
-
AXIS MOTION EXEC
1. Axis->State = mc_state_Homing;
-
AXIS MOTION COMPUTE
1. s->Step == 99
2. Axis->OTG.state & gt_Reached
1. Axis->Public.HomingCompleted = 1;
-
AXIS SUPERIMPOSED EXEC
-
AXIS SUPERIMPOSED COMPUTE
-
AXIS PHASING EXEC
-
AXIS PHASING COMPUTE
- ################## AXIS SEQUENCING ##################
void __MK_ComputeAxis(int AxisRef) {
AXIS_REF_s *Axis = &AXIS_REF_inst[AxisRef];
MC_LOGCONTEXT
{ /*##### AXIS MOTION EXEC ######*/
int cmd_abort = 0;
axis_cmd_chnl_s *active_cmd = &Axis->motion;
if (active_cmd->Lock) {
active_cmd->Starting = 1;
active_cmd->Lock = 0;
if (!active_cmd->Aborted) {
axis_cmd_chnl_s *target_cmd = active_cmd;
switch (target_cmd->Type) {
case MC_Home: {
__mcl_snapshot_MC_Home *s =
&target_cmd->snapshot.MC_Home;
if ((!Axis->Public.DigitalInputsEnabled) &&
(!Axis->HomingProcedures == MC_HOMING_PROCEDURES__MCHOMEDIRECT) &&
(!Axis->HomingProcedures == MC_HOMING_PROCEDURES__MCHOMEABSOLUTE)) {
active_cmd->Error = 1;
active_cmd->ErrorID = ERROR_ILLEGAL_COMMAND;
Axis->Public.ErrorCode = ERROR_ILLEGAL_COMMAND;
Axis->State = mc_state_ErrorStop;
cmd_abort = 1;
MC_LOG_CRITICAL("Axis option DigitalInputs not enabled")
break;
}
if ((!Axis->Public.TouchProbeEnabled) &&
(!Axis->HomingProcedures == MC_HOMING_PROCEDURES__MCHOMEDIRECT) &&
(!Axis->HomingProcedures == MC_HOMING_PROCEDURES__MCHOMEABSOLUTE)) {
active_cmd->Error = 1;
active_cmd->ErrorID = ERROR_ILLEGAL_COMMAND;
Axis->Public.ErrorCode = ERROR_ILLEGAL_COMMAND;
MC_LOG_CRITICAL("Axis option TouchProbe not enabled")
}
/* Check if Axis state change is permitted */
if (!((Axis->State == mc_state_Homing || Axis->State == mc_state_Standstill))) {
active_cmd->Error = 1;
active_cmd->ErrorID = ERROR_ILLEGAL_COMMAND;
Axis->Public.ErrorCode = ERROR_ILLEGAL_COMMAND;
Axis->State = mc_state_ErrorStop;
cmd_abort = 1;
break;
}
Axis->State = mc_state_Homing;
MC_LOG("State: Homing [MC_Home]")
s->Step = 0;
s->ProbeStep = 0;
if (Axis->HomingProcedures == MC_HOMING_PROCEDURES__MCHOMEABSOLUTE)
Axis->OTG.ctrl_mode = gt_Homing;
Axis->OTG.Gacc = Axis->Public.MaxAccelerationAppl;
Axis->OTG.Gdec = Axis->Public.MaxAccelerationAppl;
Axis->OTG.J = 0.0;
Axis->Public.TouchProbeFunction = 0;
Axis->Public.TouchProbeRisingTrigger = 0;
Axis->Public.HomingSignalFound = 0;
Axis->Public.HomingCtrlByPwr = 1;
Axis->Public.Axis_Zpulse = 0.0;
s->HomePositiveSwitchFound = 0;
s->HomeNegativeSwitchFound = 0;
s->HomeForwardFallFound = 0;
s->HomeForwardRaiseFound = 0;
s->HomeBackwardFallFound = 0;
s->HomeBackwardRaiseFound = 0;
active_cmd->BlendVelocity = 0;
active_cmd->EndVelocity = 0;
}
break;
default:
break;
}
}
if (active_cmd->PrevType != MC_NoAxisCmd) {
/* Command did replace (abort) another command */
/* aborts and purge superimposed */
if (Axis->superimposed.Type != MC_NoAxisCmd) {
Axis->superimposed.Type = MC_NoAxisCmd;
Axis->superimposed.Data = NULL;
}
/* aborts and purge phasing */
if (Axis->phasing.Type != MC_NoAxisCmd) {
Axis->phasing.Type = MC_NoAxisCmd;
Axis->phasing.Data = NULL;
}
if (Axis->phasing_buffered.Type != MC_NoAxisCmd) {
Axis->phasing_buffered.Type = MC_NoAxisCmd;
Axis->phasing_buffered.Data = NULL;
}
}
} else {
active_cmd->Starting = 0;
}
axis_cmd_chnl_s *next_cmd = &Axis->motion_buffered;
if (next_cmd->Lock) {
next_cmd->Lock = 0;
/* Handle motion command buffering and blending */
if (!next_cmd->Aborted) {
axis_cmd_chnl_s *target_cmd = next_cmd;
switch (target_cmd->Type) {
case MC_Home: {
__mcl_snapshot_MC_Home *s =
&target_cmd->snapshot.MC_Home;
next_cmd->BlendVelocity = 0;
}
break;
default:
break;
}
}
/* Change OTG parameters according to blending mode */
switch (Axis->BufferMode) {
case MC_BUFFER_MODE__MCABORTING:
active_cmd->Aborted = 1;
MC_LOG("Next command aborts previous")
break;
case MC_BUFFER_MODE__MCBUFFERED:
/* reset final speed to original */
Axis->OTG.Vf = active_cmd->EndVelocity;
MC_LOG("Next command buffered")
break;
case MC_BUFFER_MODE__MCBLENDINGLOW:
Axis->OTG.Vf = fmin(active_cmd->BlendVelocity,
next_cmd->BlendVelocity);
MC_LOG("Blending low")
break;
case MC_BUFFER_MODE__MCBLENDINGPREVIOUS:
Axis->OTG.Vf = active_cmd->BlendVelocity;
MC_LOG("Blending high")
break;
case MC_BUFFER_MODE__MCBLENDINGNEXT:
Axis->OTG.Vf = next_cmd->BlendVelocity;
MC_LOG("Blending previous")
break;
case MC_BUFFER_MODE__MCBLENDINGHIGH:
Axis->OTG.Vf = fmax(active_cmd->BlendVelocity,
next_cmd->BlendVelocity);
MC_LOG("Blending next")
break;
}
if (next_cmd->Completed) {
next_cmd->Completed = 0;
next_cmd->Type = MC_NoAxisCmd;
next_cmd->Data = NULL;
}
}
if (cmd_abort) {
active_cmd->Aborted = 1;
}
}
{ /*##### AXIS MOTION COMPUTE ######*/
int cmd_abort = 0;
axis_cmd_chnl_s *active_cmd = &Axis->motion;
if (!active_cmd->Aborted) {
axis_cmd_chnl_s *target_cmd = active_cmd;
switch (target_cmd->Type) {
case MC_Home: {
if ((Axis->HomingProcedures != MC_HOMING_PROCEDURES__MCHOMEDIRECT) &&
(Axis->HomingProcedures != MC_HOMING_PROCEDURES__MCHOMEABSOLUTE)) {
if (Axis->Public.HomingCtrlByPwr) {
__mcl_snapshot_MC_Home *s =
&target_cmd->snapshot.MC_Home;
mc_real ActualPosition = (Axis->Public.ActualPosition - Axis->Public.PositionOffset);
int Probe = (Axis->Public.DigitalInputs & 0x10000) != 0;
if ((Axis->Public.TouchProbeRisingTrigger == 0) && Probe) {
// log_info("TouchProbeRisingTrigger=1");
Axis->Public.TouchProbeRisingTrigger = 1;
}
int Home = (Axis->Public.DigitalInputs & 0x4) != 0;
int LimitPos = (Axis->Public.DigitalInputs & 0x2) != 0;
int LimitNeg = (Axis->Public.DigitalInputs & 0x1) != 0;
if (Axis->Public.LimitSwitchNC) {
LimitPos = !LimitPos;
LimitNeg = !LimitNeg;
}
if ((LimitPos && LimitNeg) || (LimitPos && Home) || (LimitNeg && Home)) {
active_cmd->Error = 1;
active_cmd->ErrorID = ERROR_POSITION_UNREACHABLE;
MC_LOG_CRITICAL("Limits and Home switch must be exclusive")
}
switch (Axis->HomingProcedures) {
case MC_HOMING_PROCEDURES__MCHOMEABSOLUTESWITCHPOSITIVE:
if (active_cmd->Completed) {
// log_info("Completed.s->Step = 99");
s->Step = 99;
break;
}
if (Axis->Public.HomingSignalFound && Axis->Public.TouchProbeRisingTrigger &&
!Home) {
log_info(
"HomingSignalFound &&TouchProbeRisingTrigger &&!Home.s->Step = 99 tick=%d",
millis());
s->Step = 99;
break;
}
if (!Axis->Public.TouchProbeRisingTrigger && LimitPos) {
s->Step = 98;
active_cmd->Error = 1;
active_cmd->ErrorID = ERROR_POSITION_UNREACHABLE;
MC_LOG_CRITICAL(
"Could not find Touch Probe Switch. while reach Limit Switch(CCW)")
break;
}
if (!Axis->Public.TouchProbeRisingTrigger && Home) {
s->Step = 98;
active_cmd->Error = 1;
active_cmd->ErrorID = ERROR_POSITION_UNREACHABLE;
MC_LOG_CRITICAL(
"Could not find Touch Probe Switch. while finding Homing Switch(Home)")
break;
}
if (!Axis->Public.HomingSignalFound)
s->Step = 1;
if (!Axis->Public.HomingSignalFound && Axis->Public.TouchProbeRisingTrigger) {
// log_info("!HomingSignalFound && TouchProbeRisingTrigger. step=3");
s->Step = 3;
}
if (!Axis->Public.HomingSignalFound && Axis->Public.TouchProbeRisingTrigger &&
LimitPos) {
s->Step = 98;
active_cmd->Error = 1;
active_cmd->ErrorID = ERROR_POSITION_UNREACHABLE;
MC_LOG_CRITICAL(
"Could not find Homing Switch(Home). while reach Limit Switch(CCW)")
}
if (!Axis->Public.HomingSignalFound && Axis->Public.TouchProbeRisingTrigger &&
Home) {
plc_led_err(1);
log_info(
"!HomingSignalFound && TouchProbeRisingTrigger && Home. step=5 tick=%d",
millis());
s->Step = 5;
Axis->Public.HomingSignalFound = 1;
}
break;
}
switch (s->Step) {
case 1: // Search Switch to Positive Direction
Axis->OTG.ctrl_mode = gt_Velocity;
Axis->OTG.Vf = Axis->Public.HomingVelocity;
break;
case 2: // Search Switch to Negative Direction
Axis->OTG.ctrl_mode = gt_Velocity;
Axis->OTG.Vf = -Axis->Public.HomingVelocity;
break;
case 3: // Search Zpulse to Positive Direction
Axis->OTG.ctrl_mode = gt_Velocity;
Axis->OTG.Vf = Axis->Public.HomingVelocitySearchIndexPulse;
if (Axis->HomingProcedures == MC_HOMING_PROCEDURES__MCHOMEREFERENCEPULSEPOSITIVE ||
Axis->HomingProcedures == MC_HOMING_PROCEDURES__MCHOMEREFERENCEPULSENEGATIVE) {
if ((Axis->Public.TouchProbeFunction & 0x05) != 0x05)
s->ProbeStep = 3;
else
s->ProbeStep = 4;
} else {
if ((Axis->Public.TouchProbeFunction & 0x07) != 0x07)
s->ProbeStep = 1;
else
s->ProbeStep = 2;
}
break;
case 4: // Search Zpulse to Negative Direction
Axis->OTG.ctrl_mode = gt_Velocity;
Axis->OTG.Vf = -Axis->Public.HomingVelocitySearchIndexPulse;
if (Axis->HomingProcedures == MC_HOMING_PROCEDURES__MCHOMEREFERENCEPULSEPOSITIVE ||
Axis->HomingProcedures == MC_HOMING_PROCEDURES__MCHOMEREFERENCEPULSENEGATIVE) {
if ((Axis->Public.TouchProbeFunction & 0x05) != 0x05)
s->ProbeStep = 3;
else
s->ProbeStep = 4;
} else {
if ((Axis->Public.TouchProbeFunction & 0x07) != 0x07)
s->ProbeStep = 1;
else
s->ProbeStep = 2;
}
break;
case 5: // go to Zpulse Positive Direction
Axis->OTG.ctrl_mode = gt_Position;
// Axis->OTG.Pf = Axis->Public.Axis_Zpulse;
Axis->OTG.Pf = Axis->Public.ActualPosition - Axis->Public.HomingLimitWindow;
Axis->OTG.Vf = 0.0;
Axis->Public.CommandedPosition = Axis->OTG.Pf;
Axis->OTG.Vmvt = Axis->Public.HomingVelocitySearchIndexPulse;
Axis->OTG.Gacc = Axis->Public.MaxAccelerationAppl;
Axis->OTG.Gdec = Axis->Public.MaxDecelerationAppl;
break;
case 6: // go to Zpulse Negative Direction
Axis->OTG.ctrl_mode = gt_Position;
Axis->OTG.Pf = Axis->Public.Axis_Zpulse;
Axis->OTG.Vf = 0.0;
Axis->Public.CommandedPosition = Axis->OTG.Pf;
Axis->OTG.Vmvt = -Axis->Public.HomingVelocitySearchIndexPulse;
Axis->OTG.Gacc = Axis->Public.MaxAccelerationAppl;
Axis->OTG.Gdec = Axis->Public.MaxDecelerationAppl;
break;
case 98:
Axis->OTG.ctrl_mode = gt_Velocity;
Axis->OTG.Vf = 0.0;
s->Step = 99;
break;
case 99:
Axis->OTG.ctrl_mode = gt_Velocity;
Axis->OTG.Pn=Axis->OTG.Pf;
Axis->OTG.Vf = 0.0;
break;
default:
break;
}
static int ProbeStep;
if (ProbeStep != s->ProbeStep) {
// log_info("s->ProbeStep %d -> %d Axis->Public.Axis_Zpulse=%f", ProbeStep, s->ProbeStep,Axis->Public.Axis_Zpulse);
ProbeStep = s->ProbeStep;
}
switch (s->ProbeStep) {
case 1: // Set TouchProbeFunction
if (((Axis->Public.TouchProbeFunction >> 4) & 0x03) != 0x0000) {
active_cmd->Error = 1;
active_cmd->ErrorID = ERROR_ILLEGAL_COMMAND;
Axis->Public.ErrorCode = ERROR_ILLEGAL_COMMAND;
MC_LOG("TouchProbe trigger invalid or already in use")
} else {
uint16_t oldTPF = Axis->Public.TouchProbeFunction;
Axis->Public.TouchProbeFunction = (0x01 << 4) | 0x0007;
}
break;
case 2: // Save Zpulse
if (((Axis->Public.TouchProbeFunction >> 4) & 0x01) != 0x01 ||
(Axis->Public.TouchProbeFunction & 0x0001) == 0x0000) {
/* Reset per edge sign bit */
Axis->Public.TouchProbeFunction &= ~(0x01 << 4);
/* Kill touch probe function if no mor edge expected */
if (((Axis->Public.TouchProbeFunction >> 4) & 0x0003) == 0x0000) {
Axis->Public.TouchProbeFunction = 0x0000;
MC_LOG("TouchProbe function disabled")
}
MC_LOG("TouchProbe aborted")
active_cmd->Aborted = 1;
} else {
Axis->Public.Axis_Zpulse = ((mc_real) (((int64_t) Axis->PosOverflows << 32) +
Axis->Public.TouchProbePos1PosValue) *
Axis->Public.RatioDenominator /
Axis->Public.RatioNumerator) -
Axis->Public.PositionOffset;
}
break;
case 3: // Set TouchProbeFunction
if (((Axis->Public.TouchProbeFunction >> 4) & 0x03) != 0x0000) {
active_cmd->Error = 1;
active_cmd->ErrorID = ERROR_ILLEGAL_COMMAND;
Axis->Public.ErrorCode = ERROR_ILLEGAL_COMMAND;
MC_LOG("TouchProbe trigger invalid or already in use")
} else {
uint16_t oldTPF = Axis->Public.TouchProbeFunction;
Axis->Public.TouchProbeFunction = (0x02 << 4) | 0x0005;
}
break;
case 4: // Save Zpulse
if (((Axis->Public.TouchProbeFunction >> 4) & 0x02) != 0x02 ||
(Axis->Public.TouchProbeFunction & 0x0001) == 0x0000) {
/* Reset per edge sign bit */
Axis->Public.TouchProbeFunction &= ~(0x02 << 4);
/* Kill touch probe function if no mor edge expected */
if (((Axis->Public.TouchProbeFunction >> 4) & 0x0003) == 0x0000) {
Axis->Public.TouchProbeFunction = 0x0000;
MC_LOG("TouchProbe function disabled")
}
MC_LOG("TouchProbe aborted")
active_cmd->Aborted = 1;
} else {
Axis->Public.Axis_Zpulse = ((mc_real) (((int64_t) Axis->PosOverflows << 32) +
Axis->Public.TouchProbePos1NegValue) *
Axis->Public.RatioDenominator /
Axis->Public.RatioNumerator) -
Axis->Public.PositionOffset;
}
break;
case 99:
break;
default:
break;
}
s->Home0 = Home;
GenTraj(&Axis->OTG); /* Computes OTG */
// static int Completed, state;
// if (active_cmd->Completed != Completed || state != Axis->OTG.state) {
// int ctrl_mode=Axis->OTG.ctrl_mode;
// log_info("Completed %d->%d ,OTG.state %d->%d ,ctrl_mode=%d Vf=%f ,Axis_Zpulse=%f", Completed, active_cmd->Completed, state,
// Axis->OTG.state,ctrl_mode,Axis->OTG.Vf,Axis->Public.Axis_Zpulse);
// Completed = active_cmd->Completed;
// state = Axis->OTG.state;
// }
if (s->Step == 98) {
active_cmd->Completed = 1;
}
if (s->Step == 99) {
if (!active_cmd->Completed && (Axis->OTG.state & gt_Reached)) {
plc_led_err(0);
log_info("Completed [MC_Home] tick=%d", millis());
Axis->Public.HomingCompleted = 1;
}
}
if (Axis->Public.HomingCompleted) {
active_cmd->Completed = 1;
mc_real PosOffset = s->Position - Axis->Public.Axis_Zpulse;
Axis->Public.PositionOffset -= PosOffset;
Axis->OTG.Pn += PosOffset;
Axis->OTG.Pf += PosOffset;
Axis->Public.TouchProbeFunction = 0x0000;
Axis->Public.HomingCtrlByPwr = 0;
}
if (active_cmd->Completed) {
/* Propagate completion status to FB directly */
if (target_cmd->Error) {
__SET_VAR(((MC_HOME *) (target_cmd->Data))->, ERROR, , 1);
__SET_VAR(((MC_HOME *) (target_cmd->Data))->, ERRORID, , target_cmd->ErrorID);
} else {
__SET_VAR(((MC_HOME *) (target_cmd->Data))->, DONE, , 1);
}
}
}
} else {
if (Axis->Public.HomingCompleted) {
active_cmd->Completed = 1;
if (Axis->HomingProcedures == MC_HOMING_PROCEDURES__MCHOMEABSOLUTE) {
Axis->Public.HomingOperationStart = 0;
Axis->State = mc_state_Standstill;
Axis->OTG.ctrl_mode = gt_Off;
}
// __mcl_snapshot_MC_Home *s=
// &target_cmd->snapshot.MC_Home;
// Axis->Public.PositionOffset -= s->Position;
// Axis->OTG.Pn += s->Position;
// Axis->OTG.Pf += s->Position;
}
if (active_cmd->Completed) {
/* Propagate completion status to FB directly */
if (target_cmd->Error) {
__SET_VAR(((MC_HOME *) (target_cmd->Data))->, ERROR, , 1);
__SET_VAR(((MC_HOME *) (target_cmd->Data))->, ERRORID, , target_cmd->ErrorID);
} else {
__SET_VAR(((MC_HOME *) (target_cmd->Data))->, DONE, , 1);
}
}
}
}
break;
case MC_NoAxisCmd:
GenTraj(&Axis->OTG); /* Computes OTG */
break;
default:
break;
}
}
if (cmd_abort) {
active_cmd->Aborted = 1;
}
}
{ /*##### AXIS SUPERIMPOSED EXEC ######*/
int cmd_abort = 0;
axis_cmd_chnl_s *active_cmd = &Axis->superimposed;
if (active_cmd->Lock) {
active_cmd->Starting = 1;
active_cmd->Lock = 0;
if (!active_cmd->Aborted) {
axis_cmd_chnl_s *target_cmd = active_cmd;
switch (target_cmd->Type) {
case MC_MoveSuperimposed: {
__mcl_snapshot_MC_MoveSuperimposed *s =
&target_cmd->snapshot.MC_MoveSuperimposed;
if (Axis->State == mc_state_Standstill)
Axis->State = mc_state_DiscreteMotion;
Axis->SOTG.Vf = 0;
Axis->SOTG.ctrl_mode = gt_Position;
Axis->SOTG.Pn = 0;
Axis->SOTG.Pf = s->Distance;
Axis->SOTG.Vmvt = s->VelocityDiff;
if (s->Acceleration != 0)
Axis->SOTG.Gacc = s->Acceleration;
else {
MC_LOG_WARNING("undefined Acceleration. defaults value is MaxAccelerationAppl")
Axis->SOTG.Gacc = Axis->Public.MaxAccelerationAppl;
}
if (s->Deceleration != 0)
Axis->SOTG.Gdec = s->Deceleration;
else {
MC_LOG_WARNING("undefined deceleration. defaults value is MaxDecelerationAppl")
Axis->SOTG.Gdec = Axis->Public.MaxDecelerationAppl;
}
Axis->SOTG.J = s->Jerk;
}
break;
case MC_HaltSuperimposed: {
__mcl_snapshot_MC_HaltSuperimposed *s =
&target_cmd->snapshot.MC_HaltSuperimposed;
/* That block does not change axis state */
Axis->SOTG.Vf = 0;
Axis->SOTG.ctrl_mode = gt_Velocity;
if (s->Deceleration != 0)
Axis->SOTG.Gdec = s->Deceleration;
else {
MC_LOG_WARNING("undefined deceleration. defaults value is MaxDecelerationAppl")
Axis->SOTG.Gdec = Axis->Public.MaxDecelerationAppl;
}
if (s->Deceleration != 0)
Axis->SOTG.Gacc = s->Deceleration;
else {
MC_LOG_WARNING("undefined deceleration. defaults value is MaxDecelerationAppl")
Axis->SOTG.Gacc = Axis->Public.MaxDecelerationAppl;
}
Axis->SOTG.J = s->Jerk;
}
break;
default:
break;
}
}
} else {
active_cmd->Starting = 0;
}
if (cmd_abort) {
active_cmd->Aborted = 1;
}
}
{ /*##### AXIS SUPERIMPOSED COMPUTE ######*/
int cmd_abort = 0;
axis_cmd_chnl_s *active_cmd = &Axis->superimposed;
if (!active_cmd->Aborted) {
axis_cmd_chnl_s *target_cmd = active_cmd;
switch (target_cmd->Type) {
case MC_MoveSuperimposed: {
__mcl_snapshot_MC_MoveSuperimposed *s =
&target_cmd->snapshot.MC_MoveSuperimposed;
GenTraj(&Axis->SOTG);
/* SOTG alone detects completion */
if (!active_cmd->Completed && Axis->SOTG.state & gt_Reached) {
MC_LOG("Completed [MC_MoveSuperimposed]")
active_cmd->Completed = 1;
}
if (active_cmd->Completed) {
/* Propagate completion status to FB directly */
if (target_cmd->Error) {
__SET_VAR(((MC_MOVESUPERIMPOSED *) (target_cmd->Data))->, ERROR, , 1);
__SET_VAR(((MC_MOVESUPERIMPOSED *) (target_cmd->Data))->, ERRORID, , target_cmd->ErrorID);
Axis->Public.ErrorCode = target_cmd->ErrorID;
} else {
__SET_VAR(((MC_MOVESUPERIMPOSED *) (target_cmd->Data))->, DONE, , 1);
}
}
}
break;
case MC_HaltSuperimposed: {
__mcl_snapshot_MC_HaltSuperimposed *s =
&target_cmd->snapshot.MC_HaltSuperimposed;
GenTraj(&Axis->SOTG);
/* SOTG alone detects completion */
if (!active_cmd->Completed && Axis->SOTG.state & gt_Reached) {
MC_LOG("Completed [MC_HaltSuperimposed]")
active_cmd->Completed = 1;
}
if (active_cmd->Completed) {
/* Propagate completion status to FB directly */
if (target_cmd->Error) {
__SET_VAR(((MC_HALTSUPERIMPOSED *) (target_cmd->Data))->, ERROR, , 1);
__SET_VAR(((MC_HALTSUPERIMPOSED *) (target_cmd->Data))->, ERRORID, , target_cmd->ErrorID);
Axis->Public.ErrorCode = target_cmd->ErrorID;
} else {
__SET_VAR(((MC_HALTSUPERIMPOSED *) (target_cmd->Data))->, DONE, , 1);
}
}
}
break;
case MC_NoAxisCmd:
if (Axis->SOTG.ctrl_mode != gt_Off) {
/*Feed axis OTG with current superimposed position */
Axis->OTG.Pn += Axis->SOTG.Pn;
Axis->OTG.Pf += Axis->SOTG.Pn;
Axis->SOTG.Pn = 0;
Axis->OTG.Vn += Axis->SOTG.Vn;
Axis->SOTG.Vn = 0;
Axis->SOTG.ctrl_mode = gt_Off;
}
break;
default:
break;
}
}
if (cmd_abort) {
active_cmd->Aborted = 1;
}
if (active_cmd->Completed) {
active_cmd->Completed = 0;
active_cmd->Type = MC_NoAxisCmd;
active_cmd->Data = NULL;
}
}
{ /*##### AXIS PHASING EXEC ######*/
int cmd_abort = 0;
axis_cmd_chnl_s *active_cmd = &Axis->phasing;
if (active_cmd->Lock) {
active_cmd->Starting = 1;
active_cmd->Lock = 0;
if (!active_cmd->Aborted) {
axis_cmd_chnl_s *target_cmd = active_cmd;
switch (target_cmd->Type) {
case MC_PhasingAbsolute: {
__mcl_snapshot_MC_PhasingAbsolute *s =
&target_cmd->snapshot.MC_PhasingAbsolute;
/* That block does not change axis state */
if (Axis->motion.Type == MC_CamIn) {
if (Axis->motion.snapshot.MC_CamIn.Master != s->Master) {
active_cmd->Error = 1;
active_cmd->ErrorID = ERROR_ILLEGAL_COMMAND;
Axis->Public.ErrorCode = ERROR_ILLEGAL_COMMAND;
break;
}
} else if (Axis->motion.Type == MC_GearIn) {
if (Axis->motion.snapshot.MC_GearIn.Master != s->Master) {
active_cmd->Error = 1;
active_cmd->ErrorID = ERROR_ILLEGAL_COMMAND;
Axis->Public.ErrorCode = ERROR_ILLEGAL_COMMAND;
break;
}
} else if (Axis->motion.Type == MC_GearInPos) {
if (Axis->motion.snapshot.MC_GearInPos.Master != s->Master) {
active_cmd->Error = 1;
active_cmd->ErrorID = ERROR_ILLEGAL_COMMAND;
Axis->Public.ErrorCode = ERROR_ILLEGAL_COMMAND;
break;
}
} else {
active_cmd->Error = 1;
active_cmd->ErrorID = ERROR_ILLEGAL_COMMAND;
Axis->Public.ErrorCode = ERROR_ILLEGAL_COMMAND;
break;
}
Axis->POTG.Pf = s->PhaseShift;
Axis->POTG.Vf = 0;
Axis->POTG.Vmvt = s->Velocity;
if (s->Acceleration != 0)
Axis->POTG.Gacc = s->Acceleration;
else {
MC_LOG_WARNING("undefined Acceleration. defaults value is MaxAccelerationAppl")
Axis->POTG.Gacc = Axis->Public.MaxAccelerationAppl;
}
if (s->Deceleration != 0)
Axis->POTG.Gdec = s->Deceleration;
else {
MC_LOG_WARNING("undefined deceleration. defaults value is MaxDecelerationAppl")
Axis->POTG.Gdec = Axis->Public.MaxDecelerationAppl;
}
Axis->POTG.J = s->Jerk;
Axis->POTG.ctrl_mode = gt_Position;
}
break;
case MC_PhasingRelative: {
__mcl_snapshot_MC_PhasingRelative *s =
&target_cmd->snapshot.MC_PhasingRelative;
/* That block does not change axis state */
if (Axis->motion.Type == MC_CamIn) {
if (Axis->motion.snapshot.MC_CamIn.Master != s->Master) {
active_cmd->Error = 1;
active_cmd->ErrorID = ERROR_ILLEGAL_COMMAND;
Axis->Public.ErrorCode = ERROR_ILLEGAL_COMMAND;
break;
}
} else if (Axis->motion.Type == MC_GearIn) {
if (Axis->motion.snapshot.MC_GearIn.Master != s->Master) {
active_cmd->Error = 1;
active_cmd->ErrorID = ERROR_ILLEGAL_COMMAND;
Axis->Public.ErrorCode = ERROR_ILLEGAL_COMMAND;
break;
}
} else if (Axis->motion.Type == MC_GearInPos) {
if (Axis->motion.snapshot.MC_GearInPos.Master != s->Master) {
active_cmd->Error = 1;
active_cmd->ErrorID = ERROR_ILLEGAL_COMMAND;
Axis->Public.ErrorCode = ERROR_ILLEGAL_COMMAND;
break;
}
} else {
active_cmd->Error = 1;
active_cmd->ErrorID = ERROR_ILLEGAL_COMMAND;
Axis->Public.ErrorCode = ERROR_ILLEGAL_COMMAND;
break;
}
Axis->POTG.Pf = s->ExecutePhaseShift + s->PhaseShift;
Axis->POTG.Vf = 0;
Axis->POTG.Vmvt = s->Velocity;
if (s->Acceleration != 0)
Axis->POTG.Gacc = s->Acceleration;
else {
MC_LOG_WARNING("undefined Acceleration. defaults value is MaxAccelerationAppl")
Axis->POTG.Gacc = Axis->Public.MaxAccelerationAppl;
}
if (s->Deceleration != 0)
Axis->POTG.Gdec = s->Deceleration;
else {
MC_LOG_WARNING("undefined deceleration. defaults value is MaxDecelerationAppl")
Axis->POTG.Gdec = Axis->Public.MaxDecelerationAppl;
}
Axis->POTG.J = s->Jerk;
Axis->POTG.ctrl_mode = gt_Position;
}
break;
default:
break;
}
}
} else {
active_cmd->Starting = 0;
}
axis_cmd_chnl_s *next_cmd = &Axis->phasing_buffered;
if (next_cmd->Lock) {
next_cmd->Lock = 0;
}
if (cmd_abort) {
active_cmd->Aborted = 1;
}
}
{ /*##### AXIS PHASING COMPUTE ######*/
int cmd_abort = 0;
axis_cmd_chnl_s *active_cmd = &Axis->phasing;
if (!active_cmd->Aborted) {
axis_cmd_chnl_s *target_cmd = active_cmd;
switch (target_cmd->Type) {
case MC_PhasingAbsolute: {
__mcl_snapshot_MC_PhasingAbsolute *s =
&target_cmd->snapshot.MC_PhasingAbsolute;
GenTraj(&Axis->POTG);
__SET_VAR(((MC_PHASINGABSOLUTE *) (active_cmd->Data))->,
ABSOLUTEPHASESHIFT, , Axis->POTG.Pn);
/* POTG alone detects completion */
if (!active_cmd->Completed && Axis->POTG.state & gt_Reached) {
MC_LOG("Completed [MC_PhasingAbsolute]")
active_cmd->Completed = 1;
}
if (active_cmd->Completed) {
/* Propagate completion status to FB directly */
if (target_cmd->Error) {
__SET_VAR(((MC_PHASINGABSOLUTE *) (target_cmd->Data))->, ERROR, , 1);
__SET_VAR(((MC_PHASINGABSOLUTE *) (target_cmd->Data))->, ERRORID, , target_cmd->ErrorID);
Axis->Public.ErrorCode = target_cmd->ErrorID;
} else {
__SET_VAR(((MC_PHASINGABSOLUTE *) (target_cmd->Data))->, DONE, , 1);
}
}
}
break;
case MC_PhasingRelative: {
__mcl_snapshot_MC_PhasingRelative *s =
&target_cmd->snapshot.MC_PhasingRelative;
GenTraj(&Axis->POTG);
__SET_VAR(((MC_PHASINGRELATIVE *) (active_cmd->Data))->,
COVEREDPHASESHIFT, , Axis->POTG.Pn - s->ExecutePhaseShift);
/* POTG alone detects completion */
if (!active_cmd->Completed && Axis->POTG.state & gt_Reached) {
MC_LOG("Completed [MC_PhasingRelative]")
active_cmd->Completed = 1;
}
if (active_cmd->Completed) {
/* Propagate completion status to FB directly */
if (target_cmd->Error) {
__SET_VAR(((MC_PHASINGRELATIVE *) (target_cmd->Data))->, ERROR, , 1);
__SET_VAR(((MC_PHASINGRELATIVE *) (target_cmd->Data))->, ERRORID, , target_cmd->ErrorID);
Axis->Public.ErrorCode = target_cmd->ErrorID;
} else {
__SET_VAR(((MC_PHASINGRELATIVE *) (target_cmd->Data))->, DONE, , 1);
}
}
}
break;
default:
break;
}
}
if (cmd_abort) {
active_cmd->Aborted = 1;
}
if (active_cmd->Completed) {
active_cmd->Completed = 0;
active_cmd->Type = MC_NoAxisCmd;
active_cmd->Data = NULL;
}
}
{ /*################## AXIS SEQUENCING ##################*/
axis_cmd_chnl_s *active_cmd = &Axis->motion;
axis_cmd_chnl_s *next_cmd = &Axis->motion_buffered;
if (active_cmd->Completed | active_cmd->Aborted) {
/* Promote following command Function Block
* as current according to buffer/blend mode */
if (next_cmd->Type != MC_NoAxisCmd) {
*active_cmd = *next_cmd;
active_cmd->Lock = 1;
next_cmd->Type = MC_NoAxisCmd;
next_cmd->Data = NULL;
} else {
active_cmd->Completed = 0;
active_cmd->Type = MC_NoAxisCmd;
active_cmd->Data = NULL;
/* State transition that happen on "Done" */
if (Axis->State == mc_state_DiscreteMotion ||
Axis->State == mc_state_Stopping ||
Axis->State == mc_state_Homing) {
Axis->State = mc_state_Standstill;
MC_LOG("State: Standstill [Done]")
}
}
/* This is not an abort */
active_cmd->PrevType = MC_NoAxisCmd;
/* Promote next buffered phasing command */
if (Axis->phasing_buffered.Type != MC_NoAxisCmd) {
Axis->phasing = Axis->phasing_buffered;
/* This is not an abort */
Axis->phasing.PrevType = MC_NoAxisCmd;
Axis->phasing_buffered.Type = MC_NoAxisCmd;
Axis->phasing_buffered.Data = NULL;
Axis->phasing.Lock = 1;
}
/* Execute queued admin requests */
if (Axis->MC_SetPosition_data) {
__mcl_snapshot_MC_SetPosition *s = &Axis->MC_SetPosition;
IEC_BOOL Done = 1;
IEC_BOOL Error = 0;
IEC_WORD ErrorID = 0;
{
mc_real PosOffset = s->Position - Axis->OTG.Pn;
Axis->Public.PositionOffset -= PosOffset;
Axis->OTG.Pn += PosOffset;
Axis->OTG.Pf += PosOffset;
Axis->Public.SWLimitNeg += PosOffset;
Axis->Public.SWLimitPos += PosOffset;
}
/* Propagate completion and error to FB directly */
__SET_VAR(
(Axis->MC_SetPosition_data)->,
DONE, , Done);
if (Error) {
__SET_VAR((Axis->MC_SetPosition_data)->, ERROR, , Error);
__SET_VAR((Axis->MC_SetPosition_data)->,
ERRORID, , ErrorID);
Axis->Public.ErrorCode = ErrorID;
}
Axis->MC_SetPosition_data = NULL;
}
if (Axis->MC_WriteParameter_data) {
__mcl_snapshot_MC_WriteParameter *s = &Axis->MC_WriteParameter;
IEC_BOOL Done = 1;
IEC_BOOL Error = 0;
IEC_WORD ErrorID = 0;
switch (s->ParameterNumber) {
case 2:
if (Axis->Public.SWLimitPos != s->Value) {
MC_LOG("SWLimitPos set to %f", s->Value)
}
Axis->Public.SWLimitPos = s->Value;
break;
case 3:
if (Axis->Public.SWLimitNeg != s->Value) {
MC_LOG("SWLimitNeg set to %f", s->Value)
}
Axis->Public.SWLimitNeg = s->Value;
break;
case 7:
if (Axis->Public.MaxPositionLag != s->Value) {
MC_LOG("MaxPositionLag set to %f", s->Value)
}
Axis->Public.MaxPositionLag = s->Value;
break;
case 9:
if (Axis->Public.MaxVelocityAppl != s->Value) {
MC_LOG("MaxVelocityAppl set to %f", s->Value)
}
Axis->Public.MaxVelocityAppl = s->Value;
break;
case 13:
if (Axis->Public.MaxAccelerationAppl != s->Value) {
MC_LOG("MaxAccelerationAppl set to %f", s->Value)
}
Axis->Public.MaxAccelerationAppl = s->Value;
break;
case 15:
if (Axis->Public.MaxDecelerationAppl != s->Value) {
MC_LOG("MaxDecelerationAppl set to %f", s->Value)
}
Axis->Public.MaxDecelerationAppl = s->Value;
break;
case 17:
if (Axis->Public.MaxJerkAppl != s->Value) {
MC_LOG("MaxJerkAppl set to %f", s->Value)
}
Axis->Public.MaxJerkAppl = s->Value;
break;
case 1003:
if (Axis->Public.RatioNumerator != s->Value) {
MC_LOG("RatioNumerator set to %f", s->Value)
}
Axis->Public.RatioNumerator = s->Value;
break;
case 1004:
if (Axis->Public.RatioDenominator != s->Value) {
MC_LOG("RatioDenominator set to %f", s->Value)
}
Axis->Public.RatioDenominator = s->Value;
break;
case 1005:
if (Axis->Public.PositionOffset != s->Value) {
MC_LOG("PositionOffset set to %f", s->Value)
}
Axis->Public.PositionOffset = s->Value;
break;
case 1007:
if (Axis->Public.JerkSetPoint != s->Value) {
MC_LOG("JerkSetPoint set to %f", s->Value)
}
Axis->Public.JerkSetPoint = s->Value;
break;
case 1009:
if (Axis->Public.HomingLimitWindow != s->Value) {
MC_LOG("HomingLimitWindow set to %f", s->Value)
}
Axis->Public.HomingLimitWindow = s->Value;
break;
case 1010:
if (Axis->Public.HomingVelocity != s->Value) {
MC_LOG("HomingVelocity set to %f", s->Value)
}
Axis->Public.HomingVelocity = s->Value;
break;
case 1013:
if (Axis->Public.TorqueRatioNumerator != s->Value) {
MC_LOG("TorqueRatioNumerator set to %f", s->Value)
}
Axis->Public.TorqueRatioNumerator = s->Value;
break;
case 1014:
if (Axis->Public.TorqueRatioDenominator != s->Value) {
MC_LOG("TorqueRatioDenominator set to %f", s->Value)
}
Axis->Public.TorqueRatioDenominator = s->Value;
break;
case 1016:
if (Axis->Public.ModuloAxisRange != s->Value) {
MC_LOG("ModuloAxisRange set to %f", s->Value)
}
Axis->Public.ModuloAxisRange = s->Value;
break;
case 1018:
if (Axis->Public.HomingVelocitySearchIndexPulse != s->Value) {
MC_LOG("HomingVelocity_Search_Index_Pulse set to %f", s->Value)
}
Axis->Public.HomingVelocitySearchIndexPulse = s->Value;
break;
default:
Error = 1;
ErrorID = ERROR_ILLEGAL_COMMAND;
Axis->Public.ErrorCode = ERROR_ILLEGAL_COMMAND;
}
/* Propagate completion and error to FB directly */
__SET_VAR(
(Axis->MC_WriteParameter_data)->,
DONE, , Done);
if (Error) {
__SET_VAR((Axis->MC_WriteParameter_data)->, ERROR, , Error);
__SET_VAR((Axis->MC_WriteParameter_data)->,
ERRORID, , ErrorID);
Axis->Public.ErrorCode = ErrorID;
}
Axis->MC_WriteParameter_data = NULL;
}
if (Axis->MC_WriteBoolParameter_data) {
__mcl_snapshot_MC_WriteBoolParameter *s = &Axis->MC_WriteBoolParameter;
IEC_BOOL Done = 1;
IEC_BOOL Error = 0;
IEC_WORD ErrorID = 0;
switch (s->ParameterNumber) {
case 4:
if (Axis->Public.EnableLimitPos != s->Value) {
MC_LOG("EnableLimitPos set to %hu", s->Value)
}
Axis->Public.EnableLimitPos = s->Value;
break;
case 5:
if (Axis->Public.EnableLimitNeg != s->Value) {
MC_LOG("EnableLimitNeg set to %hu", s->Value)
}
Axis->Public.EnableLimitNeg = s->Value;
break;
case 6:
if (Axis->Public.EnablePosLagMonitoring != s->Value) {
MC_LOG("EnablePosLagMonitoring set to %hu", s->Value)
}
Axis->Public.EnablePosLagMonitoring = s->Value;
break;
case 1000:
if (Axis->Public.Simulation != s->Value) {
MC_LOG("Simulation set to %hu", s->Value)
}
Axis->Public.Simulation = s->Value;
break;
case 1006:
if (Axis->Public.LimitSwitchNC != s->Value) {
MC_LOG("LimitSwitchNC set to %hu", s->Value)
}
Axis->Public.LimitSwitchNC = s->Value;
break;
default:
Error = 1;
ErrorID = ERROR_ILLEGAL_COMMAND;
Axis->Public.ErrorCode = ERROR_ILLEGAL_COMMAND;
}
/* Propagate completion and error to FB directly */
__SET_VAR(
(Axis->MC_WriteBoolParameter_data)->,
DONE, , Done);
if (Error) {
__SET_VAR((Axis->MC_WriteBoolParameter_data)->, ERROR, , Error);
__SET_VAR((Axis->MC_WriteBoolParameter_data)->,
ERRORID, , ErrorID);
Axis->Public.ErrorCode = ErrorID;
}
Axis->MC_WriteBoolParameter_data = NULL;
}
if (Axis->MC_WriteDigitalOutput_data) {
__mcl_snapshot_MC_WriteDigitalOutput *s = &Axis->MC_WriteDigitalOutput;
IEC_BOOL Done = 1;
IEC_BOOL Error = 0;
IEC_WORD ErrorID = 0;
/* Not implemented (AdminExec)*/
Error = 1;
ErrorID = ERROR_NOT_IMPLEMENTED;
Axis->Public.ErrorCode = ERROR_NOT_IMPLEMENTED;
/* Propagate completion and error to FB directly */
__SET_VAR(
(Axis->MC_WriteDigitalOutput_data)->,
DONE, , Done);
if (Error) {
__SET_VAR((Axis->MC_WriteDigitalOutput_data)->, ERROR, , Error);
__SET_VAR((Axis->MC_WriteDigitalOutput_data)->,
ERRORID, , ErrorID);
Axis->Public.ErrorCode = ErrorID;
}
Axis->MC_WriteDigitalOutput_data = NULL;
}
if (Axis->MC_CamTableSelect_data) {
__mcl_snapshot_MC_CamTableSelect *s = &Axis->MC_CamTableSelect;
IEC_BOOL Done = 1;
IEC_BOOL Error = 0;
IEC_WORD ErrorID = 0;
MC_CAM_ID_s *_CamTableID = &MC_CAM_ID_inst[s->CamTableID];
_CamTableID->MC_CamTableSelect.Master = s->Master; /*AXIS_REF*/
_CamTableID->MC_CamTableSelect.CamTable = s->CamTable; /*MC_CAM_REF*/
_CamTableID->MC_CamTableSelect.Periodic = s->Periodic; /*BOOL*/
_CamTableID->MC_CamTableSelect.MasterAbsolute = s->MasterAbsolute; /*BOOL*/
_CamTableID->MC_CamTableSelect.SlaveAbsolute = s->SlaveAbsolute; /*BOOL*/
_CamTableID->MC_CamTableSelect.ExecutionMode = s->ExecutionMode; /*MC_EXECUTION_MODE*/
_CamTableID->MC_CamTableSelect.CamTableID = s->CamTableID; /*MC_CAM_ID*/
/* Propagate completion and error to FB directly */
__SET_VAR(
(Axis->MC_CamTableSelect_data)->,
DONE, , Done);
if (Error) {
__SET_VAR((Axis->MC_CamTableSelect_data)->, ERROR, , Error);
__SET_VAR((Axis->MC_CamTableSelect_data)->,
ERRORID, , ErrorID);
Axis->Public.ErrorCode = ErrorID;
}
Axis->MC_CamTableSelect_data = NULL;
}
}
}
/* Update axis Public member according to internals */
if (Axis->State == mc_state_Disabled ||
Axis->State == mc_state_ErrorStop) {
Axis->Public.PositionSetPoint = Axis->Public.ActualPosition;
Axis->OTG.Pn = (Axis->Public.ActualPosition - Axis->Public.PositionOffset);
Axis->Public.VelocitySetPoint = Axis->Public.ActualVelocity;
Axis->OTG.Vn = Axis->Public.ActualVelocity;
Axis->Public.TorqueSetPoint = Axis->Public.ActualTorque;
Axis->OTG.Tn = Axis->Public.ActualTorque;
Axis->OTG.ctrl_mode = gt_Off;
if (Axis->State == mc_state_Disabled) {
if (Axis->OTG.ctrl_mode == gt_Homing) {
Axis->Public.AxisMotionMode = mc_mode_hm;
}
}
} else {
if (Axis->State == mc_state_Homing) {
if (Axis->Public.HomingCompleted) {
Axis->motion.Completed = 1;
Axis->Public.HomingOperationStart = 0;
Axis->State = mc_state_Standstill;
Axis->OTG.ctrl_mode = gt_Off;
}
}
switch (Axis->OTG.ctrl_mode) {
/* Continuous Synchronous Torque mode */
case gt_Torque:
Axis->Public.AxisMotionMode = mc_mode_cst;
Axis->Public.PositionSetPoint = Axis->Public.ActualPosition;
Axis->OTG.Pn = (Axis->Public.ActualPosition - Axis->Public.PositionOffset);
Axis->Public.VelocitySetPoint = Axis->Public.ActualVelocity;
Axis->OTG.Vn = Axis->Public.ActualVelocity;
Axis->Public.TorqueSetPoint = Axis->OTG.Tn;
break;
/* Homing Mode */
case gt_Homing:
Axis->Public.AxisMotionMode = mc_mode_hm;
Axis->Public.PositionSetPoint = Axis->Public.ActualPosition;
Axis->OTG.Pn = (Axis->Public.ActualPosition - Axis->Public.PositionOffset);
Axis->Public.VelocitySetPoint = Axis->Public.ActualVelocity;
Axis->OTG.Vn = Axis->Public.ActualVelocity;
Axis->Public.TorqueSetPoint = Axis->Public.ActualTorque;
Axis->OTG.Tn = Axis->Public.ActualTorque;
break;
/* Continuous Synchronous Positioning mode */
default:
Axis->Public.AxisMotionMode = mc_mode_csp;
Axis->Public.PositionSetPoint =
Axis->Public.PositionOffset + Axis->OTG.Pn;
if (Axis->SOTG.ctrl_mode != gt_Off) {
Axis->Public.PositionSetPoint += Axis->SOTG.Pn;
}
Axis->Public.VelocitySetPoint = Axis->OTG.Vn;
if (Axis->SOTG.ctrl_mode != gt_Off) {
Axis->Public.VelocitySetPoint += Axis->SOTG.Vn;
}
Axis->Public.TorqueSetPoint = Axis->Public.ActualTorque;
Axis->OTG.Tn = Axis->Public.ActualTorque;
break;
}
}
Axis->Public.JerkSetPoint = Axis->OTG.Jn;
Axis->Public.AccelerationSetPoint = Axis->OTG.Gn;
if (!Axis->Public.Simulation) {
Axis->Public.RawPositionSetPoint =
(int64_t) (Axis->Public.PositionSetPoint * Axis->Public.RatioNumerator / Axis->Public.RatioDenominator);
Axis->Public.RawVelocitySetPoint =
(int64_t) (Axis->Public.VelocitySetPoint * Axis->Public.RatioNumerator / Axis->Public.RatioDenominator);
Axis->Public.RawTorqueSetPoint =
(int64_t) (Axis->Public.TorqueSetPoint * Axis->Public.TorqueRatioNumerator /
Axis->Public.TorqueRatioDenominator);
}
}