rLoop Stepper ISR Draft

global:
u32TimeCounter

stepper object properties:
u8PulsePinState
u8DirPinState
u8WriteDir
u32LastStepTime
u32NextStepTime
f32StepDistance_um
f32TargetPosition_um
f32CurrentPosition_um
f32CurrentVelocity_umps
u32MaxAccel_umpss
u32MaxVelocity_umps
u32MinVelocity_umps

STEPPER__ISR()
{
	Luint8 u8Count;
	Luint8 u8VelocityDir;
	Luint32 u32StoppingDistance_um;
	Lfloat32 f32CurrentAccel_umpss

	timeCounter++;
	for (u8Count = 0; u8Count < C_FCU__NUM_STEPPERS; u8Count++)
	{
		if (steppers[u8Count].u8WriteDir == 1)
		{
			if (u8Count == 0)
			{
				C_LOCALDEF__LCCM231__M0__PIN_DIR__LATCH(steppers[u8Count].u8DirPinState);
			}
			else if (u8Count == 1)
			{
				C_LOCALDEF__LCCM231__M1__PIN_DIR__LATCH(steppers[u8Count].u8DirPinState);
			}
			steppers[u8Count].u8WriteDir = 0;
		}
		else if
		(
			u32TimeCounter >= steppers[u8Count].u32NextStepTime // if time for the next step
			&&
			(
				abs(steppers[u8Count].f32CurrentPosition_um - steppers[u8Count].f32TargetPosition_um) > steppers[u8Count].f32StepDistance_um // and either we're not at the target positoin
				||
				steppers[u8Count].f32CurrentVelocity_umps > steppers[u8Count].u32MinVelocity_umps // or the velocity is too high to stop
			)
		)
		{
			steppers[u8Count].u8PulsePinState = 1 - steppers[u8Count].u8PulsePinState; // invert pin state
			
			if (u8Count == 0)
			{
				C_LOCALDEF__LCCM231__M0__PIN_PULSE__LATCH(steppers[u8Count].u8PulsePinState);
			}
			else if (u8Count == 1)
			{
				C_LOCALDEF__LCCM231__M1__PIN_PULSE__LATCH(steppers[u8Count].u8PulsePinState);
			}
			
			steppers[u8Count].f32CurrentPosition_um += steppers[u8Count].f32StepDistance_um;

			// d = v*t + 1/2*a*t^2, t=v/a  = (3*v^2)/(2*a)
			u32StoppingDistance_um = (3 * steppers[u8Count].f32CurrentVelocity_umps * steppers[u8Count].f32CurrentVelocity_umps) / (2*steppers[u8Count].u32MaxAccel_umpss);

			if (steppers[u8Count].f32TargetPosition_um - steppers[u8Count].f32CurrentPosition_um > u32StoppingDistance_um // if we should accelerate forwards
				|| (steppers[u8Count].f32TargetPosition_um > steppers[u8Count].f32CurrentPosition_um && steppers[u8Count].f32TargetPosition_um - steppers[u8Count].f32CurrentPosition_um < u32StoppingDistance_um)) // or slow down while going backwards
			{
				f32CurrentAccel_umpss = steppers[u8Count].u32MaxAccel_umpss; // positive acceleration
			}
			else if (steppers[u8Count].f32TargetPosition_um - steppers[u8Count].f32CurrentPosition_um > u32StoppingDistance_um // if we should accelerate backwards,
				|| (steppers[u8Count].f32TargetPosition_um > steppers[u8Count].f32CurrentPosition_um && steppers[u8Count].f32TargetPosition_um - steppers[u8Count].f32CurrentPosition_um < u32StoppingDistance_um)) // or slow down while going forwards
			{
				f32CurrentAccel_umpss = -steppers[u8Count].u32MaxAccel_umpss; // negative acceleration
			}
			
			steppers[u8Count].f32CurrentVelocity_umps += f32CurrentAccel_umpss * (u32TimeCounter - steppers[u8Count].u32LastStepTime); // vnext = vlast + a*dt
			u8VelocityDir = steppers[u8Count].f32CurrentVelocity_umps >= 0;
			
			steppers[u8Count].u32NextStepTime = u32TimeCounter + steppers[u8Count].f32StepDistance_um / steppers[u8Count].f32CurrentVelocity_umps; // tnext = tnow + d / v

			if (u8VelocityDir != steppers[u8Count].u8DirPinState)
			{
				steppers[u8Count].u8DirPinState = u8VelocityDir;
				steppers[u8Count].u8WriteDir = 1; // change the direction pin on the next ISR call, 10us away
			}
			steppers[u8Count].u32LastStepTime = u32TimeCounter;
		}
	}
}