// Magill Init_v4.c
// 
// This init file does not watch the limit switches, so can be used to
// re-initialize and back off of limits if they have been hit.
// Resets DRO's and machine zeros at current location when run.
// 
// History
// v1 - original file from Carl Bruce
// v2 - edited 050518 jpm
// v3 - edited & tested on router 050818 jpm
// v4 - changed axis parameters 051118 jpm
// 
// Pins used on Jon's & Fred's routers:
// 16 = X0 limits
// 17 = Z axis limit, upper limit only
// 18 = X1 limits
// 19 = Touch off block
// 20 = E-stop switch - wired N.O.
// 21 = Y axis limits
//
// 26 = PWM 0 for spindle speed
// 29 = Spindle control (on / off)
//
//
//
//
//

#include "KMotionDef.h"


#define X0_LIMITBIT 16
#define X1_LIMITBIT 18
#define Y_LIMITBIT 21
#define Z_LIMITBIT 17  // Upper limit switch only
#define TOUCH_OFFBIT 19
#define HALTBIT 20     // E-stop latching button - N.O.


// function prototypes for compiler
int DoPC(int cmd);
int DoPCFloat(int cmd, float f);
int Debounce(int n, int *cnt, int *last, int *lastsolid);

// state variables for switch debouncing - only used with momentary e-stop
int hlast=0,hlastsolid=-1,hcount=0;

