From 3d573dca0c345342b1ca9550294c42a86af5f997 Mon Sep 17 00:00:00 2001 From: Rick Carlino Date: Wed, 19 Apr 2017 09:12:12 -0500 Subject: [PATCH] Formatting update --- .gitignore | 2 +- LICENSE | 2 +- README.md | 12 +- src/Command.cpp | 513 ++++---- src/Command.h | 76 +- src/Config.h | 174 ++- src/CurrentState.cpp | 184 +-- src/CurrentState.h | 44 +- src/F11Handler.cpp | 47 +- src/F11Handler.h | 19 +- src/F12Handler.cpp | 47 +- src/F12Handler.h | 19 +- src/F13Handler.cpp | 47 +- src/F13Handler.h | 19 +- src/F14Handler.cpp | 52 +- src/F14Handler.h | 20 +- src/F15Handler.cpp | 46 +- src/F15Handler.h | 20 +- src/F16Handler.cpp | 48 +- src/F16Handler.h | 20 +- src/F20Handler.cpp | 32 +- src/F20Handler.h | 18 +- src/F21Handler.cpp | 32 +- src/F21Handler.h | 18 +- src/F22Handler.cpp | 34 +- src/F22Handler.h | 18 +- src/F31Handler.cpp | 34 +- src/F31Handler.h | 21 +- src/F32Handler.cpp | 34 +- src/F32Handler.h | 21 +- src/F41Handler.cpp | 32 +- src/F41Handler.h | 20 +- src/F42Handler.cpp | 32 +- src/F42Handler.h | 20 +- src/F43Handler.cpp | 39 +- src/F43Handler.h | 14 +- src/F44Handler.cpp | 32 +- src/F44Handler.h | 17 +- src/F61Handler.cpp | 32 +- src/F61Handler.h | 20 +- src/F81Handler.cpp | 58 +- src/F81Handler.h | 19 +- src/F82Handler.cpp | 42 +- src/F82Handler.h | 19 +- src/F83Handler.cpp | 48 +- src/F83Handler.h | 19 +- src/G00Handler.cpp | 88 +- src/G00Handler.h | 18 +- src/G28Handler.cpp | 45 +- src/G28Handler.h | 18 +- src/GCodeHandler.cpp | 12 +- src/GCodeHandler.h | 9 +- src/GCodeProcessor.cpp | 265 ++-- src/GCodeProcessor.h | 15 +- src/LICENSE | 2 +- src/MemoryFree.cpp | 4 +- src/MemoryFree.h | 4 +- src/ParameterList.cpp | 735 ++++++----- src/ParameterList.h | 180 ++- src/PinControl.cpp | 117 +- src/PinControl.h | 19 +- src/PinGuard.cpp | 65 +- src/PinGuard.h | 29 +- src/PinGuardPin.cpp | 58 +- src/PinGuardPin.h | 26 +- src/ServoControl.cpp | 60 +- src/ServoControl.h | 16 +- src/StatusList.cpp | 71 +- src/StatusList.h | 33 +- src/StepperControl.cpp | 1956 +++++++++++++++------------- src/StepperControl.h | 156 +-- src/StepperControlAxis.cpp | 1024 ++++++++------- src/StepperControlAxis.h | 213 ++- src/StepperControlEncoder.cpp | 219 ++-- src/StepperControlEncoder.h | 67 +- src/TimerOne.cpp | 18 +- src/TimerOne.h | 517 ++++---- src/farmbot_arduino_controller.cpp | 372 +++--- src/farmbot_arduino_controller.h | 1 - src/known_16bit_timers.h | 180 +-- src/pins.h | 100 +- src/src.ino | 2 +- src/src.vcxproj | 2 +- src/src.vcxproj.filters | 2 +- 84 files changed, 4748 insertions(+), 4106 deletions(-) diff --git a/.gitignore b/.gitignore index 0e5d7b9..47cfcb0 100644 --- a/.gitignore +++ b/.gitignore @@ -25,4 +25,4 @@ /.build /.vs /src/Debug -/src/__vm \ No newline at end of file +/src/__vm diff --git a/LICENSE b/LICENSE index 4f29ca9..9957c44 100644 --- a/LICENSE +++ b/LICENSE @@ -18,4 +18,4 @@ FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. \ No newline at end of file +SOFTWARE. diff --git a/README.md b/README.md index de5948b..c42af37 100644 --- a/README.md +++ b/README.md @@ -95,7 +95,7 @@ X_MIN_PIN | 3 | X axis end stop at home position X_MAX_PIN | 2 | X axis end stop at far position X_ENCDR_A | 16 | X axis encoder A channel X_ENCDR_B | 17 | X axis encoder B channel -X_ENCDR_A_Q | 31 | X axis encoder A channel for quarature (not implemented) +X_ENCDR_A_Q | 31 | X axis encoder A channel for quarature (not implemented) X_ENCDR_B_Q | 33 | X axis encoder B channel for quarature (not implemented) Y_STEP_PIN | 60 | Y axis step signal Y_DIR_PIN | 61 | Y axis direction choice @@ -104,8 +104,8 @@ Y_MIN_PIN | 14 | Y axis end stop at home position Y_MAX_PIN | 15 | Y axis end stop at far position Y_ENCDR_A | 23 | Y axis encoder A channel Y_ENCDR_B | 25 | Y axis encoder B channel -Y_ENCDR_A_Q | 35 | Y axis encoder A channel for quarature (not implemented) -Y_ENCDR_B_Q | 37 | Y axis encoder B channel for quarature (not implemented) +Y_ENCDR_A_Q | 35 | Y axis encoder A channel for quarature (not implemented) +Y_ENCDR_B_Q | 37 | Y axis encoder B channel for quarature (not implemented) Z_STEP_PIN | 46 | Z axis step signal Z_DIR_PIN | 48 | Z axis direction choice Z_ENABLE_PIN | 62 | Z axis enable @@ -113,8 +113,8 @@ Z_MIN_PIN | 18 | Z axis end stop at home position Z_MAX_PIN | 19 | Z axis end stop at far position Z_ENCDR_A | 27 | Z axis encoder A channel Z_ENCDR_B | 29 | Z axis encoder B channel -Z_ENCDR_A_Q | 39 | Z axis encoder A channel for quarature (not implemented) -Z_ENCDR_B_Q | 41 | Z axis encoder B channel for quarature (not implemented) +Z_ENCDR_A_Q | 39 | Z axis encoder A channel for quarature (not implemented) +Z_ENCDR_B_Q | 41 | Z axis encoder B channel for quarature (not implemented) LED_PIN | 13 | on board LED FAN_PIN | 9 | RAMPS board fan pin HEATER_0_PIN | 10 | RAMPS board heating pin 0 @@ -150,7 +150,7 @@ F |31 |P |Read status F |32 |P V |Write status F |41 |P V M |Set a value V on an arduino pin in mode M (digital=0/analog=1) F |42 |P M |Read a value from an arduino pin P in mode M (digital=0/analog=1) -F |43 |P M |Set the I/O mode M (input=0/output=1) of a pin P in arduino +F |43 |P M |Set the I/O mode M (input=0/output=1) of a pin P in arduino F |44 |P V W T M |Set the value V on an arduino pin P, wait for time T in milliseconds, set value W on the arduino pin P in mode M (digital=0/analog=1) F |51 |E P V |Set a value on the tool mount with I2C (not implemented) F |52 |E P |Read value from the tool mount with I2C (not implemented) diff --git a/src/Command.cpp b/src/Command.cpp index 9a69716..b2f3069 100644 --- a/src/Command.cpp +++ b/src/Command.cpp @@ -1,276 +1,333 @@ #include "Command.h" -const char axisCodes[3] = { 'X', 'Y', 'Z' }; -const char axisSpeedCodes[3] = { 'A', 'B', 'C' }; -const char speedCode = 'S'; -const char parameterIdCode = 'P'; +const char axisCodes[3] = {'X', 'Y', 'Z'}; +const char axisSpeedCodes[3] = {'A', 'B', 'C'}; +const char speedCode = 'S'; +const char parameterIdCode = 'P'; const char parameterValueCode = 'V'; -const char parameterValue2Code= 'W'; -const char elementCode = 'E'; -const char timeCode = 'T'; -const char modeCode = 'M'; -const char msgQueueCode = 'Q'; +const char parameterValue2Code = 'W'; +const char elementCode = 'E'; +const char timeCode = 'T'; +const char modeCode = 'M'; +const char msgQueueCode = 'Q'; CommandCodeEnum commandCodeEnum = CODE_UNDEFINED; -Command::Command(char * commandChar) { +Command::Command(char *commandChar) +{ - char * charBuf = commandChar; - char* charPointer; - bool invalidCommand = false; + char *charBuf = commandChar; + char *charPointer; + bool invalidCommand = false; - charPointer = strtok(charBuf, " \n\r"); + charPointer = strtok(charBuf, " \n\r"); - if (charPointer[0] == 'G' || charPointer[0] == 'F') { - commandCodeEnum = getGCodeEnum(charPointer); - } else { - invalidCommand = true; - return; - } + if (charPointer[0] == 'G' || charPointer[0] == 'F') + { + commandCodeEnum = getGCodeEnum(charPointer); + } + else + { + invalidCommand = true; + return; + } - while (charPointer != NULL) { - getParameter(charPointer); + while (charPointer != NULL) + { + getParameter(charPointer); - charPointer = strtok(NULL, " \n\r"); - - } + charPointer = strtok(NULL, " \n\r"); + } } -CommandCodeEnum Command::getGCodeEnum(char* code) { - - - if (strcmp(code, "G0") == 0 || strcmp(code, "G00") == 0) { - return G00; - } - if (strcmp(code, "G1") == 0 || strcmp(code, "G01") == 0) { - return G01; - } - //if (strcmp(code, "F3") == 0 || strcmp(code, "F03") == 0) { - // return F03; - //} - - if (strcmp(code, "F11") == 0) { - return F11; - } - if (strcmp(code, "F12") == 0) { - return F12; - } - if (strcmp(code, "F13") == 0) { - return F13; - } - if (strcmp(code, "F14") == 0) { - return F14; - } - if (strcmp(code, "F15") == 0) { - return F15; - } - if (strcmp(code, "F16") == 0) { - return F16; - } - - if (strcmp(code, "F20") == 0) { - return F20; - } - if (strcmp(code, "F21") == 0) { - return F21; - } - if (strcmp(code, "F22") == 0) { - return F22; - } - - if (strcmp(code, "F31") == 0) { - return F31; - } - if (strcmp(code, "F32") == 0) { - return F32; - } - - if (strcmp(code, "F41") == 0) { - return F41; - } - if (strcmp(code, "F42") == 0) { - return F42; - } - if (strcmp(code, "F43") == 0) { - return F43; - } - if (strcmp(code, "F44") == 0) { - return F44; - } - - if (strcmp(code, "F61") == 0) { - return F61; - } - - if (strcmp(code, "F81") == 0) { - return F81; - } - if (strcmp(code, "F82") == 0) { - return F82; - } - if (strcmp(code, "F83") == 0) { - return F83; - } - if (strcmp(code, "F84") == 0) { - return F84; - } - - return CODE_UNDEFINED; +CommandCodeEnum Command::getGCodeEnum(char *code) +{ + + if (strcmp(code, "G0") == 0 || strcmp(code, "G00") == 0) + { + return G00; + } + if (strcmp(code, "G1") == 0 || strcmp(code, "G01") == 0) + { + return G01; + } + //if (strcmp(code, "F3") == 0 || strcmp(code, "F03") == 0) { + // return F03; + //} + + if (strcmp(code, "F11") == 0) + { + return F11; + } + if (strcmp(code, "F12") == 0) + { + return F12; + } + if (strcmp(code, "F13") == 0) + { + return F13; + } + if (strcmp(code, "F14") == 0) + { + return F14; + } + if (strcmp(code, "F15") == 0) + { + return F15; + } + if (strcmp(code, "F16") == 0) + { + return F16; + } + + if (strcmp(code, "F20") == 0) + { + return F20; + } + if (strcmp(code, "F21") == 0) + { + return F21; + } + if (strcmp(code, "F22") == 0) + { + return F22; + } + + if (strcmp(code, "F31") == 0) + { + return F31; + } + if (strcmp(code, "F32") == 0) + { + return F32; + } + + if (strcmp(code, "F41") == 0) + { + return F41; + } + if (strcmp(code, "F42") == 0) + { + return F42; + } + if (strcmp(code, "F43") == 0) + { + return F43; + } + if (strcmp(code, "F44") == 0) + { + return F44; + } + + if (strcmp(code, "F61") == 0) + { + return F61; + } + + if (strcmp(code, "F81") == 0) + { + return F81; + } + if (strcmp(code, "F82") == 0) + { + return F82; + } + if (strcmp(code, "F83") == 0) + { + return F83; + } + if (strcmp(code, "F84") == 0) + { + return F84; + } + + return CODE_UNDEFINED; } -double minusNotAllowed(double value) { - if(value < 0) { - return 0; - } - return value; +double minusNotAllowed(double value) +{ + if (value < 0) + { + return 0; + } + return value; } -void Command::getParameter(char* charPointer) { - - //msgQueue = 24; - - if (charPointer[0] == axisCodes[0] ){ - axisValue[0] = atof(charPointer + 1 ); - //msgQueue = 77; - } - - if (charPointer[0] == axisCodes[1] ){ - axisValue[1] = atof(charPointer + 1 ); - } - - if (charPointer[0] == axisCodes[2] ){ - axisValue[2] = atof(charPointer + 1 ); - } - - if (charPointer[0] == axisSpeedCodes[0] ){ - axisSpeedValue[0] = atof(charPointer + 1 ); - } - - if (charPointer[0] == axisSpeedCodes[1] ){ - axisSpeedValue[1] = atof(charPointer + 1 ); - } - - if (charPointer[0] == axisSpeedCodes[2] ){ - axisSpeedValue[2] = atof(charPointer + 1 ); - } - - if (charPointer[0] == speedCode ){ - speedValue = atof(charPointer + 1 ); - } - - if (charPointer[0] == parameterIdCode ){ - parameterId = atof(charPointer + 1 ); - } - - if (charPointer[0] == parameterValueCode ){ - parameterValue = atof(charPointer + 1 ); - - } - - if (charPointer[0] == parameterValue2Code ){ - parameterValue2 = atof(charPointer + 1 ); - } - - if (charPointer[0] == elementCode ){ - element = atof(charPointer + 1 ); - } - - if (charPointer[0] == timeCode ){ - time = atof(charPointer + 1 ); - } - - if (charPointer[0] == modeCode ){ - mode = atof(charPointer + 1 ); - } - - if (charPointer[0] == msgQueueCode ){ - msgQueue = atof(charPointer + 1 ); - //msgQueue = 5; - } +void Command::getParameter(char *charPointer) +{ + + //msgQueue = 24; + + if (charPointer[0] == axisCodes[0]) + { + axisValue[0] = atof(charPointer + 1); + //msgQueue = 77; + } + + if (charPointer[0] == axisCodes[1]) + { + axisValue[1] = atof(charPointer + 1); + } + + if (charPointer[0] == axisCodes[2]) + { + axisValue[2] = atof(charPointer + 1); + } + + if (charPointer[0] == axisSpeedCodes[0]) + { + axisSpeedValue[0] = atof(charPointer + 1); + } + + if (charPointer[0] == axisSpeedCodes[1]) + { + axisSpeedValue[1] = atof(charPointer + 1); + } + + if (charPointer[0] == axisSpeedCodes[2]) + { + axisSpeedValue[2] = atof(charPointer + 1); + } + + if (charPointer[0] == speedCode) + { + speedValue = atof(charPointer + 1); + } + + if (charPointer[0] == parameterIdCode) + { + parameterId = atof(charPointer + 1); + } + + if (charPointer[0] == parameterValueCode) + { + parameterValue = atof(charPointer + 1); + } + + if (charPointer[0] == parameterValue2Code) + { + parameterValue2 = atof(charPointer + 1); + } + + if (charPointer[0] == elementCode) + { + element = atof(charPointer + 1); + } + + if (charPointer[0] == timeCode) + { + time = atof(charPointer + 1); + } + + if (charPointer[0] == modeCode) + { + mode = atof(charPointer + 1); + } + + if (charPointer[0] == msgQueueCode) + { + msgQueue = atof(charPointer + 1); + //msgQueue = 5; + } } -void Command::print() { - Serial.print("R99 Command with code: "); - Serial.print(commandCodeEnum); - Serial.print(", X: "); - Serial.print(axisValue[0]); - Serial.print(", Y: "); - Serial.print(axisValue[1]); - Serial.print(", Z: "); - Serial.print(axisValue[2]); - Serial.print(", S: "); - Serial.print(speedValue); - Serial.print(", P: "); - Serial.print(parameterId); - Serial.print(", V: "); - Serial.print(parameterValue); - - Serial.print(", W: "); - Serial.print(parameterValue2); - Serial.print(", T: "); - Serial.print(time); - Serial.print(", E: "); - Serial.print(element); - Serial.print(", M: "); - Serial.print(mode); - Serial.print(", Q: "); - Serial.print(msgQueue); - Serial.print("\r\n"); +void Command::print() +{ + Serial.print("R99 Command with code: "); + Serial.print(commandCodeEnum); + Serial.print(", X: "); + Serial.print(axisValue[0]); + Serial.print(", Y: "); + Serial.print(axisValue[1]); + Serial.print(", Z: "); + Serial.print(axisValue[2]); + Serial.print(", S: "); + Serial.print(speedValue); + Serial.print(", P: "); + Serial.print(parameterId); + Serial.print(", V: "); + Serial.print(parameterValue); + + Serial.print(", W: "); + Serial.print(parameterValue2); + Serial.print(", T: "); + Serial.print(time); + Serial.print(", E: "); + Serial.print(element); + Serial.print(", M: "); + Serial.print(mode); + Serial.print(", Q: "); + Serial.print(msgQueue); + Serial.print("\r\n"); } -CommandCodeEnum Command::getCodeEnum() { - return commandCodeEnum; +CommandCodeEnum Command::getCodeEnum() +{ + return commandCodeEnum; } -double Command::getX() { - return axisValue[0]; +double Command::getX() +{ + return axisValue[0]; } -double Command::getY() { - return axisValue[1]; +double Command::getY() +{ + return axisValue[1]; } -double Command::getZ() { - return axisValue[2]; +double Command::getZ() +{ + return axisValue[2]; } -long Command::getA() { - return axisSpeedValue[0]; +long Command::getA() +{ + return axisSpeedValue[0]; } -long Command::getB() { - return axisSpeedValue[1]; +long Command::getB() +{ + return axisSpeedValue[1]; } -long Command::getC() { - return axisSpeedValue[2]; +long Command::getC() +{ + return axisSpeedValue[2]; } -long Command::getP() { - return parameterId; +long Command::getP() +{ + return parameterId; } -long Command::getV() { - return parameterValue; +long Command::getV() +{ + return parameterValue; } -long Command::getW() { - return parameterValue2; +long Command::getW() +{ + return parameterValue2; } -long Command::getT() { - return time; +long Command::getT() +{ + return time; } -long Command::getE() { - return element; +long Command::getE() +{ + return element; } -long Command::getM() { - return mode; +long Command::getM() +{ + return mode; } -long Command::getQ() { - //msgQueue = 9876; - return msgQueue; +long Command::getQ() +{ + //msgQueue = 9876; + return msgQueue; } diff --git a/src/Command.h b/src/Command.h index 1470501..9085b3d 100644 --- a/src/Command.h +++ b/src/Command.h @@ -7,9 +7,9 @@ enum CommandCodeEnum { CODE_UNDEFINED = -1, - G00 = 0, - G01 = 1, - G28 = 28, + G00 = 0, + G01 = 1, + G28 = 28, F01 = 101, F02 = 102, F03 = 103, @@ -37,44 +37,46 @@ enum CommandCodeEnum //#define NULL 0 -class Command { - CommandCodeEnum codeEnum; +class Command +{ + CommandCodeEnum codeEnum; + public: -// Command(String); - Command(char * commandChar); - void print(); - CommandCodeEnum getCodeEnum(); - double getX(); - double getY(); - double getZ(); - double getS(); - long getP(); - long getV(); - long getA(); - long getB(); - long getC(); - long getW(); - long getT(); - long getE(); - long getM(); - long getQ(); + // Command(String); + Command(char *commandChar); + void print(); + CommandCodeEnum getCodeEnum(); + double getX(); + double getY(); + double getZ(); + double getS(); + long getP(); + long getV(); + long getA(); + long getB(); + long getC(); + long getW(); + long getT(); + long getE(); + long getM(); + long getQ(); - void printQAndNewLine(); -private: - CommandCodeEnum getGCodeEnum(char* code); - void getParameter(char* charPointer); + void printQAndNewLine(); - double axisValue[3] = { 0.0, 0.0, 0.0 }; - long axisSpeedValue[3] = { 0, 0, 0 }; - double speedValue = 0.0; - long parameterId = 0; - long parameterValue = 0; - long parameterValue2 = 0; - long element = 0; - long time = 0; - long mode = 0; - long msgQueue = 0; +private: + CommandCodeEnum getGCodeEnum(char *code); + void getParameter(char *charPointer); + double axisValue[3] = {0.0, 0.0, 0.0}; + long axisSpeedValue[3] = {0, 0, 0}; + double speedValue = 0.0; + long parameterId = 0; + long parameterValue = 0; + long parameterValue2 = 0; + long element = 0; + long time = 0; + long mode = 0; + long msgQueue = 0; }; #endif /* COMMAND_H_ */ diff --git a/src/Config.h b/src/Config.h index d24bd04..9c58154 100644 --- a/src/Config.h +++ b/src/Config.h @@ -22,130 +22,128 @@ const String COMM_REPORT_CMD_STATUS = "R05"; const String COMM_REPORT_CALIB_STATUS = "R06"; */ -const char COMM_REPORT_CMD_IDLE[4] = {'R','0','0','\0'}; -const char COMM_REPORT_CMD_START[4] = {'R','0','1','\0'}; -const char COMM_REPORT_CMD_DONE[4] = {'R','0','2','\0'}; -const char COMM_REPORT_CMD_ERROR[4] = {'R','0','3','\0'}; -const char COMM_REPORT_CMD_BUSY[4] = {'R','0','4','\0'}; -const char COMM_REPORT_CMD_STATUS[4] = {'R','0','5','\0'}; -const char COMM_REPORT_CALIB_STATUS[4] = {'R','0','6','\0'}; -const char COMM_REPORT_NO_CONFIG[4] = {'R','8','8','\0'}; -const char COMM_REPORT_COMMENT[4] = {'R','9','9','\0'}; - -const int COMM_REPORT_MOVE_STATUS_IDLE = 0; -const int COMM_REPORT_MOVE_STATUS_START_MOTOR = 1; -const int COMM_REPORT_MOVE_STATUS_ACCELERATING = 2; -const int COMM_REPORT_MOVE_STATUS_CRUISING = 3; -const int COMM_REPORT_MOVE_STATUS_DECELERATING = 4; -const int COMM_REPORT_MOVE_STATUS_STOP_MOTOR = 5; -const int COMM_REPORT_MOVE_STATUS_CRAWLING = 6; -const int COMM_REPORT_MOVE_STATUS_ERROR = -1; - -const int COMM_REPORT_CALIBRATE_STATUS_IDLE = 0; -const int COMM_REPORT_CALIBRATE_STATUS_TO_HOME = 1; -const int COMM_REPORT_CALIBRATE_STATUS_TO_END = 2; -const int COMM_REPORT_CALIBRATE_STATUS_ERROR = -1; - +const char COMM_REPORT_CMD_IDLE[4] = {'R', '0', '0', '\0'}; +const char COMM_REPORT_CMD_START[4] = {'R', '0', '1', '\0'}; +const char COMM_REPORT_CMD_DONE[4] = {'R', '0', '2', '\0'}; +const char COMM_REPORT_CMD_ERROR[4] = {'R', '0', '3', '\0'}; +const char COMM_REPORT_CMD_BUSY[4] = {'R', '0', '4', '\0'}; +const char COMM_REPORT_CMD_STATUS[4] = {'R', '0', '5', '\0'}; +const char COMM_REPORT_CALIB_STATUS[4] = {'R', '0', '6', '\0'}; +const char COMM_REPORT_NO_CONFIG[4] = {'R', '8', '8', '\0'}; +const char COMM_REPORT_COMMENT[4] = {'R', '9', '9', '\0'}; + +const int COMM_REPORT_MOVE_STATUS_IDLE = 0; +const int COMM_REPORT_MOVE_STATUS_START_MOTOR = 1; +const int COMM_REPORT_MOVE_STATUS_ACCELERATING = 2; +const int COMM_REPORT_MOVE_STATUS_CRUISING = 3; +const int COMM_REPORT_MOVE_STATUS_DECELERATING = 4; +const int COMM_REPORT_MOVE_STATUS_STOP_MOTOR = 5; +const int COMM_REPORT_MOVE_STATUS_CRAWLING = 6; +const int COMM_REPORT_MOVE_STATUS_ERROR = -1; + +const int COMM_REPORT_CALIBRATE_STATUS_IDLE = 0; +const int COMM_REPORT_CALIBRATE_STATUS_TO_HOME = 1; +const int COMM_REPORT_CALIBRATE_STATUS_TO_END = 2; +const int COMM_REPORT_CALIBRATE_STATUS_ERROR = -1; const int MOVEMENT_INTERRUPT_SPEED = 200; // Interrupt cycle in micro seconds -const unsigned int MOVEMENT_SPEED_BASE_TIME = 2000; -const unsigned int MOVEMENT_DELAY = 250; +const unsigned int MOVEMENT_SPEED_BASE_TIME = 2000; +const unsigned int MOVEMENT_DELAY = 250; -const long PARAM_VERSION_DEFAULT = 1; -const long PARAM_TEST_DEFAULT = 0; +const long PARAM_VERSION_DEFAULT = 1; +const long PARAM_TEST_DEFAULT = 0; -const long PARAM_CONFIG_OK_DEFAULT = 0; -const long PARAM_USE_EEPROM_DEFAULT = 1; +const long PARAM_CONFIG_OK_DEFAULT = 0; +const long PARAM_USE_EEPROM_DEFAULT = 1; -const long MOVEMENT_TIMEOUT_X_DEFAULT = 120; -const long MOVEMENT_TIMEOUT_Y_DEFAULT = 120; -const long MOVEMENT_TIMEOUT_Z_DEFAULT = 120; +const long MOVEMENT_TIMEOUT_X_DEFAULT = 120; +const long MOVEMENT_TIMEOUT_Y_DEFAULT = 120; +const long MOVEMENT_TIMEOUT_Z_DEFAULT = 120; -const long MOVEMENT_ENABLE_ENDPOINTS_X_DEFAULT = 0; -const long MOVEMENT_ENABLE_ENDPOINTS_Y_DEFAULT = 0; -const long MOVEMENT_ENABLE_ENDPOINTS_Z_DEFAULT = 0; +const long MOVEMENT_ENABLE_ENDPOINTS_X_DEFAULT = 0; +const long MOVEMENT_ENABLE_ENDPOINTS_Y_DEFAULT = 0; +const long MOVEMENT_ENABLE_ENDPOINTS_Z_DEFAULT = 0; -const long MOVEMENT_INVERT_ENDPOINTS_X_DEFAULT = 0; -const long MOVEMENT_INVERT_ENDPOINTS_Y_DEFAULT = 0; -const long MOVEMENT_INVERT_ENDPOINTS_Z_DEFAULT = 0; +const long MOVEMENT_INVERT_ENDPOINTS_X_DEFAULT = 0; +const long MOVEMENT_INVERT_ENDPOINTS_Y_DEFAULT = 0; +const long MOVEMENT_INVERT_ENDPOINTS_Z_DEFAULT = 0; -const long MOVEMENT_INVERT_MOTOR_X_DEFAULT = 0; -const long MOVEMENT_INVERT_MOTOR_Y_DEFAULT = 0; -const long MOVEMENT_INVERT_MOTOR_Z_DEFAULT = 0; +const long MOVEMENT_INVERT_MOTOR_X_DEFAULT = 0; +const long MOVEMENT_INVERT_MOTOR_Y_DEFAULT = 0; +const long MOVEMENT_INVERT_MOTOR_Z_DEFAULT = 0; -const long MOVEMENT_SECONDARY_MOTOR_X_DEFAULT = 1; -const long MOVEMENT_SECONDARY_MOTOR_INVERT_X_DEFAULT = 0; +const long MOVEMENT_SECONDARY_MOTOR_X_DEFAULT = 1; +const long MOVEMENT_SECONDARY_MOTOR_INVERT_X_DEFAULT = 0; -const long MOVEMENT_HOME_UP_X_DEFAULT = 0; -const long MOVEMENT_HOME_UP_Y_DEFAULT = 0; -const long MOVEMENT_HOME_UP_Z_DEFAULT = 1; +const long MOVEMENT_HOME_UP_X_DEFAULT = 0; +const long MOVEMENT_HOME_UP_Y_DEFAULT = 0; +const long MOVEMENT_HOME_UP_Z_DEFAULT = 1; // numver of steps used for acceleration or deceleration -const long MOVEMENT_STEPS_ACC_DEC_X_DEFAULT = 500; -const long MOVEMENT_STEPS_ACC_DEC_Y_DEFAULT = 500; -const long MOVEMENT_STEPS_ACC_DEC_Z_DEFAULT = 500; +const long MOVEMENT_STEPS_ACC_DEC_X_DEFAULT = 500; +const long MOVEMENT_STEPS_ACC_DEC_Y_DEFAULT = 500; +const long MOVEMENT_STEPS_ACC_DEC_Z_DEFAULT = 500; // Minimum speed in steps per second -const long MOVEMENT_MIN_SPD_X_DEFAULT = 50; -const long MOVEMENT_MIN_SPD_Y_DEFAULT = 50; -const long MOVEMENT_MIN_SPD_Z_DEFAULT = 50; +const long MOVEMENT_MIN_SPD_X_DEFAULT = 50; +const long MOVEMENT_MIN_SPD_Y_DEFAULT = 50; +const long MOVEMENT_MIN_SPD_Z_DEFAULT = 50; // Maxumum speed in steps per second -const long MOVEMENT_MAX_SPD_X_DEFAULT = 800; -const long MOVEMENT_MAX_SPD_Y_DEFAULT = 800; -const long MOVEMENT_MAX_SPD_Z_DEFAULT = 800; +const long MOVEMENT_MAX_SPD_X_DEFAULT = 800; +const long MOVEMENT_MAX_SPD_Y_DEFAULT = 800; +const long MOVEMENT_MAX_SPD_Z_DEFAULT = 800; // Use encoder (0 or 1) -const long ENCODER_ENABLED_X_DEFAULT = 0; -const long ENCODER_ENABLED_Y_DEFAULT = 0; -const long ENCODER_ENABLED_Z_DEFAULT = 0; +const long ENCODER_ENABLED_X_DEFAULT = 0; +const long ENCODER_ENABLED_Y_DEFAULT = 0; +const long ENCODER_ENABLED_Z_DEFAULT = 0; // Type of enocder. // 0 = non-differential encoder, channel A,B // 1 = differenttial encoder, channel A, A*, B, B* -const long ENCODER_TYPE_X_DEFAULT = 0; -const long ENCODER_TYPE_Y_DEFAULT = 0; -const long ENCODER_TYPE_Z_DEFAULT = 0; +const long ENCODER_TYPE_X_DEFAULT = 0; +const long ENCODER_TYPE_Y_DEFAULT = 0; +const long ENCODER_TYPE_Z_DEFAULT = 0; // Position = encoder position * scaling / 100 -const long ENCODER_SCALING_X_DEFAULT = 100; -const long ENCODER_SCALING_Y_DEFAULT = 100; -const long ENCODER_SCALING_Z_DEFAULT = 100; +const long ENCODER_SCALING_X_DEFAULT = 100; +const long ENCODER_SCALING_Y_DEFAULT = 100; +const long ENCODER_SCALING_Z_DEFAULT = 100; // Number of stes missed before motor is seen as not moving -const long ENCODER_MISSED_STEPS_MAX_X_DEFAULT = 10; -const long ENCODER_MISSED_STEPS_MAX_Y_DEFAULT = 10; -const long ENCODER_MISSED_STEPS_MAX_Z_DEFAULT = 10; +const long ENCODER_MISSED_STEPS_MAX_X_DEFAULT = 10; +const long ENCODER_MISSED_STEPS_MAX_Y_DEFAULT = 10; +const long ENCODER_MISSED_STEPS_MAX_Z_DEFAULT = 10; // How much a good step is substracted from the missed step total (1-99) -const long ENCODER_MISSED_STEPS_DECAY_X_DEFAULT = 10; -const long ENCODER_MISSED_STEPS_DECAY_Y_DEFAULT = 10; -const long ENCODER_MISSED_STEPS_DECAY_Z_DEFAULT = 10; +const long ENCODER_MISSED_STEPS_DECAY_X_DEFAULT = 10; +const long ENCODER_MISSED_STEPS_DECAY_Y_DEFAULT = 10; +const long ENCODER_MISSED_STEPS_DECAY_Z_DEFAULT = 10; // pin guard default settings -const long PIN_GUARD_1_PIN_NR_DEFAULT = 0; -const long PIN_GUARD_1_TIME_OUT_DEFAULT = 60; -const long PIN_GUARD_1_ACTIVE_STATE_DEFAULT = 1; - -const long PIN_GUARD_2_PIN_NR_DEFAULT = 0; -const long PIN_GUARD_2_TIME_OUT_DEFAULT = 60; -const long PIN_GUARD_2_ACTIVE_STATE_DEFAULT = 1; +const long PIN_GUARD_1_PIN_NR_DEFAULT = 0; +const long PIN_GUARD_1_TIME_OUT_DEFAULT = 60; +const long PIN_GUARD_1_ACTIVE_STATE_DEFAULT = 1; -const long PIN_GUARD_3_PIN_NR_DEFAULT = 0; -const long PIN_GUARD_3_TIME_OUT_DEFAULT = 60; -const long PIN_GUARD_3_ACTIVE_STATE_DEFAULT = 1; +const long PIN_GUARD_2_PIN_NR_DEFAULT = 0; +const long PIN_GUARD_2_TIME_OUT_DEFAULT = 60; +const long PIN_GUARD_2_ACTIVE_STATE_DEFAULT = 1; -const long PIN_GUARD_4_PIN_NR_DEFAULT = 0; -const long PIN_GUARD_4_TIME_OUT_DEFAULT = 60; -const long PIN_GUARD_4_ACTIVE_STATE_DEFAULT = 1; +const long PIN_GUARD_3_PIN_NR_DEFAULT = 0; +const long PIN_GUARD_3_TIME_OUT_DEFAULT = 60; +const long PIN_GUARD_3_ACTIVE_STATE_DEFAULT = 1; -const long PIN_GUARD_5_PIN_NR_DEFAULT = 0; -const long PIN_GUARD_5_TIME_OUT_DEFAULT = 60; -const long PIN_GUARD_5_ACTIVE_STATE_DEFAULT = 1; +const long PIN_GUARD_4_PIN_NR_DEFAULT = 0; +const long PIN_GUARD_4_TIME_OUT_DEFAULT = 60; +const long PIN_GUARD_4_ACTIVE_STATE_DEFAULT = 1; +const long PIN_GUARD_5_PIN_NR_DEFAULT = 0; +const long PIN_GUARD_5_TIME_OUT_DEFAULT = 60; +const long PIN_GUARD_5_ACTIVE_STATE_DEFAULT = 1; -const long STATUS_GENERAL_DEFAULT = 0; +const long STATUS_GENERAL_DEFAULT = 0; const char SOFTWARE_VERSION[] = "GENESIS.V.01.08.EXPERIMENTAL\0"; diff --git a/src/CurrentState.cpp b/src/CurrentState.cpp index 66955f5..739fd82 100644 --- a/src/CurrentState.cpp +++ b/src/CurrentState.cpp @@ -7,7 +7,7 @@ #include "CurrentState.h" -static CurrentState* instance; +static CurrentState *instance; long x = 0; long y = 0; long z = 0; @@ -15,120 +15,138 @@ unsigned int speed = 0; bool endStopState[3][2]; long Q = 0; -CurrentState * CurrentState::getInstance() { - if (!instance) { - instance = new CurrentState(); - }; - return instance; +CurrentState *CurrentState::getInstance() +{ + if (!instance) + { + instance = new CurrentState(); + }; + return instance; }; -CurrentState::CurrentState() { - x = 0; - y = 0; - z = 0; - speed = 0; - Q = 0; +CurrentState::CurrentState() +{ + x = 0; + y = 0; + z = 0; + speed = 0; + Q = 0; } -long CurrentState::getX() { - return x; +long CurrentState::getX() +{ + return x; } -long CurrentState::getY() { - return y; +long CurrentState::getY() +{ + return y; } -long CurrentState::getZ() { - return z; +long CurrentState::getZ() +{ + return z; } -long* CurrentState::getPoint() { - static long currentPoint[3] = {x, y, z}; - return currentPoint; +long *CurrentState::getPoint() +{ + static long currentPoint[3] = {x, y, z}; + return currentPoint; } -void CurrentState::setX(long newX) { - x = newX; +void CurrentState::setX(long newX) +{ + x = newX; } -void CurrentState::setY(long newY) { - y = newY; +void CurrentState::setY(long newY) +{ + y = newY; } -void CurrentState::setZ(long newZ) { - z = newZ; +void CurrentState::setZ(long newZ) +{ + z = newZ; } -void CurrentState::setEndStopState(unsigned int axis, unsigned int position, bool state) { - endStopState[axis][position] = state; +void CurrentState::setEndStopState(unsigned int axis, unsigned int position, bool state) +{ + endStopState[axis][position] = state; } -void CurrentState::storeEndStops() { - CurrentState::getInstance()->setEndStopState(0,0,digitalRead(X_MIN_PIN)); - CurrentState::getInstance()->setEndStopState(0,1,digitalRead(X_MAX_PIN)); - CurrentState::getInstance()->setEndStopState(1,0,digitalRead(Y_MIN_PIN)); - CurrentState::getInstance()->setEndStopState(1,1,digitalRead(Y_MAX_PIN)); - CurrentState::getInstance()->setEndStopState(2,0,digitalRead(Z_MIN_PIN)); - CurrentState::getInstance()->setEndStopState(2,1,digitalRead(Z_MAX_PIN)); +void CurrentState::storeEndStops() +{ + CurrentState::getInstance()->setEndStopState(0, 0, digitalRead(X_MIN_PIN)); + CurrentState::getInstance()->setEndStopState(0, 1, digitalRead(X_MAX_PIN)); + CurrentState::getInstance()->setEndStopState(1, 0, digitalRead(Y_MIN_PIN)); + CurrentState::getInstance()->setEndStopState(1, 1, digitalRead(Y_MAX_PIN)); + CurrentState::getInstance()->setEndStopState(2, 0, digitalRead(Z_MIN_PIN)); + CurrentState::getInstance()->setEndStopState(2, 1, digitalRead(Z_MAX_PIN)); } -void CurrentState::printPosition() { - Serial.print("R82"); - Serial.print(" X"); - Serial.print(x); - Serial.print(" Y"); - Serial.print(y); - Serial.print(" Z"); - Serial.print(z); -// Serial.print("\r\n"); - printQAndNewLine(); +void CurrentState::printPosition() +{ + Serial.print("R82"); + Serial.print(" X"); + Serial.print(x); + Serial.print(" Y"); + Serial.print(y); + Serial.print(" Z"); + Serial.print(z); + // Serial.print("\r\n"); + printQAndNewLine(); } void CurrentState::printBool(bool value) { - if (value) - { - Serial.print("1"); - } - else - { - Serial.print("0"); - } + if (value) + { + Serial.print("1"); + } + else + { + Serial.print("0"); + } } -void CurrentState::printEndStops() { - Serial.print("R81"); - Serial.print(" XA"); - printBool(endStopState[0][0]); - Serial.print(" XB"); - printBool(endStopState[0][1]); - Serial.print(" YA"); - printBool(endStopState[1][0]); - Serial.print(" YB"); - printBool(endStopState[1][1]); - Serial.print(" ZA"); - printBool(endStopState[2][0]); - Serial.print(" ZB"); - printBool(endStopState[2][1]); - //Serial.print("\r\n"); - printQAndNewLine(); -} - -void CurrentState::print() { - printPosition(); - printEndStops(); +void CurrentState::printEndStops() +{ + Serial.print("R81"); + Serial.print(" XA"); + printBool(endStopState[0][0]); + Serial.print(" XB"); + printBool(endStopState[0][1]); + Serial.print(" YA"); + printBool(endStopState[1][0]); + Serial.print(" YB"); + printBool(endStopState[1][1]); + Serial.print(" ZA"); + printBool(endStopState[2][0]); + Serial.print(" ZB"); + printBool(endStopState[2][1]); + //Serial.print("\r\n"); + printQAndNewLine(); +} + +void CurrentState::print() +{ + printPosition(); + printEndStops(); } -void CurrentState::setQ(long q) { - Q = q; +void CurrentState::setQ(long q) +{ + Q = q; } -void CurrentState::resetQ() { - Q = 0; +void CurrentState::resetQ() +{ + Q = 0; } -void CurrentState::printQAndNewLine() { - Serial.print(" Q"); - Serial.print(Q); - Serial.print("\r\n"); +void CurrentState::printQAndNewLine() +{ + Serial.print(" Q"); + Serial.print(Q); + Serial.print("\r\n"); } diff --git a/src/CurrentState.h b/src/CurrentState.h index 801a083..fa96e46 100644 --- a/src/CurrentState.h +++ b/src/CurrentState.h @@ -10,32 +10,32 @@ #include "Arduino.h" #include "pins.h" -class CurrentState { +class CurrentState +{ public: - static CurrentState* getInstance(); - long getX(); - long getY(); - long getZ(); - long* getPoint(); - void setX(long); - void setY(long); - void setZ(long); - void setEndStopState(unsigned int, unsigned int, bool); - void printPosition(); - void storeEndStops(); - void printEndStops(); - void print(); - void printBool(bool); + static CurrentState *getInstance(); + long getX(); + long getY(); + long getZ(); + long *getPoint(); + void setX(long); + void setY(long); + void setZ(long); + void setEndStopState(unsigned int, unsigned int, bool); + void printPosition(); + void storeEndStops(); + void printEndStops(); + void print(); + void printBool(bool); - void setQ(long); - void resetQ(); - void printQAndNewLine(); + void setQ(long); + void resetQ(); + void printQAndNewLine(); private: - CurrentState(); - CurrentState(CurrentState const&); - void operator=(CurrentState const&); - + CurrentState(); + CurrentState(CurrentState const &); + void operator=(CurrentState const &); }; #endif /* CURRENTSTATE_H_ */ diff --git a/src/F11Handler.cpp b/src/F11Handler.cpp index c0ae60b..8f28cf4 100644 --- a/src/F11Handler.cpp +++ b/src/F11Handler.cpp @@ -8,31 +8,34 @@ #include "F11Handler.h" - -static F11Handler* instance; - -F11Handler * F11Handler::getInstance() { - if (!instance) { - instance = new F11Handler(); - }; - return instance; +static F11Handler *instance; + +F11Handler *F11Handler::getInstance() +{ + if (!instance) + { + instance = new F11Handler(); + }; + return instance; +}; + +F11Handler::F11Handler() +{ } -; -F11Handler::F11Handler() { -} +int F11Handler::execute(Command *command) +{ -int F11Handler::execute(Command* command) { + if (LOGGING) + { + Serial.print("R99 HOME X\r\n"); + } - if (LOGGING) { - Serial.print("R99 HOME X\r\n"); - } + StepperControl::getInstance()->moveToCoords(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(); - } - return 0; + if (LOGGING) + { + CurrentState::getInstance()->print(); + } + return 0; } - diff --git a/src/F11Handler.h b/src/F11Handler.h index 2ae7707..6014c34 100644 --- a/src/F11Handler.h +++ b/src/F11Handler.h @@ -14,17 +14,18 @@ #include "Config.h" #include "StepperControl.h" -class F11Handler : public GCodeHandler { +class F11Handler : public GCodeHandler +{ public: - static F11Handler* getInstance(); - int execute(Command*); + static F11Handler *getInstance(); + int execute(Command *); + private: - F11Handler(); - F11Handler(F11Handler const&); - void operator=(F11Handler const&); - long adjustStepAmount(long); - long getNumberOfSteps(double, double); + F11Handler(); + F11Handler(F11Handler const &); + void operator=(F11Handler const &); + long adjustStepAmount(long); + long getNumberOfSteps(double, double); }; #endif /* F11HANDLER_H_ */ - diff --git a/src/F12Handler.cpp b/src/F12Handler.cpp index 2660bf5..c8e434f 100644 --- a/src/F12Handler.cpp +++ b/src/F12Handler.cpp @@ -8,31 +8,34 @@ #include "F12Handler.h" - -static F12Handler* instance; - -F12Handler * F12Handler::getInstance() { - if (!instance) { - instance = new F12Handler(); - }; - return instance; +static F12Handler *instance; + +F12Handler *F12Handler::getInstance() +{ + if (!instance) + { + instance = new F12Handler(); + }; + return instance; +}; + +F12Handler::F12Handler() +{ } -; -F12Handler::F12Handler() { -} +int F12Handler::execute(Command *command) +{ -int F12Handler::execute(Command* command) { + if (LOGGING) + { + Serial.print("R99 HOME Y\r\n"); + } - if (LOGGING) { - Serial.print("R99 HOME Y\r\n"); - } + StepperControl::getInstance()->moveToCoords(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(); - } - return 0; + if (LOGGING) + { + CurrentState::getInstance()->print(); + } + return 0; } - diff --git a/src/F12Handler.h b/src/F12Handler.h index 425f78e..31851f9 100644 --- a/src/F12Handler.h +++ b/src/F12Handler.h @@ -14,17 +14,18 @@ #include "Config.h" #include "StepperControl.h" -class F12Handler : public GCodeHandler { +class F12Handler : public GCodeHandler +{ public: - static F12Handler* getInstance(); - int execute(Command*); + static F12Handler *getInstance(); + int execute(Command *); + private: - F12Handler(); - F12Handler(F12Handler const&); - void operator=(F12Handler const&); - long adjustStepAmount(long); - long getNumberOfSteps(double, double); + F12Handler(); + F12Handler(F12Handler const &); + void operator=(F12Handler const &); + long adjustStepAmount(long); + long getNumberOfSteps(double, double); }; #endif /* F12HANDLER_H_ */ - diff --git a/src/F13Handler.cpp b/src/F13Handler.cpp index b019b66..7ab6d00 100644 --- a/src/F13Handler.cpp +++ b/src/F13Handler.cpp @@ -8,31 +8,34 @@ #include "F13Handler.h" - -static F13Handler* instance; - -F13Handler * F13Handler::getInstance() { - if (!instance) { - instance = new F13Handler(); - }; - return instance; +static F13Handler *instance; + +F13Handler *F13Handler::getInstance() +{ + if (!instance) + { + instance = new F13Handler(); + }; + return instance; +}; + +F13Handler::F13Handler() +{ } -; -F13Handler::F13Handler() { -} +int F13Handler::execute(Command *command) +{ -int F13Handler::execute(Command* command) { + if (LOGGING) + { + Serial.print("R99 HOME Z\r\n"); + } - if (LOGGING) { - Serial.print("R99 HOME Z\r\n"); - } + StepperControl::getInstance()->moveToCoords(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(); - } - return 0; + if (LOGGING) + { + CurrentState::getInstance()->print(); + } + return 0; } - diff --git a/src/F13Handler.h b/src/F13Handler.h index ce23a48..6b3cd0f 100644 --- a/src/F13Handler.h +++ b/src/F13Handler.h @@ -14,17 +14,18 @@ #include "Config.h" #include "StepperControl.h" -class F13Handler : public GCodeHandler { +class F13Handler : public GCodeHandler +{ public: - static F13Handler* getInstance(); - int execute(Command*); + static F13Handler *getInstance(); + int execute(Command *); + private: - F13Handler(); - F13Handler(F13Handler const&); - void operator=(F13Handler const&); - long adjustStepAmount(long); - long getNumberOfSteps(double, double); + F13Handler(); + F13Handler(F13Handler const &); + void operator=(F13Handler const &); + long adjustStepAmount(long); + long getNumberOfSteps(double, double); }; #endif /* F13HANDLER_H_ */ - diff --git a/src/F14Handler.cpp b/src/F14Handler.cpp index 333e60c..e403cf3 100644 --- a/src/F14Handler.cpp +++ b/src/F14Handler.cpp @@ -8,41 +8,43 @@ #include "F14Handler.h" - -static F14Handler* instance; - -F14Handler * F14Handler::getInstance() { - if (!instance) { - instance = new F14Handler(); - }; - return instance; -} -; - -F14Handler::F14Handler() { +static F14Handler *instance; + +F14Handler *F14Handler::getInstance() +{ + if (!instance) + { + instance = new F14Handler(); + }; + return instance; +}; + +F14Handler::F14Handler() +{ } -int F14Handler::execute(Command* command) { +int F14Handler::execute(Command *command) +{ - int ret = 0; + int ret = 0; - if (LOGGING) { - Serial.print("R99 CALIBRATE X\r\n"); - } + if (LOGGING) + { + Serial.print("R99 CALIBRATE X\r\n"); + } - ret = StepperControl::getInstance()->calibrateAxis(0); + ret = StepperControl::getInstance()->calibrateAxis(0); - /* + /* if (ret == 0) { StepperControl::getInstance()->moveToCoords(0,0,0, 0,0,0, true, false, false); } */ - if (LOGGING) { - CurrentState::getInstance()->print(); - } + if (LOGGING) + { + CurrentState::getInstance()->print(); + } - return 0; + return 0; } - - diff --git a/src/F14Handler.h b/src/F14Handler.h index d46b593..51916d2 100644 --- a/src/F14Handler.h +++ b/src/F14Handler.h @@ -15,18 +15,18 @@ #include "Config.h" #include "StepperControl.h" -class F14Handler : public GCodeHandler { +class F14Handler : public GCodeHandler +{ public: - static F14Handler* getInstance(); - int execute(Command*); + static F14Handler *getInstance(); + int execute(Command *); + private: - F14Handler(); - F14Handler(F14Handler const&); - void operator=(F14Handler const&); - long adjustStepAmount(long); - long getNumberOfSteps(double, double); + F14Handler(); + F14Handler(F14Handler const &); + void operator=(F14Handler const &); + long adjustStepAmount(long); + long getNumberOfSteps(double, double); }; #endif /* F14HANDLER_H_ */ - - diff --git a/src/F15Handler.cpp b/src/F15Handler.cpp index ec0c4be..c9daafc 100644 --- a/src/F15Handler.cpp +++ b/src/F15Handler.cpp @@ -8,31 +8,35 @@ #include "F15Handler.h" - -static F15Handler* instance; - -F15Handler * F15Handler::getInstance() { - if (!instance) { - instance = new F15Handler(); - }; - return instance; -} -; - -F15Handler::F15Handler() { +static F15Handler *instance; + +F15Handler *F15Handler::getInstance() +{ + if (!instance) + { + instance = new F15Handler(); + }; + return instance; +}; + +F15Handler::F15Handler() +{ } -int F15Handler::execute(Command* command) { +int F15Handler::execute(Command *command) +{ - if (LOGGING) { - Serial.print("R99 HOME Z\r\n"); - } + if (LOGGING) + { + Serial.print("R99 HOME Z\r\n"); + } - StepperControl::getInstance()->calibrateAxis(1); + StepperControl::getInstance()->calibrateAxis(1); - if (LOGGING) { - CurrentState::getInstance()->print(); - } + if (LOGGING) + { + CurrentState::getInstance()->print(); + } - return 0; + return 0; } diff --git a/src/F15Handler.h b/src/F15Handler.h index d3c7346..72b0968 100644 --- a/src/F15Handler.h +++ b/src/F15Handler.h @@ -15,18 +15,18 @@ #include "Config.h" #include "StepperControl.h" -class F15Handler : public GCodeHandler { +class F15Handler : public GCodeHandler +{ public: - static F15Handler* getInstance(); - int execute(Command*); + static F15Handler *getInstance(); + int execute(Command *); + private: - F15Handler(); - F15Handler(F15Handler const&); - void operator=(F15Handler const&); - long adjustStepAmount(long); - long getNumberOfSteps(double, double); + F15Handler(); + F15Handler(F15Handler const &); + void operator=(F15Handler const &); + long adjustStepAmount(long); + long getNumberOfSteps(double, double); }; #endif /* F15HANDLER_H_ */ - - diff --git a/src/F16Handler.cpp b/src/F16Handler.cpp index c2680f2..b7f1eec 100644 --- a/src/F16Handler.cpp +++ b/src/F16Handler.cpp @@ -8,33 +8,35 @@ #include "F16Handler.h" - -static F16Handler* instance; - -F16Handler * F16Handler::getInstance() { - if (!instance) { - instance = new F16Handler(); - }; - return instance; -} -; - -F16Handler::F16Handler() { +static F16Handler *instance; + +F16Handler *F16Handler::getInstance() +{ + if (!instance) + { + instance = new F16Handler(); + }; + return instance; +}; + +F16Handler::F16Handler() +{ } -int F16Handler::execute(Command* command) { +int F16Handler::execute(Command *command) +{ - if (LOGGING) { - Serial.print("R99 HOME Z\r\n"); - } + if (LOGGING) + { + Serial.print("R99 HOME Z\r\n"); + } - StepperControl::getInstance()->calibrateAxis(2); + StepperControl::getInstance()->calibrateAxis(2); - if (LOGGING) { - CurrentState::getInstance()->print(); - } + if (LOGGING) + { + CurrentState::getInstance()->print(); + } - return 0; + return 0; } - - diff --git a/src/F16Handler.h b/src/F16Handler.h index dbd7b8a..ac795df 100644 --- a/src/F16Handler.h +++ b/src/F16Handler.h @@ -15,18 +15,18 @@ #include "Config.h" #include "StepperControl.h" -class F16Handler : public GCodeHandler { +class F16Handler : public GCodeHandler +{ public: - static F16Handler* getInstance(); - int execute(Command*); + static F16Handler *getInstance(); + int execute(Command *); + private: - F16Handler(); - F16Handler(F16Handler const&); - void operator=(F16Handler const&); - long adjustStepAmount(long); - long getNumberOfSteps(double, double); + F16Handler(); + F16Handler(F16Handler const &); + void operator=(F16Handler const &); + long adjustStepAmount(long); + long getNumberOfSteps(double, double); }; #endif /* F16HANDLER_H_ */ - - diff --git a/src/F20Handler.cpp b/src/F20Handler.cpp index be705be..de13d0e 100644 --- a/src/F20Handler.cpp +++ b/src/F20Handler.cpp @@ -7,23 +7,25 @@ #include "F20Handler.h" - -static F20Handler* instance; - -F20Handler * F20Handler::getInstance() { - if (!instance) { - instance = new F20Handler(); - }; - return instance; -} -; - -F20Handler::F20Handler() { +static F20Handler *instance; + +F20Handler *F20Handler::getInstance() +{ + if (!instance) + { + instance = new F20Handler(); + }; + return instance; +}; + +F20Handler::F20Handler() +{ } -int F20Handler::execute(Command* command) { +int F20Handler::execute(Command *command) +{ - ParameterList::getInstance()->readAllValues(); + ParameterList::getInstance()->readAllValues(); - return 1; + return 1; } diff --git a/src/F20Handler.h b/src/F20Handler.h index 6499826..8b724cb 100644 --- a/src/F20Handler.h +++ b/src/F20Handler.h @@ -14,16 +14,18 @@ #include "Config.h" #include "StepperControl.h" -class F20Handler : public GCodeHandler { +class F20Handler : public GCodeHandler +{ public: - static F20Handler* getInstance(); - int execute(Command*); + static F20Handler *getInstance(); + int execute(Command *); + private: - F20Handler(); - F20Handler(F20Handler const&); - void operator=(F20Handler const&); - long adjustStepAmount(long); - long getNumberOfSteps(double, double); + F20Handler(); + F20Handler(F20Handler const &); + void operator=(F20Handler const &); + long adjustStepAmount(long); + long getNumberOfSteps(double, double); }; #endif /* F20HANDLER_H_ */ diff --git a/src/F21Handler.cpp b/src/F21Handler.cpp index b23c0a2..5c03fb2 100644 --- a/src/F21Handler.cpp +++ b/src/F21Handler.cpp @@ -7,23 +7,25 @@ #include "F21Handler.h" - -static F21Handler* instance; - -F21Handler * F21Handler::getInstance() { - if (!instance) { - instance = new F21Handler(); - }; - return instance; -} -; - -F21Handler::F21Handler() { +static F21Handler *instance; + +F21Handler *F21Handler::getInstance() +{ + if (!instance) + { + instance = new F21Handler(); + }; + return instance; +}; + +F21Handler::F21Handler() +{ } -int F21Handler::execute(Command* command) { +int F21Handler::execute(Command *command) +{ - ParameterList::getInstance()->readValue(command->getP()); + ParameterList::getInstance()->readValue(command->getP()); - return 0; + return 0; } diff --git a/src/F21Handler.h b/src/F21Handler.h index c177d1a..e0ca468 100644 --- a/src/F21Handler.h +++ b/src/F21Handler.h @@ -15,16 +15,18 @@ #include "StepperControl.h" #include "ParameterList.h" -class F21Handler : public GCodeHandler { +class F21Handler : public GCodeHandler +{ public: - static F21Handler* getInstance(); - int execute(Command*); + static F21Handler *getInstance(); + int execute(Command *); + private: - F21Handler(); - F21Handler(F21Handler const&); - void operator=(F21Handler const&); - long adjustStepAmount(long); - long getNumberOfSteps(double, double); + F21Handler(); + F21Handler(F21Handler const &); + void operator=(F21Handler const &); + long adjustStepAmount(long); + long getNumberOfSteps(double, double); }; #endif /* F21HANDLER_H_ */ diff --git a/src/F22Handler.cpp b/src/F22Handler.cpp index e498a8e..aaea958 100644 --- a/src/F22Handler.cpp +++ b/src/F22Handler.cpp @@ -7,23 +7,25 @@ #include "F22Handler.h" - -static F22Handler* instance; - -F22Handler * F22Handler::getInstance() { - if (!instance) { - instance = new F22Handler(); - }; - return instance; +static F22Handler *instance; + +F22Handler *F22Handler::getInstance() +{ + if (!instance) + { + instance = new F22Handler(); + }; + return instance; +}; + +F22Handler::F22Handler() +{ } -; -F22Handler::F22Handler() { -} +int F22Handler::execute(Command *command) +{ -int F22Handler::execute(Command* command) { - -/* + /* Serial.print("R99"); Serial.print(" "); Serial.print("write value"); @@ -42,7 +44,7 @@ Serial.print(" "); Serial.print("\r\n"); */ - ParameterList::getInstance()->writeValue(command->getP(), command->getV()); + ParameterList::getInstance()->writeValue(command->getP(), command->getV()); - return 0; + return 0; } diff --git a/src/F22Handler.h b/src/F22Handler.h index 5cfa73b..cf7ab13 100644 --- a/src/F22Handler.h +++ b/src/F22Handler.h @@ -15,16 +15,18 @@ #include "StepperControl.h" #include "ParameterList.h" -class F22Handler : public GCodeHandler { +class F22Handler : public GCodeHandler +{ public: - static F22Handler* getInstance(); - int execute(Command*); + static F22Handler *getInstance(); + int execute(Command *); + private: - F22Handler(); - F22Handler(F22Handler const&); - void operator=(F22Handler const&); - //long adjustStepAmount(long); - //long getNumberOfSteps(double, double); + F22Handler(); + F22Handler(F22Handler const &); + void operator=(F22Handler const &); + //long adjustStepAmount(long); + //long getNumberOfSteps(double, double); }; #endif /* F22HANDLER_H_ */ diff --git a/src/F31Handler.cpp b/src/F31Handler.cpp index 05d15fd..8dcc6a2 100644 --- a/src/F31Handler.cpp +++ b/src/F31Handler.cpp @@ -8,25 +8,25 @@ #include "F31Handler.h" - -static F31Handler* instance; - -F31Handler * F31Handler::getInstance() { - if (!instance) { - instance = new F31Handler(); - }; - return instance; -} -; - -F31Handler::F31Handler() { +static F31Handler *instance; + +F31Handler *F31Handler::getInstance() +{ + if (!instance) + { + instance = new F31Handler(); + }; + return instance; +}; + +F31Handler::F31Handler() +{ } -int F31Handler::execute(Command* command) { +int F31Handler::execute(Command *command) +{ - StatusList::getInstance()->readValue(command->getP()); + StatusList::getInstance()->readValue(command->getP()); - return 0; + return 0; } - - diff --git a/src/F31Handler.h b/src/F31Handler.h index 0f70794..90b12d1 100644 --- a/src/F31Handler.h +++ b/src/F31Handler.h @@ -16,19 +16,18 @@ #include "StepperControl.h" #include "StatusList.h" -class F31Handler : public GCodeHandler { +class F31Handler : public GCodeHandler +{ public: - static F31Handler* getInstance(); - int execute(Command*); + static F31Handler *getInstance(); + int execute(Command *); + private: - F31Handler(); - F31Handler(F31Handler const&); - void operator=(F31Handler const&); - //long adjustStepAmount(long); - //long getNumberOfSteps(double, double); + F31Handler(); + F31Handler(F31Handler const &); + void operator=(F31Handler const &); + //long adjustStepAmount(long); + //long getNumberOfSteps(double, double); }; #endif /* F31HANDLER_H_ */ - - - diff --git a/src/F32Handler.cpp b/src/F32Handler.cpp index e00fd60..f7d708e 100644 --- a/src/F32Handler.cpp +++ b/src/F32Handler.cpp @@ -8,25 +8,25 @@ #include "F32Handler.h" - -static F32Handler* instance; - -F32Handler * F32Handler::getInstance() { - if (!instance) { - instance = new F32Handler(); - }; - return instance; -} -; - -F32Handler::F32Handler() { +static F32Handler *instance; + +F32Handler *F32Handler::getInstance() +{ + if (!instance) + { + instance = new F32Handler(); + }; + return instance; +}; + +F32Handler::F32Handler() +{ } -int F32Handler::execute(Command* command) { +int F32Handler::execute(Command *command) +{ - StatusList::getInstance()->readValue(command->getP()); + StatusList::getInstance()->readValue(command->getP()); - return 0; + return 0; } - - diff --git a/src/F32Handler.h b/src/F32Handler.h index 877ba97..1dd7be5 100644 --- a/src/F32Handler.h +++ b/src/F32Handler.h @@ -16,19 +16,18 @@ #include "StepperControl.h" #include "StatusList.h" -class F32Handler : public GCodeHandler { +class F32Handler : public GCodeHandler +{ public: - static F32Handler* getInstance(); - int execute(Command*); + static F32Handler *getInstance(); + int execute(Command *); + private: - F32Handler(); - F32Handler(F32Handler const&); - void operator=(F32Handler const&); - long adjustStepAmount(long); - long getNumberOfSteps(double, double); + F32Handler(); + F32Handler(F32Handler const &); + void operator=(F32Handler const &); + long adjustStepAmount(long); + long getNumberOfSteps(double, double); }; #endif /* F32HANDLER_H_ */ - - - diff --git a/src/F41Handler.cpp b/src/F41Handler.cpp index 34cf06d..e585153 100644 --- a/src/F41Handler.cpp +++ b/src/F41Handler.cpp @@ -9,23 +9,25 @@ #include "F41Handler.h" - -static F41Handler* instance; - -F41Handler * F41Handler::getInstance() { - if (!instance) { - instance = new F41Handler(); - }; - return instance; -} -; - -F41Handler::F41Handler() { +static F41Handler *instance; + +F41Handler *F41Handler::getInstance() +{ + if (!instance) + { + instance = new F41Handler(); + }; + return instance; +}; + +F41Handler::F41Handler() +{ } -int F41Handler::execute(Command* command) { +int F41Handler::execute(Command *command) +{ - PinControl::getInstance()->writeValue(command->getP(),command->getV(), command->getM()); + PinControl::getInstance()->writeValue(command->getP(), command->getV(), command->getM()); - return 0; + return 0; } diff --git a/src/F41Handler.h b/src/F41Handler.h index bfea22d..420ca27 100644 --- a/src/F41Handler.h +++ b/src/F41Handler.h @@ -14,20 +14,16 @@ #include "Config.h" #include "PinControl.h" -class F41Handler : public GCodeHandler { +class F41Handler : public GCodeHandler +{ public: - static F41Handler* getInstance(); - int execute(Command*); + static F41Handler *getInstance(); + int execute(Command *); + private: - F41Handler(); - F41Handler(F41Handler const&); - void operator=(F41Handler const&); + F41Handler(); + F41Handler(F41Handler const &); + void operator=(F41Handler const &); }; #endif /* F41HANDLER_H_ */ - - - - - - diff --git a/src/F42Handler.cpp b/src/F42Handler.cpp index 7223c11..0eb92ec 100644 --- a/src/F42Handler.cpp +++ b/src/F42Handler.cpp @@ -7,23 +7,25 @@ #include "F42Handler.h" - -static F42Handler* instance; - -F42Handler * F42Handler::getInstance() { - if (!instance) { - instance = new F42Handler(); - }; - return instance; -} -; - -F42Handler::F42Handler() { +static F42Handler *instance; + +F42Handler *F42Handler::getInstance() +{ + if (!instance) + { + instance = new F42Handler(); + }; + return instance; +}; + +F42Handler::F42Handler() +{ } -int F42Handler::execute(Command* command) { +int F42Handler::execute(Command *command) +{ - PinControl::getInstance()->readValue(command->getP(), command->getM()); + PinControl::getInstance()->readValue(command->getP(), command->getM()); - return 0; + return 0; } diff --git a/src/F42Handler.h b/src/F42Handler.h index cbbbf54..ca91924 100644 --- a/src/F42Handler.h +++ b/src/F42Handler.h @@ -14,20 +14,16 @@ #include "Config.h" #include "PinControl.h" -class F42Handler : public GCodeHandler { +class F42Handler : public GCodeHandler +{ public: - static F42Handler* getInstance(); - int execute(Command*); + static F42Handler *getInstance(); + int execute(Command *); + private: - F42Handler(); - F42Handler(F42Handler const&); - void operator=(F42Handler const&); + F42Handler(); + F42Handler(F42Handler const &); + void operator=(F42Handler const &); }; #endif /* F42HANDLER_H_ */ - - - - - - diff --git a/src/F43Handler.cpp b/src/F43Handler.cpp index bbb1db1..a17d982 100644 --- a/src/F43Handler.cpp +++ b/src/F43Handler.cpp @@ -9,30 +9,25 @@ #include "F43Handler.h" - -static F43Handler* instance; - -F43Handler * F43Handler::getInstance() { - if (!instance) { - instance = new F43Handler(); - }; - return instance; -} -; - -F43Handler::F43Handler() { +static F43Handler *instance; + +F43Handler *F43Handler::getInstance() +{ + if (!instance) + { + instance = new F43Handler(); + }; + return instance; +}; + +F43Handler::F43Handler() +{ } -int F43Handler::execute(Command* command) { +int F43Handler::execute(Command *command) +{ - PinControl::getInstance()->setMode(command->getP(),command->getM()); + PinControl::getInstance()->setMode(command->getP(), command->getM()); - return 0; + return 0; } - - - - - - - diff --git a/src/F43Handler.h b/src/F43Handler.h index d0a27dc..12c680a 100644 --- a/src/F43Handler.h +++ b/src/F43Handler.h @@ -14,14 +14,16 @@ #include "Config.h" #include "PinControl.h" -class F43Handler : public GCodeHandler { +class F43Handler : public GCodeHandler +{ public: - static F43Handler* getInstance(); - int execute(Command*); + static F43Handler *getInstance(); + int execute(Command *); + private: - F43Handler(); - F43Handler(F43Handler const&); - void operator=(F43Handler const&); + F43Handler(); + F43Handler(F43Handler const &); + void operator=(F43Handler const &); }; #endif /* F43HANDLER_H_ */ diff --git a/src/F44Handler.cpp b/src/F44Handler.cpp index 8cc81dc..7824d94 100644 --- a/src/F44Handler.cpp +++ b/src/F44Handler.cpp @@ -9,23 +9,25 @@ #include "F44Handler.h" - -static F44Handler* instance; - -F44Handler * F44Handler::getInstance() { - if (!instance) { - instance = new F44Handler(); - }; - return instance; -} -; - -F44Handler::F44Handler() { +static F44Handler *instance; + +F44Handler *F44Handler::getInstance() +{ + if (!instance) + { + instance = new F44Handler(); + }; + return instance; +}; + +F44Handler::F44Handler() +{ } -int F44Handler::execute(Command* command) { +int F44Handler::execute(Command *command) +{ - PinControl::getInstance()->writePulse(command->getP(),command->getV(),command->getW(),command->getT(), command->getM()); + PinControl::getInstance()->writePulse(command->getP(), command->getV(), command->getW(), command->getT(), command->getM()); - return 0; + return 0; } diff --git a/src/F44Handler.h b/src/F44Handler.h index 67f68fd..edb7ca8 100644 --- a/src/F44Handler.h +++ b/src/F44Handler.h @@ -14,17 +14,16 @@ #include "Config.h" #include "PinControl.h" -class F44Handler : public GCodeHandler { +class F44Handler : public GCodeHandler +{ public: - static F44Handler* getInstance(); - int execute(Command*); + static F44Handler *getInstance(); + int execute(Command *); + private: - F44Handler(); - F44Handler(F44Handler const&); - void operator=(F44Handler const&); + F44Handler(); + F44Handler(F44Handler const &); + void operator=(F44Handler const &); }; #endif /* F44HANDLER_H_ */ - - - diff --git a/src/F61Handler.cpp b/src/F61Handler.cpp index da78f40..8bf97db 100644 --- a/src/F61Handler.cpp +++ b/src/F61Handler.cpp @@ -9,23 +9,25 @@ #include "F61Handler.h" - -static F61Handler* instance; - -F61Handler * F61Handler::getInstance() { - if (!instance) { - instance = new F61Handler(); - }; - return instance; -} -; - -F61Handler::F61Handler() { +static F61Handler *instance; + +F61Handler *F61Handler::getInstance() +{ + if (!instance) + { + instance = new F61Handler(); + }; + return instance; +}; + +F61Handler::F61Handler() +{ } -int F61Handler::execute(Command* command) { +int F61Handler::execute(Command *command) +{ - ServoControl::getInstance()->setAngle(command->getP(),command->getV()); + ServoControl::getInstance()->setAngle(command->getP(), command->getV()); - return 0; + return 0; } diff --git a/src/F61Handler.h b/src/F61Handler.h index eb52017..6a5b242 100644 --- a/src/F61Handler.h +++ b/src/F61Handler.h @@ -14,20 +14,16 @@ #include "Config.h" #include "ServoControl.h" -class F61Handler : public GCodeHandler { +class F61Handler : public GCodeHandler +{ public: - static F61Handler* getInstance(); - int execute(Command*); + static F61Handler *getInstance(); + int execute(Command *); + private: - F61Handler(); - F61Handler(F61Handler const&); - void operator=(F61Handler const&); + F61Handler(); + F61Handler(F61Handler const &); + void operator=(F61Handler const &); }; #endif /* F61HANDLER_H_ */ - - - - - - diff --git a/src/F81Handler.cpp b/src/F81Handler.cpp index d565d3f..5df67c1 100644 --- a/src/F81Handler.cpp +++ b/src/F81Handler.cpp @@ -9,40 +9,40 @@ #include "F81Handler.h" - -static F81Handler* instance; - -F81Handler * F81Handler::getInstance() { - if (!instance) { - instance = new F81Handler(); - }; - return instance; -} -; - -F81Handler::F81Handler() { +static F81Handler *instance; + +F81Handler *F81Handler::getInstance() +{ + if (!instance) + { + instance = new F81Handler(); + }; + return instance; +}; + +F81Handler::F81Handler() +{ } -int F81Handler::execute(Command* command) { +int F81Handler::execute(Command *command) +{ -Serial.print("home\r\n"); + Serial.print("home\r\n"); - if (LOGGING) { - Serial.print("R99 Report end stops\r\n"); - } + if (LOGGING) + { + Serial.print("R99 Report end stops\r\n"); + } - // Report back the end stops - CurrentState::getInstance()->storeEndStops(); - CurrentState::getInstance()->printEndStops(); + // Report back the end stops + CurrentState::getInstance()->storeEndStops(); + CurrentState::getInstance()->printEndStops(); - // Also report back some selected pin numbers: 8, 9, 10, 13 - PinControl::getInstance()->readValue( 8, 0); - PinControl::getInstance()->readValue( 9, 0); - PinControl::getInstance()->readValue(10, 0); - PinControl::getInstance()->readValue(13, 0); + // Also report back some selected pin numbers: 8, 9, 10, 13 + PinControl::getInstance()->readValue(8, 0); + PinControl::getInstance()->readValue(9, 0); + PinControl::getInstance()->readValue(10, 0); + PinControl::getInstance()->readValue(13, 0); - - - return 0; + return 0; } - diff --git a/src/F81Handler.h b/src/F81Handler.h index b0aaf80..23275f0 100644 --- a/src/F81Handler.h +++ b/src/F81Handler.h @@ -15,17 +15,18 @@ #include "StepperControl.h" #include "PinControl.h" -class F81Handler : public GCodeHandler { +class F81Handler : public GCodeHandler +{ public: - static F81Handler* getInstance(); - int execute(Command*); + static F81Handler *getInstance(); + int execute(Command *); + private: - F81Handler(); - F81Handler(F81Handler const&); - void operator=(F81Handler const&); - long adjustStepAmount(long); - long getNumberOfSteps(double, double); + F81Handler(); + F81Handler(F81Handler const &); + void operator=(F81Handler const &); + long adjustStepAmount(long); + long getNumberOfSteps(double, double); }; #endif /* F81HANDLER_H_ */ - diff --git a/src/F82Handler.cpp b/src/F82Handler.cpp index 1e171ae..791359a 100644 --- a/src/F82Handler.cpp +++ b/src/F82Handler.cpp @@ -9,30 +9,30 @@ #include "F82Handler.h" - -static F82Handler* instance; - -F82Handler * F82Handler::getInstance() { - if (!instance) { - instance = new F82Handler(); - }; - return instance; -} -; - -F82Handler::F82Handler() { +static F82Handler *instance; + +F82Handler *F82Handler::getInstance() +{ + if (!instance) + { + instance = new F82Handler(); + }; + return instance; +}; + +F82Handler::F82Handler() +{ } -int F82Handler::execute(Command* command) { +int F82Handler::execute(Command *command) +{ - if (LOGGING) { - Serial.print("R99 Report current position\r\n"); - } + if (LOGGING) + { + Serial.print("R99 Report current position\r\n"); + } - CurrentState::getInstance()->printPosition(); + CurrentState::getInstance()->printPosition(); - return 0; + return 0; } - - - diff --git a/src/F82Handler.h b/src/F82Handler.h index dd5c1a0..2fecb70 100644 --- a/src/F82Handler.h +++ b/src/F82Handler.h @@ -14,17 +14,18 @@ #include "Config.h" #include "StepperControl.h" -class F82Handler : public GCodeHandler { +class F82Handler : public GCodeHandler +{ public: - static F82Handler* getInstance(); - int execute(Command*); + static F82Handler *getInstance(); + int execute(Command *); + private: - F82Handler(); - F82Handler(F82Handler const&); - void operator=(F82Handler const&); - long adjustStepAmount(long); - long getNumberOfSteps(double, double); + F82Handler(); + F82Handler(F82Handler const &); + void operator=(F82Handler const &); + long adjustStepAmount(long); + long getNumberOfSteps(double, double); }; #endif /* F82HANDLER_H_ */ - diff --git a/src/F83Handler.cpp b/src/F83Handler.cpp index 4db8cb7..422c1b1 100644 --- a/src/F83Handler.cpp +++ b/src/F83Handler.cpp @@ -9,33 +9,33 @@ #include "F83Handler.h" - -static F83Handler* instance; - -F83Handler * F83Handler::getInstance() { - if (!instance) { - instance = new F83Handler(); - }; - return instance; -} -; - -F83Handler::F83Handler() { +static F83Handler *instance; + +F83Handler *F83Handler::getInstance() +{ + if (!instance) + { + instance = new F83Handler(); + }; + return instance; +}; + +F83Handler::F83Handler() +{ } -int F83Handler::execute(Command* command) { +int F83Handler::execute(Command *command) +{ - if (LOGGING) { - Serial.print("R99 Report server version\r\n"); - } + if (LOGGING) + { + Serial.print("R99 Report server version\r\n"); + } - Serial.print("R83 "); - Serial.print(SOFTWARE_VERSION); - //Serial.print("\r\n"); - CurrentState::getInstance()->printQAndNewLine(); + Serial.print("R83 "); + Serial.print(SOFTWARE_VERSION); + //Serial.print("\r\n"); + CurrentState::getInstance()->printQAndNewLine(); - return 0; + return 0; } - - - diff --git a/src/F83Handler.h b/src/F83Handler.h index 4b5c447..1f8b90c 100644 --- a/src/F83Handler.h +++ b/src/F83Handler.h @@ -14,17 +14,18 @@ #include "Config.h" #include "StepperControl.h" -class F83Handler : public GCodeHandler { +class F83Handler : public GCodeHandler +{ public: - static F83Handler* getInstance(); - int execute(Command*); + static F83Handler *getInstance(); + int execute(Command *); + private: - F83Handler(); - F83Handler(F83Handler const&); - void operator=(F83Handler const&); - long adjustStepAmount(long); - long getNumberOfSteps(double, double); + F83Handler(); + F83Handler(F83Handler const &); + void operator=(F83Handler const &); + long adjustStepAmount(long); + long getNumberOfSteps(double, double); }; #endif /* F83HANDLER_H_ */ - diff --git a/src/G00Handler.cpp b/src/G00Handler.cpp index 30e6395..d6b083e 100644 --- a/src/G00Handler.cpp +++ b/src/G00Handler.cpp @@ -7,51 +7,49 @@ #include "G00Handler.h" - -static G00Handler* instance; - -G00Handler * G00Handler::getInstance() { - if (!instance) { - instance = new G00Handler(); - }; - return instance; -} -; - -G00Handler::G00Handler() { +static G00Handler *instance; + +G00Handler *G00Handler::getInstance() +{ + if (!instance) + { + instance = new G00Handler(); + }; + return instance; +}; + +G00Handler::G00Handler() +{ } -int G00Handler::execute(Command* command) { - - -// Serial.print("G00 was here\r\n"); - - -// Serial.print("R99"); -// Serial.print(" X "); -// Serial.print(command->getX()); -// Serial.print(" Y "); -// Serial.print(command->getY()); -// Serial.print(" Z "); -// Serial.print(command->getZ()); -// Serial.print(" A "); -// Serial.print(command->getA()); -// Serial.print(" B "); -// Serial.print(command->getB()); -// Serial.print(" C "); -// Serial.print(command->getC()); -// Serial.print("\r\n"); - - - StepperControl::getInstance()->moveToCoords - ( - command->getX(), command->getY(), command->getZ(), - command->getA(), command->getB(), command->getC(), - false, false, false - ); - - if (LOGGING) { - CurrentState::getInstance()->print(); - } - return 0; +int G00Handler::execute(Command *command) +{ + + // Serial.print("G00 was here\r\n"); + + // Serial.print("R99"); + // Serial.print(" X "); + // Serial.print(command->getX()); + // Serial.print(" Y "); + // Serial.print(command->getY()); + // Serial.print(" Z "); + // Serial.print(command->getZ()); + // Serial.print(" A "); + // Serial.print(command->getA()); + // Serial.print(" B "); + // Serial.print(command->getB()); + // Serial.print(" C "); + // Serial.print(command->getC()); + // Serial.print("\r\n"); + + StepperControl::getInstance()->moveToCoords( + command->getX(), command->getY(), command->getZ(), + command->getA(), command->getB(), command->getC(), + false, false, false); + + if (LOGGING) + { + CurrentState::getInstance()->print(); + } + return 0; } diff --git a/src/G00Handler.h b/src/G00Handler.h index 72e6d18..a981ab8 100644 --- a/src/G00Handler.h +++ b/src/G00Handler.h @@ -14,16 +14,18 @@ #include "Config.h" #include "StepperControl.h" -class G00Handler : public GCodeHandler { +class G00Handler : public GCodeHandler +{ public: - static G00Handler* getInstance(); - int execute(Command*); + static G00Handler *getInstance(); + int execute(Command *); + private: - G00Handler(); - G00Handler(G00Handler const&); - void operator=(G00Handler const&); - long adjustStepAmount(long); - long getNumberOfSteps(double, double); + G00Handler(); + G00Handler(G00Handler const &); + void operator=(G00Handler const &); + long adjustStepAmount(long); + long getNumberOfSteps(double, double); }; #endif /* G00HANDLER_H_ */ diff --git a/src/G28Handler.cpp b/src/G28Handler.cpp index f8e0953..fe8251b 100644 --- a/src/G28Handler.cpp +++ b/src/G28Handler.cpp @@ -7,29 +7,32 @@ #include "G28Handler.h" - -static G28Handler* instance; - -G28Handler * G28Handler::getInstance() { - if (!instance) { - instance = new G28Handler(); - }; - return instance; -} -; - -G28Handler::G28Handler() { +static G28Handler *instance; + +G28Handler *G28Handler::getInstance() +{ + if (!instance) + { + instance = new G28Handler(); + }; + return instance; +}; + +G28Handler::G28Handler() +{ } -int G28Handler::execute(Command* command) { +int G28Handler::execute(Command *command) +{ -//Serial.print("home\r\n"); + //Serial.print("home\r\n"); - 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(); - } - return 0; + 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(); + } + return 0; } diff --git a/src/G28Handler.h b/src/G28Handler.h index c07a675..831767f 100644 --- a/src/G28Handler.h +++ b/src/G28Handler.h @@ -14,16 +14,18 @@ #include "Config.h" #include "StepperControl.h" -class G28Handler : public GCodeHandler { +class G28Handler : public GCodeHandler +{ public: - static G28Handler* getInstance(); - int execute(Command*); + static G28Handler *getInstance(); + int execute(Command *); + private: - G28Handler(); - G28Handler(G28Handler const&); - void operator=(G28Handler const&); - long adjustStepAmount(long); - long getNumberOfSteps(double, double); + G28Handler(); + G28Handler(G28Handler const &); + void operator=(G28Handler const &); + long adjustStepAmount(long); + long getNumberOfSteps(double, double); }; #endif /* G28HANDLER_H_ */ diff --git a/src/GCodeHandler.cpp b/src/GCodeHandler.cpp index 86c4fcf..884d58c 100644 --- a/src/GCodeHandler.cpp +++ b/src/GCodeHandler.cpp @@ -7,13 +7,15 @@ #include "GCodeHandler.h" -GCodeHandler::GCodeHandler() { - +GCodeHandler::GCodeHandler() +{ } -GCodeHandler::~GCodeHandler() { +GCodeHandler::~GCodeHandler() +{ } -int GCodeHandler::execute(Command*) { - return -1; +int GCodeHandler::execute(Command *) +{ + return -1; } diff --git a/src/GCodeHandler.h b/src/GCodeHandler.h index 1f055e5..e92c6ff 100644 --- a/src/GCodeHandler.h +++ b/src/GCodeHandler.h @@ -9,11 +9,12 @@ #define GCODEHANDLER_H_ #include "Command.h" -class GCodeHandler { +class GCodeHandler +{ public: - GCodeHandler(); - virtual ~GCodeHandler(); - virtual int execute(Command*); + GCodeHandler(); + virtual ~GCodeHandler(); + virtual int execute(Command *); }; #endif /* GCODEHANDLER_H_ */ diff --git a/src/GCodeProcessor.cpp b/src/GCodeProcessor.cpp index c29d9ee..88e7306 100644 --- a/src/GCodeProcessor.cpp +++ b/src/GCodeProcessor.cpp @@ -9,31 +9,33 @@ #include "GCodeProcessor.h" #include "CurrentState.h" -GCodeProcessor::GCodeProcessor() { - +GCodeProcessor::GCodeProcessor() +{ } -GCodeProcessor::~GCodeProcessor() { +GCodeProcessor::~GCodeProcessor() +{ } -void GCodeProcessor::printCommandLog(Command* command) { - Serial.print("command == NULL: "); - Serial.println("\r\n"); +void GCodeProcessor::printCommandLog(Command *command) +{ + Serial.print("command == NULL: "); + Serial.println("\r\n"); } -int GCodeProcessor::execute(Command* command) { - - int execution = 0; +int GCodeProcessor::execute(Command *command) +{ - long Q = command->getQ(); - CurrentState::getInstance()->setQ(Q); + int execution = 0; + long Q = command->getQ(); + CurrentState::getInstance()->setQ(Q); - // Do not execute the command when the config complete parameter is not - // set by the raspberry pi and it's asked to do a move command + // Do not execute the command when the config complete parameter is not + // set by the raspberry pi and it's asked to do a move command - // Tim 2017-04-15 Disable until the raspberry code is ready - /* + // Tim 2017-04-15 Disable until the raspberry code is ready + /* if (ParameterList::getInstance()->getValue(PARAM_CONFIG_OK) != 1) { if ( command->getCodeEnum() == G00 || command->getCodeEnum() == G01 || @@ -51,90 +53,155 @@ int GCodeProcessor::execute(Command* command) { } */ - // Return error when no command or invalid command is found - - if(command == NULL) { - if(LOGGING) { - printCommandLog(command); - } - return -1; - } - - if(command->getCodeEnum() == CODE_UNDEFINED) { - if(LOGGING) { - printCommandLog(command); - } - return -1; - } - - // Get the right handler for this command - - GCodeHandler* handler = getGCodeHandler(command->getCodeEnum()); - - if(handler == NULL) { - Serial.println("R99 handler == NULL\r\n"); - return -1; - } - - // Execute te command, report start and end - - Serial.print(COMM_REPORT_CMD_START); - CurrentState::getInstance()->printQAndNewLine(); - - execution = handler->execute(command); - if(execution == 0) { - Serial.print(COMM_REPORT_CMD_DONE); - CurrentState::getInstance()->printQAndNewLine(); - } else { - Serial.print(COMM_REPORT_CMD_ERROR); - CurrentState::getInstance()->printQAndNewLine(); - } - - CurrentState::getInstance()->resetQ(); - return execution; + // Return error when no command or invalid command is found + + if (command == NULL) + { + if (LOGGING) + { + printCommandLog(command); + } + return -1; + } + + if (command->getCodeEnum() == CODE_UNDEFINED) + { + if (LOGGING) + { + printCommandLog(command); + } + return -1; + } + + // Get the right handler for this command + + GCodeHandler *handler = getGCodeHandler(command->getCodeEnum()); + + if (handler == NULL) + { + Serial.println("R99 handler == NULL\r\n"); + return -1; + } + + // Execute te command, report start and end + + Serial.print(COMM_REPORT_CMD_START); + CurrentState::getInstance()->printQAndNewLine(); + + execution = handler->execute(command); + if (execution == 0) + { + Serial.print(COMM_REPORT_CMD_DONE); + CurrentState::getInstance()->printQAndNewLine(); + } + else + { + Serial.print(COMM_REPORT_CMD_ERROR); + CurrentState::getInstance()->printQAndNewLine(); + } + + CurrentState::getInstance()->resetQ(); + return execution; }; -GCodeHandler* GCodeProcessor::getGCodeHandler(CommandCodeEnum codeEnum) { - - GCodeHandler* handler = NULL; - - // These are if statements so they can be disabled as test - // Usefull when running into memory issues again - - - if (codeEnum == G00) {handler = G00Handler::getInstance();} - - if (codeEnum == G28) {handler = G28Handler::getInstance();} - - if (codeEnum == F11) {handler = F11Handler::getInstance();} - if (codeEnum == F12) {handler = F12Handler::getInstance();} - if (codeEnum == F13) {handler = F13Handler::getInstance();} - - if (codeEnum == F14) {handler = F14Handler::getInstance();} - if (codeEnum == F15) {handler = F15Handler::getInstance();} - if (codeEnum == F16) {handler = F16Handler::getInstance();} - - if (codeEnum == F20) {handler = F20Handler::getInstance();} - if (codeEnum == F21) {handler = F21Handler::getInstance();} - if (codeEnum == F22) {handler = F22Handler::getInstance();} - -// if (codeEnum == F31) {handler = F31Handler::getInstance();} -// if (codeEnum == F32) {handler = F32Handler::getInstance();} - - if (codeEnum == F41) {handler = F41Handler::getInstance();} - if (codeEnum == F42) {handler = F42Handler::getInstance();} - if (codeEnum == F43) {handler = F43Handler::getInstance();} - if (codeEnum == F44) {handler = F44Handler::getInstance();} - - if (codeEnum == F61) {handler = F61Handler::getInstance();} - - if (codeEnum == F81) {handler = F81Handler::getInstance();} - if (codeEnum == F82) {handler = F82Handler::getInstance();} - if (codeEnum == F83) {handler = F83Handler::getInstance();} - if (codeEnum == F84) {handler = F84Handler::getInstance();} - - - return handler; +GCodeHandler *GCodeProcessor::getGCodeHandler(CommandCodeEnum codeEnum) +{ + + GCodeHandler *handler = NULL; + + // These are if statements so they can be disabled as test + // Usefull when running into memory issues again + + if (codeEnum == G00) + { + handler = G00Handler::getInstance(); + } + + if (codeEnum == G28) + { + handler = G28Handler::getInstance(); + } + + if (codeEnum == F11) + { + handler = F11Handler::getInstance(); + } + if (codeEnum == F12) + { + handler = F12Handler::getInstance(); + } + if (codeEnum == F13) + { + handler = F13Handler::getInstance(); + } + + if (codeEnum == F14) + { + handler = F14Handler::getInstance(); + } + if (codeEnum == F15) + { + handler = F15Handler::getInstance(); + } + if (codeEnum == F16) + { + handler = F16Handler::getInstance(); + } + + if (codeEnum == F20) + { + handler = F20Handler::getInstance(); + } + if (codeEnum == F21) + { + handler = F21Handler::getInstance(); + } + if (codeEnum == F22) + { + handler = F22Handler::getInstance(); + } + + // if (codeEnum == F31) {handler = F31Handler::getInstance();} + // if (codeEnum == F32) {handler = F32Handler::getInstance();} + + if (codeEnum == F41) + { + handler = F41Handler::getInstance(); + } + if (codeEnum == F42) + { + handler = F42Handler::getInstance(); + } + if (codeEnum == F43) + { + handler = F43Handler::getInstance(); + } + if (codeEnum == F44) + { + handler = F44Handler::getInstance(); + } + + if (codeEnum == F61) + { + handler = F61Handler::getInstance(); + } + + if (codeEnum == F81) + { + handler = F81Handler::getInstance(); + } + if (codeEnum == F82) + { + handler = F82Handler::getInstance(); + } + if (codeEnum == F83) + { + handler = F83Handler::getInstance(); + } + if (codeEnum == F84) + { + handler = F84Handler::getInstance(); + } + + return handler; } - - diff --git a/src/GCodeProcessor.h b/src/GCodeProcessor.h index bffc312..186d488 100644 --- a/src/GCodeProcessor.h +++ b/src/GCodeProcessor.h @@ -42,15 +42,18 @@ #include "F83Handler.h" #include "F84Handler.h" -class GCodeProcessor { +class GCodeProcessor +{ public: - GCodeProcessor(); - virtual ~GCodeProcessor(); - int execute(Command* command); + GCodeProcessor(); + virtual ~GCodeProcessor(); + int execute(Command *command); + protected: - GCodeHandler* getGCodeHandler(CommandCodeEnum); + GCodeHandler *getGCodeHandler(CommandCodeEnum); + private: - void printCommandLog(Command*); + void printCommandLog(Command *); }; #endif /* GCODEPROCESSOR_H_ */ diff --git a/src/LICENSE b/src/LICENSE index 4f29ca9..9957c44 100644 --- a/src/LICENSE +++ b/src/LICENSE @@ -18,4 +18,4 @@ FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. \ No newline at end of file +SOFTWARE. diff --git a/src/MemoryFree.cpp b/src/MemoryFree.cpp index bf57184..20b45d1 100644 --- a/src/MemoryFree.cpp +++ b/src/MemoryFree.cpp @@ -25,12 +25,12 @@ extern struct __freelist *__flp; /* Calculates the size of the free list */ int freeListSize() { - struct __freelist* current; + struct __freelist *current; int total = 0; for (current = __flp; current; current = current->nx) { total += 2; /* Add two bytes for the memory block's header */ - total += (int) current->sz; + total += (int)current->sz; } return total; diff --git a/src/MemoryFree.h b/src/MemoryFree.h index f9d7cad..b50b3cb 100644 --- a/src/MemoryFree.h +++ b/src/MemoryFree.h @@ -2,7 +2,7 @@ // http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1213583720/15 // Extended by Matthew Murdoch to include walking of the free list. -#ifndef MEMORY_FREE_H +#ifndef MEMORY_FREE_H #define MEMORY_FREE_H #ifdef __cplusplus @@ -11,7 +11,7 @@ extern "C" { int freeMemory(); -#ifdef __cplusplus +#ifdef __cplusplus } #endif diff --git a/src/ParameterList.cpp b/src/ParameterList.cpp index bb8dfd8..6f126f6 100644 --- a/src/ParameterList.cpp +++ b/src/ParameterList.cpp @@ -1,82 +1,97 @@ #include "ParameterList.h" #include -static ParameterList* instanceParam; +static ParameterList *instanceParam; int paramValues[PARAM_NR_OF_PARAMS]; -ParameterList * ParameterList::getInstance() { - if (!instanceParam) { - instanceParam = new ParameterList(); - }; - return instanceParam; +ParameterList *ParameterList::getInstance() +{ + if (!instanceParam) + { + instanceParam = new ParameterList(); + }; + return instanceParam; } -ParameterList::ParameterList() { - // at the first boot, load default parameters and set the parameter version - // so during subsequent boots the values are just loaded from eeprom - // unless the eeprom is disabled with a parameter - - int paramChangeNr = 0; - - int paramVersion = readValueEeprom(0); - if (paramVersion <= 0) { - setAllValuesToDefault(); - writeAllValuesToEeprom(); - } else { - //if (readValueEeprom(PARAM_USE_EEPROM) == 1) { - readAllValuesFromEeprom(); - //} else { - // setAllValuesToDefault(); - //} - } +ParameterList::ParameterList() +{ + // at the first boot, load default parameters and set the parameter version + // so during subsequent boots the values are just loaded from eeprom + // unless the eeprom is disabled with a parameter + + int paramChangeNr = 0; + + int paramVersion = readValueEeprom(0); + if (paramVersion <= 0) + { + setAllValuesToDefault(); + writeAllValuesToEeprom(); + } + else + { + //if (readValueEeprom(PARAM_USE_EEPROM) == 1) { + readAllValuesFromEeprom(); + //} else { + // setAllValuesToDefault(); + //} + } } // ===== Interface functions for the raspberry pi ===== -int ParameterList::readValue(int id) { - - // Check if the value is an existing parameter - if (validParam(id)) { - // Retrieve the value from memory - int value = paramValues[id]; - - // Send to the raspberrt pi - Serial.print("R21"); - Serial.print(" "); - Serial.print("P"); - Serial.print(id); - Serial.print(" "); - Serial.print("V"); - Serial.print(value); - //Serial.print("\r\n"); - CurrentState::getInstance()->printQAndNewLine(); - - } else { - Serial.print("R99 Error: invalid parameter id\r\n"); - } - - return 0; +int ParameterList::readValue(int id) +{ + + // Check if the value is an existing parameter + if (validParam(id)) + { + // Retrieve the value from memory + int value = paramValues[id]; + + // Send to the raspberrt pi + Serial.print("R21"); + Serial.print(" "); + Serial.print("P"); + Serial.print(id); + Serial.print(" "); + Serial.print("V"); + Serial.print(value); + //Serial.print("\r\n"); + CurrentState::getInstance()->printQAndNewLine(); + } + else + { + Serial.print("R99 Error: invalid parameter id\r\n"); + } + + return 0; } -int ParameterList::writeValue(int id, int value) { - - if (paramChangeNr < 9999) { - paramChangeNr++; - } else { - paramChangeNr = 0; - } - - // Check if the value is a valid parameter - if (validParam(id)) { - // Store the value in memory - paramValues[id] = value; - writeValueEeprom(id, value); - } else { - Serial.print("R99 Error: invalid parameter id\r\n"); - } - - -/* +int ParameterList::writeValue(int id, int value) +{ + + if (paramChangeNr < 9999) + { + paramChangeNr++; + } + else + { + paramChangeNr = 0; + } + + // Check if the value is a valid parameter + if (validParam(id)) + { + // Store the value in memory + paramValues[id] = value; + writeValueEeprom(id, value); + } + else + { + Serial.print("R99 Error: invalid parameter id\r\n"); + } + + /* // Debugging output Serial.print("R99"); Serial.print(" "); @@ -93,264 +108,400 @@ int ParameterList::writeValue(int id, int value) { CurrentState::getInstance()->printQAndNewLine(); */ - // If any value is written, - // trigger the loading of the new configuration in all other modules - sendConfigToModules(); + // If any value is written, + // trigger the loading of the new configuration in all other modules + sendConfigToModules(); - return 0; + return 0; } - -void ParameterList::sendConfigToModules() { - // Trigger other modules to load the new values - PinGuard::getInstance()->loadConfig(); +void ParameterList::sendConfigToModules() +{ + // Trigger other modules to load the new values + PinGuard::getInstance()->loadConfig(); } -int ParameterList::readAllValues() { - - - // Make a dump of all values - // Check if it's a valid value to keep the junk out of the list - for (int i=0; i < PARAM_NR_OF_PARAMS; i++) { - if (validParam(i)) { - readValue(i); - } - } +int ParameterList::readAllValues() +{ + + // Make a dump of all values + // Check if it's a valid value to keep the junk out of the list + for (int i = 0; i < PARAM_NR_OF_PARAMS; i++) + { + if (validParam(i)) + { + readValue(i); + } + } } -int ParameterList::getValue(int id) { - return paramValues[id]; +int ParameterList::getValue(int id) +{ + return paramValues[id]; } -int ParameterList::paramChangeNumber() { - return paramChangeNr; +int ParameterList::paramChangeNumber() +{ + return paramChangeNr; } - // ===== eeprom handling ==== -int ParameterList::readValueEeprom(int id) { +int ParameterList::readValueEeprom(int id) +{ - // Assume all values are ints and calculate address for that - int address = id * 2; + // Assume all values are ints and calculate address for that + int address = id * 2; - //Read the 2 bytes from the eeprom memory. - long two = EEPROM.read(address); - long one = EEPROM.read(address + 1); + //Read the 2 bytes from the eeprom memory. + long two = EEPROM.read(address); + long one = EEPROM.read(address + 1); - //Return the recomposed long by using bitshift. - return ((two << 0) & 0xFF) + ((one << 8) & 0xFFFF); + //Return the recomposed long by using bitshift. + return ((two << 0) & 0xFF) + ((one << 8) & 0xFFFF); } -int ParameterList::writeValueEeprom(int id, int value) { +int ParameterList::writeValueEeprom(int id, int value) +{ - // Assume all values are ints and calculate address for that - int address = id * 2; + // Assume all values are ints and calculate address for that + int address = id * 2; - //Decomposition from a int to 2 bytes by using bitshift. - //One = Most significant -> Two = Least significant byte - byte two = (value & 0xFF); - byte one = ((value >> 8) & 0xFF); + //Decomposition from a int to 2 bytes by using bitshift. + //One = Most significant -> Two = Least significant byte + byte two = (value & 0xFF); + byte one = ((value >> 8) & 0xFF); - //Write the 4 bytes into the eeprom memory. - EEPROM.write(address , two); - EEPROM.write(address + 1, one); + //Write the 4 bytes into the eeprom memory. + EEPROM.write(address, two); + EEPROM.write(address + 1, one); - return 0; + return 0; } -int ParameterList::readAllValuesFromEeprom() { - // Write all existing values to eeprom - for (int i=0; i < PARAM_NR_OF_PARAMS; i++) { - if (validParam(i)) { - paramValues[i] = readValueEeprom(i); - if (paramValues[i] == -1) { - // When parameters are still on default, - // load a good value and save it - loadDefaultValue(i); - writeValueEeprom(i,paramValues[i]); - } - } - } +int ParameterList::readAllValuesFromEeprom() +{ + // Write all existing values to eeprom + for (int i = 0; i < PARAM_NR_OF_PARAMS; i++) + { + if (validParam(i)) + { + paramValues[i] = readValueEeprom(i); + if (paramValues[i] == -1) + { + // When parameters are still on default, + // load a good value and save it + loadDefaultValue(i); + writeValueEeprom(i, paramValues[i]); + } + } + } } -int ParameterList::writeAllValuesToEeprom() { - // Write all existing values to eeprom - for (int i=0; i < 150; i++) - { - if (validParam(i)) { - writeValueEeprom(i,paramValues[i]); - } - } +int ParameterList::writeAllValuesToEeprom() +{ + // Write all existing values to eeprom + for (int i = 0; i < 150; i++) + { + if (validParam(i)) + { + writeValueEeprom(i, paramValues[i]); + } + } } // ==== parameter valdation and defaults -int ParameterList::setAllValuesToDefault() { - // Copy default values to the memory values - for (int i=0; i < PARAM_NR_OF_PARAMS; i++) - { - if (validParam(i)) { - loadDefaultValue(i); - } - } +int ParameterList::setAllValuesToDefault() +{ + // Copy default values to the memory values + for (int i = 0; i < PARAM_NR_OF_PARAMS; i++) + { + if (validParam(i)) + { + loadDefaultValue(i); + } + } } -void ParameterList::loadDefaultValue(int id) { - - switch(id) - { - case PARAM_VERSION : paramValues[id] = PARAM_VERSION_DEFAULT; break; - case PARAM_TEST : paramValues[id] = PARAM_TEST_DEFAULT; break; - case PARAM_CONFIG_OK : paramValues[id] = PARAM_CONFIG_OK_DEFAULT; break; - case PARAM_USE_EEPROM : paramValues[id] = PARAM_USE_EEPROM; break; - - case MOVEMENT_TIMEOUT_X : paramValues[id] = MOVEMENT_TIMEOUT_X_DEFAULT; break; - case MOVEMENT_TIMEOUT_Y : paramValues[id] = MOVEMENT_TIMEOUT_Y_DEFAULT; break; - case MOVEMENT_TIMEOUT_Z : paramValues[id] = MOVEMENT_TIMEOUT_Z_DEFAULT; break; - - case MOVEMENT_INVERT_ENDPOINTS_X : paramValues[id] = MOVEMENT_INVERT_ENDPOINTS_X_DEFAULT; break; - case MOVEMENT_INVERT_ENDPOINTS_Y : paramValues[id] = MOVEMENT_INVERT_ENDPOINTS_Y_DEFAULT; break; - case MOVEMENT_INVERT_ENDPOINTS_Z : paramValues[id] = MOVEMENT_INVERT_ENDPOINTS_Z_DEFAULT; break; - - case MOVEMENT_ENABLE_ENDPOINTS_X : paramValues[id] = MOVEMENT_ENABLE_ENDPOINTS_X_DEFAULT; break; - case MOVEMENT_ENABLE_ENDPOINTS_Y : paramValues[id] = MOVEMENT_ENABLE_ENDPOINTS_Y_DEFAULT; break; - case MOVEMENT_ENABLE_ENDPOINTS_Z : paramValues[id] = MOVEMENT_ENABLE_ENDPOINTS_Z_DEFAULT; break; - - case MOVEMENT_INVERT_MOTOR_X : paramValues[id] = MOVEMENT_INVERT_MOTOR_X_DEFAULT; break; - case MOVEMENT_INVERT_MOTOR_Y : paramValues[id] = MOVEMENT_INVERT_MOTOR_Y_DEFAULT; break; - case MOVEMENT_INVERT_MOTOR_Z : paramValues[id] = MOVEMENT_INVERT_MOTOR_Z_DEFAULT; break; - - case MOVEMENT_SECONDARY_MOTOR_X : paramValues[id] = MOVEMENT_SECONDARY_MOTOR_X_DEFAULT; break; - case MOVEMENT_SECONDARY_MOTOR_INVERT_X : paramValues[id] = MOVEMENT_SECONDARY_MOTOR_INVERT_X_DEFAULT; break; - - case MOVEMENT_STEPS_ACC_DEC_X : paramValues[id] = MOVEMENT_STEPS_ACC_DEC_X_DEFAULT; break; - case MOVEMENT_STEPS_ACC_DEC_Y : paramValues[id] = MOVEMENT_STEPS_ACC_DEC_Y_DEFAULT; break; - case MOVEMENT_STEPS_ACC_DEC_Z : paramValues[id] = MOVEMENT_STEPS_ACC_DEC_Z_DEFAULT; break; - - case MOVEMENT_HOME_UP_X : paramValues[id] = MOVEMENT_HOME_UP_X_DEFAULT; break; - case MOVEMENT_HOME_UP_Y : paramValues[id] = MOVEMENT_HOME_UP_Y_DEFAULT; break; - case MOVEMENT_HOME_UP_Z : paramValues[id] = MOVEMENT_HOME_UP_Z_DEFAULT; break; - - case MOVEMENT_MIN_SPD_X : paramValues[id] = MOVEMENT_MIN_SPD_X_DEFAULT; break; - case MOVEMENT_MIN_SPD_Y : paramValues[id] = MOVEMENT_MIN_SPD_Y_DEFAULT; break; - case MOVEMENT_MIN_SPD_Z : paramValues[id] = MOVEMENT_MIN_SPD_Z_DEFAULT; break; - - case MOVEMENT_MAX_SPD_X : paramValues[id] = MOVEMENT_MAX_SPD_X_DEFAULT; break; - case MOVEMENT_MAX_SPD_Y : paramValues[id] = MOVEMENT_MAX_SPD_Y_DEFAULT; break; - case MOVEMENT_MAX_SPD_Z : paramValues[id] = MOVEMENT_MAX_SPD_Z_DEFAULT; break; - - case ENCODER_ENABLED_X : paramValues[id] = ENCODER_ENABLED_X_DEFAULT; break; - case ENCODER_ENABLED_Y : paramValues[id] = ENCODER_ENABLED_Y_DEFAULT; break; - case ENCODER_ENABLED_Z : paramValues[id] = ENCODER_ENABLED_Z_DEFAULT; break; - - case ENCODER_TYPE_X : paramValues[id] = ENCODER_TYPE_X_DEFAULT; break; - case ENCODER_TYPE_Y : paramValues[id] = ENCODER_TYPE_Y_DEFAULT; break; - case ENCODER_TYPE_Z : paramValues[id] = ENCODER_TYPE_Z_DEFAULT; break; - - case ENCODER_MISSED_STEPS_MAX_X : paramValues[id] = ENCODER_MISSED_STEPS_MAX_X_DEFAULT; break; - case ENCODER_MISSED_STEPS_MAX_Y : paramValues[id] = ENCODER_MISSED_STEPS_MAX_Y_DEFAULT; break; - case ENCODER_MISSED_STEPS_MAX_Z : paramValues[id] = ENCODER_MISSED_STEPS_MAX_Z_DEFAULT; break; - - case ENCODER_SCALING_X : paramValues[id] = ENCODER_SCALING_X_DEFAULT; break; - case ENCODER_SCALING_Y : paramValues[id] = ENCODER_SCALING_Y_DEFAULT; break; - case ENCODER_SCALING_Z : paramValues[id] = ENCODER_SCALING_Z_DEFAULT; break; - - case ENCODER_MISSED_STEPS_DECAY_X : paramValues[id] = ENCODER_MISSED_STEPS_DECAY_X_DEFAULT; break; - case ENCODER_MISSED_STEPS_DECAY_Y : paramValues[id] = ENCODER_MISSED_STEPS_DECAY_Y_DEFAULT; break; - case ENCODER_MISSED_STEPS_DECAY_Z : paramValues[id] = ENCODER_MISSED_STEPS_DECAY_Z_DEFAULT; break; - - case PIN_GUARD_1_PIN_NR : paramValues[id] = PIN_GUARD_1_PIN_NR_DEFAULT; break; - case PIN_GUARD_1_TIME_OUT : paramValues[id] = PIN_GUARD_1_TIME_OUT_DEFAULT; break; - case PIN_GUARD_1_ACTIVE_STATE : paramValues[id] = PIN_GUARD_1_ACTIVE_STATE_DEFAULT; break; - - case PIN_GUARD_2_PIN_NR : paramValues[id] = PIN_GUARD_2_PIN_NR_DEFAULT; break; - case PIN_GUARD_2_TIME_OUT : paramValues[id] = PIN_GUARD_2_TIME_OUT_DEFAULT; break; - case PIN_GUARD_2_ACTIVE_STATE : paramValues[id] = PIN_GUARD_2_ACTIVE_STATE_DEFAULT; break; - - case PIN_GUARD_3_PIN_NR : paramValues[id] = PIN_GUARD_3_PIN_NR_DEFAULT; break; - case PIN_GUARD_3_TIME_OUT : paramValues[id] = PIN_GUARD_3_TIME_OUT_DEFAULT; break; - case PIN_GUARD_3_ACTIVE_STATE : paramValues[id] = PIN_GUARD_3_ACTIVE_STATE_DEFAULT; break; - - case PIN_GUARD_4_PIN_NR : paramValues[id] = PIN_GUARD_4_PIN_NR_DEFAULT; break; - case PIN_GUARD_4_TIME_OUT : paramValues[id] = PIN_GUARD_4_TIME_OUT_DEFAULT; break; - case PIN_GUARD_4_ACTIVE_STATE : paramValues[id] = PIN_GUARD_4_ACTIVE_STATE_DEFAULT; break; - - case PIN_GUARD_5_PIN_NR : paramValues[id] = PIN_GUARD_5_PIN_NR_DEFAULT; break; - case PIN_GUARD_5_TIME_OUT : paramValues[id] = PIN_GUARD_5_TIME_OUT_DEFAULT; break; - case PIN_GUARD_5_ACTIVE_STATE : paramValues[id] = PIN_GUARD_5_ACTIVE_STATE_DEFAULT; break; - - default : paramValues[id] = 0; break; - } +void ParameterList::loadDefaultValue(int id) +{ + + switch (id) + { + case PARAM_VERSION: + paramValues[id] = PARAM_VERSION_DEFAULT; + break; + case PARAM_TEST: + paramValues[id] = PARAM_TEST_DEFAULT; + break; + case PARAM_CONFIG_OK: + paramValues[id] = PARAM_CONFIG_OK_DEFAULT; + break; + case PARAM_USE_EEPROM: + paramValues[id] = PARAM_USE_EEPROM; + break; + + case MOVEMENT_TIMEOUT_X: + paramValues[id] = MOVEMENT_TIMEOUT_X_DEFAULT; + break; + case MOVEMENT_TIMEOUT_Y: + paramValues[id] = MOVEMENT_TIMEOUT_Y_DEFAULT; + break; + case MOVEMENT_TIMEOUT_Z: + paramValues[id] = MOVEMENT_TIMEOUT_Z_DEFAULT; + break; + + case MOVEMENT_INVERT_ENDPOINTS_X: + paramValues[id] = MOVEMENT_INVERT_ENDPOINTS_X_DEFAULT; + break; + case MOVEMENT_INVERT_ENDPOINTS_Y: + paramValues[id] = MOVEMENT_INVERT_ENDPOINTS_Y_DEFAULT; + break; + case MOVEMENT_INVERT_ENDPOINTS_Z: + paramValues[id] = MOVEMENT_INVERT_ENDPOINTS_Z_DEFAULT; + break; + + case MOVEMENT_ENABLE_ENDPOINTS_X: + paramValues[id] = MOVEMENT_ENABLE_ENDPOINTS_X_DEFAULT; + break; + case MOVEMENT_ENABLE_ENDPOINTS_Y: + paramValues[id] = MOVEMENT_ENABLE_ENDPOINTS_Y_DEFAULT; + break; + case MOVEMENT_ENABLE_ENDPOINTS_Z: + paramValues[id] = MOVEMENT_ENABLE_ENDPOINTS_Z_DEFAULT; + break; + + case MOVEMENT_INVERT_MOTOR_X: + paramValues[id] = MOVEMENT_INVERT_MOTOR_X_DEFAULT; + break; + case MOVEMENT_INVERT_MOTOR_Y: + paramValues[id] = MOVEMENT_INVERT_MOTOR_Y_DEFAULT; + break; + case MOVEMENT_INVERT_MOTOR_Z: + paramValues[id] = MOVEMENT_INVERT_MOTOR_Z_DEFAULT; + break; + + case MOVEMENT_SECONDARY_MOTOR_X: + paramValues[id] = MOVEMENT_SECONDARY_MOTOR_X_DEFAULT; + break; + case MOVEMENT_SECONDARY_MOTOR_INVERT_X: + paramValues[id] = MOVEMENT_SECONDARY_MOTOR_INVERT_X_DEFAULT; + break; + + case MOVEMENT_STEPS_ACC_DEC_X: + paramValues[id] = MOVEMENT_STEPS_ACC_DEC_X_DEFAULT; + break; + case MOVEMENT_STEPS_ACC_DEC_Y: + paramValues[id] = MOVEMENT_STEPS_ACC_DEC_Y_DEFAULT; + break; + case MOVEMENT_STEPS_ACC_DEC_Z: + paramValues[id] = MOVEMENT_STEPS_ACC_DEC_Z_DEFAULT; + break; + + case MOVEMENT_HOME_UP_X: + paramValues[id] = MOVEMENT_HOME_UP_X_DEFAULT; + break; + case MOVEMENT_HOME_UP_Y: + paramValues[id] = MOVEMENT_HOME_UP_Y_DEFAULT; + break; + case MOVEMENT_HOME_UP_Z: + paramValues[id] = MOVEMENT_HOME_UP_Z_DEFAULT; + break; + + case MOVEMENT_MIN_SPD_X: + paramValues[id] = MOVEMENT_MIN_SPD_X_DEFAULT; + break; + case MOVEMENT_MIN_SPD_Y: + paramValues[id] = MOVEMENT_MIN_SPD_Y_DEFAULT; + break; + case MOVEMENT_MIN_SPD_Z: + paramValues[id] = MOVEMENT_MIN_SPD_Z_DEFAULT; + break; + + case MOVEMENT_MAX_SPD_X: + paramValues[id] = MOVEMENT_MAX_SPD_X_DEFAULT; + break; + case MOVEMENT_MAX_SPD_Y: + paramValues[id] = MOVEMENT_MAX_SPD_Y_DEFAULT; + break; + case MOVEMENT_MAX_SPD_Z: + paramValues[id] = MOVEMENT_MAX_SPD_Z_DEFAULT; + break; + + case ENCODER_ENABLED_X: + paramValues[id] = ENCODER_ENABLED_X_DEFAULT; + break; + case ENCODER_ENABLED_Y: + paramValues[id] = ENCODER_ENABLED_Y_DEFAULT; + break; + case ENCODER_ENABLED_Z: + paramValues[id] = ENCODER_ENABLED_Z_DEFAULT; + break; + + case ENCODER_TYPE_X: + paramValues[id] = ENCODER_TYPE_X_DEFAULT; + break; + case ENCODER_TYPE_Y: + paramValues[id] = ENCODER_TYPE_Y_DEFAULT; + break; + case ENCODER_TYPE_Z: + paramValues[id] = ENCODER_TYPE_Z_DEFAULT; + break; + + case ENCODER_MISSED_STEPS_MAX_X: + paramValues[id] = ENCODER_MISSED_STEPS_MAX_X_DEFAULT; + break; + case ENCODER_MISSED_STEPS_MAX_Y: + paramValues[id] = ENCODER_MISSED_STEPS_MAX_Y_DEFAULT; + break; + case ENCODER_MISSED_STEPS_MAX_Z: + paramValues[id] = ENCODER_MISSED_STEPS_MAX_Z_DEFAULT; + break; + + case ENCODER_SCALING_X: + paramValues[id] = ENCODER_SCALING_X_DEFAULT; + break; + case ENCODER_SCALING_Y: + paramValues[id] = ENCODER_SCALING_Y_DEFAULT; + break; + case ENCODER_SCALING_Z: + paramValues[id] = ENCODER_SCALING_Z_DEFAULT; + break; + + case ENCODER_MISSED_STEPS_DECAY_X: + paramValues[id] = ENCODER_MISSED_STEPS_DECAY_X_DEFAULT; + break; + case ENCODER_MISSED_STEPS_DECAY_Y: + paramValues[id] = ENCODER_MISSED_STEPS_DECAY_Y_DEFAULT; + break; + case ENCODER_MISSED_STEPS_DECAY_Z: + paramValues[id] = ENCODER_MISSED_STEPS_DECAY_Z_DEFAULT; + break; + + case PIN_GUARD_1_PIN_NR: + paramValues[id] = PIN_GUARD_1_PIN_NR_DEFAULT; + break; + case PIN_GUARD_1_TIME_OUT: + paramValues[id] = PIN_GUARD_1_TIME_OUT_DEFAULT; + break; + case PIN_GUARD_1_ACTIVE_STATE: + paramValues[id] = PIN_GUARD_1_ACTIVE_STATE_DEFAULT; + break; + + case PIN_GUARD_2_PIN_NR: + paramValues[id] = PIN_GUARD_2_PIN_NR_DEFAULT; + break; + case PIN_GUARD_2_TIME_OUT: + paramValues[id] = PIN_GUARD_2_TIME_OUT_DEFAULT; + break; + case PIN_GUARD_2_ACTIVE_STATE: + paramValues[id] = PIN_GUARD_2_ACTIVE_STATE_DEFAULT; + break; + + case PIN_GUARD_3_PIN_NR: + paramValues[id] = PIN_GUARD_3_PIN_NR_DEFAULT; + break; + case PIN_GUARD_3_TIME_OUT: + paramValues[id] = PIN_GUARD_3_TIME_OUT_DEFAULT; + break; + case PIN_GUARD_3_ACTIVE_STATE: + paramValues[id] = PIN_GUARD_3_ACTIVE_STATE_DEFAULT; + break; + + case PIN_GUARD_4_PIN_NR: + paramValues[id] = PIN_GUARD_4_PIN_NR_DEFAULT; + break; + case PIN_GUARD_4_TIME_OUT: + paramValues[id] = PIN_GUARD_4_TIME_OUT_DEFAULT; + break; + case PIN_GUARD_4_ACTIVE_STATE: + paramValues[id] = PIN_GUARD_4_ACTIVE_STATE_DEFAULT; + break; + + case PIN_GUARD_5_PIN_NR: + paramValues[id] = PIN_GUARD_5_PIN_NR_DEFAULT; + break; + case PIN_GUARD_5_TIME_OUT: + paramValues[id] = PIN_GUARD_5_TIME_OUT_DEFAULT; + break; + case PIN_GUARD_5_ACTIVE_STATE: + paramValues[id] = PIN_GUARD_5_ACTIVE_STATE_DEFAULT; + break; + + default: + paramValues[id] = 0; + break; + } } -bool ParameterList::validParam(int id) { - - // Check if the id is a valid one - switch(id) - { - case PARAM_VERSION: - case PARAM_CONFIG_OK: - case PARAM_USE_EEPROM: - case MOVEMENT_TIMEOUT_X: - case MOVEMENT_TIMEOUT_Y: - case MOVEMENT_TIMEOUT_Z: - case MOVEMENT_ENABLE_ENDPOINTS_X: - case MOVEMENT_ENABLE_ENDPOINTS_Y: - case MOVEMENT_ENABLE_ENDPOINTS_Z: - case MOVEMENT_INVERT_ENDPOINTS_X: - case MOVEMENT_INVERT_ENDPOINTS_Y: - case MOVEMENT_INVERT_ENDPOINTS_Z: - case MOVEMENT_INVERT_MOTOR_X: - case MOVEMENT_INVERT_MOTOR_Y: - case MOVEMENT_INVERT_MOTOR_Z: - case MOVEMENT_SECONDARY_MOTOR_X: - case MOVEMENT_SECONDARY_MOTOR_INVERT_X: - case MOVEMENT_STEPS_ACC_DEC_X: - case MOVEMENT_STEPS_ACC_DEC_Y: - case MOVEMENT_STEPS_ACC_DEC_Z: - case MOVEMENT_HOME_UP_X: - case MOVEMENT_HOME_UP_Y: - case MOVEMENT_HOME_UP_Z: - case MOVEMENT_MIN_SPD_X: - case MOVEMENT_MIN_SPD_Y: - case MOVEMENT_MIN_SPD_Z: - case MOVEMENT_MAX_SPD_X: - case MOVEMENT_MAX_SPD_Y: - case MOVEMENT_MAX_SPD_Z: - case ENCODER_ENABLED_X: - case ENCODER_ENABLED_Y: - case ENCODER_ENABLED_Z: - case ENCODER_TYPE_X: - case ENCODER_TYPE_Y: - case ENCODER_TYPE_Z: - case ENCODER_MISSED_STEPS_MAX_X: - case ENCODER_MISSED_STEPS_MAX_Y: - case ENCODER_MISSED_STEPS_MAX_Z: - case ENCODER_SCALING_X: - case ENCODER_SCALING_Y: - case ENCODER_SCALING_Z: - case ENCODER_MISSED_STEPS_DECAY_X: - case ENCODER_MISSED_STEPS_DECAY_Y: - case ENCODER_MISSED_STEPS_DECAY_Z: - case PIN_GUARD_1_PIN_NR: - case PIN_GUARD_1_TIME_OUT: - case PIN_GUARD_1_ACTIVE_STATE: - case PIN_GUARD_2_PIN_NR: - case PIN_GUARD_2_TIME_OUT: - case PIN_GUARD_2_ACTIVE_STATE: - case PIN_GUARD_3_PIN_NR: - case PIN_GUARD_3_TIME_OUT: - case PIN_GUARD_3_ACTIVE_STATE: - case PIN_GUARD_4_PIN_NR: - case PIN_GUARD_4_TIME_OUT: - case PIN_GUARD_4_ACTIVE_STATE: - case PIN_GUARD_5_PIN_NR: - case PIN_GUARD_5_TIME_OUT: - case PIN_GUARD_5_ACTIVE_STATE: - return true; - default: - return false; - } +bool ParameterList::validParam(int id) +{ + + // Check if the id is a valid one + switch (id) + { + case PARAM_VERSION: + case PARAM_CONFIG_OK: + case PARAM_USE_EEPROM: + case MOVEMENT_TIMEOUT_X: + case MOVEMENT_TIMEOUT_Y: + case MOVEMENT_TIMEOUT_Z: + case MOVEMENT_ENABLE_ENDPOINTS_X: + case MOVEMENT_ENABLE_ENDPOINTS_Y: + case MOVEMENT_ENABLE_ENDPOINTS_Z: + case MOVEMENT_INVERT_ENDPOINTS_X: + case MOVEMENT_INVERT_ENDPOINTS_Y: + case MOVEMENT_INVERT_ENDPOINTS_Z: + case MOVEMENT_INVERT_MOTOR_X: + case MOVEMENT_INVERT_MOTOR_Y: + case MOVEMENT_INVERT_MOTOR_Z: + case MOVEMENT_SECONDARY_MOTOR_X: + case MOVEMENT_SECONDARY_MOTOR_INVERT_X: + case MOVEMENT_STEPS_ACC_DEC_X: + case MOVEMENT_STEPS_ACC_DEC_Y: + case MOVEMENT_STEPS_ACC_DEC_Z: + case MOVEMENT_HOME_UP_X: + case MOVEMENT_HOME_UP_Y: + case MOVEMENT_HOME_UP_Z: + case MOVEMENT_MIN_SPD_X: + case MOVEMENT_MIN_SPD_Y: + case MOVEMENT_MIN_SPD_Z: + case MOVEMENT_MAX_SPD_X: + case MOVEMENT_MAX_SPD_Y: + case MOVEMENT_MAX_SPD_Z: + case ENCODER_ENABLED_X: + case ENCODER_ENABLED_Y: + case ENCODER_ENABLED_Z: + case ENCODER_TYPE_X: + case ENCODER_TYPE_Y: + case ENCODER_TYPE_Z: + case ENCODER_MISSED_STEPS_MAX_X: + case ENCODER_MISSED_STEPS_MAX_Y: + case ENCODER_MISSED_STEPS_MAX_Z: + case ENCODER_SCALING_X: + case ENCODER_SCALING_Y: + case ENCODER_SCALING_Z: + case ENCODER_MISSED_STEPS_DECAY_X: + case ENCODER_MISSED_STEPS_DECAY_Y: + case ENCODER_MISSED_STEPS_DECAY_Z: + case PIN_GUARD_1_PIN_NR: + case PIN_GUARD_1_TIME_OUT: + case PIN_GUARD_1_ACTIVE_STATE: + case PIN_GUARD_2_PIN_NR: + case PIN_GUARD_2_TIME_OUT: + case PIN_GUARD_2_ACTIVE_STATE: + case PIN_GUARD_3_PIN_NR: + case PIN_GUARD_3_TIME_OUT: + case PIN_GUARD_3_ACTIVE_STATE: + case PIN_GUARD_4_PIN_NR: + case PIN_GUARD_4_TIME_OUT: + case PIN_GUARD_4_ACTIVE_STATE: + case PIN_GUARD_5_PIN_NR: + case PIN_GUARD_5_TIME_OUT: + case PIN_GUARD_5_ACTIVE_STATE: + return true; + default: + return false; + } } - diff --git a/src/ParameterList.h b/src/ParameterList.h index e459265..900522f 100644 --- a/src/ParameterList.h +++ b/src/ParameterList.h @@ -9,98 +9,96 @@ //#define NULL 0 const int PARAM_NR_OF_PARAMS = 225; - enum ParamListEnum { - PARAM_VERSION = 0, - PARAM_TEST = 1, - PARAM_CONFIG_OK = 2, - PARAM_USE_EEPROM = 3, - - // stepper motor settings + PARAM_VERSION = 0, + PARAM_TEST = 1, + PARAM_CONFIG_OK = 2, + PARAM_USE_EEPROM = 3, - MOVEMENT_TIMEOUT_X = 11, - MOVEMENT_TIMEOUT_Y = 12, - MOVEMENT_TIMEOUT_Z = 13, + // stepper motor settings - MOVEMENT_INVERT_ENDPOINTS_X = 21, - MOVEMENT_INVERT_ENDPOINTS_Y = 22, - MOVEMENT_INVERT_ENDPOINTS_Z = 23, + MOVEMENT_TIMEOUT_X = 11, + MOVEMENT_TIMEOUT_Y = 12, + MOVEMENT_TIMEOUT_Z = 13, - MOVEMENT_ENABLE_ENDPOINTS_X = 25, - MOVEMENT_ENABLE_ENDPOINTS_Y = 26, - MOVEMENT_ENABLE_ENDPOINTS_Z = 27, + MOVEMENT_INVERT_ENDPOINTS_X = 21, + MOVEMENT_INVERT_ENDPOINTS_Y = 22, + MOVEMENT_INVERT_ENDPOINTS_Z = 23, - MOVEMENT_INVERT_MOTOR_X = 31, - MOVEMENT_INVERT_MOTOR_Y = 32, - MOVEMENT_INVERT_MOTOR_Z = 33, + MOVEMENT_ENABLE_ENDPOINTS_X = 25, + MOVEMENT_ENABLE_ENDPOINTS_Y = 26, + MOVEMENT_ENABLE_ENDPOINTS_Z = 27, - MOVEMENT_SECONDARY_MOTOR_X = 36, - MOVEMENT_SECONDARY_MOTOR_INVERT_X = 37, + MOVEMENT_INVERT_MOTOR_X = 31, + MOVEMENT_INVERT_MOTOR_Y = 32, + MOVEMENT_INVERT_MOTOR_Z = 33, - MOVEMENT_STEPS_ACC_DEC_X = 41, - MOVEMENT_STEPS_ACC_DEC_Y = 42, - MOVEMENT_STEPS_ACC_DEC_Z = 43, + MOVEMENT_SECONDARY_MOTOR_X = 36, + MOVEMENT_SECONDARY_MOTOR_INVERT_X = 37, - MOVEMENT_HOME_UP_X = 51, - MOVEMENT_HOME_UP_Y = 52, - MOVEMENT_HOME_UP_Z = 53, + MOVEMENT_STEPS_ACC_DEC_X = 41, + MOVEMENT_STEPS_ACC_DEC_Y = 42, + MOVEMENT_STEPS_ACC_DEC_Z = 43, - MOVEMENT_MIN_SPD_X = 61, - MOVEMENT_MIN_SPD_Y = 62, - MOVEMENT_MIN_SPD_Z = 63, + MOVEMENT_HOME_UP_X = 51, + MOVEMENT_HOME_UP_Y = 52, + MOVEMENT_HOME_UP_Z = 53, - MOVEMENT_MAX_SPD_X = 71, - MOVEMENT_MAX_SPD_Y = 72, - MOVEMENT_MAX_SPD_Z = 73, + MOVEMENT_MIN_SPD_X = 61, + MOVEMENT_MIN_SPD_Y = 62, + MOVEMENT_MIN_SPD_Z = 63, - // encoder settings - ENCODER_ENABLED_X = 101, - ENCODER_ENABLED_Y = 102, - ENCODER_ENABLED_Z = 103, + MOVEMENT_MAX_SPD_X = 71, + MOVEMENT_MAX_SPD_Y = 72, + MOVEMENT_MAX_SPD_Z = 73, - ENCODER_TYPE_X = 105, - ENCODER_TYPE_Y = 106, - ENCODER_TYPE_Z = 107, + // encoder settings + ENCODER_ENABLED_X = 101, + ENCODER_ENABLED_Y = 102, + ENCODER_ENABLED_Z = 103, - ENCODER_MISSED_STEPS_MAX_X = 111, - ENCODER_MISSED_STEPS_MAX_Y = 112, - ENCODER_MISSED_STEPS_MAX_Z = 113, + ENCODER_TYPE_X = 105, + ENCODER_TYPE_Y = 106, + ENCODER_TYPE_Z = 107, - ENCODER_SCALING_X = 115, - ENCODER_SCALING_Y = 116, - ENCODER_SCALING_Z = 117, + ENCODER_MISSED_STEPS_MAX_X = 111, + ENCODER_MISSED_STEPS_MAX_Y = 112, + ENCODER_MISSED_STEPS_MAX_Z = 113, - ENCODER_MISSED_STEPS_DECAY_X = 121, - ENCODER_MISSED_STEPS_DECAY_Y = 122, - ENCODER_MISSED_STEPS_DECAY_Z = 123, + ENCODER_SCALING_X = 115, + ENCODER_SCALING_Y = 116, + ENCODER_SCALING_Z = 117, - // not used in software at this time - MOVEMENT_AXIS_NR_STEPS_X = 141, - MOVEMENT_AXIS_NR_STEPS_Y = 142, - MOVEMENT_AXIS_NR_STEPS_Z = 143, + ENCODER_MISSED_STEPS_DECAY_X = 121, + ENCODER_MISSED_STEPS_DECAY_Y = 122, + ENCODER_MISSED_STEPS_DECAY_Z = 123, + // not used in software at this time + MOVEMENT_AXIS_NR_STEPS_X = 141, + MOVEMENT_AXIS_NR_STEPS_Y = 142, + MOVEMENT_AXIS_NR_STEPS_Z = 143, - // pin guard settings - PIN_GUARD_1_PIN_NR = 201, - PIN_GUARD_1_TIME_OUT = 202, - PIN_GUARD_1_ACTIVE_STATE = 203, + // pin guard settings + PIN_GUARD_1_PIN_NR = 201, + PIN_GUARD_1_TIME_OUT = 202, + PIN_GUARD_1_ACTIVE_STATE = 203, - PIN_GUARD_2_PIN_NR = 205, - PIN_GUARD_2_TIME_OUT = 206, - PIN_GUARD_2_ACTIVE_STATE = 207, + PIN_GUARD_2_PIN_NR = 205, + PIN_GUARD_2_TIME_OUT = 206, + PIN_GUARD_2_ACTIVE_STATE = 207, - PIN_GUARD_3_PIN_NR = 211, - PIN_GUARD_3_TIME_OUT = 212, - PIN_GUARD_3_ACTIVE_STATE = 213, + PIN_GUARD_3_PIN_NR = 211, + PIN_GUARD_3_TIME_OUT = 212, + PIN_GUARD_3_ACTIVE_STATE = 213, - PIN_GUARD_4_PIN_NR = 215, - PIN_GUARD_4_TIME_OUT = 216, - PIN_GUARD_4_ACTIVE_STATE = 217, + PIN_GUARD_4_PIN_NR = 215, + PIN_GUARD_4_TIME_OUT = 216, + PIN_GUARD_4_ACTIVE_STATE = 217, - PIN_GUARD_5_PIN_NR = 221, - PIN_GUARD_5_TIME_OUT = 222, - PIN_GUARD_5_ACTIVE_STATE = 223 + PIN_GUARD_5_PIN_NR = 221, + PIN_GUARD_5_TIME_OUT = 222, + PIN_GUARD_5_ACTIVE_STATE = 223 }; @@ -108,37 +106,37 @@ enum ParamListEnum #define NULL 0 */ -class ParameterList { - ParamListEnum paramListEnum; +class ParameterList +{ + ParamListEnum paramListEnum; + public: - static ParameterList* getInstance(); - int writeValue(int id, int value); - int readValue(int id); - int getValue(int id); + static ParameterList *getInstance(); + int writeValue(int id, int value); + int readValue(int id); + int getValue(int id); - bool validParam(int id); - void loadDefaultValue(int id); + bool validParam(int id); + void loadDefaultValue(int id); - int readAllValues(); - int readAllValuesFromEeprom(); - int writeAllValuesToEeprom(); - int setAllValuesToDefault(); + int readAllValues(); + int readAllValuesFromEeprom(); + int writeAllValuesToEeprom(); + int setAllValuesToDefault(); - int readValueEeprom(int id); - int writeValueEeprom(int id, int value); + int readValueEeprom(int id); + int writeValueEeprom(int id, int value); - void sendConfigToModules(); + void sendConfigToModules(); - int paramChangeNumber(); + int paramChangeNumber(); private: - ParameterList(); - ParameterList(ParameterList const&); - void operator=(ParameterList const&); - - int paramChangeNr; + ParameterList(); + ParameterList(ParameterList const &); + void operator=(ParameterList const &); + int paramChangeNr; }; - #endif /* PARAMETERLIST_H_ */ diff --git a/src/PinControl.cpp b/src/PinControl.cpp index 795ef8c..5e4b6d5 100644 --- a/src/PinControl.cpp +++ b/src/PinControl.cpp @@ -1,71 +1,84 @@ #include "PinControl.h" -static PinControl* instance; +static PinControl *instance; -PinControl * PinControl::getInstance() { - if (!instance) { - instance = new PinControl(); - }; - return instance; -} -; +PinControl *PinControl::getInstance() +{ + if (!instance) + { + instance = new PinControl(); + }; + return instance; +}; -PinControl::PinControl() { +PinControl::PinControl() +{ } -int PinControl::setMode(int pinNr, int mode) { - pinMode(pinNr , mode ); - return 0; +int PinControl::setMode(int pinNr, int mode) +{ + pinMode(pinNr, mode); + return 0; } -int PinControl::writeValue(int pinNr, int value, int mode) { - if (mode == 0) { - digitalWrite(pinNr, value); - return 0; - } - if (mode == 1) { - analogWrite(pinNr, value); - return 0; - } - return 1; +int PinControl::writeValue(int pinNr, int value, int mode) +{ + if (mode == 0) + { + digitalWrite(pinNr, value); + return 0; + } + if (mode == 1) + { + analogWrite(pinNr, value); + return 0; + } + return 1; } -int PinControl::readValue(int pinNr, int mode) { +int PinControl::readValue(int pinNr, int mode) +{ - int value = 0; + int value = 0; - if (mode == 0) { - if (digitalRead(pinNr) == 1){ - value = 1; - } - } - if (mode == 1) { - value = analogRead(pinNr); - } + if (mode == 0) + { + if (digitalRead(pinNr) == 1) + { + value = 1; + } + } + if (mode == 1) + { + value = analogRead(pinNr); + } - if (mode == 0 || mode == 1) { + if (mode == 0 || mode == 1) + { - Serial.print("R41"); - Serial.print(" "); - Serial.print("P"); - Serial.print(pinNr); - Serial.print(" "); - Serial.print("V"); - Serial.print(value); - //Serial.print("\r\n"); - CurrentState::getInstance()->printQAndNewLine(); + Serial.print("R41"); + Serial.print(" "); + Serial.print("P"); + Serial.print(pinNr); + Serial.print(" "); + Serial.print("V"); + Serial.print(value); + //Serial.print("\r\n"); + CurrentState::getInstance()->printQAndNewLine(); - return 0; - } - else { - return 1; - } + return 0; + } + else + { + return 1; + } } -int PinControl::writePulse(int pinNr, int valueOne, int valueTwo, long time, int mode) { - writeValue( pinNr, valueOne, mode); - delay(time); - writeValue( pinNr, valueTwo, mode); - return 0; +int PinControl::writePulse(int pinNr, int valueOne, int valueTwo, long time, int mode) +{ + writeValue(pinNr, valueOne, mode); + delay(time); + writeValue(pinNr, valueTwo, mode); + return 0; } diff --git a/src/PinControl.h b/src/PinControl.h index a20d998..d782942 100644 --- a/src/PinControl.h +++ b/src/PinControl.h @@ -16,19 +16,20 @@ #include #include "CurrentState.h" -class PinControl { +class PinControl +{ public: - static PinControl* getInstance(); + static PinControl *getInstance(); - int setMode(int pinNr, int mode); - int writeValue(int pinNr, int value, int mode); - int readValue(int pinNr, int mode); - int writePulse(int pinNr, int valueOne, int valueTwo, long time, int mode); + int setMode(int pinNr, int mode); + int writeValue(int pinNr, int value, int mode); + int readValue(int pinNr, int mode); + int writePulse(int pinNr, int valueOne, int valueTwo, long time, int mode); private: - PinControl(); - PinControl(PinControl const&); - void operator=(PinControl const&); + PinControl(); + PinControl(PinControl const &); + void operator=(PinControl const &); }; #endif /* PINCONTROL_H_ */ diff --git a/src/PinGuard.cpp b/src/PinGuard.cpp index 556941f..6d6078b 100644 --- a/src/PinGuard.cpp +++ b/src/PinGuard.cpp @@ -1,39 +1,42 @@ #include "PinGuard.h" -static PinGuard* instance; - -PinGuard * PinGuard::getInstance() { - if (!instance) { - instance = new PinGuard(); - }; - return instance; -} -; - -PinGuard::PinGuard() { - - pinGuardPin[0] = PinGuardPin(); - pinGuardPin[1] = PinGuardPin(); - pinGuardPin[2] = PinGuardPin(); - pinGuardPin[3] = PinGuardPin(); - pinGuardPin[4] = PinGuardPin(); - loadConfig(); - +static PinGuard *instance; + +PinGuard *PinGuard::getInstance() +{ + if (!instance) + { + instance = new PinGuard(); + }; + return instance; +}; + +PinGuard::PinGuard() +{ + + pinGuardPin[0] = PinGuardPin(); + pinGuardPin[1] = PinGuardPin(); + pinGuardPin[2] = PinGuardPin(); + pinGuardPin[3] = PinGuardPin(); + pinGuardPin[4] = PinGuardPin(); + loadConfig(); } -void PinGuard::loadConfig() { - pinGuardPin[0].loadPinConfig(PIN_GUARD_1_PIN_NR, PIN_GUARD_1_ACTIVE_STATE, PIN_GUARD_1_TIME_OUT); - pinGuardPin[1].loadPinConfig(PIN_GUARD_2_PIN_NR, PIN_GUARD_2_ACTIVE_STATE, PIN_GUARD_2_TIME_OUT); - pinGuardPin[2].loadPinConfig(PIN_GUARD_3_PIN_NR, PIN_GUARD_3_ACTIVE_STATE, PIN_GUARD_3_TIME_OUT); - pinGuardPin[3].loadPinConfig(PIN_GUARD_4_PIN_NR, PIN_GUARD_4_ACTIVE_STATE, PIN_GUARD_4_TIME_OUT); - pinGuardPin[4].loadPinConfig(PIN_GUARD_5_PIN_NR, PIN_GUARD_5_ACTIVE_STATE, PIN_GUARD_5_TIME_OUT); +void PinGuard::loadConfig() +{ + pinGuardPin[0].loadPinConfig(PIN_GUARD_1_PIN_NR, PIN_GUARD_1_ACTIVE_STATE, PIN_GUARD_1_TIME_OUT); + pinGuardPin[1].loadPinConfig(PIN_GUARD_2_PIN_NR, PIN_GUARD_2_ACTIVE_STATE, PIN_GUARD_2_TIME_OUT); + pinGuardPin[2].loadPinConfig(PIN_GUARD_3_PIN_NR, PIN_GUARD_3_ACTIVE_STATE, PIN_GUARD_3_TIME_OUT); + pinGuardPin[3].loadPinConfig(PIN_GUARD_4_PIN_NR, PIN_GUARD_4_ACTIVE_STATE, PIN_GUARD_4_TIME_OUT); + pinGuardPin[4].loadPinConfig(PIN_GUARD_5_PIN_NR, PIN_GUARD_5_ACTIVE_STATE, PIN_GUARD_5_TIME_OUT); } -void PinGuard::checkPins() { - pinGuardPin[0].processTick(); - pinGuardPin[1].processTick(); - pinGuardPin[2].processTick(); - pinGuardPin[3].processTick(); - pinGuardPin[4].processTick(); +void PinGuard::checkPins() +{ + pinGuardPin[0].processTick(); + pinGuardPin[1].processTick(); + pinGuardPin[2].processTick(); + pinGuardPin[3].processTick(); + pinGuardPin[4].processTick(); } diff --git a/src/PinGuard.h b/src/PinGuard.h index 029f3dd..90f7e18 100644 --- a/src/PinGuard.h +++ b/src/PinGuard.h @@ -17,27 +17,26 @@ #include "PinGuardPin.h" #include "ParameterList.h" -class PinGuard { +class PinGuard +{ public: - static PinGuard* getInstance(); + static PinGuard *getInstance(); - void loadConfig(); - void checkPins(); + void loadConfig(); + void checkPins(); private: + //long pinTimeOut[100]; + //long pinCurrentTime[100]; + //void checkPin; + //bool pinSafeState[100]; + PinGuardPin pinGuardPin[5]; + //PinGuardPin test; - //long pinTimeOut[100]; - //long pinCurrentTime[100]; - //void checkPin; - //bool pinSafeState[100]; - - PinGuardPin pinGuardPin[5]; - //PinGuardPin test; - - PinGuard(); - PinGuard(PinGuard const&); - void operator=(PinGuard const&); + PinGuard(); + PinGuard(PinGuard const &); + void operator=(PinGuard const &); }; #endif /* PINGUARD_H_ */ diff --git a/src/PinGuardPin.cpp b/src/PinGuardPin.cpp index 6ff3dc8..bebbd8b 100644 --- a/src/PinGuardPin.cpp +++ b/src/PinGuardPin.cpp @@ -2,38 +2,44 @@ #include "PinGuardPin.h" #include "ParameterList.h" -PinGuardPin::PinGuardPin() { - pinNr = 0; +PinGuardPin::PinGuardPin() +{ + pinNr = 0; } // Set the initial settings for what pin to check -void PinGuardPin::loadPinConfig(int pinNrParamNr, int activeStateParamNr, int timeOutParamNr) { +void PinGuardPin::loadPinConfig(int pinNrParamNr, int activeStateParamNr, int timeOutParamNr) +{ - pinNr = ParameterList::getInstance()->getValue(pinNrParamNr); - activeState = (ParameterList::getInstance()->getValue(activeStateParamNr)== 1); - timeOut = ParameterList::getInstance()->getValue(timeOutParamNr); + pinNr = ParameterList::getInstance()->getValue(pinNrParamNr); + activeState = (ParameterList::getInstance()->getValue(activeStateParamNr) == 1); + timeOut = ParameterList::getInstance()->getValue(timeOutParamNr); - timeOutCount = 0; + timeOutCount = 0; } // Check each second if the time out is lapsed or the value has changed -void PinGuardPin::processTick() { - - if (pinNr==0) { - return; - } - - currentStatePin = digitalRead(pinNr); - - if (currentStatePin != activeState) { - timeOutCount = 0; - } else { - timeOutCount++; - } - - if (timeOutCount >= timeOut) { - digitalWrite(pinNr, !activeState); - } +void PinGuardPin::processTick() +{ + + if (pinNr == 0) + { + return; + } + + currentStatePin = digitalRead(pinNr); + + if (currentStatePin != activeState) + { + timeOutCount = 0; + } + else + { + timeOutCount++; + } + + if (timeOutCount >= timeOut) + { + digitalWrite(pinNr, !activeState); + } } - - diff --git a/src/PinGuardPin.h b/src/PinGuardPin.h index 83e8d3b..a412d97 100644 --- a/src/PinGuardPin.h +++ b/src/PinGuardPin.h @@ -16,23 +16,23 @@ #include //#include "ParameterList.h" -class PinGuardPin { +class PinGuardPin +{ public: + PinGuardPin(); - PinGuardPin(); + void processTick(); + void loadPinConfig(int pinNrParamNr, int activeStateParamNr, int timeOutParamNr); - void processTick(); - void loadPinConfig(int pinNrParamNr, int activeStateParamNr, int timeOutParamNr); private: - //PinControlPin(PinControlPin const&); - ///void operator=(PinControlPin const&); - - int pinNr; - long timeOut; - long timeOutCount; - bool activeState; - bool currentStatePin; - + //PinControlPin(PinControlPin const&); + ///void operator=(PinControlPin const&); + + int pinNr; + long timeOut; + long timeOutCount; + bool activeState; + bool currentStatePin; }; #endif /* PINGUARDPIN_H_ */ diff --git a/src/ServoControl.cpp b/src/ServoControl.cpp index d6a4de3..9cf110b 100644 --- a/src/ServoControl.cpp +++ b/src/ServoControl.cpp @@ -7,29 +7,33 @@ Servo pin layout D11 D6 D5 D4 */ -static ServoControl* instance; +static ServoControl *instance; Servo servos[2]; -ServoControl * ServoControl::getInstance() { - if (!instance) { - instance = new ServoControl(); - }; - return instance; -} -; +ServoControl *ServoControl::getInstance() +{ + if (!instance) + { + instance = new ServoControl(); + }; + return instance; +}; -ServoControl::ServoControl() { +ServoControl::ServoControl() +{ } -int ServoControl::attach() { - servos[0].attach(SERVO_0_PIN); - servos[1].attach(SERVO_1_PIN); +int ServoControl::attach() +{ + servos[0].attach(SERVO_0_PIN); + servos[1].attach(SERVO_1_PIN); } -int ServoControl::setAngle(int pin, int angle) { +int ServoControl::setAngle(int pin, int angle) +{ -/* + /* Serial.print("R99"); Serial.print(" "); Serial.print("SERVO"); @@ -45,19 +49,17 @@ int ServoControl::setAngle(int pin, int angle) { Serial.print("\r\n"); */ - switch(pin) { - case 4: - servos[0].write(angle); - break; - case 5: - servos[1].write(angle); - break; - default: - return 1; - } - - return 0; -} - - + switch (pin) + { + case 4: + servos[0].write(angle); + break; + case 5: + servos[1].write(angle); + break; + default: + return 1; + } + return 0; +} diff --git a/src/ServoControl.h b/src/ServoControl.h index 202a427..4ae529c 100644 --- a/src/ServoControl.h +++ b/src/ServoControl.h @@ -14,16 +14,18 @@ #include #include -class ServoControl { +class ServoControl +{ public: - static ServoControl* getInstance(); + static ServoControl *getInstance(); + + int attach(); + int setAngle(int pin, int angle); - int attach(); - int setAngle(int pin, int angle); private: - ServoControl(); - ServoControl(ServoControl const&); - void operator=(ServoControl const&); + ServoControl(); + ServoControl(ServoControl const &); + void operator=(ServoControl const &); }; #endif /* SERVOCONTROL_H_ */ diff --git a/src/StatusList.cpp b/src/StatusList.cpp index 4c24be8..b8182f6 100644 --- a/src/StatusList.cpp +++ b/src/StatusList.cpp @@ -1,50 +1,49 @@ #include "StatusList.h" -static StatusList* instanceParam; +static StatusList *instanceParam; long statusValues[150]; -StatusList * StatusList::getInstance() { - if (!instanceParam) { - instanceParam = new StatusList(); - }; - return instanceParam; +StatusList *StatusList::getInstance() +{ + if (!instanceParam) + { + instanceParam = new StatusList(); + }; + return instanceParam; } -StatusList::StatusList() { +StatusList::StatusList() +{ + statusValues[STATUS_GENERAL] = STATUS_GENERAL_DEFAULT; - statusValues[STATUS_GENERAL] = STATUS_GENERAL_DEFAULT; - - //paramValues[MOVEMENT_MAX_SPD_X] = MOVEMENT_MAX_SPD_X_DEFAULT; - //paramValues[MOVEMENT_MAX_SPD_Y] = MOVEMENT_MAX_SPD_Y_DEFAULT; - //paramValues[MOVEMENT_MAX_SPD_Z] = MOVEMENT_MAX_SPD_Z_DEFAULT; - + //paramValues[MOVEMENT_MAX_SPD_X] = MOVEMENT_MAX_SPD_X_DEFAULT; + //paramValues[MOVEMENT_MAX_SPD_Y] = MOVEMENT_MAX_SPD_Y_DEFAULT; + //paramValues[MOVEMENT_MAX_SPD_Z] = MOVEMENT_MAX_SPD_Z_DEFAULT; } +int StatusList::readValue(int id) +{ -int StatusList::readValue(int id) { - - long value = statusValues[id]; - - - Serial.print("R31"); - Serial.print(" "); - Serial.print("P"); - Serial.print(id); - Serial.print(" "); - Serial.print("V"); - Serial.print(value); - //Serial.print("\r\n"); - CurrentState::getInstance()->printQAndNewLine(); + long value = statusValues[id]; + Serial.print("R31"); + Serial.print(" "); + Serial.print("P"); + Serial.print(id); + Serial.print(" "); + Serial.print("V"); + Serial.print(value); + //Serial.print("\r\n"); + CurrentState::getInstance()->printQAndNewLine(); - return 0; + return 0; } +long StatusList::getValue(int id) +{ -long StatusList::getValue(int id) { - -/* + /* Serial.print("R99"); Serial.print(" "); Serial.print("getValue"); @@ -55,13 +54,13 @@ long StatusList::getValue(int id) { Serial.print("\r\n"); */ - return statusValues[id]; + return statusValues[id]; } -int StatusList::setValue(int id, long value) { +int StatusList::setValue(int id, long value) +{ - statusValues[id] = value; + statusValues[id] = value; - return 0; + return 0; } - diff --git a/src/StatusList.h b/src/StatusList.h index 99f8519..88206dc 100644 --- a/src/StatusList.h +++ b/src/StatusList.h @@ -7,14 +7,13 @@ //#define NULL 0 - enum StatusListEnum { - STATUS_GENERAL = 0, + STATUS_GENERAL = 0, - //MOVEMENT_MAX_SPD_X = 71, - //MOVEMENT_MAX_SPD_Y = 72, - //MOVEMENT_MAX_SPD_Z = 73 + //MOVEMENT_MAX_SPD_X = 71, + //MOVEMENT_MAX_SPD_Y = 72, + //MOVEMENT_MAX_SPD_Z = 73 }; @@ -22,19 +21,21 @@ enum StatusListEnum #define NULL 0 */ -class StatusList { - StatusListEnum statusListEnum; +class StatusList +{ + StatusListEnum statusListEnum; + public: - static StatusList* getInstance(); - int writeValue(int id, long value); - int readValue(int id); - long getValue(int id); - int setValue(int id, long value); + static StatusList *getInstance(); + int writeValue(int id, long value); + int readValue(int id); + long getValue(int id); + int setValue(int id, long value); + private: - StatusList(); - StatusList(StatusList const&); - void operator=(StatusList const&); + StatusList(); + StatusList(StatusList const &); + void operator=(StatusList const &); }; - #endif /* STATUSLIST_H_ */ diff --git a/src/StepperControl.cpp b/src/StepperControl.cpp index ba958ca..2911050 100644 --- a/src/StepperControl.cpp +++ b/src/StepperControl.cpp @@ -1,169 +1,181 @@ #include "StepperControl.h" -static StepperControl* instance; - -StepperControl * StepperControl::getInstance() { - if (!instance) { - instance = new StepperControl(); - }; - return instance; -} -; - -void StepperControl::reportStatus(StepperControlAxis* axis, int axisStatus) { - Serial.print(COMM_REPORT_CMD_STATUS); - Serial.print(" "); - Serial.print(axis->label); - Serial.print(axisStatus); - //Serial.print("\r\n"); - CurrentState::getInstance()->printQAndNewLine(); +static StepperControl *instance; + +StepperControl *StepperControl::getInstance() +{ + if (!instance) + { + instance = new StepperControl(); + }; + return instance; +}; + +void StepperControl::reportStatus(StepperControlAxis *axis, int axisStatus) +{ + Serial.print(COMM_REPORT_CMD_STATUS); + Serial.print(" "); + Serial.print(axis->label); + Serial.print(axisStatus); + //Serial.print("\r\n"); + CurrentState::getInstance()->printQAndNewLine(); } -void StepperControl::reportCalib(StepperControlAxis* axis, int calibStatus) { - Serial.print(COMM_REPORT_CALIB_STATUS); - Serial.print(" "); - Serial.print(axis->label); - Serial.print(calibStatus); - //Serial.print("\r\n"); - CurrentState::getInstance()->printQAndNewLine(); +void StepperControl::reportCalib(StepperControlAxis *axis, int calibStatus) +{ + Serial.print(COMM_REPORT_CALIB_STATUS); + Serial.print(" "); + Serial.print(axis->label); + Serial.print(calibStatus); + //Serial.print("\r\n"); + CurrentState::getInstance()->printQAndNewLine(); } -void StepperControl::checkAxisSubStatus(StepperControlAxis* axis, int* axisSubStatus) { - int newStatus = 0; - bool statusChanged = false; - - if (axis->isAccelerating()) { - newStatus = COMM_REPORT_MOVE_STATUS_ACCELERATING; - } - - if (axis->isCruising()) { - newStatus = COMM_REPORT_MOVE_STATUS_CRUISING; - } - - if (axis->isDecelerating()) { - newStatus = COMM_REPORT_MOVE_STATUS_DECELERATING; - } - - if (axis->isCrawling()) { - newStatus = COMM_REPORT_MOVE_STATUS_CRAWLING; - } - - // if the status changes, send out a status report - if (*axisSubStatus != newStatus && newStatus > 0) { - statusChanged = true; - } - *axisSubStatus = newStatus; - - if (statusChanged) { - reportStatus(&axisX, *axisSubStatus); - } +void StepperControl::checkAxisSubStatus(StepperControlAxis *axis, int *axisSubStatus) +{ + int newStatus = 0; + bool statusChanged = false; + + if (axis->isAccelerating()) + { + newStatus = COMM_REPORT_MOVE_STATUS_ACCELERATING; + } + + if (axis->isCruising()) + { + newStatus = COMM_REPORT_MOVE_STATUS_CRUISING; + } + + if (axis->isDecelerating()) + { + newStatus = COMM_REPORT_MOVE_STATUS_DECELERATING; + } + + if (axis->isCrawling()) + { + newStatus = COMM_REPORT_MOVE_STATUS_CRAWLING; + } + + // if the status changes, send out a status report + if (*axisSubStatus != newStatus && newStatus > 0) + { + statusChanged = true; + } + *axisSubStatus = newStatus; + + if (statusChanged) + { + reportStatus(&axisX, *axisSubStatus); + } } //const int MOVEMENT_INTERRUPT_SPEED = 100; // Interrupt cycle in micro seconds -StepperControl::StepperControl() { +StepperControl::StepperControl() +{ - // Initialize some variables for testing + // Initialize some variables for testing - motorMotorsEnabled = false; + motorMotorsEnabled = false; - motorConsMissedSteps[0] = 0; - motorConsMissedSteps[1] = 0; - motorConsMissedSteps[2] = 0; + motorConsMissedSteps[0] = 0; + motorConsMissedSteps[1] = 0; + motorConsMissedSteps[2] = 0; - // Create the axis controllers + // Create the axis controllers - axisX = StepperControlAxis(); - axisY = StepperControlAxis(); - axisZ = StepperControlAxis(); + axisX = StepperControlAxis(); + axisY = StepperControlAxis(); + axisZ = StepperControlAxis(); - axisX.label = 'X'; - axisY.label = 'Y'; - axisZ.label = 'Z'; + axisX.label = 'X'; + axisY.label = 'Y'; + axisZ.label = 'Z'; - // Create the encoder controller + // Create the encoder controller - encoderX = StepperControlEncoder(); - encoderY = StepperControlEncoder(); - encoderZ = StepperControlEncoder(); + encoderX = StepperControlEncoder(); + encoderY = StepperControlEncoder(); + encoderZ = StepperControlEncoder(); - // Load settings + // Load settings - loadSettings(); + loadSettings(); - - motorMotorsEnabled = false; + motorMotorsEnabled = false; } -void StepperControl::loadSettings() { - - // Load motor settings +void StepperControl::loadSettings() +{ - axisX.loadPinNumbers(X_STEP_PIN, X_DIR_PIN, X_ENABLE_PIN, X_MIN_PIN, X_MAX_PIN, E_STEP_PIN, E_DIR_PIN, E_ENABLE_PIN); - axisY.loadPinNumbers(Y_STEP_PIN, Y_DIR_PIN, Y_ENABLE_PIN, Y_MIN_PIN, Y_MAX_PIN, 0, 0, 0); - axisZ.loadPinNumbers(Z_STEP_PIN, Z_DIR_PIN, Z_ENABLE_PIN, Z_MIN_PIN, Z_MAX_PIN, 0, 0, 0); + // Load motor settings - axisSubStep[0] = COMM_REPORT_MOVE_STATUS_IDLE; - axisSubStep[1] = COMM_REPORT_MOVE_STATUS_IDLE; - axisSubStep[2] = COMM_REPORT_MOVE_STATUS_IDLE; + axisX.loadPinNumbers(X_STEP_PIN, X_DIR_PIN, X_ENABLE_PIN, X_MIN_PIN, X_MAX_PIN, E_STEP_PIN, E_DIR_PIN, E_ENABLE_PIN); + axisY.loadPinNumbers(Y_STEP_PIN, Y_DIR_PIN, Y_ENABLE_PIN, Y_MIN_PIN, Y_MAX_PIN, 0, 0, 0); + axisZ.loadPinNumbers(Z_STEP_PIN, Z_DIR_PIN, Z_ENABLE_PIN, Z_MIN_PIN, Z_MAX_PIN, 0, 0, 0); - loadMotorSettings(); + axisSubStep[0] = COMM_REPORT_MOVE_STATUS_IDLE; + axisSubStep[1] = COMM_REPORT_MOVE_STATUS_IDLE; + axisSubStep[2] = COMM_REPORT_MOVE_STATUS_IDLE; - // Load encoder settings + loadMotorSettings(); - loadEncoderSettings(); + // Load encoder settings - encoderX.loadPinNumbers(X_ENCDR_A, X_ENCDR_B, X_ENCDR_A_Q, X_ENCDR_B_Q); - encoderY.loadPinNumbers(Y_ENCDR_A, Y_ENCDR_B, Y_ENCDR_A_Q, Y_ENCDR_B_Q); - encoderZ.loadPinNumbers(Z_ENCDR_A, Z_ENCDR_B, Z_ENCDR_A_Q, Z_ENCDR_B_Q); + loadEncoderSettings(); - encoderX.loadSettings( motorConsEncoderType[0], motorConsEncoderScaling[0]); - encoderY.loadSettings( motorConsEncoderType[1], motorConsEncoderScaling[1]); - encoderZ.loadSettings( motorConsEncoderType[2], motorConsEncoderScaling[2]); + encoderX.loadPinNumbers(X_ENCDR_A, X_ENCDR_B, X_ENCDR_A_Q, X_ENCDR_B_Q); + encoderY.loadPinNumbers(Y_ENCDR_A, Y_ENCDR_B, Y_ENCDR_A_Q, Y_ENCDR_B_Q); + encoderZ.loadPinNumbers(Z_ENCDR_A, Z_ENCDR_B, Z_ENCDR_A_Q, Z_ENCDR_B_Q); + encoderX.loadSettings(motorConsEncoderType[0], motorConsEncoderScaling[0]); + encoderY.loadSettings(motorConsEncoderType[1], motorConsEncoderScaling[1]); + encoderZ.loadSettings(motorConsEncoderType[2], motorConsEncoderScaling[2]); } -void StepperControl::test() { - - Serial.print("R99"); - Serial.print(" mot x = "); - Serial.print(axisX.currentPosition()); - Serial.print(" enc x = "); - Serial.print(encoderX.currentPosition()); - Serial.print("\r\n"); - - Serial.print("R99"); - Serial.print(" mot y = "); - Serial.print(axisY.currentPosition()); - Serial.print(" enc y = "); - Serial.print(encoderY.currentPosition()); - Serial.print("\r\n"); - - Serial.print("R99"); - Serial.print(" mot z = "); - Serial.print(axisZ.currentPosition()); - Serial.print(" enc z = "); - Serial.print(encoderZ.currentPosition()); - Serial.print("\r\n"); - - // read changes in encoder - //encoderX.readEncoder(); - //encoderY.readEncoder(); - //encoderZ.readEncoder(); - //reportPosition(); - - //bool test = axisX.endStopMin(); - //Serial.print("R99"); - //Serial.print(" test = "); - //Serial.print(test); - //Serial.print("\r\n"); +void StepperControl::test() +{ + + Serial.print("R99"); + Serial.print(" mot x = "); + Serial.print(axisX.currentPosition()); + Serial.print(" enc x = "); + Serial.print(encoderX.currentPosition()); + Serial.print("\r\n"); + + Serial.print("R99"); + Serial.print(" mot y = "); + Serial.print(axisY.currentPosition()); + Serial.print(" enc y = "); + Serial.print(encoderY.currentPosition()); + Serial.print("\r\n"); + + Serial.print("R99"); + Serial.print(" mot z = "); + Serial.print(axisZ.currentPosition()); + Serial.print(" enc z = "); + Serial.print(encoderZ.currentPosition()); + Serial.print("\r\n"); + + // read changes in encoder + //encoderX.readEncoder(); + //encoderY.readEncoder(); + //encoderZ.readEncoder(); + //reportPosition(); + + //bool test = axisX.endStopMin(); + //Serial.print("R99"); + //Serial.print(" test = "); + //Serial.print(test); + //Serial.print("\r\n"); } -void StepperControl::test2() { - CurrentState::getInstance()->printPosition(); - encoderX.test(); - //encoderY.test(); - //encoderZ.test(); +void StepperControl::test2() +{ + CurrentState::getInstance()->printPosition(); + encoderX.test(); + //encoderY.test(); + //encoderZ.test(); } /** @@ -173,253 +185,269 @@ void StepperControl::test2() { * maxStepsPerSecond - maximum number of steps per second * maxAccelerationStepsPerSecond - maximum number of acceleration in steps per second */ -int StepperControl::moveToCoords( long xDest, long yDest, long zDest, - unsigned int xMaxSpd, unsigned int yMaxSpd, unsigned int zMaxSpd, - bool xHome, bool yHome, bool zHome - ) { - - unsigned long currentMillis = 0; - unsigned long timeStart = millis(); - - int incomingByte = 0; - int error = 0; - - // load motor and encoder settings - - loadMotorSettings(); - loadEncoderSettings(); - // load current encoder coordinates - //axisX.setCurrentPosition(encoderX.currentPosition()); - - - // 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.setMaxSpeed(speedMax[0]); - axisY.setMaxSpeed(speedMax[1]); - axisZ.setMaxSpeed(speedMax[2]); - - // Load coordinates into axis class - - 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; - - motorConsMissedSteps[0] = 0; - motorConsMissedSteps[1] = 0; - motorConsMissedSteps[2] = 0; - - motorLastPosition[0] = currentPoint[0]; - motorLastPosition[1] = currentPoint[1]; - motorLastPosition[2] = currentPoint[2]; - - // Load coordinates into motor control - - 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.movementStarted = false; - axisY.movementStarted = false; - axisZ.movementStarted = false; - - storeEndStops(); - reportEndStops(); - - axisX.setDirectionAxis(); - axisY.setDirectionAxis(); - axisZ.setDirectionAxis(); - - // Enable motors - - axisSubStep[0] = COMM_REPORT_MOVE_STATUS_START_MOTOR; - axisSubStep[1] = COMM_REPORT_MOVE_STATUS_START_MOTOR; - axisSubStep[2] = COMM_REPORT_MOVE_STATUS_START_MOTOR; - - reportStatus(&axisX, axisSubStep[0]); - reportStatus(&axisY, axisSubStep[1]); - reportStatus(&axisZ, axisSubStep[2]); - - enableMotors(); - - // Start movement +int StepperControl::moveToCoords(long xDest, long yDest, long zDest, + unsigned int xMaxSpd, unsigned int yMaxSpd, unsigned int zMaxSpd, + bool xHome, bool yHome, bool zHome) +{ - axisActive[0] = true; - axisActive[1] = true; - axisActive[2] = true; + unsigned long currentMillis = 0; + unsigned long timeStart = millis(); - axisX.checkMovement(); - axisY.checkMovement(); - axisZ.checkMovement(); + int incomingByte = 0; + int error = 0; - // Let the interrupt handle all the movements - while (axisActive[0] || axisActive[1] || axisActive[2]) { + // load motor and encoder settings - checkAxisSubStatus(&axisX, &axisSubStep[0]); - checkAxisSubStatus(&axisY, &axisSubStep[1]); - checkAxisSubStatus(&axisZ, &axisSubStep[2]); + loadMotorSettings(); + loadEncoderSettings(); + // load current encoder coordinates + //axisX.setCurrentPosition(encoderX.currentPosition()); - delay(1); - //delayMicroseconds(100); + // if a speed is given in the command, use that instead of the config speed - //encoderX.currentPosition(); - //encoderY.currentPosition(); - //encoderZ.currentPosition(); + if (xMaxSpd > 0 && xMaxSpd < speedMax[0]) + { + speedMax[0] = xMaxSpd; + } - //axisX.checkTiming(); - //axisY.checkTiming(); - //axisZ.checkTiming(); + if (yMaxSpd > 0 && yMaxSpd < speedMax[1]) + { + speedMax[1] = yMaxSpd; + } - //checkAxisVsEncoder(&axisX, &encoderX, &motorConsMissedSteps[0], &motorLastPosition[0], &motorConsMissedStepsDecay[0], &motorConsEncoderEnabled[0]); - //checkAxisVsEncoder(&axisY, &encoderY, &motorConsMissedSteps[1], &motorLastPosition[1], &motorConsMissedStepsDecay[1], &motorConsEncoderEnabled[1]); - //checkAxisVsEncoder(&axisZ, &encoderZ, &motorConsMissedSteps[2], &motorLastPosition[2], &motorConsMissedStepsDecay[2], &motorConsEncoderEnabled[2]); + if (zMaxSpd > 0 && zMaxSpd < speedMax[2]) + { + speedMax[2] = zMaxSpd; + } - if (motorConsMissedSteps[0] > motorConsMissedStepsMax[0]) { - axisX.deactivateAxis(); - Serial.print("R99"); - Serial.print(" deactivate motor X due to missed steps"); - Serial.print("\r\n"); + axisX.setMaxSpeed(speedMax[0]); + axisY.setMaxSpeed(speedMax[1]); + axisZ.setMaxSpeed(speedMax[2]); - if (axisX.movingToHome()) { - encoderX.setPosition(0); - axisX.setCurrentPosition(0); - Serial.print("R99"); - Serial.print(" home position X axis detected with encoder"); - Serial.print("\r\n"); - } + // Load coordinates into axis class - } + long sourcePoint[3] = {0, 0, 0}; + sourcePoint[0] = CurrentState::getInstance()->getX(); + sourcePoint[1] = CurrentState::getInstance()->getY(); + sourcePoint[2] = CurrentState::getInstance()->getZ(); - if (motorConsMissedSteps[1] > motorConsMissedStepsMax[1]) { - axisY.deactivateAxis(); - Serial.print("R99"); - Serial.print(" deactivate motor Y due to missed steps"); - Serial.print("\r\n"); + long currentPoint[3] = {0, 0, 0}; + currentPoint[0] = CurrentState::getInstance()->getX(); + currentPoint[1] = CurrentState::getInstance()->getY(); + currentPoint[2] = CurrentState::getInstance()->getZ(); - if (axisY.movingToHome()) { - encoderY.setPosition(0); - axisY.setCurrentPosition(0); - Serial.print("R99"); - Serial.print(" home position Y axis detected with encoder"); - Serial.print("\r\n"); - } - } - - if (motorConsMissedSteps[2] > motorConsMissedStepsMax[2]) { - axisZ.deactivateAxis(); - Serial.print("R99"); - Serial.print(" deactivate motor Z due to missed steps"); - Serial.print("\r\n"); - - if (axisZ.movingToHome()) { - encoderZ.setPosition(0); - axisZ.setCurrentPosition(0); - Serial.print("R99"); - Serial.print(" home position Z axis detected with encoder"); - Serial.print("\r\n"); - } - } - - if (axisX.endStopAxisReached(false)) { - axisX.setCurrentPosition(0); - encoderX.setPosition(0); - } - - if (axisY.endStopAxisReached(false)) { - axisY.setCurrentPosition(0); - encoderY.setPosition(0); - } - - if (axisZ.endStopAxisReached(false)) { - axisZ.setCurrentPosition(0); - encoderZ.setPosition(0); - } - - - axisActive[0] = axisX.isAxisActive(); - axisActive[1] = axisY.isAxisActive(); - axisActive[2] = axisZ.isAxisActive(); - - currentPoint[0] = axisX.currentPosition(); - currentPoint[1] = axisY.currentPosition(); - currentPoint[2] = axisZ.currentPosition(); - - CurrentState::getInstance()->setX(currentPoint[0]); - CurrentState::getInstance()->setY(currentPoint[1]); - CurrentState::getInstance()->setZ(currentPoint[2]); - - storeEndStops(); - - // Check timeouts - if (axisActive[0] == true && ((millis() >= timeStart && millis() - timeStart > timeOut[0] * 1000) || (millis() < timeStart && millis() > timeOut[0] * 1000))) { - Serial.print("R99 timeout X axis\r\n"); - error = 1; - } - if (axisActive[1] == true && ((millis() >= timeStart && millis() - timeStart > timeOut[0] * 1000) || (millis() < timeStart && millis() > timeOut[0] * 1000))) { - Serial.print("R99 timeout Y axis\r\n"); - error = 1; - } - if (axisActive[2] == true && ((millis() >= timeStart && millis() - timeStart > timeOut[0] * 1000) || (millis() < timeStart && millis() > timeOut[0] * 1000))) { - Serial.print("R99 timeout Z axis\r\n"); - error = 1; - } - - // Check if there is an emergency stop command - if (Serial.available() > 0) { - incomingByte = Serial.read(); - if (incomingByte == 69 || incomingByte == 101) { - Serial.print("R99 emergency stop\r\n"); - axisX.deactivateAxis(); - axisY.deactivateAxis(); - axisZ.deactivateAxis(); - error = 1; - } - } - - if (error == 1) { - Serial.print("R99 error\r\n"); - - axisActive[0] = false; - axisActive[1] = false; - axisActive[2] = false; - } - - // Periodically send message still active - currentMillis++; - - //if (currentMillis % 2500 == 0) - if (currentMillis % 750 == 0) - //if (1 == 1) - { - Serial.print(COMM_REPORT_CMD_BUSY); - //Serial.print("\r\n"); - CurrentState::getInstance()->printQAndNewLine(); - reportPosition(); - /* + long destinationPoint[3] = {0, 0, 0}; + destinationPoint[0] = xDest; + destinationPoint[1] = yDest; + destinationPoint[2] = zDest; + + motorConsMissedSteps[0] = 0; + motorConsMissedSteps[1] = 0; + motorConsMissedSteps[2] = 0; + + motorLastPosition[0] = currentPoint[0]; + motorLastPosition[1] = currentPoint[1]; + motorLastPosition[2] = currentPoint[2]; + + // Load coordinates into motor control + + 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.movementStarted = false; + axisY.movementStarted = false; + axisZ.movementStarted = false; + + storeEndStops(); + reportEndStops(); + + axisX.setDirectionAxis(); + axisY.setDirectionAxis(); + axisZ.setDirectionAxis(); + + // Enable motors + + axisSubStep[0] = COMM_REPORT_MOVE_STATUS_START_MOTOR; + axisSubStep[1] = COMM_REPORT_MOVE_STATUS_START_MOTOR; + axisSubStep[2] = COMM_REPORT_MOVE_STATUS_START_MOTOR; + + reportStatus(&axisX, axisSubStep[0]); + reportStatus(&axisY, axisSubStep[1]); + reportStatus(&axisZ, axisSubStep[2]); + + enableMotors(); + + // Start movement + + axisActive[0] = true; + axisActive[1] = true; + axisActive[2] = true; + + axisX.checkMovement(); + axisY.checkMovement(); + axisZ.checkMovement(); + + // Let the interrupt handle all the movements + while (axisActive[0] || axisActive[1] || axisActive[2]) + { + + checkAxisSubStatus(&axisX, &axisSubStep[0]); + checkAxisSubStatus(&axisY, &axisSubStep[1]); + checkAxisSubStatus(&axisZ, &axisSubStep[2]); + + delay(1); + //delayMicroseconds(100); + + //encoderX.currentPosition(); + //encoderY.currentPosition(); + //encoderZ.currentPosition(); + + //axisX.checkTiming(); + //axisY.checkTiming(); + //axisZ.checkTiming(); + + //checkAxisVsEncoder(&axisX, &encoderX, &motorConsMissedSteps[0], &motorLastPosition[0], &motorConsMissedStepsDecay[0], &motorConsEncoderEnabled[0]); + //checkAxisVsEncoder(&axisY, &encoderY, &motorConsMissedSteps[1], &motorLastPosition[1], &motorConsMissedStepsDecay[1], &motorConsEncoderEnabled[1]); + //checkAxisVsEncoder(&axisZ, &encoderZ, &motorConsMissedSteps[2], &motorLastPosition[2], &motorConsMissedStepsDecay[2], &motorConsEncoderEnabled[2]); + + if (motorConsMissedSteps[0] > motorConsMissedStepsMax[0]) + { + axisX.deactivateAxis(); + Serial.print("R99"); + Serial.print(" deactivate motor X due to missed steps"); + Serial.print("\r\n"); + + if (axisX.movingToHome()) + { + encoderX.setPosition(0); + axisX.setCurrentPosition(0); + Serial.print("R99"); + Serial.print(" home position X axis detected with encoder"); + Serial.print("\r\n"); + } + } + + if (motorConsMissedSteps[1] > motorConsMissedStepsMax[1]) + { + axisY.deactivateAxis(); + Serial.print("R99"); + Serial.print(" deactivate motor Y due to missed steps"); + Serial.print("\r\n"); + + if (axisY.movingToHome()) + { + encoderY.setPosition(0); + axisY.setCurrentPosition(0); + Serial.print("R99"); + Serial.print(" home position Y axis detected with encoder"); + Serial.print("\r\n"); + } + } + + if (motorConsMissedSteps[2] > motorConsMissedStepsMax[2]) + { + axisZ.deactivateAxis(); + Serial.print("R99"); + Serial.print(" deactivate motor Z due to missed steps"); + Serial.print("\r\n"); + + if (axisZ.movingToHome()) + { + encoderZ.setPosition(0); + axisZ.setCurrentPosition(0); + Serial.print("R99"); + Serial.print(" home position Z axis detected with encoder"); + Serial.print("\r\n"); + } + } + + if (axisX.endStopAxisReached(false)) + { + axisX.setCurrentPosition(0); + encoderX.setPosition(0); + } + + if (axisY.endStopAxisReached(false)) + { + axisY.setCurrentPosition(0); + encoderY.setPosition(0); + } + + if (axisZ.endStopAxisReached(false)) + { + axisZ.setCurrentPosition(0); + encoderZ.setPosition(0); + } + + axisActive[0] = axisX.isAxisActive(); + axisActive[1] = axisY.isAxisActive(); + axisActive[2] = axisZ.isAxisActive(); + + currentPoint[0] = axisX.currentPosition(); + currentPoint[1] = axisY.currentPosition(); + currentPoint[2] = axisZ.currentPosition(); + + CurrentState::getInstance()->setX(currentPoint[0]); + CurrentState::getInstance()->setY(currentPoint[1]); + CurrentState::getInstance()->setZ(currentPoint[2]); + + storeEndStops(); + + // Check timeouts + if (axisActive[0] == true && ((millis() >= timeStart && millis() - timeStart > timeOut[0] * 1000) || (millis() < timeStart && millis() > timeOut[0] * 1000))) + { + Serial.print("R99 timeout X axis\r\n"); + error = 1; + } + if (axisActive[1] == true && ((millis() >= timeStart && millis() - timeStart > timeOut[0] * 1000) || (millis() < timeStart && millis() > timeOut[0] * 1000))) + { + Serial.print("R99 timeout Y axis\r\n"); + error = 1; + } + if (axisActive[2] == true && ((millis() >= timeStart && millis() - timeStart > timeOut[0] * 1000) || (millis() < timeStart && millis() > timeOut[0] * 1000))) + { + Serial.print("R99 timeout Z axis\r\n"); + error = 1; + } + + // Check if there is an emergency stop command + if (Serial.available() > 0) + { + incomingByte = Serial.read(); + if (incomingByte == 69 || incomingByte == 101) + { + Serial.print("R99 emergency stop\r\n"); + axisX.deactivateAxis(); + axisY.deactivateAxis(); + axisZ.deactivateAxis(); + error = 1; + } + } + + if (error == 1) + { + Serial.print("R99 error\r\n"); + + axisActive[0] = false; + axisActive[1] = false; + axisActive[2] = false; + } + + // Periodically send message still active + currentMillis++; + + //if (currentMillis % 2500 == 0) + if (currentMillis % 750 == 0) + //if (1 == 1) + { + Serial.print(COMM_REPORT_CMD_BUSY); + //Serial.print("\r\n"); + CurrentState::getInstance()->printQAndNewLine(); + reportPosition(); + /* Serial.print("R99"); Serial.print(" encoder pos "); Serial.print(encoderX.currentPosition()); @@ -437,439 +465,454 @@ int StepperControl::moveToCoords( long xDest, long yDest, long zDest, Serial.print("\r\n"); */ - //Serial.print("R99"); - //Serial.print(" missed step nr "); - //Serial.print(motorConsMissedSteps[0]); - //Serial.print(" encoder pos "); - //Serial.print(encoderX.currentPosition()); - //Serial.print(" axis pos "); - //Serial.print(axisX.currentPosition()); - //Serial.print("\r\n"); - - } - - } + //Serial.print("R99"); + //Serial.print(" missed step nr "); + //Serial.print(motorConsMissedSteps[0]); + //Serial.print(" encoder pos "); + //Serial.print(encoderX.currentPosition()); + //Serial.print(" axis pos "); + //Serial.print(axisX.currentPosition()); + //Serial.print("\r\n"); + } + } - Serial.print("R99 stopped\r\n"); + Serial.print("R99 stopped\r\n"); - // Stop motors + // Stop motors - axisSubStep[0] = COMM_REPORT_MOVE_STATUS_STOP_MOTOR; - axisSubStep[1] = COMM_REPORT_MOVE_STATUS_STOP_MOTOR; - axisSubStep[2] = COMM_REPORT_MOVE_STATUS_STOP_MOTOR; + axisSubStep[0] = COMM_REPORT_MOVE_STATUS_STOP_MOTOR; + axisSubStep[1] = COMM_REPORT_MOVE_STATUS_STOP_MOTOR; + axisSubStep[2] = COMM_REPORT_MOVE_STATUS_STOP_MOTOR; - reportStatus(&axisX, axisSubStep[0]); - reportStatus(&axisY, axisSubStep[1]); - reportStatus(&axisZ, axisSubStep[2]); + reportStatus(&axisX, axisSubStep[0]); + reportStatus(&axisY, axisSubStep[1]); + reportStatus(&axisZ, axisSubStep[2]); - disableMotors(); + disableMotors(); - // Report end statuses + // Report end statuses - currentPoint[0] = axisX.currentPosition(); - currentPoint[1] = axisY.currentPosition(); - currentPoint[2] = axisZ.currentPosition(); + currentPoint[0] = axisX.currentPosition(); + currentPoint[1] = axisY.currentPosition(); + currentPoint[2] = axisZ.currentPosition(); - CurrentState::getInstance()->setX(currentPoint[0]); - CurrentState::getInstance()->setY(currentPoint[1]); - CurrentState::getInstance()->setZ(currentPoint[2]); + CurrentState::getInstance()->setX(currentPoint[0]); + CurrentState::getInstance()->setY(currentPoint[1]); + CurrentState::getInstance()->setZ(currentPoint[2]); - storeEndStops(); - reportEndStops(); - reportPosition(); + storeEndStops(); + reportEndStops(); + reportPosition(); - // Report axis idle + // Report axis idle - axisSubStep[0] = COMM_REPORT_MOVE_STATUS_IDLE; - axisSubStep[1] = COMM_REPORT_MOVE_STATUS_IDLE; - axisSubStep[2] = COMM_REPORT_MOVE_STATUS_IDLE; + axisSubStep[0] = COMM_REPORT_MOVE_STATUS_IDLE; + axisSubStep[1] = COMM_REPORT_MOVE_STATUS_IDLE; + axisSubStep[2] = COMM_REPORT_MOVE_STATUS_IDLE; - reportStatus(&axisX, axisSubStep[0]); - reportStatus(&axisY, axisSubStep[1]); - reportStatus(&axisZ, axisSubStep[2]); + reportStatus(&axisX, axisSubStep[0]); + reportStatus(&axisY, axisSubStep[1]); + reportStatus(&axisZ, axisSubStep[2]); - disableMotors(); + disableMotors(); - // Return error + // Return error - return error; + return error; } // // Calibration // -int StepperControl::calibrateAxis(int axis) { - - // Load motor and encoder settings - - loadMotorSettings(); - loadEncoderSettings(); - - //unsigned long timeStart = millis(); - - bool movementDone = false; - - int paramValueInt = 0; - int stepsCount = 0; - int incomingByte = 0; - int error = 0; - - - bool invertEndStops = false; - int parEndInv; - int parNbrStp; - - float * missedSteps; - int * missedStepsMax; - long * lastPosition; - float * encoderStepDecay; - bool * encoderEnabled; - int * axisStatus; - - // Prepare for movement - - storeEndStops(); - reportEndStops(); - - // Select the right axis - StepperControlAxis* calibAxis; - StepperControlEncoder* calibEncoder; - - switch (axis) { - case 0: - calibAxis = &axisX; - calibEncoder = &encoderX; - parEndInv = MOVEMENT_INVERT_ENDPOINTS_X; - parNbrStp = MOVEMENT_AXIS_NR_STEPS_X; - invertEndStops = endStInv[0]; - missedSteps = &motorConsMissedSteps[0]; - missedStepsMax = &motorConsMissedStepsMax[0]; - lastPosition = &motorLastPosition[0]; - encoderStepDecay = &motorConsMissedStepsDecay[0]; - encoderEnabled = &motorConsEncoderEnabled[0]; - axisStatus = &axisSubStep[0]; - break; - case 1: - calibAxis = &axisY; - calibEncoder = &encoderY; - parEndInv = MOVEMENT_INVERT_ENDPOINTS_Y;; - parNbrStp = MOVEMENT_AXIS_NR_STEPS_Y; - invertEndStops = endStInv[1]; - missedSteps = &motorConsMissedSteps[1]; - missedStepsMax = &motorConsMissedStepsMax[1]; - lastPosition = &motorLastPosition[1]; - encoderStepDecay = &motorConsMissedStepsDecay[1]; - encoderEnabled = &motorConsEncoderEnabled[1]; - axisStatus = &axisSubStep[1]; - break; - case 2: - calibAxis = &axisZ; - calibEncoder = &encoderZ; - parEndInv = MOVEMENT_INVERT_ENDPOINTS_Z; - parNbrStp = MOVEMENT_AXIS_NR_STEPS_Z; - invertEndStops = endStInv[2]; - missedSteps = &motorConsMissedSteps[2]; - missedStepsMax = &motorConsMissedStepsMax[2]; - lastPosition = &motorLastPosition[2]; - encoderStepDecay = &motorConsMissedStepsDecay[2]; - encoderEnabled = &motorConsEncoderEnabled[2]; - axisStatus = &axisSubStep[2]; - break; - default: - Serial.print("R99 Calibration error: invalid axis selected\r\n"); - return 1; - } - - - // Preliminary checks - - if (calibAxis->endStopMin() || calibAxis->endStopMax()) { - Serial.print("R99 Calibration error: end stop active before start\r\n"); - return 1; - } - - Serial.print("R99"); - Serial.print(" axis "); - Serial.print(calibAxis->label); - Serial.print(" move to start for calibration"); - Serial.print("\r\n"); - - *axisStatus = COMM_REPORT_MOVE_STATUS_START_MOTOR; - reportStatus(&axisX, axisSubStep[0]); - - // Move towards home - calibAxis->enableMotor(); - calibAxis->setDirectionHome(); - calibAxis->setCurrentPosition(calibEncoder->currentPosition()); - - stepsCount = 0; - *missedSteps = 0; - movementDone = false; - - motorConsMissedSteps[0] = 0; - motorConsMissedSteps[1] = 0; - motorConsMissedSteps[2] = 0; - - *axisStatus = COMM_REPORT_MOVE_STATUS_CRAWLING; - reportStatus(&axisX, axisSubStep[0]); - - reportCalib(&axisX, COMM_REPORT_CALIBRATE_STATUS_TO_HOME); - - while (!movementDone && error == 0) { - - //checkAxisVsEncoder(&axisX, &encoderX, &motorConsMissedSteps[0], &motorLastPosition[0], &motorConsMissedStepsDecay[0], &motorConsEncoderEnabled[0]); - - // Check if there is an emergency stop command - if (Serial.available() > 0) { - incomingByte = Serial.read(); - if (incomingByte == 69 || incomingByte == 101) { - Serial.print("R99 emergency stop\r\n"); - movementDone = true; - error = 1; - } - } - - // Ignore the missed steps at startup time - //if (stepsCount < 20) { - // *missedSteps = 0; - //} - - // Move until the end stop for home position is reached, either by end stop or motot skipping - if ((!calibAxis->endStopMin() && !calibAxis->endStopMax()) && !movementDone && (*missedSteps < *missedStepsMax)) { - - calibAxis->setStepAxis(); - - delayMicroseconds(100000 / speedMin[axis] /2); - - stepsCount++; - if (stepsCount % (speedMin[axis] * 3) == 0) - { - // Periodically send message still active - Serial.print(COMM_REPORT_CMD_BUSY); - //Serial.print("\r\n"); - CurrentState::getInstance()->printQAndNewLine(); - } - - if (stepsCount % (speedMin[axis] / 6) == 0 /*|| *missedSteps > 3*/) - { - Serial.print("R99"); - Serial.print(" step count "); - Serial.print(stepsCount); - Serial.print(" missed steps "); - Serial.print(*missedSteps); - Serial.print(" max steps "); - Serial.print(*missedStepsMax); - Serial.print(" cur pos mtr "); - Serial.print(calibAxis->currentPosition()); - Serial.print(" cur pos enc "); - Serial.print(calibEncoder->currentPosition()); - Serial.print("\r\n"); - } - - calibAxis->resetMotorStep(); - delayMicroseconds(100000 / speedMin[axis] /2); - - } else { - movementDone = true; - Serial.print("R99 movement done\r\n"); - - // If end stop for home is active, set the position to zero - if (calibAxis->endStopMax()) - { - invertEndStops = true; - } - } - } - - reportCalib(&axisX, COMM_REPORT_CALIBRATE_STATUS_TO_END); - - Serial.print("R99"); - Serial.print(" axis "); - Serial.print(calibAxis->label); - Serial.print(" at starting point"); - Serial.print("\r\n"); - - // Report back the end stop setting - - if (error == 0) { - if (invertEndStops) { - paramValueInt = 1; - } else { - paramValueInt = 0; - } - - Serial.print("R23"); - Serial.print(" "); - Serial.print("P"); - Serial.print(parEndInv); - Serial.print(" "); - Serial.print("V"); - Serial.print(paramValueInt); - Serial.print("\r\n"); - } - - // Store the status of the system - - storeEndStops(); - reportEndStops(); - - // Move into the other direction now, and measure the number of steps - - Serial.print("R99"); - Serial.print(" axis "); - Serial.print(calibAxis->label); - Serial.print(" calibrating length"); - Serial.print("\r\n"); - - stepsCount = 0; - movementDone = false; - *missedSteps = 0; - calibAxis->setDirectionAway(); - calibAxis->setCurrentPosition(calibEncoder->currentPosition()); - - motorConsMissedSteps[0] = 0; - motorConsMissedSteps[1] = 0; - motorConsMissedSteps[2] = 0; - - long encoderStartPoint = calibEncoder->currentPosition(); - long encoderEndPoint = calibEncoder->currentPosition(); - - while (!movementDone && error == 0) { - - // Check if there is an emergency stop command - if (Serial.available() > 0) { - incomingByte = Serial.read(); - if (incomingByte == 69 || incomingByte == 101) { - Serial.print("R99 emergency stop\r\n"); - movementDone = true; - error = 1; - } - } - - // Ignore the missed steps at startup time - if (stepsCount < 10) { - *missedSteps = 0; - } - - // Move until the end stop at the other side of the axis is reached - if (((!invertEndStops && !calibAxis->endStopMax()) || (invertEndStops && !calibAxis->endStopMin())) && !movementDone && (*missedSteps < *missedStepsMax)) { - - calibAxis->setStepAxis(); - stepsCount++; - - //checkAxisVsEncoder(&axisX, &encoderX, &motorConsMissedSteps[0], &motorLastPosition[0], &motorConsMissedStepsDecay[0], &motorConsEncoderEnabled[0]); - - - delayMicroseconds(100000 / speedMin[axis] /2); - - if (stepsCount % (speedMin[axis] * 3) == 0) - { - // Periodically send message still active - Serial.print(COMM_REPORT_CMD_BUSY); - //Serial.print("\r\n"); - CurrentState::getInstance()->printQAndNewLine(); - } - - calibAxis->resetMotorStep(); - delayMicroseconds(100000 / speedMin[axis] /2); - - } else { - Serial.print("R99 movement done\r\n"); - movementDone = true; - } - } - - - Serial.print("R99"); - Serial.print(" axis "); - Serial.print(calibAxis->label); - Serial.print(" at end point"); - Serial.print("\r\n"); - - encoderEndPoint = calibEncoder->currentPosition(); - - // if the encoder is enabled, use the encoder data instead of the step count - - if (encoderEnabled) { - stepsCount = abs(encoderEndPoint - encoderStartPoint); - } - - // Report back the end stop setting - - if (error == 0) { - Serial.print("R23"); - Serial.print(" "); - Serial.print("P"); - Serial.print(parNbrStp); - Serial.print(" "); - Serial.print("V"); - Serial.print(stepsCount); - Serial.print("\r\n"); - } - - *axisStatus = COMM_REPORT_MOVE_STATUS_STOP_MOTOR; - reportStatus(&axisX, axisSubStep[0]); - - calibAxis->disableMotor(); - - storeEndStops(); - reportEndStops(); - - - switch (axis) { - case 0: - CurrentState::getInstance()->setX(stepsCount); - break; - case 1: - CurrentState::getInstance()->setY(stepsCount); - break; - case 2: - CurrentState::getInstance()->setZ(stepsCount); - break; - } - - reportPosition(); - - - *axisStatus = COMM_REPORT_MOVE_STATUS_IDLE; - reportStatus(&axisX, axisSubStep[0]); - - reportCalib(&axisX, COMM_REPORT_CALIBRATE_STATUS_IDLE); - - return error; +int StepperControl::calibrateAxis(int axis) +{ + + // Load motor and encoder settings + + loadMotorSettings(); + loadEncoderSettings(); + + //unsigned long timeStart = millis(); + + bool movementDone = false; + + int paramValueInt = 0; + int stepsCount = 0; + int incomingByte = 0; + int error = 0; + + bool invertEndStops = false; + int parEndInv; + int parNbrStp; + + float *missedSteps; + int *missedStepsMax; + long *lastPosition; + float *encoderStepDecay; + bool *encoderEnabled; + int *axisStatus; + + // Prepare for movement + + storeEndStops(); + reportEndStops(); + + // Select the right axis + StepperControlAxis *calibAxis; + StepperControlEncoder *calibEncoder; + + switch (axis) + { + case 0: + calibAxis = &axisX; + calibEncoder = &encoderX; + parEndInv = MOVEMENT_INVERT_ENDPOINTS_X; + parNbrStp = MOVEMENT_AXIS_NR_STEPS_X; + invertEndStops = endStInv[0]; + missedSteps = &motorConsMissedSteps[0]; + missedStepsMax = &motorConsMissedStepsMax[0]; + lastPosition = &motorLastPosition[0]; + encoderStepDecay = &motorConsMissedStepsDecay[0]; + encoderEnabled = &motorConsEncoderEnabled[0]; + axisStatus = &axisSubStep[0]; + break; + case 1: + calibAxis = &axisY; + calibEncoder = &encoderY; + parEndInv = MOVEMENT_INVERT_ENDPOINTS_Y; + ; + parNbrStp = MOVEMENT_AXIS_NR_STEPS_Y; + invertEndStops = endStInv[1]; + missedSteps = &motorConsMissedSteps[1]; + missedStepsMax = &motorConsMissedStepsMax[1]; + lastPosition = &motorLastPosition[1]; + encoderStepDecay = &motorConsMissedStepsDecay[1]; + encoderEnabled = &motorConsEncoderEnabled[1]; + axisStatus = &axisSubStep[1]; + break; + case 2: + calibAxis = &axisZ; + calibEncoder = &encoderZ; + parEndInv = MOVEMENT_INVERT_ENDPOINTS_Z; + parNbrStp = MOVEMENT_AXIS_NR_STEPS_Z; + invertEndStops = endStInv[2]; + missedSteps = &motorConsMissedSteps[2]; + missedStepsMax = &motorConsMissedStepsMax[2]; + lastPosition = &motorLastPosition[2]; + encoderStepDecay = &motorConsMissedStepsDecay[2]; + encoderEnabled = &motorConsEncoderEnabled[2]; + axisStatus = &axisSubStep[2]; + break; + default: + Serial.print("R99 Calibration error: invalid axis selected\r\n"); + return 1; + } + + // Preliminary checks + + if (calibAxis->endStopMin() || calibAxis->endStopMax()) + { + Serial.print("R99 Calibration error: end stop active before start\r\n"); + return 1; + } + + Serial.print("R99"); + Serial.print(" axis "); + Serial.print(calibAxis->label); + Serial.print(" move to start for calibration"); + Serial.print("\r\n"); + + *axisStatus = COMM_REPORT_MOVE_STATUS_START_MOTOR; + reportStatus(&axisX, axisSubStep[0]); + + // Move towards home + calibAxis->enableMotor(); + calibAxis->setDirectionHome(); + calibAxis->setCurrentPosition(calibEncoder->currentPosition()); + + stepsCount = 0; + *missedSteps = 0; + movementDone = false; + + motorConsMissedSteps[0] = 0; + motorConsMissedSteps[1] = 0; + motorConsMissedSteps[2] = 0; + + *axisStatus = COMM_REPORT_MOVE_STATUS_CRAWLING; + reportStatus(&axisX, axisSubStep[0]); + + reportCalib(&axisX, COMM_REPORT_CALIBRATE_STATUS_TO_HOME); + + while (!movementDone && error == 0) + { + + //checkAxisVsEncoder(&axisX, &encoderX, &motorConsMissedSteps[0], &motorLastPosition[0], &motorConsMissedStepsDecay[0], &motorConsEncoderEnabled[0]); + + // Check if there is an emergency stop command + if (Serial.available() > 0) + { + incomingByte = Serial.read(); + if (incomingByte == 69 || incomingByte == 101) + { + Serial.print("R99 emergency stop\r\n"); + movementDone = true; + error = 1; + } + } + + // Ignore the missed steps at startup time + //if (stepsCount < 20) { + // *missedSteps = 0; + //} + + // Move until the end stop for home position is reached, either by end stop or motot skipping + if ((!calibAxis->endStopMin() && !calibAxis->endStopMax()) && !movementDone && (*missedSteps < *missedStepsMax)) + { + + calibAxis->setStepAxis(); + + delayMicroseconds(100000 / speedMin[axis] / 2); + + stepsCount++; + if (stepsCount % (speedMin[axis] * 3) == 0) + { + // Periodically send message still active + Serial.print(COMM_REPORT_CMD_BUSY); + //Serial.print("\r\n"); + CurrentState::getInstance()->printQAndNewLine(); + } + + if (stepsCount % (speedMin[axis] / 6) == 0 /*|| *missedSteps > 3*/) + { + Serial.print("R99"); + Serial.print(" step count "); + Serial.print(stepsCount); + Serial.print(" missed steps "); + Serial.print(*missedSteps); + Serial.print(" max steps "); + Serial.print(*missedStepsMax); + Serial.print(" cur pos mtr "); + Serial.print(calibAxis->currentPosition()); + Serial.print(" cur pos enc "); + Serial.print(calibEncoder->currentPosition()); + Serial.print("\r\n"); + } + + calibAxis->resetMotorStep(); + delayMicroseconds(100000 / speedMin[axis] / 2); + } + else + { + movementDone = true; + Serial.print("R99 movement done\r\n"); + + // If end stop for home is active, set the position to zero + if (calibAxis->endStopMax()) + { + invertEndStops = true; + } + } + } + + reportCalib(&axisX, COMM_REPORT_CALIBRATE_STATUS_TO_END); + + Serial.print("R99"); + Serial.print(" axis "); + Serial.print(calibAxis->label); + Serial.print(" at starting point"); + Serial.print("\r\n"); + + // Report back the end stop setting + + if (error == 0) + { + if (invertEndStops) + { + paramValueInt = 1; + } + else + { + paramValueInt = 0; + } + + Serial.print("R23"); + Serial.print(" "); + Serial.print("P"); + Serial.print(parEndInv); + Serial.print(" "); + Serial.print("V"); + Serial.print(paramValueInt); + Serial.print("\r\n"); + } + + // Store the status of the system + + storeEndStops(); + reportEndStops(); + + // Move into the other direction now, and measure the number of steps + + Serial.print("R99"); + Serial.print(" axis "); + Serial.print(calibAxis->label); + Serial.print(" calibrating length"); + Serial.print("\r\n"); + + stepsCount = 0; + movementDone = false; + *missedSteps = 0; + calibAxis->setDirectionAway(); + calibAxis->setCurrentPosition(calibEncoder->currentPosition()); + + motorConsMissedSteps[0] = 0; + motorConsMissedSteps[1] = 0; + motorConsMissedSteps[2] = 0; + + long encoderStartPoint = calibEncoder->currentPosition(); + long encoderEndPoint = calibEncoder->currentPosition(); + + while (!movementDone && error == 0) + { + + // Check if there is an emergency stop command + if (Serial.available() > 0) + { + incomingByte = Serial.read(); + if (incomingByte == 69 || incomingByte == 101) + { + Serial.print("R99 emergency stop\r\n"); + movementDone = true; + error = 1; + } + } + + // Ignore the missed steps at startup time + if (stepsCount < 10) + { + *missedSteps = 0; + } + + // Move until the end stop at the other side of the axis is reached + if (((!invertEndStops && !calibAxis->endStopMax()) || (invertEndStops && !calibAxis->endStopMin())) && !movementDone && (*missedSteps < *missedStepsMax)) + { + + calibAxis->setStepAxis(); + stepsCount++; + + //checkAxisVsEncoder(&axisX, &encoderX, &motorConsMissedSteps[0], &motorLastPosition[0], &motorConsMissedStepsDecay[0], &motorConsEncoderEnabled[0]); + + delayMicroseconds(100000 / speedMin[axis] / 2); + + if (stepsCount % (speedMin[axis] * 3) == 0) + { + // Periodically send message still active + Serial.print(COMM_REPORT_CMD_BUSY); + //Serial.print("\r\n"); + CurrentState::getInstance()->printQAndNewLine(); + } + + calibAxis->resetMotorStep(); + delayMicroseconds(100000 / speedMin[axis] / 2); + } + else + { + Serial.print("R99 movement done\r\n"); + movementDone = true; + } + } + + Serial.print("R99"); + Serial.print(" axis "); + Serial.print(calibAxis->label); + Serial.print(" at end point"); + Serial.print("\r\n"); + + encoderEndPoint = calibEncoder->currentPosition(); + + // if the encoder is enabled, use the encoder data instead of the step count + + if (encoderEnabled) + { + stepsCount = abs(encoderEndPoint - encoderStartPoint); + } + + // Report back the end stop setting + + if (error == 0) + { + Serial.print("R23"); + Serial.print(" "); + Serial.print("P"); + Serial.print(parNbrStp); + Serial.print(" "); + Serial.print("V"); + Serial.print(stepsCount); + Serial.print("\r\n"); + } + + *axisStatus = COMM_REPORT_MOVE_STATUS_STOP_MOTOR; + reportStatus(&axisX, axisSubStep[0]); + + calibAxis->disableMotor(); + + storeEndStops(); + reportEndStops(); + + switch (axis) + { + case 0: + CurrentState::getInstance()->setX(stepsCount); + break; + case 1: + CurrentState::getInstance()->setY(stepsCount); + break; + case 2: + CurrentState::getInstance()->setZ(stepsCount); + break; + } + + reportPosition(); + + *axisStatus = COMM_REPORT_MOVE_STATUS_IDLE; + reportStatus(&axisX, axisSubStep[0]); + + reportCalib(&axisX, COMM_REPORT_CALIBRATE_STATUS_IDLE); + + return error; } - // Handle movement by checking each axis -void StepperControl::handleMovementInterrupt(void){ +void StepperControl::handleMovementInterrupt(void) +{ - encoderX.readEncoder(); - encoderY.readEncoder(); - encoderZ.readEncoder(); + encoderX.readEncoder(); + encoderY.readEncoder(); + encoderZ.readEncoder(); - axisX.checkTiming(); - axisY.checkTiming(); - axisZ.checkTiming(); - - checkAxisVsEncoder(&axisX, &encoderX, &motorConsMissedSteps[0], &motorLastPosition[0], &motorConsMissedStepsDecay[0], &motorConsEncoderEnabled[0]); - checkAxisVsEncoder(&axisY, &encoderY, &motorConsMissedSteps[1], &motorLastPosition[1], &motorConsMissedStepsDecay[1], &motorConsEncoderEnabled[1]); - checkAxisVsEncoder(&axisZ, &encoderZ, &motorConsMissedSteps[2], &motorLastPosition[2], &motorConsMissedStepsDecay[2], &motorConsEncoderEnabled[2]); + axisX.checkTiming(); + axisY.checkTiming(); + axisZ.checkTiming(); + checkAxisVsEncoder(&axisX, &encoderX, &motorConsMissedSteps[0], &motorLastPosition[0], &motorConsMissedStepsDecay[0], &motorConsEncoderEnabled[0]); + checkAxisVsEncoder(&axisY, &encoderY, &motorConsMissedSteps[1], &motorLastPosition[1], &motorConsMissedStepsDecay[1], &motorConsEncoderEnabled[1]); + checkAxisVsEncoder(&axisZ, &encoderZ, &motorConsMissedSteps[2], &motorLastPosition[2], &motorConsMissedStepsDecay[2], &motorConsEncoderEnabled[2]); } int debugPrintCount = 0; // Check encoder to verify the motor is at the right position -void StepperControl::checkAxisVsEncoder(StepperControlAxis* axis, StepperControlEncoder* encoder, float* missedSteps, long* lastPosition, float* encoderStepDecay, bool* encoderEnabled) { +void StepperControl::checkAxisVsEncoder(StepperControlAxis *axis, StepperControlEncoder *encoder, float *missedSteps, long *lastPosition, float *encoderStepDecay, bool *encoderEnabled) +{ - // If a step is done - //if (axis->isStepDone() && axis->currentPosition() % 3 == 0) { - if (*encoderEnabled && axis->isStepDone()) { + // If a step is done + //if (axis->isStepDone() && axis->currentPosition() % 3 == 0) { + if (*encoderEnabled && axis->isStepDone()) + { - bool stepMissed = false; + bool stepMissed = false; - /* + /* debugPrintCount++; if (debugPrintCount % 50 == 0) { @@ -898,224 +941,261 @@ void StepperControl::checkAxisVsEncoder(StepperControlAxis* axis, StepperControl } */ - // Decrease amount of missed steps if there are no missed step - if (*missedSteps > 0) { - (*missedSteps)-= (*encoderStepDecay); - } - - // Check if the encoder goes in the wrong direction or nothing moved - if (( axis->movingUp() && *lastPosition >= axis->currentPosition()) || - (!axis->movingUp() && *lastPosition <= axis->currentPosition())) { - stepMissed = true; - } - - if (abs(axis->currentPosition() - encoder->currentPosition()) > 2) { - stepMissed = true; - } - - if (stepMissed) { - axis->setCurrentPosition(encoder->currentPosition()); - (*missedSteps)++; - } - - *lastPosition = axis->currentPosition(); - axis->resetStepDone(); - } + // Decrease amount of missed steps if there are no missed step + if (*missedSteps > 0) + { + (*missedSteps) -= (*encoderStepDecay); + } + + // Check if the encoder goes in the wrong direction or nothing moved + if ((axis->movingUp() && *lastPosition >= axis->currentPosition()) || + (!axis->movingUp() && *lastPosition <= axis->currentPosition())) + { + stepMissed = true; + } + + if (abs(axis->currentPosition() - encoder->currentPosition()) > 2) + { + stepMissed = true; + } + + if (stepMissed) + { + axis->setCurrentPosition(encoder->currentPosition()); + (*missedSteps)++; + } + + *lastPosition = axis->currentPosition(); + axis->resetStepDone(); + } } -void StepperControl::loadMotorSettings() { +void StepperControl::loadMotorSettings() +{ - // Load settings + // 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); + 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); + 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); + 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); + 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); + 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); + 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); - endStEnbl[0] = intToBool(ParameterList::getInstance()->getValue(MOVEMENT_ENABLE_ENDPOINTS_X)); - endStEnbl[1] = intToBool(ParameterList::getInstance()->getValue(MOVEMENT_ENABLE_ENDPOINTS_Y)); - endStEnbl[2] = intToBool(ParameterList::getInstance()->getValue(MOVEMENT_ENABLE_ENDPOINTS_Z)); + endStEnbl[0] = intToBool(ParameterList::getInstance()->getValue(MOVEMENT_ENABLE_ENDPOINTS_X)); + endStEnbl[1] = intToBool(ParameterList::getInstance()->getValue(MOVEMENT_ENABLE_ENDPOINTS_Y)); + endStEnbl[2] = intToBool(ParameterList::getInstance()->getValue(MOVEMENT_ENABLE_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[0] = ParameterList::getInstance()->getValue(MOVEMENT_TIMEOUT_X); + timeOut[1] = ParameterList::getInstance()->getValue(MOVEMENT_TIMEOUT_X); + timeOut[2] = ParameterList::getInstance()->getValue(MOVEMENT_TIMEOUT_X); - motor2Inv[0] = intToBool(ParameterList::getInstance()->getValue(MOVEMENT_SECONDARY_MOTOR_INVERT_X)); - motor2Inv[1] = false; - motor2Inv[2] = false; + motor2Inv[0] = intToBool(ParameterList::getInstance()->getValue(MOVEMENT_SECONDARY_MOTOR_INVERT_X)); + motor2Inv[1] = false; + motor2Inv[2] = false; - motor2Enbl[0] = intToBool(ParameterList::getInstance()->getValue(MOVEMENT_SECONDARY_MOTOR_X)); - motor2Enbl[1] = false; - motor2Enbl[2] = false; - - axisX.loadMotorSettings(speedMax[0], speedMin[0], stepsAcc[0], timeOut[0], homeIsUp[0], motorInv[0], endStInv[0], MOVEMENT_INTERRUPT_SPEED, motor2Enbl[0], motor2Inv[0], endStEnbl[0]); - axisY.loadMotorSettings(speedMax[1], speedMin[1], stepsAcc[1], timeOut[1], homeIsUp[1], motorInv[1], endStInv[1], MOVEMENT_INTERRUPT_SPEED, motor2Enbl[1], motor2Inv[1], endStEnbl[1]); - axisZ.loadMotorSettings(speedMax[2], speedMin[2], stepsAcc[2], timeOut[2], homeIsUp[2], motorInv[2], endStInv[2], MOVEMENT_INTERRUPT_SPEED, motor2Enbl[2], motor2Inv[2], endStEnbl[2]); + motor2Enbl[0] = intToBool(ParameterList::getInstance()->getValue(MOVEMENT_SECONDARY_MOTOR_X)); + motor2Enbl[1] = false; + motor2Enbl[2] = false; + axisX.loadMotorSettings(speedMax[0], speedMin[0], stepsAcc[0], timeOut[0], homeIsUp[0], motorInv[0], endStInv[0], MOVEMENT_INTERRUPT_SPEED, motor2Enbl[0], motor2Inv[0], endStEnbl[0]); + axisY.loadMotorSettings(speedMax[1], speedMin[1], stepsAcc[1], timeOut[1], homeIsUp[1], motorInv[1], endStInv[1], MOVEMENT_INTERRUPT_SPEED, motor2Enbl[1], motor2Inv[1], endStEnbl[1]); + axisZ.loadMotorSettings(speedMax[2], speedMin[2], stepsAcc[2], timeOut[2], homeIsUp[2], motorInv[2], endStInv[2], MOVEMENT_INTERRUPT_SPEED, motor2Enbl[2], motor2Inv[2], endStEnbl[2]); } -bool StepperControl::intToBool(int value) { - if (value == 1) { - return true; - } - return false; +bool StepperControl::intToBool(int value) +{ + if (value == 1) + { + return true; + } + return false; } -void StepperControl::loadEncoderSettings() { - - // Load encoder settings - - motorConsMissedStepsMax[0] = ParameterList::getInstance()->getValue(ENCODER_MISSED_STEPS_MAX_X); - motorConsMissedStepsMax[1] = ParameterList::getInstance()->getValue(ENCODER_MISSED_STEPS_MAX_Y); - motorConsMissedStepsMax[2] = ParameterList::getInstance()->getValue(ENCODER_MISSED_STEPS_MAX_Z); - - motorConsMissedStepsDecay[0] = ParameterList::getInstance()->getValue(ENCODER_MISSED_STEPS_DECAY_X); - motorConsMissedStepsDecay[1] = ParameterList::getInstance()->getValue(ENCODER_MISSED_STEPS_DECAY_Y); - motorConsMissedStepsDecay[2] = ParameterList::getInstance()->getValue(ENCODER_MISSED_STEPS_DECAY_Z); - - motorConsMissedStepsDecay[0] = motorConsMissedStepsDecay[0] / 100; - motorConsMissedStepsDecay[1] = motorConsMissedStepsDecay[1] / 100; - motorConsMissedStepsDecay[2] = motorConsMissedStepsDecay[2] / 100; - - motorConsMissedStepsDecay[0] = min(max(motorConsMissedStepsDecay[0],0.01),99); - motorConsMissedStepsDecay[1] = min(max(motorConsMissedStepsDecay[1],0.01),99); - motorConsMissedStepsDecay[2] = min(max(motorConsMissedStepsDecay[2],0.01),99); - - motorConsEncoderType[0] = ParameterList::getInstance()->getValue(ENCODER_TYPE_X); - motorConsEncoderType[1] = ParameterList::getInstance()->getValue(ENCODER_TYPE_Y); - motorConsEncoderType[2] = ParameterList::getInstance()->getValue(ENCODER_TYPE_Z); - - motorConsEncoderScaling[0] = ParameterList::getInstance()->getValue(ENCODER_SCALING_X); - motorConsEncoderScaling[1] = ParameterList::getInstance()->getValue(ENCODER_SCALING_Y); - motorConsEncoderScaling[2] = ParameterList::getInstance()->getValue(ENCODER_SCALING_Z); - - if (ParameterList::getInstance()->getValue(ENCODER_ENABLED_X) == 1) { - motorConsEncoderEnabled[0] = true; - } else { - motorConsEncoderEnabled[0] = false; - } - - if (ParameterList::getInstance()->getValue(ENCODER_ENABLED_Y) == 1) { - motorConsEncoderEnabled[1] = true; - } else { - motorConsEncoderEnabled[1] = false; - } - - if (ParameterList::getInstance()->getValue(ENCODER_ENABLED_Z) == 1) { - motorConsEncoderEnabled[2] = true; - } else { - motorConsEncoderEnabled[2] = false; - } - +void StepperControl::loadEncoderSettings() +{ + + // Load encoder settings + + motorConsMissedStepsMax[0] = ParameterList::getInstance()->getValue(ENCODER_MISSED_STEPS_MAX_X); + motorConsMissedStepsMax[1] = ParameterList::getInstance()->getValue(ENCODER_MISSED_STEPS_MAX_Y); + motorConsMissedStepsMax[2] = ParameterList::getInstance()->getValue(ENCODER_MISSED_STEPS_MAX_Z); + + motorConsMissedStepsDecay[0] = ParameterList::getInstance()->getValue(ENCODER_MISSED_STEPS_DECAY_X); + motorConsMissedStepsDecay[1] = ParameterList::getInstance()->getValue(ENCODER_MISSED_STEPS_DECAY_Y); + motorConsMissedStepsDecay[2] = ParameterList::getInstance()->getValue(ENCODER_MISSED_STEPS_DECAY_Z); + + motorConsMissedStepsDecay[0] = motorConsMissedStepsDecay[0] / 100; + motorConsMissedStepsDecay[1] = motorConsMissedStepsDecay[1] / 100; + motorConsMissedStepsDecay[2] = motorConsMissedStepsDecay[2] / 100; + + motorConsMissedStepsDecay[0] = min(max(motorConsMissedStepsDecay[0], 0.01), 99); + motorConsMissedStepsDecay[1] = min(max(motorConsMissedStepsDecay[1], 0.01), 99); + motorConsMissedStepsDecay[2] = min(max(motorConsMissedStepsDecay[2], 0.01), 99); + + motorConsEncoderType[0] = ParameterList::getInstance()->getValue(ENCODER_TYPE_X); + motorConsEncoderType[1] = ParameterList::getInstance()->getValue(ENCODER_TYPE_Y); + motorConsEncoderType[2] = ParameterList::getInstance()->getValue(ENCODER_TYPE_Z); + + motorConsEncoderScaling[0] = ParameterList::getInstance()->getValue(ENCODER_SCALING_X); + motorConsEncoderScaling[1] = ParameterList::getInstance()->getValue(ENCODER_SCALING_Y); + motorConsEncoderScaling[2] = ParameterList::getInstance()->getValue(ENCODER_SCALING_Z); + + if (ParameterList::getInstance()->getValue(ENCODER_ENABLED_X) == 1) + { + motorConsEncoderEnabled[0] = true; + } + else + { + motorConsEncoderEnabled[0] = false; + } + + if (ParameterList::getInstance()->getValue(ENCODER_ENABLED_Y) == 1) + { + motorConsEncoderEnabled[1] = true; + } + else + { + motorConsEncoderEnabled[1] = false; + } + + if (ParameterList::getInstance()->getValue(ENCODER_ENABLED_Z) == 1) + { + motorConsEncoderEnabled[2] = true; + } + else + { + motorConsEncoderEnabled[2] = false; + } } -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; +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() { - motorMotorsEnabled = true; +void StepperControl::enableMotors() +{ + motorMotorsEnabled = true; - axisX.enableMotor(); - axisY.enableMotor(); - axisZ.enableMotor(); - delay(100); + axisX.enableMotor(); + axisY.enableMotor(); + axisZ.enableMotor(); + delay(100); } -void StepperControl::disableMotors() { - motorMotorsEnabled = false; +void StepperControl::disableMotors() +{ + motorMotorsEnabled = false; - axisX.disableMotor(); - axisY.disableMotor(); - //axisZ.disableMotor(); - delay(100); + axisX.disableMotor(); + axisY.disableMotor(); + //axisZ.disableMotor(); + delay(100); } -bool StepperControl::motorsEnabled() { - return motorMotorsEnabled; +bool StepperControl::motorsEnabled() +{ + return motorMotorsEnabled; } -bool StepperControl::endStopsReached() { - - if ( axisX.endStopsReached() || - axisY.endStopsReached() || - axisZ.endStopsReached()) { - return true; - } - return false; +bool StepperControl::endStopsReached() +{ + if (axisX.endStopsReached() || + axisY.endStopsReached() || + axisZ.endStopsReached()) + { + return true; + } + return false; } -void StepperControl::storePosition(){ - - if (motorConsEncoderEnabled[0]) { - CurrentState::getInstance()->setX(encoderX.currentPosition()); - } else { - CurrentState::getInstance()->setX(axisX.currentPosition()); - } - - if (motorConsEncoderEnabled[1]) { - CurrentState::getInstance()->setY(encoderY.currentPosition()); - } else { - CurrentState::getInstance()->setY(axisY.currentPosition()); - } - - if (motorConsEncoderEnabled[2]) { - CurrentState::getInstance()->setZ(encoderZ.currentPosition()); - } else { - CurrentState::getInstance()->setZ(axisZ.currentPosition()); - } - +void StepperControl::storePosition() +{ + + if (motorConsEncoderEnabled[0]) + { + CurrentState::getInstance()->setX(encoderX.currentPosition()); + } + else + { + CurrentState::getInstance()->setX(axisX.currentPosition()); + } + + if (motorConsEncoderEnabled[1]) + { + CurrentState::getInstance()->setY(encoderY.currentPosition()); + } + else + { + CurrentState::getInstance()->setY(axisY.currentPosition()); + } + + if (motorConsEncoderEnabled[2]) + { + CurrentState::getInstance()->setZ(encoderZ.currentPosition()); + } + else + { + CurrentState::getInstance()->setZ(axisZ.currentPosition()); + } } -void StepperControl::reportEndStops() { - CurrentState::getInstance()->printEndStops(); +void StepperControl::reportEndStops() +{ + CurrentState::getInstance()->printEndStops(); } -void StepperControl::reportPosition(){ - CurrentState::getInstance()->printPosition(); +void StepperControl::reportPosition() +{ + CurrentState::getInstance()->printPosition(); } -void StepperControl::storeEndStops() { - CurrentState::getInstance()->storeEndStops(); +void StepperControl::storeEndStops() +{ + CurrentState::getInstance()->storeEndStops(); } -void StepperControl::setPositionX(long pos) { - axisX.setCurrentPosition(pos); - encoderX.setPosition(pos); +void StepperControl::setPositionX(long pos) +{ + axisX.setCurrentPosition(pos); + encoderX.setPosition(pos); } -void StepperControl::setPositionY(long pos) { - axisY.setCurrentPosition(pos); - encoderY.setPosition(pos); +void StepperControl::setPositionY(long pos) +{ + axisY.setCurrentPosition(pos); + encoderY.setPosition(pos); } -void StepperControl::setPositionZ(long pos) { - axisZ.setCurrentPosition(pos); - encoderZ.setPosition(pos); +void StepperControl::setPositionZ(long pos) +{ + axisZ.setCurrentPosition(pos); + encoderZ.setPosition(pos); } diff --git a/src/StepperControl.h b/src/StepperControl.h index b412466..226c209 100644 --- a/src/StepperControl.h +++ b/src/StepperControl.h @@ -19,87 +19,87 @@ #include #include "Command.h" -class StepperControl { +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, - // 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 - ); - - void handleMovementInterrupt(); - int calibrateAxis(int axis); - void initInterrupt(); - void enableMotors(); - void disableMotors(); - bool motorsEnabled(); - - void storePosition(); - void loadSettings(); - - void setPositionX(long pos); - void setPositionY(long pos); - void setPositionZ(long pos); - - void test(); - void test2(); + StepperControl(); + StepperControl(StepperControl const &); + void operator=(StepperControl const &); + + static StepperControl *getInstance(); + //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); + + void handleMovementInterrupt(); + int calibrateAxis(int axis); + void initInterrupt(); + void enableMotors(); + void disableMotors(); + bool motorsEnabled(); + + void storePosition(); + void loadSettings(); + + void setPositionX(long pos); + void setPositionY(long pos); + void setPositionZ(long pos); + + void test(); + void test2(); private: - StepperControlAxis axisX; - StepperControlAxis axisY; - StepperControlAxis axisZ; - - StepperControlEncoder encoderX; - StepperControlEncoder encoderY; - StepperControlEncoder encoderZ; - - void checkAxisVsEncoder(StepperControlAxis* axis, StepperControlEncoder* encoder, float* missedSteps, long* lastPosition, float* encoderStepDecay, bool* encoderEnabled); - void checkAxisSubStatus(StepperControlAxis* axis, int* axisSubStatus); - - bool axisActive[3]; - int axisSubStep[3]; - - void loadMotorSettings(); - void loadEncoderSettings(); - bool intToBool(int value); - - void reportPosition(); - void storeEndStops(); - void reportEndStops(); - void reportStatus(StepperControlAxis* axis, int axisSubStatus); - void reportCalib(StepperControlAxis* axis, int calibStatus); - - 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 motor2Inv[3]; - bool motor2Enbl[3]; - bool endStInv[3]; - bool endStEnbl[3]; - long timeOut[3]; - - float motorConsMissedSteps[3]; - long motorLastPosition[3]; - - int motorConsMissedStepsMax[3]; - float motorConsMissedStepsDecay[3]; - bool motorConsEncoderEnabled[3]; - int motorConsEncoderType[3]; - int motorConsEncoderScaling[3]; - - bool motorMotorsEnabled; + StepperControlAxis axisX; + StepperControlAxis axisY; + StepperControlAxis axisZ; + + StepperControlEncoder encoderX; + StepperControlEncoder encoderY; + StepperControlEncoder encoderZ; + + void checkAxisVsEncoder(StepperControlAxis *axis, StepperControlEncoder *encoder, float *missedSteps, long *lastPosition, float *encoderStepDecay, bool *encoderEnabled); + void checkAxisSubStatus(StepperControlAxis *axis, int *axisSubStatus); + + bool axisActive[3]; + int axisSubStep[3]; + + void loadMotorSettings(); + void loadEncoderSettings(); + bool intToBool(int value); + + void reportPosition(); + void storeEndStops(); + void reportEndStops(); + void reportStatus(StepperControlAxis *axis, int axisSubStatus); + void reportCalib(StepperControlAxis *axis, int calibStatus); + + 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 motor2Inv[3]; + bool motor2Enbl[3]; + bool endStInv[3]; + bool endStEnbl[3]; + long timeOut[3]; + + float motorConsMissedSteps[3]; + long motorLastPosition[3]; + + int motorConsMissedStepsMax[3]; + float motorConsMissedStepsDecay[3]; + bool motorConsEncoderEnabled[3]; + int motorConsEncoderType[3]; + int motorConsEncoderScaling[3]; + + bool motorMotorsEnabled; }; #endif /* STEPPERCONTROL_H_ */ diff --git a/src/StepperControlAxis.cpp b/src/StepperControlAxis.cpp index 0bf6e3a..816130e 100644 --- a/src/StepperControlAxis.cpp +++ b/src/StepperControlAxis.cpp @@ -1,507 +1,611 @@ #include "StepperControlAxis.h" -StepperControlAxis::StepperControlAxis() { - lastCalcLog = 0; - - pinStep = 0; - pinDirection = 0; - pinEnable = 0; - - pin2Step = 0; - pin2Direction = 0; - pin2Enable = 0; - - pinMin = 0; - pinMax = 0; - - axisActive = false; - - coordSourcePoint = 0; - coordCurrentPoint = 0; - coordDestinationPoint = 0; - coordHomeAxis = 0; - - movementUp = false; - movementToHome = false; - movementAccelerating = false; - movementDecelerating = false; - movementCruising = false; - movementCrawling = false; - movementMotorActive = false; - movementMoving = false; -} - -void StepperControlAxis::test() { - Serial.print("R99"); - Serial.print(" cur loc = "); - Serial.print(coordCurrentPoint); - //Serial.print(" enc loc = "); - //Serial.print(coordEncoderPoint); - //Serial.print(" cons steps missed = "); - //Serial.print(label); - //Serial.print(consMissedSteps); - Serial.print("\r\n"); -} - -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; - - - movementAccelerating = false; - movementDecelerating = false; - movementCruising = false; - movementCrawling = false; - movementMoving = false; - - 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; - movementCrawling = true; - movementMoving = true; - } else { - if (curPos >= staPos && curPos <= halfway) { - // accelerating - if (curPos > (staPos + stepsAccDec)) { - // now beyond the accelleration point to go full speed - newSpeed = maxSpeed + 1; - movementCruising = true; - movementMoving = true; - } else { - // speeding up, increase speed linear within the first period - newSpeed = (1.0 * (curPos - staPos) / stepsAccDec * (maxSpeed - minSpeed)) + minSpeed; - movementAccelerating = true; - movementMoving = true; - } - } else { - // decelerating - if (curPos < (endPos - stepsAccDec)) { - // still before the deceleration point so keep going at full speed - newSpeed = maxSpeed + 2; - movementCruising = true; - movementMoving = true; - } else { - // speeding up, increase speed linear within the first period - newSpeed = (1.0 * (endPos - curPos) / stepsAccDec * (maxSpeed - minSpeed)) + minSpeed; - movementDecelerating = true; - movementMoving = true; - } - } - } - - - if (debugPrint && (millis() - lastCalcLog > 1000)) { - - lastCalcLog = millis(); - - Serial.print("R99"); - - 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("\r\n"); - } - - - // Return the calculated speed, in steps per second - return newSpeed; -} - -void StepperControlAxis::checkAxisDirection() { - - if (coordHomeAxis){ - // When home is active, the direction is fixed - movementUp = motorHomeIsUp; - movementToHome = true; - } else { - // For normal movement, move in direction of destination - movementUp = ( coordCurrentPoint < coordDestinationPoint ); - movementToHome = (abs(coordCurrentPoint) >= abs(coordDestinationPoint)); - } - - if (coordCurrentPoint == 0) { - // Go slow when theoretical end point reached but there is no end stop siganl - axisSpeed = motorSpeedMin; - } -} - -void StepperControlAxis::setDirectionAxis() { - - if (((!coordHomeAxis && coordCurrentPoint < coordDestinationPoint) || (coordHomeAxis && motorHomeIsUp)) ^ motorMotorInv) { - setDirectionUp(); - } else { - setDirectionDown(); - } -} - -void StepperControlAxis::checkMovement() { - - checkAxisDirection(); - - // Handle movement if destination is not already reached or surpassed - if ( - ( - (coordDestinationPoint > coordSourcePoint && coordCurrentPoint < coordDestinationPoint) || - (coordDestinationPoint < coordSourcePoint && coordCurrentPoint > coordDestinationPoint) || - coordHomeAxis - ) - && axisActive - ) { - - // home or destination not reached, keep moving - - // If end stop reached or the encoder doesn't move anymore, stop moving motor, otherwise set the timing for the next step - if ((coordHomeAxis && !endStopAxisReached(false)) || (!coordHomeAxis && !endStopAxisReached(!movementToHome))) { - - // Get the axis speed, in steps per second - axisSpeed = calculateSpeed( coordSourcePoint, coordCurrentPoint, coordDestinationPoint, - motorSpeedMin, motorSpeedMax, motorStepsAcc); - - // 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 / motorInterruptSpeed / axisSpeed / 2); - stepOffTick = moveTicks + (1000.0 * 1000.0 / motorInterruptSpeed / 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)) { - coordCurrentPoint = 0; - } -} - -void StepperControlAxis::checkTiming() { - - //int i; - - if (axisActive) { - - moveTicks++; - - if (moveTicks >= stepOffTick) { - - // Negative flank for the steps - resetMotorStep(); - checkMovement(); - } - else { - - if (moveTicks == stepOnTick) { - - // Positive flank for the steps - setStepAxis(); - } - } - } -} - -void StepperControlAxis::setStepAxis() { - - if (movementUp) { - coordCurrentPoint++; - } else { - coordCurrentPoint--; - } - - // set a step on the motors - setMotorStep(); -} - -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,int step2, int dir2, int enable2) { - pinStep = step; - pinDirection = dir; - pinEnable = enable; - - pin2Step = step2; - pin2Direction = dir2; - pin2Enable = enable2; - - pinMin = min; - pinMax = max; -} - - -void StepperControlAxis::loadMotorSettings( - long speedMax, long speedMin, long stepsAcc, long timeOut, bool homeIsUp, bool motorInv, - bool endStInv, long interruptSpeed, bool motor2Enbl,bool motor2Inv, bool endStEnbl) { - - motorSpeedMax = speedMax; - motorSpeedMin = speedMin; - motorStepsAcc = stepsAcc; - motorTimeOut = timeOut; - motorHomeIsUp = homeIsUp; - motorMotorInv = motorInv; - motorEndStopInv = endStInv; - motorEndStopEnbl = endStEnbl; - motorInterruptSpeed = interruptSpeed; - motorMotor2Enl = motor2Enbl; - motorMotor2Inv = motor2Inv; -} - -void StepperControlAxis::loadCoordinates(long sourcePoint, long destinationPoint, bool home) { - - coordSourcePoint = sourcePoint; - coordCurrentPoint = sourcePoint; - coordDestinationPoint = destinationPoint; - coordHomeAxis = home; - - // 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; +StepperControlAxis::StepperControlAxis() +{ + lastCalcLog = 0; + + pinStep = 0; + pinDirection = 0; + pinEnable = 0; + + pin2Step = 0; + pin2Direction = 0; + pin2Enable = 0; + + pinMin = 0; + pinMax = 0; + + axisActive = false; + + coordSourcePoint = 0; + coordCurrentPoint = 0; + coordDestinationPoint = 0; + coordHomeAxis = 0; + + movementUp = false; + movementToHome = false; + movementAccelerating = false; + movementDecelerating = false; + movementCruising = false; + movementCrawling = false; + movementMotorActive = false; + movementMoving = false; +} + +void StepperControlAxis::test() +{ + Serial.print("R99"); + Serial.print(" cur loc = "); + Serial.print(coordCurrentPoint); + //Serial.print(" enc loc = "); + //Serial.print(coordEncoderPoint); + //Serial.print(" cons steps missed = "); + //Serial.print(label); + //Serial.print(consMissedSteps); + Serial.print("\r\n"); +} + +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; + + movementAccelerating = false; + movementDecelerating = false; + movementCruising = false; + movementCrawling = false; + movementMoving = false; + + 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; + movementCrawling = true; + movementMoving = true; + } + else + { + if (curPos >= staPos && curPos <= halfway) + { + // accelerating + if (curPos > (staPos + stepsAccDec)) + { + // now beyond the accelleration point to go full speed + newSpeed = maxSpeed + 1; + movementCruising = true; + movementMoving = true; + } + else + { + // speeding up, increase speed linear within the first period + newSpeed = (1.0 * (curPos - staPos) / stepsAccDec * (maxSpeed - minSpeed)) + minSpeed; + movementAccelerating = true; + movementMoving = true; + } + } + else + { + // decelerating + if (curPos < (endPos - stepsAccDec)) + { + // still before the deceleration point so keep going at full speed + newSpeed = maxSpeed + 2; + movementCruising = true; + movementMoving = true; + } + else + { + // speeding up, increase speed linear within the first period + newSpeed = (1.0 * (endPos - curPos) / stepsAccDec * (maxSpeed - minSpeed)) + minSpeed; + movementDecelerating = true; + movementMoving = true; + } + } + } + + if (debugPrint && (millis() - lastCalcLog > 1000)) + { + + lastCalcLog = millis(); + + Serial.print("R99"); + + 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("\r\n"); + } + + // Return the calculated speed, in steps per second + return newSpeed; +} + +void StepperControlAxis::checkAxisDirection() +{ + + if (coordHomeAxis) + { + // When home is active, the direction is fixed + movementUp = motorHomeIsUp; + movementToHome = true; + } + else + { + // For normal movement, move in direction of destination + movementUp = (coordCurrentPoint < coordDestinationPoint); + movementToHome = (abs(coordCurrentPoint) >= abs(coordDestinationPoint)); + } + + if (coordCurrentPoint == 0) + { + // Go slow when theoretical end point reached but there is no end stop siganl + axisSpeed = motorSpeedMin; + } +} + +void StepperControlAxis::setDirectionAxis() +{ + + if (((!coordHomeAxis && coordCurrentPoint < coordDestinationPoint) || (coordHomeAxis && motorHomeIsUp)) ^ motorMotorInv) + { + setDirectionUp(); + } + else + { + setDirectionDown(); + } +} + +void StepperControlAxis::checkMovement() +{ + + checkAxisDirection(); + + // Handle movement if destination is not already reached or surpassed + if ( + ( + (coordDestinationPoint > coordSourcePoint && coordCurrentPoint < coordDestinationPoint) || + (coordDestinationPoint < coordSourcePoint && coordCurrentPoint > coordDestinationPoint) || + coordHomeAxis) && + axisActive) + { + + // home or destination not reached, keep moving + + // If end stop reached or the encoder doesn't move anymore, stop moving motor, otherwise set the timing for the next step + if ((coordHomeAxis && !endStopAxisReached(false)) || (!coordHomeAxis && !endStopAxisReached(!movementToHome))) + { + + // Get the axis speed, in steps per second + axisSpeed = calculateSpeed(coordSourcePoint, coordCurrentPoint, coordDestinationPoint, + motorSpeedMin, motorSpeedMax, motorStepsAcc); + + // 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 / motorInterruptSpeed / axisSpeed / 2); + stepOffTick = moveTicks + (1000.0 * 1000.0 / motorInterruptSpeed / 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)) + { + coordCurrentPoint = 0; + } +} + +void StepperControlAxis::checkTiming() +{ + + //int i; + + if (axisActive) + { + + moveTicks++; + + if (moveTicks >= stepOffTick) + { + + // Negative flank for the steps + resetMotorStep(); + checkMovement(); + } + else + { + + if (moveTicks == stepOnTick) + { + + // Positive flank for the steps + setStepAxis(); + } + } + } +} + +void StepperControlAxis::setStepAxis() +{ + + if (movementUp) + { + coordCurrentPoint++; + } + else + { + coordCurrentPoint--; + } + + // set a step on the motors + setMotorStep(); +} + +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, int step2, int dir2, int enable2) +{ + pinStep = step; + pinDirection = dir; + pinEnable = enable; + + pin2Step = step2; + pin2Direction = dir2; + pin2Enable = enable2; + + pinMin = min; + pinMax = max; +} + +void StepperControlAxis::loadMotorSettings( + long speedMax, long speedMin, long stepsAcc, long timeOut, bool homeIsUp, bool motorInv, + bool endStInv, long interruptSpeed, bool motor2Enbl, bool motor2Inv, bool endStEnbl) +{ + + motorSpeedMax = speedMax; + motorSpeedMin = speedMin; + motorStepsAcc = stepsAcc; + motorTimeOut = timeOut; + motorHomeIsUp = homeIsUp; + motorMotorInv = motorInv; + motorEndStopInv = endStInv; + motorEndStopEnbl = endStEnbl; + motorInterruptSpeed = interruptSpeed; + motorMotor2Enl = motor2Enbl; + motorMotor2Inv = motor2Inv; +} + +void StepperControlAxis::loadCoordinates(long sourcePoint, long destinationPoint, bool home) +{ + + coordSourcePoint = sourcePoint; + coordCurrentPoint = sourcePoint; + coordDestinationPoint = destinationPoint; + coordHomeAxis = home; + + // 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() +{ + digitalWrite(pinEnable, LOW); + if (motorMotor2Enl) + { + digitalWrite(pin2Enable, LOW); + } + movementMotorActive = true; +} + +void StepperControlAxis::disableMotor() +{ + digitalWrite(pinEnable, HIGH); + if (motorMotor2Enl) + { + digitalWrite(pin2Enable, HIGH); + } + movementMotorActive = false; +} + +void StepperControlAxis::setDirectionUp() +{ + if (motorMotorInv) + { + digitalWrite(pinDirection, LOW); + } + else + { + digitalWrite(pinDirection, HIGH); + } + + if (motorMotor2Enl && motorMotor2Inv) + { + digitalWrite(pin2Direction, LOW); + } + else + { + digitalWrite(pin2Direction, HIGH); + } +} + +void StepperControlAxis::setDirectionDown() +{ + if (motorMotorInv) + { + digitalWrite(pinDirection, HIGH); + } + else + { + digitalWrite(pinDirection, LOW); + } + + if (motorMotor2Enl && motorMotor2Inv) + { + digitalWrite(pin2Direction, HIGH); + } + else + { + digitalWrite(pin2Direction, LOW); + } +} + +void StepperControlAxis::setMovementUp() +{ + movementUp = true; +} + +void StepperControlAxis::setMovementDown() +{ + movementUp = false; +} + +void StepperControlAxis::setDirectionHome() +{ + if (motorHomeIsUp) + { + setDirectionUp(); + setMovementUp(); + } + else + { + setDirectionDown(); + setMovementDown(); + } +} + +void StepperControlAxis::setDirectionAway() +{ + if (motorHomeIsUp) + { + setDirectionDown(); + setMovementDown(); + } + else + { + setDirectionUp(); + setMovementUp(); + } } -void StepperControlAxis::enableMotor() { - digitalWrite(pinEnable, LOW); - if (motorMotor2Enl) { - digitalWrite(pin2Enable, LOW); - } - movementMotorActive = true; +unsigned long StepperControlAxis::getLength(long l1, long l2) +{ + if (l1 > l2) + { + return l1 - l2; + } + else + { + return l2 - l1; + } } -void StepperControlAxis::disableMotor() { - digitalWrite(pinEnable, HIGH); - if (motorMotor2Enl) { - digitalWrite(pin2Enable, HIGH); - } - movementMotorActive = false; +bool StepperControlAxis::endStopsReached() +{ + return ((digitalRead(pinMin) == motorEndStopInv) || (digitalRead(pinMax) == motorEndStopInv)) && motorEndStopEnbl; } -void StepperControlAxis::setDirectionUp() { - if (motorMotorInv) { - digitalWrite(pinDirection, LOW); - } else { - digitalWrite(pinDirection, HIGH); - } - - if (motorMotor2Enl && motorMotor2Inv) { - digitalWrite(pin2Direction, LOW); - } else { - digitalWrite(pin2Direction, HIGH); - } -} - -void StepperControlAxis::setDirectionDown() { - if (motorMotorInv) { - digitalWrite(pinDirection, HIGH); - } else { - digitalWrite(pinDirection, LOW); - } - - if (motorMotor2Enl && motorMotor2Inv) { - digitalWrite(pin2Direction, HIGH); - } else { - digitalWrite(pin2Direction, LOW); - } +bool StepperControlAxis::endStopMin() +{ + //return ((digitalRead(pinMin) == motorEndStopInv) || (digitalRead(pinMax) == motorEndStopInv)); + return digitalRead(pinMin) && motorEndStopEnbl; } -void StepperControlAxis::setMovementUp() { - movementUp = true; +bool StepperControlAxis::endStopMax() +{ + //return ((digitalRead(pinMin) == motorEndStopInv) || (digitalRead(pinMax) == motorEndStopInv)); + return digitalRead(pinMax) && motorEndStopEnbl; } -void StepperControlAxis::setMovementDown() { - movementUp = false; +bool StepperControlAxis::isAxisActive() +{ + return axisActive; } -void StepperControlAxis::setDirectionHome() { - if (motorHomeIsUp) { - setDirectionUp(); - setMovementUp(); - } else { - setDirectionDown(); - setMovementDown(); - } +void StepperControlAxis::deactivateAxis() +{ + axisActive = false; } -void StepperControlAxis::setDirectionAway() { - if (motorHomeIsUp) { - setDirectionDown(); - setMovementDown(); - } else { - setDirectionUp(); - setMovementUp(); - } +void StepperControlAxis::setMotorStep() +{ + digitalWrite(pinStep, HIGH); + if (pin2Enable) + { + digitalWrite(pin2Step, HIGH); + } } -unsigned long StepperControlAxis::getLength(long l1, long l2) { - if (l1 > l2) { - return l1 - l2; - } else { - return l2 - l1; - } +void StepperControlAxis::resetMotorStep() +{ + movementStepDone = true; + digitalWrite(pinStep, LOW); + if (pin2Enable) + { + digitalWrite(pin2Step, LOW); + } } -bool StepperControlAxis::endStopsReached() { - return ((digitalRead(pinMin) == motorEndStopInv) || (digitalRead(pinMax) == motorEndStopInv)) && motorEndStopEnbl; +bool StepperControlAxis::pointReached(long currentPoint, long destinationPoint) +{ + return (destinationPoint == currentPoint); } -bool StepperControlAxis::endStopMin() { - //return ((digitalRead(pinMin) == motorEndStopInv) || (digitalRead(pinMax) == motorEndStopInv)); - return digitalRead(pinMin) && motorEndStopEnbl; +long StepperControlAxis::currentPosition() +{ + return coordCurrentPoint; } -bool StepperControlAxis::endStopMax() { - //return ((digitalRead(pinMin) == motorEndStopInv) || (digitalRead(pinMax) == motorEndStopInv)); - return digitalRead(pinMax) && motorEndStopEnbl; +void StepperControlAxis::setCurrentPosition(long newPos) +{ + coordCurrentPoint = newPos; } -bool StepperControlAxis::isAxisActive() { - return axisActive; +void StepperControlAxis::setMaxSpeed(long speed) +{ + motorSpeedMax = speed; } -void StepperControlAxis::deactivateAxis() { - axisActive = false; +void StepperControlAxis::activateDebugPrint() +{ + debugPrint = true; } -void StepperControlAxis::setMotorStep() { - digitalWrite(pinStep, HIGH); - if (pin2Enable) { - digitalWrite(pin2Step, HIGH); - } +bool StepperControlAxis::isStepDone() +{ + return movementStepDone; } -void StepperControlAxis::resetMotorStep() { - movementStepDone = true; - digitalWrite(pinStep, LOW); - if (pin2Enable) { - digitalWrite(pin2Step, LOW); - } +void StepperControlAxis::resetStepDone() +{ + movementStepDone = false; } -bool StepperControlAxis::pointReached(long currentPoint, long destinationPoint) { - return (destinationPoint == currentPoint); +bool StepperControlAxis::movingToHome() +{ + return movementToHome; } -long StepperControlAxis::currentPosition() { - return coordCurrentPoint; +bool StepperControlAxis::movingUp() +{ + return movementUp; } -void StepperControlAxis::setCurrentPosition(long newPos) { - coordCurrentPoint = newPos; +bool StepperControlAxis::isAccelerating() +{ + return movementAccelerating; } -void StepperControlAxis::setMaxSpeed(long speed) { - motorSpeedMax = speed; +bool StepperControlAxis::isDecelerating() +{ + return movementDecelerating; } -void StepperControlAxis::activateDebugPrint() { - debugPrint = true; +bool StepperControlAxis::isCruising() +{ + return movementCruising; } -bool StepperControlAxis::isStepDone() { - return movementStepDone; +bool StepperControlAxis::isCrawling() +{ + return movementCrawling; } -void StepperControlAxis::resetStepDone() { - movementStepDone = false; +bool StepperControlAxis::isMotorActive() +{ + return movementMotorActive; } - -bool StepperControlAxis::movingToHome() { - return movementToHome; -} - -bool StepperControlAxis::movingUp() { - return movementUp; -} - -bool StepperControlAxis::isAccelerating() { - return movementAccelerating; -} - -bool StepperControlAxis::isDecelerating() { - return movementDecelerating; -} - -bool StepperControlAxis::isCruising() { - return movementCruising; -} - -bool StepperControlAxis::isCrawling() { - return movementCrawling; -} - -bool StepperControlAxis::isMotorActive() { - return movementMotorActive; -} - diff --git a/src/StepperControlAxis.h b/src/StepperControlAxis.h index a9f95c9..7a58148 100644 --- a/src/StepperControlAxis.h +++ b/src/StepperControlAxis.h @@ -17,130 +17,127 @@ #include #include -class StepperControlAxis { +class StepperControlAxis +{ public: + StepperControlAxis(); - StepperControlAxis(); + void loadPinNumbers(int step, int dir, int enable, int min, int max, int step2, int dir2, int enable2); + void loadMotorSettings(long speedMax, long speedMin, long stepsAcc, long timeOut, bool homeIsUp, bool motorInv, bool endStInv, long interruptSpeed, bool motor2Enbl, bool motor2Inv, bool endStEnbl); + void loadCoordinates(long sourcePoint, long destinationPoint, bool home); + void setMaxSpeed(long speed); - void loadPinNumbers(int step, int dir, int enable, int min, int max, int step2, int dir2, int enable2); - void loadMotorSettings( long speedMax, long speedMin, long stepsAcc, long timeOut, bool homeIsUp, bool motorInv, bool endStInv, long interruptSpeed, bool motor2Enbl, bool motor2Inv, bool endStEnbl); - void loadCoordinates(long sourcePoint, long destinationPoint, bool home); - void setMaxSpeed(long speed); + void enableMotor(); + void disableMotor(); + void checkMovement(); + void checkTiming(); - void enableMotor(); - void disableMotor(); - void checkMovement(); - void checkTiming(); + bool isAxisActive(); + void deactivateAxis(); + bool isAccelerating(); + bool isDecelerating(); + bool isCruising(); + bool isCrawling(); + bool isMotorActive(); + bool isMoving(); - bool isAxisActive(); - void deactivateAxis(); - bool isAccelerating(); - bool isDecelerating(); - bool isCruising(); - bool isCrawling(); - bool isMotorActive(); - bool isMoving(); + bool endStopMin(); + bool endStopMax(); + bool endStopsReached(); + bool endStopAxisReached(bool movement_forward); - bool endStopMin(); - bool endStopMax(); - bool endStopsReached(); - bool endStopAxisReached(bool movement_forward); + long currentPosition(); + void setCurrentPosition(long newPos); - long currentPosition(); - void setCurrentPosition(long newPos); + void setStepAxis(); + void setMotorStep(); + void resetMotorStep(); - void setStepAxis(); - void setMotorStep(); - void resetMotorStep(); + void setDirectionUp(); + void setDirectionDown(); + void setDirectionHome(); + void setDirectionAway(); + void setDirectionAxis(); - void setDirectionUp(); - void setDirectionDown(); - void setDirectionHome(); - void setDirectionAway(); - void setDirectionAxis(); + void setMovementUp(); + void setMovementDown(); - void setMovementUp(); - void setMovementDown(); + bool movingToHome(); + bool movingUp(); - bool movingToHome(); - bool movingUp(); + bool isStepDone(); + void resetStepDone(); - bool isStepDone(); - void resetStepDone(); + void activateDebugPrint(); + void test(); - void activateDebugPrint(); - void test(); - - char label; - bool movementStarted; + char label; + bool movementStarted; private: - - int lastCalcLog = 0; - bool debugPrint = false; - - // pin settings primary motor - int pinStep; - int pinDirection; - int pinEnable; - - // pin settings primary motor - int pin2Step; - int pin2Direction; - int pin2Enable; - - // pin settings primary motor - int pinMin; - int pinMax; - - // motor settings - bool motorEndStopInv; // invert input (true/false) of end stops - bool motorEndStopEnbl; // enable the use of the end stops - - // motor settings - 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 motor direction - bool motorMotor2Enl; // enable secondary motor - bool motorMotor2Inv; // invert secondary motor direction - long motorInterruptSpeed; // period of interrupt in micro seconds - - // coordinates - long coordSourcePoint; // all coordinated in steps - long coordCurrentPoint; - long coordDestinationPoint; - bool coordHomeAxis; // homing command - - // 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; - bool movementStepDone; - bool movementAccelerating; - bool movementDecelerating; - bool movementCruising; - bool movementCrawling; - bool movementMotorActive; - bool movementMoving; - - 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); - unsigned long getLength(long l1, long l2); - void checkAxisDirection(); - + int lastCalcLog = 0; + bool debugPrint = false; + + // pin settings primary motor + int pinStep; + int pinDirection; + int pinEnable; + + // pin settings primary motor + int pin2Step; + int pin2Direction; + int pin2Enable; + + // pin settings primary motor + int pinMin; + int pinMax; + + // motor settings + bool motorEndStopInv; // invert input (true/false) of end stops + bool motorEndStopEnbl; // enable the use of the end stops + + // motor settings + 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 motor direction + bool motorMotor2Enl; // enable secondary motor + bool motorMotor2Inv; // invert secondary motor direction + long motorInterruptSpeed; // period of interrupt in micro seconds + + // coordinates + long coordSourcePoint; // all coordinated in steps + long coordCurrentPoint; + long coordDestinationPoint; + bool coordHomeAxis; // homing command + + // 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; + bool movementStepDone; + bool movementAccelerating; + bool movementDecelerating; + bool movementCruising; + bool movementCrawling; + bool movementMotorActive; + bool movementMoving; + + 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); + unsigned long getLength(long l1, long l2); + void checkAxisDirection(); }; #endif /* STEPPERCONTROLAXIS_H_ */ - diff --git a/src/StepperControlEncoder.cpp b/src/StepperControlEncoder.cpp index 5be0e80..4cf473c 100644 --- a/src/StepperControlEncoder.cpp +++ b/src/StepperControlEncoder.cpp @@ -1,28 +1,30 @@ #include "StepperControlEncoder.h" -StepperControlEncoder::StepperControlEncoder() { - //lastCalcLog = 0; - - pinChannelA = 0; - pinChannelB = 0; - - position = 0; - encoderType = 0; // default type - scalingFactor = 100; - - curValChannelA = false; - curValChannelA = false; - prvValChannelA = false; - prvValChannelA = false; - - readChannelA = false; - readChannelAQ = false; - readChannelB = false; - readChannelBQ = false; +StepperControlEncoder::StepperControlEncoder() +{ + //lastCalcLog = 0; + + pinChannelA = 0; + pinChannelB = 0; + + position = 0; + encoderType = 0; // default type + scalingFactor = 100; + + curValChannelA = false; + curValChannelA = false; + prvValChannelA = false; + prvValChannelA = false; + + readChannelA = false; + readChannelAQ = false; + readChannelB = false; + readChannelBQ = false; } -void StepperControlEncoder::test() { -/* +void StepperControlEncoder::test() +{ + /* Serial.print("R88 "); Serial.print("position "); Serial.print(position); @@ -38,34 +40,41 @@ void StepperControlEncoder::test() { */ } -void StepperControlEncoder::loadPinNumbers(int channelA, int channelB, int channelAQ, int channelBQ) { - pinChannelA = channelA; - pinChannelB = channelB; - pinChannelAQ = channelAQ; - pinChannelBQ = channelBQ; +void StepperControlEncoder::loadPinNumbers(int channelA, int channelB, int channelAQ, int channelBQ) +{ + pinChannelA = channelA; + pinChannelB = channelB; + pinChannelAQ = channelAQ; + pinChannelBQ = channelBQ; - readChannels(); - shiftChannels(); + readChannels(); + shiftChannels(); } -void StepperControlEncoder::loadSettings(int encType, int scaling) { - encoderType = encType; - scalingFactor = scaling; +void StepperControlEncoder::loadSettings(int encType, int scaling) +{ + encoderType = encType; + scalingFactor = scaling; } -void StepperControlEncoder::setPosition(long newPosition) { - position = newPosition; +void StepperControlEncoder::setPosition(long newPosition) +{ + position = newPosition; } -long StepperControlEncoder::currentPosition() { +long StepperControlEncoder::currentPosition() +{ - // Apply scaling to the output of the encoder, except when scaling is zero or lower + // Apply scaling to the output of the encoder, except when scaling is zero or lower - if (scalingFactor == 100 || scalingFactor <= 0) { - return position; - } else { - return position * scalingFactor / 100; - } + if (scalingFactor == 100 || scalingFactor <= 0) + { + return position; + } + else + { + return position * scalingFactor / 100; + } } /* Check the encoder channels for movement accoridng to thisspecification @@ -83,69 +92,95 @@ rotation -----------------------------------------------------> */ - -void StepperControlEncoder::readEncoder() { - - // save the old values, read the new values - shiftChannels(); - readChannels(); - - int delta = 0; - - // and check for a position change - // no fancy code, just a few simple compares. sorry - if (prvValChannelA == true && curValChannelA == true && prvValChannelB == false && curValChannelB == true ) {delta++;} - if (prvValChannelA == true && curValChannelA == false && prvValChannelB == true && curValChannelB == true ) {delta++;} - if (prvValChannelA == false && curValChannelA == false && prvValChannelB == true && curValChannelB == false) {delta++;} - if (prvValChannelA == false && curValChannelA == true && prvValChannelB == false && curValChannelB == false) {delta++;} - - if (prvValChannelA == false && curValChannelA == false && prvValChannelB == false && curValChannelB == true ) {delta--;} - if (prvValChannelA == false && curValChannelA == true && prvValChannelB == true && curValChannelB == true ) {delta--;} - if (prvValChannelA == true && curValChannelA == true && prvValChannelB == true && curValChannelB == false) {delta--;} - if (prvValChannelA == true && curValChannelA == false && prvValChannelB == false && curValChannelB == false) {delta--;} - - position += delta; - +void StepperControlEncoder::readEncoder() +{ + + // save the old values, read the new values + shiftChannels(); + readChannels(); + + int delta = 0; + + // and check for a position change + // no fancy code, just a few simple compares. sorry + if (prvValChannelA == true && curValChannelA == true && prvValChannelB == false && curValChannelB == true) + { + delta++; + } + if (prvValChannelA == true && curValChannelA == false && prvValChannelB == true && curValChannelB == true) + { + delta++; + } + if (prvValChannelA == false && curValChannelA == false && prvValChannelB == true && curValChannelB == false) + { + delta++; + } + if (prvValChannelA == false && curValChannelA == true && prvValChannelB == false && curValChannelB == false) + { + delta++; + } + + if (prvValChannelA == false && curValChannelA == false && prvValChannelB == false && curValChannelB == true) + { + delta--; + } + if (prvValChannelA == false && curValChannelA == true && prvValChannelB == true && curValChannelB == true) + { + delta--; + } + if (prvValChannelA == true && curValChannelA == true && prvValChannelB == true && curValChannelB == false) + { + delta--; + } + if (prvValChannelA == true && curValChannelA == false && prvValChannelB == false && curValChannelB == false) + { + delta--; + } + + position += delta; } -void StepperControlEncoder::readChannels() { - - // read the new values from the coder - - readChannelA = digitalRead(pinChannelA); - readChannelAQ = digitalRead(pinChannelAQ); - readChannelB = digitalRead(pinChannelB); - readChannelBQ = digitalRead(pinChannelBQ); +void StepperControlEncoder::readChannels() +{ - if (encoderType == 1) { + // read the new values from the coder - // differential encoder - if ((readChannelA ^ readChannelAQ) && (readChannelB ^ readChannelBQ)) { - curValChannelA = readChannelA; - curValChannelB = readChannelB; - } - } - else { + readChannelA = digitalRead(pinChannelA); + readChannelAQ = digitalRead(pinChannelAQ); + readChannelB = digitalRead(pinChannelB); + readChannelBQ = digitalRead(pinChannelBQ); - // encoderType <= 0 - // non-differential incremental encoder - curValChannelA = readChannelA; - curValChannelB = readChannelB; - } + if (encoderType == 1) + { + // differential encoder + if ((readChannelA ^ readChannelAQ) && (readChannelB ^ readChannelBQ)) + { + curValChannelA = readChannelA; + curValChannelB = readChannelB; + } + } + else + { - //curValChannelA = readChannelA; - //curValChannelB = readChannelB; + // encoderType <= 0 + // non-differential incremental encoder + curValChannelA = readChannelA; + curValChannelB = readChannelB; + } -// curValChannelA = digitalRead(pinChannelA); -// curValChannelB = digitalRead(pinChannelB); + //curValChannelA = readChannelA; + //curValChannelB = readChannelB; + // curValChannelA = digitalRead(pinChannelA); + // curValChannelB = digitalRead(pinChannelB); } -void StepperControlEncoder::shiftChannels() { +void StepperControlEncoder::shiftChannels() +{ - // Save the current enoder status to later on compare with new values + // Save the current enoder status to later on compare with new values - prvValChannelA = curValChannelA; - prvValChannelB = curValChannelB; + prvValChannelA = curValChannelA; + prvValChannelB = curValChannelB; } diff --git a/src/StepperControlEncoder.h b/src/StepperControlEncoder.h index a3497a1..cd36ee8 100644 --- a/src/StepperControlEncoder.h +++ b/src/StepperControlEncoder.h @@ -16,48 +16,45 @@ #include #include -class StepperControlEncoder { +class StepperControlEncoder +{ public: + StepperControlEncoder(); - StepperControlEncoder(); + void loadPinNumbers(int channelA, int channelB, int channelAQ, int channelBQ); + void loadSettings(int encType, int scaling); - void loadPinNumbers(int channelA, int channelB, int channelAQ, int channelBQ); - void loadSettings(int encType, int scaling); + void setPosition(long newPosition); + long currentPosition(); - void setPosition(long newPosition); - long currentPosition(); - - void readEncoder(); - void readChannels(); - void shiftChannels(); - void test(); + void readEncoder(); + void readChannels(); + void shiftChannels(); + void test(); private: - - // pin settings - int pinChannelA; - int pinChannelAQ; - int pinChannelB; - int pinChannelBQ; - - // channels - bool prvValChannelA; - bool prvValChannelB; - bool curValChannelA; - bool curValChannelB; - - bool readChannelA; - bool readChannelAQ; - bool readChannelB; - bool readChannelBQ; - - // encoder - long position; - int scalingFactor; - int encoderType; - + // pin settings + int pinChannelA; + int pinChannelAQ; + int pinChannelB; + int pinChannelBQ; + + // channels + bool prvValChannelA; + bool prvValChannelB; + bool curValChannelA; + bool curValChannelB; + + bool readChannelA; + bool readChannelAQ; + bool readChannelB; + bool readChannelBQ; + + // encoder + long position; + int scalingFactor; + int encoderType; }; #endif /* STEPPERCONTROLENCODER_H_ */ - diff --git a/src/TimerOne.cpp b/src/TimerOne.cpp index e8c286f..fa2cc85 100644 --- a/src/TimerOne.cpp +++ b/src/TimerOne.cpp @@ -8,15 +8,15 @@ * Modified again, June 2014 by Paul Stoffregen * * This is free software. You can redistribute it and/or modify it under - * the terms of Creative Commons Attribution 3.0 United States License. - * To view a copy of this license, visit http://creativecommons.org/licenses/by/3.0/us/ + * the terms of Creative Commons Attribution 3.0 United States License. + * To view a copy of this license, visit http://creativecommons.org/licenses/by/3.0/us/ * or send a letter to Creative Commons, 171 Second Street, Suite 300, San Francisco, California, 94105, USA. * */ #include "TimerOne.h" -TimerOne Timer1; // preinstatiate +TimerOne Timer1; // preinstatiate unsigned short TimerOne::pwmPeriod = 0; unsigned char TimerOne::clockSelectBits = 0; @@ -33,11 +33,13 @@ ISR(TIMER1_OVF_vect) void ftm1_isr(void) { uint32_t sc = FTM1_SC; - #ifdef KINETISL - if (sc & 0x80) FTM1_SC = sc; - #else - if (sc & 0x80) FTM1_SC = sc & 0x7F; - #endif +#ifdef KINETISL + if (sc & 0x80) + FTM1_SC = sc; +#else + if (sc & 0x80) + FTM1_SC = sc & 0x7F; +#endif Timer1.isrCallback(); } diff --git a/src/TimerOne.h b/src/TimerOne.h index 6115d21..cd10864 100644 --- a/src/TimerOne.h +++ b/src/TimerOne.h @@ -5,11 +5,11 @@ * Modified June 2009 by Michael Polli and Jesse Tane to fix a bug in setPeriod() which caused the timer to stop * Modified April 2012 by Paul Stoffregen - portable to other AVR chips, use inline functions * Modified again, June 2014 by Paul Stoffregen - support Teensy 3.x & even more AVR chips - * + * * * This is free software. You can redistribute it and/or modify it under - * the terms of Creative Commons Attribution 3.0 United States License. - * To view a copy of this license, visit http://creativecommons.org/licenses/by/3.0/us/ + * the terms of Creative Commons Attribution 3.0 United States License. + * To view a copy of this license, visit http://creativecommons.org/licenses/by/3.0/us/ * or send a letter to Creative Commons, 171 Second Street, Suite 300, San Francisco, California, 94105, USA. * */ @@ -25,273 +25,339 @@ #include "known_16bit_timers.h" -#define TIMER1_RESOLUTION 65536UL // Timer1 is 16 bit - +#define TIMER1_RESOLUTION 65536UL // Timer1 is 16 bit // Placing nearly all the code in this .h file allows the functions to be // inlined by the compiler. In the very common case with constant values // the compiler will perform all calculations and simply write constants // to the hardware registers (for example, setPeriod). - class TimerOne { - #if defined(__AVR__) - public: - //**************************** - // Configuration - //**************************** - void initialize(unsigned long microseconds=1000000) __attribute__((always_inline)) { - TCCR1B = _BV(WGM13); // set mode as phase and frequency correct pwm, stop the timer - TCCR1A = 0; // clear control register A - setPeriod(microseconds); +public: + //**************************** + // Configuration + //**************************** + void initialize(unsigned long microseconds = 1000000) __attribute__((always_inline)) + { + TCCR1B = _BV(WGM13); // set mode as phase and frequency correct pwm, stop the timer + TCCR1A = 0; // clear control register A + setPeriod(microseconds); + } + void setPeriod(unsigned long microseconds) __attribute__((always_inline)) + { + const unsigned long cycles = (F_CPU / 2000000) * microseconds; + if (cycles < TIMER1_RESOLUTION) + { + clockSelectBits = _BV(CS10); + pwmPeriod = cycles; } - void setPeriod(unsigned long microseconds) __attribute__((always_inline)) { - const unsigned long cycles = (F_CPU / 2000000) * microseconds; - if (cycles < TIMER1_RESOLUTION) { - clockSelectBits = _BV(CS10); - pwmPeriod = cycles; - } else - if (cycles < TIMER1_RESOLUTION * 8) { - clockSelectBits = _BV(CS11); - pwmPeriod = cycles / 8; - } else - if (cycles < TIMER1_RESOLUTION * 64) { - clockSelectBits = _BV(CS11) | _BV(CS10); - pwmPeriod = cycles / 64; - } else - if (cycles < TIMER1_RESOLUTION * 256) { - clockSelectBits = _BV(CS12); - pwmPeriod = cycles / 256; - } else - if (cycles < TIMER1_RESOLUTION * 1024) { - clockSelectBits = _BV(CS12) | _BV(CS10); - pwmPeriod = cycles / 1024; - } else { - clockSelectBits = _BV(CS12) | _BV(CS10); - pwmPeriod = TIMER1_RESOLUTION - 1; - } - ICR1 = pwmPeriod; - TCCR1B = _BV(WGM13) | clockSelectBits; + else if (cycles < TIMER1_RESOLUTION * 8) + { + clockSelectBits = _BV(CS11); + pwmPeriod = cycles / 8; } - - //**************************** - // Run Control - //**************************** - void start() __attribute__((always_inline)) { - TCCR1B = 0; - TCNT1 = 0; // TODO: does this cause an undesired interrupt? - resume(); + else if (cycles < TIMER1_RESOLUTION * 64) + { + clockSelectBits = _BV(CS11) | _BV(CS10); + pwmPeriod = cycles / 64; } - void stop() __attribute__((always_inline)) { - TCCR1B = _BV(WGM13); + else if (cycles < TIMER1_RESOLUTION * 256) + { + clockSelectBits = _BV(CS12); + pwmPeriod = cycles / 256; } - void restart() __attribute__((always_inline)) { - start(); + else if (cycles < TIMER1_RESOLUTION * 1024) + { + clockSelectBits = _BV(CS12) | _BV(CS10); + pwmPeriod = cycles / 1024; } - void resume() __attribute__((always_inline)) { - TCCR1B = _BV(WGM13) | clockSelectBits; + else + { + clockSelectBits = _BV(CS12) | _BV(CS10); + pwmPeriod = TIMER1_RESOLUTION - 1; } + ICR1 = pwmPeriod; + TCCR1B = _BV(WGM13) | clockSelectBits; + } - //**************************** - // PWM outputs - //**************************** - void setPwmDuty(char pin, unsigned int duty) __attribute__((always_inline)) { - unsigned long dutyCycle = pwmPeriod; - dutyCycle *= duty; - dutyCycle >>= 10; - if (pin == TIMER1_A_PIN) OCR1A = dutyCycle; - #ifdef TIMER1_B_PIN - else if (pin == TIMER1_B_PIN) OCR1B = dutyCycle; - #endif - #ifdef TIMER1_C_PIN - else if (pin == TIMER1_C_PIN) OCR1C = dutyCycle; - #endif - } - void pwm(char pin, unsigned int duty) __attribute__((always_inline)) { - if (pin == TIMER1_A_PIN) { pinMode(TIMER1_A_PIN, OUTPUT); TCCR1A |= _BV(COM1A1); } - #ifdef TIMER1_B_PIN - else if (pin == TIMER1_B_PIN) { pinMode(TIMER1_B_PIN, OUTPUT); TCCR1A |= _BV(COM1B1); } - #endif - #ifdef TIMER1_C_PIN - else if (pin == TIMER1_C_PIN) { pinMode(TIMER1_C_PIN, OUTPUT); TCCR1A |= _BV(COM1C1); } - #endif - setPwmDuty(pin, duty); - TCCR1B = _BV(WGM13) | clockSelectBits; - } - void pwm(char pin, unsigned int duty, unsigned long microseconds) __attribute__((always_inline)) { - if (microseconds > 0) setPeriod(microseconds); - pwm(pin, duty); - } - void disablePwm(char pin) __attribute__((always_inline)) { - if (pin == TIMER1_A_PIN) TCCR1A &= ~_BV(COM1A1); - #ifdef TIMER1_B_PIN - else if (pin == TIMER1_B_PIN) TCCR1A &= ~_BV(COM1B1); - #endif - #ifdef TIMER1_C_PIN - else if (pin == TIMER1_C_PIN) TCCR1A &= ~_BV(COM1C1); - #endif - } + //**************************** + // Run Control + //**************************** + void start() __attribute__((always_inline)) + { + TCCR1B = 0; + TCNT1 = 0; // TODO: does this cause an undesired interrupt? + resume(); + } + void stop() __attribute__((always_inline)) + { + TCCR1B = _BV(WGM13); + } + void restart() __attribute__((always_inline)) + { + start(); + } + void resume() __attribute__((always_inline)) + { + TCCR1B = _BV(WGM13) | clockSelectBits; + } - //**************************** - // Interrupt Function - //**************************** - void attachInterrupt(void (*isr)()) __attribute__((always_inline)) { - isrCallback = isr; - TIMSK1 = _BV(TOIE1); + //**************************** + // PWM outputs + //**************************** + void setPwmDuty(char pin, unsigned int duty) __attribute__((always_inline)) + { + unsigned long dutyCycle = pwmPeriod; + dutyCycle *= duty; + dutyCycle >>= 10; + if (pin == TIMER1_A_PIN) + OCR1A = dutyCycle; +#ifdef TIMER1_B_PIN + else if (pin == TIMER1_B_PIN) + OCR1B = dutyCycle; +#endif +#ifdef TIMER1_C_PIN + else if (pin == TIMER1_C_PIN) + OCR1C = dutyCycle; +#endif + } + void pwm(char pin, unsigned int duty) __attribute__((always_inline)) + { + if (pin == TIMER1_A_PIN) + { + pinMode(TIMER1_A_PIN, OUTPUT); + TCCR1A |= _BV(COM1A1); } - void attachInterrupt(void (*isr)(), unsigned long microseconds) __attribute__((always_inline)) { - if(microseconds > 0) setPeriod(microseconds); - attachInterrupt(isr); +#ifdef TIMER1_B_PIN + else if (pin == TIMER1_B_PIN) + { + pinMode(TIMER1_B_PIN, OUTPUT); + TCCR1A |= _BV(COM1B1); } - void detachInterrupt() __attribute__((always_inline)) { - TIMSK1 = 0; +#endif +#ifdef TIMER1_C_PIN + else if (pin == TIMER1_C_PIN) + { + pinMode(TIMER1_C_PIN, OUTPUT); + TCCR1A |= _BV(COM1C1); } - static void (*isrCallback)(); - - private: - // properties - static unsigned short pwmPeriod; - static unsigned char clockSelectBits; - - - +#endif + setPwmDuty(pin, duty); + TCCR1B = _BV(WGM13) | clockSelectBits; + } + void pwm(char pin, unsigned int duty, unsigned long microseconds) __attribute__((always_inline)) + { + if (microseconds > 0) + setPeriod(microseconds); + pwm(pin, duty); + } + void disablePwm(char pin) __attribute__((always_inline)) + { + if (pin == TIMER1_A_PIN) + TCCR1A &= ~_BV(COM1A1); +#ifdef TIMER1_B_PIN + else if (pin == TIMER1_B_PIN) + TCCR1A &= ~_BV(COM1B1); +#endif +#ifdef TIMER1_C_PIN + else if (pin == TIMER1_C_PIN) + TCCR1A &= ~_BV(COM1C1); +#endif + } + //**************************** + // Interrupt Function + //**************************** + void attachInterrupt(void (*isr)()) __attribute__((always_inline)) + { + isrCallback = isr; + TIMSK1 = _BV(TOIE1); + } + void attachInterrupt(void (*isr)(), unsigned long microseconds) __attribute__((always_inline)) + { + if (microseconds > 0) + setPeriod(microseconds); + attachInterrupt(isr); + } + void detachInterrupt() __attribute__((always_inline)) + { + TIMSK1 = 0; + } + static void (*isrCallback)(); +private: + // properties + static unsigned short pwmPeriod; + static unsigned char clockSelectBits; #elif defined(__arm__) && defined(CORE_TEENSY) #if defined(KINETISK) #define F_TIMER F_BUS #elif defined(KINETISL) -#define F_TIMER (F_PLL/2) +#define F_TIMER (F_PLL / 2) #endif - public: - //**************************** - // Configuration - //**************************** - void initialize(unsigned long microseconds=1000000) __attribute__((always_inline)) { - setPeriod(microseconds); - } - void setPeriod(unsigned long microseconds) __attribute__((always_inline)) { - const unsigned long cycles = (F_TIMER / 2000000) * microseconds; - if (cycles < TIMER1_RESOLUTION) { - clockSelectBits = 0; - pwmPeriod = cycles; - } else - if (cycles < TIMER1_RESOLUTION * 2) { - clockSelectBits = 1; - pwmPeriod = cycles >> 1; - } else - if (cycles < TIMER1_RESOLUTION * 4) { - clockSelectBits = 2; - pwmPeriod = cycles >> 2; - } else - if (cycles < TIMER1_RESOLUTION * 8) { - clockSelectBits = 3; - pwmPeriod = cycles >> 3; - } else - if (cycles < TIMER1_RESOLUTION * 16) { - clockSelectBits = 4; - pwmPeriod = cycles >> 4; - } else - if (cycles < TIMER1_RESOLUTION * 32) { - clockSelectBits = 5; - pwmPeriod = cycles >> 5; - } else - if (cycles < TIMER1_RESOLUTION * 64) { - clockSelectBits = 6; - pwmPeriod = cycles >> 6; - } else - if (cycles < TIMER1_RESOLUTION * 128) { - clockSelectBits = 7; - pwmPeriod = cycles >> 7; - } else { - clockSelectBits = 7; - pwmPeriod = TIMER1_RESOLUTION - 1; - } - uint32_t sc = FTM1_SC; - FTM1_SC = 0; - FTM1_MOD = pwmPeriod; - FTM1_SC = FTM_SC_CLKS(1) | FTM_SC_CPWMS | clockSelectBits | (sc & FTM_SC_TOIE); +public: + //**************************** + // Configuration + //**************************** + void initialize(unsigned long microseconds = 1000000) __attribute__((always_inline)) + { + setPeriod(microseconds); + } + void setPeriod(unsigned long microseconds) __attribute__((always_inline)) + { + const unsigned long cycles = (F_TIMER / 2000000) * microseconds; + if (cycles < TIMER1_RESOLUTION) + { + clockSelectBits = 0; + pwmPeriod = cycles; } - - //**************************** - // Run Control - //**************************** - void start() __attribute__((always_inline)) { - stop(); - FTM1_CNT = 0; - resume(); + else if (cycles < TIMER1_RESOLUTION * 2) + { + clockSelectBits = 1; + pwmPeriod = cycles >> 1; } - void stop() __attribute__((always_inline)) { - FTM1_SC = FTM1_SC & (FTM_SC_TOIE | FTM_SC_CPWMS | FTM_SC_PS(7)); + else if (cycles < TIMER1_RESOLUTION * 4) + { + clockSelectBits = 2; + pwmPeriod = cycles >> 2; } - void restart() __attribute__((always_inline)) { - start(); + else if (cycles < TIMER1_RESOLUTION * 8) + { + clockSelectBits = 3; + pwmPeriod = cycles >> 3; } - void resume() __attribute__((always_inline)) { - FTM1_SC = (FTM1_SC & (FTM_SC_TOIE | FTM_SC_PS(7))) | FTM_SC_CPWMS | FTM_SC_CLKS(1); + else if (cycles < TIMER1_RESOLUTION * 16) + { + clockSelectBits = 4; + pwmPeriod = cycles >> 4; } - - //**************************** - // PWM outputs - //**************************** - void setPwmDuty(char pin, unsigned int duty) __attribute__((always_inline)) { - unsigned long dutyCycle = pwmPeriod; - dutyCycle *= duty; - dutyCycle >>= 10; - if (pin == TIMER1_A_PIN) { - FTM1_C0V = dutyCycle; - } else if (pin == TIMER1_B_PIN) { - FTM1_C1V = dutyCycle; - } + else if (cycles < TIMER1_RESOLUTION * 32) + { + clockSelectBits = 5; + pwmPeriod = cycles >> 5; } - void pwm(char pin, unsigned int duty) __attribute__((always_inline)) { - setPwmDuty(pin, duty); - if (pin == TIMER1_A_PIN) { - *portConfigRegister(TIMER1_A_PIN) = PORT_PCR_MUX(3) | PORT_PCR_DSE | PORT_PCR_SRE; - } else if (pin == TIMER1_B_PIN) { - *portConfigRegister(TIMER1_B_PIN) = PORT_PCR_MUX(3) | PORT_PCR_DSE | PORT_PCR_SRE; - } + else if (cycles < TIMER1_RESOLUTION * 64) + { + clockSelectBits = 6; + pwmPeriod = cycles >> 6; } - void pwm(char pin, unsigned int duty, unsigned long microseconds) __attribute__((always_inline)) { - if (microseconds > 0) setPeriod(microseconds); - pwm(pin, duty); + else if (cycles < TIMER1_RESOLUTION * 128) + { + clockSelectBits = 7; + pwmPeriod = cycles >> 7; } - void disablePwm(char pin) __attribute__((always_inline)) { - if (pin == TIMER1_A_PIN) { - *portConfigRegister(TIMER1_A_PIN) = 0; - } else if (pin == TIMER1_B_PIN) { - *portConfigRegister(TIMER1_B_PIN) = 0; - } + else + { + clockSelectBits = 7; + pwmPeriod = TIMER1_RESOLUTION - 1; } + uint32_t sc = FTM1_SC; + FTM1_SC = 0; + FTM1_MOD = pwmPeriod; + FTM1_SC = FTM_SC_CLKS(1) | FTM_SC_CPWMS | clockSelectBits | (sc & FTM_SC_TOIE); + } + + //**************************** + // Run Control + //**************************** + void start() __attribute__((always_inline)) + { + stop(); + FTM1_CNT = 0; + resume(); + } + void stop() __attribute__((always_inline)) + { + FTM1_SC = FTM1_SC & (FTM_SC_TOIE | FTM_SC_CPWMS | FTM_SC_PS(7)); + } + void restart() __attribute__((always_inline)) + { + start(); + } + void resume() __attribute__((always_inline)) + { + FTM1_SC = (FTM1_SC & (FTM_SC_TOIE | FTM_SC_PS(7))) | FTM_SC_CPWMS | FTM_SC_CLKS(1); + } - //**************************** - // Interrupt Function - //**************************** - void attachInterrupt(void (*isr)()) __attribute__((always_inline)) { - isrCallback = isr; - FTM1_SC |= FTM_SC_TOIE; - NVIC_ENABLE_IRQ(IRQ_FTM1); + //**************************** + // PWM outputs + //**************************** + void setPwmDuty(char pin, unsigned int duty) __attribute__((always_inline)) + { + unsigned long dutyCycle = pwmPeriod; + dutyCycle *= duty; + dutyCycle >>= 10; + if (pin == TIMER1_A_PIN) + { + FTM1_C0V = dutyCycle; + } + else if (pin == TIMER1_B_PIN) + { + FTM1_C1V = dutyCycle; + } + } + void pwm(char pin, unsigned int duty) __attribute__((always_inline)) + { + setPwmDuty(pin, duty); + if (pin == TIMER1_A_PIN) + { + *portConfigRegister(TIMER1_A_PIN) = PORT_PCR_MUX(3) | PORT_PCR_DSE | PORT_PCR_SRE; } - void attachInterrupt(void (*isr)(), unsigned long microseconds) __attribute__((always_inline)) { - if(microseconds > 0) setPeriod(microseconds); - attachInterrupt(isr); + else if (pin == TIMER1_B_PIN) + { + *portConfigRegister(TIMER1_B_PIN) = PORT_PCR_MUX(3) | PORT_PCR_DSE | PORT_PCR_SRE; } - void detachInterrupt() __attribute__((always_inline)) { - FTM1_SC &= ~FTM_SC_TOIE; - NVIC_DISABLE_IRQ(IRQ_FTM1); + } + void pwm(char pin, unsigned int duty, unsigned long microseconds) __attribute__((always_inline)) + { + if (microseconds > 0) + setPeriod(microseconds); + pwm(pin, duty); + } + void disablePwm(char pin) __attribute__((always_inline)) + { + if (pin == TIMER1_A_PIN) + { + *portConfigRegister(TIMER1_A_PIN) = 0; } - static void (*isrCallback)(); + else if (pin == TIMER1_B_PIN) + { + *portConfigRegister(TIMER1_B_PIN) = 0; + } + } + + //**************************** + // Interrupt Function + //**************************** + void attachInterrupt(void (*isr)()) __attribute__((always_inline)) + { + isrCallback = isr; + FTM1_SC |= FTM_SC_TOIE; + NVIC_ENABLE_IRQ(IRQ_FTM1); + } + void attachInterrupt(void (*isr)(), unsigned long microseconds) __attribute__((always_inline)) + { + if (microseconds > 0) + setPeriod(microseconds); + attachInterrupt(isr); + } + void detachInterrupt() __attribute__((always_inline)) + { + FTM1_SC &= ~FTM_SC_TOIE; + NVIC_DISABLE_IRQ(IRQ_FTM1); + } + static void (*isrCallback)(); - private: - // properties - static unsigned short pwmPeriod; - static unsigned char clockSelectBits; +private: + // properties + static unsigned short pwmPeriod; + static unsigned char clockSelectBits; #undef F_TIMER @@ -301,4 +367,3 @@ class TimerOne extern TimerOne Timer1; #endif - diff --git a/src/farmbot_arduino_controller.cpp b/src/farmbot_arduino_controller.cpp index f89527c..dd4b851 100644 --- a/src/farmbot_arduino_controller.cpp +++ b/src/farmbot_arduino_controller.cpp @@ -8,9 +8,8 @@ #include "TimerOne.h" #include "MemoryFree.h" - static char commandEndChar = 0x0A; -static GCodeProcessor* gCodeProcessor = new GCodeProcessor(); +static GCodeProcessor *gCodeProcessor = new GCodeProcessor(); unsigned long lastAction; unsigned long currentTime; @@ -25,9 +24,10 @@ int incomingCommandPointer = 0; // Blink led routine used for testing bool blink = false; -void blinkLed() { - blink = !blink; - digitalWrite(LED_PIN,blink); +void blinkLed() +{ + blink = !blink; + digitalWrite(LED_PIN, blink); } // Interrupt handling for: @@ -37,208 +37,218 @@ void blinkLed() { // bool interruptBusy = false; int interruptSecondTimer = 0; -void interrupt(void) { - interruptSecondTimer++; - - if (interruptBusy == false) { - - interruptBusy = true; - StepperControl::getInstance()->handleMovementInterrupt(); - - // Check the actions triggered once per second - if (interruptSecondTimer >= 1000000 / MOVEMENT_INTERRUPT_SPEED) { - interruptSecondTimer = 0; - PinGuard::getInstance()->checkPins(); - //blinkLed(); - } - - interruptBusy = false; - } +void interrupt(void) +{ + interruptSecondTimer++; + + if (interruptBusy == false) + { + + interruptBusy = true; + StepperControl::getInstance()->handleMovementInterrupt(); + + // Check the actions triggered once per second + if (interruptSecondTimer >= 1000000 / MOVEMENT_INTERRUPT_SPEED) + { + interruptSecondTimer = 0; + PinGuard::getInstance()->checkPins(); + //blinkLed(); + } + + interruptBusy = false; + } } - //The setup function is called once at startup of the sketch -void setup() { - - // Setup pin input/output settings - pinMode(X_STEP_PIN , OUTPUT); - pinMode(X_DIR_PIN , OUTPUT); - pinMode(X_ENABLE_PIN, OUTPUT); - pinMode(E_STEP_PIN , OUTPUT); - pinMode(E_DIR_PIN , OUTPUT); - pinMode(E_ENABLE_PIN, OUTPUT); - pinMode(X_MIN_PIN , INPUT_PULLUP ); - pinMode(X_MAX_PIN , INPUT_PULLUP ); - - pinMode(Y_STEP_PIN , OUTPUT); - pinMode(Y_DIR_PIN , OUTPUT); - pinMode(Y_ENABLE_PIN, OUTPUT); - pinMode(Y_MIN_PIN , INPUT_PULLUP ); - pinMode(Y_MAX_PIN , INPUT_PULLUP ); - - pinMode(Z_STEP_PIN , OUTPUT); - pinMode(Z_DIR_PIN , OUTPUT); - pinMode(Z_ENABLE_PIN, OUTPUT); - pinMode(Z_MIN_PIN , INPUT_PULLUP ); - pinMode(Z_MAX_PIN , INPUT_PULLUP ); - - pinMode(HEATER_0_PIN, OUTPUT); - pinMode(HEATER_1_PIN, OUTPUT); - pinMode(FAN_PIN , OUTPUT); - pinMode(LED_PIN , OUTPUT); - - //pinMode(SERVO_0_PIN , OUTPUT); - //pinMode(SERVO_1_PIN , OUTPUT); - - digitalWrite(X_ENABLE_PIN, HIGH); - digitalWrite(E_ENABLE_PIN, HIGH); - digitalWrite(Y_ENABLE_PIN, HIGH); - digitalWrite(Z_ENABLE_PIN, HIGH); - - Serial.begin(115200); - - delay(100); - - // Start the motor handling - //ServoControl::getInstance()->attach(); - - // Dump all values to the serial interface - ParameterList::getInstance()->readAllValues(); - - // Get the settings for the pin guard - PinGuard::getInstance()->loadConfig(); - - // Start the interrupt used for moving - // Interrupt management code library written by Paul Stoffregen - // The default time 100 micro seconds - - Timer1.attachInterrupt(interrupt); - Timer1.initialize(MOVEMENT_INTERRUPT_SPEED); - Timer1.start(); - - // Initialize the inactivity check - lastAction = millis(); +void setup() +{ + + // Setup pin input/output settings + pinMode(X_STEP_PIN, OUTPUT); + pinMode(X_DIR_PIN, OUTPUT); + pinMode(X_ENABLE_PIN, OUTPUT); + pinMode(E_STEP_PIN, OUTPUT); + pinMode(E_DIR_PIN, OUTPUT); + pinMode(E_ENABLE_PIN, OUTPUT); + pinMode(X_MIN_PIN, INPUT_PULLUP); + pinMode(X_MAX_PIN, INPUT_PULLUP); + + pinMode(Y_STEP_PIN, OUTPUT); + pinMode(Y_DIR_PIN, OUTPUT); + pinMode(Y_ENABLE_PIN, OUTPUT); + pinMode(Y_MIN_PIN, INPUT_PULLUP); + pinMode(Y_MAX_PIN, INPUT_PULLUP); + + pinMode(Z_STEP_PIN, OUTPUT); + pinMode(Z_DIR_PIN, OUTPUT); + pinMode(Z_ENABLE_PIN, OUTPUT); + pinMode(Z_MIN_PIN, INPUT_PULLUP); + pinMode(Z_MAX_PIN, INPUT_PULLUP); + + pinMode(HEATER_0_PIN, OUTPUT); + pinMode(HEATER_1_PIN, OUTPUT); + pinMode(FAN_PIN, OUTPUT); + pinMode(LED_PIN, OUTPUT); + + //pinMode(SERVO_0_PIN , OUTPUT); + //pinMode(SERVO_1_PIN , OUTPUT); + + digitalWrite(X_ENABLE_PIN, HIGH); + digitalWrite(E_ENABLE_PIN, HIGH); + digitalWrite(Y_ENABLE_PIN, HIGH); + digitalWrite(Z_ENABLE_PIN, HIGH); + + Serial.begin(115200); + + delay(100); + + // Start the motor handling + //ServoControl::getInstance()->attach(); + + // Dump all values to the serial interface + ParameterList::getInstance()->readAllValues(); + + // Get the settings for the pin guard + PinGuard::getInstance()->loadConfig(); + + // Start the interrupt used for moving + // Interrupt management code library written by Paul Stoffregen + // The default time 100 micro seconds + + Timer1.attachInterrupt(interrupt); + Timer1.initialize(MOVEMENT_INTERRUPT_SPEED); + Timer1.start(); + + // Initialize the inactivity check + lastAction = millis(); } // The loop function is called in an endless loop -void loop() { - - if (Serial.available()) { - - // Save current time stamp for timeout actions - lastAction = millis(); - - // Get the input and start processing on receiving 'new line' - incomingChar = Serial.read(); - incomingCommandArray[incomingCommandPointer] = incomingChar; - incomingCommandPointer++; - - // If the string is getting to long, cap it off with a new line and let it process anyway - if (incomingCommandPointer >= INCOMING_CMD_BUF_SIZE - 1) { - incomingChar = '\n'; - incomingCommandArray[incomingCommandPointer] = incomingChar; - incomingCommandPointer++; - } - - if (incomingChar == '\n' || incomingCommandPointer >= INCOMING_CMD_BUF_SIZE) { - - //commandString += incomingChar; - //String commandString = Serial.readStringUntil(commandEndChar); - //char commandChar[currentCommand.length()]; - //currentCommand.toCharArray(commandChar, currentCommand.length()); - - char commandChar[incomingCommandPointer + 1]; - for (int i = 0; i < incomingCommandPointer -1; i++) { - commandChar[i] = incomingCommandArray[i]; - } - commandChar[incomingCommandPointer] = 0; - - if (incomingCommandPointer > 1) { - - - // Copy the command to another string object. - // because there are issues with passing the - // string to the command object - - // Create a command and let it execute - //Command* command = new Command(commandString); - Command* command = new Command(commandChar); - - if(LOGGING) { - command->print(); - } - - gCodeProcessor->execute(command); - - free(command); - } +void loop() +{ + + if (Serial.available()) + { + + // Save current time stamp for timeout actions + lastAction = millis(); + + // Get the input and start processing on receiving 'new line' + incomingChar = Serial.read(); + incomingCommandArray[incomingCommandPointer] = incomingChar; + incomingCommandPointer++; + + // If the string is getting to long, cap it off with a new line and let it process anyway + if (incomingCommandPointer >= INCOMING_CMD_BUF_SIZE - 1) + { + incomingChar = '\n'; + incomingCommandArray[incomingCommandPointer] = incomingChar; + incomingCommandPointer++; + } + + if (incomingChar == '\n' || incomingCommandPointer >= INCOMING_CMD_BUF_SIZE) + { + + //commandString += incomingChar; + //String commandString = Serial.readStringUntil(commandEndChar); + //char commandChar[currentCommand.length()]; + //currentCommand.toCharArray(commandChar, currentCommand.length()); + + char commandChar[incomingCommandPointer + 1]; + for (int i = 0; i < incomingCommandPointer - 1; i++) + { + commandChar[i] = incomingCommandArray[i]; + } + commandChar[incomingCommandPointer] = 0; + + if (incomingCommandPointer > 1) + { + + // Copy the command to another string object. + // because there are issues with passing the + // string to the command object + + // Create a command and let it execute + //Command* command = new Command(commandString); + Command *command = new Command(commandChar); + + if (LOGGING) + { + command->print(); + } - incomingCommandPointer = 0; - } - } + gCodeProcessor->execute(command); - //StepperControl::getInstance()->test(); + free(command); + } - // Check if parameters are changes, and if so load the new settings + incomingCommandPointer = 0; + } + } - if (lastParamChangeNr != ParameterList::getInstance()->paramChangeNumber()) { - lastParamChangeNr = ParameterList::getInstance()->paramChangeNumber(); + //StepperControl::getInstance()->test(); - Serial.print(COMM_REPORT_COMMENT); - Serial.print(" loading parameters "); - CurrentState::getInstance()->printQAndNewLine(); + // Check if parameters are changes, and if so load the new settings - StepperControl::getInstance()->loadSettings(); - PinGuard::getInstance()->loadConfig(); - } + if (lastParamChangeNr != ParameterList::getInstance()->paramChangeNumber()) + { + lastParamChangeNr = ParameterList::getInstance()->paramChangeNumber(); + Serial.print(COMM_REPORT_COMMENT); + Serial.print(" loading parameters "); + CurrentState::getInstance()->printQAndNewLine(); - // Do periodic checks and feedback + StepperControl::getInstance()->loadSettings(); + PinGuard::getInstance()->loadConfig(); + } - currentTime = millis(); - if (currentTime < lastAction) { + // Do periodic checks and feedback - // If the device timer overruns, reset the idle counter - lastAction = millis(); - } - else { + currentTime = millis(); + if (currentTime < lastAction) + { - if ((currentTime - lastAction) > 5000) { - // After an idle time, send the idle message + // If the device timer overruns, reset the idle counter + lastAction = millis(); + } + else + { - Serial.print(COMM_REPORT_CMD_IDLE); - CurrentState::getInstance()->printQAndNewLine(); + if ((currentTime - lastAction) > 5000) + { + // After an idle time, send the idle message - StepperControl::getInstance()->storePosition(); - CurrentState::getInstance()->printPosition(); + Serial.print(COMM_REPORT_CMD_IDLE); + CurrentState::getInstance()->printQAndNewLine(); - CurrentState::getInstance()->storeEndStops(); - CurrentState::getInstance()->printEndStops(); + StepperControl::getInstance()->storePosition(); + CurrentState::getInstance()->printPosition(); - Serial.print(COMM_REPORT_COMMENT); - Serial.print(" MEM "); - Serial.print(freeMemory()); - CurrentState::getInstance()->printQAndNewLine(); + CurrentState::getInstance()->storeEndStops(); + CurrentState::getInstance()->printEndStops(); - Serial.print(COMM_REPORT_COMMENT); - Serial.print(" Cycle "); - Serial.print(cycleCounter); - CurrentState::getInstance()->printQAndNewLine(); + Serial.print(COMM_REPORT_COMMENT); + Serial.print(" MEM "); + Serial.print(freeMemory()); + CurrentState::getInstance()->printQAndNewLine(); - StepperControl::getInstance()->test(); + Serial.print(COMM_REPORT_COMMENT); + Serial.print(" Cycle "); + Serial.print(cycleCounter); + CurrentState::getInstance()->printQAndNewLine(); - //ParameterList::getInstance()->readAllValues(); + StepperControl::getInstance()->test(); + //ParameterList::getInstance()->readAllValues(); - //StepperControl::getInstance()->test(); + //StepperControl::getInstance()->test(); -// if (ParameterList::getInstance()->getValue(PARAM_CONFIG_OK) != 1) { -// Serial.print(COMM_REPORT_NO_CONFIG); -// } + // if (ParameterList::getInstance()->getValue(PARAM_CONFIG_OK) != 1) { + // Serial.print(COMM_REPORT_NO_CONFIG); + // } - cycleCounter++; - lastAction = millis(); - } - } + cycleCounter++; + lastAction = millis(); + } + } } - diff --git a/src/farmbot_arduino_controller.h b/src/farmbot_arduino_controller.h index ad82acf..426026b 100644 --- a/src/farmbot_arduino_controller.h +++ b/src/farmbot_arduino_controller.h @@ -25,6 +25,5 @@ void setup(); - //Do not add code below this line #endif /* _farmbot_arduino_controller_H_ */ diff --git a/src/known_16bit_timers.h b/src/known_16bit_timers.h index 397381d..575ebd8 100644 --- a/src/known_16bit_timers.h +++ b/src/known_16bit_timers.h @@ -4,139 +4,139 @@ // Wiring-S // #if defined(__AVR_ATmega644P__) && defined(WIRING) - #define TIMER1_A_PIN 5 - #define TIMER1_B_PIN 4 - #define TIMER1_ICP_PIN 6 +#define TIMER1_A_PIN 5 +#define TIMER1_B_PIN 4 +#define TIMER1_ICP_PIN 6 // Teensy 2.0 // #elif defined(__AVR_ATmega32U4__) && defined(CORE_TEENSY) - #define TIMER1_A_PIN 14 - #define TIMER1_B_PIN 15 - #define TIMER1_C_PIN 4 - #define TIMER1_ICP_PIN 22 - #define TIMER1_CLK_PIN 11 - #define TIMER3_A_PIN 9 - #define TIMER3_ICP_PIN 10 +#define TIMER1_A_PIN 14 +#define TIMER1_B_PIN 15 +#define TIMER1_C_PIN 4 +#define TIMER1_ICP_PIN 22 +#define TIMER1_CLK_PIN 11 +#define TIMER3_A_PIN 9 +#define TIMER3_ICP_PIN 10 // Teensy++ 2.0 #elif defined(__AVR_AT90USB1286__) && defined(CORE_TEENSY) - #define TIMER1_A_PIN 25 - #define TIMER1_B_PIN 26 - #define TIMER1_C_PIN 27 - #define TIMER1_ICP_PIN 4 - #define TIMER1_CLK_PIN 6 - #define TIMER3_A_PIN 16 - #define TIMER3_B_PIN 15 - #define TIMER3_C_PIN 14 - #define TIMER3_ICP_PIN 17 - #define TIMER3_CLK_PIN 13 +#define TIMER1_A_PIN 25 +#define TIMER1_B_PIN 26 +#define TIMER1_C_PIN 27 +#define TIMER1_ICP_PIN 4 +#define TIMER1_CLK_PIN 6 +#define TIMER3_A_PIN 16 +#define TIMER3_B_PIN 15 +#define TIMER3_C_PIN 14 +#define TIMER3_ICP_PIN 17 +#define TIMER3_CLK_PIN 13 // Teensy 3.0 // #elif defined(__MK20DX128__) - #define TIMER1_A_PIN 3 - #define TIMER1_B_PIN 4 - #define TIMER1_ICP_PIN 4 +#define TIMER1_A_PIN 3 +#define TIMER1_B_PIN 4 +#define TIMER1_ICP_PIN 4 // Teensy 3.1 // #elif defined(__MK20DX256__) - #define TIMER1_A_PIN 3 - #define TIMER1_B_PIN 4 - #define TIMER1_ICP_PIN 4 - #define TIMER3_A_PIN 32 - #define TIMER3_B_PIN 25 - #define TIMER3_ICP_PIN 32 +#define TIMER1_A_PIN 3 +#define TIMER1_B_PIN 4 +#define TIMER1_ICP_PIN 4 +#define TIMER3_A_PIN 32 +#define TIMER3_B_PIN 25 +#define TIMER3_ICP_PIN 32 // Teensy-LC // #elif defined(__MKL26Z64__) - #define TIMER1_A_PIN 16 - #define TIMER1_B_PIN 17 - #define TIMER1_ICP_PIN 17 - #define TIMER3_A_PIN 3 - #define TIMER3_B_PIN 4 - #define TIMER3_ICP_PIN 4 +#define TIMER1_A_PIN 16 +#define TIMER1_B_PIN 17 +#define TIMER1_ICP_PIN 17 +#define TIMER3_A_PIN 3 +#define TIMER3_B_PIN 4 +#define TIMER3_ICP_PIN 4 // Arduino Mega // #elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) - #define TIMER1_A_PIN 11 - #define TIMER1_B_PIN 12 - #define TIMER1_C_PIN 13 - #define TIMER3_A_PIN 5 - #define TIMER3_B_PIN 2 - #define TIMER3_C_PIN 3 - #define TIMER4_A_PIN 6 - #define TIMER4_B_PIN 7 - #define TIMER4_C_PIN 8 - #define TIMER4_ICP_PIN 49 - #define TIMER5_A_PIN 46 - #define TIMER5_B_PIN 45 - #define TIMER5_C_PIN 44 - #define TIMER3_ICP_PIN 48 - #define TIMER3_CLK_PIN 47 +#define TIMER1_A_PIN 11 +#define TIMER1_B_PIN 12 +#define TIMER1_C_PIN 13 +#define TIMER3_A_PIN 5 +#define TIMER3_B_PIN 2 +#define TIMER3_C_PIN 3 +#define TIMER4_A_PIN 6 +#define TIMER4_B_PIN 7 +#define TIMER4_C_PIN 8 +#define TIMER4_ICP_PIN 49 +#define TIMER5_A_PIN 46 +#define TIMER5_B_PIN 45 +#define TIMER5_C_PIN 44 +#define TIMER3_ICP_PIN 48 +#define TIMER3_CLK_PIN 47 // Arduino Leonardo, Yun, etc // #elif defined(__AVR_ATmega32U4__) - #define TIMER1_A_PIN 9 - #define TIMER1_B_PIN 10 - #define TIMER1_C_PIN 11 - #define TIMER1_ICP_PIN 4 - #define TIMER1_CLK_PIN 12 - #define TIMER3_A_PIN 5 - #define TIMER3_ICP_PIN 13 +#define TIMER1_A_PIN 9 +#define TIMER1_B_PIN 10 +#define TIMER1_C_PIN 11 +#define TIMER1_ICP_PIN 4 +#define TIMER1_CLK_PIN 12 +#define TIMER3_A_PIN 5 +#define TIMER3_ICP_PIN 13 // Uno, Duemilanove, LilyPad, etc // -#elif defined (__AVR_ATmega168__) || defined (__AVR_ATmega328P__) - #define TIMER1_A_PIN 9 - #define TIMER1_B_PIN 10 - #define TIMER1_ICP_PIN 8 - #define TIMER1_CLK_PIN 5 +#elif defined(__AVR_ATmega168__) || defined(__AVR_ATmega328P__) +#define TIMER1_A_PIN 9 +#define TIMER1_B_PIN 10 +#define TIMER1_ICP_PIN 8 +#define TIMER1_CLK_PIN 5 // Sanguino // #elif defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__) - #define TIMER1_A_PIN 13 - #define TIMER1_B_PIN 12 - #define TIMER1_ICP_PIN 14 - #define TIMER1_CLK_PIN 1 +#define TIMER1_A_PIN 13 +#define TIMER1_B_PIN 12 +#define TIMER1_ICP_PIN 14 +#define TIMER1_CLK_PIN 1 // Wildfire - Wicked Devices // #elif defined(__AVR_ATmega1284P__) && defined(WILDFIRE_VERSION) && WILDFIRE_VERSION >= 3 - #define TIMER1_A_PIN 5 // PD5 - #define TIMER1_B_PIN 8 // PD4 - #define TIMER1_ICP_PIN 6 // PD6 - #define TIMER1_CLK_PIN 23 // PB1 - #define TIMER3_A_PIN 12 // PB6 - #define TIMER3_B_PIN 13 // PB7 - #define TIMER3_ICP_PIN 9 // PB5 - #define TIMER3_CLK_PIN 0 // PD0 +#define TIMER1_A_PIN 5 // PD5 +#define TIMER1_B_PIN 8 // PD4 +#define TIMER1_ICP_PIN 6 // PD6 +#define TIMER1_CLK_PIN 23 // PB1 +#define TIMER3_A_PIN 12 // PB6 +#define TIMER3_B_PIN 13 // PB7 +#define TIMER3_ICP_PIN 9 // PB5 +#define TIMER3_CLK_PIN 0 // PD0 #elif defined(__AVR_ATmega1284P__) && defined(WILDFIRE_VERSION) && WILDFIRE_VERSION < 3 - #define TIMER1_A_PIN 5 // PD5 - #define TIMER1_B_PIN 4 // PD4 - #define TIMER1_ICP_PIN 6 // PD6 - #define TIMER1_CLK_PIN 15 // PB1 - #define TIMER3_A_PIN 12 // PB6 - #define TIMER3_B_PIN 13 // PB7 - #define TIMER3_ICP_PIN 11 // PB5 - #define TIMER3_CLK_PIN 0 // PD0 +#define TIMER1_A_PIN 5 // PD5 +#define TIMER1_B_PIN 4 // PD4 +#define TIMER1_ICP_PIN 6 // PD6 +#define TIMER1_CLK_PIN 15 // PB1 +#define TIMER3_A_PIN 12 // PB6 +#define TIMER3_B_PIN 13 // PB7 +#define TIMER3_ICP_PIN 11 // PB5 +#define TIMER3_CLK_PIN 0 // PD0 // Mighty-1284 - Maniacbug // #elif defined(__AVR_ATmega1284P__) - #define TIMER1_A_PIN 12 // PD5 - #define TIMER1_B_PIN 13 // PD4 - #define TIMER1_ICP_PIN 14 // PD6 - #define TIMER1_CLK_PIN 1 // PB1 - #define TIMER3_A_PIN 6 // PB6 - #define TIMER3_B_PIN 7 // PB7 - #define TIMER3_ICP_PIN 5 // PB5 - #define TIMER3_CLK_PIN 8 // PD0 +#define TIMER1_A_PIN 12 // PD5 +#define TIMER1_B_PIN 13 // PD4 +#define TIMER1_ICP_PIN 14 // PD6 +#define TIMER1_CLK_PIN 1 // PB1 +#define TIMER3_A_PIN 6 // PB6 +#define TIMER3_B_PIN 7 // PB7 +#define TIMER3_ICP_PIN 5 // PB5 +#define TIMER3_CLK_PIN 8 // PD0 #endif diff --git a/src/pins.h b/src/pins.h index 84800f6..318673c 100644 --- a/src/pins.h +++ b/src/pins.h @@ -1,52 +1,52 @@ // For RAMPS 1.4 -#define X_STEP_PIN 54 -#define X_DIR_PIN 55 -#define X_ENABLE_PIN 38 -#define X_MIN_PIN 3 -#define X_MAX_PIN 2 -#define X_ENCDR_A 16 -#define X_ENCDR_B 17 -#define X_ENCDR_A_Q 31 -#define X_ENCDR_B_Q 33 - -#define Y_STEP_PIN 60 -#define Y_DIR_PIN 61 -#define Y_ENABLE_PIN 56 -#define Y_MIN_PIN 14 -#define Y_MAX_PIN 15 -#define Y_ENCDR_A 23 -#define Y_ENCDR_B 25 -#define Y_ENCDR_A_Q 35 -#define Y_ENCDR_B_Q 37 - -#define Z_STEP_PIN 46 -#define Z_DIR_PIN 48 -#define Z_ENABLE_PIN 62 -#define Z_MIN_PIN 18 -#define Z_MAX_PIN 19 -#define Z_ENCDR_A 27 -#define Z_ENCDR_B 29 -#define Z_ENCDR_A_Q 39 -#define Z_ENCDR_B_Q 41 - -#define E_STEP_PIN 26 -#define E_DIR_PIN 28 -#define E_ENABLE_PIN 24 - -#define SDPOWER -1 -#define SDSS 53 -#define LED_PIN 13 - -#define FAN_PIN 9 - -#define PS_ON_PIN 12 -#define KILL_PIN -1 - -#define HEATER_0_PIN 10 -#define HEATER_1_PIN 8 -#define TEMP_0_PIN 13 // ANALOG NUMBERING -#define TEMP_1_PIN 14 // ANALOG NUMBERING - -#define SERVO_0_PIN 4 -#define SERVO_1_PIN 5 +#define X_STEP_PIN 54 +#define X_DIR_PIN 55 +#define X_ENABLE_PIN 38 +#define X_MIN_PIN 3 +#define X_MAX_PIN 2 +#define X_ENCDR_A 16 +#define X_ENCDR_B 17 +#define X_ENCDR_A_Q 31 +#define X_ENCDR_B_Q 33 + +#define Y_STEP_PIN 60 +#define Y_DIR_PIN 61 +#define Y_ENABLE_PIN 56 +#define Y_MIN_PIN 14 +#define Y_MAX_PIN 15 +#define Y_ENCDR_A 23 +#define Y_ENCDR_B 25 +#define Y_ENCDR_A_Q 35 +#define Y_ENCDR_B_Q 37 + +#define Z_STEP_PIN 46 +#define Z_DIR_PIN 48 +#define Z_ENABLE_PIN 62 +#define Z_MIN_PIN 18 +#define Z_MAX_PIN 19 +#define Z_ENCDR_A 27 +#define Z_ENCDR_B 29 +#define Z_ENCDR_A_Q 39 +#define Z_ENCDR_B_Q 41 + +#define E_STEP_PIN 26 +#define E_DIR_PIN 28 +#define E_ENABLE_PIN 24 + +#define SDPOWER -1 +#define SDSS 53 +#define LED_PIN 13 + +#define FAN_PIN 9 + +#define PS_ON_PIN 12 +#define KILL_PIN -1 + +#define HEATER_0_PIN 10 +#define HEATER_1_PIN 8 +#define TEMP_0_PIN 13 // ANALOG NUMBERING +#define TEMP_1_PIN 14 // ANALOG NUMBERING + +#define SERVO_0_PIN 4 +#define SERVO_1_PIN 5 diff --git a/src/src.ino b/src/src.ino index b02cbea..6d044d8 100644 --- a/src/src.ino +++ b/src/src.ino @@ -1 +1 @@ -// All code in farmbot_arduino_controller.cpp \ No newline at end of file +// All code in farmbot_arduino_controller.cpp diff --git a/src/src.vcxproj b/src/src.vcxproj index 31dcea8..40a53cf 100644 --- a/src/src.vcxproj +++ b/src/src.vcxproj @@ -173,4 +173,4 @@ - \ No newline at end of file + diff --git a/src/src.vcxproj.filters b/src/src.vcxproj.filters index a7ab21b..44e63fc 100644 --- a/src/src.vcxproj.filters +++ b/src/src.vcxproj.filters @@ -261,4 +261,4 @@ Header Files - \ No newline at end of file +