Probe Response Delay Mach3

Moderators: TomKerekes, dynomotion

User avatar
TomKerekes
Posts: 2529
Joined: Mon Dec 04, 2017 1:49 am

Re: Probe Response Delay Mach3

Post by TomKerekes » Fri Jun 25, 2021 3:47 pm

Hi Jerry,

I can't think of any other sources of delay.

Since you have a scope you might use the code below to set an output high at the exact time the z position is sampled internally in KFLOP. I coded for bit 16 but some other bit might be more convenient for you. Also the sampled Destination in absolute steps is printed to the console. This should help us figure out what is going on. So after "Zeroing" manually with the MPG and Probe record the KFLOP Z Machine position in Steps from the KMotion Axis Screen. Then run a Probe and we should have 3 things to compare:

#1 Manual MPG based Probe Trip point
#2 Probed Trip Point (from Console)
#3 Final Position after moving back to Trip Point

Also please post the resolution of your Z axis

Code: Select all

	if (msg==20000) 
	{
		double *d = (double *)&persist.UserData[MACH3_PROBE_RESULTS_VAR];
		int flag=1;
		
		persist.UserData[MACH3_PROBE_STATUS_VAR]=PROBE_ERROR_HANDLING;

		SetBitDirection(16,1);  // set as output for scope pulse
		ClearBit(16);  // set low
		while (ReadBit(PROBE_BIT)!=PROBE_ACTIVE_STATE)
		{
			flag=2;  
			WaitNextTimeSlice();
		}
		SetBit(16);  // trigger for scope
		
		if (CS0_axis_x>=0) d[0]=chan[CS0_axis_x].Dest;
		if (CS0_axis_y>=0) d[1]=chan[CS0_axis_y].Dest;
		if (CS0_axis_z>=0) d[2]=chan[CS0_axis_z].Dest;
		if (CS0_axis_a>=0) d[3]=chan[CS0_axis_a].Dest;
		if (CS0_axis_b>=0) d[4]=chan[CS0_axis_b].Dest;
		if (CS0_axis_c>=0) d[5]=chan[CS0_axis_c].Dest;
		
		persist.UserData[MACH3_PROBE_STATUS_VAR]=flag;
		StopCoordinatedMotion();
		printf("Z Probe = %f\n", d[2]);
	}
Regards,

Tom Kerekes
Dynomotion, Inc.

macona
Posts: 39
Joined: Tue Sep 15, 2020 4:44 am

Re: Probe Response Delay Mach3

Post by macona » Sat Jun 26, 2021 1:08 am

OK, ill probably do it next week sometime. My shop is going to be murder with the temps the next few days. Supposed to hit 111 Sunday.

macona
Posts: 39
Joined: Tue Sep 15, 2020 4:44 am

Re: Probe Response Delay Mach3

Post by macona » Thu Jul 01, 2021 6:32 am

Finally cooled down, my shop hit 107.5 inside, Not fun.

So... Its working perfectly. I added your lines, hooked the scope up and everything seems to work fine. Well, most of the time. When I hit start to run the program:

G31 Z-4.0 F4 (Probe in Z)
G1 Z #2002 (move back to trip point)
M30

Sometimes the DRO in Mach 3 gets zeroed out before the move and then it starts counting down the distance. So Lets say the probe is against the surface and zeroed out. I move .1" above the surface and run the program. Normally the DRO will count down from there and trigger and then go to the trigger zero point, usually 0 +/-.0003". Sometimes, maybe 1 out of 20, the DRO will zero out and then count down and end up -.1 +/-.0003". But it is still stopped at the same point mechanically.

Using this mill I have never seen behavior like this before in normal use.

The timing results of your test, saw some longer times and shorter, this one was kind of in between. Yellow is trigger input and pink is IO16 output.

ImageProbe hit kflop out by Jerry Biehler, on Flickr

User avatar
TomKerekes
Posts: 2529
Joined: Mon Dec 04, 2017 1:49 am

Re: Probe Response Delay Mach3

Post by TomKerekes » Thu Jul 01, 2021 3:03 pm

Timing looks to be as expected and negligible.

I don't know what that could be. You might do as I requested in my last post for a clue to what is shifting. You might also look at Mach3 Offsets to see if anything changed before after.
Regards,

