#include "KMotionDef.h"

// Home and set the commutation for two 3 phase brushless motors
//
// Assume index mark  


void main() 
{
    float k=0,A=15.0f;   // set coil current amplitude PWM units
	double p0;
	
	WriteSnapAmp(SNAP0+SNAP_SUPPLY_CLAMP0 ,SNAP_CONVERT_VOLTS_TO_ADC(85.0));
	WriteSnapAmp(SNAP0+SNAP_SUPPLY_CLAMP_ENA0,1);
	
	WriteSnapAmp(SNAP0+SNAP_SUPPLY_CLAMP1 ,SNAP_CONVERT_VOLTS_TO_ADC(85.0));
	WriteSnapAmp(SNAP0+SNAP_SUPPLY_CLAMP_ENA1,1);
	
// peak current limits SnapAmp0;	
	WriteSnapAmp(SNAP0+SNAP_PEAK_CUR_LIMIT0,10);  // current limit
	WriteSnapAmp(SNAP0+SNAP_PEAK_CUR_LIMIT1,10);  // current limit
	
	WriteSnapAmp(SNAP1+SNAP_SUPPLY_CLAMP0 ,SNAP_CONVERT_VOLTS_TO_ADC(85.0));
	WriteSnapAmp(SNAP1+SNAP_SUPPLY_CLAMP_ENA0,1);
	
	WriteSnapAmp(SNAP1+SNAP_SUPPLY_CLAMP1 ,SNAP_CONVERT_VOLTS_TO_ADC(85.0));
	WriteSnapAmp(SNAP1+SNAP_SUPPLY_CLAMP_ENA1,1);
	
// peak current limits Snap Amp1;	
	WriteSnapAmp(SNAP1+SNAP_PEAK_CUR_LIMIT0,9);  // current limit
	WriteSnapAmp(SNAP1+SNAP_PEAK_CUR_LIMIT1,9);  // current limit




Delay_sec(1);  // wait for any fault to clear
   	
	// rotate until we find the index mark

	ch0->Enable=FALSE;
	ch0->OutputChan0=8;
	for (;;)
    {
		Delay_sec(0.001);  // wait a millisecond 
		
		Write3PH(ch0,A, ++k/1000.0);  // move the pole 

		if (ReadBit(70))  // check for index mark
		{
			p0=ch0->Position; // save position
			ch0->Position=0;  // set current position to Zero
			
			// set commutation offset to 1/4th of a cycle
			// encoder has 4000 counts/rev 
			// motor has 3 cycles per rev
			
			//ch0->CommutationOffset = 3*8000/2.0/12.0 * 1.02;
			// offset = 1127 per autophase test
			
			ch0->CommutationOffset = 1127;
			printf("Position = %f\n",p0);
			break;
		}
    }

	Write3PH(ch0,0,0);    // turn off the coil    

	// define the axis as 3 phase BRUSHLESS_3PH_MODE
	// and set low PID gains
//Z axis wirefeed servo

	ch0->InputMode=ENCODER_MODE;
	ch0->OutputMode=BRUSHLESS_3PH_MODE;
	ch0->Vel=100000.000000;
	ch0->Accel=100000.000000;
	ch0->Jerk=100000000.000000;
	ch0->P=0.200000;
	ch0->I=0.000000;
	ch0->D=0.000000;
	ch0->FFAccel=0.000000;
	ch0->FFVel=0.000000;
	ch0->MaxI=200.000000;
	ch0->MaxErr=1000000.000000;
	ch0->MaxOutput=230.000000;
	ch0->DeadBandGain=1.000000;
	ch0->DeadBandRange=0.000000;
	ch0->InputChan0=8;
	ch0->InputChan1=0;
	ch0->OutputChan0=8;
	ch0->OutputChan1=0;
	ch0->LimitSwitchOptions=0x0;
	ch0->InputGain0=1.000000;
	ch0->InputGain1=1.000000;
	ch0->InputOffset0=0.000000;
	ch0->InputOffset1=0.000000;
	//ch0->invDistPerCycle=2.0/8000.0; // CW (2 cycles/rev) / (8000 encoder cnts/rev)
	ch0->invDistPerCycle=0.000362318841;
	ch0->Lead=0.000000;
	ch0->MaxFollowingError=4000.000000;
	ch0->StepperAmplitude=20.000000;

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

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

	ch0->iir[2].B0=1.000000;
	ch0->iir[2].B1=0.000000;
	ch0->iir[2].B2=0.000000;
	ch0->iir[2].A1=0.000000;
	ch0->iir[2].A2=0.000000;

	EnableAxisDest(0,0.0f);  // Enable servo at destination of 0
	
	// rotate until we find the index mark

	ch2->Enable=FALSE;
	ch2->OutputChan0=12;
	for (;;)
    {
		Delay_sec(0.001);  // wait a millisecond 
		
		Write3PH(ch2,A, ++k/1000.0);  // move the pole 

		if (ReadBit(71))  // check for index mark
		{
			p0=ch2->Position; // save position
			ch2->Position=0;  // set current position to Zero
			
			// set commutation offset to 1/4th of a cycle
			// encoder has 4000 counts/rev 
			// motor has 3 cycles per rev
			//ch2->CommutationOffset = 3*8000/2.0/12.0 * 1.02;
			// offset = 193 per autophase test
			
			ch2->CommutationOffset = -193;
						
			printf("Position = %f\n",p0);
			break;
		}
    }

	Write3PH(ch2,0,0);    // turn off the coil    

	// define the axis as 3 phase BRUSHLESS_3PH_MODE
	// and set low PID gains
//Spindle servo motor
	ch2->InputMode=ENCODER_MODE;
	ch2->OutputMode=BRUSHLESS_3PH_MODE;
	ch2->Vel=40000;
	ch2->Accel=400000;
	ch2->Jerk=4e+06;
	ch2->P=0.1;
	ch2->I=0.01;
	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=9;
	ch2->InputChan1=0;
	ch2->OutputChan0=12;
	ch2->OutputChan1=0;
	ch2->MasterAxis=-1;
	ch2->LimitSwitchOptions=0x100;
	ch2->LimitSwitchNegBit=1049;
	ch2->LimitSwitchPosBit=1049;
	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=0.000487804878;
	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;

	EnableAxisDest(2,0.0f);  // Enable servo at destination of 0
	
	
//X axis cross feed stepper
	ch1->InputMode=NO_INPUT_MODE;
	ch1->OutputMode=MICROSTEP_MODE;
	ch1->Vel=2.5;
	ch1->Accel=10;
	ch1->Jerk=50000;
	ch1->P=0;
	ch1->I=0.01;
	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=0;
	ch1->InputChan1=0;
	ch1->OutputChan0=10;
	ch1->OutputChan1=11;
	ch1->MasterAxis=-1;
	ch1->LimitSwitchOptions=0x110;
	ch1->LimitSwitchNegBit=1042;
	ch1->LimitSwitchPosBit=1043;
	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=-250;
	ch1->Lead=50;
	ch1->MaxFollowingError=1000000000;
	ch1->StepperAmplitude=60;

	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;
	
	EnableAxisDest(1,0);
	
	DefineCoordSystem(1,-1,0,-1);
	
	printf("Hello World end 2\n");  // send message to console
//end new axis setup

//Initiallize Konnect i/o
	{
	InitAux();
	AddKonnect(0,&VirtualBits,VirtualBitsEx);
}	
	SetBitDirection (89,1);
	SetBitDirection (90,1);
	SetBitDirection (91,1);
	SetBitDirection (92,1);
	SetBitDirection (93,1);
	


	EnableAxisDest(0,0);
	EnableAxisDest(1,0);
	EnableAxisDest(2,0);
	DisableAxis(1);
	DisableAxis(2);
	
	
	DefineCoordSystem(1,-1,0,-1);
	ClearBit(92);
    printf("Hello World end 2\n");  // send message to console


// Set CPU Watchdog timer output bit 93 on SnapAmp GPIO 13 if processor 
//is running.  set every 1000 time slices then reset after 500 time slices
//  ( to be set up yet  for one 100 millisecond pulse every 200 miiliseconds)
printf("Hello World end 5\n");  // send message to console
	
	for (;;)  //loop forever
	{
		WaitNextTimeSlice();
// Sets and clears output bits based on channel Enable status
		if (ch0->Enable)
			SetBit(60);
			
		else
			ClearBit(60);
			
			if (ch1->Enable)
			SetBit(61);
			
		else
			ClearBit(61);
			
			if (ch2->Enable)
			SetBit(62);
			
		else
			ClearBit(62);
			
//			if (ReadBit(1047))
//			{SetBit(50);
//			Delay_sec(0.1);
	

//		else
//			ClearBit(50);
//
		
	
	
	//test Konnect output bit by flashing it continously for "i" times at "Delay_sec" rate and leaving it on;
//Use Konnect output bits #48 - 63;
//This section delays processing of Channel Enable status output bits by Delay_sec X2	
	
	int i;
	for (i=0; i<1; i++)
	{
		ClearBit(63);
		Delay_sec(0.1);
		SetBit(63);
		Delay_sec(0.1);
	}
}
	//printf("Hello World end 7\n");  // send message to console
	//return 0;
	
}
////////