#include "KMotionDef.h"

// Defines axis 0,1,2,3,4 and 5 as simple steppers
// enables them
// sets them as an xyzbc coordinate system for GCode

int main() 
{
	
	FPGA(STEP_PULSE_LENGTH_ADD)=32 + 0x40; // set the pulse time to ~ 2us and multiplex 
                                          
                                           // to JP4 and JP6                                  
	
	ch0->InputMode=NO_INPUT_MODE;
	ch0->OutputMode=STEP_DIR_MODE;
	ch0->Vel=12500;
	ch0->Accel=300000;
	ch0->Jerk=5e+06;
	ch0->P=1;
	ch0->I=0;
	ch0->D=0;
	ch0->FFAccel=0;
	ch0->FFVel=0;
	ch0->MaxI=200;
	ch0->MaxErr=1e+06;
	ch0->MaxOutput=200;
	ch0->DeadBandGain=1;
	ch0->DeadBandRange=0;
	ch0->InputChan0=0;
	ch0->InputChan1=0;
	ch0->OutputChan0=28;
	ch0->OutputChan1=0;
	ch0->MasterAxis=-1;
	ch0->LimitSwitchOptions=0x127;
	ch0->LimitSwitchNegBit=17;
	ch0->LimitSwitchPosBit=16;
	ch0->SoftLimitPos=1e+09;
	ch0->SoftLimitNeg=-1e+09;
	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=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;

	ch1->InputMode=NO_INPUT_MODE;
	ch1->OutputMode=STEP_DIR_MODE;
	ch1->Vel=125000;
	ch1->Accel=300000;
	ch1->Jerk=5e+06;
	ch1->P=0;
	ch1->I=0;
	ch1->D=0;
	ch1->FFAccel=0;
	ch1->FFVel=0;
	ch1->MaxI=200;
	ch1->MaxErr=1e+06;
	ch1->MaxOutput=200;
	ch1->DeadBandGain=1;
	ch1->DeadBandRange=0;
	ch1->InputChan0=1;
	ch1->InputChan1=0;
	ch1->OutputChan0=29;
	ch1->OutputChan1=0;
	ch1->MasterAxis=0;
	ch1->LimitSwitchOptions=0x123;
	ch1->LimitSwitchNegBit=17;
	ch1->LimitSwitchPosBit=16;
	ch1->SoftLimitPos=1e+09;
	ch1->SoftLimitNeg=-1e+09;
	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=0.000769;
	ch1->iir[2].B1=0.001538;
	ch1->iir[2].B2=0.000769;
	ch1->iir[2].A1=1.92081;
	ch1->iir[2].A2=-0.923885;

	ch2->InputMode=NO_INPUT_MODE;
	ch2->OutputMode=STEP_DIR_MODE;
	ch2->Vel=125000;
	ch2->Accel=300000;
	ch2->Jerk=5e+06;
	ch2->P=1;
	ch2->I=0;
	ch2->D=0;
	ch2->FFAccel=0;
	ch2->FFVel=0;
	ch2->MaxI=200;
	ch2->MaxErr=1e+06;
	ch2->MaxOutput=200;
	ch2->DeadBandGain=1;
	ch2->DeadBandRange=0;
	ch2->InputChan0=2;
	ch2->InputChan1=0;
	ch2->OutputChan0=30;
	ch2->OutputChan1=0;
	ch2->MasterAxis=-1;
	ch2->LimitSwitchOptions=0x123;
	ch2->LimitSwitchNegBit=19;
	ch2->LimitSwitchPosBit=18;
	ch2->SoftLimitPos=1e+09;
	ch2->SoftLimitNeg=-1e+09;
	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=0.000769;
	ch2->iir[2].B1=0.001538;
	ch2->iir[2].B2=0.000769;
	ch2->iir[2].A1=1.92081;
	ch2->iir[2].A2=-0.923885;
 


    
	
	ch3->InputMode=NO_INPUT_MODE;
	ch3->OutputMode=STEP_DIR_MODE;
	ch3->Vel=125000;
	ch3->Accel=300000;
	ch3->Jerk=5e+6;
	ch3->P=1;
	ch3->I=0;
	ch3->D=0;
	ch3->FFAccel=0;
	ch3->FFVel=0;
	ch3->MaxI=200;
	ch3->MaxErr=1e+06;
	ch3->MaxOutput=200;
	ch3->DeadBandGain=1;
	ch3->DeadBandRange=0;
	ch3->InputChan0=3;
	ch3->InputChan1=0;
	ch3->OutputChan0=31;
	ch3->OutputChan1=0;
	ch3->MasterAxis=-1;
	ch3->LimitSwitchOptions=0x123;
	ch3->LimitSwitchNegBit=21;
	ch3->LimitSwitchPosBit=20;
	ch3->SoftLimitPos=1e+09;
	ch3->SoftLimitNeg=-1e+09;
	ch3->InputGain0=1;
	ch3->InputGain1=1;
	ch3->InputOffset0=0;
	ch3->InputOffset1=0;
	ch3->OutputGain=-1;
	ch3->OutputOffset=0;
	ch3->SlaveGain=1;
	ch3->BacklashMode=BACKLASH_OFF;
	ch3->BacklashAmount=0;
	ch3->BacklashRate=0;
	ch3->invDistPerCycle=1;
	ch3->Lead=0;
	ch3->MaxFollowingError=1000000000;
	ch3->StepperAmplitude=20;

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

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

	ch3->iir[2].B0=0.000769;
	ch3->iir[2].B1=0.001538;
	ch3->iir[2].B2=0.000769;
	ch3->iir[2].A1=1.92081;
	ch3->iir[2].A2=-0.923885;


	ch4->InputMode=NO_INPUT_MODE;
	ch4->OutputMode=STEP_DIR_MODE;
	ch4->Vel=125000;
	ch4->Accel=300000;
	ch4->Jerk=4e+06;
	ch4->P=1;
	ch4->I=0;
	ch4->D=0;
	ch4->FFAccel=0;
	ch4->FFVel=0;
	ch4->MaxI=200;
	ch4->MaxErr=200;
	ch4->MaxOutput=200;
	ch4->DeadBandGain=1;
	ch4->DeadBandRange=0;
	ch4->InputChan0=1;
	ch4->InputChan1=1;
	ch4->OutputChan0=25;
	ch4->OutputChan1=1;
	ch4->MasterAxis=-1;
	ch4->LimitSwitchOptions=0x123;
	ch4->LimitSwitchNegBit=26;
	ch4->LimitSwitchPosBit=27;
	ch4->SoftLimitPos=1e+09;
	ch4->SoftLimitNeg=-1e+09;
	ch4->InputGain0=1;
	ch4->InputGain1=1;
	ch4->InputOffset0=0;
	ch4->InputOffset1=0;
	ch4->OutputGain=-1;
	ch4->OutputOffset=0;
	ch4->SlaveGain=1;
	ch4->BacklashMode=BACKLASH_OFF;
	ch4->BacklashAmount=0;
	ch4->BacklashRate=0;
	ch4->invDistPerCycle=1;
	ch4->Lead=0;
	ch4->MaxFollowingError=10000000;
	ch4->StepperAmplitude=250;

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

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

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

	ch5->InputMode=NO_INPUT_MODE;
	ch5->OutputMode=STEP_DIR_MODE;
	ch5->Vel=125000;
	ch5->Accel=300000;
	ch5->Jerk=4e+06;
	ch5->P=1;
	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=5;
	ch5->InputChan1=1;
	ch5->OutputChan0=27;
	ch5->OutputChan1=1;
	ch5->MasterAxis=-1;
	ch5->LimitSwitchOptions=0x123;
	ch5->LimitSwitchNegBit=28;
	ch5->LimitSwitchPosBit=29;
	ch5->SoftLimitPos=1e+10;
	ch5->SoftLimitNeg=-1e+10;
	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=10000000;
	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(0,0);
	EnableAxisDest(1,0);
	EnableAxisDest(2,0);
	EnableAxisDest(3,0);
	EnableAxisDest(4,0);
	EnableAxisDest(5,0);

	DefineCoordSystem6(1,2,3,-1,4,5); //XYZBC (0 master 1 slave for X axis) 


// This is used to stop all Axis if any of the motors fault.

//for (;;)  //loop forever
	//{
	//	WaitNextTimeSlice();

		// if Motor Fault present disable any enabled Axis ??

		//if (!ReadBit(128)||!ReadBit(129)||!ReadBit(130)||!ReadBit(131)||!ReadBit(30)||!ReadBit(31))   
		//{
			//if (ch1->Enable) DisableAxis(1);  // axis still enabled?  - Disable it
			//if (ch2->Enable) DisableAxis(2);  // axis still enabled?  - Disable it
			//if (ch3->Enable) DisableAxis(3);  // axis still enabled?  - Disable it
            //if (ch4->Enable) DisableAxis(4);  // axis still enabled?  - Disable it
			//if (ch5->Enable) DisableAxis(5);  // axis still enabled?  - Disable it
			

		//}
	//}
    return 0;
}