Tom Kerekes
Dynomotion, Inc.

macona
Posts: 39
Joined: Tue Sep 15, 2020 4:44 am

Re: Probe Response Delay Mach3

Post by macona » Fri Jul 02, 2021 3:25 am

Messing around again and did take your info. And it does change my Z offset in mach when it does what it does. Its also doing a couple other things. First, it seems like the klop pauses for a fraction of a second and then delivers all the pulse at once causing the z axis to jump. It only does this while probing. I actually got an error on the servo drive because the input frequency of the pulse stream was too high (over 220khz). I programmed a whole bunch if incremental z moves with the same parameters and it never does it in a normal program, just when the probing routine is running.

Next new issue is that is found touch point, paused, and then just kept going. Had to slam the e-stop before I crushed the probe.

Took some video first one is it operating normal and correctly until 0:22 where the Z jumps, the noise you hear. I touched off, zero, MPG away from the probe and hit start and it touches off. Repeat.



Second video is if where it is operating as it should and then at the end it does two issues at once. It resets Z to 0 at the start of the program, then touches, pauses, and just keeps going. You can hear it pause when it touches.



This is the console data from that last run in the video:

Mach3 Notify Call, Message = 20000
Z Probe = 918.139290
Mach3 Notify Call, Message = 20000
Z Probe = 917.985604
Mach3 Notify Call, Message = 20000
Z Probe = 917.602598
Mach3 Notify Call, Message = 20000
Z Probe = -786.605708
Mach3 Notify Message=4, Direction= 0, Spindle Set to 0.000000
Spindle Stop

10000 steps per inch. Stopping at this point. Until I have a better idea what's going on I could destroy my probe so need to come up with a way to protect it.

User avatar
TomKerekes
Posts: 2529
Joined: Mon Dec 04, 2017 1:49 am

Re: Probe Response Delay Mach3

Post by TomKerekes » Fri Jul 02, 2021 10:39 pm

I'm not really able to follow all that. But I'm thinking it may have to do with the MPG program. After you move with the MPG and the MPG is converging on the final position after some time period (usually configured for 1 second) it then stops converging to the final position and performs a move to the final position. Now if you command a Probe during this time things could get messed up. Try Halting the MPG program before Probing (and Zeroing) and see if the problem still occurs.
Regards,

Tom Kerekes
Dynomotion, Inc.

macona
Posts: 39
Joined: Tue Sep 15, 2020 4:44 am

Re: Probe Response Delay Mach3

Post by macona » Sun Jul 04, 2021 12:28 am

What would be a good way to do that? Not let the probe program run until the MPG has completed it's move?

macona
Posts: 39
Joined: Tue Sep 15, 2020 4:44 am

Re: Probe Response Delay Mach3

Post by macona » Sun Jul 04, 2021 12:48 am

This is my current Setup and MPG

Code: Select all

#include "KMotionDef.h"

// Defines axis 0, 1, 2 as simple step dir outputs
// enables them
// sets them as an xyz coordinate system for GCode

#define DISABLE_DELAY 5.0		// seconds after host stops to disable the thing

	//MPG HANDLER

	// Example Function as "smooth" MPG motion example
	// which makes use of the exponential motion command.
	// Additionally double Filtered to be very smoot with limited Jerk


#define TAU  0.01				// smoothness factor (Low Pass Time constant seconds for MoveExp)
#define TAU1 0.015				// smoothness factor (Low Pass Time constant seconds for pre filter 1)
#define TAU2 0.015				// smoothness factor (Low Pass Time constant seconds for pre filter 2)
#define FINAL_TIME 1.0			// Set final dest after this amount of time with no change

#define MPG_INPUT_AXIS 5		// Axis used to read hardware encoder (not necessarily the encoder channel)

#define increment 7
#define machAxis 8


#define XSTEPS 10240			// X Axis Steps Per Inch
#define YSTEPS 10240			// Y Axis Steps Per Inch
#define ZSTEPS 10000			// Z Axis Steps Per Inch
#define ASTEPS 400			// A Axis Steps Per Degree
#define BSTEPS 1000				// B Axis Steps Per Degree
#define CSTEPS -11.3777777				// C Axis Steps Per Degree

