#include "KMotionDef.h"
#include "MySpindleDefs.h"
#include "CSSJog.c"
main() 
{

	FPGA(STEP_PULSE_LENGTH_ADD) = 20;
	ch0->InputMode=ENCODER_MODE;
	ch0->OutputMode=CL_STEP_DIR_MODE;
	ch0->Vel=250000;
	ch0->Accel=6e+06;
	ch0->Jerk=4e+07;
	ch0->P=0.1;
	ch0->I=0.02;
	ch0->D=0.0001;
	ch0->FFAccel=1e-05;
	ch0->FFVel=0.0016;
	ch0->MaxI=700;
	ch0->MaxErr=2000;
	ch0->MaxOutput=700;
	ch0->DeadBandGain=0;
	ch0->DeadBandRange=2;
	ch0->InputChan0=1;
	ch0->InputChan1=0;
	ch0->OutputChan0=0;
	ch0->OutputChan1=0;
	ch0->MasterAxis=-1;
	ch0->LimitSwitchOptions=0x127;
	ch0->LimitSwitchNegBit=26;
	ch0->LimitSwitchPosBit=28;
	ch0->SoftLimitPos=1e+09;
	ch0->SoftLimitNeg=-1e+20;
	ch0->InputGain0=2;
	ch0->InputGain1=1;
	ch0->InputOffset0=0;
	ch0->InputOffset1=0;
	ch0->OutputGain=1;
	ch0->OutputOffset=0;
	ch0->SlaveGain=1;
	ch0->BacklashMode=BACKLASH_OFF;
	ch0->BacklashAmount=10;
	ch0->BacklashRate=1000;
	ch0->invDistPerCycle=1;
	ch0->Lead=0;
	ch0->MaxFollowingError=2000;
	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=0.000768788;
	ch0->iir[2].B1=0.00153758;
	ch0->iir[2].B2=0.000768788;
	ch0->iir[2].A1=1.92076;
	ch0->iir[2].A2=-0.923833;

    EnableAxisDest(0,0);

	ch2->InputMode=ENCODER_MODE;
	ch2->OutputMode=CL_STEP_DIR_MODE;
	ch2->Vel=40000;
	ch2->Accel=2e+06;
	ch2->Jerk=1e+07;
	ch2->P=0.2;
	ch2->I=0.006;
	ch2->D=1e-07;
	ch2->FFAccel=1e-05;
	ch2->FFVel=0.002;
	ch2->MaxI=500;
	ch2->MaxErr=1500;
	ch2->MaxOutput=700;
	ch2->DeadBandGain=0;
	ch2->DeadBandRange=3;
	ch2->InputChan0=2;
	ch2->InputChan1=0;
	ch2->OutputChan0=1;
	ch2->OutputChan1=0;
	ch2->MasterAxis=-1;
	ch2->LimitSwitchOptions=0x123;
	ch2->LimitSwitchNegBit=29;
	ch2->LimitSwitchPosBit=27;
	ch2->SoftLimitPos=1e+09;
	ch2->SoftLimitNeg=-1e+09;
	ch2->InputGain0=5;
	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=2000;
	ch2->StepperAmplitude=20;

	ch2->iir[0].B0=0.0274969;
	ch2->iir[0].B1=0.0274969;
	ch2->iir[0].B2=0;
	ch2->iir[0].A1=0.945006;
	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=0.000768809;
	ch2->iir[2].B1=0.00153762;
	ch2->iir[2].B2=0.000768809;
	ch2->iir[2].A1=1.92081;
	ch2->iir[2].A2=-0.923885;

	EnableAxisDest(2,0);

	ch5->InputMode=ENCODER_MODE;
	ch5->OutputMode=STEP_DIR_MODE;
	ch5->Vel=265000;
	ch5->Accel=100000;
	ch5->Jerk=300000;
	ch5->P=0;
	ch5->I=0;
	ch5->D=0;
	ch5->FFAccel=0;
	ch5->FFVel=0;
	ch5->MaxI=200;
	ch5->MaxErr=200;
	ch5->MaxOutput=200;
	ch5->DeadBandGain=1;
	ch5->DeadBandRange=0;
	ch5->InputChan0=3;
	ch5->InputChan1=1;
	ch5->OutputChan0=2;
	ch5->OutputChan1=1;
	ch5->MasterAxis=-1;
	ch5->LimitSwitchOptions=0x100;
	ch5->LimitSwitchNegBit=0;
	ch5->LimitSwitchPosBit=0;
	ch5->SoftLimitPos=1e+09;
	ch5->SoftLimitNeg=-1e+09;
	ch5->InputGain0=1;
	ch5->InputGain1=1;
	ch5->InputOffset0=0;
	ch5->InputOffset1=0;
	ch5->OutputGain=1;
	ch5->OutputOffset=0;
	ch5->SlaveGain=1;
	ch5->BacklashMode=BACKLASH_OFF;
	ch5->BacklashAmount=0;
	ch5->BacklashRate=0;
	ch5->invDistPerCycle=1;
	ch5->Lead=0;
	ch5->MaxFollowingError=100000000000;
	ch5->StepperAmplitude=250;

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

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

	ch5->iir[2].B0=1;
	ch5->iir[2].B1=0;
	ch5->iir[2].B2=0;
	ch5->iir[2].A1=0;
	ch5->iir[2].A2=0;
	EnableAxisDest(5,0);
	EnableAxis(0);
	EnableAxis(2);
    ConfigureSpindle (1,5,0.1,0.1,3600.0);
	
	DefineCoordSystem6(0,-1,2,-1,-1,-1); 

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