#include "KMotionDef.h"

// Configuration and Homing program for a 3 axis System
// Limit switches are disabled and used as a home switch
// then they are re-enabled


main()
{
	int SaveXLimits,SaveYLimits,SaveZLimits;  //place to save limit switch settings
	
	DisableAxis(1);  // Disable all axes
	DisableAxis(2);
	DisableAxis(3);
    DisableAxis(4);
	DisableAxis(5);



    // Set the axis parameters here
	// after everything is configured in the KMotion Screens
	// use copy C Code to clipboard on the configuration screen
	// then paste here.  Repeat for each axis
	
	
	// disable the limits (first save how they were set) 
	SaveXLimits = ch1->LimitSwitchOptions;
	SaveYLimits = ch2->LimitSwitchOptions;
	SaveZLimits = ch3->LimitSwitchOptions;
	SaveYLimits = ch4->LimitSwitchOptions;
	SaveZLimits = ch5->LimitSwitchOptions;

    ch1->LimitSwitchOptions = 0;
	ch2->LimitSwitchOptions = 0;
	ch3->LimitSwitchOptions = 0;
	ch4->LimitSwitchOptions = 0;
	ch5->LimitSwitchOptions = 0;
	

    // enable all 5 axes and begin servoing where we are 
	
	EnableAxis(1);
	EnableAxis(2);
	EnableAxis(3);
    EnableAxis(4);
	EnableAxis(5);
 
	

    // Home Z up first - jog until it sees the limit

    Jog(3,25000);             // jog slowly positive
    while (!ReadBit(20)) ;  // loop until IO bit goes high
    Delay_sec(0.3);
    Jog(3,0);				// stop
    while (!CheckDone(3)) ; // loop until motion completes 
	DisableAxis(3);			// disable the axis
    EnableAxis(3);			// re-enable the ServoTick
	MoveAtVel(3,-20000.0,20000);		// move some amount inside the limits
    while (!CheckDone(3)) ; // loop until motion completes 
	Zero(3);				// Zero the position
    ch3->LimitSwitchOptions = SaveZLimits;  // restore limit settings


	// Home X next - jog until it sees the limit

    Jog(1,-25000);  			// jog slowly negative
    while (!ReadBit(17)) ;  // loop until IO bit goes high
    Delay_sec(0.3);
    Jog(1,0);				// stop
    while (!CheckDone(1)) ; // loop until motion completes 
	DisableAxis(1);			// disable the axis
    EnableAxis(1);			// re-enable the ServoTick
	MoveAtVel(1,20000.0,20000); // move some amount inside the limits
    while (!CheckDone(1)) ; // loop until motion completes 
	Zero(1);				// Zero the position
    ch1->LimitSwitchOptions = SaveXLimits;  // restore limit settings


	// Home Y next - jog until it sees the limit

    Jog(2,-25000);  			// jog slowly negative
    while (!ReadBit(19)) ;  	// loop until IO bit goes high
    Delay_sec(0.3);
    Jog(2,0);				// stop
    while (!CheckDone(2)) ; // loop until motion completes 
	DisableAxis(2);			// disable the axis
    EnableAxis(2);			// re-enable the ServoTick
	MoveAtVel(2,20000.0,20000); // move some amount inside the limits
    while (!CheckDone(2)) ; // loop until motion completes 
	Zero(2);				// Zero the position
    ch2->LimitSwitchOptions = SaveYLimits;  // restore limit settings

	
	
	// Home B next - jog until it sees the limit

    Jog(4,-25000);  			// jog slowly negative
    while (!ReadBit(26)) ;  	// loop until IO bit goes high
    Jog(4,0);				// stop
    while (!CheckDone(4)) ; // loop until motion completes 
	DisableAxis(4);			// disable the axis
    EnableAxis(4);			// re-enable the ServoTick
	MoveAtVel(4,323100.0,125000); // move some amount inside the limits
    while (!CheckDone(4)) ; // loop until motion completes 
	Zero(4);				// Zero the position
    ch4->LimitSwitchOptions = SaveYLimits;  // restore limit settings


    
    
    // Home C next - jog until it sees the limit

    Jog(5,-25000);  			// jog slowly negative
    while (!ReadBit(28)) ;  	// loop until IO bit goes high
    Jog(5,0);				// stop
    while (!CheckDone(5)) ; // loop until motion completes 
	DisableAxis(5);			// disable the axis
    EnableAxis(5);			// re-enable the ServoTick
	MoveAtVel(5,445100.0,125000);			// move some amount inside the limits
    while (!CheckDone(5)) ; // loop until motion completes 
    Zero(5);				// Zero the position
    ch5->LimitSwitchOptions = SaveYLimits;  // restore limit settings


    DefineCoordSystem6(1,2,3,-1,4,5); //XYZBC (0 master 1 slave for X axis)
}