void ServiceMPG(void);

main()
{
	InitAux();
	AddKonnect(0, &VirtualBits, VirtualBitsEx);

	ch0->InputMode = NO_INPUT_MODE;
	ch0->OutputMode = STEP_DIR_MODE;
	ch0->Vel = 40000;
	ch0->Accel = 400000;
	ch0->Jerk = 4e+006;
	ch0->P = 0;
	ch0->I = 0.01;
	ch0->D = 0;
	ch0->FFAccel = 0;
	ch0->FFVel = 0;
	ch0->MaxI = 200;
	ch0->MaxErr = 1e+006;
	ch0->MaxOutput = 200;
	ch0->DeadBandGain = 1;
	ch0->DeadBandRange = 0;
	ch0->InputChan0 = 1;
	ch0->InputChan1 = 0;
	ch0->OutputChan0 = 8;
	ch0->OutputChan1 = 0;
	ch0->MasterAxis = -1;
	ch0->LimitSwitchOptions = 0x11f;
	ch0->LimitSwitchNegBit = 1027;
	ch0->LimitSwitchPosBit = 1027;
	ch0->SoftLimitPos = 1e+009;
	ch0->SoftLimitNeg = -1e+009;
	ch0->InputGain0 = 1;
	ch0->InputGain1 = 1;
	ch0->InputOffset0 = 0;
	ch0->InputOffset1 = 0;
	ch0->OutputGain = 1;
	ch0->OutputOffset = 0;
	ch0->SlaveGain = 1;
	ch0->BacklashMode = BACKLASH_LINEAR;
	ch0->BacklashAmount = 20;
	ch0->BacklashRate = 200000;
	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.000769;
	ch0->iir[2].B1 = 0.001538;
	ch0->iir[2].B2 = 0.000769;
	ch0->iir[2].A1 = 1.92081;
	ch0->iir[2].A2 = -0.923885;
	EnableAxisDest(0, 0);

	ch1->InputMode = NO_INPUT_MODE;
	ch1->OutputMode = STEP_DIR_MODE;
	ch1->Vel = 40000;
	ch1->Accel = 400000;
	ch1->Jerk = 4e+006;
	ch1->P = 0;
	ch1->I = 0.01;
	ch1->D = 0;
	ch1->FFAccel = 0;
	ch1->FFVel = 0;
	ch1->MaxI = 200;
	ch1->MaxErr = 1e+006;
	ch1->MaxOutput = 200;
	ch1->DeadBandGain = 1;
	ch1->DeadBandRange = 0;
	ch1->InputChan0 = 1;
	ch1->InputChan1 = 0;
	ch1->OutputChan0 = 9;
	ch1->OutputChan1 = 0;
	ch1->MasterAxis = -1;
	ch1->LimitSwitchOptions = 0x11f;
	ch1->LimitSwitchNegBit = 1028;
	ch1->LimitSwitchPosBit = 1028;
	ch1->SoftLimitPos = 1e+009;
	ch1->SoftLimitNeg = -1e+009;
	ch1->InputGain0 = 1;
	ch1->InputGain1 = 1;
	ch1->InputOffset0 = 0;
	ch1->InputOffset1 = 0;
	ch1->OutputGain = 1;
	ch1->OutputOffset = 0;
	ch1->SlaveGain = 1;
	ch1->BacklashMode = BACKLASH_LINEAR;
	ch1->BacklashAmount = 37;
	ch1->BacklashRate = 200000;
	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;
	EnableAxisDest(1, 0);

	ch2->InputMode = NO_INPUT_MODE;
	ch2->OutputMode = STEP_DIR_MODE;
	ch2->Vel = 40000;
	ch2->Accel = 400000;
	ch2->Jerk = 4e+006;
	ch2->P = 0;
	ch2->I = 0.01;
	ch2->D = 0;
	ch2->FFAccel = 0;
	ch2->FFVel = 0;
	ch2->MaxI = 200;
	ch2->MaxErr = 1e+006;
	ch2->MaxOutput = 200;
	ch2->DeadBandGain = 1;
	ch2->DeadBandRange = 0;
	ch2->InputChan0 = 2;
	ch2->InputChan1 = 0;
	ch2->OutputChan0 = 10;
	ch2->OutputChan1 = 0;
	ch2->MasterAxis = -1;
	ch2->LimitSwitchOptions = 0x11f;
	ch2->LimitSwitchNegBit = 1029;
	ch2->LimitSwitchPosBit = 1029;
	ch2->SoftLimitPos = 1e+009;
	ch2->SoftLimitNeg = -1e+009;
	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;
	EnableAxisDest(2, 0);

	ch3->InputMode = NO_INPUT_MODE;
	ch3->OutputMode = STEP_DIR_MODE;
	ch3->Vel = 70000;
	ch3->Accel = 100000;
	ch3->Jerk = 4e+006;
	ch3->P = 0;
	ch3->I = 0.01;
	ch3->D = 0;
	ch3->FFAccel = 0;
	ch3->FFVel = 0;
	ch3->MaxI = 200;
	ch3->MaxErr = 1e+006;
	ch3->MaxOutput = 200;
	ch3->DeadBandGain = 1;
	ch3->DeadBandRange = 0;
	ch3->InputChan0 = 8;
	ch3->InputChan1 = 0;
	ch3->OutputChan0 = 11;
	ch3->OutputChan1 = 0;
	ch3->MasterAxis = -1;
	ch3->LimitSwitchOptions = 0x100;
	ch3->LimitSwitchNegBit = 0;
	ch3->LimitSwitchPosBit = 0;
	ch3->SoftLimitPos = 1e+009;
	ch3->SoftLimitNeg = -1e+009;
	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;
	EnableAxisDest(3, 0);

	ch4->InputMode = NO_INPUT_MODE;
	ch4->OutputMode = STEP_DIR_MODE;
	ch4->Vel = 40000;
	ch4->Accel = 400000;
	ch4->Jerk = 4e+006;
	ch4->P = 0.2;
	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 = 4;
	ch4->InputChan1 = 0;
	ch4->OutputChan0 = 12;
	ch4->OutputChan1 = 0;
	ch4->MasterAxis = -1;
	ch4->LimitSwitchOptions = 0x100;
	ch4->LimitSwitchNegBit = 0;
	ch4->LimitSwitchPosBit = 0;
	ch4->SoftLimitPos = 1e+009;
	ch4->SoftLimitNeg = -1e+009;
	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;
	EnableAxisDest(4, 0);

	ch5->InputMode = ENCODER_MODE;
	ch5->OutputMode = NO_OUTPUT_MODE;
	ch5->Vel = 40000;
	ch5->Accel = 400000;
	ch5->Jerk = 4e+006;
	ch5->P = 0.2;
	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 = 0;
	ch5->InputChan1 = 1;
	ch5->OutputChan0 = 5;
	ch5->OutputChan1 = 1;
	ch5->MasterAxis = -1;
	ch5->LimitSwitchOptions = 0x100;
	ch5->LimitSwitchNegBit = 0;
	ch5->LimitSwitchPosBit = 0;
	ch5->SoftLimitPos = 1e+009;
	ch5->SoftLimitNeg = -1e+009;
	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;



	DefineCoordSystem(0, 1, 2, 4);


	for (;;)
	{
		//MPG HANDLER

		ServiceMPG();
		WaitNextTimeSlice();
		static int PrevStatusRequestCounter = -1;	// TOFIX: what about overflow?
		static double LastRequestTime = 0.;
		SetBitDirection(60, 1);
		SetBitDirection(63, 1);
		if (StatusRequestCounter != PrevStatusRequestCounter)
		{						// if there has been a request
			LastRequestTime = Time_sec();	// remember time of this change

			SetBit(63);			// ENABLE / Set HERE // NB: setting a bit that's already set *may* cause problems, if so then make this conditional upon the current status

			// remember the StatusRequestCounter for the next time around
			PrevStatusRequestCounter = StatusRequestCounter;
		}
		else
		{						// there hasn't been a request
			// has it been long enough since the last request?
			if ((Time_sec() - LastRequestTime) > DISABLE_DELAY)
			{
				ClearBit(60);	// Trigger servo estop
				Delay_sec(2);	// Wait for servos
				ClearBit(63);	// Shut down main power
				
			}
		}
	}
}

						//MPG HANDLER

		// Example Function as "smooth" MPG motion example
		// which makes use of the exponential motion command.
		// Additionally double Filtered to be very smoot with limited Jerk
