#include "KMotionDef.h"
#define TMP 100
#include "KflopToKMotionCNCFunctions.c"

#define GOTOZERO 61
#define ESTOP_BIT 128

#define FEEDHOLDBIT 46
#define CYCLESTARTBIT 47
#define ESTOP 26
#define HALTBIT 27
#define RESTARTBIT 28
#define ZEROALLBIT 29

#define XAXIS 0
#define YAXIS 1
#define ZAXIS 2
#define AAXIS 3
#define BAXIS 4
#define CAXIS 5
#define VAXIS 6
#define UAXIS 7

#define XPLUS  16
#define XMINUS 17
#define YPLUS  18
#define YMINUS 19
#define ZPLUS  20
#define ZMINUS 21
#define APLUS  22
#define AMINUS 23
#define BPLUS  1058
#define BMINUS 1059
#define VPLUS  1024
#define VMINUS 1025
#define UPLUS  1026
#define UMINUS 1027

#define MAX_SPEED_X 450.0
#define MAX_SPEED_Y 500.0
#define MAX_SPEED_Z 500.0
#define MAX_SPEED_A 500.0
#define MAX_SPEED_B 1000.0
#define MAX_SPEED_V 50000.0
#define MAX_SPEED_U 50000.0

#define DRIVE_X_ERROR 129
#define DRIVE_Y_ERROR 130
#define DRIVE_Z_ERROR 131
#define DRIVE_A_ERROR 132
#define DRIVE_B_ERROR 133
#define DRIVE_U_ERROR 134
#define DRIVE_V_ERROR 135

// function prototypes for compiler
void DoSpeedAxis(int ch, int Plus, int Minus, double *LastSpeed, double Speed);
void DoGoToZero(void);
int DoPC(int cmd);
int DoPCFloat(int cmd, float f);
int Debounce(int n, int *cnt, int *last, int *lastsolid);

double T0=0.0,T1,Angle_step,value;
int Answer;
char p[100];

// state variables for switch debouncing
int flast=0,flastsolid=-1,fcount=0;
int clast=0,clastsolid=-1,ccount=0;
int elast=0,elastsolid=-1,ecount=0;
int hlast=0,hlastsolid=-1,hcount=0;
int rlast=0,rlastsolid=-1,rcount=0;
int zlast=0,zlastsolid=-1,zcount=0;


