Open topic with navigation

//Steps of homing routine
CASE StepCounter OF

0://Wait for user to press home button, make sure that drives are enabled
ON bHome and CloseLoop DO
   Homed := FALSE;
   StepCounter :=1;
END_DO;

1://Move linear axis so two rotary axis are not engaged
Inst_MC_MoveAbsolute( FALSE, LinearZAxis, LinearClearedPosition, 500, 10000, 10000, 0, 0, 0 );
Inst_MC_MoveAbsolute( TRUE, LinearZAxis, LinearClearedPosition, 500, 10000, 10000, 0, 0, 0 );
StepCounter := 2;

2://Wait until linear axis move is complete, then move rotary axis to home positions
Inst_MC_MoveAbsolute( TRUE, LinearZAxis, LinearClearedPosition, 500, 10000, 10000, 0, 0, 0 );


IF Inst_MC_MoveAbsolute.Done THEN
   Inst_MC_MoveAbsolute1( FALSE, UpperRotary, 304.482, 800, 10000, 10000, 0, 2, 0 );
   Inst_MC_MoveAbsolute2( FALSE, LowerRotary, 245.850, 800, 10000, 10000, 0, 2, 0 );
   Inst_MC_MoveAbsolute1( TRUE, UpperRotary, 304.482, 800, 10000, 10000, 0, 2, 0 );
   Inst_MC_MoveAbsolute2( TRUE, LowerRotary, 245.850, 800, 10000, 10000, 0, 2, 0 );
   StepCounter := 3;
END_IF;

3://Wait for rotary axis to move to home positions, then lower linear axis to starting lower position
Inst_MC_MoveAbsolute1( TRUE, UpperRotary, 304.482, 800, 10000, 10000, 0, 2, 0 );
Inst_MC_MoveAbsolute2( TRUE, LowerRotary, 245.850, 800, 10000, 10000, 0, 2, 0 );
IF Inst_MC_MoveAbsolute1.Done AND Inst_MC_MoveAbsolute2.Done THEN
   Inst_MC_MoveAbsolute( FALSE, LinearZAxis, LinearStartingPos, 500, 10000, 10000, 0, 0, 0 );
   Inst_MC_MoveAbsolute( TRUE, LinearZAxis, LinearStartingPos, 500, 10000, 10000, 0, 0, 0 );
   StepCounter := 4;
END_IF;

4://Wait for linear move to complete, then homing routine is complete
Inst_MC_MoveAbsolute( TRUE, LinearZAxis, LinearStartingPos, 500, 10000, 10000, 0, 0, 0 );
IF Inst_MC_MoveAbsolute.Done THEN
   Homed := TRUE;
   StepCounter :=0;
END_IF;
END_CASE;


//ResetNew start of the microprocessor rotary positions to zero
ON SetRotaryZero DO
   MC_SetPosition( UpperRotary, 0, 0 );
   MC_SetPosition( LowerRotary, 0, 0 );
END_DO;

//Check how far off the two rotary axis are away from homed, lined up state
Comparison := absl(ActPosUpper - (360 - ActPosLower));
IF Comparison > 180 THEN
   Comparison := 360 - Comparison;
END_IF;
IF Comparison > MaxDifference THEN
   MaxDifference := Comparison;
END_IF;

Stay Connected with Kollmorgen

Copyright © 2015 Kollmorgen™

Open topic with navigation