#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,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;
	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(2);           // Allows time for Teknic home sequence
   	Jog(3,0);              // stop
    while (!CheckDone(3)); // loop until motion completes 
	Zero(3);
    EnableAxis(3);			// re-enable the ServoTick
	MoveAtVel(3,-30000,125000); // move some amount inside the limits
    while (!CheckDone(3));  // loop until motion completes
    Zero(3);
    ch1->LimitSwitchOptions = SaveZLimits;  // restore limit setting
	
// 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

	

	// Home y next - jog until it sees the limit
    
    
     Jog(2,-25000);  			// jog slowly positive
    while (!ReadBit(19));  	// loop until IO bit goes high
    Delay_sec(2);           // Allows time for Teknic home sequence
   	Jog(2,0);              // stop
    while (!CheckDone(2)); // loop until motion completes 
	Zero(2);
    EnableAxis(2);			// re-enable the ServoTick
	MoveAtVel(2,125000,125000); // move some amount inside the limits
    while (!CheckDone(2));  // loop until motion completes
    Zero(2);
    ch2->LimitSwitchOptions = SaveYLimits;  // restore limit setting
    

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

    
    Jog(4,25000);  			// jog slowly positive
    while (!ReadBit(27));  	// loop until IO bit goes high
    Delay_sec(2);
   	Jog(4,0);              // stop
    while (!CheckDone(4)); // loop until motion completes 
	Zero(4);
    EnableAxis(4);			// re-enable the ServoTick
	MoveAtVel(4,-323100,125000); // move some amount inside the limits
    while (!CheckDone(4));  // loop until motion completes
    Zero(4);
    ch4->LimitSwitchOptions = SaveBLimits;  // restore limit settings	

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

     Jog(5,-25000);  			// jog slowly positive
    while (!ReadBit(28));  	// loop until IO bit goes high
    Delay_sec(2);
   	Jog(5,0);              // stop
    while (!CheckDone(5)); // loop until motion completes 
	Zero(5);
    EnableAxis(5);			// re-enable the ServoTick
	MoveAtVel(5,443300,125000); // move some amount inside the limits
    while (!CheckDone(5));  // loop until motion completes
    Zero(5);
    ch5->LimitSwitchOptions = SaveCLimits;  // restore limit setting
    

    

	

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