main()
{
	SetBit(151);

	ch0->InputMode=ENCODER_MODE;
	ch0->OutputMode=DAC_SERVO_MODE;
	ch0->Vel=45000;
	ch0->Accel=33600;
	ch0->Jerk=5e+09;
	ch0->P=0.016;
	ch0->I=0;
	ch0->D=0;
	ch0->FFAccel=0;
	ch0->FFVel=0;
	ch0->MaxI=0;
	ch0->MaxErr=1e+06;
	ch0->MaxOutput=9000;
	ch0->DeadBandGain=1;
	ch0->DeadBandRange=0;
	ch0->InputChan0=1;
	ch0->InputChan1=0;
	ch0->OutputChan0=1;
	ch0->OutputChan1=0;
	ch0->MasterAxis=-1;
	ch0->LimitSwitchOptions=0x12f;
	ch0->LimitSwitchNegBit=139;
	ch0->LimitSwitchPosBit=138;
	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=ENCODER_MODE;
	ch1->OutputMode=DAC_SERVO_MODE;
	ch1->Vel=430000;
	ch1->Accel=200000;
	ch1->Jerk=5e+09;
	ch1->P=0.3;
	ch1->I=0;
	ch1->D=0;
	ch1->FFAccel=0;
	ch1->FFVel=0;
	ch1->MaxI=0;
	ch1->MaxErr=1e+06;
	ch1->MaxOutput=10000;
	ch1->DeadBandGain=1;
	ch1->DeadBandRange=0;
	ch1->InputChan0=0;
	ch1->InputChan1=0;
	ch1->OutputChan0=0;
	ch1->OutputChan1=0;
	ch1->MasterAxis=-1;
	ch1->LimitSwitchOptions=0x12f;
	ch1->LimitSwitchNegBit=136;
	ch1->LimitSwitchPosBit=137;
	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=ENCODER_MODE;
	ch2->OutputMode=DAC_SERVO_MODE;
	ch2->Vel=400000;
	ch2->Accel=500000;
	ch2->Jerk=4e+09;
	ch2->P=1;
	ch2->I=0;
	ch2->D=0;
	ch2->FFAccel=0;
	ch2->FFVel=0;
	ch2->MaxI=200;
	ch2->MaxErr=1e+06;
	ch2->MaxOutput=5000;
	ch2->DeadBandGain=1;
	ch2->DeadBandRange=0;
	ch2->InputChan0=2;
	ch2->InputChan1=0;
	ch2->OutputChan0=2;
	ch2->OutputChan1=0;
	ch2->MasterAxis=-1;
	ch2->LimitSwitchOptions=0x12f;
	ch2->LimitSwitchNegBit=141;
	ch2->LimitSwitchPosBit=140;
	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=ENCODER_MODE;
	ch3->OutputMode=DAC_SERVO_MODE;
	ch3->Vel=1000;
	ch3->Accel=1500;
	ch3->Jerk=4e+09;
	ch3->P=1.5;
	ch3->I=0;
	ch3->D=0;
	ch3->FFAccel=0;
	ch3->FFVel=0;
	ch3->MaxI=200;
	ch3->MaxErr=1e+06;
	ch3->MaxOutput=5000;
	ch3->DeadBandGain=1;
	ch3->DeadBandRange=0;
	ch3->InputChan0=3;
	ch3->InputChan1=0;
	ch3->OutputChan0=3;
	ch3->OutputChan1=0;
	ch3->MasterAxis=-1;
	ch3->LimitSwitchOptions=0x12a;
	ch3->LimitSwitchNegBit=0;
	ch3->LimitSwitchPosBit=143;
	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=ENCODER_MODE;
	ch4->OutputMode=DAC_SERVO_MODE;
	ch4->Vel=5000;
	ch4->Accel=5000;
	ch4->Jerk=5e+08;
	ch4->P=0.6;
	ch4->I=1e-07;
	ch4->D=0;
	ch4->FFAccel=0;
	ch4->FFVel=0;
	ch4->MaxI=1000;
	ch4->MaxErr=1e+05;
	ch4->MaxOutput=7000;
	ch4->DeadBandGain=1;
	ch4->DeadBandRange=0;
	ch4->InputChan0=4;
	ch4->InputChan1=1;
	ch4->OutputChan0=4;
	ch4->OutputChan1=1;
	ch4->MasterAxis=-1;
	ch4->LimitSwitchOptions=0x100;
	ch4->LimitSwitchNegBit=0;
	ch4->LimitSwitchPosBit=0;
	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=500000000000;
	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;

	ch6->InputMode=ENCODER_MODE;
	ch6->OutputMode=DAC_SERVO_MODE;
	ch6->Vel=200000;
	ch6->Accel=200000;
	ch6->Jerk=4e+09;
	ch6->P=0.05;
	ch6->I=0;
	ch6->D=0;
	ch6->FFAccel=0;
	ch6->FFVel=0;
	ch6->MaxI=0;
	ch6->MaxErr=5e+08;
	ch6->MaxOutput=5000;
	ch6->DeadBandGain=1;
	ch6->DeadBandRange=0;
	ch6->InputChan0=5;
	ch6->InputChan1=1;
	ch6->OutputChan0=5;
	ch6->OutputChan1=1;
	ch6->MasterAxis=-1;
	ch6->LimitSwitchOptions=0x100;
	ch6->LimitSwitchNegBit=0;
	ch6->LimitSwitchPosBit=0;
	ch6->SoftLimitPos=1e+09;
	ch6->SoftLimitNeg=-1e+09;
	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=10000000;
	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;

	ch7->InputMode=ENCODER_MODE;
	ch7->OutputMode=DAC_SERVO_MODE;
	ch7->Vel=200000;
	ch7->Accel=200000;
	ch7->Jerk=4e+09;
	ch7->P=0.05;
	ch7->I=0;
	ch7->D=0;
	ch7->FFAccel=0;
	ch7->FFVel=0;
	ch7->MaxI=0;
	ch7->MaxErr=5e+08;
	ch7->MaxOutput=5000;
	ch7->DeadBandGain=1;
	ch7->DeadBandRange=0;
	ch7->InputChan0=6;
	ch7->InputChan1=1;
	ch7->OutputChan0=6;
	ch7->OutputChan1=1;
	ch7->MasterAxis=-1;
	ch7->LimitSwitchOptions=0x100;
	ch7->LimitSwitchNegBit=0;
	ch7->LimitSwitchPosBit=0;
	ch7->SoftLimitPos=1e+09;
	ch7->SoftLimitNeg=-1e+09;
	ch7->InputGain0=1;
	ch7->InputGain1=1;
	ch7->InputOffset0=0;
	ch7->InputOffset1=0;
	ch7->OutputGain=1;
	ch7->OutputOffset=0;
	ch7->SlaveGain=1;
	ch7->BacklashMode=BACKLASH_OFF;
	ch7->BacklashAmount=0;
	ch7->BacklashRate=0;
	ch7->invDistPerCycle=1;
	ch7->Lead=0;
	ch7->MaxFollowingError=10000000;
	ch7->StepperAmplitude=250;

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

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

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

    EnableAxis(0);
	EnableAxis(1);
	EnableAxis(2);
	EnableAxis(3);	// A AXIS
	EnableAxis(4);	// B AXIS
	EnableAxis(5);	
	EnableAxis(6);
	EnableAxis(7);
	
    DefineCoordSystem8(0,1,2,3,4,-1,6,7);
    
	double Pot,LastSpeed_X=0.0,LastSpeed_Y=0.0,LastSpeed_Z=0.0,LastSpeed_B=0.0,AxisU,AxisV,Ares,Bres,Vres,Ures,T2,T3;
	char p[100],r[200],s[200];
	int result;
	
	DoPCInt(PC_COMM_GETAXISRES, 150);
	
	Ares = *(float *)&persist.UserData[153];
	Bres = *(float *)&persist.UserData[154];
	Ures = 886;
	Vres = 886;
	
	//printf("XRes=%f\n", *(float *)&persist.UserData[150]);
	//printf("YRes=%f\n", *(float *)&persist.UserData[151]);
	//printf("ZRes=%f\n", *(float *)&persist.UserData[152]);
	//printf("ARes=%f\n", *(float *)&persist.UserData[153]);
	//printf("BRes=%f\n", *(float *)&persist.UserData[154]);
	//printf("CRes=%f\n", *(float *)&persist.UserData[155]);
	//printf("URes=%f\n", *(float *)&persist.UserData[156]);
	//printf("VRes=%f\n", *(float *)&persist.UserData[157]);

	//printf("%f\n",KANALOG_CONVERT_ADC_TO_VOLTS(ADC(0)));
	T3=Time_sec();
	for (;;)
	{
		T1=WaitNextTimeSlice();
		//T2=Time_sec()-T3;

		// Handle ESTOP_BIT
		result = Debounce(ReadBit(ESTOP_BIT),&ecount,&elast,&elastsolid);
		if  (result == 1)
		{
			ClearBit(151);
			DoPC(PC_COMM_ESTOP);
			if (ch0->Enable) DisableAxis(0);
			if (ch1->Enable) DisableAxis(1);
			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
			if (ch6->Enable) DisableAxis(6);  // axis still enabled?  - Disable it
			if (ch7->Enable) DisableAxis(7);  // axis still enabled?  - Disable it
		}

		if (T1-T0 > 0.05)  // only change speed every so often
		{
			if(ch3->Enable){
				SetBit(153);
			}	
			else if(!ch3->Enable){
				ClearBit(153);
			}
			if(ch4->Enable){
				SetBit(154);
			}	
			else if(!ch4->Enable){
				ClearBit(154);
			}
			// Check the position error of axis A
			if (fast_fabs(ch3->Position - ch3->Dest) > 100.0) 
				SetBit(1060);
			else ClearBit(1060);
			// Check the position error of axis B
			if (fast_fabs(ch4->Position - ch4->Dest) > 100.0) 
				SetBit(1061);
			else ClearBit(1061);
			
			if(ReadBit(DRIVE_X_ERROR)){
				sprintf(p,"DRIVE X ERROR");
				DROLabel(1400, 174, p);				
			}
			//else if(!ReadBit(DRIVE_X_ERROR)){
			//	sprintf(p,"");
			//	DROLabel(1400, 174, p);							
			//}
			if(ReadBit(DRIVE_Y_ERROR)){
				sprintf(p,"DRIVE Y ERROR");
				DROLabel(1400, 174, p);				
			}
			if(ReadBit(DRIVE_Z_ERROR)){
				sprintf(p,"DRIVE Z ERROR");
				DROLabel(1400, 174, p);				
			}
			if(ReadBit(DRIVE_A_ERROR)){
				sprintf(p,"DRIVE A ERROR");
				DROLabel(1400, 174, p);				
			}
			if(ReadBit(DRIVE_B_ERROR)){
				sprintf(p,"DRIVE B ERROR");
				DROLabel(1400, 174, p);				
			}
			//if(ReadBit(DRIVE_U_ERROR)){
				//sprintf(p,"DRIVE U ERROR");
				//DROLabel(1400, 174, p);				
			//}
			//if(ReadBit(DRIVE_V_ERROR)){
				//sprintf(p,"DRIVE V ERROR");
				//DROLabel(1400, 174, p);				
			//}


			AxisU = ch6->Position;			
			AxisV = ch7->Position;
			sprintf(s,"%3.1f",AxisU);
			sprintf(r,"%3.1f",AxisV);
			DROLabel(1000, 170, s);
			DROLabel(1200, 172, r);
			
			if(ReadBit(BPLUS)){
					//SetBit(154);
					EnableAxis(4);
					Delay_sec(2.0);
					Jog(4,5000);
			}
			if(ReadBit(BMINUS)){
					//SetBit(154);
					EnableAxis(4);	
					Delay_sec(2.0);
					Jog(4,-5000);
			}
			
			if(!ReadBit(BPLUS) && !ReadBit(BMINUS) && !ReadBit(48) && !ReadBit(49)){
				Jog(4,0.0);
				DisableAxis(4);
			}
			//if(ReadBit(49) && ReadBit(50) && CheckDone(4)) ClearBit(55);
			
			DoSpeedAxis(VAXIS, VPLUS, VMINUS, &LastSpeed_Z, MAX_SPEED_V);
			DoSpeedAxis(UAXIS, UPLUS, UMINUS, &LastSpeed_X, MAX_SPEED_U);
			
			if(ReadBit(GOTOZERO)){
				DoGoToZero();
			}
			if(ReadBit(1040)){
				Angle_step = 0.1;
			}
			if(ReadBit(1041)){
				Angle_step = 0.5;
			}		
			if(ReadBit(1042)){
				Angle_step = 1.0;
			}		
			if(ReadBit(1043)){
				Angle_step = 5.0;
			}		
			if(ReadBit(1044)){
				Angle_step = 10.0;
			}
			if(ReadBit(1045)){
				Angle_step = 15.0;
			}		
			if(ReadBit(1046)){
				Angle_step = 30.0;
			}		
			if(ReadBit(1047)){
				Angle_step = 45.0;
			}		
			if(ReadBit(1048)){
				Angle_step = 90.0;
			}
			if(ReadBit(1049)){
				MoveRel(3,-Angle_step*Ares);
				ClearBit(1049);
			}
			if(ReadBit(1050)){
				MoveRel(3,Angle_step*Ares);
				ClearBit(1050);
			}
			// B Axis step movemend
			if(ReadBit(1051)){
				SetBit(154);
				EnableAxis(4);
				Delay_sec(1.0);
				MoveRel(4,Angle_step*Bres);
				SetBit(48);
				ClearBit(1051);
			}
			if(ReadBit(1052)){
				SetBit(154);
				EnableAxis(4);
				Delay_sec(1.0);
				MoveRel(4,-Angle_step*Bres);
				SetBit(48);
				ClearBit(1052);
			}
			if(CheckDone(4)) ClearBit(48); 
			
			// U Axis step movement
			if(ReadBit(1056)){
				MoveRel(7,5*Ures);
				ClearBit(1056);
			}
			if(ReadBit(1057)){
				MoveRel(7,-5*Ures);
				ClearBit(1057);
			}
			if(ReadBit(1053)){
				DoGoTo();
			}
		}
	}
}

