experimenting with encoder integration with axis conrtoller

src/StepperControl.cpp

@@ -156,6 +156,14 @@ int StepperControl::moveToCoords(		long xDest, long yDest, long zDest,
 		/**/
        		encoderX.readEncoder();
 
+		currentPoint[0] = encoderX.currentPosition();
+		currentPoint[1] = encoderY.currentPosition();
+		currentPoint[2] = encoderZ.currentPosition();
+
+		axisX.setCurrentPosition(currentPoint[0]);
+		axisY.setCurrentPosition(currentPoint[1]);
+		axisZ.setCurrentPosition(currentPoint[2]);
+
         	axisX.checkTiming();
         	axisY.checkTiming();
         	axisZ.checkTiming();
@@ -211,9 +219,9 @@ int StepperControl::moveToCoords(		long xDest, long yDest, long zDest,
 	//Timer1.stop();
 	disableMotors();
 
-	currentPoint[0] = axisX.currentPoint();
-	currentPoint[1] = axisY.currentPoint();
-	currentPoint[2] = axisZ.currentPoint();
+	currentPoint[0] = axisX.currentPosition();
+	currentPoint[1] = axisY.currentPosition();
+	currentPoint[2] = axisZ.currentPosition();
 
 	CurrentState::getInstance()->setX(currentPoint[0]);
 	CurrentState::getInstance()->setY(currentPoint[1]);
@@ -223,6 +231,8 @@ int StepperControl::moveToCoords(		long xDest, long yDest, long zDest,
 	reportEndStops();
 	reportPosition();
 
+	/**/encoderX.test();
+
 	return error;
 }
 

src/StepperControlAxis.cpp

@@ -10,11 +10,17 @@ StepperControlAxis::StepperControlAxis() {
         pinMax          = 0;
 
 	axisActive	= false;
+
+	consMissedSteps = 0;
+
+	motorConsMissedStepsMax = 10;
 }
 
 void StepperControlAxis::test() {
-                Serial.print("R99 ");
-                Serial.print(label);
+                Serial.print("R99");
+                Serial.print(" cons steps missed ");
+                //Serial.print(label);
+                Serial.print(consMissedSteps);
                 Serial.print("\n");
 }
 
@@ -63,20 +69,12 @@ unsigned int StepperControlAxis::calculateSpeed(long sourcePosition, long curren
 	}
 
 
-	if (debugPrint /* && (millis() - lastCalcLog > 1000)*/) {
+	if (debugPrint && (millis() - lastCalcLog > 1000)) {
 
 		lastCalcLog = millis();
 
 		Serial.print("R99");
 
-	//	Serial.print(" a ");
-	//	Serial.print(endPos);
-	//	Serial.print(" b ");
-	//	Serial.print((endPos - stepsAccDec));
-	//	Serial.print(" c ");
-	//	Serial.print(curPos < (endPos - stepsAccDec));
-
-
 		Serial.print(" sta ");
 		Serial.print(staPos);
 		Serial.print(" cur ");
@@ -143,8 +141,8 @@ void StepperControlAxis::checkMovement() {
 		axisSpeed = calculateSpeed(	coordSourcePoint, coordCurrentPoint, coordDestinationPoint,
 				 		motorSpeedMin, motorSpeedMax, motorStepsAcc);
 
-		// If end stop reached, don't move anymore
-		if ((coordHomeAxis && !endStopAxisReached(false)) || (!coordHomeAxis &&  !endStopAxisReached(!movementToHome))) {
+		// If end stop reached or the encoder doesn't move anymore, don't move anymore
+		if ((consMissedSteps <= motorConsMissedStepsMax) && ((coordHomeAxis && !endStopAxisReached(false)) || (!coordHomeAxis &&  !endStopAxisReached(!movementToHome)))) {
 
 			// Set the moments when the step is set to true and false
 
@@ -183,6 +181,7 @@ void StepperControlAxis::checkTiming() {
 		if (moveTicks >= stepOffTick) {
 
 			// Negative flank for the steps
+			verifyStepAxis();
 			resetMotorStep();
 			checkMovement();
 		}
@@ -197,6 +196,15 @@ void StepperControlAxis::checkTiming() {
 	}
 }
 
+void StepperControlAxis::verifyStepAxis() {
+	if (coordCurrentPoint != coordEncoderPoint) {
+		consMissedSteps++;
+	} else {
+		consMissedSteps = 0;
+	}
+	coordCurrentPoint = coordEncoderPoint;
+}
+
 void StepperControlAxis::setStepAxis() {
 
 	if (coordHomeAxis && coordCurrentPoint == 0) {
@@ -374,10 +382,18 @@ bool StepperControlAxis::pointReached(long currentPoint, long destinationPoint)
 	return (destinationPoint == currentPoint);
 }
 
-long StepperControlAxis::currentPoint() {
+long StepperControlAxis::currentPosition() {
 	return coordCurrentPoint;
 }
 
+void StepperControlAxis::setCurrentPosition(long newPos) {
+	coordCurrentPoint = newPos;
+}
+
+void StepperControlAxis::setEncoderPosition(long newPos) {
+	coordEncoderPoint = newPos;
+}
+
 void StepperControlAxis::setMaxSpeed(long speed) {
 	motorSpeedMax = speed;
 }

src/StepperControlAxis.h

@@ -38,7 +38,10 @@ public:
 	bool endStopMax();
 	bool endStopsReached();
 	bool endStopAxisReached(bool movement_forward);
-	long currentPoint();
+
+	long currentPosition();
+	void setCurrentPosition(long newPos);
+	void setEncoderPosition(long newPos);
 
 	void setMotorStep();
 	void resetMotorStep();
@@ -49,7 +52,6 @@ public:
 	void setDirectionAway();
 	void setDirectionAxis();
 
-
 	void activateDebugPrint();
 	void test();
 
@@ -76,12 +78,14 @@ private:
 	bool motorMotorInv;		// invert motot direction
 	bool motorEndStopInv;		// invert input (true/false) of end stops
 	long motorInterruptSpeed;	// period of interrupt in micro seconds
+	int motorConsMissedStepsMax;	// the number of consecutive missed steps before the axis is seen as stuck
 
 	// coordinates
 	long coordSourcePoint;		// all coordinated in steps
 	long coordCurrentPoint;
 	long coordDestinationPoint;
 	bool coordHomeAxis;		// homing command
+	long coordEncoderPoint;
 
 	// movement handling
 	unsigned long movementLength;
@@ -93,8 +97,10 @@ private:
 	bool axisActive;
 	bool movementUp;
 	bool movementToHome;
+	int consMissedSteps;
 
 	void setStepAxis();
+	void verifyStepAxis();
 	void step(long &currentPoint, unsigned long steps, long destinationPoint);
 	bool pointReached(long currentPoint, long destinationPoint);
 	unsigned int calculateSpeed(long sourcePosition, long currentPosition, long destinationPosition, long minSpeed, long maxSpeed, long stepsAccDec);

src/StepperControlEncoder.h

@@ -45,7 +45,7 @@ private:
 	bool curValChannelB;
 
 	// encoder
-	int position;
+	long position;
 
 };