// Example Init program that includes “smooth” MPG motion example
// which makes use of the exponential motion command.
// Which Pins were somewhat arbitrary; 11 I/O were needed, so JP4 and JP6 were used. The remainder were wired to level shifting boards and remain unused.
#define SELECTX 24 // KFLOP JP4, Pin 15 (I/O 24):
#define SELECTY 17 // KFLOP JP4, Pin 06 (I/O 17):
#define SELECTZ 19 // KFLOP JP4, Pin 10 (I/O 19):
#define SELECT4 21 // KFLOP JP4, Pin 12 (I/O 21):
#define FACTOR1 23 // KFLOP JP4, Pin 14 (I/O 23):
#define FACTOR10 25 // KFLOP JP4, Pin 16 (I/O 25):
#define FACTOR100 26 // KFLOP JP6, Pin 05 (I/O 26):
#define TAU 0.02 // smoothness factor (Low Pass Time constant seconds)
#define FINAL_TIME 1.0 // Set final dest after this amount of time with no change
#define QA 16 // KFLOP JP4, Pin 5 (I/O 16): define to which IO bits the AB signals are connected; I’ve assumed A+ and B+
#define QB 18 // KFLOP JP4, Pin 7 (I/O 18):
//Note: Dynomotion seems to just use single ended, the pendant is dual/differental; as such, two inputs are not needed, could reduce total to nine from eleven.
//Note: Current assignment of QA and QB results in backwards operation. Swapped lower down in code. Need to verify hardwire assignment correct.
double T0, LastX=0, LastY=0, LastZ=0, Tau;
int BitA,Change1=0,Change2=0, DiffX2;
int PosNoWrap, NewPos, Pos=0, wraps;
// Add a small amount of Coordinated Motion Path smoothing if desired
// Tau = 0.001; // seconds for Low Pass Filter Time Constant
// KLP = exp(-TIMEBASE/Tau);
KLP=0; // force to 0 to disable
// printf(“Tau=%f KLP=%f\n”,Tau,KLP);
// Main program starts here; includes MPGSmooth subroutines
for (;;) //Main program, cycle forever
// Pendant reading code
// convert quadrature to 2 bit binary
//BitA = ReadBit(QA);
//PosNoWrap = (ReadBit(QB) ^ BitA) | (BitA<<1);
BitA = ReadBit(QB);
PosNoWrap = (ReadBit(QA) ^ BitA) | (BitA<<1);
// Diff between expected position based on average of two prev deltas
// and position with no wraps. (Keep as X2 to avoid division by 2)
DiffX2 = 2*(Pos-PosNoWrap) + (Change2+Change1);
// Calc quadrature wraparounds to bring Diff nearest zero
// offset by 128 wraps to avoid requiring floor()
wraps = ((DiffX2+1028)>>3)-128;
// factor in the quadrature wraparounds
NewPos = PosNoWrap + (wraps<<2);
Change2 = Change1;
Change1 = NewPos – Pos;
Pos = NewPos;
// Determine which Axis is selected; Logic tree. If none are selected, disable pendant MPG control, as the unit is in the OFF position
// Pendant purchased did not have a ENABLE switch, no free conductors to install one; this will work fine to function as an erstaz one.
if (ReadBit(SELECTX)) // is x selected?
Axis=0; //X axis in my setup
else if (ReadBit(SELECTY)) // is y selected?
Axis=1; //Y Axis in my setup
else if (ReadBit(SELECTZ)) // is z selected?
Axis=2; //Z axis in my setup
else if (ReadBit(SELECT4)) // is 4th axis selected?
Axis=3; //A axis in my setup
Change1 = 0; // Disable Pendant, since OFF is selected (none of the above resolve to TRUE)
Factor = 0; // Force the change factor off (13 Nov 2015)
// Determine which multiplier is selected; unlike above, must be one of the three positions by hardware design
// Factor numbers by design, how much to move per MPG pulse
if (ReadBit(FACTOR1)) // is X1 selected?
// Factor = 1.5748031496062992125984251968504;
Factor = 1;
else if (ReadBit(FACTOR10)) // is X10 selected?
Factor = 10;
//Factor = 15.748031496062992125984251968504;
else if (ReadBit(FACTOR100)) // is X100 selected?
//Factor =157.48031496062992125984251968504 ;
// 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)++;
*cnt = 0; // reset count
*last = n;