void DoSpeedAxis(int ch, int Plus, int Minus, double *LastSpeed, double Speed)
{
	if (ReadBit(Plus))  //toggle to plus direction?
	{
		Jog(ch,Speed);
		*LastSpeed = Speed;
		T0=T1;  // save last time speed was changed		
	}
	else if (ReadBit(Minus)) //toggle to minus direction
	{
		Jog(ch,-Speed);
		*LastSpeed = -Speed;
		T0=T1; // save last time speed was changed
	}
	else  //neither toggle so stop, if axis isn't already
	{
		//if (*LastSpeed != 0.0) Jog(ch,0.0);
		//*LastSpeed=0.0;
		Jog(ch,0.0);
		T0=T1;
	}
}    
    
void DoGoToZero(void){
	MDI("G0X0Y0");
	ClearBit(61);
	T0=T1;
}    

void DoGoTo(void){
	Answer = InputBox("Enter Distance to go X",&value);
	printf(".\n");
	if (Answer)
		printf("Operator Canceled\n");
	else
		printf("Operator Entered %.3f\n",value);
		//sprintf(p,"G0X%3.1f",value);
		//printf("String %s\n",p);
		//MDI(p);
		ClearBit(1053);
}

// 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;
}

// Debounce a bit
//
// return 1 one time when first debounced high 
// return 0 one time when first debounced low 
// return -1 otherwise 
#define DBTIME 300

int Debounce(int n, int *cnt, int *last, int *lastsolid)
{
	int v = -1;
	
	if (n == *last)  // same as last time?
	{
		if (*cnt == DBTIME-1)
		{
			if (n != *lastsolid)
			{
				v = *lastsolid = n;  // return debounced value
			}
		}
		if (*cnt < DBTIME)	(*cnt)++;
	}
	else
	{
		*cnt = 0;  // reset count
	}
	*last = n;
	return v;
}