diff --git a/src/Board.h b/src/Board.h index c1dd951..8231d57 100644 --- a/src/Board.h +++ b/src/Board.h @@ -1,9 +1,15 @@ #ifndef FARMBOT_BOARD_ID + // Farmbot using RAMPS board //#define RAMPS_V14 + //#define FARMDUINO_V10 //#define FARMDUINO_V14 + + // Farmbot Genesis 1.5 //#define FARMDUINO_V30 + + // Farmbot Express #define FARMDUINO_EXP_V20 #else diff --git a/src/Movement.cpp b/src/Movement.cpp index 0a9b688..f5f41e4 100644 --- a/src/Movement.cpp +++ b/src/Movement.cpp @@ -208,7 +208,7 @@ void Movement::loadSettings() } -#if defined(FARMDUINO_EXP_V20) +#if defined(FARMDUINO_EXP_V20) || defined(FARMDUINO_V30) void Movement::initTMC2130() { axisX.initTMC2130(); @@ -289,7 +289,7 @@ void Movement::loadSettings() void Movement::test() { - #if defined(FARMDUINO_EXP_V20) + #if defined(FARMDUINO_EXP_V20) || defined(FARMDUINO_V30) //axisX.enableMotor(); //axisX.setMotorStep(); //delayMicroseconds(500); @@ -747,18 +747,16 @@ int Movement::moveToCoords(double xDestScaled, double yDestScaled, double zDestS 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 + //#if defined(FARMDUINO_EXP_V20) || defined(FARMDUINO_V30) + //serialBuffer += "R89"; + //serialBuffer += " X"; + //serialBuffer += axisX.getLoad(); + //serialBuffer += " Y"; + //serialBuffer += axisY.getLoad(); + //serialBuffer += " Z"; + //serialBuffer += axisZ.getLoad(); + //serialBuffer += CurrentState::getInstance()->getQAndNewLine(); + //#endif break; default: @@ -767,14 +765,14 @@ int Movement::moveToCoords(double xDestScaled, double yDestScaled, double zDestS serialMessageNr++; - #if !defined(FARMDUINO_EXP_V20) + #if !defined(FARMDUINO_EXP_V20) && !defined(FARMDUINO_V30) if (serialMessageNr > 1) { serialMessageNr = 0; } #endif - #if defined(FARMDUINO_EXP_V20) + #if defined(FARMDUINO_EXP_V20) || defined(FARMDUINO_V30) if (serialMessageNr > 2) { @@ -1400,27 +1398,6 @@ void Movement::checkAxisVsEncoder(MovementAxis *axis, MovementEncoder *encoder, #if defined(FARMDUINO_EXP_V20) void Movement::checkAxisVsEncoder(MovementAxis *axis, MovementEncoder *encoder, float *missedSteps, long *lastPosition, long *encoderLastPosition, int *encoderUseForPos, float *encoderStepDecay, bool *encoderEnabled) { - - // - // /**/ - // /* - // Serial.print("R99"); - // Serial.print(" XXX "); - // Serial.print(" cur pos "); - // Serial.print(axis->currentPosition()); - // Serial.print(" last pos "); - // Serial.print(*lastPosition); - // */ - // - - //if (axis->stallDetected()) { - // Serial.print("X"); - //} - //else - //{ - // Serial.print("."); - //} - /**/ bool stallGuard = false; bool standStill = false; @@ -1428,28 +1405,15 @@ void Movement::checkAxisVsEncoder(MovementAxis *axis, MovementEncoder *encoder, if (*encoderEnabled) { - //TMC2130X.read_STAT(); - status = axis->getStatus(); stallGuard = status & FB_TMC_SPISTATUS_STALLGUARD_MASK ? true : false; standStill = status & FB_TMC_SPISTATUS_STANDSTILL_MASK ? true : false; if (stallGuard || standStill) { - //Serial.print("."); // In case of stall detection, count this as a missed step (*missedSteps)++; axis->setCurrentPosition(*lastPosition); - - - //if (int(*missedSteps) % 10 == 0) { - // Serial.println(); - // Serial.print(*missedSteps); - // Serial.print("/"); - // Serial.print(axis->currentPosition()); - // Serial.println(); - //} - } else { // Decrease amount of missed steps if there are no missed step @@ -1570,7 +1534,7 @@ void Movement::loadMotorSettings() /**/ /* -#if defined(FARMDUINO_EXP_V20) +#if defined(FARMDUINO_EXP_V20) || defined(FARMDUINO_V30) loadSettingsTMC2130(); #endif */ @@ -1722,7 +1686,7 @@ bool Movement::endStopsReached() void Movement::storePosition() { -#if !defined(FARMDUINO_EXP_V20) +#if !defined(FARMDUINO_EXP_V20) && !defined(FARMDUINO_V30) if (motorConsEncoderEnabled[0]) { CurrentState::getInstance()->setX(encoderX.currentPosition()); @@ -1751,7 +1715,7 @@ void Movement::storePosition() } #endif -#if defined(FARMDUINO_EXP_V20) +#if defined(FARMDUINO_EXP_V20) || defined(FARMDUINO_V30) CurrentState::getInstance()->setX(axisX.currentPosition()); CurrentState::getInstance()->setY(axisY.currentPosition()); CurrentState::getInstance()->setZ(axisZ.currentPosition()); diff --git a/src/Movement.h b/src/Movement.h index 7c93228..b620b24 100644 --- a/src/Movement.h +++ b/src/Movement.h @@ -37,7 +37,7 @@ class Movement void handleMovementInterrupt(); void checkEncoders(); - #if defined(FARMDUINO_EXP_V20) + #if defined(FARMDUINO_EXP_V20) || defined(FARMDUINO_V30) void initTMC2130(); void loadSettingsTMC2130(); #endif diff --git a/src/MovementAxis.cpp b/src/MovementAxis.cpp index 32807a4..b20adb9 100644 --- a/src/MovementAxis.cpp +++ b/src/MovementAxis.cpp @@ -71,7 +71,7 @@ void MovementAxis::test() } -#if defined(FARMDUINO_EXP_V20) +#if defined(FARMDUINO_EXP_V20) || defined(FARMDUINO_V30) unsigned int MovementAxis::getLostSteps() { @@ -611,7 +611,7 @@ void MovementAxis::setDirectionUp() //#endif /* -#if defined(FARMDUINO_EXP_V20) +#if defined(FARMDUINO_EXP_V20) || defined(FARMDUINO_V30) // The TMC2130 uses a command to change direction, not a pin if (motorMotorInv) @@ -660,7 +660,7 @@ void MovementAxis::setDirectionDown() //#endif /* - #if defined(FARMDUINO_EXP_V20) + #if defined(FARMDUINO_EXP_V20) || defined(FARMDUINO_V30) // The TMC2130 uses a command to change direction, not a pin if (motorMotorInv) @@ -941,7 +941,7 @@ void MovementAxis::resetMotorStepWrite46() PORTL &= B11110111; } -#if defined(FARMDUINO_EXP_V20) +#if defined(FARMDUINO_EXP_V20) || defined(FARMDUINO_V30) //// TMC2130 Functions void MovementAxis::setMotorStepWriteTMC2130() diff --git a/src/MovementAxis.h b/src/MovementAxis.h index bb59b59..20362a4 100644 --- a/src/MovementAxis.h +++ b/src/MovementAxis.h @@ -29,7 +29,7 @@ class MovementAxis public: MovementAxis(); -#if defined(FARMDUINO_EXP_V20) +#if defined(FARMDUINO_EXP_V20) || defined(FARMDUINO_V30) TMC2130_Basics *TMC2130A; TMC2130_Basics *TMC2130B; #endif @@ -92,14 +92,14 @@ class MovementAxis char channelLabel; bool movementStarted; -#if defined(FARMDUINO_EXP_V20) +#if defined(FARMDUINO_EXP_V20) || defined(FARMDUINO_V30) void initTMC2130(); void loadSettingsTMC2130(int motorCurrent, int stallSensitivity, int microSteps); uint16_t getLoad(); #endif -#if defined(FARMDUINO_EXP_V20) +#if defined(FARMDUINO_EXP_V20) || defined(FARMDUINO_V30) void setMotorStepWriteTMC2130(); void setMotorStepWriteTMC2130_2(); void resetMotorStepWriteTMC2130(); diff --git a/src/MovementEncoder.cpp b/src/MovementEncoder.cpp index c8f776f..c65a9f7 100644 --- a/src/MovementEncoder.cpp +++ b/src/MovementEncoder.cpp @@ -76,7 +76,7 @@ void MovementEncoder::loadMdlEncoderId(MdlSpiEncoders encoder) void MovementEncoder::setPosition(long newPosition) { - #if defined(RAMPS_V14) || defined(FARMDUINO_V10) + #if defined(RAMPS_V14) || defined(FARMDUINO_V10) || defined(FARMDUINO_EXP_V20) || defined(FARMDUINO_V30) position = newPosition; #endif @@ -277,7 +277,7 @@ float MovementEncoder::getMissedSteps() void MovementEncoder::checkMissedSteps() { - #if !defined(FARMDUINO_EXP_V20) + #if !defined(FARMDUINO_EXP_V20) && !defined(FARMDUINO_V30) if (encoderEnabled) { bool stepMissed = false; @@ -307,7 +307,7 @@ void MovementEncoder::checkMissedSteps() #endif /* - #if defined(FARMDUINO_EXP_V20) + #if defined(FARMDUINO_EXP_V20) || defined(FARMDUINO_V30) if (encoderEnabled) { if (axis->stallDetected()) { diff --git a/src/TMC2130.cpp b/src/TMC2130.cpp index b7e7d49..bb6ab11 100644 --- a/src/TMC2130.cpp +++ b/src/TMC2130.cpp @@ -8,7 +8,7 @@ #include "TMC2130.h" -#if defined(FARMDUINO_EXP_V20) +#if defined(FARMDUINO_EXP_V20) || defined(FARMDUINO_V30) //void loadTMC2130ParametersMotor(Trinamic_TMC2130 *myStepper, int microsteps, int current, int sensitivity) void loadTMC2130ParametersMotor(TMC2130_Basics *tb, int microsteps, int current, int sensitivity) diff --git a/src/TMC2130.h b/src/TMC2130.h index 35713b5..2ca6967 100644 --- a/src/TMC2130.h +++ b/src/TMC2130.h @@ -14,7 +14,7 @@ #include "pins.h" #include "Board.h" -#if defined(FARMDUINO_EXP_V20) +#if defined(FARMDUINO_EXP_V20) || defined(FARMDUINO_V30) #include "TMC2130_Basics.h" static TMC2130_Basics TMC2130X(X_CHIP_SELECT); diff --git a/src/farmbot_arduino_controller.cpp b/src/farmbot_arduino_controller.cpp index 3abaa2b..9677c04 100644 --- a/src/farmbot_arduino_controller.cpp +++ b/src/farmbot_arduino_controller.cpp @@ -2,7 +2,7 @@ #include "farmbot_arduino_controller.h" /**/ -//#if !defined(FARMDUINO_EXP_V20) +//#if !defined(FARMDUINO_EXP_V20) || defined(FARMDUINO_V30) #include "TimerOne.h" //#endif @@ -52,7 +52,7 @@ unsigned long interruptDurationMax = 0; bool interruptBusy = false; int interruptSecondTimer = 0; -//#if !defined(FARMDUINO_EXP_V20) +//#if !defined(FARMDUINO_EXP_V20) || defined(FARMDUINO_V30) void interrupt(void) { if (!debugInterrupt) @@ -284,7 +284,7 @@ void checkParamsChanged() } - #if defined(FARMDUINO_EXP_V20) + #if defined(FARMDUINO_EXP_V20) || defined(FARMDUINO_V30) Movement::getInstance()->loadSettingsTMC2130(); #endif @@ -503,7 +503,7 @@ void setPinInputOutput() } #endif -#if defined(FARMDUINO_EXP_V20) +#if defined(FARMDUINO_EXP_V20) || defined(FARMDUINO_V30) void setPinInputOutput() { Serial.print(COMM_REPORT_COMMENT); @@ -621,13 +621,13 @@ void startInterrupt() // Interrupt management code library written by Paul Stoffregen // The default time 100 micro seconds - //#if !defined(FARMDUINO_EXP_V20) + //#if !defined(FARMDUINO_EXP_V20) || defined(FARMDUINO_V30) Timer1.attachInterrupt(interrupt); Timer1.initialize(MOVEMENT_INTERRUPT_SPEED); Timer1.start(); //#endif - //#if defined(FARMDUINO_EXP_V20) + //#if defined(FARMDUINO_EXP_V20) || defined(FARMDUINO_V30) // Serial.println("set timer"); // TIMSK2 = (TIMSK2 & B11111110) | 0x01; // Enable timer overflow // TCCR2B = (TCCR2B & B11111000) | 0x01; // Set divider to 1 @@ -735,7 +735,7 @@ void startServo() ServoControl::getInstance()->attach(); } -#if defined(FARMDUINO_EXP_V20) +#if defined(FARMDUINO_EXP_V20) || defined(FARMDUINO_V30) /**/ //static Trinamic_TMC2130 controllerTMC2130_X(X_CHIP_SELECT); diff --git a/src/farmbot_arduino_controller.h b/src/farmbot_arduino_controller.h index f1b8b34..ca71b34 100644 --- a/src/farmbot_arduino_controller.h +++ b/src/farmbot_arduino_controller.h @@ -16,7 +16,7 @@ #include "MemoryFree.h" #include "Debug.h" -#if defined(FARMDUINO_EXP_V20) +#if defined(FARMDUINO_EXP_V20) || defined(FARMDUINO_V30) #include "TMC2130.h" #endif @@ -49,7 +49,7 @@ void setup(); void setPinInputOutput(); void startSerial(); - #if defined(FARMDUINO_EXP_V20) + #if defined(FARMDUINO_EXP_V20) || defined(FARMDUINO_V30) void loadTMC2130drivers(); void loadTMC2130parameters(); void startupTmc(); diff --git a/src/pins.h b/src/pins.h index bb9d4df..9a2d0c8 100644 --- a/src/pins.h +++ b/src/pins.h @@ -257,7 +257,7 @@ #endif -#if defined(FARMDUINO_EXP_V20) +#if defined(FARMDUINO_EXP_V20) || defined(FARMDUINO_V30) // X1-AXIS #define X_STEP_PIN 26 // X1_STEP_PIN diff --git a/src/src.ino b/src/src.ino index 06b08c9..fefc0a3 100644 --- a/src/src.ino +++ b/src/src.ino @@ -19,7 +19,7 @@ void setup() readParameters(); -#if defined(FARMDUINO_EXP_V20) +#if defined(FARMDUINO_EXP_V20) || defined(FARMDUINO_V30) loadTMC2130drivers(); startupTmc(); loadTMC2130parameters();