#include "KMotionDef.h"

// Configuration and Homing program for a 5 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);  // Disable axes



    // 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 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
	Jog(5,1000);
    while (ReadBit(28));
    MoveAtVel(5,445100,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 ( 1 sends signal to X1 X2 axis)
    
    return 0;

}