#include "KMotionDef.h"
#include "CSS\MySpindleDefs.h"
#include "CSS\CSSJog.c"
/*
 DACS
 7 - X
 6 - Z
 
 0 - C
 
 ENCODERS
 
 0 - C
 1 - Z
 2 - X
 
 LIMITS
 
 143 - home x
 141 - LIM- x
 140 - LIM+ x
 
 136 - home z
 137 - LIM- z
 138 - LIM+ z
 
 
 
*/
main()
{
/*
	int BitA,Change1=0,Change2=0, DiffX2;
	int PosNoWrap, NewPos, Pos=0, wraps;
	double Factor=0;
	
	SetBitDirection(16,0);
	SetBitDirection(17,0);
	SetBitDirection(18,0);
	SetBitDirection(19,0);
	SetBitDirection(20,0);
	SetBitDirection(21,0);
	SetBitDirection(22,0);
	SetBitDirection(23,0);
	SetBitDirection(24,0);
	SetBitDirection(25,0);
	
	
	SetBitDirection(26,0);
	SetBitDirection(27,0);
	SetBitDirection(28,0);
	SetBitDirection(29,0);
	SetBitDirection(30,0);
	SetBitDirection(31,0);
	SetBitDirection(32,0);
	SetBitDirection(33,0);
	SetBitDirection(34,0);
	SetBitDirection(35,0);
	//SetBit(152);
	//SetBit(157);
	//SetBit(158);
	*/
	ch0->InputMode=ENCODER_MODE;
	ch0->OutputMode=DAC_SERVO_MODE;
	ch0->Vel=333000;
	ch0->Accel=4e+008;
	ch0->Jerk=400000;
	ch0->P=0.08;
	ch0->I=1e-005;
	ch0->D=0;
	ch0->FFAccel=0;
	ch0->FFVel=0.00455;
	ch0->MaxI=50;
	ch0->MaxErr=2047;
	ch0->MaxOutput=2047;
	ch0->DeadBandGain=1;
	ch0->DeadBandRange=0;
	ch0->InputChan0=2;
	ch0->InputChan1=0;
	ch0->OutputChan0=6;
	ch0->OutputChan1=1;
	ch0->MasterAxis=-1;
	ch0->LimitSwitchOptions=0x120;
	ch0->LimitSwitchNegBit=0;
	ch0->LimitSwitchPosBit=0;
	ch0->SoftLimitPos=1e+030;
	ch0->SoftLimitNeg=-1e+030;
	ch0->InputGain0=-1;
	ch0->InputGain1=1;
	ch0->InputOffset0=0;
	ch0->InputOffset1=0;
	ch0->OutputGain=-1;
	ch0->OutputOffset=0;
	ch0->SlaveGain=1;
	ch0->BacklashMode=BACKLASH_OFF;
	ch0->BacklashAmount=0;
	ch0->BacklashRate=0;
	ch0->invDistPerCycle=1;
	ch0->Lead=0;
	ch0->MaxFollowingError=1000000000;
	ch0->StepperAmplitude=20;

	ch0->iir[0].B0=1;
	ch0->iir[0].B1=0;
	ch0->iir[0].B2=0;
	ch0->iir[0].A1=0;
	ch0->iir[0].A2=0;

	ch0->iir[1].B0=1;
	ch0->iir[1].B1=0;
	ch0->iir[1].B2=0;
	ch0->iir[1].A1=0;
	ch0->iir[1].A2=0;

	ch0->iir[2].B0=1;
	ch0->iir[2].B1=0;
	ch0->iir[2].B2=0;
	ch0->iir[2].A1=0;
	ch0->iir[2].A2=0;
    
	ch1->InputMode=ENCODER_MODE;
	ch1->OutputMode=DAC_SERVO_MODE;
	ch1->Vel=330000;
	ch1->Accel=4e+007;
	ch1->Jerk=200000;
	ch1->P=0.08;
	ch1->I=1e-005;
	ch1->D=0;
	ch1->FFAccel=0;
	ch1->FFVel=0.00476;
	ch1->MaxI=50;
	ch1->MaxErr=2047;
	ch1->MaxOutput=2047;
	ch1->DeadBandGain=1;
	ch1->DeadBandRange=0;
	ch1->InputChan0=1;
	ch1->InputChan1=0;
	ch1->OutputChan0=7;
	ch1->OutputChan1=0;
	ch1->MasterAxis=-1;
	ch1->LimitSwitchOptions=0x100;
	ch1->LimitSwitchNegBit=0;
	ch1->LimitSwitchPosBit=0;
	ch1->SoftLimitPos=1e+009;
	ch1->SoftLimitNeg=-1e+009;
	ch1->InputGain0=-1;
	ch1->InputGain1=-1;
	ch1->InputOffset0=0;
	ch1->InputOffset1=0;
	ch1->OutputGain=-1;
	ch1->OutputOffset=0;
	ch1->SlaveGain=1;
	ch1->BacklashMode=BACKLASH_OFF;
	ch1->BacklashAmount=0;
	ch1->BacklashRate=0;
	ch1->invDistPerCycle=1;
	ch1->Lead=0;
	ch1->MaxFollowingError=1000000000;
	ch1->StepperAmplitude=20;

	ch1->iir[0].B0=1;
	ch1->iir[0].B1=0;
	ch1->iir[0].B2=0;
	ch1->iir[0].A1=0;
	ch1->iir[0].A2=0;

	ch1->iir[1].B0=1;
	ch1->iir[1].B1=0;
	ch1->iir[1].B2=0;
	ch1->iir[1].A1=0;
	ch1->iir[1].A2=0;

	ch1->iir[2].B0=1;
	ch1->iir[2].B1=0;
	ch1->iir[2].B2=0;
	ch1->iir[2].A1=0;
	ch1->iir[2].A2=0;
	
	ch2->InputMode=NO_INPUT_MODE;
	ch2->OutputMode=STEP_DIR_MODE;
	ch2->Vel=40000;
	ch2->Accel=400000;
	ch2->Jerk=4e+006;
	ch2->P=0.01;
	ch2->I=0;
	ch2->D=0;
	ch2->FFAccel=0;
	ch2->FFVel=0;
	ch2->MaxI=200;
	ch2->MaxErr=1e+006;
	ch2->MaxOutput=2047;
	ch2->DeadBandGain=1;
	ch2->DeadBandRange=0;
	ch2->InputChan0=2;
	ch2->InputChan1=0;
	ch2->OutputChan0=2;
	ch2->OutputChan1=0;
	ch2->MasterAxis=-1;
	ch2->LimitSwitchOptions=0x100;
	ch2->LimitSwitchNegBit=0;
	ch2->LimitSwitchPosBit=0;
	ch2->SoftLimitPos=1e+009;
	ch2->SoftLimitNeg=-1e+009;
	ch2->InputGain0=-1;
	ch2->InputGain1=-1;
	ch2->InputOffset0=0;
	ch2->InputOffset1=0;
	ch2->OutputGain=1;
	ch2->OutputOffset=0;
	ch2->SlaveGain=1;
	ch2->BacklashMode=BACKLASH_OFF;
	ch2->BacklashAmount=0;
	ch2->BacklashRate=0;
	ch2->invDistPerCycle=1;
	ch2->Lead=0;
	ch2->MaxFollowingError=1000000000;
	ch2->StepperAmplitude=20;

	ch2->iir[0].B0=1;
	ch2->iir[0].B1=0;
	ch2->iir[0].B2=0;
	ch2->iir[0].A1=0;
	ch2->iir[0].A2=0;

	ch2->iir[1].B0=1;
	ch2->iir[1].B1=0;
	ch2->iir[1].B2=0;
	ch2->iir[1].A1=0;
	ch2->iir[1].A2=0;

	ch2->iir[2].B0=1;
	ch2->iir[2].B1=0;
	ch2->iir[2].B2=0;
	ch2->iir[2].A1=0;
	ch2->iir[2].A2=0;

	ch6->InputMode=ENCODER_MODE;
	ch6->OutputMode=DAC_SERVO_MODE;
	ch6->Vel=1;
	ch6->Accel=1;
	ch6->Jerk=1;
	ch6->P=0;
	ch6->I=0;
	ch6->D=0;
	ch6->FFAccel=0;
	ch6->FFVel=2047;
	ch6->MaxI=2047;
	ch6->MaxErr=2047;
	ch6->MaxOutput=2047;
	ch6->DeadBandGain=1;
	ch6->DeadBandRange=0;
	ch6->InputChan0=0;
	ch6->InputChan1=1;
	ch6->OutputChan0=0;
	ch6->OutputChan1=1;
	ch6->MasterAxis=-1;
	ch6->LimitSwitchOptions=0x100;
	ch6->LimitSwitchNegBit=0;
	ch6->LimitSwitchPosBit=0;
	ch6->SoftLimitPos=1e+009;
	ch6->SoftLimitNeg=-1e+009;
	ch6->InputGain0=-1;
	ch6->InputGain1=1;
	ch6->InputOffset0=0;
	ch6->InputOffset1=0;
	ch6->OutputGain=1;
	ch6->OutputOffset=0;
	ch6->SlaveGain=1;
	ch6->BacklashMode=BACKLASH_OFF;
	ch6->BacklashAmount=0;
	ch6->BacklashRate=0;
	ch6->invDistPerCycle=1;
	ch6->Lead=0;
	ch6->MaxFollowingError=10000000000;
	ch6->StepperAmplitude=250;

	ch6->iir[0].B0=1;
	ch6->iir[0].B1=0;
	ch6->iir[0].B2=0;
	ch6->iir[0].A1=0;
	ch6->iir[0].A2=0;

	ch6->iir[1].B0=1;
	ch6->iir[1].B1=0;
	ch6->iir[1].B2=0;
	ch6->iir[1].A1=0;
	ch6->iir[1].A2=0;

	ch6->iir[2].B0=1;
	ch6->iir[2].B1=0;
	ch6->iir[2].B2=0;
	ch6->iir[2].A1=0;
	ch6->iir[2].A2=0;


    EnableAxis(0);
	EnableAxis(1);
	EnableAxis(2);
	EnableAxis(6);
    DefineCoordSystem(0,2,1,-1);




	for (;;)
	{
		WaitNextTimeSlice();
		ServiceCSS();
	}

}