From 521b1c55246928f1b63dbccbb520fe3c05454f59 Mon Sep 17 00:00:00 2001 From: TimEvWw Date: Sun, 26 Jul 2015 21:13:13 -0100 Subject: [PATCH 1/8] adding StepperControlAxis --- src/StepperControl.h | 1 + src/StepperControlAxis.cpp | 452 +++++++++++++++++++++++++++++++++++++ src/StepperControlAxis.h | 81 +++++++ 3 files changed, 534 insertions(+) create mode 100644 src/StepperControlAxis.cpp create mode 100644 src/StepperControlAxis.h diff --git a/src/StepperControl.h b/src/StepperControl.h index 90b4b2a..0f4bba9 100644 --- a/src/StepperControl.h +++ b/src/StepperControl.h @@ -11,6 +11,7 @@ #include "Arduino.h" #include "CurrentState.h" #include "ParameterList.h" +#include "StepperControlAxis.h" #include "pins.h" #include "Config.h" #include diff --git a/src/StepperControlAxis.cpp b/src/StepperControlAxis.cpp new file mode 100644 index 0000000..3472246 --- /dev/null +++ b/src/StepperControlAxis.cpp @@ -0,0 +1,452 @@ +#include "StepperControlAxis.h" + +//int test = 0; + +long interruptSpeed = 100; + +StepperControlAxis::StepperControlAxis() { + lastCalcLog = 0; +} + +bool StepperControlAxis::endStopMin() { + + int axis_nr; + + long stepPin[3] = { X_MIN_PIN, + Y_MIN_PIN, + Z_MIN_PIN }; + + return digitalRead(stepPin[axis_nr]); +} + +bool StepperControlAxis::endStopMax() { + + int axis_nr; + + long stepPin[3] = { X_MAX_PIN, + Y_MAX_PIN, + Z_MAX_PIN }; + + return digitalRead(stepPin[axis_nr]); +} + +bool StepperControlAxis::endStopAxisReached(bool movement_forward) { + + int axis_nr; + + bool endStInv[3] = { ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_X), + ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_Y), + ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_Z) }; + + bool min_endstop = false; + bool max_endstop = false; + bool invert = false; + + if (endStInv[axis_nr]) { + invert = true; + } + + // for the axis to check, retrieve the end stop status + + if (!invert) { + min_endstop = endStopMin(); + max_endstop = endStopMax(); + } else { + min_endstop = endStopMax(); + max_endstop = endStopMin(); + } + + // if moving forward, only check the end stop max + // for moving backwards, check only the end stop min + + if((!movement_forward && min_endstop) || (movement_forward && max_endstop)) { + return 1; + } + return 0; +} + +void StepperControlAxis::StepperControlAxis::step(long ¤tPoint, unsigned long steps, + long destinationPoint) { + + int axis; + + if (currentPoint < destinationPoint) { + currentPoint += steps; + } else if (currentPoint > destinationPoint) { + currentPoint -= steps; + } + + switch (axis) { + case 0: + digitalWrite(X_STEP_PIN, HIGH); + //digitalWrite(X_STEP_PIN, LOW); + break; + case 1: + digitalWrite(Y_STEP_PIN, HIGH); + //digitalWrite(Y_STEP_PIN, LOW); + break; + case 2: + digitalWrite(Z_STEP_PIN, HIGH); + //digitalWrite(Z_STEP_PIN, LOW); + break; + } + + // if the home end stop is reached, set the current position + if (endStopAxisReached(false)) + { + currentPoint = 0; + } +} + +void StepperControlAxis::resetStep() { + + int axis; + switch (axis) { + case 0: + digitalWrite(X_STEP_PIN, LOW); + break; + case 1: + digitalWrite(Y_STEP_PIN, LOW); + break; + case 2: + digitalWrite(Z_STEP_PIN, LOW); + break; + } + +} + +bool StepperControlAxis::pointReached(long currentPoint[3], + long destinationPoint[3]) { + for (int i = 0; i < 3; i++) { + if (destinationPoint[i] != currentPoint[i]) { + return false; + } + } + return true; +} + +unsigned int StepperControlAxis::calculateSpeed(long sourcePosition, long currentPosition, long destinationPosition, long minSpeed, long maxSpeed, long stepsAccDec) { + + int newSpeed = 0; + + long curPos = abs(currentPosition); + + long staPos; + long endPos; + + if (abs(sourcePosition) < abs(destinationPosition)) { + staPos = abs(sourcePosition); + endPos = abs(destinationPosition);; + } else { + staPos = abs(destinationPosition);; + endPos = abs(sourcePosition); + } + + unsigned long halfway = ((endPos - staPos) / 2) + staPos; + + // Set the minimum speed if the position would be out of bounds + if (curPos < staPos || curPos > endPos) { + newSpeed = minSpeed; + } else { + if (curPos >= staPos && curPos <= halfway) { + // accelerating + if (curPos > (staPos + stepsAccDec)) { + // now beyond the accelleration point to go full speed + newSpeed = maxSpeed + 1; + } else { + // speeding up, increase speed linear within the first period + newSpeed = (1.0 * (curPos - staPos) / stepsAccDec * (maxSpeed - minSpeed)) + minSpeed; + } + } else { + // decelerating + if (curPos < (endPos - stepsAccDec)) { + // still before the deceleration point so keep going at full speed + newSpeed = maxSpeed + 2; + } else { + // speeding up, increase speed linear within the first period + newSpeed = (1.0 * (endPos - curPos) / stepsAccDec * (maxSpeed - minSpeed)) + minSpeed; + } + } + } + +/* + if (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 "); + Serial.print(curPos); + Serial.print(" end "); + Serial.print(endPos); + Serial.print(" half "); + Serial.print(halfway); + Serial.print(" len "); + Serial.print(stepsAccDec); + Serial.print(" min "); + Serial.print(minSpeed); + Serial.print(" max "); + Serial.print(maxSpeed); + Serial.print(" spd "); + + Serial.print(" "); + Serial.print(newSpeed); + + Serial.print("\n"); + } +*/ + + // Return the calculated speed, in steps per second + return newSpeed; +} + +void StepperControlAxis::enableMotors(bool enable) { + if (enable) { + digitalWrite(X_ENABLE_PIN, LOW); + digitalWrite(Y_ENABLE_PIN, LOW); + digitalWrite(Z_ENABLE_PIN, LOW); + delay(100); + } else { + digitalWrite(X_ENABLE_PIN, HIGH); + digitalWrite(Y_ENABLE_PIN, HIGH); + digitalWrite(Z_ENABLE_PIN, HIGH); + } +} + +void StepperControlAxis::setDirectionAxis(int dirPin, long currentPoint, long destinationPoint, bool goHome, bool homeIsUp, bool motorInv) { + + if (((!goHome && currentPoint < destinationPoint) || (goHome && homeIsUp)) ^ motorInv) { + digitalWrite(dirPin, HIGH); + } else { + digitalWrite(dirPin, LOW); + } + + +} + +void StepperControlAxis::setDirections(long* currentPoint, long* destinationPoint, bool* homeAxis, bool* motorInv) { + + bool homeIsUp[3] = { ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_X), + ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_Y), + ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_Z) }; + + setDirectionAxis(X_DIR_PIN, currentPoint[0], destinationPoint[0], homeAxis[0], homeIsUp[0], motorInv[0]); + setDirectionAxis(Y_DIR_PIN, currentPoint[1], destinationPoint[1], homeAxis[1], homeIsUp[1], motorInv[1]); + setDirectionAxis(Z_DIR_PIN, currentPoint[2], destinationPoint[2], homeAxis[2], homeIsUp[2], motorInv[2]); +} + +unsigned long StepperControlAxis::getLength(long l1, long l2) { + if(l1 > l2) { + return l1 - l2; + } else { + return l2 - l1; + } +} + +int StepperControlAxis::endStopsReached() { + + bool endStInv[3] = { ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_X), + ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_Y), + ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_Z) }; + + bool x_min_endstop=(digitalRead(X_MIN_PIN) == endStInv[0]); + bool x_max_endstop=(digitalRead(X_MAX_PIN) == endStInv[0]); + bool y_min_endstop=(digitalRead(Y_MIN_PIN) == endStInv[1]); + bool y_max_endstop=(digitalRead(Y_MAX_PIN) == endStInv[1]); + bool z_min_endstop=(digitalRead(Z_MIN_PIN) == endStInv[2]); + bool z_max_endstop=(digitalRead(Z_MAX_PIN) == endStInv[2]); + if(x_min_endstop || x_max_endstop || y_min_endstop || y_max_endstop || z_min_endstop || z_max_endstop) { + return 1; + } + return 0; +} + +void StepperControlAxis::reportEndStops() { + + CurrentState::getInstance()->printEndStops(); +} + +void StepperControlAxis::reportPosition(){ + CurrentState::getInstance()->printPosition(); +} + +void StepperControlAxis::storeEndStops() { + + CurrentState::getInstance()->storeEndStops(); +} + +void StepperControlAxis::checkAxisDirection() { + if (homeAxis){ + // When home is active, the direction is fixed + movementUp = homeIsUp; + movementToHome = true; + } else { + // For normal movement, move in direction of destination + movementUp = ( currentPoint < destinationPoint ); + movementToHome = (abs(currentPoint) >= abs(destinationPoint)); + } + + if (currentPoint == 0) { + // Go slow when theoretical end point reached but there is no end stop siganl + axisSpeed = speedMin; + } +} + +void StepperControlAxis::stepAxis(bool state) { + + if (homeAxis && currentPoint == 0) { + + // Keep moving toward end stop even when position is zero + // but end stop is not yet active + if (homeIsUp) { + currentPoint = -1; + } else { + currentPoint = 1; + } + } + + // set a step on the motors + step(currentPoint, 1, destinationPoint); + + //stepMade = true; + //lastStepMillis[i] = currentMillis; + +} + +void StepperControlAxis::checkAxis() { + + int i = 1; + + //moveTicks[i]++; + checkAxisDirection(); + + +/* +Serial.print("R99 check axis "); +Serial.print(i); +Serial.print(" axis active "); +Serial.print(axisActive[i]); +Serial.print(" current point "); +Serial.print(currentPoint[i]); +Serial.print(" destination point "); +Serial.print(destinationPoint[i]); + +Serial.print(" home stop reached "); +Serial.print(endStopAxisReached(i, false)); +Serial.print(" axis stop reached "); +Serial.print(endStopAxisReached(i, true)); +Serial.print(" home axis "); +Serial.print(homeAxis[i]); +Serial.print(" movement to home "); +Serial.print(movementToHome[i]); +Serial.print("\n"); +*/ + + + //if ((!pointReached(currentPoint, destinationPoint) || home) && axisActive[i]) { + if (((currentPoint != destinationPoint) || homeAxis) && axisActive) { +//Serial.print("R99 point not reached yet\n"); + // home or destination not reached, keep moving +/* + Serial.print("R99 calc axis speed"); + Serial.print(" speed max "); + Serial.print(speedMax[i]); + Serial.print("\n"); +*/ + + // Get the axis speed, in steps per second + axisSpeed = calculateSpeed( sourcePoint,currentPoint,destinationPoint, + speedMin, speedMax, stepsAcc); + +//Serial.print("R99 axis speed "); +//Serial.print(axisSpeed[i]); +//Serial.print("\n"); + +/* +Serial.print("R99 check axis b "); +Serial.print(i); +Serial.print(" home part true "); +Serial.print(homeAxis[i] && !endStopAxisReached(i, false)); +Serial.print(" !homeAxis[i] "); +Serial.print(!homeAxis[i]); +Serial.print(" !endStopAxisReached(i, !movementToHome[i]) "); +Serial.print(!endStopAxisReached(i, !movementToHome[i])); +Serial.print("\n"); +*/ + // If end stop reached, don't move anymore + if ((homeAxis && !endStopAxisReached(false)) || (!homeAxis && !endStopAxisReached(!movementToHome))) { + + // Set the moments when the step is set to true and false + + if (axisSpeed > 0) + { + + // Take the requested speed (steps / second) and divide by the interrupt speed (interrupts per seconde) + // This gives the number of interrupts (called ticks here) before the pulse needs to be set for the next step + stepOnTick = moveTicks + (1000.0 * 1000.0 / interruptSpeed / axisSpeed / 2); + stepOffTick = moveTicks + (1000.0 * 1000.0 / interruptSpeed / axisSpeed ); + + } + } + else { + axisActive = false; + } + + } else { + // Destination or home reached. Deactivate the axis. + axisActive = false; + } + + // If end stop for home is active, set the position to zero + if (endStopAxisReached(false)) { + currentPoint = 0; + } + + +} + +void StepperControlAxis::checkTicksAxis() { + + int i; + + moveTicks++; + + if (axisActive) { + if (moveTicks >= stepOffTick) { + +/* +Serial.print("R99 reset step "); +Serial.print(" moveTicks "); +Serial.print(moveTicks[i]); +Serial.print("\n"); +*/ + // Negative flank for the steps + resetStep(); + checkAxis(); + } + else { + + if (moveTicks == stepOnTick) { +/* +Serial.print("R99 set step "); +Serial.print(" moveTicks "); +Serial.print(moveTicks[i]); +Serial.print("\n"); +*/ + // Positive flank for the steps + stepAxis(true); + } + } + } +} diff --git a/src/StepperControlAxis.h b/src/StepperControlAxis.h new file mode 100644 index 0000000..3305983 --- /dev/null +++ b/src/StepperControlAxis.h @@ -0,0 +1,81 @@ +/* + * StepperControlAxis.h + * + * Created on: 18 juli 2015 + * Author: Tim Evers + */ + +#ifndef STEPPERCONTROLAXIS_H_ +#define STEPPERCONTROLAXIS_H_ + +#include "Arduino.h" +#include "CurrentState.h" +#include "ParameterList.h" +#include "pins.h" +#include "Config.h" +#include +#include + +class StepperControlAxis { + +public: + + long interruptSpeed; + + long sourcePoint; + long currentPoint; + long destinationPoint; + unsigned long movementLength; + unsigned long maxLenth; + unsigned long moveTicks; + unsigned long stepOnTick; + unsigned long stepOffTick; + unsigned long axisSpeed; + double lengthRatio; + long speedMax; + long speedMin; + long stepsAcc; + long timeOut; + bool homeIsUp; + bool motorInv; + bool endStInv; + bool homeAxis; + bool axisActive; + bool movementUp; + bool movementToHome; + bool home; + + void checkTicksAxis(); + +private: + + int lastCalcLog = 0; + + + bool endStopMin(); + bool endStopMax(); + bool endStopAxisReached(bool movement_forward); + void step(long ¤tPoint, unsigned long steps, long destinationPoint); + void stepAxis(bool state); + void resetStep(); + bool pointReached(long currentPoint[3], long destinationPoint[3]); + unsigned int calculateSpeed(long sourcePosition, long currentPosition, long destinationPosition, long minSpeed, long maxSpeed, long stepsAccDec); + void enableMotors(bool enable); + void setDirectionAxis(int dirPin, long currentPoint, long destinationPoint, bool goHome, bool homeIsUp, bool motorInv); + void setDirections(long* currentPoint, long* destinationPoint, bool* homeAxis, bool* motorInv); + unsigned long getLength(long l1, long l2); + int endStopsReached(); + void reportPosition(); + void storeEndStops(); + void reportEndStops(); + void checkAxisDirection(); + void checkAxis(); + + + StepperControlAxis(); + //StepperControlAxis(StepperControlAxis const&); + //void operator=(StepperControlAxis const&); +}; + +#endif /* STEPPERCONTROLAXIS_H_ */ + From 8aa8a329af2dc0b32a9321b2fa15e2b64161d06b Mon Sep 17 00:00:00 2001 From: TimEvWw Date: Mon, 27 Jul 2015 19:32:38 -0100 Subject: [PATCH 2/8] creating functions to set pin numbers and motor settings --- src/StepperControlAxis.cpp | 156 ++++++++++++------------------------- src/StepperControlAxis.h | 39 ++++++---- 2 files changed, 74 insertions(+), 121 deletions(-) diff --git a/src/StepperControlAxis.cpp b/src/StepperControlAxis.cpp index 3472246..a9ac746 100644 --- a/src/StepperControlAxis.cpp +++ b/src/StepperControlAxis.cpp @@ -5,44 +5,32 @@ long interruptSpeed = 100; StepperControlAxis::StepperControlAxis() { - lastCalcLog = 0; + lastCalcLog = 0; + + pinStep = 0; + pinDirection = 0; + pinEnable = 0; + pinMin = 0; + pinMax = 0; + } bool StepperControlAxis::endStopMin() { - - int axis_nr; - - long stepPin[3] = { X_MIN_PIN, - Y_MIN_PIN, - Z_MIN_PIN }; - - return digitalRead(stepPin[axis_nr]); + return digitalRead(pinMin); } bool StepperControlAxis::endStopMax() { - int axis_nr; - - long stepPin[3] = { X_MAX_PIN, - Y_MAX_PIN, - Z_MAX_PIN }; - - return digitalRead(stepPin[axis_nr]); + return digitalRead(pinMax); } bool StepperControlAxis::endStopAxisReached(bool movement_forward) { - int axis_nr; - - bool endStInv[3] = { ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_X), - ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_Y), - ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_Z) }; - bool min_endstop = false; bool max_endstop = false; bool invert = false; - if (endStInv[axis_nr]) { + if (motorEndStopInv) { invert = true; } @@ -68,28 +56,13 @@ bool StepperControlAxis::endStopAxisReached(bool movement_forward) { void StepperControlAxis::StepperControlAxis::step(long ¤tPoint, unsigned long steps, long destinationPoint) { - int axis; - if (currentPoint < destinationPoint) { currentPoint += steps; } else if (currentPoint > destinationPoint) { currentPoint -= steps; } - switch (axis) { - case 0: - digitalWrite(X_STEP_PIN, HIGH); - //digitalWrite(X_STEP_PIN, LOW); - break; - case 1: - digitalWrite(Y_STEP_PIN, HIGH); - //digitalWrite(Y_STEP_PIN, LOW); - break; - case 2: - digitalWrite(Z_STEP_PIN, HIGH); - //digitalWrite(Z_STEP_PIN, LOW); - break; - } + digitalWrite(pinStep, HIGH); // if the home end stop is reached, set the current position if (endStopAxisReached(false)) @@ -99,30 +72,11 @@ void StepperControlAxis::StepperControlAxis::step(long ¤tPoint, unsigned l } void StepperControlAxis::resetStep() { - - int axis; - switch (axis) { - case 0: - digitalWrite(X_STEP_PIN, LOW); - break; - case 1: - digitalWrite(Y_STEP_PIN, LOW); - break; - case 2: - digitalWrite(Z_STEP_PIN, LOW); - break; - } - + digitalWrite(pinStep, LOW); } -bool StepperControlAxis::pointReached(long currentPoint[3], - long destinationPoint[3]) { - for (int i = 0; i < 3; i++) { - if (destinationPoint[i] != currentPoint[i]) { - return false; - } - } - return true; +bool StepperControlAxis::pointReached(long currentPoint, long destinationPoint) { + return (destinationPoint == currentPoint); } unsigned int StepperControlAxis::calculateSpeed(long sourcePosition, long currentPosition, long destinationPosition, long minSpeed, long maxSpeed, long stepsAccDec) { @@ -213,67 +167,35 @@ unsigned int StepperControlAxis::calculateSpeed(long sourcePosition, long curren void StepperControlAxis::enableMotors(bool enable) { if (enable) { - digitalWrite(X_ENABLE_PIN, LOW); - digitalWrite(Y_ENABLE_PIN, LOW); - digitalWrite(Z_ENABLE_PIN, LOW); + digitalWrite(pinEnable, LOW); delay(100); } else { - digitalWrite(X_ENABLE_PIN, HIGH); - digitalWrite(Y_ENABLE_PIN, HIGH); - digitalWrite(Z_ENABLE_PIN, HIGH); + digitalWrite(pinEnable, HIGH); } } -void StepperControlAxis::setDirectionAxis(int dirPin, long currentPoint, long destinationPoint, bool goHome, bool homeIsUp, bool motorInv) { +void StepperControlAxis::setDirectionAxis(long currentPoint, long destinationPoint, bool goHome, bool homeIsUp, bool motorInv) { if (((!goHome && currentPoint < destinationPoint) || (goHome && homeIsUp)) ^ motorInv) { - digitalWrite(dirPin, HIGH); + digitalWrite(pinDirection, HIGH); } else { - digitalWrite(dirPin, LOW); + digitalWrite(pinDirection, LOW); } - - -} - -void StepperControlAxis::setDirections(long* currentPoint, long* destinationPoint, bool* homeAxis, bool* motorInv) { - - bool homeIsUp[3] = { ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_X), - ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_Y), - ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_Z) }; - - setDirectionAxis(X_DIR_PIN, currentPoint[0], destinationPoint[0], homeAxis[0], homeIsUp[0], motorInv[0]); - setDirectionAxis(Y_DIR_PIN, currentPoint[1], destinationPoint[1], homeAxis[1], homeIsUp[1], motorInv[1]); - setDirectionAxis(Z_DIR_PIN, currentPoint[2], destinationPoint[2], homeAxis[2], homeIsUp[2], motorInv[2]); } unsigned long StepperControlAxis::getLength(long l1, long l2) { - if(l1 > l2) { + if (l1 > l2) { return l1 - l2; } else { return l2 - l1; } } -int StepperControlAxis::endStopsReached() { - - bool endStInv[3] = { ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_X), - ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_Y), - ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_Z) }; - - bool x_min_endstop=(digitalRead(X_MIN_PIN) == endStInv[0]); - bool x_max_endstop=(digitalRead(X_MAX_PIN) == endStInv[0]); - bool y_min_endstop=(digitalRead(Y_MIN_PIN) == endStInv[1]); - bool y_max_endstop=(digitalRead(Y_MAX_PIN) == endStInv[1]); - bool z_min_endstop=(digitalRead(Z_MIN_PIN) == endStInv[2]); - bool z_max_endstop=(digitalRead(Z_MAX_PIN) == endStInv[2]); - if(x_min_endstop || x_max_endstop || y_min_endstop || y_max_endstop || z_min_endstop || z_max_endstop) { - return 1; - } - return 0; +bool StepperControlAxis::endStopsReached() { + return ((digitalRead(pinMin) == motorEndStopInv) || (digitalRead(pinMax) == motorEndStopInv)); } void StepperControlAxis::reportEndStops() { - CurrentState::getInstance()->printEndStops(); } @@ -282,14 +204,13 @@ void StepperControlAxis::reportPosition(){ } void StepperControlAxis::storeEndStops() { - CurrentState::getInstance()->storeEndStops(); } void StepperControlAxis::checkAxisDirection() { if (homeAxis){ // When home is active, the direction is fixed - movementUp = homeIsUp; + movementUp = motorHomeIsUp; movementToHome = true; } else { // For normal movement, move in direction of destination @@ -299,17 +220,17 @@ void StepperControlAxis::checkAxisDirection() { if (currentPoint == 0) { // Go slow when theoretical end point reached but there is no end stop siganl - axisSpeed = speedMin; + axisSpeed = motorSpeedMin; } } -void StepperControlAxis::stepAxis(bool state) { +void StepperControlAxis::stepAxis() { if (homeAxis && currentPoint == 0) { // Keep moving toward end stop even when position is zero // but end stop is not yet active - if (homeIsUp) { + if (motorHomeIsUp) { currentPoint = -1; } else { currentPoint = 1; @@ -367,7 +288,7 @@ Serial.print("\n"); // Get the axis speed, in steps per second axisSpeed = calculateSpeed( sourcePoint,currentPoint,destinationPoint, - speedMin, speedMax, stepsAcc); + motorSpeedMin, motorSpeedMax, motorStepsAcc); //Serial.print("R99 axis speed "); //Serial.print(axisSpeed[i]); @@ -445,8 +366,27 @@ Serial.print(moveTicks[i]); Serial.print("\n"); */ // Positive flank for the steps - stepAxis(true); + stepAxis(); } } } } + +void StepperControlAxis::StepperControlAxis::loadPinNumbers(int step, int dir, int enable, int min, int max) { + pinStep = step; + pinDirection = dir; + pinEnable = enable; + pinMin = min; + pinMax = max; +} + + +void StepperControlAxis::loadMotorSettings(long speedMax, long speedMin, long stepsAcc, long timeOut, bool homeIsUp, bool motorInv, bool endStInv) { + motorSpeedMax = speedMax; + motorSpeedMin = speedMin; + motorStepsAcc = stepsAcc; + motorTimeOut = timeOut; + motorHomeIsUp = homeIsUp; + motorMotorInv = motorInv; + motorEndStopInv = endStInv; +} diff --git a/src/StepperControlAxis.h b/src/StepperControlAxis.h index 3305983..4e870d6 100644 --- a/src/StepperControlAxis.h +++ b/src/StepperControlAxis.h @@ -25,6 +25,7 @@ public: long sourcePoint; long currentPoint; long destinationPoint; + unsigned long movementLength; unsigned long maxLenth; unsigned long moveTicks; @@ -32,18 +33,11 @@ public: unsigned long stepOffTick; unsigned long axisSpeed; double lengthRatio; - long speedMax; - long speedMin; - long stepsAcc; - long timeOut; - bool homeIsUp; - bool motorInv; - bool endStInv; - bool homeAxis; bool axisActive; bool movementUp; bool movementToHome; bool home; + bool homeAxis; void checkTicksAxis(); @@ -51,20 +45,39 @@ private: int lastCalcLog = 0; + // pin settings + int pinStep; + int pinDirection; + int pinEnable; + int pinMin; + int pinMax; + + // motor settings + long motorSpeedMax; + long motorSpeedMin; + long motorStepsAcc; + long motorTimeOut; + bool motorHomeIsUp; + bool motorMotorInv; + bool motorEndStopInv; + + void loadPinNumbers(int step, int dir, int enable, int min, int max); + void loadMotorSettings( long speedMax, long speedMin, long stepsAcc, long timeOut, bool homeIsUp, bool motorInv, bool endStInv); + bool endStopMin(); bool endStopMax(); bool endStopAxisReached(bool movement_forward); void step(long ¤tPoint, unsigned long steps, long destinationPoint); - void stepAxis(bool state); + void stepAxis(); void resetStep(); - bool pointReached(long currentPoint[3], long destinationPoint[3]); + bool pointReached(long currentPoint, long destinationPoint); unsigned int calculateSpeed(long sourcePosition, long currentPosition, long destinationPosition, long minSpeed, long maxSpeed, long stepsAccDec); void enableMotors(bool enable); - void setDirectionAxis(int dirPin, long currentPoint, long destinationPoint, bool goHome, bool homeIsUp, bool motorInv); - void setDirections(long* currentPoint, long* destinationPoint, bool* homeAxis, bool* motorInv); + void setDirectionAxis(long currentPoint, long destinationPoint, bool goHome, bool homeIsUp, bool motorInv); + //void setDirections(long* currentPoint, long* destinationPoint, bool* homeAxis, bool* motorInv); unsigned long getLength(long l1, long l2); - int endStopsReached(); + bool endStopsReached(); void reportPosition(); void storeEndStops(); void reportEndStops(); From c28d5242bbf43f78c49e3fc494afecd2d0d635e9 Mon Sep 17 00:00:00 2001 From: TimEvWw Date: Sat, 1 Aug 2015 21:22:07 -0100 Subject: [PATCH 3/8] debug compile errors --- src/StepperControl.cpp | 825 +++++++++---------------------------- src/StepperControl.h | 21 +- src/StepperControlAxis.cpp | 76 ++-- src/StepperControlAxis.h | 44 +- 4 files changed, 272 insertions(+), 694 deletions(-) diff --git a/src/StepperControl.cpp b/src/StepperControl.cpp index bb10c65..21f62d3 100644 --- a/src/StepperControl.cpp +++ b/src/StepperControl.cpp @@ -18,29 +18,30 @@ int test = 0; long sourcePoint[3] = {0,0,0}; long currentPoint[3] = {0,0,0}; long destinationPoint[3] = {0,0,0}; -unsigned long movementLength[3] = {0,0,0}; +//unsigned long movementLength[3] = {0,0,0}; unsigned long maxLenth = 0; -unsigned long moveTicks[3] = {0,0,0}; -unsigned long stepOnTick[3] = {0,0,0}; -unsigned long stepOffTick[3] = {0,0,0}; -unsigned long axisSpeed[3] = {0,0,0}; -double lengthRatio[3] = {0,0,0}; +//unsigned long moveTicks[3] = {0,0,0}; +//unsigned long stepOnTick[3] = {0,0,0}; +//unsigned long stepOffTick[3] = {0,0,0}; +//unsigned long axisSpeed[3] = {0,0,0}; +//double lengthRatio[3] = {0,0,0}; long speedMax[3] = {0,0,0}; -long speedMin[3] = {0,0,0}; -long stepsAcc[3] = {0,0,0}; -long timeOut[3] = {0,0,0}; -bool homeIsUp[3] = {false,false,false}; -bool motorInv[3] = {false,false,false}; -bool endStInv[3] = {false,false,false}; -bool homeAxis[3] = {false,false,false}; -bool axisActive[3] = {false,false,false}; -bool movementUp[3] = {false,false,false}; -bool movementToHome[3] = {false,false,false}; -bool home = false; +//long speedMin[3] = {0,0,0}; +//long stepsAcc[3] = {0,0,0}; +//long timeOut[3] = {0,0,0}; +//bool homeIsUp[3] = {false,false,false}; +//bool motorInv[3] = {false,false,false}; +//bool endStInv[3] = {false,false,false}; +//bool homeAxis[3] = {false,false,false}; +//bool axisActive[3] = {false,false,false}; +//bool movementUp[3] = {false,false,false}; +//bool movementToHome[3] = {false,false,false}; +//bool home = false; // ** test code +/* // The interrupt will blink the LED, and keep // track of how many times it has blinked. int ledState = LOW; @@ -57,12 +58,22 @@ void blinkLed() { digitalWrite(LED_PIN, ledState); } +*/ StepperControl::StepperControl() { + //axisX = new StepperControlAxis(); + //axisY = new StepperControlAxis(); + //axisZ = new StepperControlAxis(); + axisX = StepperControlAxis(); + axisY = StepperControlAxis(); + axisZ = StepperControlAxis(); + axisX.loadPinNumbers(X_STEP_PIN, X_DIR_PIN, X_ENABLE_PIN, X_MIN_PIN, X_MAX_PIN); + axisY.loadPinNumbers(Y_STEP_PIN, Y_DIR_PIN, Y_ENABLE_PIN, Y_MIN_PIN, Y_MAX_PIN); + axisZ.loadPinNumbers(Z_STEP_PIN, Z_DIR_PIN, Z_ENABLE_PIN, Z_MIN_PIN, Z_MAX_PIN); } -unsigned long getMaxLength(unsigned long lengths[3]) { +unsigned long StepperControl::getMaxLength(unsigned long lengths[3]) { unsigned long max = lengths[0]; for (int i = 1; i < 3; i++) { if (lengths[i] > max) { @@ -72,325 +83,33 @@ unsigned long getMaxLength(unsigned long lengths[3]) { return max; } -bool endStopMin(int axis_nr) { - - long stepPin[3] = { X_MIN_PIN, - Y_MIN_PIN, - Z_MIN_PIN }; - - return digitalRead(stepPin[axis_nr]); +void StepperControl::enableMotors(bool enable) { + axisX.enableMotors(enable); + axisY.enableMotors(enable); + axisZ.enableMotors(enable); + delay(100); } -bool endStopMax(int axis_nr) { +bool StepperControl::endStopsReached() { - long stepPin[3] = { X_MAX_PIN, - Y_MAX_PIN, - Z_MAX_PIN }; - - return digitalRead(stepPin[axis_nr]); -} - -int endStopAxisReached(int axis_nr, bool movement_forward) { - - bool endStInv[3] = { ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_X), - ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_Y), - ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_Z) }; - - bool min_endstop = false; - bool max_endstop = false; - bool invert = false; - - if (endStInv[axis_nr]) { - invert = true; - } - - // for the axis to check, retrieve the end stop status - - if (!invert) { - min_endstop = endStopMin(axis_nr); - max_endstop = endStopMax(axis_nr); - } else { - min_endstop = endStopMax(axis_nr); - max_endstop = endStopMin(axis_nr); - } -/* - if (axis_nr == 0) { - min_endstop=(digitalRead(X_MIN_PIN) ^ invert); - max_endstop=(digitalRead(X_MAX_PIN) ^ invert); - } - - if (axis_nr == 1) { - min_endstop=(digitalRead(Y_MIN_PIN) ^ invert); - max_endstop=(digitalRead(Y_MAX_PIN) ^ invert); - } - - if (axis_nr == 2) { - min_endstop=(digitalRead(Z_MIN_PIN) ^ invert); - max_endstop=(digitalRead(Z_MAX_PIN) ^ invert); - } -*/ - - // if moving forward, only check the end stop max - // for moving backwards, check only the end stop min - - if((!movement_forward && min_endstop) || (movement_forward && max_endstop)) { - return 1; - } - return 0; -} - -void step(int axis, long ¤tPoint, unsigned long steps, - long destinationPoint) { - - if (currentPoint < destinationPoint) { - currentPoint += steps; - } else if (currentPoint > destinationPoint) { - currentPoint -= steps; - } - - switch (axis) { - case 0: - digitalWrite(X_STEP_PIN, HIGH); - //digitalWrite(X_STEP_PIN, LOW); - break; - case 1: - digitalWrite(Y_STEP_PIN, HIGH); - //digitalWrite(Y_STEP_PIN, LOW); - break; - case 2: - digitalWrite(Z_STEP_PIN, HIGH); - //digitalWrite(Z_STEP_PIN, LOW); - break; - } - - // if the home end stop is reached, set the current position - if (endStopAxisReached(axis, false)) - { - currentPoint = 0; - } -} - -void resetStep(int axis) { - switch (axis) { - case 0: - digitalWrite(X_STEP_PIN, LOW); - break; - case 1: - digitalWrite(Y_STEP_PIN, LOW); - break; - case 2: - digitalWrite(Z_STEP_PIN, LOW); - break; + if ( axisX.endStopsReached() || + axisY.endStopsReached() || + axisZ.endStopsReached()) { + return true; } + return false; } -bool pointReached(long currentPoint[3], - long destinationPoint[3]) { - for (int i = 0; i < 3; i++) { - if (destinationPoint[i] != currentPoint[i]) { - return false; - } - } - return true; -} - -/* -unsigned int calculateSpeed(unsigned int ¤tStepsPerSecond, - unsigned int maxStepsPerSecond, unsigned int currentSteps, - unsigned int currentMillis, unsigned int maxLength, - unsigned int &accelerationSteps, - unsigned int maxAccelerationStepsPerSecond) { - - if (currentStepsPerSecond < maxStepsPerSecond) { - if (currentSteps < maxLength / 2) { - accelerationSteps = currentSteps; - if (currentMillis % 100 == 0) { - currentStepsPerSecond += maxAccelerationStepsPerSecond / 10; - } - } else { - if (currentMillis % 100 == 0 - && currentStepsPerSecond - > maxAccelerationStepsPerSecond / 10) { - currentStepsPerSecond -= maxAccelerationStepsPerSecond / 10; - } - } - } else if (currentSteps > maxLength - accelerationSteps - && currentStepsPerSecond > maxAccelerationStepsPerSecond / 10) { - if (currentMillis % 100 == 0) { - currentStepsPerSecond -= maxAccelerationStepsPerSecond / 10; - } - } else { - currentStepsPerSecond = maxStepsPerSecond; - } - if (currentStepsPerSecond < maxAccelerationStepsPerSecond) { - currentStepsPerSecond = maxAccelerationStepsPerSecond; - } - return currentStepsPerSecond; -} -*/ - - -unsigned int lastCalcLog = 0; - -unsigned int calculateSpeed(long sourcePosition, long currentPosition, long destinationPosition, long minSpeed, long maxSpeed, long stepsAccDec) { - - int newSpeed = 0; - - long curPos = abs(currentPosition); - - long staPos; - long endPos; - - if (abs(sourcePosition) < abs(destinationPosition)) { - staPos = abs(sourcePosition); - endPos = abs(destinationPosition);; - } else { - staPos = abs(destinationPosition);; - endPos = abs(sourcePosition); - } - - unsigned long halfway = ((endPos - staPos) / 2) + staPos; - - // Set the minimum speed if the position would be out of bounds - if (curPos < staPos || curPos > endPos) { - newSpeed = minSpeed; - } else { - if (curPos >= staPos && curPos <= halfway) { - // accelerating - if (curPos > (staPos + stepsAccDec)) { - // now beyond the accelleration point to go full speed - newSpeed = maxSpeed + 1; - } else { - // speeding up, increase speed linear within the first period - newSpeed = (1.0 * (curPos - staPos) / stepsAccDec * (maxSpeed - minSpeed)) + minSpeed; - } - } else { - // decelerating - if (curPos < (endPos - stepsAccDec)) { - // still before the deceleration point so keep going at full speed - newSpeed = maxSpeed + 2; - } else { - // speeding up, increase speed linear within the first period - newSpeed = (1.0 * (endPos - curPos) / stepsAccDec * (maxSpeed - minSpeed)) + minSpeed; - } - } - } - -/* - if (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 "); - Serial.print(curPos); - Serial.print(" end "); - Serial.print(endPos); - Serial.print(" half "); - Serial.print(halfway); - Serial.print(" len "); - Serial.print(stepsAccDec); - Serial.print(" min "); - Serial.print(minSpeed); - Serial.print(" max "); - Serial.print(maxSpeed); - Serial.print(" spd "); - - Serial.print(" "); - Serial.print(newSpeed); - - Serial.print("\n"); - } -*/ - - // Return the calculated speed, in steps per second - return newSpeed; -} - -void enableMotors(bool enable) { - if (enable) { - digitalWrite(X_ENABLE_PIN, LOW); - digitalWrite(Y_ENABLE_PIN, LOW); - digitalWrite(Z_ENABLE_PIN, LOW); - delay(100); - } else { - digitalWrite(X_ENABLE_PIN, HIGH); - digitalWrite(Y_ENABLE_PIN, HIGH); - digitalWrite(Z_ENABLE_PIN, HIGH); - } -} - -void setDirectionAxis(int dirPin, long currentPoint, long destinationPoint, bool goHome, bool homeIsUp, bool motorInv) { - - if (((!goHome && currentPoint < destinationPoint) || (goHome && homeIsUp)) ^ motorInv) { - digitalWrite(dirPin, HIGH); - } else { - digitalWrite(dirPin, LOW); - } - - -} - -void setDirections(long* currentPoint, long* destinationPoint, bool* homeAxis, bool* motorInv) { - - bool homeIsUp[3] = { ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_X), - ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_Y), - ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_Z) }; - - setDirectionAxis(X_DIR_PIN, currentPoint[0], destinationPoint[0], homeAxis[0], homeIsUp[0], motorInv[0]); - setDirectionAxis(Y_DIR_PIN, currentPoint[1], destinationPoint[1], homeAxis[1], homeIsUp[1], motorInv[1]); - setDirectionAxis(Z_DIR_PIN, currentPoint[2], destinationPoint[2], homeAxis[2], homeIsUp[2], motorInv[2]); -} - -unsigned long getLength(long l1, long l2) { - if(l1 > l2) { - return l1 - l2; - } else { - return l2 - l1; - } -} - -int endStopsReached() { - - bool endStInv[3] = { ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_X), - ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_Y), - ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_Z) }; - - bool x_min_endstop=(digitalRead(X_MIN_PIN) == endStInv[0]); - bool x_max_endstop=(digitalRead(X_MAX_PIN) == endStInv[0]); - bool y_min_endstop=(digitalRead(Y_MIN_PIN) == endStInv[1]); - bool y_max_endstop=(digitalRead(Y_MAX_PIN) == endStInv[1]); - bool z_min_endstop=(digitalRead(Z_MIN_PIN) == endStInv[2]); - bool z_max_endstop=(digitalRead(Z_MAX_PIN) == endStInv[2]); - if(x_min_endstop || x_max_endstop || y_min_endstop || y_max_endstop || z_min_endstop || z_max_endstop) { - return 1; - } - return 0; -} - -void reportEndStops() { - +void StepperControl::reportEndStops() { CurrentState::getInstance()->printEndStops(); } -void reportPosition(){ +void StepperControl::reportPosition(){ CurrentState::getInstance()->printPosition(); } -void storeEndStops() { - +void StepperControl::storeEndStops() { CurrentState::getInstance()->storeEndStops(); } @@ -399,206 +118,11 @@ void storeEndStops() { * */ -void doseWaterByTime(long time) { - digitalWrite(HEATER_1_PIN, HIGH); - delay(time); - digitalWrite(HEATER_1_PIN, LOW); -} - -void checkAxisDirection(int i) { - if (homeAxis[i]){ - // When home is active, the direction is fixed - movementUp[i] = homeIsUp[i]; - movementToHome[i] = true; - } else { - // For normal movement, move in direction of destination - movementUp[i] = ( currentPoint[i] < destinationPoint[i] ); - movementToHome[i] = (abs(currentPoint[i]) >= abs(destinationPoint[i])); - } - - if (currentPoint[i] == 0) { - // Go slow when theoretical end point reached but there is no end stop siganl - axisSpeed[i] = speedMin[i]; - } -} - -void stepAxis(int i, bool state) { - - if (homeAxis[i] && currentPoint[i] == 0) { - - // Keep moving toward end stop even when position is zero - // but end stop is not yet active - if (homeIsUp[i]) { - currentPoint[i] = -1; - } else { - currentPoint[i] = 1; - } - } - - // set a step on the motors - step(i, currentPoint[i], 1, destinationPoint[i]); - blinkLed(); - //stepMade = true; - //lastStepMillis[i] = currentMillis; - -} - -void checkAxis(int i) { - - //moveTicks[i]++; - checkAxisDirection(i); - - -/* -Serial.print("R99 check axis "); -Serial.print(i); -Serial.print(" axis active "); -Serial.print(axisActive[i]); -Serial.print(" current point "); -Serial.print(currentPoint[i]); -Serial.print(" destination point "); -Serial.print(destinationPoint[i]); - -Serial.print(" home stop reached "); -Serial.print(endStopAxisReached(i, false)); -Serial.print(" axis stop reached "); -Serial.print(endStopAxisReached(i, true)); -Serial.print(" home axis "); -Serial.print(homeAxis[i]); -Serial.print(" movement to home "); -Serial.print(movementToHome[i]); -Serial.print("\n"); -*/ - - - //if ((!pointReached(currentPoint, destinationPoint) || home) && axisActive[i]) { - if (((currentPoint[i] != destinationPoint[i]) || homeAxis[i]) && axisActive[i]) { -//Serial.print("R99 point not reached yet\n"); - // home or destination not reached, keep moving -/* - Serial.print("R99 calc axis speed"); - Serial.print(" speed max "); - Serial.print(speedMax[i]); - Serial.print("\n"); -*/ - - // Get the axis speed, in steps per second - axisSpeed[i] = calculateSpeed( sourcePoint[i],currentPoint[i],destinationPoint[i], - speedMin[i], speedMax[i], stepsAcc[i]); - -//Serial.print("R99 axis speed "); -//Serial.print(axisSpeed[i]); -//Serial.print("\n"); - -/* -Serial.print("R99 check axis b "); -Serial.print(i); -Serial.print(" home part true "); -Serial.print(homeAxis[i] && !endStopAxisReached(i, false)); -Serial.print(" !homeAxis[i] "); -Serial.print(!homeAxis[i]); -Serial.print(" !endStopAxisReached(i, !movementToHome[i]) "); -Serial.print(!endStopAxisReached(i, !movementToHome[i])); -Serial.print("\n"); -*/ - // If end stop reached, don't move anymore - if ((homeAxis[i] && !endStopAxisReached(i, false)) || (!homeAxis[i] && !endStopAxisReached(i, !movementToHome[i]))) { - - // Set the moments when the step is set to true and false - - if (axisSpeed[i] > 0) - { -/* - Serial.print("R99 calculated steps "); - Serial.print(" ** additional ticks steps "); - Serial.print(1000.0 * 1000.0 / MOVEMENT_INTERRUPT_SPEED / axisSpeed[i]); - Serial.print("\n"); -*/ - - // Take the requested speed (steps / second) and divide by the interrupt speed (interrupts per seconde) - // This gives the number of interrupts (called ticks here) before the pulse needs to be set for the next step - stepOnTick[i] = moveTicks[i] + (1000.0 * 1000.0 / MOVEMENT_INTERRUPT_SPEED / axisSpeed[i] / 2); - stepOffTick[i] = moveTicks[i] + (1000.0 * 1000.0 / MOVEMENT_INTERRUPT_SPEED / axisSpeed[i] ); - //stepOnTick[i] = moveTicks[i] + (axisSpeed[i] * 1000 / MOVEMENT_INTERRUPT_SPEED / 2); - //stepOffTick[i] = moveTicks[i] + (axisSpeed[i] * 1000 / MOVEMENT_INTERRUPT_SPEED ); - -/* - Serial.print("R99 calculated steps "); - Serial.print(" interrupt speed "); - Serial.print(MOVEMENT_INTERRUPT_SPEED); - Serial.print(" axisSpeed "); - Serial.print(axisSpeed[i]); - Serial.print(" moveTicks "); - Serial.print(moveTicks[i]); - Serial.print(" stepOnTick "); - Serial.print(stepOnTick[i]); - Serial.print(" stepOffTick "); - Serial.print(stepOffTick[i]); - Serial.print("\n"); -*/ - } - } - else { - axisActive[i] = false; - } - - } else { - // Destination or home reached. Deactivate the axis. - axisActive[i] = false; - } - - // If end stop for home is active, set the position to zero - if (endStopAxisReached(i, false)) { - currentPoint[i] = 0; - } - - -} - -void checkTicksAxis(int i) { - - moveTicks[i]++; - -//Serial.print("R99 checkTicksAxis "); -//Serial.print(" destination point "); -//Serial.print(destinationPoint[0]); -//Serial.print(" moveTicks "); -//Serial.print(moveTicks[i]); -//Serial.print(" stepOnTick "); -//Serial.print(stepOnTick[i]); -//Serial.print(" stepOffTick "); -//Serial.print(stepOffTick[i]); -//Serial.print("\n"); - - if (axisActive[i]) { - if (moveTicks[i] >= stepOffTick[i]) { - -/* -Serial.print("R99 reset step "); -Serial.print(" moveTicks "); -Serial.print(moveTicks[i]); -Serial.print("\n"); -*/ - // Negative flank for the steps - resetStep(i); - checkAxis(i); - } - else { - - if (moveTicks[i] == stepOnTick[i]) { -/* -Serial.print("R99 set step "); -Serial.print(" moveTicks "); -Serial.print(moveTicks[i]); -Serial.print("\n"); -*/ - // Positive flank for the steps - stepAxis(i, true); - } - } - } -} - +//void StepperControl::doseWaterByTime(long time) { +// digitalWrite(HEATER_1_PIN, HIGH); +// delay(time); +// digitalWrite(HEATER_1_PIN, LOW); +//} // Handle movement by checking each axis @@ -613,12 +137,9 @@ void StepperControl::handleMovementInterrupt(void){ //Serial.print(test); //Serial.print("\n"); - checkTicksAxis(0); - checkTicksAxis(1); - checkTicksAxis(2); - -// blinkLed(); - + axisX.checkTicksAxis(); + axisY.checkTicksAxis(); + axisZ.checkTicksAxis(); } bool interruptBusy = false; @@ -658,32 +179,84 @@ int StepperControl::moveAbsoluteConstant( long xDest, long yDest, long zDest, //Serial.print(xMaxSpd); //Serial.print("\n"); - sourcePoint[0] = CurrentState::getInstance()->getX(); - sourcePoint[1] = CurrentState::getInstance()->getY(); - sourcePoint[2] = CurrentState::getInstance()->getZ(); - - currentPoint[0] = CurrentState::getInstance()->getX(); - currentPoint[1] = CurrentState::getInstance()->getY(); - currentPoint[2] = CurrentState::getInstance()->getZ(); - - destinationPoint[0] = xDest; - destinationPoint[1] = yDest; - destinationPoint[2] = zDest; - - movementLength[0] = getLength(destinationPoint[0], currentPoint[0]); - movementLength[1] = getLength(destinationPoint[1], currentPoint[1]); - movementLength[2] = getLength(destinationPoint[2], currentPoint[2]); - - maxLenth = getMaxLength(movementLength); - - lengthRatio[0] = 1.0 * movementLength[0] / maxLenth; - lengthRatio[1] = 1.0 * movementLength[1] / maxLenth; - lengthRatio[2] = 1.0 * movementLength[2] / maxLenth; + // Load settings + bool homeIsUp[3] = {false,false,false}; homeIsUp[0] = ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_X); homeIsUp[1] = ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_Y); homeIsUp[2] = ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_Z); + long speedMax[3] = {0,0,0}; + speedMax[0] = ParameterList::getInstance()->getValue(MOVEMENT_MAX_SPD_X); + speedMax[1] = ParameterList::getInstance()->getValue(MOVEMENT_MAX_SPD_Y); + speedMax[2] = ParameterList::getInstance()->getValue(MOVEMENT_MAX_SPD_Z); + + long speedMin[3] = {0,0,0}; + speedMin[0] = ParameterList::getInstance()->getValue(MOVEMENT_MIN_SPD_X); + speedMin[1] = ParameterList::getInstance()->getValue(MOVEMENT_MIN_SPD_Y); + speedMin[2] = ParameterList::getInstance()->getValue(MOVEMENT_MIN_SPD_Z); + + long stepsAcc[3] = {0,0,0}; + stepsAcc[0] = ParameterList::getInstance()->getValue(MOVEMENT_STEPS_ACC_DEC_X); + stepsAcc[1] = ParameterList::getInstance()->getValue(MOVEMENT_STEPS_ACC_DEC_Y); + stepsAcc[2] = ParameterList::getInstance()->getValue(MOVEMENT_STEPS_ACC_DEC_Z); + + bool motorInv[3] = {false,false,false}; + motorInv[0] = ParameterList::getInstance()->getValue(MOVEMENT_INVERT_MOTOR_X); + motorInv[1] = ParameterList::getInstance()->getValue(MOVEMENT_INVERT_MOTOR_Y); + motorInv[2] = ParameterList::getInstance()->getValue(MOVEMENT_INVERT_MOTOR_Z); + + bool endStInv[3] = {false,false,false}; + endStInv[0] = ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_X); + endStInv[1] = ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_Y); + endStInv[2] = ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_Z); + + long timeOut[3] = {0,0,0}; + timeOut[0] = ParameterList::getInstance()->getValue(MOVEMENT_TIMEOUT_X); + timeOut[1] = ParameterList::getInstance()->getValue(MOVEMENT_TIMEOUT_X); + timeOut[2] = ParameterList::getInstance()->getValue(MOVEMENT_TIMEOUT_X); + +//Serial.print("R99 "); +//Serial.print(" after vars "); +//Serial.print(destinationPoint[0]); +//Serial.print("\n"); + + + unsigned long currentMillis = 0; + + unsigned long timeStart = millis(); + + + //bool movementDone = false; + //bool movementUp = false; + //bool movementToHome = false; + + //bool moving = false; + + //bool stepMade = false; + int incomingByte = 0; + //int axisSpeed = 0; + int error = 0; + + // if a speed is given in the command, use that instead of the config speed + + if (xMaxSpd > 0 && xMaxSpd < speedMax[0]) { + speedMax[0] = xMaxSpd; + } + + if (yMaxSpd > 0 && yMaxSpd < speedMax[1]) { + speedMax[1] = yMaxSpd; + } + + if (zMaxSpd > 0 && zMaxSpd < speedMax[2]) { + speedMax[2] = zMaxSpd; + } + + + homeIsUp[0] = ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_X); + homeIsUp[1] = ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_Y); + homeIsUp[2] = ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_Z); + speedMax[0] = ParameterList::getInstance()->getValue(MOVEMENT_MAX_SPD_X); speedMax[1] = ParameterList::getInstance()->getValue(MOVEMENT_MAX_SPD_Y); speedMax[2] = ParameterList::getInstance()->getValue(MOVEMENT_MAX_SPD_Z); @@ -705,64 +278,51 @@ int StepperControl::moveAbsoluteConstant( long xDest, long yDest, long zDest, endStInv[2] = ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_Z); timeOut[0] = ParameterList::getInstance()->getValue(MOVEMENT_TIMEOUT_X); - timeOut[1] = ParameterList::getInstance()->getValue(MOVEMENT_TIMEOUT_X); - timeOut[2] = ParameterList::getInstance()->getValue(MOVEMENT_TIMEOUT_X); + timeOut[1] = ParameterList::getInstance()->getValue(MOVEMENT_TIMEOUT_Y); + timeOut[2] = ParameterList::getInstance()->getValue(MOVEMENT_TIMEOUT_Z); - homeAxis[0] = xHome; - homeAxis[1] = yHome; - homeAxis[2] = zHome; - - moveTicks[0] = 0; - moveTicks[1] = 0; - moveTicks[2] = 0; - - home = xHome || yHome || zHome; - -//Serial.print("R99 "); -//Serial.print(" after vars "); -//Serial.print(destinationPoint[0]); -//Serial.print("\n"); - - - unsigned long currentMillis = 0; - - unsigned long timeStart = millis(); - - - bool movementDone = false; - bool movementUp = false; - bool movementToHome = false; - - bool moving = false; - - bool stepMade = false; - int incomingByte = 0; - //int axisSpeed = 0; - int error = 0; - - // if a speed is given in the command, use that instead of the config speed - - if (xMaxSpd > 0 && xMaxSpd < speedMax[0]) { - speedMax[0] = xMaxSpd; - } - - if (yMaxSpd > 0 && yMaxSpd < speedMax[1]) { - speedMax[1] = yMaxSpd; - } - - if (zMaxSpd > 0 && zMaxSpd < speedMax[2]) { - speedMax[2] = zMaxSpd; - } + axisX.loadMotorSettings(speedMax[0], speedMin[0], stepsAcc[0], timeOut[0], homeIsUp[0], motorInv[0], endStInv[0], MOVEMENT_INTERRUPT_SPEED); + axisY.loadMotorSettings(speedMax[1], speedMin[1], stepsAcc[1], timeOut[1], homeIsUp[1], motorInv[1], endStInv[1], MOVEMENT_INTERRUPT_SPEED); + axisZ.loadMotorSettings(speedMax[2], speedMin[2], stepsAcc[2], timeOut[2], homeIsUp[2], motorInv[2], endStInv[2], MOVEMENT_INTERRUPT_SPEED); // Prepare for movement - setDirections(currentPoint, destinationPoint, homeAxis, motorInv); + sourcePoint[0] = CurrentState::getInstance()->getX(); + sourcePoint[1] = CurrentState::getInstance()->getY(); + sourcePoint[2] = CurrentState::getInstance()->getZ(); + currentPoint[0] = CurrentState::getInstance()->getX(); + currentPoint[1] = CurrentState::getInstance()->getY(); + currentPoint[2] = CurrentState::getInstance()->getZ(); + + destinationPoint[0] = xDest; + destinationPoint[1] = yDest; + destinationPoint[2] = zDest; + + //movementLength[0] = getLength(destinationPoint[0], currentPoint[0]); + //movementLength[1] = getLength(destinationPoint[1], currentPoint[1]); + //movementLength[2] = getLength(destinationPoint[2], currentPoint[2]); + + //maxLenth = getMaxLength(movementLength); + + //home = xHome || yHome || zHome; + + + axisX.loadCoordinates(currentPoint[0],destinationPoint[0],xHome); + axisY.loadCoordinates(currentPoint[1],destinationPoint[1],yHome); + axisZ.loadCoordinates(currentPoint[2],destinationPoint[2],zHome); + + //axisX.setDirection() + //axisY.setDirection() + //axisZ.setDirection() storeEndStops(); reportEndStops(); + enableMotors(true); + + // Limit normal movmement to the home position for (int i = 0; i < 3; i++) { if (!homeIsUp[i] && destinationPoint[i] < 0) { @@ -780,46 +340,35 @@ int StepperControl::moveAbsoluteConstant( long xDest, long yDest, long zDest, axisActive[1] = true; axisActive[2] = true; - checkAxis(0); - checkAxis(1); - checkAxis(2); - - Timer1.start(); + axisX.checkAxis(); + axisY.checkAxis(); + axisZ.checkAxis(); + //Timer1.start(); // Let the interrupt handle all the movements while (axisActive[0] || axisActive[1] || axisActive[2]) { -/**///Serial.print("R99 while loop\n"); + //delay(50); delayMicroseconds(100); -//Serial.print("R99 "); -//Serial.print(" maximim speed "); -//Serial.print(speedMax[0]); -//Serial.print("\n"); + //Serial.print("R99 "); + //Serial.print(" maximim speed "); + //Serial.print(speedMax[0]); + //Serial.print("\n"); - checkTicksAxis(0); - checkTicksAxis(1); - checkTicksAxis(2); + axisX.checkTicksAxis(); + axisY.checkTicksAxis(); + axisZ.checkTicksAxis(); + axisActive[0] = axisX.isAxisActive(); + axisActive[1] = axisY.isAxisActive(); + axisActive[2] = axisZ.isAxisActive(); -//axisActive[0] = false; -//axisActive[1] = false; -//axisActive[2] = false; - -/* - Serial.print("R99"); - Serial.print(" cur "); - Serial.print(currentPoint[0]); - Serial.print(" end "); - Serial.print(destinationPoint[0]); - Serial.print(" max spd "); - Serial.print(speedMax[0]); - Serial.print(" axis spd "); - Serial.print(axisSpeed[0]); - Serial.print("\n"); -*/ + //axisActive[0] = false; + //axisActive[1] = false; + //axisActive[2] = false; storeEndStops(); @@ -843,7 +392,7 @@ int StepperControl::moveAbsoluteConstant( long xDest, long yDest, long zDest, } if (error == 1) { -Serial.print("R99 error\n"); + //Serial.print("R99 error\n"); Timer1.stop(); axisActive[0] = false; axisActive[1] = false; @@ -859,7 +408,7 @@ Serial.print("R99 error\n"); } -/**/Serial.print("R99 stopped\n"); + /**/Serial.print("R99 stopped\n"); Timer1.stop(); enableMotors(false); @@ -881,7 +430,9 @@ Serial.print("R99 error\n"); int StepperControl::calibrateAxis(int axis) { + int error = 1; +/* int parMotInv[3] = { 31, 32, 33}; int parEndInv[3] = { 21, 22, 23}; int parNbrStp[3] = {801,802,803}; @@ -940,7 +491,7 @@ int StepperControl::calibrateAxis(int axis) { Serial.print("R99 Calibration error: end stop active before start\n"); return 1; } - +*/ /* @@ -950,7 +501,7 @@ Serial.print("calibration start"); Serial.print("\n"); */ - +/* // Move towards home enableMotors(true); @@ -1006,7 +557,7 @@ Serial.print("\n"); } } } - +*/ /* Serial.print("R99"); @@ -1023,7 +574,7 @@ Serial.print(error); Serial.print("\n"); */ - +/* // Report back the end stop setting if (error == 0) { @@ -1102,7 +653,7 @@ Serial.print("\n"); } - +*/ /* Serial.print("R99"); @@ -1120,7 +671,7 @@ Serial.print("\n"); */ - +/* // Report back the end stop setting if (error == 0) { @@ -1153,7 +704,7 @@ Serial.print("\n"); } reportPosition(); - +*/ return error; } diff --git a/src/StepperControl.h b/src/StepperControl.h index 0f4bba9..10f3081 100644 --- a/src/StepperControl.h +++ b/src/StepperControl.h @@ -19,6 +19,10 @@ class StepperControl { public: + StepperControl(); + StepperControl(StepperControl const&); + void operator=(StepperControl const&); + static StepperControl* getInstance(); int moveAbsolute(long xDest, long yDest,long zDest, unsigned int maxStepsPerSecond, @@ -31,9 +35,20 @@ public: int calibrateAxis(int axis); void initInterrupt(); private: - StepperControl(); - StepperControl(StepperControl const&); - void operator=(StepperControl const&); + StepperControlAxis axisX; + StepperControlAxis axisY; + StepperControlAxis axisZ; + + bool axisActive[3]; + + void reportPosition(); + void storeEndStops(); + void reportEndStops(); + + unsigned long getMaxLength(unsigned long lengths[3]); + void enableMotors(bool enable); + bool endStopsReached(); + }; #endif /* STEPPERCONTROL_H_ */ diff --git a/src/StepperControlAxis.cpp b/src/StepperControlAxis.cpp index a9ac746..e6005b2 100644 --- a/src/StepperControlAxis.cpp +++ b/src/StepperControlAxis.cpp @@ -16,12 +16,15 @@ StepperControlAxis::StepperControlAxis() { } bool StepperControlAxis::endStopMin() { - return digitalRead(pinMin); + return digitalRead(pinMin); } bool StepperControlAxis::endStopMax() { + return digitalRead(pinMax); +} - return digitalRead(pinMax); +bool StepperControlAxis::isAxisActive() { + return axisActive; } bool StepperControlAxis::endStopAxisReached(bool movement_forward) { @@ -168,7 +171,6 @@ unsigned int StepperControlAxis::calculateSpeed(long sourcePosition, long curren void StepperControlAxis::enableMotors(bool enable) { if (enable) { digitalWrite(pinEnable, LOW); - delay(100); } else { digitalWrite(pinEnable, HIGH); } @@ -195,30 +197,30 @@ bool StepperControlAxis::endStopsReached() { return ((digitalRead(pinMin) == motorEndStopInv) || (digitalRead(pinMax) == motorEndStopInv)); } -void StepperControlAxis::reportEndStops() { - CurrentState::getInstance()->printEndStops(); -} +//void StepperControlAxis::reportEndStops() { +// CurrentState::getInstance()->printEndStops(); +//} -void StepperControlAxis::reportPosition(){ - CurrentState::getInstance()->printPosition(); -} +//void StepperControlAxis::reportPosition(){ +// CurrentState::getInstance()->printPosition(); +//} -void StepperControlAxis::storeEndStops() { - CurrentState::getInstance()->storeEndStops(); -} +//void StepperControlAxis::storeEndStops() { +// CurrentState::getInstance()->storeEndStops(); +//} void StepperControlAxis::checkAxisDirection() { - if (homeAxis){ + if (coordHomeAxis){ // When home is active, the direction is fixed movementUp = motorHomeIsUp; movementToHome = true; } else { // For normal movement, move in direction of destination - movementUp = ( currentPoint < destinationPoint ); - movementToHome = (abs(currentPoint) >= abs(destinationPoint)); + movementUp = ( coordCurrentPoint < coordDestinationPoint ); + movementToHome = (abs(coordCurrentPoint) >= abs(coordDestinationPoint)); } - if (currentPoint == 0) { + if (coordCurrentPoint == 0) { // Go slow when theoretical end point reached but there is no end stop siganl axisSpeed = motorSpeedMin; } @@ -226,19 +228,19 @@ void StepperControlAxis::checkAxisDirection() { void StepperControlAxis::stepAxis() { - if (homeAxis && currentPoint == 0) { + if (coordHomeAxis && coordCurrentPoint == 0) { // Keep moving toward end stop even when position is zero // but end stop is not yet active if (motorHomeIsUp) { - currentPoint = -1; + coordCurrentPoint = -1; } else { - currentPoint = 1; + coordCurrentPoint = 1; } } // set a step on the motors - step(currentPoint, 1, destinationPoint); + step(coordCurrentPoint, 1, coordDestinationPoint); //stepMade = true; //lastStepMillis[i] = currentMillis; @@ -276,7 +278,7 @@ Serial.print("\n"); //if ((!pointReached(currentPoint, destinationPoint) || home) && axisActive[i]) { - if (((currentPoint != destinationPoint) || homeAxis) && axisActive) { + if (((coordCurrentPoint != coordDestinationPoint) || coordHomeAxis) && axisActive) { //Serial.print("R99 point not reached yet\n"); // home or destination not reached, keep moving /* @@ -287,7 +289,7 @@ Serial.print("\n"); */ // Get the axis speed, in steps per second - axisSpeed = calculateSpeed( sourcePoint,currentPoint,destinationPoint, + axisSpeed = calculateSpeed( coordSourcePoint, coordCurrentPoint, coordDestinationPoint, motorSpeedMin, motorSpeedMax, motorStepsAcc); //Serial.print("R99 axis speed "); @@ -306,7 +308,7 @@ Serial.print(!endStopAxisReached(i, !movementToHome[i])); Serial.print("\n"); */ // If end stop reached, don't move anymore - if ((homeAxis && !endStopAxisReached(false)) || (!homeAxis && !endStopAxisReached(!movementToHome))) { + if ((coordHomeAxis && !endStopAxisReached(false)) || (!coordHomeAxis && !endStopAxisReached(!movementToHome))) { // Set the moments when the step is set to true and false @@ -331,7 +333,7 @@ Serial.print("\n"); // If end stop for home is active, set the position to zero if (endStopAxisReached(false)) { - currentPoint = 0; + coordCurrentPoint = 0; } @@ -381,12 +383,22 @@ void StepperControlAxis::StepperControlAxis::loadPinNumbers(int step, int dir, i } -void StepperControlAxis::loadMotorSettings(long speedMax, long speedMin, long stepsAcc, long timeOut, bool homeIsUp, bool motorInv, bool endStInv) { - motorSpeedMax = speedMax; - motorSpeedMin = speedMin; - motorStepsAcc = stepsAcc; - motorTimeOut = timeOut; - motorHomeIsUp = homeIsUp; - motorMotorInv = motorInv; - motorEndStopInv = endStInv; +void StepperControlAxis::loadMotorSettings(long speedMax, long speedMin, long stepsAcc, long timeOut, bool homeIsUp, bool motorInv, bool endStInv, long interruptSpeed) { + motorSpeedMax = speedMax; + motorSpeedMin = speedMin; + motorStepsAcc = stepsAcc; + motorTimeOut = timeOut; + motorHomeIsUp = homeIsUp; + motorMotorInv = motorInv; + motorEndStopInv = endStInv; + motorInterruptSpeed = interruptSpeed; } + +void StepperControlAxis::loadCoordinates(long sourcePoint, long destinationPoint, bool home) { + + coordSourcePoint = sourcePoint; + coordCurrentPoint = destinationPoint; + coordHomeAxis = home; + +} + diff --git a/src/StepperControlAxis.h b/src/StepperControlAxis.h index 4e870d6..e219a8b 100644 --- a/src/StepperControlAxis.h +++ b/src/StepperControlAxis.h @@ -20,11 +20,7 @@ class StepperControlAxis { public: - long interruptSpeed; - - long sourcePoint; - long currentPoint; - long destinationPoint; + StepperControlAxis(); unsigned long movementLength; unsigned long maxLenth; @@ -32,15 +28,25 @@ public: unsigned long stepOnTick; unsigned long stepOffTick; unsigned long axisSpeed; - double lengthRatio; + //double lengthRatio; bool axisActive; bool movementUp; bool movementToHome; - bool home; - bool homeAxis; + //bool homeAxis; + void enableMotors(bool enable); + void checkAxis(); void checkTicksAxis(); + void loadPinNumbers(int step, int dir, int enable, int min, int max); + void loadMotorSettings( long speedMax, long speedMin, long stepsAcc, long timeOut, bool homeIsUp, bool motorInv, bool endStInv, long interruptSpeed); + void loadCoordinates(long sourcePoint, long destinationPoint, bool home); + bool isAxisActive(); + bool endStopAxisReached(bool movement_forward); + bool endStopMin(); + bool endStopMax(); + bool endStopsReached(); + private: int lastCalcLog = 0; @@ -60,32 +66,26 @@ private: bool motorHomeIsUp; bool motorMotorInv; bool motorEndStopInv; + long motorInterruptSpeed; - void loadPinNumbers(int step, int dir, int enable, int min, int max); - void loadMotorSettings( long speedMax, long speedMin, long stepsAcc, long timeOut, bool homeIsUp, bool motorInv, bool endStInv); + long coordSourcePoint; + long coordCurrentPoint; + long coordDestinationPoint; + bool coordHomeAxis; - - bool endStopMin(); - bool endStopMax(); - bool endStopAxisReached(bool movement_forward); void step(long ¤tPoint, unsigned long steps, long destinationPoint); void stepAxis(); void resetStep(); bool pointReached(long currentPoint, long destinationPoint); unsigned int calculateSpeed(long sourcePosition, long currentPosition, long destinationPosition, long minSpeed, long maxSpeed, long stepsAccDec); - void enableMotors(bool enable); void setDirectionAxis(long currentPoint, long destinationPoint, bool goHome, bool homeIsUp, bool motorInv); //void setDirections(long* currentPoint, long* destinationPoint, bool* homeAxis, bool* motorInv); unsigned long getLength(long l1, long l2); - bool endStopsReached(); - void reportPosition(); - void storeEndStops(); - void reportEndStops(); + //void reportPosition(); + //void storeEndStops(); + //void reportEndStops(); void checkAxisDirection(); - void checkAxis(); - - StepperControlAxis(); //StepperControlAxis(StepperControlAxis const&); //void operator=(StepperControlAxis const&); }; From 38feaf344533d0e0c09ee403e3c690aba3ce482b Mon Sep 17 00:00:00 2001 From: TimEvWw Date: Sun, 2 Aug 2015 21:16:35 -0100 Subject: [PATCH 4/8] renaming methods in motor control --- src/StepperControl.cpp | 317 +++++++++++++++---------------------- src/StepperControl.h | 4 +- src/StepperControlAxis.cpp | 46 ++---- src/StepperControlAxis.h | 45 +++--- 4 files changed, 165 insertions(+), 247 deletions(-) diff --git a/src/StepperControl.cpp b/src/StepperControl.cpp index 21f62d3..9a8d970 100644 --- a/src/StepperControl.cpp +++ b/src/StepperControl.cpp @@ -13,57 +13,7 @@ StepperControl * StepperControl::getInstance() { const int MOVEMENT_INTERRUPT_SPEED = 100; // Interrupt cycle in micro seconds -int test = 0; - -long sourcePoint[3] = {0,0,0}; -long currentPoint[3] = {0,0,0}; -long destinationPoint[3] = {0,0,0}; -//unsigned long movementLength[3] = {0,0,0}; -unsigned long maxLenth = 0; -//unsigned long moveTicks[3] = {0,0,0}; -//unsigned long stepOnTick[3] = {0,0,0}; -//unsigned long stepOffTick[3] = {0,0,0}; -//unsigned long axisSpeed[3] = {0,0,0}; -//double lengthRatio[3] = {0,0,0}; -long speedMax[3] = {0,0,0}; -//long speedMin[3] = {0,0,0}; -//long stepsAcc[3] = {0,0,0}; -//long timeOut[3] = {0,0,0}; -//bool homeIsUp[3] = {false,false,false}; -//bool motorInv[3] = {false,false,false}; -//bool endStInv[3] = {false,false,false}; -//bool homeAxis[3] = {false,false,false}; -//bool axisActive[3] = {false,false,false}; -//bool movementUp[3] = {false,false,false}; -//bool movementToHome[3] = {false,false,false}; -//bool home = false; - - -// ** test code - -/* -// The interrupt will blink the LED, and keep -// track of how many times it has blinked. -int ledState = LOW; -//volatile unsigned long blinkCount = 0; // use volatile for shared variables - -void blinkLed() { - if (ledState == LOW) { - ledState = HIGH; -//Serial.print("R99 led on"); - } else { - ledState = LOW; -//Serial.print("R99 led off"); - } - digitalWrite(LED_PIN, ledState); -} - -*/ - StepperControl::StepperControl() { - //axisX = new StepperControlAxis(); - //axisY = new StepperControlAxis(); - //axisZ = new StepperControlAxis(); axisX = StepperControlAxis(); axisY = StepperControlAxis(); axisZ = StepperControlAxis(); @@ -73,95 +23,6 @@ StepperControl::StepperControl() { axisZ.loadPinNumbers(Z_STEP_PIN, Z_DIR_PIN, Z_ENABLE_PIN, Z_MIN_PIN, Z_MAX_PIN); } -unsigned long StepperControl::getMaxLength(unsigned long lengths[3]) { - unsigned long max = lengths[0]; - for (int i = 1; i < 3; i++) { - if (lengths[i] > max) { - max = lengths[i]; - } - } - return max; -} - -void StepperControl::enableMotors(bool enable) { - axisX.enableMotors(enable); - axisY.enableMotors(enable); - axisZ.enableMotors(enable); - delay(100); -} - -bool StepperControl::endStopsReached() { - - if ( axisX.endStopsReached() || - axisY.endStopsReached() || - axisZ.endStopsReached()) { - return true; - } - return false; - -} - -void StepperControl::reportEndStops() { - CurrentState::getInstance()->printEndStops(); -} - -void StepperControl::reportPosition(){ - CurrentState::getInstance()->printPosition(); -} - -void StepperControl::storeEndStops() { - CurrentState::getInstance()->storeEndStops(); -} - -/** - * water is dosed by setting the pin for the water high for a number of miliseconds - * - */ - -//void StepperControl::doseWaterByTime(long time) { -// digitalWrite(HEATER_1_PIN, HIGH); -// delay(time); -// digitalWrite(HEATER_1_PIN, LOW); -//} - - -// Handle movement by checking each axis -void StepperControl::handleMovementInterrupt(void){ -//Serial.print("R99 interrupt\n"); - -//Serial.print("R99 interrupt data "); -//Serial.print(" destination point "); -//Serial.print(destinationPoint[0]); -//Serial.print(" "); -//Serial.print("test "); -//Serial.print(test); -//Serial.print("\n"); - - axisX.checkTicksAxis(); - axisY.checkTicksAxis(); - axisZ.checkTicksAxis(); -} - -bool interruptBusy = false; -void handleMovementInterruptTest(void) { - if (interruptBusy == false) { - interruptBusy = true; - StepperControl::getInstance()->handleMovementInterrupt(); - //blinkLed(); - interruptBusy = false; - } -} - -// Start the interrupt used for moviing -// Interrupt management code library written by Paul Stoffregen -void StepperControl::initInterrupt() { - //Timer1.attachInterrupt(StepperControl::getInstance()->handleMovementInterrupt); - Timer1.attachInterrupt(handleMovementInterruptTest); - Timer1.initialize(MOVEMENT_INTERRUPT_SPEED); - Timer1.stop(); -} - - /** * xDest - destination X in steps * yDest - destination Y in steps @@ -173,12 +34,6 @@ int StepperControl::moveAbsoluteConstant( long xDest, long yDest, long zDest, unsigned int xMaxSpd, unsigned int yMaxSpd, unsigned int zMaxSpd, bool xHome, bool yHome, bool zHome) { - -//Serial.print("R99 "); -//Serial.print(" xMaxSpd "); -//Serial.print(xMaxSpd); -//Serial.print("\n"); - // Load settings bool homeIsUp[3] = {false,false,false}; @@ -216,26 +71,12 @@ int StepperControl::moveAbsoluteConstant( long xDest, long yDest, long zDest, timeOut[1] = ParameterList::getInstance()->getValue(MOVEMENT_TIMEOUT_X); timeOut[2] = ParameterList::getInstance()->getValue(MOVEMENT_TIMEOUT_X); -//Serial.print("R99 "); -//Serial.print(" after vars "); -//Serial.print(destinationPoint[0]); -//Serial.print("\n"); - unsigned long currentMillis = 0; - unsigned long timeStart = millis(); - //bool movementDone = false; - //bool movementUp = false; - //bool movementToHome = false; - - //bool moving = false; - - //bool stepMade = false; int incomingByte = 0; - //int axisSpeed = 0; int error = 0; // if a speed is given in the command, use that instead of the config speed @@ -285,44 +126,23 @@ int StepperControl::moveAbsoluteConstant( long xDest, long yDest, long zDest, axisY.loadMotorSettings(speedMax[1], speedMin[1], stepsAcc[1], timeOut[1], homeIsUp[1], motorInv[1], endStInv[1], MOVEMENT_INTERRUPT_SPEED); axisZ.loadMotorSettings(speedMax[2], speedMin[2], stepsAcc[2], timeOut[2], homeIsUp[2], motorInv[2], endStInv[2], MOVEMENT_INTERRUPT_SPEED); - // Prepare for movement + // Load coordinates + long sourcePoint[3] = {0,0,0}; sourcePoint[0] = CurrentState::getInstance()->getX(); sourcePoint[1] = CurrentState::getInstance()->getY(); sourcePoint[2] = CurrentState::getInstance()->getZ(); + long currentPoint[3] = {0,0,0}; currentPoint[0] = CurrentState::getInstance()->getX(); currentPoint[1] = CurrentState::getInstance()->getY(); currentPoint[2] = CurrentState::getInstance()->getZ(); + long destinationPoint[3]= {0,0,0}; destinationPoint[0] = xDest; destinationPoint[1] = yDest; destinationPoint[2] = zDest; - //movementLength[0] = getLength(destinationPoint[0], currentPoint[0]); - //movementLength[1] = getLength(destinationPoint[1], currentPoint[1]); - //movementLength[2] = getLength(destinationPoint[2], currentPoint[2]); - - //maxLenth = getMaxLength(movementLength); - - //home = xHome || yHome || zHome; - - - axisX.loadCoordinates(currentPoint[0],destinationPoint[0],xHome); - axisY.loadCoordinates(currentPoint[1],destinationPoint[1],yHome); - axisZ.loadCoordinates(currentPoint[2],destinationPoint[2],zHome); - - //axisX.setDirection() - //axisY.setDirection() - //axisZ.setDirection() - - storeEndStops(); - reportEndStops(); - - enableMotors(true); - - - // Limit normal movmement to the home position for (int i = 0; i < 3; i++) { if (!homeIsUp[i] && destinationPoint[i] < 0) { @@ -334,15 +154,30 @@ int StepperControl::moveAbsoluteConstant( long xDest, long yDest, long zDest, } } + axisX.loadCoordinates(currentPoint[0],destinationPoint[0],xHome); + axisY.loadCoordinates(currentPoint[1],destinationPoint[1],yHome); + axisZ.loadCoordinates(currentPoint[2],destinationPoint[2],zHome); + + // Prepare for movement + + //axisX.setDirection() + //axisY.setDirection() + //axisZ.setDirection() + + storeEndStops(); + reportEndStops(); + + enableMotors(); + // Start movement axisActive[0] = true; axisActive[1] = true; axisActive[2] = true; - axisX.checkAxis(); - axisY.checkAxis(); - axisZ.checkAxis(); + axisX.checkMovement(); + axisY.checkMovement(); + axisZ.checkMovement(); //Timer1.start(); @@ -357,10 +192,9 @@ int StepperControl::moveAbsoluteConstant( long xDest, long yDest, long zDest, //Serial.print(speedMax[0]); //Serial.print("\n"); - - axisX.checkTicksAxis(); - axisY.checkTicksAxis(); - axisZ.checkTicksAxis(); + axisX.checkTiming(); + axisY.checkTiming(); + axisZ.checkTiming(); axisActive[0] = axisX.isAxisActive(); axisActive[1] = axisY.isAxisActive(); @@ -411,7 +245,7 @@ int StepperControl::moveAbsoluteConstant( long xDest, long yDest, long zDest, /**/Serial.print("R99 stopped\n"); Timer1.stop(); - enableMotors(false); + disableMotors(); CurrentState::getInstance()->setX(currentPoint[0]); CurrentState::getInstance()->setY(currentPoint[1]); @@ -708,3 +542,102 @@ Serial.print("\n"); return error; } + +// Handle movement by checking each axis +void StepperControl::handleMovementInterrupt(void){ +//Serial.print("R99 interrupt\n"); + +//Serial.print("R99 interrupt data "); +//Serial.print(" destination point "); +//Serial.print(destinationPoint[0]); +//Serial.print(" "); +//Serial.print("test "); +//Serial.print(test); +//Serial.print("\n"); + + axisX.checkTiming(); + axisY.checkTiming(); + axisZ.checkTiming(); +} + +bool interruptBusy = false; +void handleMovementInterruptTest(void) { + if (interruptBusy == false) { + interruptBusy = true; + StepperControl::getInstance()->handleMovementInterrupt(); + //blinkLed(); + interruptBusy = false; + } +} + +// Start the interrupt used for moviing +// Interrupt management code library written by Paul Stoffregen +void StepperControl::initInterrupt() { + //Timer1.attachInterrupt(StepperControl::getInstance()->handleMovementInterrupt); + Timer1.attachInterrupt(handleMovementInterruptTest); + Timer1.initialize(MOVEMENT_INTERRUPT_SPEED); + Timer1.stop(); +} + + + + +unsigned long StepperControl::getMaxLength(unsigned long lengths[3]) { + unsigned long max = lengths[0]; + for (int i = 1; i < 3; i++) { + if (lengths[i] > max) { + max = lengths[i]; + } + } + return max; +} + +void StepperControl::enableMotors() { + axisX.enableMotor(); + axisY.enableMotor(); + axisZ.enableMotor(); + delay(100); +} + +void StepperControl::disableMotors() { + axisX.disableMotor(); + axisY.disableMotor(); + axisZ.disableMotor(); + delay(100); +} + +bool StepperControl::endStopsReached() { + + if ( axisX.endStopsReached() || + axisY.endStopsReached() || + axisZ.endStopsReached()) { + return true; + } + return false; + +} + +void StepperControl::reportEndStops() { + CurrentState::getInstance()->printEndStops(); +} + +void StepperControl::reportPosition(){ + CurrentState::getInstance()->printPosition(); +} + +void StepperControl::storeEndStops() { + CurrentState::getInstance()->storeEndStops(); +} + +/** + * water is dosed by setting the pin for the water high for a number of miliseconds + * + */ + +//void StepperControl::doseWaterByTime(long time) { +// digitalWrite(HEATER_1_PIN, HIGH); +// delay(time); +// digitalWrite(HEATER_1_PIN, LOW); +//} + + diff --git a/src/StepperControl.h b/src/StepperControl.h index 10f3081..602afc1 100644 --- a/src/StepperControl.h +++ b/src/StepperControl.h @@ -34,6 +34,9 @@ public: void handleMovementInterrupt(); int calibrateAxis(int axis); void initInterrupt(); + void enableMotors(); + void disableMotors(); + private: StepperControlAxis axisX; StepperControlAxis axisY; @@ -46,7 +49,6 @@ private: void reportEndStops(); unsigned long getMaxLength(unsigned long lengths[3]); - void enableMotors(bool enable); bool endStopsReached(); }; diff --git a/src/StepperControlAxis.cpp b/src/StepperControlAxis.cpp index e6005b2..69dd747 100644 --- a/src/StepperControlAxis.cpp +++ b/src/StepperControlAxis.cpp @@ -168,12 +168,12 @@ unsigned int StepperControlAxis::calculateSpeed(long sourcePosition, long curren return newSpeed; } -void StepperControlAxis::enableMotors(bool enable) { - if (enable) { - digitalWrite(pinEnable, LOW); - } else { - digitalWrite(pinEnable, HIGH); - } +void StepperControlAxis::enableMotor() { + digitalWrite(pinEnable, HIGH); +} + +void StepperControlAxis::disableMotor() { + digitalWrite(pinEnable, LOW); } void StepperControlAxis::setDirectionAxis(long currentPoint, long destinationPoint, bool goHome, bool homeIsUp, bool motorInv) { @@ -197,18 +197,6 @@ bool StepperControlAxis::endStopsReached() { return ((digitalRead(pinMin) == motorEndStopInv) || (digitalRead(pinMax) == motorEndStopInv)); } -//void StepperControlAxis::reportEndStops() { -// CurrentState::getInstance()->printEndStops(); -//} - -//void StepperControlAxis::reportPosition(){ -// CurrentState::getInstance()->printPosition(); -//} - -//void StepperControlAxis::storeEndStops() { -// CurrentState::getInstance()->storeEndStops(); -//} - void StepperControlAxis::checkAxisDirection() { if (coordHomeAxis){ // When home is active, the direction is fixed @@ -226,7 +214,7 @@ void StepperControlAxis::checkAxisDirection() { } } -void StepperControlAxis::stepAxis() { +void StepperControlAxis::stepMotor() { if (coordHomeAxis && coordCurrentPoint == 0) { @@ -241,13 +229,9 @@ void StepperControlAxis::stepAxis() { // set a step on the motors step(coordCurrentPoint, 1, coordDestinationPoint); - - //stepMade = true; - //lastStepMillis[i] = currentMillis; - } -void StepperControlAxis::checkAxis() { +void StepperControlAxis::checkMovement() { int i = 1; @@ -339,7 +323,7 @@ Serial.print("\n"); } -void StepperControlAxis::checkTicksAxis() { +void StepperControlAxis::checkTiming() { int i; @@ -348,15 +332,15 @@ void StepperControlAxis::checkTicksAxis() { if (axisActive) { if (moveTicks >= stepOffTick) { -/* + Serial.print("R99 reset step "); Serial.print(" moveTicks "); -Serial.print(moveTicks[i]); +Serial.print(moveTicks); Serial.print("\n"); -*/ + // Negative flank for the steps resetStep(); - checkAxis(); + checkMovement(); } else { @@ -368,7 +352,7 @@ Serial.print(moveTicks[i]); Serial.print("\n"); */ // Positive flank for the steps - stepAxis(); + stepMotor(); } } } @@ -400,5 +384,7 @@ void StepperControlAxis::loadCoordinates(long sourcePoint, long destinationPoint coordCurrentPoint = destinationPoint; coordHomeAxis = home; + moveTicks = 0; + } diff --git a/src/StepperControlAxis.h b/src/StepperControlAxis.h index e219a8b..cf06da6 100644 --- a/src/StepperControlAxis.h +++ b/src/StepperControlAxis.h @@ -22,30 +22,21 @@ public: StepperControlAxis(); - unsigned long movementLength; - unsigned long maxLenth; - unsigned long moveTicks; - unsigned long stepOnTick; - unsigned long stepOffTick; - unsigned long axisSpeed; - //double lengthRatio; - bool axisActive; - bool movementUp; - bool movementToHome; - //bool homeAxis; - - void enableMotors(bool enable); - void checkAxis(); - void checkTicksAxis(); - void loadPinNumbers(int step, int dir, int enable, int min, int max); void loadMotorSettings( long speedMax, long speedMin, long stepsAcc, long timeOut, bool homeIsUp, bool motorInv, bool endStInv, long interruptSpeed); void loadCoordinates(long sourcePoint, long destinationPoint, bool home); + + void enableMotor(); + void disableMotor(); + void checkMovement(); + void checkTiming(); + + bool isAxisActive(); - bool endStopAxisReached(bool movement_forward); bool endStopMin(); bool endStopMax(); bool endStopsReached(); + bool endStopAxisReached(bool movement_forward); private: @@ -68,26 +59,32 @@ private: bool motorEndStopInv; long motorInterruptSpeed; + // coordinates long coordSourcePoint; long coordCurrentPoint; long coordDestinationPoint; bool coordHomeAxis; + // movement handling + unsigned long movementLength; + unsigned long maxLenth; + unsigned long moveTicks; + unsigned long stepOnTick; + unsigned long stepOffTick; + unsigned long axisSpeed; + bool axisActive; + bool movementUp; + bool movementToHome; + void step(long ¤tPoint, unsigned long steps, long destinationPoint); - void stepAxis(); + void stepMotor(); void resetStep(); bool pointReached(long currentPoint, long destinationPoint); unsigned int calculateSpeed(long sourcePosition, long currentPosition, long destinationPosition, long minSpeed, long maxSpeed, long stepsAccDec); void setDirectionAxis(long currentPoint, long destinationPoint, bool goHome, bool homeIsUp, bool motorInv); - //void setDirections(long* currentPoint, long* destinationPoint, bool* homeAxis, bool* motorInv); unsigned long getLength(long l1, long l2); - //void reportPosition(); - //void storeEndStops(); - //void reportEndStops(); void checkAxisDirection(); - //StepperControlAxis(StepperControlAxis const&); - //void operator=(StepperControlAxis const&); }; #endif /* STEPPERCONTROLAXIS_H_ */ From 4c0d147c53602f5d464455f8e4620023fd2e33e4 Mon Sep 17 00:00:00 2001 From: TimEvWw Date: Mon, 3 Aug 2015 20:12:49 -0100 Subject: [PATCH 5/8] renamig methods in motor control --- src/F11Handler.cpp | 2 +- src/F12Handler.cpp | 2 +- src/F13Handler.cpp | 2 +- src/F14Handler.cpp | 2 +- src/G00Handler.cpp | 3 +- src/G28Handler.cpp | 4 +- src/StepperControl.cpp | 2 +- src/StepperControl.h | 8 +- src/StepperControlAxis.cpp | 154 ++++++++++++++++++------------------- src/StepperControlAxis.h | 6 +- 10 files changed, 92 insertions(+), 93 deletions(-) diff --git a/src/F11Handler.cpp b/src/F11Handler.cpp index 03d69b2..7cc1853 100644 --- a/src/F11Handler.cpp +++ b/src/F11Handler.cpp @@ -28,7 +28,7 @@ int F11Handler::execute(Command* command) { Serial.print("R99 HOME X\n"); } - StepperControl::getInstance()->moveAbsoluteConstant(0,0,0, 0,0,0, true, false, false); + StepperControl::getInstance()->moveToCoords(0,0,0, 0,0,0, true, false, false); if (LOGGING) { CurrentState::getInstance()->print(); } diff --git a/src/F12Handler.cpp b/src/F12Handler.cpp index 644af93..71673c2 100644 --- a/src/F12Handler.cpp +++ b/src/F12Handler.cpp @@ -28,7 +28,7 @@ int F12Handler::execute(Command* command) { Serial.print("R99 HOME Y\n"); } - StepperControl::getInstance()->moveAbsoluteConstant(0,0,0, 0,0,0, false, true, false); + StepperControl::getInstance()->moveToCoords(0,0,0, 0,0,0, false, true, false); if (LOGGING) { CurrentState::getInstance()->print(); } diff --git a/src/F13Handler.cpp b/src/F13Handler.cpp index 5919b7a..8f69c17 100644 --- a/src/F13Handler.cpp +++ b/src/F13Handler.cpp @@ -28,7 +28,7 @@ int F13Handler::execute(Command* command) { Serial.print("R99 HOME Z\n"); } - StepperControl::getInstance()->moveAbsoluteConstant(0,0,0, 0,0,0, false, false, true); + StepperControl::getInstance()->moveToCoords(0,0,0, 0,0,0, false, false, true); if (LOGGING) { CurrentState::getInstance()->print(); } diff --git a/src/F14Handler.cpp b/src/F14Handler.cpp index bee8694..15a3282 100644 --- a/src/F14Handler.cpp +++ b/src/F14Handler.cpp @@ -33,7 +33,7 @@ int F14Handler::execute(Command* command) { ret = StepperControl::getInstance()->calibrateAxis(0); if (ret == 0) { - StepperControl::getInstance()->moveAbsoluteConstant(0,0,0, 0,0,0, true, false, false); + StepperControl::getInstance()->moveToCoords(0,0,0, 0,0,0, true, false, false); } if (LOGGING) { diff --git a/src/G00Handler.cpp b/src/G00Handler.cpp index 0b3a474..3ec4b20 100644 --- a/src/G00Handler.cpp +++ b/src/G00Handler.cpp @@ -40,9 +40,8 @@ int G00Handler::execute(Command* command) { // Serial.print("\n"); - StepperControl::getInstance()->moveAbsoluteConstant( + StepperControl::getInstance()->moveToCoords( command->getX(), command->getY(), command->getZ(), - //0,0,0, command->getA(), command->getB(), command->getC(), false, false, false); if (LOGGING) { diff --git a/src/G28Handler.cpp b/src/G28Handler.cpp index 219e12c..37eec5b 100644 --- a/src/G28Handler.cpp +++ b/src/G28Handler.cpp @@ -25,8 +25,8 @@ int G28Handler::execute(Command* command) { Serial.print("home\n"); - StepperControl::getInstance()->moveAbsoluteConstant(0,0,0, 0,0,0, false, false, true); - StepperControl::getInstance()->moveAbsoluteConstant(0,0,0, 0,0,0, true, true, false); + StepperControl::getInstance()->moveToCoords(0,0,0, 0,0,0, false, false, true); + StepperControl::getInstance()->moveToCoords(0,0,0, 0,0,0, true, true, false); //StepperControl::getInstance()->moveAbsoluteConstant(0,0,0,HOME_MOVEMENT_SPEED_S_P_S,true); if (LOGGING) { CurrentState::getInstance()->print(); diff --git a/src/StepperControl.cpp b/src/StepperControl.cpp index 9a8d970..f58602a 100644 --- a/src/StepperControl.cpp +++ b/src/StepperControl.cpp @@ -30,7 +30,7 @@ StepperControl::StepperControl() { * maxStepsPerSecond - maximum number of steps per second * maxAccelerationStepsPerSecond - maximum number of acceleration in steps per second */ -int StepperControl::moveAbsoluteConstant( long xDest, long yDest, long zDest, +int StepperControl::moveToCoords( long xDest, long yDest, long zDest, unsigned int xMaxSpd, unsigned int yMaxSpd, unsigned int zMaxSpd, bool xHome, bool yHome, bool zHome) { diff --git a/src/StepperControl.h b/src/StepperControl.h index 602afc1..341c0dd 100644 --- a/src/StepperControl.h +++ b/src/StepperControl.h @@ -24,10 +24,10 @@ public: void operator=(StepperControl const&); static StepperControl* getInstance(); - int moveAbsolute(long xDest, long yDest,long zDest, - unsigned int maxStepsPerSecond, - unsigned int maxAccelerationStepsPerSecond); - int moveAbsoluteConstant(long xDest, long yDest, long zDest, + //int moveAbsolute( long xDest, long yDest,long zDest, + // unsigned int maxStepsPerSecond, + // unsigned int maxAccelerationStepsPerSecond); + int moveToCoords( long xDest, long yDest, long zDest, unsigned int xMaxSpd, unsigned int yMaxSpd, unsigned int zMaxSpd, bool homeX, bool homeY, bool homeZ); diff --git a/src/StepperControlAxis.cpp b/src/StepperControlAxis.cpp index 69dd747..aa67a3f 100644 --- a/src/StepperControlAxis.cpp +++ b/src/StepperControlAxis.cpp @@ -15,18 +15,6 @@ StepperControlAxis::StepperControlAxis() { } -bool StepperControlAxis::endStopMin() { - return digitalRead(pinMin); -} - -bool StepperControlAxis::endStopMax() { - return digitalRead(pinMax); -} - -bool StepperControlAxis::isAxisActive() { - return axisActive; -} - bool StepperControlAxis::endStopAxisReached(bool movement_forward) { bool min_endstop = false; @@ -56,32 +44,35 @@ bool StepperControlAxis::endStopAxisReached(bool movement_forward) { return 0; } -void StepperControlAxis::StepperControlAxis::step(long ¤tPoint, unsigned long steps, - long destinationPoint) { +void StepperControlAxis::setStepAxis() { - if (currentPoint < destinationPoint) { - currentPoint += steps; - } else if (currentPoint > destinationPoint) { - currentPoint -= steps; + if (coordHomeAxis && coordCurrentPoint == 0) { + + // Keep moving toward end stop even when position is zero + // but end stop is not yet active + if (motorHomeIsUp) { + coordCurrentPoint = -1; + } else { + coordCurrentPoint = 1; + } } - digitalWrite(pinStep, HIGH); + if (coordCurrentPoint < coordDestinationPoint) { + coordCurrentPoint++; + } else if (coordCurrentPoint > coordDestinationPoint) { + coordCurrentPoint--; + } + + // set a step on the motors + setMotorStep(); // if the home end stop is reached, set the current position if (endStopAxisReached(false)) { - currentPoint = 0; + coordCurrentPoint = 0; } } -void StepperControlAxis::resetStep() { - digitalWrite(pinStep, LOW); -} - -bool StepperControlAxis::pointReached(long currentPoint, long destinationPoint) { - return (destinationPoint == currentPoint); -} - unsigned int StepperControlAxis::calculateSpeed(long sourcePosition, long currentPosition, long destinationPosition, long minSpeed, long maxSpeed, long stepsAccDec) { int newSpeed = 0; @@ -168,35 +159,6 @@ unsigned int StepperControlAxis::calculateSpeed(long sourcePosition, long curren return newSpeed; } -void StepperControlAxis::enableMotor() { - digitalWrite(pinEnable, HIGH); -} - -void StepperControlAxis::disableMotor() { - digitalWrite(pinEnable, LOW); -} - -void StepperControlAxis::setDirectionAxis(long currentPoint, long destinationPoint, bool goHome, bool homeIsUp, bool motorInv) { - - if (((!goHome && currentPoint < destinationPoint) || (goHome && homeIsUp)) ^ motorInv) { - digitalWrite(pinDirection, HIGH); - } else { - digitalWrite(pinDirection, LOW); - } -} - -unsigned long StepperControlAxis::getLength(long l1, long l2) { - if (l1 > l2) { - return l1 - l2; - } else { - return l2 - l1; - } -} - -bool StepperControlAxis::endStopsReached() { - return ((digitalRead(pinMin) == motorEndStopInv) || (digitalRead(pinMax) == motorEndStopInv)); -} - void StepperControlAxis::checkAxisDirection() { if (coordHomeAxis){ // When home is active, the direction is fixed @@ -214,23 +176,6 @@ void StepperControlAxis::checkAxisDirection() { } } -void StepperControlAxis::stepMotor() { - - if (coordHomeAxis && coordCurrentPoint == 0) { - - // Keep moving toward end stop even when position is zero - // but end stop is not yet active - if (motorHomeIsUp) { - coordCurrentPoint = -1; - } else { - coordCurrentPoint = 1; - } - } - - // set a step on the motors - step(coordCurrentPoint, 1, coordDestinationPoint); -} - void StepperControlAxis::checkMovement() { int i = 1; @@ -332,14 +277,14 @@ void StepperControlAxis::checkTiming() { if (axisActive) { if (moveTicks >= stepOffTick) { - +/* Serial.print("R99 reset step "); Serial.print(" moveTicks "); Serial.print(moveTicks); Serial.print("\n"); - +*/ // Negative flank for the steps - resetStep(); + resetMotorStep(); checkMovement(); } else { @@ -352,7 +297,7 @@ Serial.print(moveTicks[i]); Serial.print("\n"); */ // Positive flank for the steps - stepMotor(); + setStepAxis(); } } } @@ -388,3 +333,56 @@ void StepperControlAxis::loadCoordinates(long sourcePoint, long destinationPoint } +void StepperControlAxis::enableMotor() { + digitalWrite(pinEnable, HIGH); +} + +void StepperControlAxis::disableMotor() { + digitalWrite(pinEnable, LOW); +} + +void StepperControlAxis::setDirectionAxis(long currentPoint, long destinationPoint, bool goHome, bool homeIsUp, bool motorInv) { + + if (((!goHome && currentPoint < destinationPoint) || (goHome && homeIsUp)) ^ motorInv) { + digitalWrite(pinDirection, HIGH); + } else { + digitalWrite(pinDirection, LOW); + } +} + +unsigned long StepperControlAxis::getLength(long l1, long l2) { + if (l1 > l2) { + return l1 - l2; + } else { + return l2 - l1; + } +} + +bool StepperControlAxis::endStopsReached() { + return ((digitalRead(pinMin) == motorEndStopInv) || (digitalRead(pinMax) == motorEndStopInv)); +} + +bool StepperControlAxis::endStopMin() { + return digitalRead(pinMin); +} + +bool StepperControlAxis::endStopMax() { + return digitalRead(pinMax); +} + +bool StepperControlAxis::isAxisActive() { + return axisActive; +} + +void StepperControlAxis::setMotorStep() { + digitalWrite(pinStep, HIGH); +} + +void StepperControlAxis::resetMotorStep() { + digitalWrite(pinStep, LOW); +} + +bool StepperControlAxis::pointReached(long currentPoint, long destinationPoint) { + return (destinationPoint == currentPoint); +} + diff --git a/src/StepperControlAxis.h b/src/StepperControlAxis.h index cf06da6..492e381 100644 --- a/src/StepperControlAxis.h +++ b/src/StepperControlAxis.h @@ -38,6 +38,9 @@ public: bool endStopsReached(); bool endStopAxisReached(bool movement_forward); + void setMotorStep(); + void resetMotorStep(); + private: int lastCalcLog = 0; @@ -76,9 +79,8 @@ private: bool movementUp; bool movementToHome; + void setStepAxis(); void step(long ¤tPoint, unsigned long steps, long destinationPoint); - void stepMotor(); - void resetStep(); bool pointReached(long currentPoint, long destinationPoint); unsigned int calculateSpeed(long sourcePosition, long currentPosition, long destinationPosition, long minSpeed, long maxSpeed, long stepsAccDec); void setDirectionAxis(long currentPoint, long destinationPoint, bool goHome, bool homeIsUp, bool motorInv); From cc0be0517429c717abbf4ba059bce19850822994 Mon Sep 17 00:00:00 2001 From: TimEvWw Date: Fri, 7 Aug 2015 20:30:24 -0100 Subject: [PATCH 6/8] refactoring calibration --- src/ParameterList.h | 6 +- src/StepperControl.cpp | 259 +++++++++++++++++-------------------- src/StepperControl.h | 8 ++ src/StepperControlAxis.cpp | 205 ++++++++++++++++++----------- src/StepperControlAxis.h | 30 +++-- 5 files changed, 274 insertions(+), 234 deletions(-) diff --git a/src/ParameterList.h b/src/ParameterList.h index 0a08fab..3b6d103 100644 --- a/src/ParameterList.h +++ b/src/ParameterList.h @@ -39,7 +39,11 @@ enum ParamListEnum MOVEMENT_MAX_SPD_X = 71, MOVEMENT_MAX_SPD_Y = 72, - MOVEMENT_MAX_SPD_Z = 73 + MOVEMENT_MAX_SPD_Z = 73, + + MOVEMENT_AXIS_NR_STEPS_X = 801, + MOVEMENT_AXIS_NR_STEPS_Y = 802, + MOVEMENT_AXIS_NR_STEPS_Z = 803 }; diff --git a/src/StepperControl.cpp b/src/StepperControl.cpp index f58602a..5c87be2 100644 --- a/src/StepperControl.cpp +++ b/src/StepperControl.cpp @@ -34,44 +34,6 @@ int StepperControl::moveToCoords( long xDest, long yDest, long zDest, unsigned int xMaxSpd, unsigned int yMaxSpd, unsigned int zMaxSpd, bool xHome, bool yHome, bool zHome) { - // Load settings - - bool homeIsUp[3] = {false,false,false}; - homeIsUp[0] = ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_X); - homeIsUp[1] = ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_Y); - homeIsUp[2] = ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_Z); - - long speedMax[3] = {0,0,0}; - speedMax[0] = ParameterList::getInstance()->getValue(MOVEMENT_MAX_SPD_X); - speedMax[1] = ParameterList::getInstance()->getValue(MOVEMENT_MAX_SPD_Y); - speedMax[2] = ParameterList::getInstance()->getValue(MOVEMENT_MAX_SPD_Z); - - long speedMin[3] = {0,0,0}; - speedMin[0] = ParameterList::getInstance()->getValue(MOVEMENT_MIN_SPD_X); - speedMin[1] = ParameterList::getInstance()->getValue(MOVEMENT_MIN_SPD_Y); - speedMin[2] = ParameterList::getInstance()->getValue(MOVEMENT_MIN_SPD_Z); - - long stepsAcc[3] = {0,0,0}; - stepsAcc[0] = ParameterList::getInstance()->getValue(MOVEMENT_STEPS_ACC_DEC_X); - stepsAcc[1] = ParameterList::getInstance()->getValue(MOVEMENT_STEPS_ACC_DEC_Y); - stepsAcc[2] = ParameterList::getInstance()->getValue(MOVEMENT_STEPS_ACC_DEC_Z); - - bool motorInv[3] = {false,false,false}; - motorInv[0] = ParameterList::getInstance()->getValue(MOVEMENT_INVERT_MOTOR_X); - motorInv[1] = ParameterList::getInstance()->getValue(MOVEMENT_INVERT_MOTOR_Y); - motorInv[2] = ParameterList::getInstance()->getValue(MOVEMENT_INVERT_MOTOR_Z); - - bool endStInv[3] = {false,false,false}; - endStInv[0] = ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_X); - endStInv[1] = ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_Y); - endStInv[2] = ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_Z); - - long timeOut[3] = {0,0,0}; - timeOut[0] = ParameterList::getInstance()->getValue(MOVEMENT_TIMEOUT_X); - timeOut[1] = ParameterList::getInstance()->getValue(MOVEMENT_TIMEOUT_X); - timeOut[2] = ParameterList::getInstance()->getValue(MOVEMENT_TIMEOUT_X); - - unsigned long currentMillis = 0; unsigned long timeStart = millis(); @@ -79,8 +41,17 @@ int StepperControl::moveToCoords( long xDest, long yDest, long zDest, int incomingByte = 0; int error = 0; + // load motor settings + + loadMotorSettings(); + // if a speed is given in the command, use that instead of the config speed + long speedMax[3] = {0,0,0}; + speedMax[0] = xMaxSpd; + speedMax[1] = yMaxSpd; + speedMax[2] = zMaxSpd; + if (xMaxSpd > 0 && xMaxSpd < speedMax[0]) { speedMax[0] = xMaxSpd; } @@ -94,39 +65,11 @@ int StepperControl::moveToCoords( long xDest, long yDest, long zDest, } - homeIsUp[0] = ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_X); - homeIsUp[1] = ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_Y); - homeIsUp[2] = ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_Z); + axisX.setMaxSpeed(speedMax[0]); + axisY.setMaxSpeed(speedMax[1]); + axisZ.setMaxSpeed(speedMax[2]); - speedMax[0] = ParameterList::getInstance()->getValue(MOVEMENT_MAX_SPD_X); - speedMax[1] = ParameterList::getInstance()->getValue(MOVEMENT_MAX_SPD_Y); - speedMax[2] = ParameterList::getInstance()->getValue(MOVEMENT_MAX_SPD_Z); - - speedMin[0] = ParameterList::getInstance()->getValue(MOVEMENT_MIN_SPD_X); - speedMin[1] = ParameterList::getInstance()->getValue(MOVEMENT_MIN_SPD_Y); - speedMin[2] = ParameterList::getInstance()->getValue(MOVEMENT_MIN_SPD_Z); - - stepsAcc[0] = ParameterList::getInstance()->getValue(MOVEMENT_STEPS_ACC_DEC_X); - stepsAcc[1] = ParameterList::getInstance()->getValue(MOVEMENT_STEPS_ACC_DEC_Y); - stepsAcc[2] = ParameterList::getInstance()->getValue(MOVEMENT_STEPS_ACC_DEC_Z); - - motorInv[0] = ParameterList::getInstance()->getValue(MOVEMENT_INVERT_MOTOR_X); - motorInv[1] = ParameterList::getInstance()->getValue(MOVEMENT_INVERT_MOTOR_Y); - motorInv[2] = ParameterList::getInstance()->getValue(MOVEMENT_INVERT_MOTOR_Z); - - endStInv[0] = ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_X); - endStInv[1] = ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_Y); - endStInv[2] = ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_Z); - - timeOut[0] = ParameterList::getInstance()->getValue(MOVEMENT_TIMEOUT_X); - timeOut[1] = ParameterList::getInstance()->getValue(MOVEMENT_TIMEOUT_Y); - timeOut[2] = ParameterList::getInstance()->getValue(MOVEMENT_TIMEOUT_Z); - - axisX.loadMotorSettings(speedMax[0], speedMin[0], stepsAcc[0], timeOut[0], homeIsUp[0], motorInv[0], endStInv[0], MOVEMENT_INTERRUPT_SPEED); - axisY.loadMotorSettings(speedMax[1], speedMin[1], stepsAcc[1], timeOut[1], homeIsUp[1], motorInv[1], endStInv[1], MOVEMENT_INTERRUPT_SPEED); - axisZ.loadMotorSettings(speedMax[2], speedMin[2], stepsAcc[2], timeOut[2], homeIsUp[2], motorInv[2], endStInv[2], MOVEMENT_INTERRUPT_SPEED); - - // Load coordinates + // Load coordinates into axis class long sourcePoint[3] = {0,0,0}; sourcePoint[0] = CurrentState::getInstance()->getX(); @@ -143,16 +86,7 @@ int StepperControl::moveToCoords( long xDest, long yDest, long zDest, destinationPoint[1] = yDest; destinationPoint[2] = zDest; - // Limit normal movmement to the home position - for (int i = 0; i < 3; i++) { - if (!homeIsUp[i] && destinationPoint[i] < 0) { - destinationPoint[i] = 0; - } - - if ( homeIsUp[i] && destinationPoint[i] > 0) { - destinationPoint[i] = 0; - } - } + // Load coordinates into motor control axisX.loadCoordinates(currentPoint[0],destinationPoint[0],xHome); axisY.loadCoordinates(currentPoint[1],destinationPoint[1],yHome); @@ -176,8 +110,8 @@ int StepperControl::moveToCoords( long xDest, long yDest, long zDest, axisActive[2] = true; axisX.checkMovement(); - axisY.checkMovement(); - axisZ.checkMovement(); +// axisY.checkMovement(); +// axisZ.checkMovement(); //Timer1.start(); @@ -186,12 +120,16 @@ int StepperControl::moveToCoords( long xDest, long yDest, long zDest, //delay(50); delayMicroseconds(100); - - //Serial.print("R99 "); - //Serial.print(" maximim speed "); - //Serial.print(speedMax[0]); - //Serial.print("\n"); - +/* + Serial.print("R99 "); + Serial.print(" x axis active "); + Serial.print(axisActive[0]); + Serial.print(" y axis active "); + Serial.print(axisActive[1]); + Serial.print(" z axis active "); + Serial.print(axisActive[2]); + Serial.print("\n"); +*/ axisX.checkTiming(); axisY.checkTiming(); axisZ.checkTiming(); @@ -201,8 +139,8 @@ int StepperControl::moveToCoords( long xDest, long yDest, long zDest, axisActive[2] = axisZ.isAxisActive(); //axisActive[0] = false; - //axisActive[1] = false; - //axisActive[2] = false; + axisActive[1] = false; + axisActive[2] = false; storeEndStops(); @@ -247,6 +185,10 @@ int StepperControl::moveToCoords( long xDest, long yDest, long zDest, Timer1.stop(); disableMotors(); + currentPoint[0] = axisX.currentPoint(); + currentPoint[1] = axisY.currentPoint(); + currentPoint[2] = axisZ.currentPoint(); + CurrentState::getInstance()->setX(currentPoint[0]); CurrentState::getInstance()->setY(currentPoint[1]); CurrentState::getInstance()->setZ(currentPoint[2]); @@ -264,39 +206,16 @@ int StepperControl::moveToCoords( long xDest, long yDest, long zDest, int StepperControl::calibrateAxis(int axis) { - int error = 1; + // Load motor settings -/* - int parMotInv[3] = { 31, 32, 33}; - int parEndInv[3] = { 21, 22, 23}; - int parNbrStp[3] = {801,802,803}; + loadMotorSettings(); - bool motorInv[3] = { ParameterList::getInstance()->getValue(MOVEMENT_INVERT_MOTOR_X), - ParameterList::getInstance()->getValue(MOVEMENT_INVERT_MOTOR_Y), - ParameterList::getInstance()->getValue(MOVEMENT_INVERT_MOTOR_Z) }; - - bool homeAxis[3] = {false,false,false}; - - bool homeIsUp[3] = {false,false,false}; - - long speedMin[3] = {200,200,200}; - - bool endStInv[3] = {false,false,false}; - - long timeOut[3] = {5000,5000,5000}; - - long stepPin[3] = { X_STEP_PIN, - Y_STEP_PIN, - Z_STEP_PIN }; - - long dirPin[3] = { X_DIR_PIN, - Y_DIR_PIN, - Z_DIR_PIN }; + //long timeOut[3] = {5000,5000,5000}; // Set the coordinate variables for homing, so the default functions can be used for settign direction - long sourcePoint[3] = { 0, 0, 0 }; - long destinationPoint[3] = { 0, 0, 0 }; + //long sourcePoint[3] = { 0, 0, 0 }; + //long destinationPoint[3] = { 0, 0, 0 }; unsigned long currentMillis = 0; @@ -308,25 +227,50 @@ int StepperControl::calibrateAxis(int axis) { bool movementDone = false; int paramValueInt = 0; - bool invertEndStops = false; int stepsCount = 0; int incomingByte = 0; int error = 0; + bool invertEndStops = false; + int parEndInv; + int parNbrStp; // Prepare for movement storeEndStops(); reportEndStops(); + + // Select the right axis + StepperControlAxis calibAxis; + switch (axis) { + case 1: + calibAxis = axisX; + parEndInv = MOVEMENT_INVERT_ENDPOINTS_X; + parNbrStp = MOVEMENT_AXIS_NR_STEPS_X; + break; + case 2: + calibAxis = axisY; + parEndInv = MOVEMENT_INVERT_ENDPOINTS_Y;; + parNbrStp = MOVEMENT_AXIS_NR_STEPS_Y; + break; + case 3: + calibAxis = axisZ; + parEndInv = MOVEMENT_INVERT_ENDPOINTS_Z; + parNbrStp = MOVEMENT_AXIS_NR_STEPS_Z; + break; + default: + Serial.print("R99 Calibration error: invalid axis selected\n"); + return 1; + } + + // Preliminary checks - if (endStopMin(axis) || endStopMax(axis)) { + if (calibAxis.endStopMin() || calibAxis.endStopMax()) { Serial.print("R99 Calibration error: end stop active before start\n"); return 1; } -*/ - /* Serial.print("R99"); @@ -335,14 +279,13 @@ Serial.print("calibration start"); Serial.print("\n"); */ -/* + // Move towards home - enableMotors(true); + calibAxis.enableMotor(); + calibAxis.setDirectionHome(); movementDone = false; - setDirectionAxis(dirPin[0], 0, -1, homeAxis[axis], homeIsUp[axis], motorInv[axis]); - while (!movementDone && error == 0) { // Check if there is an emergency stop command @@ -355,7 +298,7 @@ Serial.print("\n"); } // Move until the end stop for home position is reached - if ((!endStopMin(axis) && !endStopMax(axis)) && !movementDone) { + if ((!calibAxis.endStopMin() && !calibAxis.endStopMax()) && !movementDone) { if ((millis() >= timeStart && millis() - timeStart > timeOut[axis] * 1000) || (millis() < timeStart && millis() > timeOut[axis] * 1000)) { movementDone = true; @@ -364,7 +307,7 @@ Serial.print("\n"); // Only do a step every x milliseconds (x is calculated above) if (currentMillis - lastStepMillis >= MOVEMENT_SPEED_BASE_TIME / speedMin[axis]) { - digitalWrite(X_STEP_PIN, HIGH); + calibAxis.setMotorStep(); lastStepMillis = currentMillis; } } @@ -378,20 +321,19 @@ Serial.print("\n"); Serial.print("R04\n"); } - digitalWrite(X_STEP_PIN, LOW); + calibAxis.resetMotorStep(); delayMicroseconds(MOVEMENT_DELAY); } else { movementDone = true; // If end stop for home is active, set the position to zero - if (endStopMax(axis)) + if (calibAxis.endStopMax()) { invertEndStops = true; } } } -*/ /* Serial.print("R99"); @@ -408,7 +350,7 @@ Serial.print(error); Serial.print("\n"); */ -/* + // Report back the end stop setting if (error == 0) { @@ -421,7 +363,7 @@ Serial.print("\n"); Serial.print("R23"); Serial.print(" "); Serial.print("P"); - Serial.print(parEndInv[axis]); + Serial.print(parEndInv); Serial.print(" "); Serial.print("V"); Serial.print(paramValueInt); @@ -438,7 +380,8 @@ Serial.print("\n"); stepsCount = 0; movementDone = false; - setDirectionAxis(dirPin[0], 0, 1, homeAxis[axis], homeIsUp[axis], motorInv[axis]); + //setDirectionAxis(dirPin[0], 0, 1, homeAxis[axis], homeIsUp[axis], motorInv[axis]); + calibAxis.setDirectionAway(); while (!movementDone && error == 0) { @@ -453,7 +396,7 @@ Serial.print("\n"); // Move until the end stop at the other side of the axis is reached - if (((!invertEndStops && !endStopMax(axis)) || (invertEndStops && !endStopMin(axis))) && !movementDone) { + if (((!invertEndStops && !calibAxis.endStopMax()) || (invertEndStops && !calibAxis.endStopMin())) && !movementDone) { if (millis() - timeStart > timeOut[axis] * MOVEMENT_SPEED_BASE_TIME) { movementDone = true; @@ -462,7 +405,7 @@ Serial.print("\n"); // Only do a step every x milliseconds (x is calculated above) if (currentMillis - lastStepMillis >= MOVEMENT_SPEED_BASE_TIME / speedMin[axis]) { - digitalWrite(X_STEP_PIN, HIGH); + calibAxis.setMotorStep(); stepsCount++; lastStepMillis = currentMillis; } @@ -478,7 +421,7 @@ Serial.print("\n"); } - digitalWrite(X_STEP_PIN, LOW); + calibAxis.resetMotorStep(); delayMicroseconds(MOVEMENT_DELAY); } else { @@ -487,7 +430,7 @@ Serial.print("\n"); } -*/ + /* Serial.print("R99"); @@ -505,21 +448,21 @@ Serial.print("\n"); */ -/* + // Report back the end stop setting if (error == 0) { Serial.print("R23"); Serial.print(" "); Serial.print("P"); - Serial.print(parNbrStp[axis]); + Serial.print(parNbrStp); Serial.print(" "); Serial.print("V"); Serial.print(stepsCount); Serial.print("\n"); } - enableMotors(false); + calibAxis.enableMotor(); storeEndStops(); reportEndStops(); @@ -538,7 +481,7 @@ Serial.print("\n"); } reportPosition(); -*/ + return error; } @@ -641,3 +584,35 @@ void StepperControl::storeEndStops() { //} +void StepperControl::loadMotorSettings() { + + // Load settings + + homeIsUp[0] = ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_X); + homeIsUp[1] = ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_Y); + homeIsUp[2] = ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_Z); + + speedMax[0] = ParameterList::getInstance()->getValue(MOVEMENT_MAX_SPD_X); + speedMax[1] = ParameterList::getInstance()->getValue(MOVEMENT_MAX_SPD_Y); + speedMax[2] = ParameterList::getInstance()->getValue(MOVEMENT_MAX_SPD_Z); + + speedMin[0] = ParameterList::getInstance()->getValue(MOVEMENT_MIN_SPD_X); + speedMin[1] = ParameterList::getInstance()->getValue(MOVEMENT_MIN_SPD_Y); + speedMin[2] = ParameterList::getInstance()->getValue(MOVEMENT_MIN_SPD_Z); + + stepsAcc[0] = ParameterList::getInstance()->getValue(MOVEMENT_STEPS_ACC_DEC_X); + stepsAcc[1] = ParameterList::getInstance()->getValue(MOVEMENT_STEPS_ACC_DEC_Y); + stepsAcc[2] = ParameterList::getInstance()->getValue(MOVEMENT_STEPS_ACC_DEC_Z); + + motorInv[0] = ParameterList::getInstance()->getValue(MOVEMENT_INVERT_MOTOR_X); + motorInv[1] = ParameterList::getInstance()->getValue(MOVEMENT_INVERT_MOTOR_Y); + motorInv[2] = ParameterList::getInstance()->getValue(MOVEMENT_INVERT_MOTOR_Z); + + endStInv[0] = ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_X); + endStInv[1] = ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_Y); + endStInv[2] = ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_Z); + + timeOut[0] = ParameterList::getInstance()->getValue(MOVEMENT_TIMEOUT_X); + timeOut[1] = ParameterList::getInstance()->getValue(MOVEMENT_TIMEOUT_X); + timeOut[2] = ParameterList::getInstance()->getValue(MOVEMENT_TIMEOUT_X); +} diff --git a/src/StepperControl.h b/src/StepperControl.h index 341c0dd..66caed7 100644 --- a/src/StepperControl.h +++ b/src/StepperControl.h @@ -44,6 +44,7 @@ private: bool axisActive[3]; + void loadMotorSettings(); void reportPosition(); void storeEndStops(); void reportEndStops(); @@ -51,6 +52,13 @@ private: unsigned long getMaxLength(unsigned long lengths[3]); bool endStopsReached(); + bool homeIsUp[3]; + long speedMax[3]; + long speedMin[3]; + long stepsAcc[3]; + bool motorInv[3]; + bool endStInv[3]; + long timeOut[3]; }; #endif /* STEPPERCONTROL_H_ */ diff --git a/src/StepperControlAxis.cpp b/src/StepperControlAxis.cpp index aa67a3f..c460a46 100644 --- a/src/StepperControlAxis.cpp +++ b/src/StepperControlAxis.cpp @@ -15,64 +15,6 @@ StepperControlAxis::StepperControlAxis() { } -bool StepperControlAxis::endStopAxisReached(bool movement_forward) { - - bool min_endstop = false; - bool max_endstop = false; - bool invert = false; - - if (motorEndStopInv) { - invert = true; - } - - // for the axis to check, retrieve the end stop status - - if (!invert) { - min_endstop = endStopMin(); - max_endstop = endStopMax(); - } else { - min_endstop = endStopMax(); - max_endstop = endStopMin(); - } - - // if moving forward, only check the end stop max - // for moving backwards, check only the end stop min - - if((!movement_forward && min_endstop) || (movement_forward && max_endstop)) { - return 1; - } - return 0; -} - -void StepperControlAxis::setStepAxis() { - - if (coordHomeAxis && coordCurrentPoint == 0) { - - // Keep moving toward end stop even when position is zero - // but end stop is not yet active - if (motorHomeIsUp) { - coordCurrentPoint = -1; - } else { - coordCurrentPoint = 1; - } - } - - if (coordCurrentPoint < coordDestinationPoint) { - coordCurrentPoint++; - } else if (coordCurrentPoint > coordDestinationPoint) { - coordCurrentPoint--; - } - - // set a step on the motors - setMotorStep(); - - // if the home end stop is reached, set the current position - if (endStopAxisReached(false)) - { - coordCurrentPoint = 0; - } -} - unsigned int StepperControlAxis::calculateSpeed(long sourcePosition, long currentPosition, long destinationPosition, long minSpeed, long maxSpeed, long stepsAccDec) { int newSpeed = 0; @@ -176,6 +118,15 @@ void StepperControlAxis::checkAxisDirection() { } } +void StepperControlAxis::setDirectionAxis() { + + if (((!coordHomeAxis && coordCurrentPoint < coordDestinationPoint) || (coordHomeAxis && motorHomeIsUp)) ^ motorMotorInv) { + setDirectionUp(); + } else { + setDirectionDown(); + } +} + void StepperControlAxis::checkMovement() { int i = 1; @@ -184,26 +135,24 @@ void StepperControlAxis::checkMovement() { checkAxisDirection(); -/* + Serial.print("R99 check axis "); -Serial.print(i); -Serial.print(" axis active "); -Serial.print(axisActive[i]); +Serial.print(axisActive); Serial.print(" current point "); -Serial.print(currentPoint[i]); +Serial.print(coordCurrentPoint); Serial.print(" destination point "); -Serial.print(destinationPoint[i]); +Serial.print(coordDestinationPoint); Serial.print(" home stop reached "); -Serial.print(endStopAxisReached(i, false)); +Serial.print(endStopAxisReached(false)); Serial.print(" axis stop reached "); -Serial.print(endStopAxisReached(i, true)); +Serial.print(endStopAxisReached(true)); Serial.print(" home axis "); -Serial.print(homeAxis[i]); +Serial.print(coordHomeAxis); Serial.print(" movement to home "); -Serial.print(movementToHome[i]); +Serial.print(movementToHome); Serial.print("\n"); -*/ + //if ((!pointReached(currentPoint, destinationPoint) || home) && axisActive[i]) { @@ -276,7 +225,6 @@ void StepperControlAxis::checkTiming() { if (axisActive) { if (moveTicks >= stepOffTick) { - /* Serial.print("R99 reset step "); Serial.print(" moveTicks "); @@ -293,7 +241,7 @@ Serial.print("\n"); /* Serial.print("R99 set step "); Serial.print(" moveTicks "); -Serial.print(moveTicks[i]); +Serial.print(moveTicks); Serial.print("\n"); */ // Positive flank for the steps @@ -303,6 +251,64 @@ Serial.print("\n"); } } +void StepperControlAxis::setStepAxis() { + + if (coordHomeAxis && coordCurrentPoint == 0) { + + // Keep moving toward end stop even when position is zero + // but end stop is not yet active + if (motorHomeIsUp) { + coordCurrentPoint = -1; + } else { + coordCurrentPoint = 1; + } + } + + if (coordCurrentPoint < coordDestinationPoint) { + coordCurrentPoint++; + } else if (coordCurrentPoint > coordDestinationPoint) { + coordCurrentPoint--; + } + + // set a step on the motors + setMotorStep(); + + // if the home end stop is reached, set the current position + if (endStopAxisReached(false)) + { + coordCurrentPoint = 0; + } +} + +bool StepperControlAxis::endStopAxisReached(bool movement_forward) { + + bool min_endstop = false; + bool max_endstop = false; + bool invert = false; + + if (motorEndStopInv) { + invert = true; + } + + // for the axis to check, retrieve the end stop status + + if (!invert) { + min_endstop = endStopMin(); + max_endstop = endStopMax(); + } else { + min_endstop = endStopMax(); + max_endstop = endStopMin(); + } + + // if moving forward, only check the end stop max + // for moving backwards, check only the end stop min + + if((!movement_forward && min_endstop) || (movement_forward && max_endstop)) { + return 1; + } + return 0; +} + void StepperControlAxis::StepperControlAxis::loadPinNumbers(int step, int dir, int enable, int min, int max) { pinStep = step; pinDirection = dir; @@ -326,11 +332,22 @@ void StepperControlAxis::loadMotorSettings(long speedMax, long speedMin, long st void StepperControlAxis::loadCoordinates(long sourcePoint, long destinationPoint, bool home) { coordSourcePoint = sourcePoint; - coordCurrentPoint = destinationPoint; + coordCurrentPoint = sourcePoint; + coordDestinationPoint = destinationPoint; coordHomeAxis = home; - moveTicks = 0; + // Limit normal movmement to the home position + if (!motorHomeIsUp && coordDestinationPoint < 0) { + coordDestinationPoint = 0; + } + if ( motorHomeIsUp && coordDestinationPoint > 0) { + coordDestinationPoint = 0; + } + + // Initialize movement variables + moveTicks = 0; + axisActive = true; } void StepperControlAxis::enableMotor() { @@ -341,13 +358,36 @@ void StepperControlAxis::disableMotor() { digitalWrite(pinEnable, LOW); } -void StepperControlAxis::setDirectionAxis(long currentPoint, long destinationPoint, bool goHome, bool homeIsUp, bool motorInv) { +void StepperControlAxis::setDirectionUp() { + if (motorMotorInv) { + digitalWrite(pinDirection, HIGH); + } else { + digitalWrite(pinDirection, LOW); + } +} - if (((!goHome && currentPoint < destinationPoint) || (goHome && homeIsUp)) ^ motorInv) { - digitalWrite(pinDirection, HIGH); - } else { - digitalWrite(pinDirection, LOW); - } +void StepperControlAxis::setDirectionDown() { + if (motorMotorInv) { + digitalWrite(pinDirection, LOW); + } else { + digitalWrite(pinDirection, HIGH); + } +} + +void StepperControlAxis::setDirectionHome() { + if (motorHomeIsUp) { + setDirectionUp(); + } else { + setDirectionDown(); + } +} + +void StepperControlAxis::setDirectionAway() { + if (motorHomeIsUp) { + setDirectionDown(); + } else { + setDirectionUp(); + } } unsigned long StepperControlAxis::getLength(long l1, long l2) { @@ -386,3 +426,10 @@ bool StepperControlAxis::pointReached(long currentPoint, long destinationPoint) return (destinationPoint == currentPoint); } +long StepperControlAxis::currentPoint() { + return coordCurrentPoint; +} + +void StepperControlAxis::setMaxSpeed(long speed) { + motorSpeedMax = speed; +} diff --git a/src/StepperControlAxis.h b/src/StepperControlAxis.h index 492e381..dbb7700 100644 --- a/src/StepperControlAxis.h +++ b/src/StepperControlAxis.h @@ -25,22 +25,28 @@ public: void loadPinNumbers(int step, int dir, int enable, int min, int max); void loadMotorSettings( long speedMax, long speedMin, long stepsAcc, long timeOut, bool homeIsUp, bool motorInv, bool endStInv, long interruptSpeed); void loadCoordinates(long sourcePoint, long destinationPoint, bool home); + void setMaxSpeed(long speed); void enableMotor(); void disableMotor(); void checkMovement(); void checkTiming(); - bool isAxisActive(); bool endStopMin(); bool endStopMax(); bool endStopsReached(); bool endStopAxisReached(bool movement_forward); + long currentPoint(); void setMotorStep(); void resetMotorStep(); + void setDirectionUp(); + void setDirectionDown(); + void setDirectionHome(); + void setDirectionAway(); + private: int lastCalcLog = 0; @@ -53,20 +59,20 @@ private: int pinMax; // motor settings - long motorSpeedMax; - long motorSpeedMin; - long motorStepsAcc; - long motorTimeOut; - bool motorHomeIsUp; - bool motorMotorInv; - bool motorEndStopInv; - long motorInterruptSpeed; + long motorSpeedMax; // maximum speed in steps per second + long motorSpeedMin; // minimum speed in steps per second + long motorStepsAcc; // number of steps used for acceleration + long motorTimeOut; // timeout in seconds + bool motorHomeIsUp; // direction to move when reached 0 on axis but no end stop detected while homing + bool motorMotorInv; // invert motot direction + bool motorEndStopInv; // invert input (true/false) of end stops + long motorInterruptSpeed; // period of interrupt in micro seconds // coordinates - long coordSourcePoint; + long coordSourcePoint; // all coordinated in steps long coordCurrentPoint; long coordDestinationPoint; - bool coordHomeAxis; + bool coordHomeAxis; // homing command // movement handling unsigned long movementLength; @@ -83,7 +89,7 @@ private: void step(long ¤tPoint, 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); - void setDirectionAxis(long currentPoint, long destinationPoint, bool goHome, bool homeIsUp, bool motorInv); + void setDirectionAxis(); unsigned long getLength(long l1, long l2); void checkAxisDirection(); From 890ff84385b66d514d5a99fb385697082add319b Mon Sep 17 00:00:00 2001 From: TimEvWw Date: Wed, 12 Aug 2015 20:23:21 -0100 Subject: [PATCH 7/8] debugging axis class --- src/G00Handler.cpp | 1 + src/StepperControl.cpp | 64 +++++++++++++++------------ src/StepperControlAxis.cpp | 88 ++++++++------------------------------ src/StepperControlAxis.h | 6 ++- 4 files changed, 61 insertions(+), 98 deletions(-) diff --git a/src/G00Handler.cpp b/src/G00Handler.cpp index 3ec4b20..816fd39 100644 --- a/src/G00Handler.cpp +++ b/src/G00Handler.cpp @@ -44,6 +44,7 @@ int G00Handler::execute(Command* command) { command->getX(), command->getY(), command->getZ(), command->getA(), command->getB(), command->getC(), false, false, false); + if (LOGGING) { CurrentState::getInstance()->print(); } diff --git a/src/StepperControl.cpp b/src/StepperControl.cpp index 5c87be2..4d2e328 100644 --- a/src/StepperControl.cpp +++ b/src/StepperControl.cpp @@ -14,13 +14,20 @@ StepperControl * StepperControl::getInstance() { const int MOVEMENT_INTERRUPT_SPEED = 100; // Interrupt cycle in micro seconds StepperControl::StepperControl() { + axisX = StepperControlAxis(); axisY = StepperControlAxis(); axisZ = StepperControlAxis(); + axisX.label = 'X'; + axisY.label = 'Y'; + axisZ.label = 'Z'; + axisX.loadPinNumbers(X_STEP_PIN, X_DIR_PIN, X_ENABLE_PIN, X_MIN_PIN, X_MAX_PIN); axisY.loadPinNumbers(Y_STEP_PIN, Y_DIR_PIN, Y_ENABLE_PIN, Y_MIN_PIN, Y_MAX_PIN); axisZ.loadPinNumbers(Z_STEP_PIN, Z_DIR_PIN, Z_ENABLE_PIN, Z_MIN_PIN, Z_MAX_PIN); + + loadMotorSettings(); } /** @@ -37,7 +44,6 @@ int StepperControl::moveToCoords( long xDest, long yDest, long zDest, unsigned long currentMillis = 0; unsigned long timeStart = millis(); - int incomingByte = 0; int error = 0; @@ -47,11 +53,6 @@ int StepperControl::moveToCoords( long xDest, long yDest, long zDest, // if a speed is given in the command, use that instead of the config speed - long speedMax[3] = {0,0,0}; - speedMax[0] = xMaxSpd; - speedMax[1] = yMaxSpd; - speedMax[2] = zMaxSpd; - if (xMaxSpd > 0 && xMaxSpd < speedMax[0]) { speedMax[0] = xMaxSpd; } @@ -64,7 +65,6 @@ int StepperControl::moveToCoords( long xDest, long yDest, long zDest, speedMax[2] = zMaxSpd; } - axisX.setMaxSpeed(speedMax[0]); axisY.setMaxSpeed(speedMax[1]); axisZ.setMaxSpeed(speedMax[2]); @@ -94,13 +94,14 @@ int StepperControl::moveToCoords( long xDest, long yDest, long zDest, // Prepare for movement - //axisX.setDirection() - //axisY.setDirection() - //axisZ.setDirection() - storeEndStops(); reportEndStops(); + + axisX.setDirectionAxis(); + axisY.setDirectionAxis(); + axisZ.setDirectionAxis(); + enableMotors(); // Start movement @@ -109,9 +110,13 @@ int StepperControl::moveToCoords( long xDest, long yDest, long zDest, axisActive[1] = true; axisActive[2] = true; + //axisActive[0] = false; + axisActive[1] = false; + axisActive[2] = false; + axisX.checkMovement(); -// axisY.checkMovement(); -// axisZ.checkMovement(); + //axisY.checkMovement(); + //axisZ.checkMovement(); //Timer1.start(); @@ -120,19 +125,19 @@ int StepperControl::moveToCoords( long xDest, long yDest, long zDest, //delay(50); delayMicroseconds(100); -/* - Serial.print("R99 "); - Serial.print(" x axis active "); - Serial.print(axisActive[0]); - Serial.print(" y axis active "); - Serial.print(axisActive[1]); - Serial.print(" z axis active "); - Serial.print(axisActive[2]); - Serial.print("\n"); -*/ + +// Serial.print("R99 "); +// Serial.print(" x axis active "); +// Serial.print(axisActive[0]); +// Serial.print(" y axis active "); +// Serial.print(axisActive[1]); +// Serial.print(" z axis active "); +// Serial.print(axisActive[2]); +// Serial.print("\n"); + axisX.checkTiming(); - axisY.checkTiming(); - axisZ.checkTiming(); + //axisY.checkTiming(); + //axisZ.checkTiming(); axisActive[0] = axisX.isAxisActive(); axisActive[1] = axisY.isAxisActive(); @@ -180,9 +185,9 @@ int StepperControl::moveToCoords( long xDest, long yDest, long zDest, } - /**/Serial.print("R99 stopped\n"); + Serial.print("R99 stopped\n"); - Timer1.stop(); + //Timer1.stop(); disableMotors(); currentPoint[0] = axisX.currentPoint(); @@ -615,4 +620,9 @@ void StepperControl::loadMotorSettings() { timeOut[0] = ParameterList::getInstance()->getValue(MOVEMENT_TIMEOUT_X); timeOut[1] = ParameterList::getInstance()->getValue(MOVEMENT_TIMEOUT_X); timeOut[2] = ParameterList::getInstance()->getValue(MOVEMENT_TIMEOUT_X); + + axisX.loadMotorSettings(speedMax[0], speedMin[0], stepsAcc[0], timeOut[0], homeIsUp[0], motorInv[0], endStInv[0], MOVEMENT_INTERRUPT_SPEED); + axisY.loadMotorSettings(speedMax[1], speedMin[1], stepsAcc[1], timeOut[1], homeIsUp[1], motorInv[1], endStInv[1], MOVEMENT_INTERRUPT_SPEED); + axisZ.loadMotorSettings(speedMax[2], speedMin[2], stepsAcc[2], timeOut[2], homeIsUp[2], motorInv[2], endStInv[2], MOVEMENT_INTERRUPT_SPEED); + } diff --git a/src/StepperControlAxis.cpp b/src/StepperControlAxis.cpp index c460a46..10fc147 100644 --- a/src/StepperControlAxis.cpp +++ b/src/StepperControlAxis.cpp @@ -1,7 +1,5 @@ #include "StepperControlAxis.h" -//int test = 0; - long interruptSpeed = 100; StepperControlAxis::StepperControlAxis() { @@ -15,6 +13,12 @@ StepperControlAxis::StepperControlAxis() { } +void StepperControlAxis::test() { + Serial.print("R99 "); + Serial.print(label); + Serial.print("\n"); +} + unsigned int StepperControlAxis::calculateSpeed(long sourcePosition, long currentPosition, long destinationPosition, long minSpeed, long maxSpeed, long stepsAccDec) { int newSpeed = 0; @@ -102,6 +106,7 @@ unsigned int StepperControlAxis::calculateSpeed(long sourcePosition, long curren } void StepperControlAxis::checkAxisDirection() { + if (coordHomeAxis){ // When home is active, the direction is fixed movementUp = motorHomeIsUp; @@ -129,62 +134,16 @@ void StepperControlAxis::setDirectionAxis() { void StepperControlAxis::checkMovement() { - int i = 1; - - //moveTicks[i]++; checkAxisDirection(); - - -Serial.print("R99 check axis "); -Serial.print(axisActive); -Serial.print(" current point "); -Serial.print(coordCurrentPoint); -Serial.print(" destination point "); -Serial.print(coordDestinationPoint); - -Serial.print(" home stop reached "); -Serial.print(endStopAxisReached(false)); -Serial.print(" axis stop reached "); -Serial.print(endStopAxisReached(true)); -Serial.print(" home axis "); -Serial.print(coordHomeAxis); -Serial.print(" movement to home "); -Serial.print(movementToHome); -Serial.print("\n"); - - - - //if ((!pointReached(currentPoint, destinationPoint) || home) && axisActive[i]) { if (((coordCurrentPoint != coordDestinationPoint) || coordHomeAxis) && axisActive) { -//Serial.print("R99 point not reached yet\n"); + // home or destination not reached, keep moving -/* - Serial.print("R99 calc axis speed"); - Serial.print(" speed max "); - Serial.print(speedMax[i]); - Serial.print("\n"); -*/ // Get the axis speed, in steps per second axisSpeed = calculateSpeed( coordSourcePoint, coordCurrentPoint, coordDestinationPoint, motorSpeedMin, motorSpeedMax, motorStepsAcc); -//Serial.print("R99 axis speed "); -//Serial.print(axisSpeed[i]); -//Serial.print("\n"); - -/* -Serial.print("R99 check axis b "); -Serial.print(i); -Serial.print(" home part true "); -Serial.print(homeAxis[i] && !endStopAxisReached(i, false)); -Serial.print(" !homeAxis[i] "); -Serial.print(!homeAxis[i]); -Serial.print(" !endStopAxisReached(i, !movementToHome[i]) "); -Serial.print(!endStopAxisReached(i, !movementToHome[i])); -Serial.print("\n"); -*/ // If end stop reached, don't move anymore if ((coordHomeAxis && !endStopAxisReached(false)) || (!coordHomeAxis && !endStopAxisReached(!movementToHome))) { @@ -197,7 +156,6 @@ Serial.print("\n"); // This gives the number of interrupts (called ticks here) before the pulse needs to be set for the next step stepOnTick = moveTicks + (1000.0 * 1000.0 / interruptSpeed / axisSpeed / 2); stepOffTick = moveTicks + (1000.0 * 1000.0 / interruptSpeed / axisSpeed ); - } } else { @@ -213,8 +171,6 @@ Serial.print("\n"); if (endStopAxisReached(false)) { coordCurrentPoint = 0; } - - } void StepperControlAxis::checkTiming() { @@ -225,12 +181,7 @@ void StepperControlAxis::checkTiming() { if (axisActive) { if (moveTicks >= stepOffTick) { -/* -Serial.print("R99 reset step "); -Serial.print(" moveTicks "); -Serial.print(moveTicks); -Serial.print("\n"); -*/ + // Negative flank for the steps resetMotorStep(); checkMovement(); @@ -238,12 +189,7 @@ Serial.print("\n"); else { if (moveTicks == stepOnTick) { -/* -Serial.print("R99 set step "); -Serial.print(" moveTicks "); -Serial.print(moveTicks); -Serial.print("\n"); -*/ + // Positive flank for the steps setStepAxis(); } @@ -351,26 +297,26 @@ void StepperControlAxis::loadCoordinates(long sourcePoint, long destinationPoint } void StepperControlAxis::enableMotor() { - digitalWrite(pinEnable, HIGH); + digitalWrite(pinEnable, LOW); } void StepperControlAxis::disableMotor() { - digitalWrite(pinEnable, LOW); + digitalWrite(pinEnable, HIGH); } void StepperControlAxis::setDirectionUp() { if (motorMotorInv) { - digitalWrite(pinDirection, HIGH); - } else { digitalWrite(pinDirection, LOW); + } else { + digitalWrite(pinDirection, HIGH); } } void StepperControlAxis::setDirectionDown() { if (motorMotorInv) { - digitalWrite(pinDirection, LOW); - } else { digitalWrite(pinDirection, HIGH); + } else { + digitalWrite(pinDirection, LOW); } } @@ -403,10 +349,12 @@ bool StepperControlAxis::endStopsReached() { } bool StepperControlAxis::endStopMin() { + //return ((digitalRead(pinMin) == motorEndStopInv) || (digitalRead(pinMax) == motorEndStopInv)); return digitalRead(pinMin); } bool StepperControlAxis::endStopMax() { + //return ((digitalRead(pinMin) == motorEndStopInv) || (digitalRead(pinMax) == motorEndStopInv)); return digitalRead(pinMax); } diff --git a/src/StepperControlAxis.h b/src/StepperControlAxis.h index dbb7700..70cad48 100644 --- a/src/StepperControlAxis.h +++ b/src/StepperControlAxis.h @@ -46,6 +46,11 @@ public: void setDirectionDown(); void setDirectionHome(); void setDirectionAway(); + void setDirectionAxis(); + + void test(); + + char label; private: @@ -89,7 +94,6 @@ private: void step(long ¤tPoint, 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); - void setDirectionAxis(); unsigned long getLength(long l1, long l2); void checkAxisDirection(); From aaab53ef3a8e19d6475633922a32b019162d6f49 Mon Sep 17 00:00:00 2001 From: TimEvWw Date: Thu, 13 Aug 2015 20:07:36 -0100 Subject: [PATCH 8/8] cleaning up calibration routine --- src/StepperControl.cpp | 188 +++++++++++------------------------------ 1 file changed, 51 insertions(+), 137 deletions(-) diff --git a/src/StepperControl.cpp b/src/StepperControl.cpp index 4d2e328..5e0d08e 100644 --- a/src/StepperControl.cpp +++ b/src/StepperControl.cpp @@ -110,43 +110,22 @@ int StepperControl::moveToCoords( long xDest, long yDest, long zDest, axisActive[1] = true; axisActive[2] = true; - //axisActive[0] = false; - axisActive[1] = false; - axisActive[2] = false; axisX.checkMovement(); - //axisY.checkMovement(); - //axisZ.checkMovement(); + axisY.checkMovement(); + axisZ.checkMovement(); - //Timer1.start(); + Timer1.start(); // Let the interrupt handle all the movements while (axisActive[0] || axisActive[1] || axisActive[2]) { - //delay(50); - delayMicroseconds(100); - -// Serial.print("R99 "); -// Serial.print(" x axis active "); -// Serial.print(axisActive[0]); -// Serial.print(" y axis active "); -// Serial.print(axisActive[1]); -// Serial.print(" z axis active "); -// Serial.print(axisActive[2]); -// Serial.print("\n"); - - axisX.checkTiming(); - //axisY.checkTiming(); - //axisZ.checkTiming(); + delay(1); axisActive[0] = axisX.isAxisActive(); axisActive[1] = axisY.isAxisActive(); axisActive[2] = axisZ.isAxisActive(); - //axisActive[0] = false; - axisActive[1] = false; - axisActive[2] = false; - storeEndStops(); // Check timeouts @@ -169,7 +148,7 @@ int StepperControl::moveToCoords( long xDest, long yDest, long zDest, } if (error == 1) { - //Serial.print("R99 error\n"); + Serial.print("R99 error\n"); Timer1.stop(); axisActive[0] = false; axisActive[1] = false; @@ -187,7 +166,7 @@ int StepperControl::moveToCoords( long xDest, long yDest, long zDest, Serial.print("R99 stopped\n"); - //Timer1.stop(); + Timer1.stop(); disableMotors(); currentPoint[0] = axisX.currentPoint(); @@ -215,31 +194,20 @@ int StepperControl::calibrateAxis(int axis) { loadMotorSettings(); - //long timeOut[3] = {5000,5000,5000}; - - // Set the coordinate variables for homing, so the default functions can be used for settign direction - - //long sourcePoint[3] = { 0, 0, 0 }; - //long destinationPoint[3] = { 0, 0, 0 }; - - unsigned long currentMillis = 0; - - unsigned long currentSteps = 0; - unsigned long lastStepMillis = 0; - - unsigned long timeStart = millis(); + //unsigned long timeStart = millis(); bool movementDone = false; int paramValueInt = 0; int stepsCount = 0; - int incomingByte = 0; - int error = 0; + int incomingByte = 0; + int error = 0; bool invertEndStops = false; int parEndInv; int parNbrStp; + // Prepare for movement storeEndStops(); @@ -249,20 +217,23 @@ int StepperControl::calibrateAxis(int axis) { // Select the right axis StepperControlAxis calibAxis; switch (axis) { + case 0: + calibAxis = axisX; + parEndInv = MOVEMENT_INVERT_ENDPOINTS_X; + parNbrStp = MOVEMENT_AXIS_NR_STEPS_X; + invertEndStops = endStInv[0]; + break; case 1: - calibAxis = axisX; - parEndInv = MOVEMENT_INVERT_ENDPOINTS_X; - parNbrStp = MOVEMENT_AXIS_NR_STEPS_X; + calibAxis = axisY; + parEndInv = MOVEMENT_INVERT_ENDPOINTS_Y;; + parNbrStp = MOVEMENT_AXIS_NR_STEPS_Y; + invertEndStops = endStInv[0]; break; case 2: - calibAxis = axisY; - parEndInv = MOVEMENT_INVERT_ENDPOINTS_Y;; - parNbrStp = MOVEMENT_AXIS_NR_STEPS_Y; - break; - case 3: - calibAxis = axisZ; - parEndInv = MOVEMENT_INVERT_ENDPOINTS_Z; - parNbrStp = MOVEMENT_AXIS_NR_STEPS_Z; + calibAxis = axisZ; + parEndInv = MOVEMENT_INVERT_ENDPOINTS_Z; + parNbrStp = MOVEMENT_AXIS_NR_STEPS_Z; + invertEndStops = endStInv[0]; break; default: Serial.print("R99 Calibration error: invalid axis selected\n"); @@ -277,19 +248,18 @@ int StepperControl::calibrateAxis(int axis) { return 1; } -/* -Serial.print("R99"); -Serial.print(" "); -Serial.print("calibration start"); -Serial.print("\n"); -*/ - + Serial.print("R99"); + Serial.print(" axis "); + Serial.print(calibAxis.label); + Serial.print(" calibration start"); + Serial.print("\n"); // Move towards home calibAxis.enableMotor(); calibAxis.setDirectionHome(); + stepsCount = 0; movementDone = false; while (!movementDone && error == 0) { @@ -305,29 +275,19 @@ Serial.print("\n"); // Move until the end stop for home position is reached if ((!calibAxis.endStopMin() && !calibAxis.endStopMax()) && !movementDone) { - if ((millis() >= timeStart && millis() - timeStart > timeOut[axis] * 1000) || (millis() < timeStart && millis() > timeOut[axis] * 1000)) { - movementDone = true; - error = 1; - } else { + calibAxis.setMotorStep(); - // Only do a step every x milliseconds (x is calculated above) - if (currentMillis - lastStepMillis >= MOVEMENT_SPEED_BASE_TIME / speedMin[axis]) { - calibAxis.setMotorStep(); - lastStepMillis = currentMillis; - } - } + delayMicroseconds(1000000 / speedMin[axis] /2); - delayMicroseconds(MOVEMENT_DELAY); - - currentMillis++; - if (currentMillis % 10000 == 0) + stepsCount++; + if (stepsCount % (speedMin[axis] * 3) == 0) { // Periodically send message still active Serial.print("R04\n"); } calibAxis.resetMotorStep(); - delayMicroseconds(MOVEMENT_DELAY); + delayMicroseconds(1000000 / speedMin[axis] /2); } else { movementDone = true; @@ -340,21 +300,11 @@ Serial.print("\n"); } } -/* -Serial.print("R99"); -Serial.print(" "); -Serial.print("calibration halfway"); -Serial.print(" "); -Serial.print("end stop invert"); -Serial.print(" "); -Serial.print(invertEndStops); -Serial.print(" "); -Serial.print("error"); -Serial.print(" "); -Serial.print(error); -Serial.print("\n"); -*/ - + Serial.print("R99"); + Serial.print(" axis "); + Serial.print(calibAxis.label); + Serial.print(" at first end stop"); + Serial.print("\n"); // Report back the end stop setting @@ -379,13 +329,11 @@ Serial.print("\n"); storeEndStops(); reportEndStops(); - //reportPosition(); // Move into the other direction now, and measure the number of steps stepsCount = 0; movementDone = false; - //setDirectionAxis(dirPin[0], 0, 1, homeAxis[axis], homeIsUp[axis], motorInv[axis]); calibAxis.setDirectionAway(); while (!movementDone && error == 0) { @@ -399,27 +347,15 @@ Serial.print("\n"); } } - // Move until the end stop at the other side of the axis is reached if (((!invertEndStops && !calibAxis.endStopMax()) || (invertEndStops && !calibAxis.endStopMin())) && !movementDone) { - if (millis() - timeStart > timeOut[axis] * MOVEMENT_SPEED_BASE_TIME) { - movementDone = true; - error = 1; - } else { + calibAxis.setMotorStep(); + stepsCount++; - // Only do a step every x milliseconds (x is calculated above) - if (currentMillis - lastStepMillis >= MOVEMENT_SPEED_BASE_TIME / speedMin[axis]) { - calibAxis.setMotorStep(); - stepsCount++; - lastStepMillis = currentMillis; - } - } + delayMicroseconds(1000000 / speedMin[axis] /2); - delayMicroseconds(MOVEMENT_DELAY); - - currentMillis++; - if (currentMillis % 10000 == 0) + if (stepsCount % (speedMin[axis] * 3) == 0) { // Periodically send message still active Serial.print("R04\n"); @@ -427,7 +363,7 @@ Serial.print("\n"); calibAxis.resetMotorStep(); - delayMicroseconds(MOVEMENT_DELAY); + delayMicroseconds(1000000 / speedMin[axis] /2); } else { movementDone = true; @@ -435,24 +371,11 @@ Serial.print("\n"); } - - -/* -Serial.print("R99"); -Serial.print(" "); -Serial.print("calibration done"); -Serial.print(" "); -Serial.print("nr steps"); -Serial.print(" "); -Serial.print(stepsCount); -Serial.print(" "); -Serial.print("error"); -Serial.print(" "); -Serial.print(error); -Serial.print("\n"); -*/ - - + Serial.print("R99"); + Serial.print(" axis "); + Serial.print(calibAxis.label); + Serial.print(" at second end stop"); + Serial.print("\n"); // Report back the end stop setting @@ -467,7 +390,7 @@ Serial.print("\n"); Serial.print("\n"); } - calibAxis.enableMotor(); + calibAxis.disableMotor(); storeEndStops(); reportEndStops(); @@ -493,15 +416,6 @@ Serial.print("\n"); // Handle movement by checking each axis void StepperControl::handleMovementInterrupt(void){ -//Serial.print("R99 interrupt\n"); - -//Serial.print("R99 interrupt data "); -//Serial.print(" destination point "); -//Serial.print(destinationPoint[0]); -//Serial.print(" "); -//Serial.print("test "); -//Serial.print(test); -//Serial.print("\n"); axisX.checkTiming(); axisY.checkTiming();