Motors moving

src/Helper_Functions.cpp

@@ -0,0 +1,190 @@
+#include "EggDuino.h"
+
+void initHardware()
+{
+	if (!initConfigStore())
+	{
+		penUpPos = 5;
+		penDownPos = 20;
+	}
+	penState = penUpPos;
+
+	g_stepEngine.init();
+	g_pStepperRotate = g_stepEngine.stepperConnectToPin(step1);
+	if (g_pStepperRotate)
+	{
+		// rotMotor.setMaxSpeed(2000.0);
+		// rotMotor.setAcceleration(10000.0);
+
+		g_pStepperRotate->setDirectionPin(dir1);
+		g_pStepperRotate->setEnablePin(enableRotMotor);
+		g_pStepperRotate->setAcceleration(10000);
+		g_pStepperRotate->setAutoEnable(false);
+	}
+
+	// Stepper pen init
+	g_pStepperPen = g_stepEngine.stepperConnectToPin(step2);
+	if (g_pStepperPen)
+	{
+		// penMotor.setMaxSpeed(2000.0);
+		// penMotor.setAcceleration(10000.0);
+		g_pStepperPen->setDirectionPin(dir2);
+		g_pStepperPen->setEnablePin(enablePenMotor);
+		g_pStepperPen->setAcceleration(10000);
+		g_pStepperPen->setAutoEnable(false);
+	}
+
+	motorsOff();
+	penServo.attach(servoPin);
+	penServo.write(penState);
+}
+
+void storePenUpPosInEE()
+{
+	saveConfigToFile();
+}
+
+void storePenDownPosInEE()
+{
+	saveConfigToFile();
+}
+
+void sendAck()
+{
+	Serial.print("OK\r\n");
+}
+
+void sendError()
+{
+	Serial.print("unknown CMD\r\n");
+}
+
+void motorsOff()
+{
+	// digitalWrite(enableRotMotor, HIGH);
+	// digitalWrite(enablePenMotor, HIGH);
+	g_pStepperPen->disableOutputs();
+	g_pStepperRotate->disableOutputs();
+	motorsEnabled = 0;
+}
+
+void motorsOn()
+{
+	// digitalWrite(enableRotMotor, LOW) ;
+	// digitalWrite(enablePenMotor, LOW) ;
+	g_pStepperPen->enableOutputs();
+	g_pStepperRotate->enableOutputs();
+	motorsEnabled = 1;
+}
+
+void toggleMotors()
+{
+	if (motorsEnabled)
+	{
+		motorsOff();
+	}
+	else
+	{
+		motorsOn();
+	}
+}
+
+bool parseSMArgs(uint16_t *duration, int *penStepsEBB, int *rotStepsEBB)
+{
+	char *arg1;
+	char *arg2;
+	char *arg3;
+	arg1 = SCmd.next();
+	if (arg1 != NULL)
+	{
+		*duration = atoi(arg1);
+		arg2 = SCmd.next();
+	}
+	if (arg2 != NULL)
+	{
+		*penStepsEBB = atoi(arg2);
+		arg3 = SCmd.next();
+	}
+	if (arg3 != NULL)
+	{
+		*rotStepsEBB = atoi(arg3);
+
+		return true;
+	}
+
+	return false;
+}
+
+void prepareMove(uint16_t duration, int penStepsEBB, int rotStepsEBB)
+{
+	if (!motorsEnabled)
+	{
+		motorsOn();
+	}
+	if ((1 == rotStepCorrection) && (1 == penStepCorrection))
+	{ // if coordinatessystems are identical
+		// set Coordinates and Speed
+		// rotMotor.move(rotStepsEBB);
+		// rotMotor.setSpeed(abs((float)rotStepsEBB * (float)1000 / (float)duration));
+		g_pStepperRotate->move(rotStepsEBB);
+		g_pStepperRotate->setSpeedInTicks(abs((float)rotStepsEBB * (float)1000 / (float)duration));
+		
+		// penMotor.move(penStepsEBB);
+		// penMotor.setSpeed(abs((float)penStepsEBB * (float)1000 / (float)duration));
+		g_pStepperPen->move(penStepsEBB);
+		g_pStepperRotate->setSpeedInTicks(abs((float)penStepsEBB * (float)1000 / (float)duration));
+	}
+	else
+	{
+		// incoming EBB-Steps will be multiplied by 16, then Integer-maths is done, result will be divided by 16
+		//  This make thinks here really complicated, but floating point-math kills performance and memory, believe me... I tried...
+		long rotSteps = ((long)rotStepsEBB * 16 / rotStepCorrection) + (long)rotStepError; // correct incoming EBB-Steps to our microstep-Setting and multiply  by 16 to avoid floatingpoint...
+		long penSteps = ((long)penStepsEBB * 16 / penStepCorrection) + (long)penStepError;
+
+		int rotStepsToGo = (int)(rotSteps / 16); // Calc Steps to go, which are possible on our machine
+		int penStepsToGo = (int)(penSteps / 16);
+
+		rotStepError = (long)rotSteps - ((long)rotStepsToGo * (long)16); // calc Position-Error, if there is one
+		penStepError = (long)penSteps - ((long)penStepsToGo * (long)16);
+
+		long temp_rotSpeed = ((long)rotStepsToGo * (long)1000 / (long)duration); // calc Speed in Integer Math
+		long temp_penSpeed = ((long)penStepsToGo * (long)1000 / (long)duration);
+
+		float rotSpeed = (float)abs(temp_rotSpeed); // type cast
+		float penSpeed = (float)abs(temp_penSpeed);
+
+		// set Coordinates and Speed
+		// rotMotor.move(rotStepsToGo); // finally, let us set the target position...
+		// rotMotor.setSpeed(rotSpeed); // and the Speed!
+		g_pStepperRotate->move(rotStepsToGo);
+		g_pStepperRotate->setSpeedInTicks(rotSpeed);
+
+		// penMotor.move(penStepsToGo);
+		// penMotor.setSpeed(penSpeed);
+		g_pStepperPen->move(penStepsToGo);
+		g_pStepperRotate->setSpeedInTicks(penSpeed);
+	}
+}
+
+void moveOneStep()
+{
+	// if (penMotor.distanceToGo() || rotMotor.distanceToGo())
+	// {
+	// 	penMotor.runSpeedToPosition(); // Moving.... moving... moving....
+	// 	rotMotor.runSpeedToPosition();
+	// }
+}
+
+void moveToDestination()
+{
+	// while (penMotor.distanceToGo() || rotMotor.distanceToGo())
+	// {
+	// 	penMotor.runSpeedToPosition(); // Moving.... moving... moving....
+	// 	rotMotor.runSpeedToPosition();
+	// }
+}
+
+void setprgButtonState()
+{
+	prgButtonState = 1;
+}