void ServiceMPG(void)
{
	double IncrementValue = *(double *)&persist.UserData[(increment - 1) * 2];	// Get increment value
	double AxisSteps;
	static int Pos, FirstTime = TRUE;
	static int InMotion = FALSE, Axis, LastAxis = -1;
	static double LastChangeTime = 0, Target, Factor = 0;
	static double Target1, Target2, K1, K2, K1M, K2M;
	static double Td;

	int Change1, NewPos;

	if (FirstTime)
	{
		Pos = chan[MPG_INPUT_AXIS].Position;
		K1 = exp(-2 * TIMEBASE / TAU1);	// filter coefficients
		K2 = exp(-2 * TIMEBASE / TAU2);
		K1M = 1.0 - K1;
		K2M = 1.0 - K2;
		FirstTime = FALSE;
		Td=Time_sec();
	}

	NewPos = chan[MPG_INPUT_AXIS].Position;
	Change1 = NewPos - Pos;
	Pos = NewPos;

	Axis = *(double *)&persist.UserData[(machAxis - 1) * 2];
	
	if (Axis == 6 || JOB_ACTIVE)	// if not button pressed or Job Active ignore the encoder.
		Change1 = 0;

	if (Axis == 0)				// Select X Steps per unit
		AxisSteps = XSTEPS;
	else if (Axis == 1)			// Select Y Steps per unit
		AxisSteps = YSTEPS;
	else if (Axis == 2)			// Select Z Steps per unit
		AxisSteps = ZSTEPS;
	else if (Axis == 4)			// Select A Steps per unit
		AxisSteps = ASTEPS;
	else if (Axis == 5)			// Select B Steps per unit
		AxisSteps = BSTEPS;
	else if (Axis == 3)			// Select C Steps per unit
		AxisSteps = CSTEPS;


	Factor = (AxisSteps * IncrementValue) / 4;

	//if (Time_sec() > Td + .25)
	//{
	//	Td = Time_sec();
	//	printf("Axis = %d, Pos = %d, NewPos = %d, Change1 = %d\n",Axis, Pos, NewPos, Change1);
	//}

	// Feedhold fully stopped ??
	if (CS0_StoppingState == 4 && InMotion)
	{
		Change1 = 0;			// ignore any MPG change
		Jog(LastAxis, 0);
		InMotion = FALSE;
	}

	// check if the Axis just changed or we have been 
	// converging to the target for a long time
	if (Axis != LastAxis || (InMotion && Time_sec() > LastChangeTime + FINAL_TIME))
	{
		if (InMotion)
			Move(LastAxis, Target1);	//finalize any motion

		LastAxis = Axis;
		InMotion = FALSE;
	}

	if (Change1)				// did we move?
	{
		if (!InMotion)
			Target = Target1 = Target2 = chan[Axis].Dest;
		Target1 += Change1 * Factor;
		LastChangeTime = Time_sec();
		InMotion = TRUE;
	}

	if (InMotion)				// If moving
	{
		if (Target1 > chan[LastAxis].SoftLimitPos)
			Target1 = chan[LastAxis].SoftLimitPos;
		if (Target1 < chan[LastAxis].SoftLimitNeg)
			Target1 = chan[LastAxis].SoftLimitNeg;
		Target2 = Target2 * K1 + Target1 * K1M;
		Target = Target * K2 + Target2 * K2M;
		MoveExp(Axis, Target, TAU);
	}
}
This is the notify with the probe