int main()
{
   
    int result;

     
	// Setup bits for limit switches, touch off and halt / e-stop
	SetBitDirection(X0_LIMITBIT,0);
    SetBitDirection(X1_LIMITBIT,0);
    SetBitDirection(Y_LIMITBIT,0);
    SetBitDirection(Z_LIMITBIT,0);
    SetBitDirection(HALTBIT,0);
    SetBitDirection(TOUCH_OFFBIT,0);
	
	// For future 4th axis
    SetBitDirection(36,1);   // 4th axis step
    SetBitDirection(37,1);   // 4th axis dir
	
	// Used by KTOPPER for second ATC spindle
    // SetBitDirection(23,0);
    // SetBitDirection(24,0);
    // SetBitDirection(25,0);
    // SetBitDirection(30,1);
    // SetBitDirection(31,1);

	// Setup spindle and PWM control
    SetBitDirection(28,1);
	ClearBit(28);
    SetBitDirection(29,1);
	ClearBit(29);
	
	// set polarity and pulse length to 4us
    FPGA(STEP_PULSE_LENGTH_ADD) = 63 + 0x80;
	
	SetBitDirection(26,1);  	// Set bit 26 (PWM 0 as an output)
	SetBitDirection(27,1); 		// Set bit 27 (PWM 1 as an output)
	FPGA(IO_PWMS_PRESCALE)=46;	// Set pwm period to 30KHz
	FPGA(IO_PWMS+3)=1;			//IO27 enable PWM 1
	FPGA(IO_PWMS+2)=0;			//IO27 speed control
	FPGA(IO_PWMS+1)=1;			//IO26 enable PWM 0
	FPGA(IO_PWMS+0)=0;			//IO26 speed control

    
    
    // Ch 0 is X 0 axis -- aka Left Side drive
    ch0->InputMode=ENCODER_MODE;
	ch0->OutputMode=CL_STEP_DIR_MODE;
	ch0->Vel=26500;
	ch0->Accel=150000;
	ch0->Jerk=8e+007;
	ch0->P=0;
	ch0->I=0.0001;
	ch0->D=0;
	ch0->FFAccel=0;
	ch0->FFVel=0;
	ch0->MaxI=10000;
	ch0->MaxErr=500;
	ch0->MaxOutput=250;
	ch0->DeadBandGain=1;
	ch0->DeadBandRange=0;
	ch0->InputChan0=0;
	ch0->InputChan1=1;
	ch0->OutputChan0=0;
	ch0->OutputChan1=1;
	ch0->MasterAxis=-1;
	// not watching limit switches in init routine
	// limit settings: 0x0 = off, 0x11f = disallow drive into limits, 0x12f = stop movement
	ch0->LimitSwitchOptions=0x0;
	ch0->LimitSwitchNegBit=0;
	ch0->LimitSwitchPosBit=0;
	ch0->SoftLimitPos=1e+009;
	ch0->SoftLimitNeg=-1e+009;
	ch0->InputGain0=-0.5;
	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=250;

	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;

      
    //CH 1 is X 1 axis -- aka Right Side drive, slaved to X 0
    ch1->InputMode=ENCODER_MODE;
	ch1->OutputMode=CL_STEP_DIR_MODE;
	ch1->Vel=26500;
	ch1->Accel=150000;
	ch1->Jerk=8e+007;
	ch1->P=0;
	ch1->I=0.0001;
	ch1->D=0;
	ch1->FFAccel=0;
	ch1->FFVel=0;
	ch1->MaxI=10000;
	ch1->MaxErr=500;
	ch1->MaxOutput=250;
	ch1->DeadBandGain=1;
	ch1->DeadBandRange=0;
	ch1->InputChan0=1;
	ch1->InputChan1=2;
	ch1->OutputChan0=1;
	ch1->OutputChan1=3;
	ch1->MasterAxis=0;
	// not watching limit switches in init routine
	// limit settings: 0x0 = off, 0x11f = disallow drive into limits, 0x12f = stop movement
	ch1->LimitSwitchOptions=0x0;
	ch1->LimitSwitchNegBit=0;
	ch1->LimitSwitchPosBit=0;
	ch1->SoftLimitPos=1e+009;
	ch1->SoftLimitNeg=-1e+009;
	ch1->InputGain0=-0.5;
	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=250;

	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;

    // ch 2 is Y axis
	ch2->InputMode=ENCODER_MODE;
	ch2->OutputMode=CL_STEP_DIR_MODE;
	ch2->Vel=31000;
	ch2->Accel=100000;
	ch2->Jerk=8e+007;
	ch2->P=0;
	ch2->I=0.0001;
	ch2->D=0;
	ch2->FFAccel=0;
	ch2->FFVel=0;
	ch2->MaxI=10000;
	ch2->MaxErr=200;
	ch2->MaxOutput=250;
	ch2->DeadBandGain=1;
	ch2->DeadBandRange=0;
	ch2->InputChan0=2;
	ch2->InputChan1=3;
	ch2->OutputChan0=2;
	ch2->OutputChan1=5;
	ch2->MasterAxis=-1;
	// not watching limit switches in init routine
	// limit settings: 0x0 = off, 0x11f = disallow drive into limits, 0x12f = stop movement
	ch2->LimitSwitchOptions=0x0;
	ch2->LimitSwitchNegBit=0;
	ch2->LimitSwitchPosBit=0;
	ch2->SoftLimitPos=1e+009;
	ch2->SoftLimitNeg=-1e+009;
	ch2->InputGain0=0.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=1000000000;
	ch2->StepperAmplitude=250;

	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;

    
    // Ch3 is Z axis
    ch3->InputMode=ENCODER_MODE;
	ch3->OutputMode=CL_STEP_DIR_MODE;
	ch3->Vel=17000;
	ch3->Accel=100000;
	ch3->Jerk=5e+007;
	ch3->P=0;
	ch3->I=0.00001;
	ch3->D=0;
	ch3->FFAccel=0;
	ch3->FFVel=0;
	ch3->MaxI=10000;
	ch3->MaxErr=200;
	ch3->MaxOutput=250;
	ch3->DeadBandGain=1;
	ch3->DeadBandRange=0;
	ch3->InputChan0=3;
	ch3->InputChan1=4;
	ch3->OutputChan0=3;
	ch3->OutputChan1=7;
	ch3->MasterAxis=-1;
	// not watching limit switches in init routine
	// limit settings: 0x0 = off, 0x11f = disallow drive into limits, 0x12f = stop movement
	ch3->LimitSwitchOptions=0x0;
	ch3->LimitSwitchNegBit=0;
	ch3->LimitSwitchPosBit=0;
	ch3->SoftLimitPos=1e+009;
	ch3->SoftLimitNeg=-1e+009;
	ch3->InputGain0=-0.5;
	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=250;

	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=1;
	ch3->iir[2].B1=0;
	ch3->iir[2].B2=0;
	ch3->iir[2].A1=0;
	ch3->iir[2].A2=0;



	EnableAxisDest(0,0);
    EnableAxisDest(1,0);
    EnableAxisDest(2,0);
    EnableAxisDest(3,0);
    // EnableAxisDest(4,0);  // Unused for the time being until rotary axis is added


	/*
    // Since init program doesn't monitor limits, don't set soft limits here
    ch0->SoftLimitPos=1e+030;
	ch0->SoftLimitNeg=-1e+030;
	ch1->SoftLimitPos=1e+030;
	ch1->SoftLimitNeg=-1e+030;
	ch2->SoftLimitPos=1e+030;
	ch2->SoftLimitNeg=-1e+030;
	ch3->SoftLimitPos=1e+030;
	ch3->SoftLimitNeg=-1e+030;
	ch4->SoftLimitPos=1e+030;
	ch4->SoftLimitNeg=-1e+030;
	*/


	// Zero the axes where they are
    Zero(0);
    Zero(1);
    Zero(2);
    Zero(3);
    // Zero(4);

    DefineCoordSystem(0,2,3,-1);  //Define XYZA coordinated motion axes, -1 = not used
	
	
    for (;;) // loop forever
    {
        WaitNextTimeSlice();
		
		// Handle HALT from E-Stop button press
		if (!ReadBit(HALTBIT)) DoPC(PC_COMM_HALT);
    }
}


// Put a Float as a parameter and pass the command to the App
int DoPCFloat(int cmd, float f)
{
    int result;
    persist.UserData[PC_COMM_PERSIST+1] = *(int*)&f;
    return DoPC(cmd);
}


// Pass a command to the PC and wait for it to handshake
// that it was received by either clearing the command
// or changing it to a negative error code
int DoPC(int cmd)
{
    int result;
   
    persist.UserData[PC_COMM_PERSIST]=cmd;
   
    do
    {
        WaitNextTimeSlice();   
    }while (result=persist.UserData[PC_COMM_PERSIST]>0);
   
    printf("Result = %d\n",result);

    return result;
}