From 764e55052a85281c0184bc6cb652f921deb059db Mon Sep 17 00:00:00 2001 From: Tim Evers Date: Wed, 29 Nov 2017 22:50:59 +0100 Subject: [PATCH 1/2] Fix for pin guard, fix for axis length parameter --- src/ParameterList.cpp | 45 +++++++++++++++++++++++------- src/ParameterList.h | 10 +++++-- src/farmbot_arduino_controller.cpp | 41 +++++++++++++++++++-------- 3 files changed, 72 insertions(+), 24 deletions(-) diff --git a/src/ParameterList.cpp b/src/ParameterList.cpp index dd2837e..32b658a 100644 --- a/src/ParameterList.cpp +++ b/src/ParameterList.cpp @@ -148,35 +148,60 @@ int ParameterList::paramChangeNumber() // ===== eeprom handling ==== -int ParameterList::readValueEeprom(int id) +long ParameterList::readValueEeprom(int id) { // Assume all values are ints and calculate address for that int address = id * 2; //Read the 2 bytes from the eeprom memory. - long two = EEPROM.read(address); - long one = EEPROM.read(address + 1); + long one = EEPROM.read(address + 0); + long two = EEPROM.read(address + 1); + + long three = 0; + long four = 0; + + if (id == 141 || id == 142 || id == 143) + { + three = EEPROM.read(address + 20); + four = EEPROM.read(address + 21); + } + + // just in case there are non uninitialized EEPROM bytes + // put them both to zero + if (three == -1 && four == -1) + { + three = 0; + four = 0; + } //Return the recomposed long by using bitshift. - return ((two << 0) & 0xFF) + ((one << 8) & 0xFFFF); + return ((one << 0) & 0xFF) + ((two << 8) & 0xFF00) + ((three << 16) & 0xFF0000); +((four << 24) & 0xFF000000); } -int ParameterList::writeValueEeprom(int id, int value) +int ParameterList::writeValueEeprom(int id, long value) { // Assume all values are ints and calculate address for that int address = id * 2; //Decomposition from a int to 2 bytes by using bitshift. - //One = Most significant -> Two = Least significant byte - byte two = (value & 0xFF); - byte one = ((value >> 8) & 0xFF); + //One = Least significant -> Four = Most significant byte + byte one= (value & 0xFF); + byte two = ((value >> 8) & 0xFF); + byte three = ((value >> 16) & 0xFF); + byte four = ((value >> 24) & 0xFF); //Write the 4 bytes into the eeprom memory. - EEPROM.write(address, two); - EEPROM.write(address + 1, one); + EEPROM.write(address + 0, one); + EEPROM.write(address + 1, two); + // Only this parameter needs a long value + if (id == 141 || id == 142 || id == 143) + { + EEPROM.write(address + 20, one); + EEPROM.write(address + 21, two); + } return 0; } diff --git a/src/ParameterList.h b/src/ParameterList.h index 635b84e..bd7b4d9 100644 --- a/src/ParameterList.h +++ b/src/ParameterList.h @@ -105,11 +105,15 @@ enum ParamListEnum ENCODER_INVERT_Y = 132, ENCODER_INVERT_Z = 133, - // sizes of axis + // sizes of axis, lower byte and higher byte MOVEMENT_AXIS_NR_STEPS_X = 141, MOVEMENT_AXIS_NR_STEPS_Y = 142, MOVEMENT_AXIS_NR_STEPS_Z = 143, + MOVEMENT_AXIS_NR_STEPS_H_X = 151,/**/ + MOVEMENT_AXIS_NR_STEPS_H_Y = 152, + MOVEMENT_AXIS_NR_STEPS_H_Z = 153, + // stop at end of axis MOVEMENT_STOP_AT_MAX_X = 145, MOVEMENT_STOP_AT_MAX_Y = 146, @@ -160,8 +164,8 @@ public: int writeAllValuesToEeprom(); int setAllValuesToDefault(); - int readValueEeprom(int id); - int writeValueEeprom(int id, int value); + long readValueEeprom(int id); + int writeValueEeprom(int id, long value); void sendConfigToModules(); diff --git a/src/farmbot_arduino_controller.cpp b/src/farmbot_arduino_controller.cpp index d937425..c2eec65 100644 --- a/src/farmbot_arduino_controller.cpp +++ b/src/farmbot_arduino_controller.cpp @@ -18,6 +18,9 @@ unsigned long currentTime; unsigned long cycleCounter = 0; bool previousEmergencyStop = false; +unsigned long pinGuardLastCheck; +unsigned long pinGuardCurrentTime; + int lastParamChangeNr = 0; String commandString = ""; @@ -50,11 +53,11 @@ void interrupt(void) { if (!debugInterrupt) { - interruptSecondTimer++; + //interruptSecondTimer++; if (interruptBusy == false) { - interruptStartTime = micros(); + //interruptStartTime = micros(); interruptBusy = true; StepperControl::getInstance()->handleMovementInterrupt(); @@ -67,17 +70,17 @@ void interrupt(void) // //blinkLed(); //} - interruptStopTime = micros(); + //interruptStopTime = micros(); - if (interruptStopTime > interruptStartTime) - { - interruptDuration = interruptStopTime - interruptStartTime; - } + //if (interruptStopTime > interruptStartTime) + //{ + // interruptDuration = interruptStopTime - interruptStartTime; + //} - if (interruptDuration > interruptDurationMax) - { - interruptDurationMax = interruptDuration; - } + //if (interruptDuration > interruptDurationMax) + //{ + // interruptDurationMax = interruptDuration; + //} interruptBusy = false; } @@ -252,6 +255,7 @@ void setup() // 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 @@ -300,6 +304,21 @@ void loop() if (Serial.available()) { + unsigned long pinGuardLastCheck; + pinGuardCurrentTime = millis(); + if (pinGuardCurrentTime < pinGuardLastCheck) + { + pinGuardLastCheck = pinGuardCurrentTime; + } + else + { + if (pinGuardLastCheck - pinGuardCurrentTime >= 999) + { + // check the pins for timeouts + PinGuard::getInstance()->checkPins(); + } + } + // Save current time stamp for timeout actions lastAction = millis(); From b35914a325e46e0d28de19875b4635e4ddcb4f81 Mon Sep 17 00:00:00 2001 From: Tim Evers Date: Sun, 10 Dec 2017 22:01:13 +0100 Subject: [PATCH 2/2] Debugging pin guard --- src/CurrentState.cpp | 2 +- src/CurrentState.h | 8 +++---- src/StepperControl.cpp | 2 +- src/farmbot_arduino_controller.cpp | 37 +++++++++++++++++------------- src/pins.h | 3 +++ src/src.vcxproj | 2 +- 6 files changed, 31 insertions(+), 23 deletions(-) diff --git a/src/CurrentState.cpp b/src/CurrentState.cpp index b04602a..9c26735 100644 --- a/src/CurrentState.cpp +++ b/src/CurrentState.cpp @@ -86,7 +86,7 @@ void CurrentState::setEndStopState(unsigned int axis, unsigned int position, boo endStopState[axis][position] = state; } -void CurrentState::setStepsPerMm(int stepsX, int stepsY, int stepsZ) +void CurrentState::setStepsPerMm(long stepsX, long stepsY, long stepsZ) { stepsPerMmX = stepsX; stepsPerMmY = stepsY; diff --git a/src/CurrentState.h b/src/CurrentState.h index 84455bc..018e5fb 100644 --- a/src/CurrentState.h +++ b/src/CurrentState.h @@ -43,16 +43,16 @@ public: void resetEmergencyStop(); bool isEmergencyStop(); - void setStepsPerMm(int stepsX, int stepsY, int stepsZ); + void setStepsPerMm(long stepsX, long stepsY, long stepsZ); private: CurrentState(); CurrentState(CurrentState const &); void operator=(CurrentState const &); - int stepsPerMmX; - int stepsPerMmY; - int stepsPerMmZ; + long stepsPerMmX; + long stepsPerMmY; + long stepsPerMmZ; bool emergencyStop = false; }; diff --git a/src/StepperControl.cpp b/src/StepperControl.cpp index bcd3a0b..4f695ab 100644 --- a/src/StepperControl.cpp +++ b/src/StepperControl.cpp @@ -826,7 +826,7 @@ int StepperControl::calibrateAxis(int axis) encoderEnabled = &motorConsEncoderEnabled[2]; axisStatus = &axisSubStep[2]; //axisStepsPerMm = &axisStepsPerMm[2]; - axisStepsPerMm = &stepsPerMm[1]; + axisStepsPerMm = &stepsPerMm[2]; break; default: Serial.print("R99 Calibration error: invalid axis selected\r\n"); diff --git a/src/farmbot_arduino_controller.cpp b/src/farmbot_arduino_controller.cpp index c2eec65..8678511 100644 --- a/src/farmbot_arduino_controller.cpp +++ b/src/farmbot_arduino_controller.cpp @@ -173,6 +173,7 @@ void setup() 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); @@ -222,7 +223,7 @@ void setup() pinMode(UTM_E, INPUT_PULLUP); pinMode(UTM_F, INPUT_PULLUP); pinMode(UTM_G, INPUT_PULLUP); - pinMode(UTM_H, INPUT_PULLUP); + if (UTM_H > 0) { pinMode(UTM_H, INPUT_PULLUP) }; pinMode(UTM_I, INPUT_PULLUP); pinMode(UTM_J, INPUT_PULLUP); pinMode(UTM_K, INPUT_PULLUP); @@ -268,6 +269,9 @@ void setup() // Initialize the inactivity check lastAction = millis(); + pinGuardCurrentTime = millis(); + pinGuardLastCheck = millis(); + if (ParameterList::getInstance()->getValue(MOVEMENT_HOME_AT_BOOT_Z) == 1) { Serial.print("R99 HOME Z ON STARTUP\r\n"); @@ -301,24 +305,25 @@ void loop() StepperControl::getInstance()->handleMovementInterrupt(); } + pinGuardCurrentTime = millis(); + if (pinGuardCurrentTime < pinGuardLastCheck) + { + pinGuardLastCheck = pinGuardCurrentTime; + } + else + { + if (pinGuardCurrentTime - pinGuardLastCheck >= 1000) + { + pinGuardLastCheck += 1000; + + // check the pins for timeouts + PinGuard::getInstance()->checkPins(); + } + } + if (Serial.available()) { - unsigned long pinGuardLastCheck; - pinGuardCurrentTime = millis(); - if (pinGuardCurrentTime < pinGuardLastCheck) - { - pinGuardLastCheck = pinGuardCurrentTime; - } - else - { - if (pinGuardLastCheck - pinGuardCurrentTime >= 999) - { - // check the pins for timeouts - PinGuard::getInstance()->checkPins(); - } - } - // Save current time stamp for timeout actions lastAction = millis(); diff --git a/src/pins.h b/src/pins.h index f303ce4..fbad230 100644 --- a/src/pins.h +++ b/src/pins.h @@ -134,13 +134,16 @@ #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 E_STEP_PIN 15 // X2_STEP_PIN #define E_DIR_PIN 30 // X2_DIR_PIN #define E_ENABLE_PIN 14 // X2_ENABLE_PIN + #define X_MIN_PIN 69 #define X_MAX_PIN 68 #define X_ENCDR_A 16 #define X_ENCDR_B 17 + #define X2_ENCDR_A 22 #define X2_ENCDR_B 39 #define X_ENCDR_A_Q -1 // N/A diff --git a/src/src.vcxproj b/src/src.vcxproj index 90ad932..eb7dc1f 100644 --- a/src/src.vcxproj +++ b/src/src.vcxproj @@ -174,7 +174,7 @@ - + \ No newline at end of file