Code: Select all

#include "KMotionDef.h"

//Plugin calls for Mach3 NotifyPlugins Commands
#define X 0
#define Y 1
#define Z 2

float DecelTimeForAxis(int axis);
main()
{


	//if (msg==10084)
		//{
			//Tap();
		//}
		
	int msg = persist.UserData[6];  // Mach3 notify Message 10000-10999

	printf("Mach3 Notify Call, Message = %d\n",msg); 
	
	if (msg==10100)
	{
		// adjust the commanded position to match the glass scale encoder
		DisableAxis(X);
		EnableAxisDest(X,chan[X].Position);
	}
	if (msg==10101)
	{
		// adjust the commanded position to match the glass scale encoder
		DisableAxis(Y);
		EnableAxisDest(Y,chan[Y].Position);
	}
	if (msg==10102)
	{
		// adjust the commanded position to match the glass scale encoder
		DisableAxis(Z);
		EnableAxisDest(Z,chan[Z].Position);
	}
	if (msg==10500)
	{
		if (CS0_StoppingState == 0)
			StopCoordinatedMotion();
		else
			ResumeCoordinatedMotion();
	}
	if (msg==10300)
	{
		// User wants to disable Z (switch to OL)
		
		DisableAxis(Z);
		chan[Z].OutputMode = STEP_DIR_MODE;
		
		EnableAxisDest(Z,chan[Z].Position);
	}
	if (msg==10301)
	{          
		// User wants to enable Z (switch to CL)
		DisableAxis(Z);
		chan[Z].OutputMode = CL_STEP_DIR_MODE;
		
		EnableAxisDest(Z,chan[Z].Position);
	}

	// handles probing
	//
	// flag is 0 - while watching for probe hit
	// flag is 1 - if probe was already set from start
	// flag is 2 - after successful probe hit
	// flag is 3 - Tells Plugin to upload status (3) to
	//             DRO 1100 and let User handle the error
	//
	// returns the captured results in User Variables
	// X - 50+51
	// Y - 52+53
	// Z - 54+55
	// A - 56+57
	// B - 58+59
	// C - 60+61
	// status result 62
	
	#define PROBE_BIT 44
	#define PROBE_ACTIVE_STATE 0
	#define PROBE_ERROR_HANDLING 0  // 0 Stops Mach3 on probe error
//	#define PROBE_ERROR_HANDLING 3  // 3 User must query DRO 1100 and handle error
	
	if (msg==20000) 
	{
		double *d = (double *)&persist.UserData[MACH3_PROBE_RESULTS_VAR];
		int flag=1;
		
		persist.UserData[MACH3_PROBE_STATUS_VAR]=PROBE_ERROR_HANDLING;

		SetBitDirection(16,1);  // set as output for scope pulse
		ClearBit(16);  // set low
       
		while (ReadBit(PROBE_BIT)!=PROBE_ACTIVE_STATE)
		{
			flag=2;  
			WaitNextTimeSlice();
		}
		SetBit(16);  // trigger for scope
		
		if (CS0_axis_x>=0) d[0]=chan[CS0_axis_x].Dest;
		if (CS0_axis_y>=0) d[1]=chan[CS0_axis_y].Dest;
		if (CS0_axis_z>=0) d[2]=chan[CS0_axis_z].Dest;
		if (CS0_axis_a>=0) d[3]=chan[CS0_axis_a].Dest;
		if (CS0_axis_b>=0) d[4]=chan[CS0_axis_b].Dest;
		if (CS0_axis_c>=0) d[5]=chan[CS0_axis_c].Dest;
		
		persist.UserData[MACH3_PROBE_STATUS_VAR]=flag;
		StopCoordinatedMotion();
		printf("Z Probe = %f\n", d[2]);
	}
}

	

macona
Posts: 39
Joined: Tue Sep 15, 2020 4:44 am

Re: Probe Response Delay Mach3

Post by macona » Sun Jul 04, 2021 2:00 am

I deleted the MPG code from my startup .c and that seems to work.

One thing I found is it will happily run the probe program with the probe input already active. What would be a good way to stop that from happening. Probably just stop the program and notify that probe is active.

User avatar
TomKerekes
Posts: 2529
Joined: Mon Dec 04, 2017 1:49 am

Re: Probe Response Delay Mach3

Post by TomKerekes » Mon Jul 05, 2021 4:23 pm

I can't think of a simple way to hold off running a Job (Probe) in Mach3 until any MPG motion is complete. Maybe reduce the FINAL_TIME from 1 second to 0.3 second to make it almost impossible.

Regarding Probe already active: That should be detected and return a status of 2 to the plugin. You might add a printf to the Notify Program to verify this is happening. The status message of "Probe Active at start Error" should then be displayed and then the Plugin does DoButton(1003); which should be the Mach3 Stop Button.

What version of KMotion are you running?
Regards,

Tom Kerekes
Dynomotion, Inc.

Post Reply