MPG with Mach3 using mach3 controls

[macona]

Ahh, I see. I will try to put something together and hopefully it all works.

Thanks!

-Jerry

[macona]

would you take a look at these files and see if I did it right? (I appended the macro pump with a .c so it would attach)

-Jerry

macropump.c

(541 Bytes) Downloaded 90 times

SupermaxMPGServiceSmoothHardwareEncFilteredRev3.c

(2.79 KiB) Downloaded 84 times

[TomKerekes]

Hi Jerry,

The macropump seems reasonable.

But the MPG program is using "increment" which is the variable index number where the Basic program put the value, not the value of the variable.

To get the value of the increment do:

Code: Select all

	double IncrementValue = *(double *)&persist.UserData[(increment -1)*2];

Now you can remove the if/else code that used to check all the inputs and compute the "Factor" directly with a formula. Since KFLOP moves in motor steps you will need to know how many motor steps per inch your system has. For example if your system has 2000 steps/inch and the increment is 0.01 inches then the Factor should be 20 steps per MPG click. So code something like:

Code: Select all

Factor = 2000.0 * IncrementValue;

Similarly with the Axis. The Basic program put the Axis number in the variable, so no need to do any 'if' testing to determine what the axis is. Just get the value with:

Code: Select all

	Axis  = *(double *)&persist.UserData[(machAxis-1)*2];

In case you are interested the code looks so confusing because the UserData variables are defined as 32 bit integer values, but Mach3 DROs are 64-bit double precision floating point values. So a pair of UserData variables is being used. Also Mach3 DROs start with 1, but UserData variables start with 0. So the conversion process is:

-1 correct for the 0/1 difference
*2 index by pairs
& get the memory address of the pair of integers
(double *) declare it as the address of a double
* get the value at that address
Axis = save as an integer into Axis


Although you will be removing the 'if' statements you should be aware that in C Programs '=' is used for assignment. '==' must be used for comparing.

[macona]

OK, lets try this. My axis have different steps per unit because of belting, motors, etc. So I repurposed the if sequence to select the steps based on the position.

Also, the mpg encoder is 100PPR so there are 400 edge transitions per rotation, do I need to factor that in and divide by 4?

Code: Select all

// 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 MPG_INPUT_AXIS 0  // Axis used to read hardware encoder (not necessarily the encoder channel)

#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 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 46574.81 // A Axis Steps Per Degree
#define BSTEPS 1000 // B Axis Steps Per Degree
#define CSTEPS 1500 // C Axis Steps Per Degree

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;

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

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

	if (machAxis < 6 || JOB_ACTIVE)	// if not button pressed or Job Active ignore the encoder.
		Change1 = 0;

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


	Factor = AxisSteps * IncrementValue;
	Axis  = *(double *)&persist.UserData[(machAxis-1)*2];

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

[TomKerekes]

Hi Jerry,

Very close but machAxis, which is always the number 8, is now still being tested instead of the Axis value that was passed into Variable 8. Note you will also need to get the Axis value before testing it to set the steps per inch.

No I don’t think you need to divide by 4. Each click of the mpg should be counted and should cause one increment of motion.

[macona]

Like this?

-Jerry

Code: Select all

// 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 MPG_INPUT_AXIS 0  // Axis used to read hardware encoder (not necessarily the encoder channel)

#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 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 46574.81 // A Axis Steps Per Degree
#define BSTEPS 1000 // B Axis Steps Per Degree
#define CSTEPS 1500 // C Axis Steps Per Degree

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;

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

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

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

	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;
	

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

[TomKerekes]

Hi Jerry,

Yes but missed one spot:

if (machAxis < 6 || JOB_ACTIVE) // if not button pressed or Job Active ignore the encoder.

[macona]

OK, fixed that. Now to see if it works. I am a bit confused by how to set up the init.c file for the encoder. I have the MPG on pins for Enc 0 A-B and can see them go active in the Digital IO windows. How do I set that up in the init.c? Do I add it to the ch0 section where I also have the motor drive setup?

-Jerry

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



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=30000;
	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=1;
	ch4->OutputChan0=4;
	ch4->OutputChan1=1;
	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);


	DefineCoordSystem(0,1,2,4);

    for (;;)
	{
		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
		}
	}
	}
}

[TomKerekes]

Hi Jerry,

I am a bit confused by how to set up the init.c file for the encoder. I have the MPG on pins for Enc 0 A-B and can see them go active in the Digital IO windows. How do I set that up in the init.c? Do I add it to the ch0 section where I also have the motor drive setup?

If you have an unused KFLOP Axis it would be more logical to use it. In your case ch5. Configure it as Input Mode Encoder and InputChan0=0.

Then modify the MPG Program to use Axis 5 with:

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

HTH

[macona]

OK, set it up as channel 5.

Modified my init file and notifymach3.c and I get nothing.

Is the MPG code supposed to reside in the main section of the notifymach3?

Attaching the complete files.

NotifyMach3.c

(5.95 KiB) Downloaded 82 times

InitStepDir3AxisSupermax2.c

(8.6 KiB) Downloaded 77 times