rLoop Stepper ISR Draft

#include <stdlib.h>
#include <cmath>
#include <stdio.h>
#include <algorithm>
using namespace std;

#define Lint8 char
#define Luint8 unsigned char
#define Lint16 short int
#define Luint16 unsigned short int
#define Lint32 int
#define Luint32 unsigned int
#define Lfloat32 float

#define C_FCU__NUM_STEPPERS 1

Luint32 u32TimeCounter = 0;

struct Stepper
{
	Luint8 u8PulsePinState;
	Luint8 u8DirPinState;
	Luint8 u8WriteDir;
	Luint32 u32LastStepTime;
	Luint32 u32NextStepTime;
	Lfloat32 f32StepDistance_um;
	Lfloat32 f32TargetPosition_um;
	Lfloat32 f32CurrentPosition_um;
	Lfloat32 f32CurrentVelocity_umps;
	Lfloat32 f32DesiredVelocity_umps;
	Luint32 u32MaxAccel_umpss;
	Lfloat32 f32MaxVelocity_umps;
	Lfloat32 f32MinVelocity_umps;
	Lfloat32 f32ElapsedTime;
};

Stepper steppers[C_FCU__NUM_STEPPERS];

FILE * outFile;


int STEPPER__ISR()
{
	u32TimeCounter++;
	for (Luint8 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/2 // and either we're not at the target position
				||
				steppers[u8Count].f32CurrentVelocity_umps > steppers[u8Count].f32MinVelocity_umps * 2 // or the velocity is too high to stop
			)
		)
		{
			Luint8 u8VelocityDir;
			Luint32 u32StoppingDistance_um;
			Lfloat32 f32DesiredVelocity_umps;
			Lint32 s32CurrentAccel_umpss;
			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, d = (3*v^2)/(2*a)
			u32StoppingDistance_um = 1.3 * (steppers[u8Count].f32CurrentVelocity_umps * steppers[u8Count].f32CurrentVelocity_umps) / (2*steppers[u8Count].u32MaxAccel_umpss);
			// if we should be going forwards
				if (steppers[u8Count].f32TargetPosition_um > steppers[u8Count].f32CurrentPosition_um)
			{
				// if we should accelerate forwards
				if (steppers[u8Count].f32TargetPosition_um - steppers[u8Count].f32CurrentPosition_um > u32StoppingDistance_um)
				{
					s32CurrentAccel_umpss = steppers[u8Count].u32MaxAccel_umpss; // maximum positive acceleration
					fprintf(outFile, "Faster\n");
				}
				else
				{
					fprintf(outFile, "Slower\n");
					s32CurrentAccel_umpss = (Luint32) (( steppers[u8Count].f32CurrentVelocity_umps * steppers[u8Count].f32CurrentVelocity_umps) / (2*(steppers[u8Count].f32CurrentPosition_um - steppers[u8Count].f32TargetPosition_um)));
				}
			}
			// if we should be going backwards
			else if (steppers[u8Count].f32CurrentPosition_um > steppers[u8Count].f32TargetPosition_um)
			{
				// if we should accelerate forwards
				if (steppers[u8Count].f32CurrentPosition_um - steppers[u8Count].f32TargetPosition_um > u32StoppingDistance_um) 
				{ 
					s32CurrentAccel_umpss = -steppers[u8Count].u32MaxAccel_umpss; // maximum negative acceleration
				}
				// if should slow down while going backwards
				else
				{
					s32CurrentAccel_umpss = -min(steppers[u8Count].u32MaxAccel_umpss, (Luint32) ((3 * steppers[u8Count].f32CurrentVelocity_umps * steppers[u8Count].f32CurrentVelocity_umps) / (2*(steppers[u8Count].f32TargetPosition_um - steppers[u8Count].f32CurrentPosition_um))));
				}
			}
			else
			{
				fprintf(outFile, "ERROR\n");
				s32CurrentAccel_umpss = 0;
			}
			f32DesiredVelocity_umps = max(steppers[u8Count].f32MinVelocity_umps, min(steppers[u8Count].f32MaxVelocity_umps, steppers[u8Count].f32CurrentVelocity_umps + s32CurrentAccel_umpss * ((u32TimeCounter - steppers[u8Count].u32LastStepTime) / (Lfloat32) 100000.0))); // vnext = vlast + a*dt
			u8VelocityDir = f32DesiredVelocity_umps >= 0;
			
			steppers[u8Count].u32NextStepTime = u32TimeCounter + ceil((steppers[u8Count].f32StepDistance_um / f32DesiredVelocity_umps)*100000); // tnext = tnow + d / v

			if (abs(steppers[u8Count].f32CurrentVelocity_umps) == 0) 
			{
				steppers[u8Count].f32CurrentVelocity_umps = steppers[u8Count].f32MinVelocity_umps;
			}
			else
			{
				steppers[u8Count].f32CurrentVelocity_umps = steppers[u8Count].f32StepDistance_um / ((steppers[u8Count].u32NextStepTime - u32TimeCounter)/100000.0);
			}

			if (u8VelocityDir == 0)	steppers[u8Count].f32CurrentVelocity_umps *= -1;

			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;
			fprintf(outFile, "%d,%f,%f,%f,%f,%d,%d,%d,%d,%f\n",
				u32TimeCounter,
				steppers[0].f32TargetPosition_um,
				steppers[0].f32CurrentPosition_um,
				f32DesiredVelocity_umps,
				steppers[0].f32CurrentVelocity_umps,
				steppers[u8Count].u8PulsePinState*100,
				steppers[u8Count].u8DirPinState*100,
				s32CurrentAccel_umpss,
				u32StoppingDistance_um,
				steppers[0].f32TargetPosition_um -steppers[0].f32CurrentPosition_um
			);
		}
	}
	return 0;
}

int main()
{
	steppers[0].f32StepDistance_um   = 10.0;
	steppers[0].u32MaxAccel_umpss    = 3000;
	steppers[0].f32MaxVelocity_umps  = 900.0;
	steppers[0].f32MinVelocity_umps  = 200.0;
	steppers[0].f32CurrentPosition_um = 0;
	steppers[0].f32TargetPosition_um = 2000;

	outFile = fopen("StepperOutput.csv", "w");

	for (Luint32 t=1; t<400000; t++) {
		STEPPER__ISR();
		if(t==500000) steppers[0].f32TargetPosition_um = 0;
	}

	fclose(outFile);
	system("python3 stepper-plot.py");
	return 0;
}