#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,SaveBLimits,SaveCLimits;  //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;
	//SaveBLimits = ch4->LimitSwitchOptions;
	//SaveCLimits = 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 X next - jog until it sees the limit

     Jog(1,-25000);  			// jog slowly positive
    while (!ReadBit(17));  	// loop until IO bit goes high
    Delay_sec(3);           // Allows time for Teknic home sequence
   	Jog(1,0);              // stop
    while (!CheckDone(1)); // loop until motion completes 
	Zero(1);
    EnableAxis(1);			// re-enable the ServoTick
	MoveAtVel(1,25000,125000); // move some amount inside the limits
    while (!CheckDone(1));  // loop until motion completes
    Zero(1);
    ch1->LimitSwitchOptions = SaveXLimits;  // restore limit setting
    


    DefineCoordSystem6(1,2,3,-1,4,5); //XYZBC ( 1 sends signal to X1 X2 axis)
    
    return 0;

}