diff --git a/.gitignore b/.gitignore index e0ed02d..3f2452e 100644 --- a/.gitignore +++ b/.gitignore @@ -29,9 +29,11 @@ # Visual Studio files /.vs /src/Debug +/src/Release /src/__vm # Setting files Debug.h +/__vm _build bin diff --git a/CppProperties.json b/CppProperties.json new file mode 100644 index 0000000..ba79286 --- /dev/null +++ b/CppProperties.json @@ -0,0 +1,21 @@ +{ + "configurations": [ + { + "inheritEnvironments": [ + "msvc_x86" + ], + "name": "x86-Release", + "includePath": [ + "${env.INCLUDE}", + "${workspaceRoot}\\**" + ], + "defines": [ + "WIN32", + "NDEBUG", + "UNICODE", + "_UNICODE" + ], + "intelliSenseMode": "windows-msvc-x86" + } + ] +} \ No newline at end of file diff --git a/Makefile b/Makefile index 147847d..40d6fcc 100644 --- a/Makefile +++ b/Makefile @@ -60,9 +60,9 @@ strings_test: all $(OBJ_COPY) -I ihex $(TARGET_ramps_v14_HEX) -O binary $(TARGET_ramps_v14_HEX).bin $(OBJ_COPY) -I ihex $(TARGET_farmduino_v10_HEX) -O binary $(TARGET_farmduino_v10_HEX).bin $(OBJ_COPY) -I ihex $(TARGET_farmduino_k14_HEX) -O binary $(TARGET_farmduino_k14_HEX).bin - @strings $(TARGET_ramps_v14_HEX).bin | grep -q "6.4.0.R" - @strings $(TARGET_farmduino_v10_HEX).bin | grep -q "6.4.0.F" - @strings $(TARGET_farmduino_k14_HEX).bin | grep -q "6.4.0.G" + @strings $(TARGET_ramps_v14_HEX).bin | grep -q "6.4.0.R" + @strings $(TARGET_farmduino_v10_HEX).bin | grep -q "6.4.0.F" + @strings $(TARGET_farmduino_k14_HEX).bin | grep -q "6.4.0.G" force_clean: $(RM) -r $(BUILD_DIR) $(BIN_DIR) diff --git a/src/Board.h b/src/Board.h index 7ef76df..4c1de87 100644 --- a/src/Board.h +++ b/src/Board.h @@ -2,13 +2,15 @@ //#define RAMPS_V14 //#define FARMDUINO_V10 - #define FARMDUINO_V14 + //#define FARMDUINO_V14 + #define FARMDUINO_EXP_V20 #else #undef RAMPS_V14 #undef FARMDUINO_V10 #undef FARMDUINO_V14 + #undef FARMDUINO_EXP_V20 #if FARMBOT_BOARD_ID == 0 #define RAMPS_V14 @@ -16,6 +18,8 @@ #define FARMDUINO_V10 #elif FARMBOT_BOARD_ID == 2 #define FARMDUINO_V14 + #elif FARMBOT_BOARD_ID == 3 + #define FARMDUINO_EXP_V20 #endif #endif diff --git a/src/Config.h b/src/Config.h index eb73fbd..0738aba 100644 --- a/src/Config.h +++ b/src/Config.h @@ -13,6 +13,8 @@ const int INCOMING_CMD_BUF_SIZE = 100; + const char SPACE[2] = { ' ', '\0' }; + const char CRLF[3] = { '\r', '\n', '\0' }; 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'}; @@ -141,6 +143,21 @@ const long MOVEMENT_STOP_AT_MAX_Y_DEFAULT = 0; const long MOVEMENT_STOP_AT_MAX_Z_DEFAULT = 0; + // motor current (used with TMC2130) + const long MOVEMENT_MOTOR_CURRENT_X_DEFAULT = 600; + const long MOVEMENT_MOTOR_CURRENT_Y_DEFAULT = 600; + const long MOVEMENT_MOTOR_CURRENT_Z_DEFAULT = 600; + + // stall sensitivity (used with TMC2130) + const long MOVEMENT_STALL_SENSITIVITY_X_DEFAULT = 30; + const long MOVEMENT_STALL_SENSITIVITY_Y_DEFAULT = 30; + const long MOVEMENT_STALL_SENSITIVITY_Z_DEFAULT = 30; + + // micro steps setting (used with TMC2130) + const long MOVEMENT_MICROSTEPS_X_DEFAULT = 0; + const long MOVEMENT_MICROSTEPS_Y_DEFAULT = 0; + const long MOVEMENT_MICROSTEPS_Z_DEFAULT = 0; + // Use encoder (0 or 1) const long ENCODER_ENABLED_X_DEFAULT = 0; const long ENCODER_ENABLED_Y_DEFAULT = 0; @@ -234,3 +251,7 @@ enum MdlSpiEncoders #if defined(FARMDUINO_V14) && !defined(SOFTWARE_VERSION) #define SOFTWARE_VERSION "6.4.2.G\0" #endif + +#if defined(FARMDUINO_EXP_V20) && !defined(SOFTWARE_VERSION) +#define SOFTWARE_VERSION "6.4.2.E\0" +#endif diff --git a/src/CurrentState.cpp b/src/CurrentState.cpp index 9c26735..59b27a7 100644 --- a/src/CurrentState.cpp +++ b/src/CurrentState.cpp @@ -136,7 +136,6 @@ String CurrentState::getPosition() output += (float)y / (float)stepsPerMmY * 1.0; output += " Z"; output += (float)z / (float)stepsPerMmZ * 1.0; - //output += getQAndNewLine(); return output; } diff --git a/src/CurrentState.h b/src/CurrentState.h index 018e5fb..a553915 100644 --- a/src/CurrentState.h +++ b/src/CurrentState.h @@ -10,6 +10,18 @@ #include "Arduino.h" #include "pins.h" +enum ErrorListEnum +{ + ERR_NO_ERROR = 0, + ERR_EMERGENCY_STOP = 1, + ERR_TIMEOUT = 2, + ERR_STALL_DETECTED = 3, + + ERR_INVALID_COMMAND = 14, + ERR_PARAMS_NOT_OK = 15, + +}; + class CurrentState { public: @@ -50,9 +62,11 @@ private: CurrentState(CurrentState const &); void operator=(CurrentState const &); - long stepsPerMmX; - long stepsPerMmY; - long stepsPerMmZ; + long stepsPerMmX = 1; + long stepsPerMmY = 1; + long stepsPerMmZ = 1; + + int errorCode = 0; bool emergencyStop = false; }; diff --git a/src/F11Handler.cpp b/src/F11Handler.cpp index 8f28cf4..d75968d 100644 --- a/src/F11Handler.cpp +++ b/src/F11Handler.cpp @@ -31,7 +31,40 @@ int F11Handler::execute(Command *command) Serial.print("R99 HOME X\r\n"); } - StepperControl::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, true, false, false); + int homeIsUp = ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_X); + int moveAwayCoord = 10; + int execution; + bool emergencyStop; + + if (homeIsUp == 1) + { + moveAwayCoord = -moveAwayCoord; + } + + int stepNr; + + // Move to home position. Then 3 times move away and move to home again. + for (int stepNr = 0; stepNr < 7; stepNr++) + { + switch (stepNr) + { + case 0: Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, true, false, false); break; + case 1: Movement::getInstance()->moveToCoords(moveAwayCoord, 0, 0, 0, 0, 0, false, false, false); break; + case 2: Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, true, false, false); break; + case 3: Movement::getInstance()->moveToCoords(moveAwayCoord, 0, 0, 0, 0, 0, false, false, false); break; + case 4: Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, true, false, false); break; + case 5: Movement::getInstance()->moveToCoords(moveAwayCoord, 0, 0, 0, 0, 0, false, false, false); break; + case 6: Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, true, false, false); break; + } + + execution = CurrentState::getInstance()->getLastError(); + emergencyStop = CurrentState::getInstance()->isEmergencyStop(); + + if (emergencyStop || execution != 0) + { + break; + } + } if (LOGGING) { diff --git a/src/F11Handler.h b/src/F11Handler.h index 6014c34..8bbfb8d 100644 --- a/src/F11Handler.h +++ b/src/F11Handler.h @@ -12,7 +12,7 @@ #include "CurrentState.h" #include "pins.h" #include "Config.h" -#include "StepperControl.h" +#include "Movement.h" class F11Handler : public GCodeHandler { diff --git a/src/F12Handler.cpp b/src/F12Handler.cpp index c8e434f..7c00aa9 100644 --- a/src/F12Handler.cpp +++ b/src/F12Handler.cpp @@ -31,7 +31,40 @@ int F12Handler::execute(Command *command) Serial.print("R99 HOME Y\r\n"); } - StepperControl::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, true, false); + int homeIsUp = ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_X); + int moveAwayCoord = 10; + int execution; + bool emergencyStop; + + if (homeIsUp == 1) + { + moveAwayCoord = -moveAwayCoord; + } + + int stepNr; + + // Move to home position. Then 3 times move away and move to home again. + for (int stepNr = 0; stepNr < 7; stepNr++) + { + switch (stepNr) + { + case 0: Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, true, false); break; + case 1: Movement::getInstance()->moveToCoords(0, moveAwayCoord, 0, 0, 0, 0, false, false, false); break; + case 2: Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, true, false); break; + case 3: Movement::getInstance()->moveToCoords(0, moveAwayCoord, 0, 0, 0, 0, false, false, false); break; + case 4: Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, true, false); break; + case 5: Movement::getInstance()->moveToCoords(0, moveAwayCoord, 0, 0, 0, 0, false, false, false); break; + case 6: Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, true, false); break; + } + + execution = CurrentState::getInstance()->getLastError(); + emergencyStop = CurrentState::getInstance()->isEmergencyStop(); + + if (emergencyStop || execution != 0) + { + break; + } + } if (LOGGING) { diff --git a/src/F12Handler.h b/src/F12Handler.h index 31851f9..1e6a983 100644 --- a/src/F12Handler.h +++ b/src/F12Handler.h @@ -12,7 +12,7 @@ #include "CurrentState.h" #include "pins.h" #include "Config.h" -#include "StepperControl.h" +#include "Movement.h" class F12Handler : public GCodeHandler { diff --git a/src/F13Handler.cpp b/src/F13Handler.cpp index 7ab6d00..31055e4 100644 --- a/src/F13Handler.cpp +++ b/src/F13Handler.cpp @@ -31,7 +31,42 @@ int F13Handler::execute(Command *command) Serial.print("R99 HOME Z\r\n"); } - StepperControl::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, false, true); + Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, false, true); + + int homeIsUp = ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_X); + int moveAwayCoord = 10; + int execution; + bool emergencyStop; + + if (homeIsUp == 1) + { + moveAwayCoord = -moveAwayCoord; + } + + int stepNr; + + // Move to home position. Then 3 times move away and move to home again. + for (int stepNr = 0; stepNr < 7; stepNr++) + { + switch (stepNr) + { + case 0: Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, false, true); break; + case 1: Movement::getInstance()->moveToCoords(0, 0, moveAwayCoord, 0, 0, 0, false, false, false); break; + case 2: Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, false, true); break; + case 3: Movement::getInstance()->moveToCoords(0, 0, moveAwayCoord, 0, 0, 0, false, false, false); break; + case 4: Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, false, true); break; + case 5: Movement::getInstance()->moveToCoords(0, 0, moveAwayCoord, 0, 0, 0, false, false, false); break; + case 6: Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, false, true); break; + } + + execution = CurrentState::getInstance()->getLastError(); + emergencyStop = CurrentState::getInstance()->isEmergencyStop(); + + if (emergencyStop || execution != 0) + { + break; + } + } if (LOGGING) { diff --git a/src/F13Handler.h b/src/F13Handler.h index 6b3cd0f..9941247 100644 --- a/src/F13Handler.h +++ b/src/F13Handler.h @@ -12,7 +12,7 @@ #include "CurrentState.h" #include "pins.h" #include "Config.h" -#include "StepperControl.h" +#include "Movement.h" class F13Handler : public GCodeHandler { diff --git a/src/F14Handler.cpp b/src/F14Handler.cpp index 5c2e321..86fcc5d 100644 --- a/src/F14Handler.cpp +++ b/src/F14Handler.cpp @@ -33,11 +33,11 @@ int F14Handler::execute(Command *command) Serial.print("R99 CALIBRATE X\r\n"); } - ret = StepperControl::getInstance()->calibrateAxis(0); + ret = Movement::getInstance()->calibrateAxis(0); if (ret == 0) { - StepperControl::getInstance()->moveToCoords(0,0,0, 0,0,0, true, false, false); + Movement::getInstance()->moveToCoords(0,0,0, 0,0,0, true, false, false); } diff --git a/src/F14Handler.h b/src/F14Handler.h index 51916d2..f21563b 100644 --- a/src/F14Handler.h +++ b/src/F14Handler.h @@ -13,7 +13,7 @@ #include "CurrentState.h" #include "pins.h" #include "Config.h" -#include "StepperControl.h" +#include "Movement.h" class F14Handler : public GCodeHandler { diff --git a/src/F15Handler.cpp b/src/F15Handler.cpp index 6622353..f60e514 100644 --- a/src/F15Handler.cpp +++ b/src/F15Handler.cpp @@ -32,10 +32,10 @@ int F15Handler::execute(Command *command) Serial.print("R99 HOME Z\r\n"); } - ret = StepperControl::getInstance()->calibrateAxis(1); + ret = Movement::getInstance()->calibrateAxis(1); if (ret == 0) { - StepperControl::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, true, false); + Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, true, false); } if (LOGGING) diff --git a/src/F15Handler.h b/src/F15Handler.h index 72b0968..c252009 100644 --- a/src/F15Handler.h +++ b/src/F15Handler.h @@ -13,7 +13,7 @@ #include "CurrentState.h" #include "pins.h" #include "Config.h" -#include "StepperControl.h" +#include "Movement.h" class F15Handler : public GCodeHandler { diff --git a/src/F16Handler.cpp b/src/F16Handler.cpp index df470af..d88f1c3 100644 --- a/src/F16Handler.cpp +++ b/src/F16Handler.cpp @@ -32,10 +32,10 @@ int F16Handler::execute(Command *command) Serial.print("R99 HOME Z\r\n"); } - ret = StepperControl::getInstance()->calibrateAxis(2); + ret = Movement::getInstance()->calibrateAxis(2); if (ret == 0) { - StepperControl::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, false, true); + Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, false, true); } diff --git a/src/F16Handler.h b/src/F16Handler.h index ac795df..21045ee 100644 --- a/src/F16Handler.h +++ b/src/F16Handler.h @@ -13,7 +13,7 @@ #include "CurrentState.h" #include "pins.h" #include "Config.h" -#include "StepperControl.h" +#include "Movement.h" class F16Handler : public GCodeHandler { diff --git a/src/F20Handler.h b/src/F20Handler.h index 8b724cb..ce283be 100644 --- a/src/F20Handler.h +++ b/src/F20Handler.h @@ -12,7 +12,7 @@ #include "CurrentState.h" #include "pins.h" #include "Config.h" -#include "StepperControl.h" +#include "Movement.h" class F20Handler : public GCodeHandler { diff --git a/src/F21Handler.h b/src/F21Handler.h index e0ca468..76fe729 100644 --- a/src/F21Handler.h +++ b/src/F21Handler.h @@ -12,7 +12,7 @@ #include "CurrentState.h" #include "pins.h" #include "Config.h" -#include "StepperControl.h" +#include "Movement.h" #include "ParameterList.h" class F21Handler : public GCodeHandler diff --git a/src/F22Handler.h b/src/F22Handler.h index cf7ab13..4abe0d5 100644 --- a/src/F22Handler.h +++ b/src/F22Handler.h @@ -12,7 +12,7 @@ #include "CurrentState.h" #include "pins.h" #include "Config.h" -#include "StepperControl.h" +#include "Movement.h" #include "ParameterList.h" class F22Handler : public GCodeHandler diff --git a/src/F31Handler.h b/src/F31Handler.h index 90b12d1..1ad93fd 100644 --- a/src/F31Handler.h +++ b/src/F31Handler.h @@ -13,7 +13,7 @@ #include "CurrentState.h" #include "pins.h" #include "Config.h" -#include "StepperControl.h" +#include "Movement.h" #include "StatusList.h" class F31Handler : public GCodeHandler diff --git a/src/F32Handler.h b/src/F32Handler.h index 1dd7be5..84faf59 100644 --- a/src/F32Handler.h +++ b/src/F32Handler.h @@ -13,7 +13,7 @@ #include "CurrentState.h" #include "pins.h" #include "Config.h" -#include "StepperControl.h" +#include "Movement.h" #include "StatusList.h" class F32Handler : public GCodeHandler diff --git a/src/F81Handler.h b/src/F81Handler.h index 23275f0..fd60e0f 100644 --- a/src/F81Handler.h +++ b/src/F81Handler.h @@ -12,7 +12,7 @@ #include "CurrentState.h" #include "pins.h" #include "Config.h" -#include "StepperControl.h" +#include "Movement.h" #include "PinControl.h" class F81Handler : public GCodeHandler diff --git a/src/F82Handler.h b/src/F82Handler.h index 2fecb70..924064a 100644 --- a/src/F82Handler.h +++ b/src/F82Handler.h @@ -12,7 +12,7 @@ #include "CurrentState.h" #include "pins.h" #include "Config.h" -#include "StepperControl.h" +#include "Movement.h" class F82Handler : public GCodeHandler { diff --git a/src/F83Handler.h b/src/F83Handler.h index 1f8b90c..b08fc11 100644 --- a/src/F83Handler.h +++ b/src/F83Handler.h @@ -12,7 +12,7 @@ #include "CurrentState.h" #include "pins.h" #include "Config.h" -#include "StepperControl.h" +#include "Movement.h" class F83Handler : public GCodeHandler { diff --git a/src/F84Handler.cpp b/src/F84Handler.cpp index 1a55602..fee3f2d 100644 --- a/src/F84Handler.cpp +++ b/src/F84Handler.cpp @@ -29,19 +29,19 @@ int F84Handler::execute(Command *command) if (command->getX() == DO_RESET) { Serial.print("R99 Will zero X"); - StepperControl::getInstance()->setPositionX(0); + Movement::getInstance()->setPositionX(0); } if (command->getY() == DO_RESET) { Serial.print("R99 Will zero Y"); - StepperControl::getInstance()->setPositionY(0); + Movement::getInstance()->setPositionY(0); } if (command->getZ() == DO_RESET) { Serial.print("R99 Will zero Z"); - StepperControl::getInstance()->setPositionZ(0); + Movement::getInstance()->setPositionZ(0); } CurrentState::getInstance()->printQAndNewLine(); diff --git a/src/F84Handler.h b/src/F84Handler.h index b028997..c4ea774 100644 --- a/src/F84Handler.h +++ b/src/F84Handler.h @@ -12,7 +12,7 @@ #include "CurrentState.h" #include "pins.h" #include "Config.h" -#include "StepperControl.h" +#include "Movement.h" class F84Handler : public GCodeHandler { diff --git a/src/G00Handler.cpp b/src/G00Handler.cpp index 414153e..bba8885 100644 --- a/src/G00Handler.cpp +++ b/src/G00Handler.cpp @@ -43,7 +43,7 @@ int G00Handler::execute(Command *command) Serial.print("\r\n");*/ - StepperControl::getInstance()->moveToCoords( + Movement::getInstance()->moveToCoords( command->getX(), command->getY(), command->getZ(), command->getA(), command->getB(), command->getC(), false, false, false); diff --git a/src/G00Handler.h b/src/G00Handler.h index a981ab8..fab5eff 100644 --- a/src/G00Handler.h +++ b/src/G00Handler.h @@ -12,7 +12,7 @@ #include "CurrentState.h" #include "pins.h" #include "Config.h" -#include "StepperControl.h" +#include "Movement.h" class G00Handler : public GCodeHandler { diff --git a/src/G28Handler.cpp b/src/G28Handler.cpp index 14ce2e5..8b50906 100644 --- a/src/G28Handler.cpp +++ b/src/G28Handler.cpp @@ -39,9 +39,9 @@ int G28Handler::execute(Command *command) currentPoint[0] = sourcePoint[0] / (float)stepsPerMm[0]; currentPoint[1] = sourcePoint[1] / (float)stepsPerMm[1]; - StepperControl::getInstance()->moveToCoords(currentPoint[0], currentPoint[1], 0, 0, 0, 0, false, false, false); - StepperControl::getInstance()->moveToCoords(currentPoint[0], 0, 0, 0, 0, 0, false, false, false); - StepperControl::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, false, false); + Movement::getInstance()->moveToCoords(currentPoint[0], currentPoint[1], 0, 0, 0, 0, false, false, false); + Movement::getInstance()->moveToCoords(currentPoint[0], 0, 0, 0, 0, 0, false, false, false); + Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, false, false); if (LOGGING) { diff --git a/src/G28Handler.h b/src/G28Handler.h index 831767f..cf48ce5 100644 --- a/src/G28Handler.h +++ b/src/G28Handler.h @@ -12,7 +12,7 @@ #include "CurrentState.h" #include "pins.h" #include "Config.h" -#include "StepperControl.h" +#include "Movement.h" class G28Handler : public GCodeHandler { diff --git a/src/GCodeProcessor.cpp b/src/GCodeProcessor.cpp index 0730000..eba59c1 100644 --- a/src/GCodeProcessor.cpp +++ b/src/GCodeProcessor.cpp @@ -84,6 +84,7 @@ int GCodeProcessor::execute(Command *command) { Serial.print(COMM_REPORT_NO_CONFIG); CurrentState::getInstance()->printQAndNewLine(); + CurrentState::getInstance()->setLastError(ERR_PARAMS_NOT_OK); return -1; } } @@ -95,6 +96,7 @@ int GCodeProcessor::execute(Command *command) Serial.print(COMM_REPORT_BAD_CMD); CurrentState::getInstance()->printQAndNewLine(); + CurrentState::getInstance()->setLastError(ERR_INVALID_COMMAND); if (LOGGING) { @@ -107,6 +109,7 @@ int GCodeProcessor::execute(Command *command) { Serial.print(COMM_REPORT_BAD_CMD); CurrentState::getInstance()->printQAndNewLine(); + CurrentState::getInstance()->setLastError(ERR_INVALID_COMMAND); if (LOGGING) { @@ -123,6 +126,7 @@ int GCodeProcessor::execute(Command *command) { Serial.print(COMM_REPORT_BAD_CMD); CurrentState::getInstance()->printQAndNewLine(); + CurrentState::getInstance()->setLastError(ERR_INVALID_COMMAND); Serial.println("R99 handler == NULL\r\n"); return -1; @@ -197,6 +201,8 @@ int GCodeProcessor::execute(Command *command) else { Serial.print(COMM_REPORT_CMD_ERROR); + Serial.print(" V"); + Serial.print(CurrentState::getInstance()->getLastError()); CurrentState::getInstance()->printQAndNewLine(); } diff --git a/src/StepperControl.cpp b/src/Movement.cpp similarity index 77% rename from src/StepperControl.cpp rename to src/Movement.cpp index 7c7deb1..68c5d92 100644 --- a/src/StepperControl.cpp +++ b/src/Movement.cpp @@ -1,19 +1,19 @@ -#include "StepperControl.h" +#include "Movement.h" #include "Debug.h" #include "Config.h" -static StepperControl *instance; +static Movement *instance; -StepperControl *StepperControl::getInstance() +Movement *Movement::getInstance() { if (!instance) { - instance = new StepperControl(); + instance = new Movement(); }; return instance; }; -void StepperControl::reportEncoders() +void Movement::reportEncoders() { Serial.print(COMM_REPORT_ENCODER_SCALED); Serial.print(" X"); @@ -35,7 +35,7 @@ void StepperControl::reportEncoders() } -void StepperControl::getEncoderReport() +void Movement::getEncoderReport() { serialBuffer += COMM_REPORT_ENCODER_SCALED; serialBuffer += " X"; @@ -57,11 +57,11 @@ void StepperControl::getEncoderReport() } -void StepperControl::reportStatus(StepperControlAxis *axis, int axisStatus) +void Movement::reportStatus(MovementAxis *axis, int axisStatus) { serialBuffer += COMM_REPORT_CMD_STATUS; serialBuffer += " "; - serialBuffer += axis->label; + serialBuffer += axis->channelLabel; serialBuffer += axisStatus; serialBuffer += CurrentState::getInstance()->getQAndNewLine(); @@ -72,16 +72,16 @@ void StepperControl::reportStatus(StepperControlAxis *axis, int axisStatus) //CurrentState::getInstance()->printQAndNewLine(); } -void StepperControl::reportCalib(StepperControlAxis *axis, int calibStatus) +void Movement::reportCalib(MovementAxis *axis, int calibStatus) { Serial.print(COMM_REPORT_CALIB_STATUS); Serial.print(" "); - Serial.print(axis->label); + Serial.print(axis->channelLabel); Serial.print(calibStatus); CurrentState::getInstance()->printQAndNewLine(); } -void StepperControl::checkAxisSubStatus(StepperControlAxis *axis, int *axisSubStatus) +void Movement::checkAxisSubStatus(MovementAxis *axis, int *axisSubStatus) { int newStatus = 0; bool statusChanged = false; @@ -121,7 +121,7 @@ void StepperControl::checkAxisSubStatus(StepperControlAxis *axis, int *axisSubSt //const int MOVEMENT_INTERRUPT_SPEED = 100; // Interrupt cycle in micro seconds -StepperControl::StepperControl() +Movement::Movement() { // Initialize some variables for testing @@ -142,19 +142,19 @@ StepperControl::StepperControl() // Create the axis controllers - axisX = StepperControlAxis(); - axisY = StepperControlAxis(); - axisZ = StepperControlAxis(); + axisX = MovementAxis(); + axisY = MovementAxis(); + axisZ = MovementAxis(); - axisX.label = 'X'; - axisY.label = 'Y'; - axisZ.label = 'Z'; + axisX.channelLabel = 'X'; + axisY.channelLabel = 'Y'; + axisZ.channelLabel = 'Z'; // Create the encoder controller - encoderX = StepperControlEncoder(); - encoderY = StepperControlEncoder(); - encoderZ = StepperControlEncoder(); + encoderX = MovementEncoder(); + encoderY = MovementEncoder(); + encoderZ = MovementEncoder(); // Load settings @@ -163,9 +163,11 @@ StepperControl::StepperControl() motorMotorsEnabled = false; } -void StepperControl::loadSettings() +void Movement::loadSettings() { + /**/ //Serial.println("== load pin numbers =="); + // Load motor settings 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); @@ -176,48 +178,194 @@ void StepperControl::loadSettings() axisSubStep[1] = COMM_REPORT_MOVE_STATUS_IDLE; axisSubStep[2] = COMM_REPORT_MOVE_STATUS_IDLE; + /**/ //Serial.println("== load motor settings =="); + loadMotorSettings(); // Load encoder settings + /**/ //Serial.println("== load encoder settings =="); + loadEncoderSettings(); + /**/ //Serial.println("== load mdl encoder settings =="); + encoderX.loadMdlEncoderId(_MDL_X1); encoderY.loadMdlEncoderId(_MDL_Y); encoderZ.loadMdlEncoderId(_MDL_Z); + /**/ //Serial.println("== load encoder pin numbers =="); + 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); + /**/ //Serial.println("== load encoder load settings 2 =="); + encoderX.loadSettings(motorConsEncoderType[0], motorConsEncoderScaling[0], motorConsEncoderInvert[0]); encoderY.loadSettings(motorConsEncoderType[1], motorConsEncoderScaling[1], motorConsEncoderInvert[1]); encoderZ.loadSettings(motorConsEncoderType[2], motorConsEncoderScaling[2], motorConsEncoderInvert[2]); + } -void StepperControl::test() +#if defined(FARMDUINO_EXP_V20) + void Movement::initTMC2130() + { + axisX.initTMC2130(); + axisY.initTMC2130(); + axisZ.initTMC2130(); + } + + void Movement::loadSettingsTMC2130() + { + /**/ + int motorCurrentX; + int stallSensitivityX; + int microStepsX; + + int motorCurrentY; + int stallSensitivityY; + int microStepsY; + + int motorCurrentZ; + int stallSensitivityZ; + int microStepsZ; + + motorCurrentX = ParameterList::getInstance()->getValue(MOVEMENT_MOTOR_CURRENT_X); + stallSensitivityX = ParameterList::getInstance()->getValue(MOVEMENT_STALL_SENSITIVITY_X); + microStepsX = ParameterList::getInstance()->getValue(MOVEMENT_MICROSTEPS_X); + //axisX.loadSettingsTMC2130(motorCurrent, stallSensitivity, microSteps); + + motorCurrentY = ParameterList::getInstance()->getValue(MOVEMENT_MOTOR_CURRENT_Y); + stallSensitivityY = ParameterList::getInstance()->getValue(MOVEMENT_STALL_SENSITIVITY_Y); + microStepsY = ParameterList::getInstance()->getValue(MOVEMENT_MICROSTEPS_Y); + //axisY.loadSettingsTMC2130(motorCurrent, stallSensitivity, microSteps); + + motorCurrentZ = ParameterList::getInstance()->getValue(MOVEMENT_MOTOR_CURRENT_Z); + stallSensitivityZ = ParameterList::getInstance()->getValue(MOVEMENT_STALL_SENSITIVITY_Z); + microStepsX = ParameterList::getInstance()->getValue(MOVEMENT_MICROSTEPS_Z); + //axisZ.loadSettingsTMC2130(motorCurrent, stallSensitivity, microSteps); + + motorCurrentX = 600; + stallSensitivityX = 0; + microStepsX = 0; + + motorCurrentY = 600; + stallSensitivityY = 0; + microStepsY = 0; + + motorCurrentZ = 600; + stallSensitivityZ = 0; + microStepsZ = 0; + + TMC2130X->push(); + TMC2130X->toff(3); + TMC2130X->tbl(1); + TMC2130X->hysteresis_start(4); + TMC2130X->hysteresis_end(-2); + TMC2130X->rms_current(motorCurrentX); // mA + TMC2130X->microsteps(microStepsX); + TMC2130X->diag1_stall(1); + TMC2130X->diag1_active_high(1); + TMC2130X->coolstep_min_speed(0xFFFFF); // 20bit max + TMC2130X->THIGH(0); + TMC2130X->semin(5); + TMC2130X->semax(2); + TMC2130X->sedn(0b01); + TMC2130X->sg_stall_value(stallSensitivityX); + + TMC2130Y->push(); + TMC2130Y->toff(3); + TMC2130Y->tbl(1); + TMC2130Y->hysteresis_start(4); + TMC2130Y->hysteresis_end(-2); + TMC2130Y->rms_current(motorCurrentY); // mA + TMC2130Y->microsteps(microStepsY); + TMC2130Y->diag1_stall(1); + TMC2130Y->diag1_active_high(1); + TMC2130Y->coolstep_min_speed(0xFFFFF); // 20bit max + TMC2130Y->THIGH(0); + TMC2130Y->semin(5); + TMC2130Y->semax(2); + TMC2130Y->sedn(0b01); + TMC2130Y->sg_stall_value(stallSensitivityY); + + TMC2130Z->push(); + TMC2130Z->toff(3); + TMC2130Z->tbl(1); + TMC2130Z->hysteresis_start(4); + TMC2130Z->hysteresis_end(-2); + TMC2130Z->rms_current(motorCurrentZ); // mA + TMC2130Z->microsteps(microStepsZ); + TMC2130Z->diag1_stall(1); + TMC2130Z->diag1_active_high(1); + TMC2130Z->coolstep_min_speed(0xFFFFF); // 20bit max + TMC2130Z->THIGH(0); + TMC2130Z->semin(5); + TMC2130Z->semax(2); + TMC2130Z->sedn(0b01); + TMC2130Z->sg_stall_value(stallSensitivityZ); + + TMC2130E->push(); + TMC2130E->toff(3); + TMC2130E->tbl(1); + TMC2130E->hysteresis_start(4); + TMC2130E->hysteresis_end(-2); + TMC2130E->rms_current(600); // mA + TMC2130E->microsteps(0); + TMC2130E->diag1_stall(1); + TMC2130E->diag1_active_high(1); + TMC2130E->coolstep_min_speed(0xFFFFF); // 20bit max + TMC2130E->THIGH(0); + TMC2130E->semin(5); + TMC2130E->semax(2); + TMC2130E->sedn(0b01); + TMC2130E->sg_stall_value(0); + + } + +#endif + +void Movement::test() { + axisX.enableMotor(); - Serial.print("R99"); - Serial.print(" mot x = "); - Serial.print(axisX.currentPosition()); - Serial.print(" enc x = "); - Serial.print(encoderX.currentPosition()); - Serial.print("\r\n"); + //axisY.test(); - Serial.print("R99"); - Serial.print(" mot y = "); - Serial.print(axisY.currentPosition()); - Serial.print(" enc y = "); - Serial.print(encoderY.currentPosition()); - Serial.print("\r\n"); + //axisX.enableMotor(); + //axisX.setMotorStep(); + //delayMicroseconds(10); + //axisX.setMotorStep(); + //delayMicroseconds(10); - Serial.print("R99"); - Serial.print(" mot z = "); - Serial.print(axisZ.currentPosition()); - Serial.print(" enc z = "); - Serial.print(encoderZ.currentPosition()); - Serial.print("\r\n"); + //digitalWrite(X_STEP_PIN, HIGH); + //delayMicroseconds(10); + //digitalWrite(X_STEP_PIN, LOW); + //delayMicroseconds(10); + + //axisX.setMotorStepWriteTMC2130(); + //axisX.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(); @@ -232,10 +380,12 @@ void StepperControl::test() //Serial.print("\r\n"); } -void StepperControl::test2() +void Movement::test2() { - CurrentState::getInstance()->printPosition(); - encoderX.test(); + + axisX.setMotorStep(); + //CurrentState::getInstance()->printPosition(); + //encoderX.test(); //encoderY.test(); //encoderZ.test(); } @@ -247,11 +397,14 @@ void StepperControl::test2() * maxStepsPerSecond - maximum number of steps per second * maxAccelerationStepsPerSecond - maximum number of acceleration in steps per second */ -int StepperControl::moveToCoords(double xDestScaled, double yDestScaled, double zDestScaled, +int Movement::moveToCoords(double xDestScaled, double yDestScaled, double zDestScaled, unsigned int xMaxSpd, unsigned int yMaxSpd, unsigned int zMaxSpd, bool xHome, bool yHome, bool zHome) { + /**/ //Serial.println("AAA"); + /**/ //test(); + long xDest = xDestScaled * stepsPerMm[0]; long yDest = yDestScaled * stepsPerMm[1]; long zDest = zDestScaled * stepsPerMm[2]; @@ -355,6 +508,7 @@ int StepperControl::moveToCoords(double xDestScaled, double yDestScaled, double Serial.print(axisZ.destinationPosition() / stepsPerMm[2]); CurrentState::getInstance()->printQAndNewLine(); } + // Prepare for movement axisX.movementStarted = false; @@ -407,20 +561,40 @@ int StepperControl::moveToCoords(double xDestScaled, double yDestScaled, double emergencyStop = CurrentState::getInstance()->isEmergencyStop(); + unsigned long loopCounts = 0; + // Let the interrupt handle all the movements while ((axisActive[0] || axisActive[1] || axisActive[2]) && !emergencyStop) { - #if defined(FARMDUINO_V14) checkEncoders(); #endif + /**/ + if (loopCounts % 1000 == 0) + { + + //Serial.print("R99"); + //Serial.print(" missed step "); + //Serial.print(motorConsMissedSteps[1]); + //Serial.print(" axis pos "); + //Serial.print(axisY.currentPosition()); + //Serial.print("\r\n"); + + //Serial.print("X - "); + //axisX.test(); + + //Serial.print("Y - "); + //axisY.test(); + + } + loopCounts++; + + checkAxisSubStatus(&axisX, &axisSubStep[0]); checkAxisSubStatus(&axisY, &axisSubStep[1]); checkAxisSubStatus(&axisZ, &axisSubStep[2]); - //checkEncoders(); - axisX.checkTiming(); axisY.checkTiming(); axisZ.checkTiming(); @@ -428,64 +602,24 @@ int StepperControl::moveToCoords(double xDestScaled, double yDestScaled, double if (axisX.isStepDone()) { axisX.checkMovement(); - checkAxisVsEncoder(&axisX, &encoderX, &motorConsMissedSteps[0], &motorLastPosition[0], &motorConsEncoderLastPosition[0], &motorConsEncoderUseForPos[0], &motorConsMissedStepsDecay[0], &motorConsEncoderEnabled[0]); + //checkAxisVsEncoder(&axisX, &encoderX, &motorConsMissedSteps[0], &motorLastPosition[0], &motorConsEncoderLastPosition[0], &motorConsEncoderUseForPos[0], &motorConsMissedStepsDecay[0], &motorConsEncoderEnabled[0]); axisX.resetStepDone(); } if (axisY.isStepDone()) { axisY.checkMovement(); - checkAxisVsEncoder(&axisY, &encoderY, &motorConsMissedSteps[1], &motorLastPosition[1], &motorConsEncoderLastPosition[1], &motorConsEncoderUseForPos[1], &motorConsMissedStepsDecay[1], &motorConsEncoderEnabled[1]); + //checkAxisVsEncoder(&axisY, &encoderY, &motorConsMissedSteps[1], &motorLastPosition[1], &motorConsEncoderLastPosition[1], &motorConsEncoderUseForPos[1], &motorConsMissedStepsDecay[1], &motorConsEncoderEnabled[1]); axisY.resetStepDone(); } if (axisZ.isStepDone()) { axisZ.checkMovement(); - checkAxisVsEncoder(&axisZ, &encoderZ, &motorConsMissedSteps[2], &motorLastPosition[2], &motorConsEncoderLastPosition[2], &motorConsEncoderUseForPos[2], &motorConsMissedStepsDecay[2], &motorConsEncoderEnabled[2]); + //checkAxisVsEncoder(&axisZ, &encoderZ, &motorConsMissedSteps[2], &motorLastPosition[2], &motorConsEncoderLastPosition[2], &motorConsEncoderUseForPos[2], &motorConsMissedStepsDecay[2], &motorConsEncoderEnabled[2]); axisZ.resetStepDone(); } - - //if (debugInterrupt) - //{ - // delayMicroseconds(100); - // handleMovementInterrupt(); - //} - //else - //{ - // delay(1); - //} - - /* - if ((int)motorConsMissedSteps[0] != motorConsMissedStepsPrev[0]) - { - motorConsMissedStepsPrev[0] = (int) motorConsMissedSteps[0]; - Serial.print("R99"); - Serial.print(" missed steps X = "); - Serial.print(motorConsMissedStepsPrev[0]); - Serial.print("\r\n"); - } - - if ((int)motorConsMissedSteps[1] != motorConsMissedStepsPrev[1]) - { - motorConsMissedStepsPrev[1] = (int)motorConsMissedSteps[1]; - Serial.print("R99"); - Serial.print(" missed steps Y = "); - Serial.print(motorConsMissedStepsPrev[1]); - Serial.print("\r\n"); - } - - if ((int)motorConsMissedSteps[2] != motorConsMissedStepsPrev[2]) - { - motorConsMissedStepsPrev[2] = (int)motorConsMissedSteps[2]; - Serial.print("R99"); - Serial.print(" missed steps Z = "); - Serial.print(motorConsMissedStepsPrev[2]); - Serial.print("\r\n"); - } - */ - if (axisX.isAxisActive() && motorConsMissedSteps[0] > motorConsMissedStepsMax[0]) { axisX.deactivateAxis(); @@ -500,7 +634,7 @@ int StepperControl::moveToCoords(double xDestScaled, double yDestScaled, double } else { - error = 1; + error = ERR_STALL_DETECTED; } } @@ -518,7 +652,7 @@ int StepperControl::moveToCoords(double xDestScaled, double yDestScaled, double } else { - error = 1; + error = ERR_STALL_DETECTED; } } @@ -537,7 +671,7 @@ int StepperControl::moveToCoords(double xDestScaled, double yDestScaled, double } else { - error = 1; + error = ERR_STALL_DETECTED; } } @@ -579,21 +713,21 @@ int StepperControl::moveToCoords(double xDestScaled, double yDestScaled, double serialBuffer += COMM_REPORT_TIMEOUT_X; serialBuffer += "\r\n"; serialBuffer += "R99 timeout X axis\r\n"; - error = 2; + error = ERR_TIMEOUT; } if (axisActive[1] == true && ((millis() >= timeStart && millis() - timeStart > timeOut[1] * 1000) || (millis() < timeStart && millis() > timeOut[1] * 1000))) { serialBuffer += COMM_REPORT_TIMEOUT_Y; serialBuffer += "\r\n"; serialBuffer += "R99 timeout Y axis\r\n"; - error = 2; + error = ERR_TIMEOUT; } if (axisActive[2] == true && ((millis() >= timeStart && millis() - timeStart > timeOut[2] * 1000) || (millis() < timeStart && millis() > timeOut[2] * 1000))) { serialBuffer += COMM_REPORT_TIMEOUT_Z; serialBuffer += "\r\n"; serialBuffer += "R99 timeout Z axis\r\n"; - error = 2; + error = ERR_TIMEOUT; } // Check if there is an emergency stop command @@ -617,7 +751,7 @@ int StepperControl::moveToCoords(double xDestScaled, double yDestScaled, double axisActive[1] = false; axisActive[2] = false; - error = 1; + error = ERR_EMERGENCY_STOP; } } @@ -639,6 +773,10 @@ int StepperControl::moveToCoords(double xDestScaled, double yDestScaled, double if (serialMessageDelay > 300 && serialBuffer.length() == 0 && serialBufferSending == 0) { + + //Serial.print("Y-"); + //axisY.test();/**/ + serialMessageDelay = 0; switch(serialMessageNr) @@ -654,13 +792,42 @@ int StepperControl::moveToCoords(double xDestScaled, double yDestScaled, double getEncoderReport(); #endif break; + + case 2: + #if defined(FARMDUINO_EXP_V20) + + serialBuffer += "R89"; + serialBuffer += " X"; + serialBuffer += axisX.getLoad(); + serialBuffer += " Y"; + serialBuffer += axisY.getLoad(); + serialBuffer += " Z"; + serialBuffer += axisZ.getLoad(); + serialBuffer += CurrentState::getInstance()->getQAndNewLine(); + + #endif + break; + + default: + break; } serialMessageNr++; + + #if !defined(FARMDUINO_EXP_V20) if (serialMessageNr > 1) { serialMessageNr = 0; } + #endif + + #if defined(FARMDUINO_EXP_V20) + + if (serialMessageNr > 2) + { + serialMessageNr = 0; + } + #endif serialBufferSending = 0; @@ -744,6 +911,7 @@ int StepperControl::moveToCoords(double xDestScaled, double yDestScaled, double if (emergencyStop) { CurrentState::getInstance()->setEmergencyStop(); + error = ERR_EMERGENCY_STOP; } Serial.print("R99 error "); @@ -757,7 +925,7 @@ int StepperControl::moveToCoords(double xDestScaled, double yDestScaled, double return error; } -void StepperControl::serialBufferEmpty() +void Movement::serialBufferEmpty() { while (serialBuffer.length() > 0) { @@ -765,7 +933,7 @@ void StepperControl::serialBufferEmpty() } } -void StepperControl::serialBufferSendNext() +void Movement::serialBufferSendNext() { // Send the next char in the serialBuffer if (serialBuffer.length() > 0) @@ -801,7 +969,7 @@ void StepperControl::serialBufferSendNext() // Calibration // -int StepperControl::calibrateAxis(int axis) +int Movement::calibrateAxis(int axis) { // Load motor and encoder settings @@ -836,8 +1004,8 @@ int StepperControl::calibrateAxis(int axis) reportEndStops(); // Select the right axis - StepperControlAxis *calibAxis; - StepperControlEncoder *calibEncoder; + MovementAxis *calibAxis; + MovementEncoder *calibEncoder; switch (axis) { @@ -902,7 +1070,7 @@ int StepperControl::calibrateAxis(int axis) Serial.print("R99"); Serial.print(" axis "); - Serial.print(calibAxis->label); + Serial.print(calibAxis->channelLabel); Serial.print(" move to start for calibration"); Serial.print("\r\n"); @@ -911,7 +1079,11 @@ int StepperControl::calibrateAxis(int axis) // Move towards home calibAxis->enableMotor(); - calibAxis->setDirectionHome(); + + /**/ + //calibAxis->setDirectionHome(); + calibAxis->setDirectionAway(); + calibAxis->setCurrentPosition(calibEncoder->currentPosition()); stepsCount = 0; @@ -951,7 +1123,11 @@ int StepperControl::calibrateAxis(int axis) } } - // Move until the end stop for home position is reached, either by end stop or motor skipping + // Move until any end stop is reached or the motor is skipping. That end should be the far end stop. First, ram the end at high speed. + + /**/ + //if (((!invertEndStops && !calibAxis->endStopMax()) || (invertEndStops && !calibAxis->endStopMin())) && !movementDone && (*missedSteps < *missedStepsMax)) + //if ((!calibAxis->endStopMin() && !calibAxis->endStopMax()) && !movementDone && (*missedSteps < *missedStepsMax)) if ((!calibAxis->endStopMin() && !calibAxis->endStopMax()) && !movementDone && (*missedSteps < *missedStepsMax)) { @@ -996,7 +1172,7 @@ int StepperControl::calibrateAxis(int axis) Serial.print("R99 movement done\r\n"); // If end stop for home is active, set the position to zero - if (calibAxis->endStopMax()) + if (calibAxis->endStopMin()) { invertEndStops = true; } @@ -1007,7 +1183,7 @@ int StepperControl::calibrateAxis(int axis) Serial.print("R99"); Serial.print(" axis "); - Serial.print(calibAxis->label); + Serial.print(calibAxis->channelLabel); Serial.print(" at starting point"); Serial.print("\r\n"); @@ -1044,14 +1220,18 @@ int StepperControl::calibrateAxis(int axis) Serial.print("R99"); Serial.print(" axis "); - Serial.print(calibAxis->label); + Serial.print(calibAxis->channelLabel); Serial.print(" calibrating length"); Serial.print("\r\n"); stepsCount = 0; movementDone = false; *missedSteps = 0; - calibAxis->setDirectionAway(); + + /**/ + //calibAxis->setDirectionAway(); + calibAxis->setDirectionHome(); + calibAxis->setCurrentPosition(calibEncoder->currentPosition()); motorConsMissedSteps[0] = 0; @@ -1091,8 +1271,12 @@ int StepperControl::calibrateAxis(int axis) *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)) + // Move until the end stop is at the home position by detecting the other end stop or missed steps are detected + /**/ + //if ((!calibAxis->endStopMin() && !calibAxis->endStopMax()) && !movementDone && (*missedSteps < *missedStepsMax)) + //if (((!invertEndStops && !calibAxis->endStopMax()) || (invertEndStops && !calibAxis->endStopMin())) && !movementDone && (*missedSteps < *missedStepsMax)) + if (((!invertEndStops && !calibAxis->endStopMin()) || (invertEndStops && !calibAxis->endStopMax())) && !movementDone && (*missedSteps < *missedStepsMax)) + { calibAxis->setStepAxis(); @@ -1127,7 +1311,7 @@ int StepperControl::calibrateAxis(int axis) Serial.print("R99"); Serial.print(" axis "); - Serial.print(calibAxis->label); + Serial.print(calibAxis->channelLabel); Serial.print(" at end point"); Serial.print("\r\n"); @@ -1189,8 +1373,9 @@ int StepperControl::calibrateAxis(int axis) 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, long *encoderLastPosition, int *encoderUseForPos, float *encoderStepDecay, bool *encoderEnabled) +void Movement::checkAxisVsEncoder(MovementAxis *axis, MovementEncoder *encoder, float *missedSteps, long *lastPosition, long *encoderLastPosition, int *encoderUseForPos, float *encoderStepDecay, bool *encoderEnabled) { +#if !defined(FARMDUINO_EXP_V20) if (*encoderEnabled) { bool stepMissed = false; @@ -1256,9 +1441,49 @@ void StepperControl::checkAxisVsEncoder(StepperControlAxis *axis, StepperControl axis->setCurrentPosition(encoder->currentPosition()); } } +#endif + +#if defined(FARMDUINO_EXP_V20) + + /**/ + /* + Serial.print("R99"); + Serial.print(" XXX "); + Serial.print(" cur pos "); + Serial.print(axis->currentPosition()); + Serial.print(" last pos "); + Serial.print(*lastPosition); + */ + + if (*encoderEnabled) { + if (axis->stallDetected()) { + // In case of stall detection, count this as a missed step + (*missedSteps)++; + axis->setCurrentPosition(*lastPosition); + } + else { + // Decrease amount of missed steps if there are no missed step + if (*missedSteps > 0) + { + (*missedSteps) -= (*encoderStepDecay); + } + *lastPosition = axis->currentPosition(); + encoder->setPosition(axis->currentPosition()); + } + } + + //Serial.print(" new last pos "); + //Serial.print(*lastPosition); + //Serial.print(" en pos "); + //Serial.print(encoder->currentPosition()); + //Serial.print("\r\n"); + + +#endif + } -void StepperControl::loadMotorSettings() +void Movement::loadMotorSettings() { // Load settings @@ -1352,10 +1577,17 @@ void StepperControl::loadMotorSettings() axisY.loadMotorSettings(speedMax[1], speedMin[1], speedHome[1], stepsAcc[1], timeOut[1], homeIsUp[1], motorInv[1], endStInv[1], endStInv2[1], MOVEMENT_INTERRUPT_SPEED, motor2Enbl[1], motor2Inv[1], endStEnbl[1], motorStopAtHome[1], motorMaxSize[1], motorStopAtMax[1]); axisZ.loadMotorSettings(speedMax[2], speedMin[2], speedHome[2], stepsAcc[2], timeOut[2], homeIsUp[2], motorInv[2], endStInv[2], endStInv2[2], MOVEMENT_INTERRUPT_SPEED, motor2Enbl[2], motor2Inv[2], endStEnbl[2], motorStopAtHome[2], motorMaxSize[2], motorStopAtMax[2]); + /**/ + +/* +#if defined(FARMDUINO_EXP_V20) + loadSettingsTMC2130(); +#endif +*/ primeMotors(); } -bool StepperControl::intToBool(int value) +bool Movement::intToBool(int value) { if (value == 1) { @@ -1364,7 +1596,7 @@ bool StepperControl::intToBool(int value) return false; } -void StepperControl::loadEncoderSettings() +void Movement::loadEncoderSettings() { // Load encoder settings @@ -1429,7 +1661,7 @@ void StepperControl::loadEncoderSettings() } } -unsigned long StepperControl::getMaxLength(unsigned long lengths[3]) +unsigned long Movement::getMaxLength(unsigned long lengths[3]) { unsigned long max = lengths[0]; for (int i = 1; i < 3; i++) @@ -1442,7 +1674,7 @@ unsigned long StepperControl::getMaxLength(unsigned long lengths[3]) return max; } -void StepperControl::enableMotors() +void Movement::enableMotors() { motorMotorsEnabled = true; @@ -1453,7 +1685,7 @@ void StepperControl::enableMotors() delay(100); } -void StepperControl::disableMotorsEmergency() +void Movement::disableMotorsEmergency() { motorMotorsEnabled = false; @@ -1462,7 +1694,7 @@ void StepperControl::disableMotorsEmergency() axisZ.disableMotor(); } -void StepperControl::disableMotors() +void Movement::disableMotors() { motorMotorsEnabled = false; @@ -1473,19 +1705,19 @@ void StepperControl::disableMotors() delay(100); } -void StepperControl::primeMotors() +void Movement::primeMotors() { if (motorKeepActive[0] == 1) { axisX.enableMotor(); } else { axisX.disableMotor(); } if (motorKeepActive[1] == 1) { axisY.enableMotor(); } else { axisY.disableMotor(); } if (motorKeepActive[2] == 1) { axisZ.enableMotor(); } else { axisZ.disableMotor(); } } -bool StepperControl::motorsEnabled() +bool Movement::motorsEnabled() { return motorMotorsEnabled; } -bool StepperControl::endStopsReached() +bool Movement::endStopsReached() { if (axisX.endStopsReached() || @@ -1497,9 +1729,10 @@ bool StepperControl::endStopsReached() return false; } -void StepperControl::storePosition() +void Movement::storePosition() { +#if !defined(FARMDUINO_EXP_V20) if (motorConsEncoderEnabled[0]) { CurrentState::getInstance()->setX(encoderX.currentPosition()); @@ -1526,49 +1759,52 @@ void StepperControl::storePosition() { CurrentState::getInstance()->setZ(axisZ.currentPosition()); } +#endif + +#if defined(FARMDUINO_EXP_V20) + CurrentState::getInstance()->setX(axisX.currentPosition()); + CurrentState::getInstance()->setY(axisY.currentPosition()); + CurrentState::getInstance()->setZ(axisZ.currentPosition()); +#endif + } -void StepperControl::reportEndStops() +void Movement::reportEndStops() { CurrentState::getInstance()->printEndStops(); } -void StepperControl::reportPosition() +void Movement::reportPosition() { CurrentState::getInstance()->printPosition(); } -void StepperControl::storeEndStops() +void Movement::storeEndStops() { CurrentState::getInstance()->storeEndStops(); } -void StepperControl::setPositionX(long pos) +void Movement::setPositionX(long pos) { axisX.setCurrentPosition(pos); encoderX.setPosition(pos); } -void StepperControl::setPositionY(long pos) +void Movement::setPositionY(long pos) { axisY.setCurrentPosition(pos); encoderY.setPosition(pos); } -void StepperControl::setPositionZ(long pos) +void Movement::setPositionZ(long pos) { axisZ.setCurrentPosition(pos); encoderZ.setPosition(pos); } // Handle movement by checking each axis -void StepperControl::handleMovementInterrupt(void) +void Movement::handleMovementInterrupt(void) { - //if (debugMessages) - //{ - // i1 = micros(); - //} - // No need to check the encoders for Farmduino 1.4 #if defined(RAMPS_V14) || defined(FARMDUINO_V10) checkEncoders(); @@ -1580,94 +1816,31 @@ void StepperControl::handleMovementInterrupt(void) axisY.incrementTick(); axisZ.incrementTick(); - /* - axisServiced = axisServicedNext; - switch (axisServiced) - { - case 0: - axisX.incrementTick(); - axisX.checkTiming(); - axisServicedNext=1; - break; - case 1: - axisY.incrementTick(); - axisY.checkTiming(); - axisServicedNext=2; - break; - case 2: - axisZ.incrementTick(); - axisZ.checkTiming(); - axisServicedNext=0; - break; - } - */ - - //if (debugMessages) - //{ - // i2 = micros(); - // i3 = i2 - i1; - // if (i3 > i4) - // { - // i4 = i3; - // } - //} } -void StepperControl::checkEncoders() +void Movement::checkEncoders() { // read encoder pins using the arduino IN registers instead of digital in // because it used much fewer cpu cycles - //encoderX.shiftChannels(); - //encoderY.shiftChannels(); - //encoderZ.shiftChannels(); - - /* - Serial.print("R99"); - Serial.print(" PINA "); - Serial.print(PINA); - Serial.print(" "); - Serial.print((bool)(PINA & 0x02)); - Serial.print((bool)(PINA & 0x08)); - Serial.print(" Y "); - Serial.print(digitalRead(23)); - Serial.print(digitalRead(25)); - Serial.print("\r\n"); - */ - - // A=16/PH1 B=17/PH0 AQ=31/PC6 BQ=33/PC4 - //encoderX.checkEncoder(PINH & 0x02, PINH & 0x01, PINC & 0x40, PINC & 0x10); - //encoderX.setChannels(PINH & 0x02, PINH & 0x01, PINC & 0x40, PINC & 0x10); encoderX.checkEncoder( ENC_X_A_PORT & ENC_X_A_BYTE, ENC_X_B_PORT & ENC_X_B_BYTE, ENC_X_A_Q_PORT & ENC_X_A_Q_BYTE, ENC_X_B_Q_PORT & ENC_X_B_Q_BYTE); - // A=23/PA1 B=25/PA3 AQ=35/PC2 BQ=37/PC0 - //encoderY.checkEncoder(PINA & 0x02, PINA & 0x08, PINC & 0x04, PINC & 0x01); - //encoderY.setChannels(PINA & 0x02, PINA & 0x08, PINC & 0x04, PINC & 0x01); encoderY.checkEncoder( ENC_Y_A_PORT & ENC_Y_A_BYTE, ENC_Y_B_PORT & ENC_Y_B_BYTE, ENC_Y_A_Q_PORT & ENC_Y_A_Q_BYTE, ENC_Y_B_Q_PORT & ENC_Y_B_Q_BYTE); - // A=27/PA5 B=29/PA7 AQ=39/PG2 BQ=41/PG0 - //encoderZ.checkEncoder(PINA & 0x20, PINA & 0x80, PING & 0x04, PING & 0x01); - //encoderZ.setChannels(PINA & 0x20, PINA & 0x80, PING & 0x04, PING & 0x01); encoderZ.checkEncoder( ENC_Z_A_PORT & ENC_Z_A_BYTE, ENC_Z_B_PORT & ENC_Z_B_BYTE, ENC_Z_A_Q_PORT & ENC_Z_A_Q_BYTE, ENC_Z_B_Q_PORT & ENC_Z_B_Q_BYTE); - - - //encoderX.processEncoder(); - //encoderY.processEncoder(); - //encoderZ.processEncoder(); - -} +} \ No newline at end of file diff --git a/src/StepperControl.h b/src/Movement.h similarity index 77% rename from src/StepperControl.h rename to src/Movement.h index e5805dd..abcf608 100644 --- a/src/StepperControl.h +++ b/src/Movement.h @@ -1,33 +1,32 @@ /* - * StepperControl.h + * Movement.h * * Created on: 16 maj 2014 * Author: MattLech */ -#ifndef STEPPERCONTROL_H_ -#define STEPPERCONTROL_H_ +#ifndef MOVEMENT_H_ +#define MOVEMENT_H_ #include "Arduino.h" #include "CurrentState.h" #include "ParameterList.h" -#include "StepperControlAxis.h" -#include "StepperControlEncoder.h" +#include "MovementAxis.h" +#include "MovementEncoder.h" #include "pins.h" #include "Config.h" #include #include #include "Command.h" - -class StepperControl +class Movement { public: - StepperControl(); - StepperControl(StepperControl const &); - void operator=(StepperControl const &); + Movement(); + Movement(Movement const &); + void operator=(Movement const &); - static StepperControl *getInstance(); + static Movement *getInstance(); //int moveAbsolute( long xDest, long yDest,long zDest, // unsigned int maxStepsPerSecond, // unsigned int maxAccelerationStepsPerSecond); @@ -38,6 +37,12 @@ public: void handleMovementInterrupt(); void checkEncoders(); + #if defined(FARMDUINO_EXP_V20) + void initTMC2130(); + void loadSettingsTMC2130(); + #endif + + int calibrateAxis(int axis); //void initInterrupt(); void enableMotors(); @@ -64,13 +69,14 @@ public: unsigned long i4 = 0; private: - StepperControlAxis axisX; - StepperControlAxis axisY; - StepperControlAxis axisZ; - StepperControlEncoder encoderX; - StepperControlEncoder encoderY; - StepperControlEncoder encoderZ; + MovementAxis axisX; + MovementAxis axisY; + MovementAxis axisZ; + + MovementEncoder encoderX; + MovementEncoder encoderY; + MovementEncoder encoderZ; //char serialBuffer[100]; String serialBuffer; @@ -82,8 +88,8 @@ private: void serialBufferSendNext(); void serialBufferEmpty(); - void checkAxisVsEncoder(StepperControlAxis *axis, StepperControlEncoder *encoder, float *missedSteps, long *lastPosition, long *encoderLastPosition, int *encoderUseForPos, float *encoderStepDecay, bool *encoderEnabled); - void checkAxisSubStatus(StepperControlAxis *axis, int *axisSubStatus); + void checkAxisVsEncoder(MovementAxis *axis, MovementEncoder *encoder, float *missedSteps, long *lastPosition, long *encoderLastPosition, int *encoderUseForPos, float *encoderStepDecay, bool *encoderEnabled); + void checkAxisSubStatus(MovementAxis *axis, int *axisSubStatus); bool axisActive[3] = { false, false, false }; int axisSubStep[3] = { 0, 0, 0 }; @@ -97,8 +103,8 @@ private: void storeEndStops(); void reportEndStops(); - void reportStatus(StepperControlAxis *axis, int axisSubStatus); - void reportCalib(StepperControlAxis *axis, int calibStatus); + void reportStatus(MovementAxis *axis, int axisSubStatus); + void reportCalib(MovementAxis *axis, int calibStatus); unsigned long getMaxLength(unsigned long lengths[3]); bool endStopsReached(); @@ -138,6 +144,7 @@ private: int axisServiced = 0; int axisServicedNext = 0; bool motorMotorsEnabled = false; + }; -#endif /* STEPPERCONTROL_H_ */ +#endif /* MOVEMENT_H_ */ diff --git a/src/StepperControlAxis.cpp b/src/MovementAxis.cpp similarity index 54% rename from src/StepperControlAxis.cpp rename to src/MovementAxis.cpp index 72bd76b..b838060 100644 --- a/src/StepperControlAxis.cpp +++ b/src/MovementAxis.cpp @@ -1,6 +1,22 @@ -#include "StepperControlAxis.h" +/* + * MovementAxis.cpp + * + * Created on: 18 juli 2015 + * Author: Tim Evers + */ -StepperControlAxis::StepperControlAxis() +#include "MovementAxis.h" + +/* +#if defined(FARMDUINO_EXP_V20) + static TMC2130Stepper TMC2130X = TMC2130Stepper(X_ENABLE_PIN, X_DIR_PIN, X_STEP_PIN, X_CHIP_SELECT); + static TMC2130Stepper TMC2130Y = TMC2130Stepper(Y_ENABLE_PIN, Y_DIR_PIN, Y_STEP_PIN, Y_CHIP_SELECT); + static TMC2130Stepper TMC2130Z = TMC2130Stepper(Z_ENABLE_PIN, Z_DIR_PIN, Z_STEP_PIN, Z_CHIP_SELECT); + static TMC2130Stepper TMC2130E = TMC2130Stepper(E_ENABLE_PIN, E_DIR_PIN, E_STEP_PIN, E_CHIP_SELECT); +#endif +*/ + +MovementAxis::MovementAxis() { lastCalcLog = 0; @@ -33,27 +49,203 @@ StepperControlAxis::StepperControlAxis() stepIsOn = false; - setMotorStepWrite = &StepperControlAxis::setMotorStepWriteDefault; - setMotorStepWrite2 = &StepperControlAxis::setMotorStepWriteDefault2; - resetMotorStepWrite = &StepperControlAxis::resetMotorStepWriteDefault; - resetMotorStepWrite2 = &StepperControlAxis::resetMotorStepWriteDefault2; + setMotorStepWrite = &MovementAxis::setMotorStepWriteDefault; + setMotorStepWrite2 = &MovementAxis::setMotorStepWriteDefault2; + resetMotorStepWrite = &MovementAxis::resetMotorStepWriteDefault; + resetMotorStepWrite2 = &MovementAxis::resetMotorStepWriteDefault2; } -void StepperControlAxis::test() +void MovementAxis::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"); + + Serial.print("Check timing "); + Serial.print(" motorInterruptSpeed "); + Serial.print(motorInterruptSpeed); + Serial.print(" axisSpeed "); + Serial.print(axisSpeed); + Serial.print(" active "); + Serial.print(axisActive); + Serial.print(" move ticks "); + Serial.print(moveTicks); + Serial.print(" step on "); + Serial.print(stepIsOn); + Serial.print(" tick on "); + Serial.print(stepOnTick); + Serial.print(" tick off "); + Serial.print(stepOffTick); + Serial.print(" mov step done"); + Serial.print(movementStepDone); + Serial.print("\r\n"); + } -unsigned int StepperControlAxis::calculateSpeed(long sourcePosition, long currentPosition, long destinationPosition, long minSpeed, long maxSpeed, long stepsAccDec) +#if defined(FARMDUINO_EXP_V20) + +unsigned int MovementAxis::getLostSteps() +{ + return TMC2130A->LOST_STEPS(); +} + +void MovementAxis::initTMC2130() +{ + + if (channelLabel == 'X') + { + TMC2130A = TMC2130X; + TMC2130B = TMC2130E; + } + if (channelLabel == 'Y') + { + TMC2130A = TMC2130Y; + } + if (channelLabel == 'Z') + { + TMC2130A = TMC2130Z; + } + + //TMC2130A->begin(); // Initiate pins and registeries + //TMC2130A->shaft_dir(0); // Set direction + + if (channelLabel == 'X') + { + //TMC2130B->begin(); // Initiate pins and registeries + //TMC2130B->shaft_dir(0); // Set direction + } + + setMotorStepWrite = &MovementAxis::setMotorStepWriteTMC2130; + setMotorStepWrite2 = &MovementAxis::setMotorStepWriteTMC2130_2; + resetMotorStepWrite = &MovementAxis::resetMotorStepWriteTMC2130; + resetMotorStepWrite2 = &MovementAxis::resetMotorStepWriteTMC2130_2; + +} + +void MovementAxis::loadSettingsTMC2130(int motorCurrent, int stallSensitivity, int microSteps) +{ + /**/ + /* + Serial.println("loading settings"); + + Serial.print("channelLabel"); + Serial.print(" = "); + Serial.print(channelLabel); + Serial.println(" "); + + Serial.print("motorCurrent"); + Serial.print(" = "); + Serial.print(motorCurrent); + Serial.println(" "); + + Serial.print("microSteps"); + Serial.print(" = "); + Serial.print(microSteps); + Serial.println(" "); + + Serial.print("stallSensitivity"); + Serial.print(" = "); + Serial.print(stallSensitivity); + Serial.println(" = "); + */ + +/* + TMC2130A->rms_current(motorCurrent); // Set the required current in mA + TMC2130A->microsteps(microSteps); // Minimum of micro steps needed + TMC2130A->chm(true); // Set the chopper mode to classic const. off time + TMC2130A->diag1_stall(1); // Activate stall diagnostics + TMC2130A->sgt(stallSensitivity); // Set stall detection sensitivity. most -64 to +64 least + //TMC2130A->shaft_dir(0); // Set direction +*/ + + /* + TMC2130A->push(); + TMC2130A->toff(3); + TMC2130A->tbl(1); + TMC2130A->hysteresis_start(4); + TMC2130A->hysteresis_end(-2); + TMC2130A->rms_current(motorCurrent); // mA + TMC2130A->microsteps(microSteps); + TMC2130A->diag1_stall(1); + TMC2130A->diag1_active_high(1); + TMC2130A->coolstep_min_speed(0xFFFFF); // 20bit max + TMC2130A->THIGH(0); + TMC2130A->semin(5); + TMC2130A->semax(2); + TMC2130A->sedn(0b01); + TMC2130A->sg_stall_value(stallSensitivity); + + + /* + TMC2130A->push(); + TMC2130A->toff(3); + TMC2130A->tbl(1); + TMC2130A->hysteresis_start(4); + TMC2130A->hysteresis_end(-2); + TMC2130A->rms_current(600); // mA + TMC2130A->microsteps(16); + TMC2130A->diag1_stall(1); + TMC2130A->diag1_active_high(1); + TMC2130A->coolstep_min_speed(0xFFFFF); // 20bit max + TMC2130A->THIGH(0); + TMC2130A->semin(5); + TMC2130A->semax(2); + TMC2130A->sedn(0b01); + TMC2130A->sg_stall_value(0); + */ + + if (channelLabel == 'X') + { + /* + TMC2130B->rms_current(motorCurrent); // Set the required current in mA + TMC2130B->microsteps(microSteps); // Minimum of micro steps needed + TMC2130B->chm(true); // Set the chopper mode to classic const. off time + TMC2130B->diag1_stall(1); // Activate stall diagnostics + TMC2130B->sgt(stallSensitivity); // Set stall detection sensitivity. most -64 to +64 least + TMC2130B->shaft_dir(0); // Set direction + */ + + /* + TMC2130B->push(); + TMC2130B->toff(3); + TMC2130B->tbl(1); + TMC2130B->hysteresis_start(4); + TMC2130B->hysteresis_end(-2); + TMC2130B->rms_current(motorCurrent); // mA + TMC2130B->microsteps(microSteps); + TMC2130B->diag1_stall(1); + TMC2130B->diag1_active_high(1); + TMC2130B->coolstep_min_speed(0xFFFFF); // 20bit max + TMC2130B->THIGH(0); + TMC2130B->semin(5); + TMC2130B->semax(2); + TMC2130B->sedn(0b01); + TMC2130B->sg_stall_value(stallSensitivity); + */ + } +} + +bool MovementAxis::stallDetected() { + return TMC2130A->stallguard(); +} + +uint16_t MovementAxis::getLoad() { + //return TMC2130A->sg_result(); + /**/ + return 0; +} + +#endif + +long MovementAxis::getLastPosition() +{ + return axisLastPosition; +} + +void MovementAxis::setLastPosition(long position) +{ + axisLastPosition = position; +} + +unsigned int MovementAxis::calculateSpeed(long sourcePosition, long currentPosition, long destinationPosition, long minSpeed, long maxSpeed, long stepsAccDec) { int newSpeed = 0; @@ -69,20 +261,6 @@ unsigned int StepperControlAxis::calculateSpeed(long sourcePosition, long curren movementCrawling = false; movementMoving = false; - - /* - if (abs(sourcePosition) < abs(destinationPosition)) - { - staPos = abs(sourcePosition); - endPos = abs(destinationPosition); - } - else - { - staPos = abs(destinationPosition); - endPos = abs(sourcePosition); - } - */ - // Set the possible negative coordinates to all positive numbers // so the calculation code still works after the changes staPos = 0; @@ -190,7 +368,7 @@ unsigned int StepperControlAxis::calculateSpeed(long sourcePosition, long curren return newSpeed; } -void StepperControlAxis::checkAxisDirection() +void MovementAxis::checkAxisDirection() { if (coordHomeAxis) @@ -213,7 +391,7 @@ void StepperControlAxis::checkAxisDirection() } } -void StepperControlAxis::setDirectionAxis() +void MovementAxis::setDirectionAxis() { if (((!coordHomeAxis && coordCurrentPoint < coordDestinationPoint) || (coordHomeAxis && motorHomeIsUp))) @@ -226,7 +404,7 @@ void StepperControlAxis::setDirectionAxis() } } -void StepperControlAxis::checkMovement() +void MovementAxis::checkMovement() { checkAxisDirection(); @@ -250,15 +428,6 @@ void StepperControlAxis::checkMovement() 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 { @@ -278,30 +447,25 @@ void StepperControlAxis::checkMovement() } } -void StepperControlAxis::incrementTick() +void MovementAxis::incrementTick() { if (axisActive) { moveTicks++; - //moveTicks+=3; } } -void StepperControlAxis::checkTiming() +void MovementAxis::checkTiming() { - - //int i; - - // moveTicks++; if (stepIsOn) { if (moveTicks >= stepOffTick) { - // Negative flank for the steps resetMotorStep(); setTicks(); - //stepOnTick = moveTicks + (500000.0 / motorInterruptSpeed / axisSpeed); + + //test(); } } else @@ -310,16 +474,16 @@ void StepperControlAxis::checkTiming() { if (moveTicks >= stepOnTick) { - // Positive flank for the steps setStepAxis(); - //stepOffTick = moveTicks + (1000000.0 / motorInterruptSpeed / axisSpeed); + + //test(); } } } } -void StepperControlAxis::setTicks() +void MovementAxis::setTicks() { // 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 @@ -327,7 +491,7 @@ void StepperControlAxis::setTicks() stepOffTick = moveTicks + (1000.0 * 1000.0 / motorInterruptSpeed / axisSpeed); } -void StepperControlAxis::setStepAxis() +void MovementAxis::setStepAxis() { stepIsOn = true; @@ -345,7 +509,7 @@ void StepperControlAxis::setStepAxis() setMotorStep(); } -bool StepperControlAxis::endStopAxisReached(bool movement_forward) +bool MovementAxis::endStopAxisReached(bool movement_forward) { bool min_endstop = false; @@ -381,7 +545,7 @@ bool StepperControlAxis::endStopAxisReached(bool movement_forward) return 0; } -void StepperControlAxis::StepperControlAxis::loadPinNumbers(int step, int dir, int enable, int min, int max, int step2, int dir2, int enable2) +void MovementAxis::MovementAxis::loadPinNumbers(int step, int dir, int enable, int min, int max, int step2, int dir2, int enable2) { pinStep = step; pinDirection = dir; @@ -395,7 +559,7 @@ void StepperControlAxis::StepperControlAxis::loadPinNumbers(int step, int dir, i pinMax = max; } -void StepperControlAxis::loadMotorSettings( +void MovementAxis::loadMotorSettings( long speedMax, long speedMin, long speedHome, long stepsAcc, long timeOut, bool homeIsUp, bool motorInv, bool endStInv, bool endStInv2, long interruptSpeed, bool motor2Enbl, bool motor2Inv, bool endStEnbl, bool stopAtHome, long maxSize, bool stopAtMax) @@ -420,32 +584,32 @@ void StepperControlAxis::loadMotorSettings( if (pinStep == 54) { - setMotorStepWrite = &StepperControlAxis::setMotorStepWrite54; - resetMotorStepWrite = &StepperControlAxis::resetMotorStepWrite54; + setMotorStepWrite = &MovementAxis::setMotorStepWrite54; + resetMotorStepWrite = &MovementAxis::resetMotorStepWrite54; } if (pinStep == 60) { - setMotorStepWrite = &StepperControlAxis::setMotorStepWrite60; - resetMotorStepWrite = &StepperControlAxis::resetMotorStepWrite60; + setMotorStepWrite = &MovementAxis::setMotorStepWrite60; + resetMotorStepWrite = &MovementAxis::resetMotorStepWrite60; } if (pinStep == 46) { - setMotorStepWrite = &StepperControlAxis::setMotorStepWrite46; - resetMotorStepWrite = &StepperControlAxis::resetMotorStepWrite46; + setMotorStepWrite = &MovementAxis::setMotorStepWrite46; + resetMotorStepWrite = &MovementAxis::resetMotorStepWrite46; } if (pin2Step == 26) { - setMotorStepWrite2 = &StepperControlAxis::setMotorStepWrite26; - resetMotorStepWrite2 = &StepperControlAxis::resetMotorStepWrite26; + setMotorStepWrite2 = &MovementAxis::setMotorStepWrite26; + resetMotorStepWrite2 = &MovementAxis::resetMotorStepWrite26; } } -bool StepperControlAxis::loadCoordinates(long sourcePoint, long destinationPoint, bool home) +bool MovementAxis::loadCoordinates(long sourcePoint, long destinationPoint, bool home) { coordSourcePoint = sourcePoint; @@ -497,7 +661,7 @@ bool StepperControlAxis::loadCoordinates(long sourcePoint, long destinationPoint return changed; } -void StepperControlAxis::enableMotor() +void MovementAxis::enableMotor() { digitalWrite(pinEnable, LOW); if (motorMotor2Enl) @@ -507,7 +671,7 @@ void StepperControlAxis::enableMotor() movementMotorActive = true; } -void StepperControlAxis::disableMotor() +void MovementAxis::disableMotor() { digitalWrite(pinEnable, HIGH); if (motorMotor2Enl) @@ -517,9 +681,10 @@ void StepperControlAxis::disableMotor() movementMotorActive = false; } -void StepperControlAxis::setDirectionUp() +void MovementAxis::setDirectionUp() { +//#if !defined(FARMDUINO_EXP_V20) if (motorMotorInv) { digitalWrite(pinDirection, LOW); @@ -537,10 +702,38 @@ void StepperControlAxis::setDirectionUp() { digitalWrite(pin2Direction, HIGH); } +//#endif + +/* +#if defined(FARMDUINO_EXP_V20) + + // The TMC2130 uses a command to change direction, not a pin + if (motorMotorInv) + { + TMC2130A->shaft_dir(0); + } + else + { + TMC2130A->shaft_dir(1); + } + + if (motorMotor2Enl && motorMotor2Inv) + { + TMC2130B->shaft_dir(0); + } + else + { + TMC2130B->shaft_dir(1); + } + +#endif +*/ } -void StepperControlAxis::setDirectionDown() +void MovementAxis::setDirectionDown() { + //#if !defined(FARMDUINO_EXP_V20) + if (motorMotorInv) { digitalWrite(pinDirection, HIGH); @@ -558,19 +751,45 @@ void StepperControlAxis::setDirectionDown() { digitalWrite(pin2Direction, LOW); } + + //#endif +/* + #if defined(FARMDUINO_EXP_V20) + + // The TMC2130 uses a command to change direction, not a pin + if (motorMotorInv) + { + TMC2130A->shaft_dir(1); + } + else + { + TMC2130A->shaft_dir(0); + } + + if (motorMotor2Enl && motorMotor2Inv) + { + TMC2130B->shaft_dir(1); + } + else + { + TMC2130B->shaft_dir(0); + } + + #endif +*/ } -void StepperControlAxis::setMovementUp() +void MovementAxis::setMovementUp() { movementUp = true; } -void StepperControlAxis::setMovementDown() +void MovementAxis::setMovementDown() { movementUp = false; } -void StepperControlAxis::setDirectionHome() +void MovementAxis::setDirectionHome() { if (motorHomeIsUp) { @@ -584,7 +803,7 @@ void StepperControlAxis::setDirectionHome() } } -void StepperControlAxis::setDirectionAway() +void MovementAxis::setDirectionAway() { if (motorHomeIsUp) { @@ -598,7 +817,7 @@ void StepperControlAxis::setDirectionAway() } } -unsigned long StepperControlAxis::getLength(long l1, long l2) +unsigned long MovementAxis::getLength(long l1, long l2) { if (l1 > l2) { @@ -610,34 +829,34 @@ unsigned long StepperControlAxis::getLength(long l1, long l2) } } -bool StepperControlAxis::endStopsReached() +bool MovementAxis::endStopsReached() { return ((digitalRead(pinMin) == motorEndStopInv2) || (digitalRead(pinMax) == motorEndStopInv2)) && motorEndStopEnbl; } -bool StepperControlAxis::endStopMin() +bool MovementAxis::endStopMin() { //return ((digitalRead(pinMin) == motorEndStopInv) || (digitalRead(pinMax) == motorEndStopInv)); return ((digitalRead(pinMin) == motorEndStopInv2) && motorEndStopEnbl); } -bool StepperControlAxis::endStopMax() +bool MovementAxis::endStopMax() { //return ((digitalRead(pinMin) == motorEndStopInv) || (digitalRead(pinMax) == motorEndStopInv)); return ((digitalRead(pinMax) == motorEndStopInv2) && motorEndStopEnbl); } -bool StepperControlAxis::isAxisActive() +bool MovementAxis::isAxisActive() { return axisActive; } -void StepperControlAxis::deactivateAxis() +void MovementAxis::deactivateAxis() { axisActive = false; } -void StepperControlAxis::setMotorStep() +void MovementAxis::setMotorStep() { stepIsOn = true; @@ -651,7 +870,7 @@ void StepperControlAxis::setMotorStep() } } -void StepperControlAxis::resetMotorStep() +void MovementAxis::resetMotorStep() { stepIsOn = false; movementStepDone = true; @@ -666,77 +885,77 @@ void StepperControlAxis::resetMotorStep() } } -bool StepperControlAxis::pointReached(long currentPoint, long destinationPoint) +bool MovementAxis::pointReached(long currentPoint, long destinationPoint) { return (destinationPoint == currentPoint); } -long StepperControlAxis::currentPosition() +long MovementAxis::currentPosition() { return coordCurrentPoint; } -void StepperControlAxis::setCurrentPosition(long newPos) +void MovementAxis::setCurrentPosition(long newPos) { coordCurrentPoint = newPos; } -long StepperControlAxis::destinationPosition() +long MovementAxis::destinationPosition() { return coordDestinationPoint; } -void StepperControlAxis::setMaxSpeed(long speed) +void MovementAxis::setMaxSpeed(long speed) { motorSpeedMax = speed; } -void StepperControlAxis::activateDebugPrint() +void MovementAxis::activateDebugPrint() { debugPrint = true; } -bool StepperControlAxis::isStepDone() +bool MovementAxis::isStepDone() { return movementStepDone; } -void StepperControlAxis::resetStepDone() +void MovementAxis::resetStepDone() { movementStepDone = false; } -bool StepperControlAxis::movingToHome() +bool MovementAxis::movingToHome() { return movementToHome; } -bool StepperControlAxis::movingUp() +bool MovementAxis::movingUp() { return movementUp; } -bool StepperControlAxis::isAccelerating() +bool MovementAxis::isAccelerating() { return movementAccelerating; } -bool StepperControlAxis::isDecelerating() +bool MovementAxis::isDecelerating() { return movementDecelerating; } -bool StepperControlAxis::isCruising() +bool MovementAxis::isCruising() { return movementCruising; } -bool StepperControlAxis::isCrawling() +bool MovementAxis::isCrawling() { return movementCrawling; } -bool StepperControlAxis::isMotorActive() +bool MovementAxis::isMotorActive() { return movementMotorActive; } @@ -744,34 +963,34 @@ bool StepperControlAxis::isMotorActive() /// Functions for pin writing using alternative method // Pin write default functions -void StepperControlAxis::setMotorStepWriteDefault() +void MovementAxis::setMotorStepWriteDefault() { digitalWrite(pinStep, HIGH); } -void StepperControlAxis::setMotorStepWriteDefault2() +void MovementAxis::setMotorStepWriteDefault2() { digitalWrite(pin2Step, HIGH); } -void StepperControlAxis::resetMotorStepWriteDefault() +void MovementAxis::resetMotorStepWriteDefault() { digitalWrite(pinStep, LOW); } -void StepperControlAxis::resetMotorStepWriteDefault2() +void MovementAxis::resetMotorStepWriteDefault2() { digitalWrite(pin2Step, LOW); } // X step -void StepperControlAxis::setMotorStepWrite54() +void MovementAxis::setMotorStepWrite54() { //PF0 PORTF |= B00000001; } -void StepperControlAxis::resetMotorStepWrite54() +void MovementAxis::resetMotorStepWrite54() { //PF0 PORTF &= B11111110; @@ -779,39 +998,85 @@ void StepperControlAxis::resetMotorStepWrite54() // X step 2 -void StepperControlAxis::setMotorStepWrite26() +void MovementAxis::setMotorStepWrite26() { //PA4 PORTA |= B00010000; } -void StepperControlAxis::resetMotorStepWrite26() +void MovementAxis::resetMotorStepWrite26() { PORTA &= B11101111; } // Y step -void StepperControlAxis::setMotorStepWrite60() +void MovementAxis::setMotorStepWrite60() { //PF6 PORTF |= B01000000; } -void StepperControlAxis::resetMotorStepWrite60() +void MovementAxis::resetMotorStepWrite60() { //PF6 PORTF &= B10111111; } // Z step -void StepperControlAxis::setMotorStepWrite46() +void MovementAxis::setMotorStepWrite46() { //PL3 PORTL |= B00001000; } -void StepperControlAxis::resetMotorStepWrite46() +void MovementAxis::resetMotorStepWrite46() { //PL3 PORTL &= B11110111; } + +#if defined(FARMDUINO_EXP_V20) +//// TMC2130 Functions + +void MovementAxis::setMotorStepWriteTMC2130() +{ + // TMC2130 works on each edge of the step pulse, + // so instead of setting the step bit, + // toggle the bit here + + if (tmcStep) + { + digitalWrite(pinStep, HIGH); + tmcStep = false; + } + else + { + digitalWrite(pinStep, LOW); + tmcStep = true; + } +} + +void MovementAxis::setMotorStepWriteTMC2130_2() +{ + if (tmcStep2) + { + digitalWrite(pin2Step, HIGH); + tmcStep2 = false; + } + else + { + digitalWrite(pin2Step, LOW); + tmcStep2 = true; + } +} + +void MovementAxis::resetMotorStepWriteTMC2130() +{ + // No action needed +} + +void MovementAxis::resetMotorStepWriteTMC2130_2() +{ + // No action needed +} +#endif diff --git a/src/StepperControlAxis.h b/src/MovementAxis.h similarity index 50% rename from src/StepperControlAxis.h rename to src/MovementAxis.h index 43dbc4d..dd128dd 100644 --- a/src/StepperControlAxis.h +++ b/src/MovementAxis.h @@ -1,27 +1,37 @@ /* - * StepperControlAxis.h + * MovementAxis.h * * Created on: 18 juli 2015 * Author: Tim Evers */ -#ifndef STEPPERCONTROLAXIS_H_ -#define STEPPERCONTROLAXIS_H_ +#ifndef MovementAXIS_H_ +#define MovementAXIS_H_ #include "Arduino.h" #include "CurrentState.h" #include "ParameterList.h" #include "pins.h" -#include "StepperControlEncoder.h" +#include "MovementEncoder.h" #include "Config.h" #include #include -class StepperControlAxis +#if defined(FARMDUINO_EXP_V20) + #include "TMC2130.h" +#endif + + +class MovementAxis { public: - StepperControlAxis(); + MovementAxis(); + +#if defined(FARMDUINO_EXP_V20) + TMC2130Stepper *TMC2130A; + TMC2130Stepper *TMC2130B; +#endif 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 speedHome, long stepsAcc, long timeOut, bool homeIsUp, bool motorInv, bool endStInv, bool endStInv2, long interruptSpeed, bool motor2Enbl, bool motor2Inv, bool endStEnbl, bool stopAtHome, long maxSize, bool stopAtMax); @@ -42,7 +52,7 @@ public: bool isCruising(); bool isCrawling(); bool isMotorActive(); - bool isMoving(); + //bool isMoving(); bool endStopMin(); bool endStopMax(); @@ -66,6 +76,9 @@ public: void setMovementUp(); void setMovementDown(); + long getLastPosition(); + void setLastPosition(long position); + bool movingToHome(); bool movingUp(); @@ -75,9 +88,28 @@ public: void activateDebugPrint(); void test(); - char label; + char channelLabel; bool movementStarted; +#if defined(FARMDUINO_EXP_V20) + void initTMC2130(); + void loadSettingsTMC2130(int motorCurrent, int stallSensitivity, int microSteps); + bool stallDetected(); + uint16_t getLoad(); +#endif + +#if defined(FARMDUINO_EXP_V20) + void setMotorStepWriteTMC2130(); + void setMotorStepWriteTMC2130_2(); + void resetMotorStepWriteTMC2130(); + void resetMotorStepWriteTMC2130_2(); + + unsigned int getLostSteps(); + + bool tmcStep = true; + bool tmcStep2 = true; +#endif + private: int lastCalcLog = 0; bool debugPrint = false; @@ -102,56 +134,59 @@ private: 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 motorSpeedHome; // homing 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 - bool motorStopAtHome; // stop at home position or also use other side of the axis - long motorMaxSize; // maximum size of the axis in steps - bool motorStopAtMax; // stop at the maximum size + long motorSpeedMax = 300; // maximum speed in steps per second + long motorSpeedMin = 50; // minimum speed in steps per second + long motorSpeedHome = 50; // homing speed in steps per second + long motorStepsAcc = 300; // number of steps used for acceleration + long motorTimeOut = 120; // timeout in seconds + bool motorHomeIsUp = false; // direction to move when reached 0 on axis but no end stop detected while homing + bool motorMotorInv = false; // invert motor direction + bool motorMotor2Enl = false; // enable secondary motor + bool motorMotor2Inv = false; // invert secondary motor direction + long motorInterruptSpeed = 100; // period of interrupt in micro seconds + bool motorStopAtHome = false; // stop at home position or also use other side of the axis + long motorMaxSize = 0; // maximum size of the axis in steps + bool motorStopAtMax = false; // stop at the maximum size // coordinates - long coordSourcePoint; // all coordinated in steps - long coordCurrentPoint; - long coordDestinationPoint; - bool coordHomeAxis; // homing command + long coordSourcePoint = 0; // all coordinated in steps + long coordCurrentPoint = 0; + long coordDestinationPoint = 0; + bool coordHomeAxis = false; // homing command + + long axisLastPosition = 0; // movement handling - unsigned long movementLength; - unsigned long maxLenth; - unsigned long moveTicks; - unsigned long stepOnTick; - unsigned long stepOffTick; - unsigned long axisSpeed; + unsigned long movementLength = 0; + unsigned long maxLenth = 0; + unsigned long moveTicks = 0; + unsigned long stepOnTick = 0; + unsigned long stepOffTick = 0; + unsigned long axisSpeed = 0; + + bool axisActive = false; + bool movementUp = false; + bool movementToHome = false; + bool movementStepDone = false; + bool movementAccelerating = false; + bool movementDecelerating = false; + bool movementCruising = false; + bool movementCrawling = false; + bool movementMotorActive = false; + bool movementMoving = false; + bool stepIsOn = false; - bool axisActive; - bool movementUp; - bool movementToHome; - bool movementStepDone; - bool movementAccelerating; - bool movementDecelerating; - bool movementCruising; - bool movementCrawling; - bool movementMotorActive; - bool movementMoving; - bool stepIsOn; 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(); - void (StepperControlAxis::*setMotorStepWrite)(); - void (StepperControlAxis::*setMotorStepWrite2)(); - void (StepperControlAxis::*resetMotorStepWrite)(); - void (StepperControlAxis::*resetMotorStepWrite2)(); + void (MovementAxis::*setMotorStepWrite)(); + void (MovementAxis::*setMotorStepWrite2)(); + void (MovementAxis::*resetMotorStepWrite)(); + void (MovementAxis::*resetMotorStepWrite2)(); void setMotorStepWriteDefault(); void setMotorStepWriteDefault2(); @@ -168,4 +203,4 @@ private: }; -#endif /* STEPPERCONTROLAXIS_H_ */ +#endif /* MovementAXIS_H_ */ diff --git a/src/StepperControlEncoder.cpp b/src/MovementEncoder.cpp similarity index 68% rename from src/StepperControlEncoder.cpp rename to src/MovementEncoder.cpp index 59e4533..c8f776f 100644 --- a/src/StepperControlEncoder.cpp +++ b/src/MovementEncoder.cpp @@ -1,6 +1,6 @@ -#include "StepperControlEncoder.h" +#include "MovementEncoder.h" -StepperControlEncoder::StepperControlEncoder() +MovementEncoder::MovementEncoder() { //lastCalcLog = 0; @@ -24,7 +24,7 @@ StepperControlEncoder::StepperControlEncoder() mdlEncoder = _MDL_X1; } -void StepperControlEncoder::test() +void MovementEncoder::test() { /* Serial.print("R88 "); @@ -42,7 +42,7 @@ void StepperControlEncoder::test() */ } -void StepperControlEncoder::loadPinNumbers(int channelA, int channelB, int channelAQ, int channelBQ) +void MovementEncoder::loadPinNumbers(int channelA, int channelB, int channelAQ, int channelBQ) { pinChannelA = channelA; pinChannelB = channelB; @@ -53,7 +53,7 @@ void StepperControlEncoder::loadPinNumbers(int channelA, int channelB, int chann shiftChannels(); } -void StepperControlEncoder::loadSettings(int encType, long scaling, int invert) +void MovementEncoder::loadSettings(int encType, long scaling, int invert) { encoderType = encType; scalingFactor = scaling; @@ -69,12 +69,12 @@ void StepperControlEncoder::loadSettings(int encType, long scaling, int invert) // encoderType = 0; // TEVE 2017-04-20 Disabling the differential channels. They take too much time to read. } -void StepperControlEncoder::loadMdlEncoderId(MdlSpiEncoders encoder) +void MovementEncoder::loadMdlEncoderId(MdlSpiEncoders encoder) { mdlEncoder = encoder; } -void StepperControlEncoder::setPosition(long newPosition) +void MovementEncoder::setPosition(long newPosition) { #if defined(RAMPS_V14) || defined(FARMDUINO_V10) position = newPosition; @@ -94,7 +94,7 @@ void StepperControlEncoder::setPosition(long newPosition) #endif } -long StepperControlEncoder::currentPosition() +long MovementEncoder::currentPosition() { @@ -114,12 +114,12 @@ long StepperControlEncoder::currentPosition() } } -long StepperControlEncoder::currentPositionRaw() +long MovementEncoder::currentPositionRaw() { return position * encoderInvert; } -void StepperControlEncoder::checkEncoder(bool channelA, bool channelB, bool channelAQ, bool channelBQ) +void MovementEncoder::checkEncoder(bool channelA, bool channelB, bool channelAQ, bool channelBQ) { #if defined(RAMPS_V14) || defined(FARMDUINO_V10) shiftChannels(); @@ -149,7 +149,7 @@ rotation -----------------------------------------------------> */ -void StepperControlEncoder::processEncoder() +void MovementEncoder::processEncoder() { #if defined(RAMPS_V14) || defined(FARMDUINO_V10) @@ -190,7 +190,7 @@ void StepperControlEncoder::processEncoder() } -void StepperControlEncoder::readChannels() +void MovementEncoder::readChannels() { #if defined(RAMPS_V14) || defined(FARMDUINO_V10) @@ -223,7 +223,7 @@ void StepperControlEncoder::readChannels() } -void StepperControlEncoder::setChannels(bool channelA, bool channelB, bool channelAQ, bool channelBQ) +void MovementEncoder::setChannels(bool channelA, bool channelB, bool channelAQ, bool channelBQ) { // read the new values from the coder @@ -245,7 +245,7 @@ void StepperControlEncoder::setChannels(bool channelA, bool channelB, bool chann } } -void StepperControlEncoder::shiftChannels() +void MovementEncoder::shiftChannels() { // Save the current enoder status to later on compare with new values @@ -253,3 +253,78 @@ void StepperControlEncoder::shiftChannels() prvValChannelA = curValChannelA; prvValChannelB = curValChannelB; } + + +void MovementEncoder::setEnable(bool enable) +{ + encoderEnabled = enable; +} + +void MovementEncoder::setStepDecay(float stepDecay) +{ + encoderStepDecay = stepDecay; +} + +void MovementEncoder::setMovementDirection(bool up) +{ + encoderMovementUp = up; +} + +float MovementEncoder::getMissedSteps() +{ + return missedSteps; +} + +void MovementEncoder::checkMissedSteps() +{ + #if !defined(FARMDUINO_EXP_V20) + if (encoderEnabled) + { + bool stepMissed = false; + + // 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 ((encoderMovementUp && encoderLastPosition > currentPositionRaw()) || + (!encoderMovementUp && encoderLastPosition < currentPositionRaw())) + { + stepMissed = true; + } + + if (stepMissed && missedSteps < 32000) + { + (missedSteps)++; + } + + encoderLastPosition = currentPositionRaw(); + //axis->setLastPosition(axis->currentPosition()); + } + + #endif + +/* + #if defined(FARMDUINO_EXP_V20) + + if (encoderEnabled) { + if (axis->stallDetected()) { + // In case of stall detection, count this as a missed step + (*missedSteps)++; + //axis->setCurrentPosition(*lastPosition); + } + else { + // Decrease amount of missed steps if there are no missed step + if (missedSteps > 0) + { + (missedSteps) -= (encoderStepDecay); + } + setPosition(axis->currentPosition()); + //axis->setLastPosition(axis->currentPosition()); + } + } + #endif +*/ +} \ No newline at end of file diff --git a/src/MovementEncoder.h b/src/MovementEncoder.h new file mode 100644 index 0000000..b932277 --- /dev/null +++ b/src/MovementEncoder.h @@ -0,0 +1,87 @@ +/* + * MovementEncoder.h + * + * Created on: 29 okt 2015 + * Author: Tim Evers + */ + +#ifndef MovementENCODER_H_ +#define MovementENCODER_H_ + +#include "Arduino.h" +//#include "CurrentState.h" +//#include "ParameterList.h" +#include "pins.h" +#include "Config.h" +#include +#include +#include +#include "MovementAxis.h" + +class MovementEncoder +{ + +public: + MovementEncoder(); + + void loadPinNumbers(int channelA, int channelB, int channelAQ, int channelBQ); + void loadSettings(int encType, long scaling, int invert); + + // Load the id for the motor dynamics lab encoder + void loadMdlEncoderId(MdlSpiEncoders encoder); + + void setPosition(long newPosition); + long currentPosition(); + long currentPositionRaw(); + + void checkEncoder(bool channelA, bool channelB, bool channelAQ, bool channelBQ); + void processEncoder(); + void readChannels(); + void setChannels(bool channelA, bool channelB, bool channelAQ, bool channelBQ); + void shiftChannels(); + void test(); + + void setMovementDirection(bool up); + void setEnable(bool enable); + void setStepDecay(float stepDecay); + float getMissedSteps(); + + void checkMissedSteps(); + +private: + // pin settings + int pinChannelA = 0; + int pinChannelAQ = 0; + int pinChannelB = 0; + int pinChannelBQ = 0; + + // channels + bool prvValChannelA = false; + bool prvValChannelB = false; + bool curValChannelA = false; + bool curValChannelB = false; + + bool readChannelA = false; + bool readChannelAQ = false; + bool readChannelB = false; + bool readChannelBQ = false; + + // encoder + long position = 0; + long scalingFactor = 0; + float floatScalingFactor = 0; + int encoderType = 0; + int encoderInvert = 0; + + + float missedSteps = 0; + long encoderLastPosition = 0; + float encoderStepDecay = 0; + bool encoderEnabled = false; + bool encoderMovementUp = false; + + MdlSpiEncoders mdlEncoder = _MDL_X1; + +}; + +#endif /* MovementENCODER_H_ */ diff --git a/src/ParameterList.cpp b/src/ParameterList.cpp index 5640a3f..acb74fa 100644 --- a/src/ParameterList.cpp +++ b/src/ParameterList.cpp @@ -439,6 +439,36 @@ void ParameterList::loadDefaultValue(int id) paramValues[id] = MOVEMENT_STOP_AT_MAX_Z_DEFAULT; break; + case MOVEMENT_MOTOR_CURRENT_X: + paramValues[id] = MOVEMENT_MOTOR_CURRENT_X_DEFAULT; + break; + case MOVEMENT_MOTOR_CURRENT_Y: + paramValues[id] = MOVEMENT_MOTOR_CURRENT_Y_DEFAULT; + break; + case MOVEMENT_MOTOR_CURRENT_Z: + paramValues[id] = MOVEMENT_MOTOR_CURRENT_Z_DEFAULT; + break; + + case MOVEMENT_STALL_SENSITIVITY_X: + paramValues[id] = MOVEMENT_STALL_SENSITIVITY_X_DEFAULT; + break; + case MOVEMENT_STALL_SENSITIVITY_Y: + paramValues[id] = MOVEMENT_STALL_SENSITIVITY_Y_DEFAULT; + break; + case MOVEMENT_STALL_SENSITIVITY_Z: + paramValues[id] = MOVEMENT_STALL_SENSITIVITY_Z_DEFAULT; + break; + + case MOVEMENT_MICROSTEPS_X: + paramValues[id] = MOVEMENT_MICROSTEPS_X_DEFAULT; + break; + case MOVEMENT_MICROSTEPS_Y: + paramValues[id] = MOVEMENT_MICROSTEPS_Y_DEFAULT; + break; + case MOVEMENT_MICROSTEPS_Z: + paramValues[id] = MOVEMENT_MICROSTEPS_Z_DEFAULT; + break; + case ENCODER_ENABLED_X: paramValues[id] = ENCODER_ENABLED_X_DEFAULT; break; @@ -620,6 +650,15 @@ bool ParameterList::validParam(int id) case MOVEMENT_MAX_SPD_X: case MOVEMENT_MAX_SPD_Y: case MOVEMENT_MAX_SPD_Z: + case MOVEMENT_MOTOR_CURRENT_X: + case MOVEMENT_MOTOR_CURRENT_Y: + case MOVEMENT_MOTOR_CURRENT_Z: + case MOVEMENT_STALL_SENSITIVITY_X: + case MOVEMENT_STALL_SENSITIVITY_Y: + case MOVEMENT_STALL_SENSITIVITY_Z: + case MOVEMENT_MICROSTEPS_X: + case MOVEMENT_MICROSTEPS_Y: + case MOVEMENT_MICROSTEPS_Z: case ENCODER_ENABLED_X: case ENCODER_ENABLED_Y: case ENCODER_ENABLED_Z: diff --git a/src/ParameterList.h b/src/ParameterList.h index 1962d7e..4c21721 100644 --- a/src/ParameterList.h +++ b/src/ParameterList.h @@ -81,6 +81,21 @@ enum ParamListEnum MOVEMENT_INVERT_2_ENDPOINTS_Y = 76, MOVEMENT_INVERT_2_ENDPOINTS_Z = 77, + // motor current (used with TMC2130) + MOVEMENT_MOTOR_CURRENT_X = 81, + MOVEMENT_MOTOR_CURRENT_Y = 82, + MOVEMENT_MOTOR_CURRENT_Z = 83, + + // stall sensitivity (used with TMC2130) + MOVEMENT_STALL_SENSITIVITY_X = 85, + MOVEMENT_STALL_SENSITIVITY_Y = 86, + MOVEMENT_STALL_SENSITIVITY_Z = 87, + + // microstepping (used with TMC2130) + MOVEMENT_MICROSTEPS_X = 91, + MOVEMENT_MICROSTEPS_Y = 92, + MOVEMENT_MICROSTEPS_Z = 93, + // encoder settings ENCODER_ENABLED_X = 101, ENCODER_ENABLED_Y = 102, diff --git a/src/README.md b/src/README.md deleted file mode 100644 index b6dc710..0000000 --- a/src/README.md +++ /dev/null @@ -1,7 +0,0 @@ -farmbot-arduino-controller -========================== -This software is responsible for receiving G-Codes from the Raspberry Pi, execute them and report back the results. - -Technicals -========================== -Created with eclipseArduino V2 - For more details see http://www.baeyens.it/eclipse/ diff --git a/src/StepperControlEncoder.h b/src/StepperControlEncoder.h deleted file mode 100644 index 6bd07e9..0000000 --- a/src/StepperControlEncoder.h +++ /dev/null @@ -1,72 +0,0 @@ -/* - * StepperControlEncoder.h - * - * Created on: 29 okt 2015 - * Author: Tim Evers - */ - -#ifndef STEPPERCONTROLENCODER_H_ -#define STEPPERCONTROLENCODER_H_ - -#include "Arduino.h" -//#include "CurrentState.h" -//#include "ParameterList.h" -#include "pins.h" -#include "Config.h" -#include -#include -#include - -class StepperControlEncoder -{ - -public: - StepperControlEncoder(); - - void loadPinNumbers(int channelA, int channelB, int channelAQ, int channelBQ); - void loadSettings(int encType, long scaling, int invert); - - // Load the id for the motor dynamics lab encoder - void loadMdlEncoderId(MdlSpiEncoders encoder); - - void setPosition(long newPosition); - long currentPosition(); - long currentPositionRaw(); - - void checkEncoder(bool channelA, bool channelB, bool channelAQ, bool channelBQ); - void processEncoder(); - void readChannels(); - void setChannels(bool channelA, bool channelB, bool channelAQ, bool channelBQ); - 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; - long scalingFactor; - float floatScalingFactor; - int encoderType; - int encoderInvert; - - MdlSpiEncoders mdlEncoder = _MDL_X1; - -}; - -#endif /* STEPPERCONTROLENCODER_H_ */ diff --git a/src/TMC2130.cpp b/src/TMC2130.cpp new file mode 100644 index 0000000..1bb18f7 --- /dev/null +++ b/src/TMC2130.cpp @@ -0,0 +1,74 @@ +/* + * TMC2130.cpp + * + * Created on: 03 nov 2019 + * Author: Tim Evers + */ + +#include "TMC2130.h" + +/* +static TMC2130Stepper *TMC2130X; +static TMC2130Stepper *TMC2130Y; +static TMC2130Stepper *TMC2130Z; +static TMC2130Stepper *TMC2130E; +*/ + +/* +static TMC2130Stepper TMC2130_X = TMC2130Stepper(X_ENABLE_PIN, X_DIR_PIN, X_STEP_PIN, X_CHIP_SELECT); +static TMC2130Stepper TMC2130_Y = TMC2130Stepper(Y_ENABLE_PIN, Y_DIR_PIN, Y_STEP_PIN, Y_CHIP_SELECT); +static TMC2130Stepper TMC2130_Z = TMC2130Stepper(Z_ENABLE_PIN, Z_DIR_PIN, Z_STEP_PIN, Z_CHIP_SELECT); +static TMC2130Stepper TMC2130_E = TMC2130Stepper(E_ENABLE_PIN, E_DIR_PIN, E_STEP_PIN, E_CHIP_SELECT); +*/ + +/* +static TMC2130Holder *instance; + +TMC2130Holder *TMC2130Holder::getInstance() +{ + if (!instance) + { + instance = new TMC2130Holder(); + }; + return instance; +}; + +TMC2130Holder::TMC2130Holder() +{ +} + +void TMC2130Holder::loadDrivers() +{ + TMC2130Stepper TMC2130X = new TMC2130Stepper(X_ENABLE_PIN, X_DIR_PIN, X_STEP_PIN, X_CHIP_SELECT); + TMC2130Stepper TMC2130Y = new TMC2130Stepper(Y_ENABLE_PIN, Y_DIR_PIN, Y_STEP_PIN, Y_CHIP_SELECT); + TMC2130Stepper TMC2130Z = new TMC2130Stepper(Z_ENABLE_PIN, Z_DIR_PIN, Z_STEP_PIN, Z_CHIP_SELECT); + TMC2130Stepper TMC2130E = new TMC2130Stepper(E_ENABLE_PIN, E_DIR_PIN, E_STEP_PIN, E_CHIP_SELECT); + + tmcTMC2130X = &TMC2130X; + tmcTMC2130Y = &TMC2130Y; + tmcTMC2130Z = &TMC2130Z; + tmcTMC2130E = &TMC2130E; + +} + + +TMC2130Stepper* TMC2130Holder::TMC2130X() +{ + return tmcTMC2130X; +} + +TMC2130Stepper* TMC2130Holder::TMC2130Y() +{ + return tmcTMC2130Y; +} + +TMC2130Stepper* TMC2130Holder::TMC2130Z() +{ + return tmcTMC2130Z; +} +c +TMC2130Stepper* TMC2130Holder::TMC2130E() +{ + return tmcTMC213EZ; +} +*/ \ No newline at end of file diff --git a/src/TMC2130.h b/src/TMC2130.h new file mode 100644 index 0000000..84b82d5 --- /dev/null +++ b/src/TMC2130.h @@ -0,0 +1,78 @@ +/* + * TMC2130.h + * + * Created on: 03 nov 2019 + * Author: Tim Evers + */ + + +#ifndef TMC2130_H_ +#define TMC2130_H_ + +//#include +//#include + +#include "TMC2130/TMC2130Stepper.h" +#include "TMC2130/TMC2130Stepper_REGDEFS.h" + +#include "pins.h" + +static TMC2130Stepper *TMC2130X; +static TMC2130Stepper *TMC2130Y; +static TMC2130Stepper *TMC2130Z; +static TMC2130Stepper *TMC2130E; + + +//static TMC2130Stepper TMC2130_X = TMC2130Stepper(X_ENABLE_PIN, X_DIR_PIN, X_STEP_PIN, X_CHIP_SELECT); +//static TMC2130Stepper TMC2130_Y = TMC2130Stepper(Y_ENABLE_PIN, Y_DIR_PIN, Y_STEP_PIN, Y_CHIP_SELECT); +//static TMC2130Stepper TMC2130_Z = TMC2130Stepper(Z_ENABLE_PIN, Z_DIR_PIN, Z_STEP_PIN, Z_CHIP_SELECT); +//static TMC2130Stepper TMC2130_E = TMC2130Stepper(E_ENABLE_PIN, E_DIR_PIN, E_STEP_PIN, E_CHIP_SELECT); + +//static TMC2130Stepper TMC2130X = TMC2130Stepper(X_ENABLE_PIN, X_DIR_PIN, X_STEP_PIN, X_CHIP_SELECT); +//static TMC2130Stepper TMC2130Y = TMC2130Stepper(Y_ENABLE_PIN, Y_DIR_PIN, Y_STEP_PIN, Y_CHIP_SELECT); +//static TMC2130Stepper TMC2130Z = TMC2130Stepper(Z_ENABLE_PIN, Z_DIR_PIN, Z_STEP_PIN, Z_CHIP_SELECT); +//static TMC2130Stepper TMC2130E = TMC2130Stepper(E_ENABLE_PIN, E_DIR_PIN, E_STEP_PIN, E_CHIP_SELECT); + +#endif /* TMC2130_H_ */ + + + +/* + +#if defined(FARMDUINO_EXP_V20) + #include + //#include "TMC2130Stepper.h" +#endif + +#ifndef TMC2130Holder_H_ +#define TMC2130Holder_H_ + +class TMC2130Holder +{ +public: + TMC2130Holder(); + TMC2130Holder(TMC2130Holder const &); + void operator=(TMC2130Holder const &); + + static TMC2130Holder *getInstance(); + + void loadDrivers(); + + TMC2130Stepper* TMC2130X(); + TMC2130Stepper* TMC2130Y(); + TMC2130Stepper* TMC2130Z(); + TMC2130Stepper* TMC2130E(); + +private: + + static TMC2130Stepper *tmcTMC2130X; + static TMC2130Stepper *tmcTMC2130Y; + static TMC2130Stepper *tmcTMC2130Z; + static TMC2130Stepper *tmcTMC2130E; + +}; + +#endif +*/ + +/* TMC2130Holder_H_ */ diff --git a/src/TMC2130/SPI/SW_SPI.cpp b/src/TMC2130/SPI/SW_SPI.cpp new file mode 100644 index 0000000..713a544 --- /dev/null +++ b/src/TMC2130/SPI/SW_SPI.cpp @@ -0,0 +1,83 @@ +#include "SW_SPI.h" + +SW_SPIClass TMC_SW_SPI; + +#if defined(ARDUINO_ARCH_AVR) + #define getPort(P) digitalPinToPort(P) + #define writeMOSI_H *mosi_register |= mosi_bm + #define writeMOSI_L *mosi_register &= ~mosi_bm + #define writeSCK_H *sck_register |= sck_bm + #define writeSCK_L *sck_register &= ~sck_bm + #define readMISO *miso_register & miso_bm +#elif defined(ARDUINO_ARCH_SAM) // DUE:1.2MHz + // by stimmer https://forum.arduino.cc/index.php?topic=129868.msg980466#msg980466 + #define writeMOSI_H g_APinDescription[mosi_pin].pPort -> PIO_SODR = g_APinDescription[mosi_pin].ulPin + #define writeMOSI_L g_APinDescription[mosi_pin].pPort -> PIO_CODR = g_APinDescription[mosi_pin].ulPin + #define writeSCK_H g_APinDescription[sck_pin].pPort -> PIO_SODR = g_APinDescription[sck_pin].ulPin + #define writeSCK_L g_APinDescription[sck_pin].pPort -> PIO_CODR = g_APinDescription[sck_pin].ulPin + #define readMISO !!(g_APinDescription[miso_pin].pPort -> PIO_PDSR & g_APinDescription[miso_pin].ulPin) +#else // DUE:116kHz + #define writeMOSI_H digitalWrite(mosi_pin, HIGH) + #define writeMOSI_L digitalWrite(mosi_pin, LOW) + #define writeSCK_H digitalWrite(sck_pin, HIGH) + #define writeSCK_L digitalWrite(sck_pin, LOW) + #define readMISO digitalRead(miso_pin) +#endif + +void SW_SPIClass::setPins(uint16_t sw_mosi_pin, uint16_t sw_miso_pin, uint16_t sw_sck_pin) { + mosi_pin = sw_mosi_pin; + miso_pin = sw_miso_pin; + sck_pin = sw_sck_pin; +} + +void SW_SPIClass::init() { + pinMode(mosi_pin, OUTPUT); + pinMode(sck_pin, OUTPUT); + pinMode(miso_pin, INPUT_PULLUP); + #ifndef TARGET_LPC1768 + mosi_bm = digitalPinToBitMask(mosi_pin); + miso_bm = digitalPinToBitMask(miso_pin); + sck_bm = digitalPinToBitMask(sck_pin); + #ifdef ARDUINO_ARCH_AVR + mosi_register = portOutputRegister(getPort(mosi_pin)); + miso_register = portInputRegister(getPort(miso_pin)); + sck_register = portOutputRegister(getPort(sck_pin)); + #endif + #endif +} + +//Combined shiftOut and shiftIn from Arduino wiring_shift.c +byte SW_SPIClass::transfer(uint8_t ulVal, uint8_t ulBitOrder) { + uint8_t value = 0; + + for (uint8_t i=0 ; i<8 ; ++i) { + // Write bit + if ( ulBitOrder == LSBFIRST ) { + !!(ulVal & (1 << i)) ? writeMOSI_H : writeMOSI_L; + } else { + !!(ulVal & (1 << (7 - i))) ? writeMOSI_H : writeMOSI_L; + } + + // Start clock pulse + writeSCK_H; + + // Read bit + if ( ulBitOrder == LSBFIRST ) { + value |= ( readMISO ? 1 : 0) << i ; + } else { + value |= ( readMISO ? 1 : 0) << (7 - i) ; + } + + // Stop clock pulse + writeSCK_L; + } + + return value; +} + +uint16_t SW_SPIClass::transfer16(uint16_t data) { + uint16_t returnVal = 0x0000; + returnVal |= transfer((data>>8)&0xFF) << 8; + returnVal |= transfer(data&0xFF) & 0xFF; + return returnVal; +} diff --git a/src/TMC2130/SPI/SW_SPI.h b/src/TMC2130/SPI/SW_SPI.h new file mode 100644 index 0000000..ddf236e --- /dev/null +++ b/src/TMC2130/SPI/SW_SPI.h @@ -0,0 +1,25 @@ +#pragma once + +#include + +class SW_SPIClass { + public: + void setPins(uint16_t sw_mosi_pin, uint16_t sw_miso_pin, uint16_t sw_sck_pin); + void init(); + void begin() {}; + byte transfer(uint8_t ulVal, uint8_t ulBitOrder=MSBFIRST); + uint16_t transfer16(uint16_t data); + void endTransaction() {}; + private: + uint16_t mosi_pin, + miso_pin, + sck_pin; + uint8_t mosi_bm, + miso_bm, + sck_bm; + volatile uint8_t *mosi_register, + *miso_register, + *sck_register; +}; + +extern SW_SPIClass TMC_SW_SPI; diff --git a/src/TMC2130/TMC2130Stepper.cpp b/src/TMC2130/TMC2130Stepper.cpp new file mode 100644 index 0000000..81fe7bc --- /dev/null +++ b/src/TMC2130/TMC2130Stepper.cpp @@ -0,0 +1,387 @@ +#include "TMC2130Stepper.h" +#include "TMC2130Stepper_MACROS.h" +#include + +TMC2130Stepper::TMC2130Stepper(uint16_t pinCS) : _pinCS(pinCS) {} + +TMC2130Stepper::TMC2130Stepper(uint16_t pinEN, uint16_t pinDIR, uint16_t pinStep, uint16_t pinCS) : + _pinEN(pinEN), + _pinSTEP(pinStep), + _pinCS(pinCS), + _pinDIR(pinDIR) + {} + +void TMC2130Stepper::begin() { +#ifdef TMC2130DEBUG + Serial.println("TMC2130 Stepper driver library"); + Serial.print("Enable pin: "); + Serial.println(_pinEN); + Serial.print("Direction pin: "); + Serial.println(_pinDIR); + Serial.print("Step pin: "); + Serial.println(_pinSTEP); + Serial.print("Chip select pin: "); + Serial.println(_pinCS); +#endif + delay(200); + + //set pins + if (_pinEN != 0xFFFF) { + pinMode(_pinEN, OUTPUT); + digitalWrite(_pinEN, HIGH); //deactivate driver (LOW active) + } + if (_pinDIR != 0xFFFF) { + pinMode(_pinDIR, OUTPUT); + } + if (_pinSTEP != 0xFFFF) { + pinMode(_pinSTEP, OUTPUT); + digitalWrite(_pinSTEP, LOW); + } + + pinMode(_pinCS, OUTPUT); + digitalWrite(_pinCS, HIGH); + + // Reset registers + push(); + + toff(8); //off_time(8); + tbl(1); //blank_time(24); +} + +void TMC2130Stepper::send2130(uint8_t addressByte, uint32_t *config) { + + SPI.begin(); + SPI.beginTransaction(SPISettings(16000000/8, MSBFIRST, SPI_MODE3)); + digitalWrite(_pinCS, LOW); + + status_response = SPI.transfer(addressByte & 0xFF); // s = + + if (addressByte >> 7) { // Check if WRITE command + //*config &= ~mask; // Clear bits being set + //*config |= (value & mask); // Set new values + SPI.transfer((*config >> 24) & 0xFF); + SPI.transfer((*config >> 16) & 0xFF); + SPI.transfer((*config >> 8) & 0xFF); + SPI.transfer(*config & 0xFF); + } else { // READ command + SPI.transfer16(0x0000); // Clear SPI + SPI.transfer16(0x0000); + digitalWrite(_pinCS, HIGH); + digitalWrite(_pinCS, LOW); + + SPI.transfer(addressByte & 0xFF); // Send the address byte again + *config = SPI.transfer(0x00); + *config <<= 8; + *config|= SPI.transfer(0x00); + *config <<= 8; + *config|= SPI.transfer(0x00); + *config <<= 8; + *config|= SPI.transfer(0x00); + } + + digitalWrite(_pinCS, HIGH); + SPI.endTransaction(); + +} + +bool TMC2130Stepper::checkOT() { + uint32_t response = DRV_STATUS(); + if (response & OTPW_bm) { + flag_otpw = 1; + return true; // bit 26 for overtemperature warning flag + } + return false; +} + +bool TMC2130Stepper::getOTPW() { return flag_otpw; } + +void TMC2130Stepper::clear_otpw() { flag_otpw = 0; } + +bool TMC2130Stepper::isEnabled() { return !digitalRead(_pinEN) && toff(); } + +void TMC2130Stepper::push() { + GCONF(GCONF_sr); + IHOLD_IRUN(IHOLD_IRUN_sr); + TPOWERDOWN(TPOWERDOWN_sr); + TPWMTHRS(TPWMTHRS_sr); + TCOOLTHRS(TCOOLTHRS_sr); + THIGH(THIGH_sr); + XDIRECT(XDIRECT_sr); + VDCMIN(VDCMIN_sr); + CHOPCONF(CHOPCONF_sr); + //MSLUT0(MSLUT0_sr); + //MSLUT1(MSLUT1_sr); + //MSLUT2(MSLUT2_sr); + //MSLUT3(MSLUT3_sr); + //MSLUT4(MSLUT4_sr); + //MSLUT5(MSLUT5_sr); + //MSLUT6(MSLUT6_sr); + //MSLUT7(MSLUT7_sr); + //MSLUT0(MSLUT0_sr); + //MSLUT0(MSLUT0_sr); + //MSLUTSTART(MSLUTSTART_sr); + COOLCONF(COOLCONF_sr); + PWMCONF(PWMCONF_sr); + ENCM_CTRL(ENCM_CTRL_sr); +} + +uint8_t TMC2130Stepper::test_connection() { + uint32_t drv_status = DRV_STATUS(); + switch (drv_status) { + case 0xFFFFFFFF: return 1; + case 0: return 2; + default: return 0; + } +} + +/////////////////////////////////////////////////////////////////////////////////////// +// R+C: GSTAT +void TMC2130Stepper::GSTAT(uint8_t input){ + GSTAT_sr = input; + TMC_WRITE_REG(GSTAT); +} +uint8_t TMC2130Stepper::GSTAT() { TMC_READ_REG_R(GSTAT); } +bool TMC2130Stepper::reset() { TMC_GET_BYTE(GSTAT, RESET); } +bool TMC2130Stepper::drv_err() { TMC_GET_BYTE(GSTAT, DRV_ERR); } +bool TMC2130Stepper::uv_cp() { TMC_GET_BYTE(GSTAT, UV_CP); } +/////////////////////////////////////////////////////////////////////////////////////// +// R: IOIN +uint32_t TMC2130Stepper::IOIN() { TMC_READ_REG_R(IOIN); } +bool TMC2130Stepper::step() { TMC_GET_BYTE_R(IOIN, STEP); } +bool TMC2130Stepper::dir() { TMC_GET_BYTE_R(IOIN, DIR); } +bool TMC2130Stepper::dcen_cfg4() { TMC_GET_BYTE_R(IOIN, DCEN_CFG4); } +bool TMC2130Stepper::dcin_cfg5() { TMC_GET_BYTE_R(IOIN, DCIN_CFG5); } +bool TMC2130Stepper::drv_enn_cfg6() { TMC_GET_BYTE_R(IOIN, DRV_ENN_CFG6); } +bool TMC2130Stepper::dco() { TMC_GET_BYTE_R(IOIN, DCO); } +uint8_t TMC2130Stepper::version() { TMC_GET_BYTE_R(IOIN, VERSION); } +/////////////////////////////////////////////////////////////////////////////////////// +// W: TPOWERDOWN +uint8_t TMC2130Stepper::TPOWERDOWN() { return TPOWERDOWN_sr; } +void TMC2130Stepper::TPOWERDOWN(uint8_t input) { + TPOWERDOWN_sr = input; + TMC_WRITE_REG(TPOWERDOWN); +} +/////////////////////////////////////////////////////////////////////////////////////// +// R: TSTEP +uint32_t TMC2130Stepper::TSTEP() { TMC_READ_REG_R(TSTEP); } +/////////////////////////////////////////////////////////////////////////////////////// +// W: TPWMTHRS +uint32_t TMC2130Stepper::TPWMTHRS() { return TPWMTHRS_sr; } +void TMC2130Stepper::TPWMTHRS(uint32_t input) { + TPWMTHRS_sr = input; + TMC_WRITE_REG(TPWMTHRS); +} +/////////////////////////////////////////////////////////////////////////////////////// +// W: TCOOLTHRS +uint32_t TMC2130Stepper::TCOOLTHRS() { return TCOOLTHRS_sr; } +void TMC2130Stepper::TCOOLTHRS(uint32_t input) { + TCOOLTHRS_sr = input; + TMC_WRITE_REG(TCOOLTHRS); +} +/////////////////////////////////////////////////////////////////////////////////////// +// W: THIGH +uint32_t TMC2130Stepper::THIGH() { return THIGH_sr; } +void TMC2130Stepper::THIGH(uint32_t input) { + THIGH_sr = input; + TMC_WRITE_REG(THIGH); +} +/////////////////////////////////////////////////////////////////////////////////////// +// RW: XDIRECT +uint32_t TMC2130Stepper::XDIRECT() { TMC_READ_REG(XDIRECT); } +void TMC2130Stepper::XDIRECT(uint32_t input) { + XDIRECT_sr = input; + TMC_WRITE_REG(XDIRECT); +} +void TMC2130Stepper::coil_A(int16_t B) { TMC_MOD_REG(XDIRECT, COIL_A); } +void TMC2130Stepper::coil_B(int16_t B) { TMC_MOD_REG(XDIRECT, COIL_B); } +int16_t TMC2130Stepper::coil_A() { TMC_GET_BYTE_R(XDIRECT, COIL_A); } +int16_t TMC2130Stepper::coil_B() { TMC_GET_BYTE_R(XDIRECT, COIL_A); } +/////////////////////////////////////////////////////////////////////////////////////// +// W: VDCMIN +uint32_t TMC2130Stepper::VDCMIN() { return VDCMIN_sr; } +void TMC2130Stepper::VDCMIN(uint32_t input) { + VDCMIN_sr = input; + TMC_WRITE_REG(VDCMIN); +} +/////////////////////////////////////////////////////////////////////////////////////// +// W: MSLUT +uint32_t TMC2130Stepper::MSLUT0() { return MSLUT0_sr; } +void TMC2130Stepper::MSLUT0(uint32_t input) { + MSLUT0_sr = input; + TMC_WRITE_REG(MSLUT0); +} +uint32_t TMC2130Stepper::MSLUT1() { return MSLUT1_sr; } +void TMC2130Stepper::MSLUT1(uint32_t input) { + MSLUT1_sr = input; + TMC_WRITE_REG(MSLUT1); +} +uint32_t TMC2130Stepper::MSLUT2() { return MSLUT2_sr; } +void TMC2130Stepper::MSLUT2(uint32_t input) { + MSLUT2_sr = input; + TMC_WRITE_REG(MSLUT2); +} +uint32_t TMC2130Stepper::MSLUT3() { return MSLUT3_sr; } +void TMC2130Stepper::MSLUT3(uint32_t input) { + MSLUT3_sr = input; + TMC_WRITE_REG(MSLUT3); +} +uint32_t TMC2130Stepper::MSLUT4() { return MSLUT4_sr; } +void TMC2130Stepper::MSLUT4(uint32_t input) { + MSLUT4_sr = input; + TMC_WRITE_REG(MSLUT4); +} +uint32_t TMC2130Stepper::MSLUT5() { return MSLUT5_sr; } +void TMC2130Stepper::MSLUT5(uint32_t input) { + MSLUT0_sr = input; + TMC_WRITE_REG(MSLUT5); +} +uint32_t TMC2130Stepper::MSLUT6() { return MSLUT6_sr; } +void TMC2130Stepper::MSLUT6(uint32_t input) { + MSLUT0_sr = input; + TMC_WRITE_REG(MSLUT6); +} +uint32_t TMC2130Stepper::MSLUT7() { return MSLUT7_sr; } +void TMC2130Stepper::MSLUT7(uint32_t input) { + MSLUT0_sr = input; + TMC_WRITE_REG(MSLUT7); +} +/////////////////////////////////////////////////////////////////////////////////////// +// W: MSLUTSEL +uint32_t TMC2130Stepper::MSLUTSEL() { return MSLUTSEL_sr; } +void TMC2130Stepper::MSLUTSEL(uint32_t input) { + MSLUTSEL_sr = input; + TMC_WRITE_REG(MSLUTSEL); +} +/////////////////////////////////////////////////////////////////////////////////////// +// W: MSLUTSTART +uint32_t TMC2130Stepper::MSLUTSTART() { return MSLUTSTART_sr; } +void TMC2130Stepper::MSLUTSTART(uint32_t input) { + MSLUTSTART_sr = input; + TMC_WRITE_REG(MSLUTSTART); +} +/////////////////////////////////////////////////////////////////////////////////////// +// R: MSCNT +uint16_t TMC2130Stepper::MSCNT() { TMC_READ_REG_R(MSCNT); } +/////////////////////////////////////////////////////////////////////////////////////// +// R: MSCURACT +uint32_t TMC2130Stepper::MSCURACT() { TMC_READ_REG_R(MSCURACT); } +int16_t TMC2130Stepper::cur_a() { + int16_t value = (MSCURACT()&CUR_A_bm) >> CUR_A_bp; + if (value > 255) value -= 512; + return value; +} +int16_t TMC2130Stepper::cur_b() { + int16_t value = (MSCURACT()&CUR_B_bm) >> CUR_B_bp; + if (value > 255) value -= 512; + return value; +} +/////////////////////////////////////////////////////////////////////////////////////// +// R: PWM_SCALE +uint8_t TMC2130Stepper::PWM_SCALE() { TMC_READ_REG_R(PWM_SCALE); } +/////////////////////////////////////////////////////////////////////////////////////// +// W: ENCM_CTRL +uint8_t TMC2130Stepper::ENCM_CTRL() { return ENCM_CTRL_sr; } +void TMC2130Stepper::ENCM_CTRL(uint8_t input) { + ENCM_CTRL_sr = input; + TMC_WRITE_REG(ENCM_CTRL); +} +void TMC2130Stepper::inv(bool B) { TMC_MOD_REG(ENCM_CTRL, INV); } +void TMC2130Stepper::maxspeed(bool B) { TMC_MOD_REG(ENCM_CTRL, MAXSPEED); } +bool TMC2130Stepper::inv() { TMC_GET_BYTE(ENCM_CTRL, INV); } +bool TMC2130Stepper::maxspeed() { TMC_GET_BYTE(ENCM_CTRL, MAXSPEED);} +/////////////////////////////////////////////////////////////////////////////////////// +// R: LOST_STEPS +uint32_t TMC2130Stepper::LOST_STEPS() { TMC_READ_REG_R(LOST_STEPS); } + + +/** + * Helper functions + */ + + +/* + Requested current = mA = I_rms/1000 + Equation for current: + I_rms = (CS+1)/32 * V_fs/(R_sense+0.02ohm) * 1/sqrt(2) + Solve for CS -> + CS = 32*sqrt(2)*I_rms*(R_sense+0.02)/V_fs - 1 + + Example: + vsense = 0b0 -> V_fs = 0.325V + mA = 1640mA = I_rms/1000 = 1.64A + R_sense = 0.10 Ohm + -> + CS = 32*sqrt(2)*1.64*(0.10+0.02)/0.325 - 1 = 26.4 + CS = 26 +*/ +void TMC2130Stepper::rms_current(uint16_t mA, float multiplier, float RS) { + Rsense = RS; + uint8_t CS = 32.0*1.41421*mA/1000.0*(Rsense+0.02)/0.325 - 1; + // If Current Scale is too low, turn on high sensitivity R_sense and calculate again + if (CS < 16) { + vsense(true); + CS = 32.0*1.41421*mA/1000.0*(Rsense+0.02)/0.180 - 1; + } else if(vsense()) { // If CS >= 16, turn off high_sense_r if it's currently ON + vsense(false); + } + irun(CS); + ihold(CS*multiplier); + val_mA = mA; +} + +uint16_t TMC2130Stepper::rms_current() { + return (float)(irun()+1)/32.0 * (vsense()?0.180:0.325)/(Rsense+0.02) / 1.41421 * 1000; +} + +void TMC2130Stepper::setCurrent(uint16_t mA, float R, float multiplier) { rms_current(mA, multiplier, R); } +uint16_t TMC2130Stepper::getCurrent() { return val_mA; } + +void TMC2130Stepper::SilentStepStick2130(uint16_t current) { rms_current(current); } + +void TMC2130Stepper::microsteps(uint16_t ms) { + switch(ms) { + case 256: mres(0); break; + case 128: mres(1); break; + case 64: mres(2); break; + case 32: mres(3); break; + case 16: mres(4); break; + case 8: mres(5); break; + case 4: mres(6); break; + case 2: mres(7); break; + case 0: mres(8); break; + default: break; + } +} + +uint16_t TMC2130Stepper::microsteps() { + switch(mres()) { + case 0: return 256; + case 1: return 128; + case 2: return 64; + case 3: return 32; + case 4: return 16; + case 5: return 8; + case 6: return 4; + case 7: return 2; + case 8: return 0; + } + return 0; +} + +void TMC2130Stepper::sg_current_decrease(uint8_t value) { + switch(value) { + case 32: sedn(0b00); break; + case 8: sedn(0b01); break; + case 2: sedn(0b10); break; + case 1: sedn(0b11); break; + } +} +uint8_t TMC2130Stepper::sg_current_decrease() { + switch(sedn()) { + case 0b00: return 32; + case 0b01: return 8; + case 0b10: return 2; + case 0b11: return 1; + } + return 0; +} diff --git a/src/TMC2130/TMC2130Stepper.h b/src/TMC2130/TMC2130Stepper.h new file mode 100644 index 0000000..eefea8b --- /dev/null +++ b/src/TMC2130/TMC2130Stepper.h @@ -0,0 +1,446 @@ +#pragma once + +//#define TMC2130DEBUG +#include +#include +#include + +#define TMC2130STEPPER_VERSION 0x020500 // v2.5.0 + +class TMC2130Stepper { + public: + TMC2130Stepper(uint16_t pinCS); + TMC2130Stepper(uint16_t pinEN, uint16_t pinDIR, uint16_t pinStep, uint16_t pinCS); + TMC2130Stepper(uint16_t pinEN, uint16_t pinDIR, uint16_t pinStep, uint16_t pinCS, uint16_t pinMOSI, uint16_t pinMISO, uint16_t pinSCK); + void begin(); + void checkStatus(); + void rms_current(uint16_t mA, float multiplier=0.5, float RS=0.11); + uint16_t rms_current(); + void SilentStepStick2130(uint16_t mA); + void setCurrent(uint16_t mA, float Rsense, float multiplier); + uint16_t getCurrent(); + bool checkOT(); + bool getOTPW(); + void clear_otpw(); + bool isEnabled(); + void push(); + uint8_t test_connection(); + // GCONF + uint32_t GCONF(); + void GCONF( uint32_t value); + void I_scale_analog( bool B); + void internal_Rsense( bool B); + void en_pwm_mode( bool B); + void enc_commutation( bool B); + void shaft( bool B); + void diag0_error( bool B); + void diag0_otpw( bool B); + void diag0_stall( bool B); + void diag1_stall( bool B); + void diag1_index( bool B); + void diag1_onstate( bool B); + void diag1_steps_skipped( bool B); + void diag0_int_pushpull( bool B); + void diag1_pushpull( bool B); + void small_hysteresis( bool B); + void stop_enable( bool B); + void direct_mode( bool B); + bool I_scale_analog(); + bool internal_Rsense(); + bool en_pwm_mode(); + bool enc_commutation(); + bool shaft(); + bool diag0_error(); + bool diag0_otpw(); + bool diag0_stall(); + bool diag1_stall(); + bool diag1_index(); + bool diag1_onstate(); + bool diag1_steps_skipped(); + bool diag0_int_pushpull(); + bool diag1_pushpull(); + bool small_hysteresis(); + bool stop_enable(); + bool direct_mode(); + // IHOLD_IRUN + void IHOLD_IRUN( uint32_t input); + uint32_t IHOLD_IRUN(); + void ihold( uint8_t B); + void irun( uint8_t B); + void iholddelay( uint8_t B); + uint8_t ihold(); + uint8_t irun(); + uint8_t iholddelay(); + // GSTAT + void GSTAT( uint8_t input); + uint8_t GSTAT(); + bool reset(); + bool drv_err(); + bool uv_cp(); + // IOIN + uint32_t IOIN(); + bool step(); + bool dir(); + bool dcen_cfg4(); + bool dcin_cfg5(); + bool drv_enn_cfg6(); + bool dco(); + uint8_t version(); + // TPOWERDOWN + uint8_t TPOWERDOWN(); + void TPOWERDOWN( uint8_t input); + // TSTEP + uint32_t TSTEP(); + // TPWMTHRS + uint32_t TPWMTHRS(); + void TPWMTHRS( uint32_t input); + // TCOOLTHRS + uint32_t TCOOLTHRS(); + void TCOOLTHRS( uint32_t input); + // THIGH + uint32_t THIGH(); + void THIGH( uint32_t input); + // XDRIRECT + uint32_t XDIRECT(); + void XDIRECT( uint32_t input); + void coil_A( int16_t B); + void coil_B( int16_t B); + int16_t coil_A(); + int16_t coil_B(); + // VDCMIN + uint32_t VDCMIN(); + void VDCMIN( uint32_t input); + // MSLUT0..MSLUT7 + uint32_t MSLUT0(); + void MSLUT0( uint32_t input); + uint32_t MSLUT1(); + void MSLUT1( uint32_t input); + uint32_t MSLUT2(); + void MSLUT2( uint32_t input); + uint32_t MSLUT3(); + void MSLUT3( uint32_t input); + uint32_t MSLUT4(); + void MSLUT4( uint32_t input); + uint32_t MSLUT5(); + void MSLUT5( uint32_t input); + uint32_t MSLUT6(); + void MSLUT6( uint32_t input); + uint32_t MSLUT7(); + void MSLUT7( uint32_t input); + // MSLUTSEL + uint32_t MSLUTSEL(); + void MSLUTSEL( uint32_t input); + // MSLUTSTART + uint32_t MSLUTSTART(); + void MSLUTSTART( uint32_t input); + // MSCNT + uint16_t MSCNT(); + // MSCURACT + uint32_t MSCURACT(); + int16_t cur_a(); + int16_t cur_b(); + // CHOPCONF + uint32_t CHOPCONF(); + void CHOPCONF( uint32_t value); + void toff( uint8_t B); + void hstrt( uint8_t B); + void hend( uint8_t B); + void fd( uint8_t B); + void disfdcc( bool B); + void rndtf( bool B); + void chm( bool B); + void tbl( uint8_t B); + void vsense( bool B); + void vhighfs( bool B); + void vhighchm( bool B); + void sync( uint8_t B); + void mres( uint8_t B); + void intpol( bool B); + void dedge( bool B); + void diss2g( bool B); + uint8_t toff(); + uint8_t hstrt(); + uint8_t hend(); + uint8_t fd(); + bool disfdcc(); + bool rndtf(); + bool chm(); + uint8_t tbl(); + bool vsense(); + bool vhighfs(); + bool vhighchm(); + uint8_t sync(); + uint8_t mres(); + bool intpol(); + bool dedge(); + bool diss2g(); + // COOLCONF + void COOLCONF(uint32_t value); + uint32_t COOLCONF(); + void semin( uint8_t B); + void seup( uint8_t B); + void semax( uint8_t B); + void sedn( uint8_t B); + void seimin( bool B); + void sgt( int8_t B); + void sfilt( bool B); + uint8_t semin(); + uint8_t seup(); + uint8_t semax(); + uint8_t sedn(); + bool seimin(); + int8_t sgt(); + bool sfilt(); + // PWMCONF + void PWMCONF( uint32_t value); + uint32_t PWMCONF(); + void pwm_ampl( uint8_t B); + void pwm_grad( uint8_t B); + void pwm_freq( uint8_t B); + void pwm_autoscale( bool B); + void pwm_symmetric( bool B); + void freewheel( uint8_t B); + uint8_t pwm_ampl(); + uint8_t pwm_grad(); + uint8_t pwm_freq(); + bool pwm_autoscale(); + bool pwm_symmetric(); + uint8_t freewheel(); + // DRVSTATUS + uint32_t DRV_STATUS(); + uint16_t sg_result(); + bool fsactive(); + uint8_t cs_actual(); + bool stallguard(); + bool ot(); + bool otpw(); + bool s2ga(); + bool s2gb(); + bool ola(); + bool olb(); + bool stst(); + // PWM_SCALE + uint8_t PWM_SCALE(); + // ENCM_CTRL + uint8_t ENCM_CTRL(); + void ENCM_CTRL( uint8_t input); + void inv( bool B); + void maxspeed( bool B); + bool inv(); + bool maxspeed(); + // LOST_STEPS + uint32_t LOST_STEPS(); + + // Helper functions + void microsteps(uint16_t ms); + uint16_t microsteps(); + void blank_time(uint8_t value); + uint8_t blank_time(); + void hysteresis_end(int8_t value); + int8_t hysteresis_end(); + void hysteresis_start(uint8_t value); + uint8_t hysteresis_start(); + void sg_current_decrease(uint8_t value); + uint8_t sg_current_decrease(); + + // Aliases + + // RW: GCONF + inline bool external_ref() __attribute__((always_inline)) { return I_scale_analog(); } + inline bool internal_sense_R() __attribute__((always_inline)) { return internal_Rsense(); } + inline bool stealthChop() __attribute__((always_inline)) { return en_pwm_mode(); } + inline bool commutation() __attribute__((always_inline)) { return enc_commutation(); } + inline bool shaft_dir() __attribute__((always_inline)) { return shaft(); } + inline bool diag0_errors() __attribute__((always_inline)) { return diag0_error(); } + inline bool diag0_temp_prewarn() __attribute__((always_inline)) { return diag0_otpw(); } + inline bool diag1_chopper_on() __attribute__((always_inline)) { return diag1_onstate(); } + inline bool diag0_active_high() __attribute__((always_inline)) { return diag0_int_pushpull(); } + inline bool diag1_active_high() __attribute__((always_inline)) { return diag1_pushpull(); } + inline void external_ref( bool value) __attribute__((always_inline)) { I_scale_analog(value); } + inline void internal_sense_R( bool value) __attribute__((always_inline)) { internal_Rsense(value); } + inline void stealthChop( bool value) __attribute__((always_inline)) { en_pwm_mode(value); } + inline void commutation( bool value) __attribute__((always_inline)) { enc_commutation(value); } + inline void shaft_dir( bool value) __attribute__((always_inline)) { shaft(value); } + inline void diag0_errors( bool value) __attribute__((always_inline)) { diag0_error(value); } + inline void diag0_temp_prewarn( bool value) __attribute__((always_inline)) { diag0_otpw(value); } + inline void diag1_chopper_on( bool value) __attribute__((always_inline)) { diag1_onstate(value); } + inline void diag0_active_high( bool value) __attribute__((always_inline)) { diag0_int_pushpull(value); } + inline void diag1_active_high( bool value) __attribute__((always_inline)) { diag1_pushpull(value); } + // RC + inline uint8_t status_flags() __attribute__((always_inline)) { return GSTAT(); } + // R + inline uint32_t input() __attribute__((always_inline)) { return IOIN(); } + // W: IHOLD_IRUN + inline uint8_t hold_current() __attribute__((always_inline)) { return ihold(); } + inline uint8_t run_current() __attribute__((always_inline)) { return irun(); } + inline uint8_t hold_delay() __attribute__((always_inline)) { return iholddelay(); } + inline void hold_current( uint8_t value) __attribute__((always_inline)) { ihold(value); } + inline void run_current( uint8_t value) __attribute__((always_inline)) { irun(value); } + inline void hold_delay( uint8_t value) __attribute__((always_inline)) { iholddelay(value); } + // W + inline uint8_t power_down_delay() __attribute__((always_inline)) { return TPOWERDOWN(); } + inline void power_down_delay( uint8_t value) __attribute__((always_inline)) { TPOWERDOWN(value); } + // R + inline uint32_t microstep_time() __attribute__((always_inline)) { return TSTEP(); } + // W + inline uint32_t stealth_max_speed() __attribute__((always_inline)) { return TPWMTHRS(); } + inline void stealth_max_speed(uint32_t value) __attribute__((always_inline)) { TPWMTHRS(value); } + // W + inline uint32_t coolstep_min_speed() __attribute__((always_inline)) { return TCOOLTHRS(); } + inline void coolstep_min_speed(uint32_t value) __attribute__((always_inline)) { TCOOLTHRS(value); } + // W + inline uint32_t mode_sw_speed() __attribute__((always_inline)) { return THIGH(); } + inline void mode_sw_speed( uint32_t value) __attribute__((always_inline)) { THIGH(value); } + // RW: XDIRECT + inline int16_t coil_A_current() __attribute__((always_inline)) { return coil_A(); } + inline void coil_A_current( int16_t value) __attribute__((always_inline)) { coil_B(value); } + inline int16_t coil_B_current() __attribute__((always_inline)) { return coil_A(); } + inline void coil_B_current( int16_t value) __attribute__((always_inline)) { coil_B(value); } + // W + inline uint32_t DCstep_min_speed() __attribute__((always_inline)) { return VDCMIN(); } + inline void DCstep_min_speed( uint32_t value) __attribute__((always_inline)) { VDCMIN(value); } + // W + inline uint32_t lut_mslutstart() __attribute__((always_inline)) { return MSLUTSTART(); } + inline void lut_mslutstart( uint32_t value) __attribute__((always_inline)) { MSLUTSTART(value); } + inline uint32_t lut_msutsel() __attribute__((always_inline)) { return MSLUTSEL(); } + inline void lut_msutsel( uint32_t value) __attribute__((always_inline)) { MSLUTSEL(value); } + inline uint32_t ms_lookup_0() __attribute__((always_inline)) { return MSLUT0(); } + inline void ms_lookup_0( uint32_t value) __attribute__((always_inline)) { MSLUT0(value); } + inline uint32_t ms_lookup_1() __attribute__((always_inline)) { return MSLUT1(); } + inline void ms_lookup_1( uint32_t value) __attribute__((always_inline)) { MSLUT1(value); } + inline uint32_t ms_lookup_2() __attribute__((always_inline)) { return MSLUT2(); } + inline void ms_lookup_2( uint32_t value) __attribute__((always_inline)) { MSLUT2(value); } + inline uint32_t ms_lookup_3() __attribute__((always_inline)) { return MSLUT3(); } + inline void ms_lookup_3( uint32_t value) __attribute__((always_inline)) { MSLUT3(value); } + inline uint32_t ms_lookup_4() __attribute__((always_inline)) { return MSLUT4(); } + inline void ms_lookup_4( uint32_t value) __attribute__((always_inline)) { MSLUT4(value); } + inline uint32_t ms_lookup_5() __attribute__((always_inline)) { return MSLUT5(); } + inline void ms_lookup_5( uint32_t value) __attribute__((always_inline)) { MSLUT5(value); } + inline uint32_t ms_lookup_6() __attribute__((always_inline)) { return MSLUT6(); } + inline void ms_lookup_6( uint32_t value) __attribute__((always_inline)) { MSLUT6(value); } + inline uint32_t ms_lookup_7() __attribute__((always_inline)) { return MSLUT7(); } + inline void ms_lookup_7( uint32_t value) __attribute__((always_inline)) { MSLUT7(value); } + // RW: CHOPCONF + inline uint8_t off_time() __attribute__((always_inline)) { return toff(); } +// inline uint8_t hysteresis_start() __attribute__((always_inline)) { return hstrt(); } +// inline int8_t hysteresis_low() __attribute__((always_inline)) { return hend(); } + inline int8_t hysteresis_low() __attribute__((always_inline)) { return hysteresis_end(); } + inline uint8_t fast_decay_time() __attribute__((always_inline)) { return fd(); } + inline bool disable_I_comparator() __attribute__((always_inline)) { return disfdcc(); } + inline bool random_off_time() __attribute__((always_inline)) { return rndtf(); } + inline bool chopper_mode() __attribute__((always_inline)) { return chm(); } +// inline uint8_t blank_time() __attribute__((always_inline)) { return tbl(); } + inline bool high_sense_R() __attribute__((always_inline)) { return vsense(); } + inline bool fullstep_threshold() __attribute__((always_inline)) { return vhighfs(); } + inline bool high_speed_mode() __attribute__((always_inline)) { return vhighchm(); } + inline uint8_t sync_phases() __attribute__((always_inline)) { return sync(); } +// inline uint16_t microsteps() __attribute__((always_inline)) { return mres(); } + inline bool interpolate() __attribute__((always_inline)) { return intpol(); } + inline bool double_edge_step() __attribute__((always_inline)) { return dedge(); } + inline bool disable_short_protection() __attribute__((always_inline)) { return diss2g(); } + inline void off_time( uint8_t value) __attribute__((always_inline)) { toff(value); } +// inline void hysteresis_start( uint8_t value) __attribute__((always_inline)) { hstrt(value); } +// inline void hysteresis_low( int8_t value) __attribute__((always_inline)) { hend(value); } + inline void hysteresis_low( int8_t value) __attribute__((always_inline)) { hysteresis_end(value); } + inline void fast_decay_time( uint8_t value) __attribute__((always_inline)) { fd(value); } + inline void disable_I_comparator( bool value) __attribute__((always_inline)) { disfdcc(value); } + inline void random_off_time( bool value) __attribute__((always_inline)) { rndtf(value); } + inline void chopper_mode( bool value) __attribute__((always_inline)) { chm(value); } +// inline void blank_time( uint8_t value) __attribute__((always_inline)) { tbl(value); } + inline void high_sense_R( bool value) __attribute__((always_inline)) { vsense(value); } + inline void fullstep_threshold( bool value) __attribute__((always_inline)) { vhighfs(value); } + inline void high_speed_mode( bool value) __attribute__((always_inline)) { vhighchm(value); } + inline void sync_phases( uint8_t value) __attribute__((always_inline)) { sync(value); } +// inline void microsteps( uint16_t value) __attribute__((always_inline)) { mres(value); } + inline void interpolate( bool value) __attribute__((always_inline)) { intpol(value); } + inline void double_edge_step( bool value) __attribute__((always_inline)) { dedge(value); } + inline void disable_short_protection(bool value)__attribute__((always_inline)) { diss2g(value); } + // W: COOLCONF + inline uint8_t sg_min() __attribute__((always_inline)) { return semin(); } + inline uint8_t sg_step_width() __attribute__((always_inline)) { return seup(); } + inline uint8_t sg_max() __attribute__((always_inline)) { return semax(); } +// inline uint8_t sg_current_decrease() __attribute__((always_inline)) { return sedn(); } + inline uint8_t smart_min_current() __attribute__((always_inline)) { return seimin(); } + inline int8_t sg_stall_value() __attribute__((always_inline)) { return sgt(); } + inline bool sg_filter() __attribute__((always_inline)) { return sfilt(); } + inline void sg_min( uint8_t value) __attribute__((always_inline)) { semin(value); } + inline void sg_step_width( uint8_t value) __attribute__((always_inline)) { seup(value); } + inline void sg_max( uint8_t value) __attribute__((always_inline)) { semax(value); } +// inline void sg_current_decrease(uint8_t value)__attribute__((always_inline)) { sedn(value); } + inline void smart_min_current( uint8_t value) __attribute__((always_inline)) { seimin(value); } + inline void sg_stall_value( int8_t value) __attribute__((always_inline)) { sgt(value); } + inline void sg_filter( bool value) __attribute__((always_inline)) { sfilt(value); } + // W: PWMCONF + inline uint8_t stealth_amplitude() __attribute__((always_inline)) { return pwm_ampl(); } + inline uint8_t stealth_gradient() __attribute__((always_inline)) { return pwm_grad(); } + inline uint8_t stealth_freq() __attribute__((always_inline)) { return pwm_freq(); } + inline bool stealth_autoscale() __attribute__((always_inline)) { return pwm_autoscale(); } + inline bool stealth_symmetric() __attribute__((always_inline)) { return pwm_symmetric(); } + inline uint8_t standstill_mode() __attribute__((always_inline)) { return freewheel(); } + inline void stealth_amplitude( uint8_t value) __attribute__((always_inline)) { pwm_ampl(value); } + inline void stealth_gradient( uint8_t value) __attribute__((always_inline)) { pwm_grad(value); } + inline void stealth_freq( uint8_t value) __attribute__((always_inline)) { pwm_freq(value); } + inline void stealth_autoscale( bool value) __attribute__((always_inline)) { pwm_autoscale(value); } + inline void stealth_symmetric( bool value) __attribute__((always_inline)) { pwm_symmetric(value); } + inline void standstill_mode( uint8_t value) __attribute__((always_inline)) { freewheel(value); } + // W: ENCM_CTRL + inline bool invert_encoder() __attribute__((always_inline)) { return inv(); } + inline void invert_encoder( bool value) __attribute__((always_inline)) { inv(value); } + // R: DRV_STATUS + inline uint32_t DRVSTATUS() __attribute__((always_inline)) { return DRV_STATUS(); } + + // Backwards compatibility + inline void hysterisis_end(int8_t value) __attribute__((always_inline)) { hysteresis_end(value); } + inline int8_t hysterisis_end() __attribute__((always_inline)) { return hysteresis_end(); } + inline void hysterisis_start(uint8_t value) __attribute__((always_inline)) { hysteresis_start(value); } + inline uint8_t hysterisis_start() __attribute__((always_inline)) { return hysteresis_start(); } + + + float Rsense = 0.11; + uint8_t status_response; + bool flag_otpw = false; + + private: + //const uint8_t WRITE = 0b10000000; + //const uint8_t READ = 0b00000000; + const uint16_t _pinEN = 0xFFFF; + const uint16_t _pinSTEP = 0xFFFF; + const uint16_t _pinCS = 0xFFFF; + //const int MOSI_PIN = 12; + //const int MISO_PIN = 11; + //const int SCK_PIN = 13; + const uint16_t _pinDIR = 0xFFFF; + + // Shadow registers + uint32_t GCONF_sr = 0x00000000UL, + IHOLD_IRUN_sr = 0x00000000UL, + TSTEP_sr = 0x00000000UL, + TPWMTHRS_sr = 0x00000000UL, + TCOOLTHRS_sr = 0x00000000UL, + THIGH_sr = 0x00000000UL, + XDIRECT_sr = 0x00000000UL, + VDCMIN_sr = 0x00000000UL, + MSLUT0_sr = 0xAAAAB554UL, + MSLUT1_sr = 0x4A9554AAUL, + MSLUT2_sr = 0x24492929UL, + MSLUT3_sr = 0x10104222UL, + MSLUT4_sr = 0xFBFFFFFFUL, + MSLUT5_sr = 0xB5BB777DUL, + MSLUT6_sr = 0x49295556UL, + MSLUT7_sr = 0x00404222UL, + MSLUTSEL_sr = 0xFFFF8056UL, + CHOPCONF_sr = 0x00000000UL, + COOLCONF_sr = 0x00000000UL, + DCCTRL_sr = 0x00000000UL, + PWMCONF_sr = 0x00050480UL, + tmp_sr = 0x00000000UL, + TPOWERDOWN_sr = 0x00000000UL, + ENCM_CTRL_sr = 0x00000000UL, + GSTAT_sr = 0x00000000UL, + MSLUTSTART_sr = 0x00F70000UL; + + void send2130(uint8_t addressByte, uint32_t *config); + + uint16_t val_mA = 0; + const bool uses_sw_spi; +}; diff --git a/src/TMC2130/TMC2130Stepper_CHOPCONF.cpp b/src/TMC2130/TMC2130Stepper_CHOPCONF.cpp new file mode 100644 index 0000000..a94c421 --- /dev/null +++ b/src/TMC2130/TMC2130Stepper_CHOPCONF.cpp @@ -0,0 +1,68 @@ +#include "TMC2130Stepper.h" +#include "TMC2130Stepper_MACROS.h" + +// CHOPCONF +uint32_t TMC2130Stepper::CHOPCONF() { TMC_READ_REG(CHOPCONF); } +void TMC2130Stepper::CHOPCONF(uint32_t input) { + CHOPCONF_sr = input; + TMC_WRITE_REG(CHOPCONF); +} + +void TMC2130Stepper::toff( uint8_t B ) { TMC_MOD_REG(CHOPCONF, TOFF); } +void TMC2130Stepper::hstrt( uint8_t B ) { TMC_MOD_REG(CHOPCONF, HSTRT); } +void TMC2130Stepper::hend( uint8_t B ) { TMC_MOD_REG(CHOPCONF, HEND); } +void TMC2130Stepper::fd( uint8_t B ) { TMC_MOD_REG(CHOPCONF, FD); } +void TMC2130Stepper::disfdcc( bool B ) { TMC_MOD_REG(CHOPCONF, DISFDCC); } +void TMC2130Stepper::rndtf( bool B ) { TMC_MOD_REG(CHOPCONF, RNDTF); } +void TMC2130Stepper::chm( bool B ) { TMC_MOD_REG(CHOPCONF, CHM); } +void TMC2130Stepper::tbl( uint8_t B ) { TMC_MOD_REG(CHOPCONF, TBL); } +void TMC2130Stepper::vsense( bool B ) { TMC_MOD_REG(CHOPCONF, VSENSE); } +void TMC2130Stepper::vhighfs( bool B ) { TMC_MOD_REG(CHOPCONF, VHIGHFS); } +void TMC2130Stepper::vhighchm( bool B ) { TMC_MOD_REG(CHOPCONF, VHIGHCHM); } +void TMC2130Stepper::sync( uint8_t B ) { TMC_MOD_REG(CHOPCONF, SYNC); } +void TMC2130Stepper::mres( uint8_t B ) { TMC_MOD_REG(CHOPCONF, MRES); } +void TMC2130Stepper::intpol( bool B ) { TMC_MOD_REG(CHOPCONF, INTPOL); } +void TMC2130Stepper::dedge( bool B ) { TMC_MOD_REG(CHOPCONF, DEDGE); } +void TMC2130Stepper::diss2g( bool B ) { TMC_MOD_REG(CHOPCONF, DISS2G); } + +uint8_t TMC2130Stepper::toff() { TMC_GET_BYTE(CHOPCONF, TOFF); } +uint8_t TMC2130Stepper::hstrt() { TMC_GET_BYTE(CHOPCONF, HSTRT); } +uint8_t TMC2130Stepper::hend() { TMC_GET_BYTE(CHOPCONF, HEND); } +uint8_t TMC2130Stepper::fd() { TMC_GET_BYTE(CHOPCONF, FD); } +bool TMC2130Stepper::disfdcc() { TMC_GET_BYTE(CHOPCONF, DISFDCC); } +bool TMC2130Stepper::rndtf() { TMC_GET_BYTE(CHOPCONF, RNDTF); } +bool TMC2130Stepper::chm() { TMC_GET_BYTE(CHOPCONF, CHM); } +uint8_t TMC2130Stepper::tbl() { TMC_GET_BYTE(CHOPCONF, TBL); } +bool TMC2130Stepper::vsense() { TMC_GET_BIT( CHOPCONF, VSENSE); } +bool TMC2130Stepper::vhighfs() { TMC_GET_BYTE(CHOPCONF, VHIGHFS); } +bool TMC2130Stepper::vhighchm() { TMC_GET_BYTE(CHOPCONF, VHIGHCHM); } +uint8_t TMC2130Stepper::sync() { TMC_GET_BYTE(CHOPCONF, SYNC); } +uint8_t TMC2130Stepper::mres() { TMC_GET_BYTE(CHOPCONF, MRES); } +bool TMC2130Stepper::intpol() { TMC_GET_BYTE(CHOPCONF, INTPOL); } +bool TMC2130Stepper::dedge() { TMC_GET_BYTE(CHOPCONF, DEDGE); } +bool TMC2130Stepper::diss2g() { TMC_GET_BYTE(CHOPCONF, DISS2G); } + +void TMC2130Stepper::hysteresis_end(int8_t value) { hend(value+3); } +int8_t TMC2130Stepper::hysteresis_end() { return hend()-3; }; + +void TMC2130Stepper::hysteresis_start(uint8_t value) { hstrt(value-1); } +uint8_t TMC2130Stepper::hysteresis_start() { return hstrt()+1; } + +void TMC2130Stepper::blank_time(uint8_t value) { + switch (value) { + case 16: tbl(0b00); break; + case 24: tbl(0b01); break; + case 36: tbl(0b10); break; + case 54: tbl(0b11); break; + } +} + +uint8_t TMC2130Stepper::blank_time() { + switch (tbl()) { + case 0b00: return 16; + case 0b01: return 24; + case 0b10: return 36; + case 0b11: return 54; + } + return 0; +} diff --git a/src/TMC2130/TMC2130Stepper_COOLCONF.cpp b/src/TMC2130/TMC2130Stepper_COOLCONF.cpp new file mode 100644 index 0000000..602b7c9 --- /dev/null +++ b/src/TMC2130/TMC2130Stepper_COOLCONF.cpp @@ -0,0 +1,31 @@ +#include "TMC2130Stepper.h" +#include "TMC2130Stepper_MACROS.h" + +// COOLCONF +uint32_t TMC2130Stepper::COOLCONF() { return COOLCONF_sr; } +void TMC2130Stepper::COOLCONF(uint32_t input) { + COOLCONF_sr = input; + TMC_WRITE_REG(COOLCONF); +} + +void TMC2130Stepper::semin( uint8_t B ) { TMC_MOD_REG(COOLCONF, SEMIN); } +void TMC2130Stepper::seup( uint8_t B ) { TMC_MOD_REG(COOLCONF, SEUP); } +void TMC2130Stepper::semax( uint8_t B ) { TMC_MOD_REG(COOLCONF, SEMAX); } +void TMC2130Stepper::sedn( uint8_t B ) { TMC_MOD_REG(COOLCONF, SEDN); } +void TMC2130Stepper::seimin( bool B ) { TMC_MOD_REG(COOLCONF, SEIMIN); } +void TMC2130Stepper::sgt( int8_t B ) { TMC_MOD_REG(COOLCONF, SGT); } +void TMC2130Stepper::sfilt( bool B ) { TMC_MOD_REG(COOLCONF, SFILT); } + +uint8_t TMC2130Stepper::semin() { TMC_GET_BYTE(COOLCONF, SEMIN); } +uint8_t TMC2130Stepper::seup() { TMC_GET_BYTE(COOLCONF, SEUP); } +uint8_t TMC2130Stepper::semax() { TMC_GET_BYTE(COOLCONF, SEMAX); } +uint8_t TMC2130Stepper::sedn() { TMC_GET_BYTE(COOLCONF, SEDN); } +bool TMC2130Stepper::seimin() { TMC_GET_BYTE(COOLCONF, SEIMIN); } +//int8_t TMC2130Stepper::sgt() { TMC_GET_BYTE(COOLCONF, SGT); } +bool TMC2130Stepper::sfilt() { TMC_GET_BYTE(COOLCONF, SFILT); } + +int8_t TMC2130Stepper::sgt() { + // Two's complement in a 7bit value + int8_t val = (COOLCONF()&SGT_bm) >> SGT_bp; + return val <= 63 ? val : val - 128; +} diff --git a/src/TMC2130/TMC2130Stepper_DRV_STATUS.cpp b/src/TMC2130/TMC2130Stepper_DRV_STATUS.cpp new file mode 100644 index 0000000..59553a4 --- /dev/null +++ b/src/TMC2130/TMC2130Stepper_DRV_STATUS.cpp @@ -0,0 +1,29 @@ +#include "TMC2130Stepper.h" +#include "TMC2130Stepper_MACROS.h" + +uint32_t TMC2130Stepper::DRV_STATUS() { TMC_READ_REG_R(DRV_STATUS); } + +uint16_t TMC2130Stepper::sg_result(){ TMC_GET_BYTE_R(DRV_STATUS, SG_RESULT); } +bool TMC2130Stepper::fsactive() { TMC_GET_BYTE_R(DRV_STATUS, FSACTIVE); } +uint8_t TMC2130Stepper::cs_actual() { TMC_GET_BYTE_R(DRV_STATUS, CS_ACTUAL); } +bool TMC2130Stepper::stallguard() { TMC_GET_BYTE_R(DRV_STATUS, STALLGUARD); } +bool TMC2130Stepper::ot() { TMC_GET_BYTE_R(DRV_STATUS, OT); } +bool TMC2130Stepper::otpw() { TMC_GET_BYTE_R(DRV_STATUS, OTPW); } +bool TMC2130Stepper::s2ga() { TMC_GET_BYTE_R(DRV_STATUS, S2GA); } +bool TMC2130Stepper::s2gb() { TMC_GET_BYTE_R(DRV_STATUS, S2GB); } +bool TMC2130Stepper::ola() { TMC_GET_BYTE_R(DRV_STATUS, OLA); } +bool TMC2130Stepper::olb() { TMC_GET_BYTE_R(DRV_STATUS, OLB); } +bool TMC2130Stepper::stst() { TMC_GET_BYTE_R(DRV_STATUS, STST); } +/* +uint16_t TMC2130Stepper::sg_result() { + uint32_t drv_status = 0x00000000UL; + Serial.print("drv_status="); + Serial.print(drv_status); + drv_status = DRV_STATUS(); + Serial.print("\tdrv_status="); + Serial.print(drv_status); + Serial.print("\t"); + + return drv_status&SG_RESULT_bm; +} +*/ \ No newline at end of file diff --git a/src/TMC2130/TMC2130Stepper_GCONF.cpp b/src/TMC2130/TMC2130Stepper_GCONF.cpp new file mode 100644 index 0000000..5df33ff --- /dev/null +++ b/src/TMC2130/TMC2130Stepper_GCONF.cpp @@ -0,0 +1,54 @@ +#include "TMC2130Stepper.h" +#include "TMC2130Stepper_MACROS.h" + +// GCONF +uint32_t TMC2130Stepper::GCONF() { TMC_READ_REG(GCONF); } +void TMC2130Stepper::GCONF(uint32_t input) { + GCONF_sr = input; + TMC_WRITE_REG(GCONF); +} + +void TMC2130Stepper::I_scale_analog(bool B) { TMC_MOD_REG(GCONF, I_SCALE_ANALOG); } +void TMC2130Stepper::internal_Rsense(bool B) { TMC_MOD_REG(GCONF, INTERNAL_RSENSE); } +void TMC2130Stepper::en_pwm_mode(bool B) { TMC_MOD_REG(GCONF, EN_PWM_MODE); } +void TMC2130Stepper::enc_commutation(bool B) { TMC_MOD_REG(GCONF, ENC_COMMUTATION); } +void TMC2130Stepper::shaft(bool B) { TMC_MOD_REG(GCONF, SHAFT); } +void TMC2130Stepper::diag0_error(bool B) { TMC_MOD_REG(GCONF, DIAG0_ERROR); } +void TMC2130Stepper::diag0_otpw(bool B) { TMC_MOD_REG(GCONF, DIAG0_OTPW); } +void TMC2130Stepper::diag0_stall(bool B) { TMC_MOD_REG(GCONF, DIAG0_STALL); } +void TMC2130Stepper::diag1_stall(bool B) { TMC_MOD_REG(GCONF, DIAG1_STALL); } +void TMC2130Stepper::diag1_index(bool B) { TMC_MOD_REG(GCONF, DIAG1_INDEX); } +void TMC2130Stepper::diag1_onstate(bool B) { TMC_MOD_REG(GCONF, DIAG1_ONSTATE); } +void TMC2130Stepper::diag1_steps_skipped(bool B) { TMC_MOD_REG(GCONF, DIAG1_STEPS_SKIPPED); } +void TMC2130Stepper::diag0_int_pushpull(bool B) { TMC_MOD_REG(GCONF, DIAG0_INT_PUSHPULL); } +void TMC2130Stepper::diag1_pushpull(bool B) { TMC_MOD_REG(GCONF, DIAG1_PUSHPULL); } +void TMC2130Stepper::small_hysteresis(bool B) { TMC_MOD_REG(GCONF, SMALL_HYSTERESIS); } +void TMC2130Stepper::stop_enable(bool B) { TMC_MOD_REG(GCONF, STOP_ENABLE); } +void TMC2130Stepper::direct_mode(bool B) { TMC_MOD_REG(GCONF, DIRECT_MODE); } + +bool TMC2130Stepper::I_scale_analog() { TMC_GET_BYTE(GCONF, I_SCALE_ANALOG); } +bool TMC2130Stepper::internal_Rsense() { TMC_GET_BYTE(GCONF, INTERNAL_RSENSE); } +bool TMC2130Stepper::en_pwm_mode() { TMC_GET_BYTE(GCONF, EN_PWM_MODE); } +bool TMC2130Stepper::enc_commutation() { TMC_GET_BYTE(GCONF, ENC_COMMUTATION); } +bool TMC2130Stepper::shaft() { TMC_GET_BYTE(GCONF, SHAFT); } +bool TMC2130Stepper::diag0_error() { TMC_GET_BYTE(GCONF, DIAG0_ERROR); } +bool TMC2130Stepper::diag0_otpw() { TMC_GET_BYTE(GCONF, DIAG0_OTPW); } +bool TMC2130Stepper::diag0_stall() { TMC_GET_BYTE(GCONF, DIAG0_STALL); } +bool TMC2130Stepper::diag1_stall() { TMC_GET_BYTE(GCONF, DIAG1_STALL); } +bool TMC2130Stepper::diag1_index() { TMC_GET_BYTE(GCONF, DIAG1_INDEX); } +bool TMC2130Stepper::diag1_onstate() { TMC_GET_BYTE(GCONF, DIAG1_ONSTATE); } +bool TMC2130Stepper::diag1_steps_skipped() { TMC_GET_BYTE(GCONF, DIAG1_STEPS_SKIPPED); } +bool TMC2130Stepper::diag0_int_pushpull() { TMC_GET_BYTE(GCONF, DIAG0_INT_PUSHPULL); } +bool TMC2130Stepper::diag1_pushpull() { TMC_GET_BYTE(GCONF, DIAG1_PUSHPULL); } +bool TMC2130Stepper::small_hysteresis() { TMC_GET_BYTE(GCONF, SMALL_HYSTERESIS); } +bool TMC2130Stepper::stop_enable() { TMC_GET_BYTE(GCONF, STOP_ENABLE); } +bool TMC2130Stepper::direct_mode() { TMC_GET_BYTE(GCONF, DIRECT_MODE); } + +/* +bit 18 not implemented: +test_mode 0: +Normal operation 1: +Enable analog test output on pin DCO. IHOLD[1..0] selects the function of DCO: +0…2: T120, DAC, VDDH Attention: +Not for user, set to 0 for normal operation! +*/ diff --git a/src/TMC2130/TMC2130Stepper_IHOLD_IRUN.cpp b/src/TMC2130/TMC2130Stepper_IHOLD_IRUN.cpp new file mode 100644 index 0000000..f737114 --- /dev/null +++ b/src/TMC2130/TMC2130Stepper_IHOLD_IRUN.cpp @@ -0,0 +1,16 @@ +#include "TMC2130Stepper.h" +#include "TMC2130Stepper_MACROS.h" + +// IHOLD_IRUN +void TMC2130Stepper::IHOLD_IRUN(uint32_t input) { + IHOLD_IRUN_sr = input; + TMC_WRITE_REG(IHOLD_IRUN); +} +uint32_t TMC2130Stepper::IHOLD_IRUN() { return IHOLD_IRUN_sr; } + +void TMC2130Stepper::ihold(uint8_t B) { TMC_MOD_REG(IHOLD_IRUN, IHOLD); } +void TMC2130Stepper::irun(uint8_t B) { TMC_MOD_REG(IHOLD_IRUN, IRUN); } +void TMC2130Stepper::iholddelay(uint8_t B) { TMC_MOD_REG(IHOLD_IRUN, IHOLDDELAY); } +uint8_t TMC2130Stepper::ihold() { TMC_GET_BYTE(IHOLD_IRUN, IHOLD); } +uint8_t TMC2130Stepper::irun() { TMC_GET_BYTE(IHOLD_IRUN, IRUN); } +uint8_t TMC2130Stepper::iholddelay() { TMC_GET_BYTE(IHOLD_IRUN, IHOLDDELAY); } diff --git a/src/TMC2130/TMC2130Stepper_MACROS.h b/src/TMC2130/TMC2130Stepper_MACROS.h new file mode 100644 index 0000000..183a0a0 --- /dev/null +++ b/src/TMC2130/TMC2130Stepper_MACROS.h @@ -0,0 +1,22 @@ +#ifndef TMC2130Stepper_MACROS_H +#define TMC2130Stepper_MACROS_H +#include "TMC2130Stepper.h" +#include "TMC2130Stepper_REGDEFS.h" + +#define TMC_WRITE_REG(R) send2130(TMC2130_WRITE|REG_##R, &R##_sr); + +#define TMC_READ_REG(R) send2130(TMC2130_READ|REG_##R, &R##_sr); return R##_sr + +#define TMC_READ_REG_R(R) tmp_sr=0; send2130(TMC2130_READ|REG_##R, &tmp_sr); return tmp_sr; + +#define TMC_MOD_REG(REG, SETTING) REG##_sr &= ~SETTING##_bm; \ + REG##_sr |= ((uint32_t)B<> SETTING##_bp; + +#define TMC_GET_BYTE_R(REG, SETTING) return (REG()&SETTING##_bm) >> SETTING##_bp; + +#define TMC_GET_BIT(REG, SETTING) return (bool)((REG()&SETTING##_bm) >> SETTING##_bp); + +#endif diff --git a/src/TMC2130/TMC2130Stepper_PWMCONF.cpp b/src/TMC2130/TMC2130Stepper_PWMCONF.cpp new file mode 100644 index 0000000..3e774e9 --- /dev/null +++ b/src/TMC2130/TMC2130Stepper_PWMCONF.cpp @@ -0,0 +1,24 @@ +#include "TMC2130Stepper.h" +#include "TMC2130Stepper_MACROS.h" + + +// PWMCONF +uint32_t TMC2130Stepper::PWMCONF() { return PWMCONF_sr; } +void TMC2130Stepper::PWMCONF(uint32_t input) { + PWMCONF_sr = input; + TMC_WRITE_REG(PWMCONF); +} + +void TMC2130Stepper::pwm_ampl( uint8_t B ) { TMC_MOD_REG(PWMCONF, PWM_AMPL); } +void TMC2130Stepper::pwm_grad( uint8_t B ) { TMC_MOD_REG(PWMCONF, PWM_GRAD); } +void TMC2130Stepper::pwm_freq( uint8_t B ) { TMC_MOD_REG(PWMCONF, PWM_FREQ); } +void TMC2130Stepper::pwm_autoscale( bool B ) { TMC_MOD_REG(PWMCONF, PWM_AUTOSCALE); } +void TMC2130Stepper::pwm_symmetric( bool B ) { TMC_MOD_REG(PWMCONF, PWM_SYMMETRIC); } +void TMC2130Stepper::freewheel( uint8_t B ) { TMC_MOD_REG(PWMCONF, FREEWHEEL); } + +uint8_t TMC2130Stepper::pwm_ampl() { TMC_GET_BYTE(PWMCONF, PWM_AMPL); } +uint8_t TMC2130Stepper::pwm_grad() { TMC_GET_BYTE(PWMCONF, PWM_GRAD); } +uint8_t TMC2130Stepper::pwm_freq() { TMC_GET_BYTE(PWMCONF, PWM_FREQ); } +bool TMC2130Stepper::pwm_autoscale() { TMC_GET_BYTE(PWMCONF, PWM_AUTOSCALE); } +bool TMC2130Stepper::pwm_symmetric() { TMC_GET_BYTE(PWMCONF, PWM_SYMMETRIC); } +uint8_t TMC2130Stepper::freewheel() { TMC_GET_BYTE(PWMCONF, FREEWHEEL); } diff --git a/src/TMC2130/TMC2130Stepper_REGDEFS.h b/src/TMC2130/TMC2130Stepper_REGDEFS.h new file mode 100644 index 0000000..547e7b8 --- /dev/null +++ b/src/TMC2130/TMC2130Stepper_REGDEFS.h @@ -0,0 +1,288 @@ +#ifndef TMC2130Stepper_REGDEFS_h +#define TMC2130Stepper_REGDEFS_h + +constexpr uint8_t TMC2130_READ = 0x00; +constexpr uint8_t TMC2130_WRITE = 0x80; + +// Register memory positions +constexpr uint8_t REG_GCONF = 0x00; +constexpr uint8_t REG_GSTAT = 0x01; +constexpr uint8_t REG_IOIN = 0x04; +constexpr uint8_t REG_IHOLD_IRUN = 0x10; +constexpr uint8_t REG_TPOWERDOWN = 0x11; +constexpr uint8_t REG_TSTEP = 0x12; +constexpr uint8_t REG_TPWMTHRS = 0x13; +constexpr uint8_t REG_TCOOLTHRS = 0x14; +constexpr uint8_t REG_THIGH = 0x15; +constexpr uint8_t REG_XDIRECT = 0x2D; +constexpr uint8_t REG_VDCMIN = 0x33; +constexpr uint8_t REG_MSLUT0 = 0x60; +constexpr uint8_t REG_MSLUT1 = 0x61; +constexpr uint8_t REG_MSLUT2 = 0x62; +constexpr uint8_t REG_MSLUT3 = 0x63; +constexpr uint8_t REG_MSLUT4 = 0x64; +constexpr uint8_t REG_MSLUT5 = 0x65; +constexpr uint8_t REG_MSLUT6 = 0x66; +constexpr uint8_t REG_MSLUT7 = 0x67; +constexpr uint8_t REG_MSLUTSEL = 0x68; +constexpr uint8_t REG_MSLUTSTART = 0x69; +constexpr uint8_t REG_MSCNT = 0x6A; +constexpr uint8_t REG_MSCURACT = 0x6B; +constexpr uint8_t REG_CHOPCONF = 0x6C; +constexpr uint8_t REG_COOLCONF = 0x6D; +constexpr uint8_t REG_DCCTRL = 0x6E; +constexpr uint8_t REG_DRV_STATUS = 0x6F; +constexpr uint8_t REG_PWMCONF = 0x70; +constexpr uint8_t REG_PWM_SCALE = 0x71; +constexpr uint8_t REG_ENCM_CTRL = 0x72; +constexpr uint8_t REG_LOST_STEPS = 0x73; + +// SPI_STATUS +constexpr uint8_t RESET_FLAG_bp = 0; +constexpr uint8_t DRIVER_ERROR_bp = 1; +constexpr uint8_t SG2_bp = 2; +constexpr uint8_t STANDSTILL_bp = 3; +constexpr uint32_t RESET_FLAG_bm = 0x1UL; +constexpr uint32_t DRIVER_ERROR_bm = 0x2UL; +constexpr uint32_t SG2_bm = 0x4UL; +constexpr uint32_t STANDSTILL_bm = 0x8UL; + +// GCONF +constexpr uint8_t I_SCALE_ANALOG_bp = 0; +constexpr uint8_t INTERNAL_RSENSE_bp = 1; +constexpr uint8_t EN_PWM_MODE_bp = 2; +constexpr uint8_t ENC_COMMUTATION_bp = 3; +constexpr uint8_t SHAFT_bp = 4; +constexpr uint8_t DIAG0_ERROR_bp = 5; +constexpr uint8_t DIAG0_OTPW_bp = 6; +constexpr uint8_t DIAG0_STALL_bp = 7; +constexpr uint8_t DIAG1_STALL_bp = 8; +constexpr uint8_t DIAG1_INDEX_bp = 9; +constexpr uint8_t DIAG1_ONSTATE_bp = 10; +constexpr uint8_t DIAG1_STEPS_SKIPPED_bp = 11; +constexpr uint8_t DIAG0_INT_PUSHPULL_bp = 12; +constexpr uint8_t DIAG1_PUSHPULL_bp = 13; +constexpr uint8_t SMALL_HYSTERESIS_bp = 14; +constexpr uint8_t STOP_ENABLE_bp = 15; +constexpr uint8_t DIRECT_MODE_bp = 16; +constexpr uint32_t GCONF_bm = 0x3FFFFUL; +constexpr uint32_t I_SCALE_ANALOG_bm = 0x1UL; +constexpr uint32_t INTERNAL_RSENSE_bm = 0x2UL; +constexpr uint32_t EN_PWM_MODE_bm = 0x4UL; +constexpr uint32_t ENC_COMMUTATION_bm = 0x8UL; +constexpr uint32_t SHAFT_bm = 0x10UL; +constexpr uint32_t DIAG0_ERROR_bm = 0x20UL; +constexpr uint32_t DIAG0_OTPW_bm = 0x40UL; +constexpr uint32_t DIAG0_STALL_bm = 0x80UL; +constexpr uint32_t DIAG1_STALL_bm = 0x100UL; +constexpr uint32_t DIAG1_INDEX_bm = 0x200UL; +constexpr uint32_t DIAG1_ONSTATE_bm = 0x400UL; +constexpr uint32_t DIAG1_STEPS_SKIPPED_bm = 0x800UL; +constexpr uint32_t DIAG0_INT_PUSHPULL_bm = 0x1000UL; +constexpr uint32_t DIAG1_PUSHPULL_bm = 0x2000UL; +constexpr uint32_t SMALL_HYSTERESIS_bm = 0x4000UL; +constexpr uint32_t STOP_ENABLE_bm = 0x8000UL; +constexpr uint32_t DIRECT_MODE_bm = 0x10000UL; +// GSTAT +constexpr uint8_t RESET_bp = 0; +constexpr uint8_t DRV_ERR_bp = 1; +constexpr uint8_t UV_CP_bp = 2; +constexpr uint32_t GSTAT_bm = 0x7UL; +constexpr uint32_t RESET_bm = 0b1UL; +constexpr uint32_t DRV_ERR_bm = 0b10UL; +constexpr uint32_t UV_CP_bm = 0b100UL; +// IOIN +constexpr uint8_t STEP_bp = 0; +constexpr uint8_t DIR_bp = 1; +constexpr uint8_t DCEN_CFG4_bp = 2; +constexpr uint8_t DCIN_CFG5_bp = 3; +constexpr uint8_t DRV_ENN_CFG6_bp = 4; +constexpr uint8_t DCO_bp = 5; +constexpr uint8_t VERSION_bp = 24; +constexpr uint32_t IOIN_bm = 0xFF00003FUL; +constexpr uint32_t STEP_bm = 0x1UL; +constexpr uint32_t DIR_bm = 0x2UL; +constexpr uint32_t DCEN_CFG4_bm = 0x4UL; +constexpr uint32_t DCIN_CFG5_bm = 0x8UL; +constexpr uint32_t DRV_ENN_CFG6_bm = 0x10UL; +constexpr uint32_t DCO_bm = 0x20UL; +constexpr uint32_t VERSION_bm = 0xFF000000UL; +// IHOLD_IRUN +constexpr uint8_t IHOLD_bp = 0; +constexpr uint8_t IRUN_bp = 8; +constexpr uint8_t IHOLDDELAY_bp = 16; +constexpr uint32_t IHOLD_IRUN_bm = 0xF1F1FUL; +constexpr uint32_t IHOLD_bm = 0x1FUL; +constexpr uint32_t IRUN_bm = 0x1F00UL; +constexpr uint32_t IHOLDDELAY_bm = 0xF0000UL; +// TPOWERDOWN +constexpr uint8_t TPOWERDOWN_bp = 0; +constexpr uint32_t TPOWERDOWN_bm = 0xFFUL; +// TSTEP +constexpr uint8_t TSTEP_bp = 0; +constexpr uint32_t TSTEP_bm = 0xFFFFFUL; +// TPWMTHRS +constexpr uint8_t TPWMTHRS_bp = 0; +constexpr uint32_t TPWMTHRS_bm = 0xFFFFFUL; +// TCOOLTHRS +constexpr uint8_t TCOOLTHRS_bp = 0; +constexpr uint32_t TCOOLTHRS_bm = 0xFFFFFUL; +// THIGH +constexpr uint8_t THIGH_bp = 0; +constexpr uint32_t THIGH_bm = 0xFFFFFUL; +// XDIRECT +constexpr uint8_t XDIRECT_bp = 0; +constexpr uint32_t XDIRECT_bm = 0xFFFFFFFFUL; +constexpr uint8_t COIL_A_bp = 0; +constexpr uint8_t COIL_B_bp = 16; +constexpr uint32_t COIL_A_bm = 0x1FFUL; +constexpr uint32_t COIL_B_bm = 0x1FF0000UL; +// VDCMIN +constexpr uint8_t VDCMIN_bp = 0; +constexpr uint32_t VDCMIN_bm = 0x7FFFFFUL; +// MSLUT0 +constexpr uint8_t MSLUT0_bp = 0; +constexpr uint32_t MSLUT0_bm = 0xFFFFFFFFUL; +// MSLUT1 +constexpr uint8_t MSLUT1_bp = 0; +constexpr uint32_t MSLUT1_bm = 0xFFFFFFFFUL; +// MSLUT2 +constexpr uint8_t MSLUT2_bp = 0; +constexpr uint32_t MSLUT2_bm = 0xFFFFFFFFUL; +// MSLUT3 +constexpr uint8_t MSLUT3_bp = 0; +constexpr uint32_t MSLUT3_bm = 0xFFFFFFFFUL; +// MSLUT4 +constexpr uint8_t MSLUT4_bp = 0; +constexpr uint32_t MSLUT4_bm = 0xFFFFFFFFUL; +// MSLUT5 +constexpr uint8_t MSLUT5_bp = 0; +constexpr uint32_t MSLUT5_bm = 0xFFFFFFFFUL; +// MSLUT6 +constexpr uint8_t MSLUT6_bp = 0; +constexpr uint32_t MSLUT6_bm = 0xFFFFFFFFUL; +// MSLUT7 +constexpr uint8_t MSLUT7_bp = 0; +constexpr uint32_t MSLUT7_bm = 0xFFFFFFFFUL; +// MSLUTSEL +constexpr uint8_t MSLUTSEL_bp = 0; +constexpr uint32_t MSLUTSEL_bm = 0xFFFFFFFFUL; +// MSLUTSTART +constexpr uint8_t START_SIN_bp = 0; +constexpr uint8_t START_SIN90_bp = 16; +constexpr uint32_t START_SIN_bm = 0xFFUL; +constexpr uint32_t START_SIN90_bm = 0xFF0000UL; +// MSCNT +constexpr uint8_t MSCNT_bp = 0; +constexpr uint32_t MSCNT_bm = 0x3FFUL; +// MSCURACT +constexpr uint8_t CUR_A_bp = 0; +constexpr uint8_t CUR_B_bp = 16; +constexpr uint32_t CUR_A_bm = 0x1FFUL; +constexpr uint32_t CUR_B_bm = 0x1FF0000UL; +// CHOPCONF +constexpr uint8_t TOFF_bp = 0; +constexpr uint8_t HSTRT_bp = 4; +constexpr uint8_t FD_bp = 4; +constexpr uint8_t HEND_bp = 7; +constexpr uint8_t DISFDCC_bp = 12; +constexpr uint8_t RNDTF_bp = 13; +constexpr uint8_t CHM_bp = 14; +constexpr uint8_t TBL_bp = 15; +constexpr uint8_t VSENSE_bp = 17; +constexpr uint8_t VHIGHFS_bp = 18; +constexpr uint8_t VHIGHCHM_bp = 19; +constexpr uint8_t SYNC_bp = 20; +constexpr uint8_t MRES_bp = 24; +constexpr uint8_t INTPOL_bp = 28; +constexpr uint8_t DEDGE_bp = 29; +constexpr uint8_t DISS2G_bp = 30; +constexpr uint32_t CHOPCONF_bm = 0xFFFFFFFFUL; +constexpr uint32_t TOFF_bm = 0xFUL; +constexpr uint32_t HSTRT_bm = 0x70UL; +constexpr uint32_t FD_bm = 0x830UL; +constexpr uint32_t HEND_bm = 0x780UL; +constexpr uint32_t DISFDCC_bm = 0x1000UL; +constexpr uint32_t RNDTF_bm = 0x2000UL; +constexpr uint32_t CHM_bm = 0x4000UL; +constexpr uint32_t TBL_bm = 0x18000UL; +constexpr uint32_t VSENSE_bm = 0x20000UL; +constexpr uint32_t VHIGHFS_bm = 0x40000UL; +constexpr uint32_t VHIGHCHM_bm = 0x80000UL; +constexpr uint32_t SYNC_bm = 0xF00000UL; +constexpr uint32_t MRES_bm = 0xF000000UL; +constexpr uint32_t INTPOL_bm = 0x10000000UL; +constexpr uint32_t DEDGE_bm = 0x20000000UL; +constexpr uint32_t DISS2G_bm = 0x40000000UL; +// COOLCONF +constexpr uint8_t SEMIN_bp = 0; +constexpr uint8_t SEUP_bp = 5; +constexpr uint8_t SEMAX_bp = 8; +constexpr uint8_t SEDN_bp = 13; +constexpr uint8_t SEIMIN_bp = 15; +constexpr uint8_t SGT_bp = 16; +constexpr uint8_t SFILT_bp = 24; +constexpr uint32_t COOLCONF_bm = 0x3FFFFFFUL; +constexpr uint32_t SEMIN_bm = 0xFUL; +constexpr uint32_t SEUP_bm = 0x60UL; +constexpr uint32_t SEMAX_bm = 0xF00UL; +constexpr uint32_t SEDN_bm = 0x6000UL; +constexpr uint32_t SEIMIN_bm = 0x8000UL; +constexpr uint32_t SGT_bm = 0x7F0000UL; +constexpr uint32_t SFILT_bm = 0x1000000UL; +// DCCTRL +constexpr uint8_t DC_TIME_bp = 0; +constexpr uint8_t DC_SG_bp = 16; +constexpr uint32_t DC_TIME_bm = 0x3FFUL; +constexpr uint32_t DC_SG_bm = 0xFF0000UL; +// DRV_STATUS +constexpr uint8_t SG_RESULT_bp = 0; +constexpr uint8_t FSACTIVE_bp = 15; +constexpr uint8_t CS_ACTUAL_bp = 16; +constexpr uint8_t STALLGUARD_bp = 24; +constexpr uint8_t OT_bp = 25; +constexpr uint8_t OTPW_bp = 26; +constexpr uint8_t S2GA_bp = 27; +constexpr uint8_t S2GB_bp = 28; +constexpr uint8_t OLA_bp = 29; +constexpr uint8_t OLB_bp = 30; +constexpr uint8_t STST_bp = 31; +constexpr uint32_t DRV_STATUS_bm = 0xFFFFFFFFUL; +constexpr uint32_t SG_RESULT_bm = 0x3FFUL; +constexpr uint32_t FSACTIVE_bm = 0x8000UL; +constexpr uint32_t CS_ACTUAL_bm = 0x1F0000UL; +constexpr uint32_t STALLGUARD_bm = 0x1000000UL; +constexpr uint32_t OT_bm = 0x2000000UL; +constexpr uint32_t OTPW_bm = 0x4000000UL; +constexpr uint32_t S2GA_bm = 0x8000000UL; +constexpr uint32_t S2GB_bm = 0x10000000UL; +constexpr uint32_t OLA_bm = 0x20000000UL; +constexpr uint32_t OLB_bm = 0x40000000UL; +constexpr uint32_t STST_bm = 0x80000000UL; +// PWMCONF +constexpr uint8_t PWM_AMPL_bp = 0; +constexpr uint8_t PWM_GRAD_bp = 8; +constexpr uint8_t PWM_FREQ_bp = 16; +constexpr uint8_t PWM_AUTOSCALE_bp = 18; +constexpr uint8_t PWM_SYMMETRIC_bp = 19; +constexpr uint8_t FREEWHEEL_bp = 20; +constexpr uint32_t PWMCONF_bm = 0x7FFFFFUL; +constexpr uint32_t PWM_AMPL_bm = 0xFFUL; +constexpr uint32_t PWM_GRAD_bm = 0xFF00UL; +constexpr uint32_t PWM_FREQ_bm = 0x30000UL; +constexpr uint32_t PWM_AUTOSCALE_bm = 0x40000UL; +constexpr uint32_t PWM_SYMMETRIC_bm = 0x80000UL; +constexpr uint32_t FREEWHEEL_bm = 0x300000UL; +// PWM_SCALE +constexpr uint8_t PWM_SCALE_bp = 0; +constexpr uint32_t PWM_SCALE_bm = 0xFFUL; +// ENCM_CTRL +constexpr uint8_t INV_bp = 0; +constexpr uint8_t MAXSPEED_bp = 1; +constexpr uint32_t INV_bm = 0x1UL; +constexpr uint32_t MAXSPEED_bm = 0x2UL; +// LOST_STEPS +constexpr uint8_t LOST_STEPS_bp = 0; +constexpr uint32_t LOST_STEPS_bm = 0xFFFFFUL; + +#endif diff --git a/src/TMC2130/TMC2130Stepper_UTILITY.h b/src/TMC2130/TMC2130Stepper_UTILITY.h new file mode 100644 index 0000000..a5eb605 --- /dev/null +++ b/src/TMC2130/TMC2130Stepper_UTILITY.h @@ -0,0 +1,33 @@ +#ifndef TMC2130Stepper_UTILITY_h +#define TMC2130Stepper_UTILITY_h + +void print_HEX(uint32_t data) { + for(uint8_t B=24; B>=4; B-=8){ + Serial.print((data>>(B+4))&0xF, HEX); + Serial.print((data>>B)&0xF, HEX); + Serial.print(":"); + } + Serial.print((data>>4)&0xF, HEX); + Serial.print(data&0xF, HEX); +} + +void print_BIN(uint32_t data) { + int b = 31; + for(; b>=24; b--){ + Serial.print((data>>b)&0b1); + } + Serial.print("."); + for(; b>=16; b--){ + Serial.print((data>>b)&0b1); + } + Serial.print("."); + for(; b>=8; b--){ + Serial.print((data>>b)&0b1); + } + Serial.print("."); + for(; b>=0; b--){ + Serial.print((data>>b)&0b1); + } +} + +#endif diff --git a/src/TimerOne.cpp b/src/TimerOne.cpp index fa2cc85..9843680 100644 --- a/src/TimerOne.cpp +++ b/src/TimerOne.cpp @@ -16,7 +16,7 @@ #include "TimerOne.h" -TimerOne Timer1; // preinstatiate +TimerOne Timer1; // preinstantiate unsigned short TimerOne::pwmPeriod = 0; unsigned char TimerOne::clockSelectBits = 0; diff --git a/src/farmbot_arduino_controller.cpp b/src/farmbot_arduino_controller.cpp index 5b3cacc..5abbca1 100644 --- a/src/farmbot_arduino_controller.cpp +++ b/src/farmbot_arduino_controller.cpp @@ -1,17 +1,20 @@ // Do not remove the include below #include "farmbot_arduino_controller.h" -#include "pins.h" -#include "Config.h" -#include "StepperControl.h" -#include "ServoControl.h" -#include "PinGuard.h" + +#if !defined(FARMDUINO_EXP_V20) #include "TimerOne.h" -#include "MemoryFree.h" -#include "Debug.h" -#include "CurrentState.h" -#include +#endif + +bool stepperInit = false; +bool stepperFlip = false; static char commandEndChar = 0x0A; +char commandChar[INCOMING_CMD_BUF_SIZE + 1]; +//String commandString = ""; +char incomingChar = 0; +char incomingCommandArray[INCOMING_CMD_BUF_SIZE]; +int incomingCommandPointer = 0; + static GCodeProcessor *gCodeProcessor = new GCodeProcessor(); int reportingPeriod = 5000; @@ -25,17 +28,13 @@ unsigned long pinGuardCurrentTime; int lastParamChangeNr = 0; -String commandString = ""; -char incomingChar = 0; -char incomingCommandArray[INCOMING_CMD_BUF_SIZE]; -int incomingCommandPointer = 0; - // Blink led routine used for testing bool blink = false; void blinkLed() { blink = !blink; - digitalWrite(LED_PIN, blink); + //digitalWrite(LED_PIN, blink); + digitalWrite(13, blink); } // Interrupt handling for: @@ -51,6 +50,8 @@ unsigned long interruptDurationMax = 0; bool interruptBusy = false; int interruptSecondTimer = 0; + +#if !defined(FARMDUINO_EXP_V20) void interrupt(void) { if (!debugInterrupt) @@ -62,287 +63,30 @@ void interrupt(void) //interruptStartTime = micros(); interruptBusy = true; - StepperControl::getInstance()->handleMovementInterrupt(); - - // Check the actions triggered once per second - //if (interruptSecondTimer >= 1000000 / MOVEMENT_INTERRUPT_SPEED) - //{ - // interruptSecondTimer = 0; - // PinGuard::getInstance()->checkPins(); - // //blinkLed(); - //} - - //interruptStopTime = micros(); - - //if (interruptStopTime > interruptStartTime) - //{ - // interruptDuration = interruptStopTime - interruptStartTime; - //} - - //if (interruptDuration > interruptDurationMax) - //{ - // interruptDurationMax = interruptDuration; - //} - + Movement::getInstance()->handleMovementInterrupt(); interruptBusy = false; } } } +#endif -//The setup function is called once at startup of the sketch -void setup() -{ +/**/ // unsigned long intrCounter = 0; - #ifdef RAMPS_V14 +#if defined(FARMDUINO_EXP_V20) +ISR(TIMER2_OVF_vect) { - // 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(X_ENCDR_A, INPUT_PULLUP); - pinMode(X_ENCDR_B, INPUT_PULLUP); - pinMode(X_ENCDR_A_Q, INPUT_PULLUP); - pinMode(X_ENCDR_B_Q, 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(Y_ENCDR_A, INPUT_PULLUP); - pinMode(Y_ENCDR_B, INPUT_PULLUP); - pinMode(Y_ENCDR_A_Q, INPUT_PULLUP); - pinMode(Y_ENCDR_B_Q, 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(Z_ENCDR_A, INPUT_PULLUP); - pinMode(Z_ENCDR_B, INPUT_PULLUP); - pinMode(Z_ENCDR_A_Q, INPUT_PULLUP); - pinMode(Z_ENCDR_B_Q, INPUT_PULLUP); - - pinMode(HEATER_0_PIN, OUTPUT); - pinMode(HEATER_1_PIN, OUTPUT); - pinMode(FAN_PIN, OUTPUT); - pinMode(LED_PIN, OUTPUT); - - pinMode(UTM_C, INPUT_PULLUP); - pinMode(UTM_D, INPUT_PULLUP); - pinMode(UTM_E, INPUT_PULLUP); - pinMode(UTM_F, INPUT_PULLUP); - pinMode(UTM_G, INPUT_PULLUP); - pinMode(UTM_H, INPUT_PULLUP); - pinMode(UTM_I, INPUT_PULLUP); - pinMode(UTM_J, INPUT_PULLUP); - pinMode(UTM_K, INPUT_PULLUP); - pinMode(UTM_L, INPUT_PULLUP); - - // Aux 1 pins to safer state - pinMode(AUX1_00, INPUT_PULLUP); - pinMode(AUX1_01, INPUT_PULLUP); - pinMode(AUX1_57, INPUT_PULLUP); - pinMode(AUX1_58, INPUT_PULLUP); - - // Aux 3 pins to safer state - pinMode(AUX3_49, INPUT_PULLUP); - pinMode(AUX3_50, INPUT_PULLUP); - pinMode(AUX3_51, INPUT_PULLUP); - - // Aux 4 pins to safer state - pinMode(AUX4_43, INPUT_PULLUP); - pinMode(AUX4_45, INPUT_PULLUP); - pinMode(AUX4_47, INPUT_PULLUP); - pinMode(AUX4_32, INPUT_PULLUP); - - pinMode(SERVO_0_PIN, OUTPUT); - pinMode(SERVO_1_PIN, OUTPUT); - pinMode(SERVO_2_PIN, OUTPUT); - pinMode(SERVO_3_PIN, OUTPUT); - - #endif - - #if defined(FARMDUINO_V10) || defined(FARMDUINO_V14) - - // 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(X_ENCDR_A, INPUT_PULLUP); - pinMode(X_ENCDR_B, INPUT_PULLUP); - //pinMode(X_ENCDR_A_Q, INPUT_PULLUP); - //pinMode(X_ENCDR_B_Q, 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(Y_ENCDR_A, INPUT_PULLUP); - pinMode(Y_ENCDR_B, INPUT_PULLUP); - //pinMode(Y_ENCDR_A_Q, INPUT_PULLUP); - //pinMode(Y_ENCDR_B_Q, 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(Z_ENCDR_A, INPUT_PULLUP); - pinMode(Z_ENCDR_B, INPUT_PULLUP); - //pinMode(Z_ENCDR_A_Q, INPUT_PULLUP); - //pinMode(Z_ENCDR_B_Q, INPUT_PULLUP); - - pinMode(AUX_STEP_PIN, OUTPUT); - pinMode(AUX_DIR_PIN, OUTPUT); - pinMode(AUX_ENABLE_PIN, OUTPUT); - digitalWrite(AUX_ENABLE_PIN, HIGH); - - pinMode(LED_PIN, OUTPUT); - pinMode(VACUUM_PIN, OUTPUT); - pinMode(WATER_PIN, OUTPUT); - pinMode(LIGHTING_PIN, OUTPUT); - pinMode(PERIPHERAL_4_PIN, OUTPUT); - pinMode(PERIPHERAL_5_PIN, OUTPUT); - - pinMode(UTM_C, INPUT_PULLUP); - pinMode(UTM_D, INPUT_PULLUP); - if (UTM_E > 0) { pinMode(UTM_E, INPUT_PULLUP); }; - if (UTM_F > 0) { pinMode(UTM_F, INPUT_PULLUP); }; - if (UTM_G > 0) { pinMode(UTM_G, INPUT_PULLUP); }; - if (UTM_H > 0) { pinMode(UTM_H, INPUT_PULLUP); }; - if (UTM_I > 0) { pinMode(UTM_I, INPUT_PULLUP); }; - if (UTM_J > 0) { pinMode(UTM_J, INPUT_PULLUP); }; - if (UTM_K > 0) { pinMode(UTM_K, INPUT_PULLUP); }; - if (UTM_L > 0) { pinMode(UTM_L, INPUT_PULLUP); }; - - pinMode(SERVO_0_PIN, OUTPUT); - pinMode(SERVO_1_PIN, OUTPUT); - pinMode(SERVO_2_PIN, OUTPUT); - pinMode(SERVO_3_PIN, OUTPUT); - - #endif - - digitalWrite(X_ENABLE_PIN, HIGH); - digitalWrite(E_ENABLE_PIN, HIGH); - digitalWrite(Y_ENABLE_PIN, HIGH); - digitalWrite(Z_ENABLE_PIN, HIGH); - - #if defined(FARMDUINO_V14) - - reportingPeriod = 500; - - pinMode(READ_ENA_PIN, INPUT_PULLUP); - pinMode(NSS_PIN, OUTPUT); - digitalWrite(NSS_PIN, HIGH); - - SPI.setBitOrder(MSBFIRST); - SPI.setDataMode(SPI_MODE0); - SPI.setClockDivider(SPI_CLOCK_DIV4); - SPI.begin(); - - #endif - - Serial.begin(115200); - - delay(100); - - // Start the motor handling - //ServoControl::getInstance()->attach(); - - // Load motor settings - StepperControl::getInstance()->loadSettings(); - - // Dump all values to the serial interface - // ParameterList::getInstance()->readAllValues(); - - // Get the settings for the pin guard - PinGuard::getInstance()->loadConfig(); - pinGuardLastCheck = millis(); - - // 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(); - - pinGuardCurrentTime = millis(); - pinGuardLastCheck = millis(); - - if - ( - (ParameterList::getInstance()->getValue(PARAM_CONFIG_OK) == 1) && - (ParameterList::getInstance()->getValue(MOVEMENT_HOME_AT_BOOT_Z) == 1) - ) + if (interruptBusy == false) { - Serial.print("R99 HOME Z ON STARTUP\r\n"); - StepperControl::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, false, true); - } + interruptBusy = true; - if - ( - (ParameterList::getInstance()->getValue(PARAM_CONFIG_OK) == 1) && - (ParameterList::getInstance()->getValue(MOVEMENT_HOME_AT_BOOT_Y) == 1) - ) - { - Serial.print("R99 HOME Y ON STARTUP\r\n"); - StepperControl::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, true, false); + Movement::getInstance()->handleMovementInterrupt(); + interruptBusy = false; } - - if - ( - (ParameterList::getInstance()->getValue(PARAM_CONFIG_OK) == 1) && - (ParameterList::getInstance()->getValue(MOVEMENT_HOME_AT_BOOT_X) == 1) - ) - { - Serial.print("R99 HOME X ON STARTUP\r\n"); - StepperControl::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, true, false, false); - } - - Serial.print("R99 ARDUINO STARTUP COMPLETE\r\n"); } +#endif -//char commandIn[100]; -char commandChar[INCOMING_CMD_BUF_SIZE + 1]; - -// The loop function is called in an endless loop -void loop() +void checkPinGuard() { - - if (debugInterrupt) - { - StepperControl::getInstance()->handleMovementInterrupt(); - } - - #if defined(FARMDUINO_V14) - // Check encoders out of interrupt for farmduino 1.4 - StepperControl::getInstance()->checkEncoders(); - #endif - pinGuardCurrentTime = millis(); if (pinGuardCurrentTime < pinGuardLastCheck) { @@ -358,10 +102,86 @@ void loop() PinGuard::getInstance()->checkPins(); } } +} - if (Serial.available()) +void periodicChecksAndReport() +{ + + // Do periodic checks and feedback + currentTime = millis(); + if (currentTime < lastAction) { + // If the device timer overruns, reset the idle counter + lastAction = millis(); + } + else + { + + if ((currentTime - lastAction) > reportingPeriod) + { + // After an idle time, send the idle message + + if (CurrentState::getInstance()->isEmergencyStop()) + { + Serial.print(COMM_REPORT_EMERGENCY_STOP); + CurrentState::getInstance()->printQAndNewLine(); + + if (debugMessages) + { + Serial.print(COMM_REPORT_COMMENT); + Serial.print(" Emergency stop engaged"); + CurrentState::getInstance()->printQAndNewLine(); + } + } + else + { + Serial.print(COMM_REPORT_CMD_IDLE); + CurrentState::getInstance()->printQAndNewLine(); + } + + Movement::getInstance()->storePosition(); + CurrentState::getInstance()->printPosition(); + + Movement::getInstance()->reportEncoders(); + CurrentState::getInstance()->storeEndStops(); + CurrentState::getInstance()->printEndStops(); + + // Movement::getInstance()->test(); + + if (debugMessages) + { + Serial.print(COMM_REPORT_COMMENT); + Serial.print(" MEM "); + Serial.print(freeMemory()); + CurrentState::getInstance()->printQAndNewLine(); + + Serial.print(COMM_REPORT_COMMENT); + Serial.print(" IND DUR "); + Serial.print(interruptDuration); + Serial.print(" MAX "); + Serial.print(interruptDurationMax); + CurrentState::getInstance()->printQAndNewLine(); + + //Movement::getInstance()->test(); + } + + if (ParameterList::getInstance()->getValue(PARAM_CONFIG_OK) != 1) + { + Serial.print(COMM_REPORT_NO_CONFIG); + CurrentState::getInstance()->printQAndNewLine(); + } + + cycleCounter++; + lastAction = millis(); + } + } +} + +void checkSerialInputs() +{ + if (Serial.available()) + { // Save current time stamp for timeout actions lastAction = millis(); @@ -409,12 +229,8 @@ void loop() Serial.print("\r\n"); // Create a command and let it execute - //Command* command = new Command(commandString); Command *command = new Command(commandChar); - //strcpy() - //commandEcho - // Log the values if needed for debugging if (LOGGING || debugMessages) { @@ -430,12 +246,15 @@ void loop() incomingCommandPointer = 0; } } +} +void checkEmergencyStop() +{ // In case of emergency stop, disable movement and // shut down the pins used if (previousEmergencyStop == false && CurrentState::getInstance()->isEmergencyStop()) { - StepperControl::getInstance()->disableMotorsEmergency(); + Movement::getInstance()->disableMotorsEmergency(); PinControl::getInstance()->resetPinsUsed(); ServoControl::getInstance()->detachServos(); if (debugMessages) @@ -446,8 +265,11 @@ void loop() } } previousEmergencyStop = CurrentState::getInstance()->isEmergencyStop(); +} - // Check if parameters are changes, and if so load the new settings +void checkParamsChanged() +{ + // Check if parameters are changed, and if so load the new settings if (lastParamChangeNr != ParameterList::getInstance()->paramChangeNumber()) { lastParamChangeNr = ParameterList::getInstance()->paramChangeNumber(); @@ -459,100 +281,629 @@ void loop() CurrentState::getInstance()->printQAndNewLine(); } - StepperControl::getInstance()->loadSettings(); + /* + #if defined(FARMDUINO_EXP_V20) + loadTMC2130paraeters(); + #endif + */ + + Movement::getInstance()->loadSettings(); PinGuard::getInstance()->loadConfig(); } +} - // Do periodic checks and feedback +void checkEncoders() +{ + + #if defined(FARMDUINO_V14) + // Check encoders out of interrupt for farmduino 1.4 + Movement::getInstance()->checkEncoders(); + #endif + +} - currentTime = millis(); - if (currentTime < lastAction) +void runTestForDebug() +{ + + //Serial.print("* "); + //Serial.print(intrCounter / 1000); + //Serial.print("\r\n"); + //delay(500); + + //Serial.print("z"); + //blinkLed(); + //delay(500); + + //Serial.print(millis()); + //Serial.print("X"); + //Serial.print("\r\n"); + //delay(1000); + + //blinkLed(); + + //Movement::getInstance()->test(); + //delayMicroseconds(100); + + //digitalWrite(LED_PIN, true); + //delay(250); + //digitalWrite(LED_PIN, false); + //delay(250); + + //if (debugInterrupt) + //{ + // Movement::getInstance()->handleMovementInterrupt(); + //} +} + +// Set pins input output +#ifdef RAMPS_V14 +void setPinInputOutput() +{ + + // 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(X_ENCDR_A, INPUT_PULLUP); + pinMode(X_ENCDR_B, INPUT_PULLUP); + pinMode(X_ENCDR_A_Q, INPUT_PULLUP); + pinMode(X_ENCDR_B_Q, 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(Y_ENCDR_A, INPUT_PULLUP); + pinMode(Y_ENCDR_B, INPUT_PULLUP); + pinMode(Y_ENCDR_A_Q, INPUT_PULLUP); + pinMode(Y_ENCDR_B_Q, 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(Z_ENCDR_A, INPUT_PULLUP); + pinMode(Z_ENCDR_B, INPUT_PULLUP); + pinMode(Z_ENCDR_A_Q, INPUT_PULLUP); + pinMode(Z_ENCDR_B_Q, INPUT_PULLUP); + + pinMode(HEATER_0_PIN, OUTPUT); + pinMode(HEATER_1_PIN, OUTPUT); + pinMode(FAN_PIN, OUTPUT); + pinMode(LED_PIN, OUTPUT); + + pinMode(UTM_C, INPUT_PULLUP); + pinMode(UTM_D, INPUT_PULLUP); + pinMode(UTM_E, INPUT_PULLUP); + pinMode(UTM_F, INPUT_PULLUP); + pinMode(UTM_G, INPUT_PULLUP); + pinMode(UTM_H, INPUT_PULLUP); + pinMode(UTM_I, INPUT_PULLUP); + pinMode(UTM_J, INPUT_PULLUP); + pinMode(UTM_K, INPUT_PULLUP); + pinMode(UTM_L, INPUT_PULLUP); + + // Aux 1 pins to safer state + pinMode(AUX1_00, INPUT_PULLUP); + pinMode(AUX1_01, INPUT_PULLUP); + pinMode(AUX1_57, INPUT_PULLUP); + pinMode(AUX1_58, INPUT_PULLUP); + + // Aux 3 pins to safer state + pinMode(AUX3_49, INPUT_PULLUP); + pinMode(AUX3_50, INPUT_PULLUP); + pinMode(AUX3_51, INPUT_PULLUP); + + // Aux 4 pins to safer state + pinMode(AUX4_43, INPUT_PULLUP); + pinMode(AUX4_45, INPUT_PULLUP); + pinMode(AUX4_47, INPUT_PULLUP); + pinMode(AUX4_32, INPUT_PULLUP); + + pinMode(SERVO_0_PIN, OUTPUT); + pinMode(SERVO_1_PIN, OUTPUT); + pinMode(SERVO_2_PIN, OUTPUT); + pinMode(SERVO_3_PIN, OUTPUT); + + Serial.print(COMM_REPORT_COMMENT); + Serial.print(SPACE); + Serial.print("Set input/output"); + Serial.print(CRLF); +} +#endif + +#if defined(FARMDUINO_V10) || defined(FARMDUINO_V14) +void setPinInputOutput() +{ + + // 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(X_ENCDR_A, INPUT_PULLUP); + pinMode(X_ENCDR_B, INPUT_PULLUP); + //pinMode(X_ENCDR_A_Q, INPUT_PULLUP); + //pinMode(X_ENCDR_B_Q, 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(Y_ENCDR_A, INPUT_PULLUP); + pinMode(Y_ENCDR_B, INPUT_PULLUP); + //pinMode(Y_ENCDR_A_Q, INPUT_PULLUP); + //pinMode(Y_ENCDR_B_Q, 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(Z_ENCDR_A, INPUT_PULLUP); + pinMode(Z_ENCDR_B, INPUT_PULLUP); + //pinMode(Z_ENCDR_A_Q, INPUT_PULLUP); + //pinMode(Z_ENCDR_B_Q, INPUT_PULLUP); + + pinMode(AUX_STEP_PIN, OUTPUT); + pinMode(AUX_DIR_PIN, OUTPUT); + pinMode(AUX_ENABLE_PIN, OUTPUT); + digitalWrite(AUX_ENABLE_PIN, HIGH); + + pinMode(LED_PIN, OUTPUT); + pinMode(VACUUM_PIN, OUTPUT); + pinMode(WATER_PIN, OUTPUT); + pinMode(LIGHTING_PIN, OUTPUT); + pinMode(PERIPHERAL_4_PIN, OUTPUT); + pinMode(PERIPHERAL_5_PIN, OUTPUT); + + pinMode(UTM_C, INPUT_PULLUP); + pinMode(UTM_D, INPUT_PULLUP); + if (UTM_E > 0) { pinMode(UTM_E, INPUT_PULLUP); }; + if (UTM_F > 0) { pinMode(UTM_F, INPUT_PULLUP); }; + if (UTM_G > 0) { pinMode(UTM_G, INPUT_PULLUP); }; + if (UTM_H > 0) { pinMode(UTM_H, INPUT_PULLUP); }; + if (UTM_I > 0) { pinMode(UTM_I, INPUT_PULLUP); }; + if (UTM_J > 0) { pinMode(UTM_J, INPUT_PULLUP); }; + if (UTM_K > 0) { pinMode(UTM_K, INPUT_PULLUP); }; + if (UTM_L > 0) { pinMode(UTM_L, INPUT_PULLUP); }; + + pinMode(SERVO_0_PIN, OUTPUT); + pinMode(SERVO_1_PIN, OUTPUT); + pinMode(SERVO_2_PIN, OUTPUT); + pinMode(SERVO_3_PIN, OUTPUT); + + Serial.print(COMM_REPORT_COMMENT); + Serial.print(SPACE); + Serial.print("Set input/output"); + Serial.print(CRLF); + +#if defined(FARMDUINO_V14) + + reportingPeriod = 500; + + pinMode(READ_ENA_PIN, INPUT_PULLUP); + pinMode(NSS_PIN, OUTPUT); + digitalWrite(NSS_PIN, HIGH); + + SPI.setBitOrder(MSBFIRST); + SPI.setDataMode(SPI_MODE0); + SPI.setClockDivider(SPI_CLOCK_DIV4); + SPI.begin(); +#endif +} +#endif + +#if defined(FARMDUINO_EXP_V20) +void setPinInputOutput() +{ + + // Motor step, direction and pin is set up using the control chip library + // This board also does not use encoders + + pinMode(X_ENABLE_PIN, OUTPUT); + pinMode(X_MIN_PIN, INPUT_PULLUP); + pinMode(X_MAX_PIN, INPUT_PULLUP); + + pinMode(E_ENABLE_PIN, OUTPUT); + + pinMode(Y_ENABLE_PIN, OUTPUT); + pinMode(Y_MIN_PIN, INPUT_PULLUP); + pinMode(Y_MAX_PIN, INPUT_PULLUP); + + pinMode(Z_ENABLE_PIN, OUTPUT); + pinMode(Z_MIN_PIN, INPUT_PULLUP); + pinMode(Z_MAX_PIN, INPUT_PULLUP); + + pinMode(AUX_STEP_PIN, OUTPUT); + pinMode(AUX_DIR_PIN, OUTPUT); + pinMode(AUX_ENABLE_PIN, OUTPUT); + + pinMode(LED_PIN, OUTPUT); + pinMode(VACUUM_PIN, OUTPUT); + pinMode(WATER_PIN, OUTPUT); + pinMode(LIGHTING_PIN, OUTPUT); + pinMode(PERIPHERAL_4_PIN, OUTPUT); + pinMode(PERIPHERAL_5_PIN, OUTPUT); + + pinMode(UTM_C, INPUT_PULLUP); + pinMode(UTM_D, INPUT_PULLUP); + if (UTM_E > 0) { pinMode(UTM_E, INPUT_PULLUP); }; + if (UTM_F > 0) { pinMode(UTM_F, INPUT_PULLUP); }; + if (UTM_G > 0) { pinMode(UTM_G, INPUT_PULLUP); }; + if (UTM_H > 0) { pinMode(UTM_H, INPUT_PULLUP); }; + if (UTM_I > 0) { pinMode(UTM_I, INPUT_PULLUP); }; + if (UTM_J > 0) { pinMode(UTM_J, INPUT_PULLUP); }; + if (UTM_K > 0) { pinMode(UTM_K, INPUT_PULLUP); }; + if (UTM_L > 0) { pinMode(UTM_L, INPUT_PULLUP); }; + + pinMode(SERVO_0_PIN, OUTPUT); + pinMode(SERVO_1_PIN, OUTPUT); + pinMode(SERVO_2_PIN, OUTPUT); + pinMode(SERVO_3_PIN, OUTPUT); + + Serial.print(COMM_REPORT_COMMENT); + Serial.print(SPACE); + Serial.print("Set input/output"); + Serial.print(CRLF); +} +#endif + +// other initialisation functions +void startSerial() +{ + Serial.begin(115200); + delay(100); + while (!Serial); + + Serial.print(COMM_REPORT_COMMENT); + Serial.print(SPACE); + Serial.print("Serial connection started"); + Serial.print(CRLF); +} + + +#if defined(FARMDUINO_EXP_V20) +void startupTmc() +{ + + // Initialize the drivers + Serial.print(COMM_REPORT_COMMENT); + Serial.print(SPACE); + Serial.print("Init TMC2130 drivers"); + Serial.print(CRLF); + + TMC2130X->begin(); + TMC2130Y->begin(); + TMC2130Z->begin(); + TMC2130E->begin(); + + // Load the motor parameters + Serial.print(COMM_REPORT_COMMENT); + Serial.print(SPACE); + Serial.print("Load TMC2130 parameters"); + Serial.print(CRLF); + + Movement::getInstance()->initTMC2130(); + Movement::getInstance()->loadSettingsTMC2130(); + + + /* + Serial.print(COMM_REPORT_COMMENT); + Serial.print(SPACE); + Serial.print("Set shaft dir"); + Serial.print(CRLF); + + TMC2130X->shaft_dir(0); + TMC2130Y->shaft_dir(0); + TMC2130Z->shaft_dir(0); + TMC2130E->shaft_dir(0); + */ +} +#endif + +void startInterrupt() +{ + Serial.print(COMM_REPORT_COMMENT); + Serial.print(SPACE); + Serial.print("Start interrupt"); + Serial.print(CRLF); + + + // Start the interrupt used for moving + // Interrupt management code library written by Paul Stoffregen + // The default time 100 micro seconds + + #if !defined(FARMDUINO_EXP_V20) + Timer1.attachInterrupt(interrupt); + Timer1.initialize(MOVEMENT_INTERRUPT_SPEED); + Timer1.start(); + #endif + + #if defined(FARMDUINO_EXP_V20) + Serial.println("set timer"); + TIMSK2 = (TIMSK2 & B11111110) | 0x01; // Enable timer overflow + TCCR2B = (TCCR2B & B11111000) | 0x01; // Set divider to 1 + OCR2A = 4; // Set overflow to 4 for total of 64 µs + #endif +} + +void homeOnBoot() +{ + Serial.print(COMM_REPORT_COMMENT); + Serial.print(SPACE); + Serial.print("Check homing on boot"); + Serial.print(CRLF); + + if + ( + (ParameterList::getInstance()->getValue(PARAM_CONFIG_OK) == 1) && + (ParameterList::getInstance()->getValue(MOVEMENT_HOME_AT_BOOT_Z) == 1) + ) { - - // If the device timer overruns, reset the idle counter - lastAction = millis(); + Serial.print("R99 HOME Z ON STARTUP\r\n"); + Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, false, true); } - else + + if + ( + (ParameterList::getInstance()->getValue(PARAM_CONFIG_OK) == 1) && + (ParameterList::getInstance()->getValue(MOVEMENT_HOME_AT_BOOT_Y) == 1) + ) { + Serial.print("R99 HOME Y ON STARTUP\r\n"); + Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, true, false); + } - if ((currentTime - lastAction) > reportingPeriod) - { - // After an idle time, send the idle message - - if (CurrentState::getInstance()->isEmergencyStop()) - { - Serial.print(COMM_REPORT_EMERGENCY_STOP); - CurrentState::getInstance()->printQAndNewLine(); - - if (debugMessages) - { - Serial.print(COMM_REPORT_COMMENT); - Serial.print(" Emergency stop engaged"); - CurrentState::getInstance()->printQAndNewLine(); - } - } - else - { - Serial.print(COMM_REPORT_CMD_IDLE); - CurrentState::getInstance()->printQAndNewLine(); - } - - StepperControl::getInstance()->storePosition(); - CurrentState::getInstance()->printPosition(); - - StepperControl::getInstance()->reportEncoders(); - - CurrentState::getInstance()->storeEndStops(); - CurrentState::getInstance()->printEndStops(); - - if (debugMessages) - { - Serial.print(COMM_REPORT_COMMENT); - Serial.print(" MEM "); - Serial.print(freeMemory()); - CurrentState::getInstance()->printQAndNewLine(); - - Serial.print(COMM_REPORT_COMMENT); - Serial.print(" IND DUR "); - Serial.print(interruptDuration); - Serial.print(" MAX "); - Serial.print(interruptDurationMax); - CurrentState::getInstance()->printQAndNewLine(); - - //StepperControl::getInstance()->checkEncoders(); - //Serial.print(COMM_REPORT_COMMENT); - //Serial.print(" Cycle "); - //Serial.print(cycleCounter); - //CurrentState::getInstance()->printQAndNewLine(); - - -// Serial.print(COMM_REPORT_COMMENT); -// Serial.print(" 16 "); -// Serial.print(PORTH & 0x02); -// Serial.print(PH4); -// Serial.print(" "); -// Serial.print(PINH); -// Serial.print(" "); -// Serial.print(digitalRead(16)); -// Serial.print(" "); -// Serial.print(" 17 "); -// Serial.print(PORTH & 0x01); -// Serial.print(PH5); -// Serial.print(" "); -// Serial.print(digitalRead(17)); -// CurrentState::getInstance()->printQAndNewLine(); - - StepperControl::getInstance()->test(); - } - - if (ParameterList::getInstance()->getValue(PARAM_CONFIG_OK) != 1) - { - Serial.print(COMM_REPORT_NO_CONFIG); - CurrentState::getInstance()->printQAndNewLine(); - } - - cycleCounter++; - lastAction = millis(); - } + if + ( + (ParameterList::getInstance()->getValue(PARAM_CONFIG_OK) == 1) && + (ParameterList::getInstance()->getValue(MOVEMENT_HOME_AT_BOOT_X) == 1) + ) + { + Serial.print("R99 HOME X ON STARTUP\r\n"); + Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, true, false, false); } } + +void startMotor() +{ + // Start the motor handling + Serial.print(COMM_REPORT_COMMENT); + Serial.print(SPACE); + Serial.print("Set motor enables off"); + Serial.print(CRLF); + + digitalWrite(X_ENABLE_PIN, HIGH); + digitalWrite(E_ENABLE_PIN, HIGH); + digitalWrite(Y_ENABLE_PIN, HIGH); + digitalWrite(Z_ENABLE_PIN, HIGH); +} + +void loadMovementSetting() +{ + + // Load motor settings + Serial.print(COMM_REPORT_COMMENT); + Serial.print(SPACE); + Serial.print("Load movement settings"); + Serial.print(CRLF); + + Movement::getInstance()->loadSettings(); +} + +void readParameters() +{ + // Dump all values to the serial interface + Serial.print(COMM_REPORT_COMMENT); + Serial.print(SPACE); + Serial.print("Read parameters"); + Serial.print(CRLF); + + ParameterList::getInstance()->readAllValues(); +} + +void startPinGuard() +{ + Serial.print(COMM_REPORT_COMMENT); + Serial.print(SPACE); + Serial.print("Start pin guard"); + Serial.print(CRLF); + + // Get the settings for the pin guard + PinGuard::getInstance()->loadConfig(); + + pinGuardCurrentTime = millis(); + pinGuardLastCheck = millis(); +} + +void startServo() +{ + + Serial.print(COMM_REPORT_COMMENT); + Serial.print(SPACE); + Serial.print("Start servo"); + Serial.print(CRLF); + + ServoControl::getInstance()->attach(); +} + +#if defined(FARMDUINO_EXP_V20) +void loadTMC2130drivers() +{ + Serial.print(COMM_REPORT_COMMENT); + Serial.print(SPACE); + Serial.print("Load TMC drivers"); + Serial.print(CRLF); + + TMC2130Stepper controllerTMC2130_X = TMC2130Stepper(X_CHIP_SELECT); + TMC2130Stepper controllerTMC2130_Y = TMC2130Stepper(Y_CHIP_SELECT); + TMC2130Stepper controllerTMC2130_Z = TMC2130Stepper(Z_CHIP_SELECT); + TMC2130Stepper controllerTMC2130_E = TMC2130Stepper(E_CHIP_SELECT); + + TMC2130X = &controllerTMC2130_X; + TMC2130Y = &controllerTMC2130_Y; + TMC2130Z = &controllerTMC2130_Z; + TMC2130E = &controllerTMC2130_E; + + int motorCurrentX; + int stallSensitivityX; + int microStepsX; + + int motorCurrentY; + int stallSensitivityY; + int microStepsY; + + int motorCurrentZ; + int stallSensitivityZ; + int microStepsZ; + + motorCurrentX = 600; + stallSensitivityX = 0; + microStepsX = 0; + + motorCurrentY = 600; + stallSensitivityY = 0; + microStepsY = 0; + + motorCurrentZ = 600; + stallSensitivityZ = 0; + microStepsZ = 0; + + motorCurrentX = ParameterList::getInstance()->getValue(MOVEMENT_MOTOR_CURRENT_X); + stallSensitivityX = ParameterList::getInstance()->getValue(MOVEMENT_STALL_SENSITIVITY_X); + microStepsX = ParameterList::getInstance()->getValue(MOVEMENT_MICROSTEPS_X); + + motorCurrentY = ParameterList::getInstance()->getValue(MOVEMENT_MOTOR_CURRENT_Y); + stallSensitivityY = ParameterList::getInstance()->getValue(MOVEMENT_STALL_SENSITIVITY_Y); + microStepsY = ParameterList::getInstance()->getValue(MOVEMENT_MICROSTEPS_Y); + + motorCurrentZ = ParameterList::getInstance()->getValue(MOVEMENT_MOTOR_CURRENT_Z); + stallSensitivityZ = ParameterList::getInstance()->getValue(MOVEMENT_STALL_SENSITIVITY_Z); + microStepsX = ParameterList::getInstance()->getValue(MOVEMENT_MICROSTEPS_Z); + +/* + TMC2130X->push(); + TMC2130X->toff(3); + TMC2130X->tbl(1); + TMC2130X->hysteresis_start(4); + TMC2130X->hysteresis_end(-2); + TMC2130X->rms_current(motorCurrentX); // mA + TMC2130X->microsteps(microStepsX); + TMC2130X->diag1_stall(1); + TMC2130X->diag1_active_high(1); + TMC2130X->coolstep_min_speed(0xFFFFF); // 20bit max + TMC2130X->THIGH(0); + TMC2130X->semin(5); + TMC2130X->semax(2); + TMC2130X->sedn(0b01); + TMC2130X->sg_stall_value(stallSensitivityX); + + TMC2130Y->push(); + TMC2130Y->toff(3); + TMC2130Y->tbl(1); + TMC2130Y->hysteresis_start(4); + TMC2130Y->hysteresis_end(-2); + TMC2130Y->rms_current(motorCurrentY); // mA + TMC2130Y->microsteps(microStepsY); + TMC2130Y->diag1_stall(1); + TMC2130Y->diag1_active_high(1); + TMC2130Y->coolstep_min_speed(0xFFFFF); // 20bit max + TMC2130Y->THIGH(0); + TMC2130Y->semin(5); + TMC2130Y->semax(2); + TMC2130Y->sedn(0b01); + TMC2130Y->sg_stall_value(stallSensitivityY); + + TMC2130Z->push(); + TMC2130Z->toff(3); + TMC2130Z->tbl(1); + TMC2130Z->hysteresis_start(4); + TMC2130Z->hysteresis_end(-2); + TMC2130Z->rms_current(motorCurrentZ); // mA + TMC2130Z->microsteps(microStepsZ); + TMC2130Z->diag1_stall(1); + TMC2130Z->diag1_active_high(1); + TMC2130Z->coolstep_min_speed(0xFFFFF); // 20bit max + TMC2130Z->THIGH(0); + TMC2130Z->semin(5); + TMC2130Z->semax(2); + TMC2130Z->sedn(0b01); + TMC2130Z->sg_stall_value(stallSensitivityZ); + + TMC2130E->push(); + TMC2130E->toff(3); + TMC2130E->tbl(1); + TMC2130E->hysteresis_start(4); + TMC2130E->hysteresis_end(-2); + TMC2130E->rms_current(motorCurrentX); // mA + TMC2130E->microsteps(microStepsX); + TMC2130E->diag1_stall(1); + TMC2130E->diag1_active_high(1); + TMC2130E->coolstep_min_speed(0xFFFFF); // 20bit max + TMC2130E->THIGH(0); + TMC2130E->semin(5); + TMC2130E->semax(2); + TMC2130E->sedn(0b01); + TMC2130E->sg_stall_value(stallSensitivityX); +*/ + + + TMC2130X->rms_current(motorCurrentX); // Set the required current in mA + TMC2130X->microsteps(microStepsX); // Minimum of micro steps needed + TMC2130X->chm(true); // Set the chopper mode to classic const. off time + TMC2130X->diag1_stall(1); // Activate stall diagnostics + TMC2130X->sgt(stallSensitivityX); // Set stall detection sensitivity. most -64 to +64 least + TMC2130X->shaft_dir(0); // Set direction + + TMC2130Y->rms_current(motorCurrentX); // Set the required current in mA + TMC2130Y->microsteps(microStepsX); // Minimum of micro steps needed + TMC2130Y->chm(true); // Set the chopper mode to classic const. off time + TMC2130Y->diag1_stall(1); // Activate stall diagnostics + TMC2130Y->sgt(stallSensitivityX); // Set stall detection sensitivity. most -64 to +64 least + TMC2130Y->shaft_dir(0); // Set direction + + TMC2130Z->rms_current(motorCurrentX); // Set the required current in mA + TMC2130Z->microsteps(microStepsX); // Minimum of micro steps needed + TMC2130Z->chm(true); // Set the chopper mode to classic const. off time + TMC2130Z->diag1_stall(1); // Activate stall diagnostics + TMC2130Z->sgt(stallSensitivityX); // Set stall detection sensitivity. most -64 to +64 least + TMC2130Z->shaft_dir(0); // Set direction + + TMC2130E->rms_current(motorCurrentX); // Set the required current in mA + TMC2130E->microsteps(microStepsX); // Minimum of micro steps needed + TMC2130E->chm(true); // Set the chopper mode to classic const. off time + TMC2130E->diag1_stall(1); // Activate stall diagnostics + TMC2130E->sgt(stallSensitivityX); // Set stall detection sensitivity. most -64 to +64 least + TMC2130E->shaft_dir(0); // Set direction +} + +void loadTMC2130parameters() +{ +} +#endif + +void initLastAction() +{ + // Initialize the inactivity check + lastAction = millis(); +} \ No newline at end of file diff --git a/src/farmbot_arduino_controller.h b/src/farmbot_arduino_controller.h index 426026b..6029496 100644 --- a/src/farmbot_arduino_controller.h +++ b/src/farmbot_arduino_controller.h @@ -7,10 +7,31 @@ #ifndef _farmbot_arduino_controller_H_ #define _farmbot_arduino_controller_H_ #include "Arduino.h" +//#include "Arduino.h" + //add your includes for the project farmbot_arduino_controller here +#include "Board.h"; +#include "pins.h" +#include "Config.h" +#include "MemoryFree.h" +#include "Debug.h" + +#include "TMC2130.h" + +/**/ +#include "Movement.h" +#include "ServoControl.h" +#include "PinGuard.h" +#include "CurrentState.h" +#include #include "Command.h" #include "GCodeProcessor.h" + +//#include +//#include + +/* //end of add your includes here #ifdef __cplusplus extern "C" { @@ -20,10 +41,35 @@ void setup(); #ifdef __cplusplus } // extern "C" #endif +*/ //add your function definitions for the project farmbot_arduino_controller here + void setPinInputOutput(); + void startSerial(); + + #if defined(FARMDUINO_EXP_V20) + void loadTMC2130drivers(); + void loadTMC2130parameters(); + void startupTmc(); + #endif + + void startMotor(); + void readParameters(); + void loadMovementSetting(); + void startPinGuard(); + void startServo(); + void startInterrupt(); + void homeOnBoot(); + void runTestForDebug(); + void checkEncoders(); + void checkPinGuard(); + void checkSerialInputs(); + void checkEmergencyStop(); + void checkParamsChanged(); + void periodicChecksAndReport(); + void initLastAction(); //Do not add code below this line -#endif /* _farmbot_arduino_controller_H_ */ +#endif /* _farmbot_arduino_controller_H_ */ \ No newline at end of file diff --git a/src/pins.h b/src/pins.h index b6480dc..bb9d4df 100644 --- a/src/pins.h +++ b/src/pins.h @@ -256,3 +256,132 @@ #endif + +#if defined(FARMDUINO_EXP_V20) + + // X1-AXIS + #define X_STEP_PIN 26 // X1_STEP_PIN + #define X_DIR_PIN 27 // X1_DIR_PIN + #define X_ENABLE_PIN 25 // X1_ENABLE_PIN + #define X_CHIP_SELECT 24 // X1_CHIP_SELECT + #define X_MIN_PIN 69 + #define X_MAX_PIN 68 + #define X_ENCDR_A 16 + #define X_ENCDR_B 17 + #define X_ENCDR_A_Q -1 // N/A + #define X_ENCDR_B_Q -1 // N/A + + // X2-AXIS + #define E_STEP_PIN 15 // X2_STEP_PIN + #define E_DIR_PIN 30 // X2_DIR_PIN + #define E_ENABLE_PIN 14 // X2_ENABLE_PIN + #define E_CHIP_SELECT 29 // X2_CHIP_SELECT + #define X2_ENCDR_A 22 + #define X2_ENCDR_B 39 + + // Y-AXIS + #define Y_STEP_PIN 32 + #define Y_DIR_PIN 33 + #define Y_ENABLE_PIN 31 + #define Y_CHIP_SELECT 28 // Y_CHIP_SELECT + #define Y_MIN_PIN 67 + #define Y_MAX_PIN 66 + #define Y_ENCDR_A 23 + #define Y_ENCDR_B 24 + #define Y_ENCDR_A_Q -1 // N/A + #define Y_ENCDR_B_Q -1 // N/A + + // Z-AXIS + #define Z_STEP_PIN 35 + #define Z_DIR_PIN 36 + #define Z_ENABLE_PIN 34 + #define Z_CHIP_SELECT 23 // Z_CHIP_SELECT + #define Z_MIN_PIN 65 + #define Z_MAX_PIN 64 + #define Z_ENCDR_A 29 + #define Z_ENCDR_B 28 + #define Z_ENCDR_A_Q -1 // N/A + #define Z_ENCDR_B_Q -1 // N/A + + // UTM + #define UTM_C 63 // TOOL VERIFICATION + #define UTM_D 59 // SOIL SENSOR + #define UTM_E -1 + #define UTM_F -1 + #define UTM_G -1 + #define UTM_H -1 + #define UTM_I -1 + #define UTM_J -1 + #define UTM_K -1 + #define UTM_L -1 + // Available digital pins: 2,3,18,19,38,42,43,44,45,46,47,48,49 + // Available analog pins: 0,1,2,3,4,6,7,8 + + #define LED_PIN 13 + + // Peripherals + #define LIGHTING_PIN 7 + #define WATER_PIN 8 + #define VACUUM_PIN 9 + #define PERIPHERAL_4_PIN 10 + #define PERIPHERAL_5_PIN 12 + + // Auxiliary motors + #define AUX_STEP_PIN 40 + #define AUX_DIR_PIN 41 + #define AUX_ENABLE_PIN 37 + + #define SERVO_0_PIN 4 + #define SERVO_1_PIN 5 + #define SERVO_2_PIN 6 + #define SERVO_3_PIN 7 + + // Encoder X channel A: pin 16, port H1 + #define ENC_X_A_PORT PINH + #define ENC_X_A_BYTE 0x02 + + // Encoder X channel B: pin 17, port H0 + #define ENC_X_B_PORT PINH + #define ENC_X_B_BYTE 0x01 + + // Encoder X channel A Q (disabled, use LED pin): pin 13, port B7 + #define ENC_X_A_Q_PORT PINB + #define ENC_X_A_Q_BYTE 0x80 + + // Encoder X channel B Q (disabled, use LED pin): pin 13, port B7 + #define ENC_X_B_Q_PORT PINB + #define ENC_X_B_Q_BYTE 0x80 + + // Encoder Y channel A: pin 23, port A1 + #define ENC_Y_A_PORT PINA + #define ENC_Y_A_BYTE 0x02 + + // Encoder Y channel B: pin 24, port A2 + #define ENC_Y_B_PORT PINA + #define ENC_Y_B_BYTE 0x04 + + // Encoder Y channel A Q (disabled, use LED pin): pin 13, port B7 + #define ENC_Y_A_Q_PORT PINB + #define ENC_Y_A_Q_BYTE 0x80 + + // Encoder Y channel B Q (disabled, use LED pin): pin 13, port B7 + #define ENC_Y_B_Q_PORT PINB + #define ENC_Y_B_Q_BYTE 0x80 + + // Encoder Z channel A: pin 29, port A7 + #define ENC_Z_A_PORT PINA + #define ENC_Z_A_BYTE 0x80 + + // Encoder Z channel B: pin 28, port A6 + #define ENC_Z_B_PORT PINA + #define ENC_Z_B_BYTE 0x40 + + // Encoder Z channel A Q (disabled, use LED pin): pin 13, port B7 + #define ENC_Z_A_Q_PORT PINB + #define ENC_Z_A_Q_BYTE 0x80 + + // Encoder Z channel B Q (disabled, use LED pin): pin 13, port B7 + #define ENC_Z_B_Q_PORT PINB + #define ENC_Z_B_Q_BYTE 0x80 + +#endif diff --git a/src/src.ino b/src/src.ino index 6d044d8..d3a65b4 100644 --- a/src/src.ino +++ b/src/src.ino @@ -1 +1,49 @@ -// All code in farmbot_arduino_controller.cpp +/* + Name: src.ino + Created: 11/14/2019 9:51:10 PM + Author: Tim Evers +*/ + +#include "farmbot_arduino_controller.h" + +// the setup function runs once when you press reset or power the board +void setup() +{ + setPinInputOutput(); + startSerial(); + readParameters(); + +#if defined(FARMDUINO_EXP_V20) + loadTMC2130drivers(); + //loadTMC2130parameters(); + startupTmc(); +#endif + + loadMovementSetting(); + startMotor(); + startPinGuard(); + startServo(); + startInterrupt(); + initLastAction(); + homeOnBoot(); + + Serial.print(COMM_REPORT_COMMENT); + Serial.print(SPACE); + Serial.print("ARDUINO STARTUP COMPLETE"); + Serial.print(CRLF); + +} + +// the loop function runs over and over again until power down or reset +void loop() +{ + //runTestForDebug(); + + checkEncoders(); + checkPinGuard(); + checkSerialInputs(); + checkEmergencyStop(); + checkParamsChanged(); + periodicChecksAndReport(); + +} diff --git a/src/src.vcxproj b/src/src.vcxproj index 4c7fc52..e2fff09 100644 --- a/src/src.vcxproj +++ b/src/src.vcxproj @@ -47,10 +47,10 @@ Level3 Disabled true - $(ProjectDir)..\src;$(ProjectDir)..\..\..\..\AppData\Local\arduino15\packages\arduino\hardware\avr\1.6.19\libraries\SPI\src;$(ProjectDir)..\..\..\..\AppData\Local\arduino15\packages\arduino\hardware\avr\1.6.19\libraries\EEPROM\src;$(ProjectDir)..\..\..\..\..\..\Program Files (x86)\Arduino\libraries\Servo\src;$(ProjectDir)..\..\..\..\..\..\Program Files (x86)\Arduino\libraries;$(ProjectDir)..\..\..\..\AppData\Local\arduino15\packages\arduino\hardware\avr\1.6.19\libraries;$(ProjectDir)..\..\..\..\Documents\Arduino\libraries;$(ProjectDir)..\..\..\..\AppData\Local\arduino15\packages\arduino\hardware\avr\1.6.19\cores\arduino;$(ProjectDir)..\..\..\..\AppData\Local\arduino15\packages\arduino\hardware\avr\1.6.19\variants\mega;$(ProjectDir)..\..\..\..\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\4.9.2-atmel3.5.4-arduino2\avr\include\;$(ProjectDir)..\..\..\..\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\4.9.2-atmel3.5.4-arduino2\avr\include\avr\;$(ProjectDir)..\..\..\..\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\4.9.2-atmel3.5.4-arduino2\lib\gcc\avr\4.8.1\include;$(ProjectDir)..\..\..\..\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\4.9.2-atmel3.5.4-arduino2\lib\gcc\avr\4.9.2\include;$(ProjectDir)..\..\..\..\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\4.9.2-atmel3.5.4-arduino2\lib\gcc\avr\4.9.3\include;%(AdditionalIncludeDirectories) - $(ProjectDir)__vm\.src.vsarduino.h;%(ForcedIncludeFiles) + $(ProjectDir)..\src;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\hardware\avr\1.6.19\libraries\SPI\src;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\hardware\avr\1.6.19\libraries\EEPROM\src;$(ProjectDir)..\..\..\..\Program Files (x86)\Arduino\libraries\Servo\src;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\hardware\avr\1.6.19\cores\arduino;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\hardware\avr\1.6.19\variants\mega;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\4.9.2-atmel3.5.4-arduino2\avr\include;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\4.9.2-atmel3.5.4-arduino2\avr\include\avr;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\4.9.2-atmel3.5.4-arduino2\lib\gcc\avr\4.8.1\include;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\4.9.2-atmel3.5.4-arduino2\lib\gcc\avr\4.9.2\include;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\4.9.2-atmel3.5.4-arduino2\lib\gcc\avr\4.9.3\include;%(AdditionalIncludeDirectories) + $(ProjectDir)__vm\.farmbot-arduino-firmware.vsarduino.h;%(ForcedIncludeFiles) false - __AVR_ATmega2560__;_VMDEBUG=1;F_CPU=16000000L;ARDUINO=10802;ARDUINO_AVR_MEGA2560;ARDUINO_ARCH_AVR;__cplusplus=201103L;_VMICRO_INTELLISENSE;%(PreprocessorDefinitions) + __AVR_atmega2560__;__AVR_ATmega2560__;_VMDEBUG=1;F_CPU=16000000L;ARDUINO=10802;ARDUINO_AVR_MEGA2560;ARDUINO_ARCH_AVR;__cplusplus=201103L;_VMICRO_INTELLISENSE;%(PreprocessorDefinitions) true @@ -63,10 +63,10 @@ true true true - $(ProjectDir)..\..\..\..\AppData\Local\arduino15\packages\arduino\hardware\avr\1.6.19\libraries\SPI\src;$(ProjectDir)..\..\..\..\AppData\Local\arduino15\packages\arduino\hardware\avr\1.6.19\libraries\EEPROM\src;$(ProjectDir)..\..\..\..\..\..\Program Files (x86)\Arduino\libraries\Servo\src;$(ProjectDir)..\..\..\..\..\..\Program Files (x86)\Arduino\libraries;$(ProjectDir)..\..\..\..\AppData\Local\arduino15\packages\arduino\hardware\avr\1.6.19\libraries;$(ProjectDir)..\..\..\..\Documents\Arduino\libraries;$(ProjectDir)..\..\..\..\AppData\Local\arduino15\packages\arduino\hardware\avr\1.6.19\cores\arduino;$(ProjectDir)..\..\..\..\AppData\Local\arduino15\packages\arduino\hardware\avr\1.6.19\variants\mega;$(ProjectDir)..\src;$(ProjectDir)..\..\..\..\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\4.9.2-atmel3.5.4-arduino2\avr\include\;$(ProjectDir)..\..\..\..\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\4.9.2-atmel3.5.4-arduino2\avr\include\avr\;$(ProjectDir)..\..\..\..\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\4.9.2-atmel3.5.4-arduino2\lib\gcc\avr\4.8.1\include;%(AdditionalIncludeDirectories) - $(ProjectDir)__vm\.src.vsarduino.h;%(ForcedIncludeFiles) + $(ProjectDir)..\src;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\hardware\avr\1.6.19\libraries\SPI\src;$(ProjectDir)..\..\..\..\Users\Bro\Documents\Arduino\libraries\TMC2130Stepper\src;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\hardware\avr\1.6.19\libraries\EEPROM\src;$(ProjectDir)..\..\..\..\Program Files (x86)\Arduino\libraries\Servo\src;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\hardware\avr\1.6.19\cores\arduino;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\hardware\avr\1.6.19\variants\mega;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\4.9.2-atmel3.5.4-arduino2\avr\include;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\4.9.2-atmel3.5.4-arduino2\avr\include\avr;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\4.9.2-atmel3.5.4-arduino2\lib\gcc\avr\4.8.1\include;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\4.9.2-atmel3.5.4-arduino2\lib\gcc\avr\4.9.2\include;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\4.9.2-atmel3.5.4-arduino2\lib\gcc\avr\4.9.3\include;%(AdditionalIncludeDirectories) + $(ProjectDir)__vm\.farmbot-arduino-controller.vsarduino.h;%(ForcedIncludeFiles) false - __AVR_ATmega2560__;F_CPU=16000000L;ARDUINO=10802;ARDUINO_AVR_MEGA2560;ARDUINO_ARCH_AVR;__cplusplus=201103L;%(PreprocessorDefinitions) + __AVR_atmega2560__;__AVR_ATmega2560__;F_CPU=16000000L;ARDUINO=10802;ARDUINO_AVR_MEGA2560;ARDUINO_ARCH_AVR;__cplusplus=201103L;_VMICRO_INTELLISENSE;%(PreprocessorDefinitions) true @@ -115,16 +115,24 @@ + + + - - - + + + + + + + + @@ -160,6 +168,9 @@ + + + @@ -167,10 +178,14 @@ - - - + + + + + + + @@ -178,7 +193,7 @@ - + \ No newline at end of file diff --git a/src/src.vcxproj.filters b/src/src.vcxproj.filters index b98be88..272c0bf 100644 --- a/src/src.vcxproj.filters +++ b/src/src.vcxproj.filters @@ -117,22 +117,46 @@ Source Files - - Source Files - - - Source Files - Source Files Source Files - + Source Files - + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + Source Files @@ -245,21 +269,12 @@ Header Files - - Header Files - - - Header Files - Header Files Header Files - - Header Files - Header Files @@ -272,5 +287,35 @@ Header Files + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + \ No newline at end of file