bump version (6.5.24)

Calibration changes

.circleci/config.yml

@@ -0,0 +1,71 @@
+version: 2.1
+
+jobs:
+  build:
+    machine: true
+    parameters:
+      arduino_version:
+        description: "Arduino version"
+        default: "1.8.12"
+        type: string
+    steps:
+      - checkout
+      - restore_cache:
+          keys:
+            - arduino-<< parameters.arduino_version >>
+      - run:
+          name: Install Arduino
+          command: |
+            if [ ! -d "$HOME/arduino-<< parameters.arduino_version >>" ]
+            then
+              wget https://downloads.arduino.cc/arduino-<< parameters.arduino_version >>-linux64.tar.xz
+              tar xf arduino-<< parameters.arduino_version >>-linux64.tar.xz -C $HOME
+            fi
+      - save_cache:
+          key: arduino-<< parameters.arduino_version >>
+          paths:
+            - ~/arduino-<< parameters.arduino_version >>
+      - run:
+          name: Create bin directory
+          command: mkdir -p bin
+      - run:
+          name: Compile Genesis v1.2 (RAMPS_V14)
+          command: make dep_core dep_Servo dep_SPI dep_EEPROM target_ramps_v14 remove_temp
+          when: always
+      - run:
+          name: Compile Genesis v1.3 (FARMDUINO_V10)
+          command: make dep_core dep_Servo dep_SPI dep_EEPROM target_farmduino_v10 remove_temp
+          when: always
+      - run:
+          name: Compile Genesis v1.4 (FARMDUINO_V14)
+          command: make dep_core dep_Servo dep_SPI dep_EEPROM target_farmduino_k14 remove_temp
+          when: always
+      - run:
+          name: Compile Genesis v1.5 (FARMDUINO_V30)
+          command: make dep_core dep_Servo dep_SPI dep_EEPROM target_farmduino_k15 remove_temp
+          when: always
+      - run:
+          name: Compile Express v1.0 (FARMDUINO_EXP_V20)
+          command: make dep_core dep_Servo dep_SPI dep_EEPROM target_express_k10 remove_temp
+          when: always
+      - run:
+          name: Compile all
+          command: |
+            make force_clean
+            make
+      - run:
+          name: Check strings
+          command: make strings_test
+      - run:
+          name: Check software version
+          command: |
+            export SOFTWARE_VERSION=$(cat src/Config.h | grep -w SOFTWARE_VERSION | cut -d'"' -f 2 | cut -d'\' -f 1)
+            strings bin/arduino_firmware.hex.bin | grep $SOFTWARE_VERSION
+            export SHORT_COMMIT_SHA=$(echo $CIRCLE_SHA1 | cut -c1-8)
+            export COMMIT_SUFFIX=$(strings bin/arduino_firmware.hex.bin | grep $SHORT_COMMIT_SHA)
+            export NEW_FW_DIR=bin/$SOFTWARE_VERSION$COMMIT_SUFFIX
+            mkdir $NEW_FW_DIR
+            rm bin/*.bin
+            mv bin/*.hex $NEW_FW_DIR
+      - store_artifacts:
+          path: bin

.gitattributes

@@ -0,0 +1,63 @@
+###############################################################################
+# Set default behavior to automatically normalize line endings.
+###############################################################################
+* text=auto
+
+###############################################################################
+# Set default behavior for command prompt diff.
+#
+# This is need for earlier builds of msysgit that does not have it on by
+# default for csharp files.
+# Note: This is only used by command line
+###############################################################################
+#*.cs     diff=csharp
+
+###############################################################################
+# Set the merge driver for project and solution files
+#
+# Merging from the command prompt will add diff markers to the files if there
+# are conflicts (Merging from VS is not affected by the settings below, in VS
+# the diff markers are never inserted). Diff markers may cause the following 
+# file extensions to fail to load in VS. An alternative would be to treat
+# these files as binary and thus will always conflict and require user
+# intervention with every merge. To do so, just uncomment the entries below
+###############################################################################
+#*.sln       merge=binary
+#*.csproj    merge=binary
+#*.vbproj    merge=binary
+#*.vcxproj   merge=binary
+#*.vcproj    merge=binary
+#*.dbproj    merge=binary
+#*.fsproj    merge=binary
+#*.lsproj    merge=binary
+#*.wixproj   merge=binary
+#*.modelproj merge=binary
+#*.sqlproj   merge=binary
+#*.wwaproj   merge=binary
+
+###############################################################################
+# behavior for image files
+#
+# image files are treated as binary by default.
+###############################################################################
+#*.jpg   binary
+#*.png   binary
+#*.gif   binary
+
+###############################################################################
+# diff behavior for common document formats
+# 
+# Convert binary document formats to text before diffing them. This feature
+# is only available from the command line. Turn it on by uncommenting the 
+# entries below.
+###############################################################################
+#*.doc   diff=astextplain
+#*.DOC   diff=astextplain
+#*.docx  diff=astextplain
+#*.DOCX  diff=astextplain
+#*.dot   diff=astextplain
+#*.DOT   diff=astextplain
+#*.pdf   diff=astextplain
+#*.PDF   diff=astextplain
+#*.rtf   diff=astextplain
+#*.RTF   diff=astextplain

.gitignore

@@ -3,6 +3,7 @@
 *.lo
 *.o
 *.obj
+*.d
 
 # Compiled Dynamic libraries
 *.so
@@ -23,3 +24,23 @@
 /spec.d
 /arduino
 /.build
+
+
+# Visual Studio files
+/.vs
+/src/Debug
+/src/Release
+/src/__vm
+
+# Setting files
+Debug.h
+/__vm
+.vscode
+
+# Build artifacts
+_build
+bin
+src.ino.cpp
+src.ino.mega.hex
+src.ino.with_bootloader.mega.hex
+src/CommitSHA.h

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"
+    }
+  ]
+}

LICENSE

@@ -18,4 +18,4 @@ FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-SOFTWARE.
+SOFTWARE.

Makefile

@@ -0,0 +1,89 @@
+rwildcard=$(foreach d,$(wildcard $1*),$(call rwildcard,$d/,$2)$(filter $(subst *,%,$2),$d))
+
+BUILD_DIR ?= $(shell pwd)/_build
+BIN_DIR ?= $(shell pwd)/bin
+FBARDUINO_FIRMWARE_SRC_DIR ?= src
+FBARDUINO_FIRMWARE_BUILD_DIR ?= $(BUILD_DIR)/sketch
+FBARDUINO_FIRMWARE_LIB_BUILD_DIR ?= $(BUILD_DIR)/libraries
+
+ARDUINO_INSTALL_DIR ?= $(HOME)/arduino-1.8.12
+
+# Get current commit SHA
+COMMIT_SHA := $(shell git -C $(FBARDUINO_FIRMWARE_SRC_DIR)/.. rev-parse --short=8 HEAD)
+MODIFIER := $(shell echo $$(if [ -z "$$(git -C $(FBARDUINO_FIRMWARE_SRC_DIR)/.. status -s -uall)" ];then echo "";else echo "+";fi))
+CREATE_COMMIT_SHA_H := $(shell echo "\#ifndef COMMIT_SHA_H_\n\#define COMMIT_SHA_H_\n\#define SOFTWARE_COMMIT \"-$(COMMIT_SHA)$(MODIFIER)\"\n\#endif" > $(FBARDUINO_FIRMWARE_SRC_DIR)/CommitSHA.h)
+
+# Files to be tracked for make to know to rebuild.
+COPY_INO := $(shell cp $(FBARDUINO_FIRMWARE_SRC_DIR)/src.ino $(FBARDUINO_FIRMWARE_SRC_DIR)/src.ino.cpp)
+CXX_SRC := $(wildcard $(FBARDUINO_FIRMWARE_SRC_DIR)/*.cpp)
+SRC := $(CXX_SRC)
+HEADERS := $(wildcard $(FBARDUINO_FIRMWARE_SRC_DIR)/*.h)
+
+# Object files and Dependency files That will eventually be built.
+CXX_OBJ := $(CXX_SRC:.cpp=.o)
+
+## Commands needed to compile and whatnot.
+CXX := $(ARDUINO_INSTALL_DIR)/hardware/tools/avr/bin/avr-g++
+CC := $(ARDUINO_INSTALL_DIR)/hardware/tools/avr/bin/avr-gcc
+AR := $(ARDUINO_INSTALL_DIR)/hardware/tools/avr/bin/avr-gcc-ar
+OBJ_COPY := $(ARDUINO_INSTALL_DIR)/hardware/tools/avr/bin/avr-objcopy
+MKDIR_P := mkdir -p
+
+CXX_FLAGS := -c -g -Os -w -std=gnu++11 -fpermissive -fno-exceptions -ffunction-sections -fdata-sections -fno-threadsafe-statics -MMD -flto -mmcu=atmega2560 -DF_CPU=16000000L -DARDUINO=10600 -DARDUINO_AVR_MEGA2560 -DARDUINO_ARCH_AVR
+CFLAGS := -w -Os -g -flto -fuse-linker-plugin -Wl,--gc-sections,--relax -mmcu=atmega2560
+
+.DEFAULT_GOAL := all
+
+## Dependencies
+include lib/core.Makefile
+include lib/SPI.Makefile
+include lib/Servo.Makefile
+include lib/EEPROM.Makefile
+
+# Targets
+include lib/targets/ramps_v14.Makefile
+include lib/targets/farmduino_v10.Makefile
+include lib/targets/farmduino_k14.Makefile
+include lib/targets/farmduino_k15.Makefile
+include lib/targets/express_k10.Makefile
+
+.PHONY: all clean strings_test force_clean remove_temp \
+	dep_core dep_core_clean \
+	dep_Servo dep_Servo_clean \
+	dep_SPI dep_SPI_clean \
+	dep_EEPROM dep_EEPROM_clean \
+	target_ramps_v14 target_ramps_v14_clean \
+	target_farmduino_v10 target_farmduino_v10_clean \
+	target_farmduino_k14 target_farmduino_k14_clean \
+	target_farmduino_k15 target_farmduino_k15_clean \
+	target_express_k10 target_express_k10_clean
+
+DEPS := $(DEP_CORE) $(DEP_SPI) $(DEP_Servo) $(DEP_EEPROM)
+DEPS_OBJ := $(DEP_SPI_OBJ) $(DEP_Servo_OBJ) $(DEP_EEPROM_OBJ)
+DEPS_CFLAGS := $(DEP_CORE_CFLAGS) $(DEP_SPI_CFLAGS) $(DEP_Servo_CFLAGS) $(DEP_EEPROM_CFLAGS)
+
+all: $(BIN_DIR) $(DEPS) target_ramps_v14 target_farmduino_v10 target_farmduino_k14 target_farmduino_k15 target_express_k10 remove_temp
+
+clean: remove_temp target_ramps_v14_clean target_farmduino_v10_clean target_farmduino_k14_clean target_farmduino_k15_clean target_express_k10_clean
+
+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
+	$(OBJ_COPY) -I ihex $(TARGET_farmduino_k15_HEX) -O binary $(TARGET_farmduino_k15_HEX).bin
+	$(OBJ_COPY) -I ihex $(TARGET_express_k10_HEX) -O binary $(TARGET_express_k10_HEX).bin
+	@strings $(TARGET_ramps_v14_HEX).bin | grep -q ".R.genesisK12"
+	@strings $(TARGET_farmduino_v10_HEX).bin | grep -q ".F.genesisK13"
+	@strings $(TARGET_farmduino_k14_HEX).bin | grep -q ".G.genesisK14"
+	@strings $(TARGET_farmduino_k15_HEX).bin | grep -q ".H.genesisK15"
+	@strings $(TARGET_express_k10_HEX).bin | grep -q ".E.expressK10"
+
+force_clean: remove_temp
+	$(RM) -r $(BUILD_DIR) $(BIN_DIR)
+
+$(BIN_DIR):
+	$(MKDIR_P) $(BIN_DIR)
+
+remove_temp:
+	$(RM) $(FBARDUINO_FIRMWARE_SRC_DIR)/src.ino.cpp
+	$(RM) $(FBARDUINO_FIRMWARE_SRC_DIR)/CommitSHA.h

README.md

@@ -1,29 +1,18 @@
-farmbot-arduino-controller
+farmbot-arduino-firmware
 ==========================
-This software is responsible for receiving G-Codes from the Raspberry Pi, execute them and report back the results.
+This software is responsible for receiving G-Codes from the Raspberry Pi, executing them, and reporting back the results.
 
 Technicals
 ==========================
 Created with eclipseArduino V2 - For more details see http://www.baeyens.it/eclipse/
 
-Command line flash tool installation
+
+Development build instructions
 ==========================
 
-```
-sudo apt-get install arduino gcc-avr avr-libc avrdude python-configobj python-jinja2 python-serial
-mkdir tmp
-cd tmp
-git clone https://github.com/miracle2k/python-glob2
-cd python-glob2
-wget https://bootstrap.pypa.io/ez_setup.py -O - | sudo python
-sudo python setup.py install
-git clone git://github.com/amperka/ino.git
-cd ino
-sudo make install
-```
+**This firmware is automatically bundled into [FarmBot OS](https://github.com/FarmBot/farmbot_os).
+The following instructions are for isolated Arduino development only.**
 
-Command line flash tool use
-==========================
 **NOTE:** We tag releases when they are stable. The latest version (on master) is not guaranteed to be stable.
 
 See [releases](https://github.com/FarmBot/farmbot-arduino-firmware/releases) to find a stable release.
@@ -34,25 +23,41 @@ See [releases](https://github.com/FarmBot/farmbot-arduino-firmware/releases) to
 git clone  https://github.com/FarmBot/farmbot-arduino-firmware
 ```
 
-**OPTION B:** For stable release 1.0:
+**OPTION B:** For stable release v6.4.2:
 
 ```
-git clone -b 'alpha-1.0' --single-branch  https://github.com/FarmBot/farmbot-arduino-firmware
+git clone -b 'v6.4.2' --single-branch  https://github.com/FarmBot/farmbot-arduino-firmware
 ```
-To flash the firmware onto the device, run this:
 
-```
-cd farmbot-arduino-firmware
-ino build
-ino upload
-```
+Options for compiling and uploading:
+ * [Arduino IDE](https://www.arduino.cc/en/main/software):
+   * Open `farmbot-arduino-firmware/src/src.ino`.
+   * Select the `Mega 2560` board in _Tools_ > _Board_.
+   * Uncomment only the desired board in `src/Board.h`.
+   * To compile and flash the firmware onto the device:
+     * Connect a device via USB.
+     * Select _Sketch_ > _Upload_ or click the _upload_ button.
+   * To compile without flashing:
+     * Select _Sketch_ > _Export compiled binary_.
+     * The `.hex` file will save to the `src` directory.
+ * Make (Linux)
+   * [Download the Arduino 1.8.12 IDE](https://www.arduino.cc/download_handler.php?f=/arduino-1.8.12-linux64.tar.xz) and unpack to the `/home` directory.
+   * `cd farmbot-arduino-firmware`
+   * To compile:
+     * `make`
+     * `.hex` files for each board type will save to the `bin` directory.
+ * VSCode
+   * Set Arduino path and board type.
+   * To compile and flash the firmware onto the device:
+     * Connect a device via USB.
+     * Select _Arduino: Upload_.
 
 Software overview
 =================
 
-All files are in /src
+All files are in `/src`
 
-Farmbot_arduino_controller contains the setup() and main(). This the main sequence:
+`src.ino` contains the setup() and main loop(). This is the main sequence:
 
 ```
      +--------------------------+
@@ -71,113 +76,128 @@ Farmbot_arduino_controller contains the setup() and main(). This the main sequen
      |***Handler                |
      +-------+-----------+------+
              |           |
-             |           +---+
-             v               v
-     +--------------+   +-----------+
-     |StepperControl|   | PinControl|
-     +--------------+   +-----------+
+             |           |
+             v           v
+      +--------+   +-----------+
+      |Movement|   | PinControl|
+      +--------+   +-----------+
 
 ```
 
+Board Feature Overview
+======================
+| board             | kit          | pin encoders | SPI encoders | SPI motors | SPI stall detection |
+|:----------------- |:------------ |:------------:|:------------:|:----------:|:-------------------:|
+| RAMPS_V14         | Genesis v1.2 |      x       |              |            |                     |
+| FARMDUINO_V10     | Genesis v1.3 |      x       |              |            |                     |
+| FARMDUINO_V14     | Genesis v1.4 |              |      x       |            |                     |
+| FARMDUINO_V30     | Genesis v1.5 |              |      x       |     x      |                     |
+| FARMDUINO_EXP_V20 | Express v1.0 |              |              |     x      |          x          |
+
 Codes used for communication
 ============================
 
-Pin Numbering
--------------
+IMPORTANT
+---------
 
+Farmbot will NOT move until the configuration has been approved.
+To approve manually, send `F22 P2 V1 Q0`
 
-Tag              |Pin Nr|Comment
------------------|------|-------
-X_STEP_PIN       |  54  | X axis step signal
-X_DIR_PIN        |  55  | X axis direction choice
-X_ENABLE_PIN     |  38  | X axis enable
-X_MIN_PIN        |   3  | X axis end stop at home position
-X_MAX_PIN        |   2  | X axis end stop at far position
-X_ENCDR_A        |  16  | X axis encoder A channel
-X_ENCDR_B        |  17  | X axis encoder B channel
-X_ENCDR_A_Q      |  31  | X axis encoder A channel for quarature (not implemented) 
-X_ENCDR_B_Q      |  33  | X axis encoder B channel for quarature (not implemented)
-Y_STEP_PIN       |  60  | Y axis step signal
-Y_DIR_PIN        |  61  | Y axis direction choice
-Y_ENABLE_PIN     |  56  | Y axis enable
-Y_MIN_PIN        |  14  | Y axis end stop at home position
-Y_MAX_PIN        |  15  | Y axis end stop at far position
-Y_ENCDR_A        |  23  | Y axis encoder A channel
-Y_ENCDR_B        |  25  | Y axis encoder B channel
-Y_ENCDR_A_Q      |  35  | Y axis encoder A channel for quarature (not implemented) 
-Y_ENCDR_B_Q      |  37  | Y axis encoder B channel for quarature (not implemented) 
-Z_STEP_PIN       |  46  | Z axis step signal
-Z_DIR_PIN        |  48  | Z axis direction choice
-Z_ENABLE_PIN     |  62  | Z axis enable
-Z_MIN_PIN        |  18  | Z axis end stop at home position
-Z_MAX_PIN        |  19  | Z axis end stop at far position
-Z_ENCDR_A        |  27  | Z axis encoder A channel
-Z_ENCDR_B        |  29  | Z axis encoder B channel
-Z_ENCDR_A_Q      |  39  | Z axis encoder A channel for quarature (not implemented) 
-Z_ENCDR_B_Q      |  41  | Z axis encoder B channel for quarature (not implemented) 
-LED_PIN          |  13  | on board LED
-FAN_PIN          |   9  | RAMPS board fan pin
-HEATER_0_PIN     |  10  | RAMPS board heating pin 0
-HEATER_1_PIN     |   8  | RAMPS board heating pin 1
-SERVO_0_PIN	 |   4  | Servo motor 0 signal pin
-SERVO_1_PIN	 |   5  | Servo motor 1 signal pin
+To move, use the command `G00 X0 Y0 Z0 Q0` where you type in the coordinates just after `X`, `Y` and `Z`.
 
 G-Codes
 -------
 
+### Codes sent to the arduino
 
 Code type|Number|Parameters|Function
 ---------|------|----------|--------
-         |      |          |
-         |      |          |Codes send to the arduino
-         |      |          |
-G        |      |          |G-Code, the codes working the same as a 3D printer
-G        |00    |X Y Z S   |Move to location at given speed for axis (don't have to be a straight line), in absolute coordinates
-G        |01    |X Y Z S   |Move to location on a straight line
-G        |28    |          |Move home all axis
-F        |      |          |Farm commands, commands specially added for the farmbot
-F        |01    |T         |Dose amount of water using time in millisecond
-F        |02    |N         |Dose amount of water using flow meter that measures pulses
-F        |11    |          |Home X axis
-F        |12    |          |Home Y axis
-F        |13    |          |Home Z axis
-F        |14    |          |Calibrate X axis
-F        |15    |          |Calibrate Y axis
-F        |16    |          |Calibrate Z axis
+_G_      |      |          |_G-Code, the codes working the same as a 3D printer_
+G        |00    |X Y Z A B C|Move to location at given speed for axis (don't have to be a straight line), in absolute coordinates
+G        |01    |X Y Z A B C|Move to location on a straight line (not implemented)
+G        |28    |          |Move home all axis (Z, Y, X axis order)
+_F_      |      |          |_Farm commands, commands specially added for FarmBot_
+F        |01    |T         |Dose amount of water using time in millisecond (not implemented)
+F        |02    |N         |Dose amount of water using flow meter that measures pulses (not implemented)
+F        |09    |          |Reset emergency stop
+F        |11    |          |Home X axis (find 0, 3 attempts) __*__
+F        |12    |          |Home Y axis (find 0, 3 attempts) __*__
+F        |13    |          |Home Z axis (find 0, 3 attempts) __*__
+F        |14    |          |Calibrate X axis (measure length + find 0) __*__
+F        |15    |          |Calibrate Y axis (measure length + find 0) __*__
+F        |16    |          |Calibrate Z axis (measure length + find 0) __*__
 F        |20    |          |List all parameters and value
 F        |21    |P         |Read parameter
 F        |22    |P V       |Write parameter
 F        |23    |P V       |Update parameter (during calibration)
-F        |31    |P         |Read status
-F        |32    |P V       |Write status
+F        |31    |P         |Read status (not enabled)
+F        |32    |P V       |Write status (not enabled)
 F        |41    |P V M     |Set a value V on an arduino pin in mode M (digital=0/analog=1)
 F        |42    |P M       |Read a value from an arduino pin P in mode M (digital=0/analog=1)
-F        |43    |P M       |Set the I/O mode M (input=0/output=1) of a pin P in arduino 
+F        |43    |P M       |Set the I/O mode M (input=0/output=1) of a pin P in arduino
 F        |44    |P V W T M |Set the value V on an arduino pin P, wait for time T in milliseconds, set value W on the arduino pin P in mode M (digital=0/analog=1)
 F        |51    |E P V     |Set a value on the tool mount with I2C (not implemented)
 F        |52    |E P       |Read value from the tool mount with I2C (not implemented)
-F        |61    |P V       |Set the servo on the pin P (only pin 4 and 5) to the requested angle V
+F        |61    |P V       |Set the servo on the pin P (only pins 4, 5, 6, and 11) to the requested angle V
 F        |81    |          |Report end stop
 F        |82    |          |Report current position
-F        |83    |          |Report software version
-E        |      |          |Emergency stop
-         |      |          |
-         |      |          |Codes received from the arduino
-         |      |          |
-R        |      |          |Report messages
-R        |01    |          |Current command started
-R        |02    |          |Current command finished successfully
-R        |03    |          |Current command finished with error
-R        |04    |          |Current command running
-R        |05    |          |Report motor/axis state
-R        |06    |          |Report calibration state during execution
-R        |21    |P V       |Report parameter value
-R        |31    |P V       |Report status value
-R        |41    |P V       |Report pin value
-R        |81    |X1 X2 Y1 Y2 Z1 Z2|Reporting end stops - parameters: X1 (end stop x axis min) X2 (end stop x axis max) Y1 Y2 Z1 Z2
-R        |82    |X Y Z     |Report current position
-R        |83    |C         |Report software version
-R        |99    |C         |Debug message
+F        |83    |          |Report software version
+F        |84    |X Y Z     |Set axis current position to zero (yes=1/no=0)
+E        |      |          |Emergency stop
+
+__*__ Requires the use of encoders or end stops.
+
+### Codes received from the arduino
+
+Code type|Number|Parameters       |Function
+---------|------|-----------------|--------
+_R_      |      |                 |_Report messages_
+R        |00    |                 |Idle
+R        |01    |                 |Current command started
+R        |02    |                 |Current command finished successfully
+R        |03    |V                |Current command finished with error
+R        |04    |                 |Current command running
+R        |05    |X Y Z            |Report motor/axis state
+R        |06    |X Y Z            |Report calibration state during execution
+R        |07    |                 |Retry movement
+R        |08    |                 |Command echo
+R        |09    |                 |Command invalid
+R        |11    |                 |X axis homing complete
+R        |12    |                 |Y axis homing complete
+R        |13    |                 |Z axis homing complete
+R        |15    | X               |Firmware used a different X coordinate than given in move command
+R        |16    | Y               |Firmware used a different Y coordinate than given in move command
+R        |17    | Z               |Firmware used a different Z coordinate than given in move command
+R        |20    |                 |Report all paramaters complete
+R        |21    |P V              |Report parameter value
+R        |23    |P V              |Report updated parameter (during calibration)
+R        |31    |P V              |Report status value (not enabled)
+R        |41    |P V              |Report pin value
+R        |71    |                 |X axis timeout
+R        |72    |                 |Y axis timeout
+R        |73    |                 |Z axis timeout
+R        |81    |XA XB YA YB ZA ZB|Report end stops
+R        |82    |X Y Z            |Report current position
+R        |83    |                 |Report software version
+R        |84    |X Y Z            |Report encoder position scaled
+R        |85    |X Y Z            |Report encoder position raw
+R        |87    |                 |Emergency lock
+R        |88    |                 |No config (see [configuration approval](#important))
+R        |89    |U X V Y W Z      |Report # axis steps (U,V,W) and highest missed steps in last 500 (X,Y,Z)
+R        |99    |                 |Debug message
+
+Error codes (R03)
+-----------------
+
+Value |Description
+------|------------
+0     |No error
+1     |Emergency stop
+2     |Timeout
+3     |Stall detected
+4     |Calibration error
+14    |Invalid command
+15    |No config
 
 Axis states (R05)
 -----------------
@@ -210,21 +230,21 @@ Parameters for commands
 
 Parameters|Description            |Unit of Measurement
 ----------|-----------------------|-------------------
-X         |X movement             |steps
-Y         |Y movement             |steps
-Z         |Z movement	          |steps
-S         |Speed                  |steps/second
-Q         |Quantity               |counter ticks
+X         |X movement             |millimeters
+Y         |Y movement             |millimeters
+Z         |Z movement             |millimeters
+A         |X speed                |steps/second
+B         |Y speed                |steps/second
+C         |Z speed                |steps/second
+Q         |Queue number           |#
 T         |Time                   |seconds
-C         |Comment                |text
+N         |Number                 |#
 P         |Parameter/pin number   |#
 V         |Value number           |#
 W         |Secondary value        |#
-L         |Number                 |#
 E         |Element (in tool mount)|#
 M         |Mode (set pin mode)    |0 = output / 1 = input
 M         |Mode (read/write)      |0 = digital / 1 = analog
-          |                       |
 XA        |End stop 1 on x axis   |0/1
 XB        |End stop 2 on x axis   |0/1
 YA        |End stop 1 on y axis   |0/1
@@ -235,62 +255,147 @@ ZB        |End stop 2 on z axis   |0/1
 Arduino parameter numbers
 ------------------------
 
-Parameter name               |Parameter id
------------------------------|------------
-PARAM_VERSION                |0
-MOVEMENT_TIMEOUT_X           |11
-MOVEMENT_TIMEOUT_Y           |12
-MOVEMENT_TIMEOUT_Z           |13
-MOVEMENT_INVERT_ENDPOINTS_X  |21
-MOVEMENT_INVERT_ENDPOINTS_Y  |22
-MOVEMENT_INVERT_ENDPOINTS_Z  |23
-MOVEMENT_ENABLE_ENDPOINTS_X  |25
-MOVEMENT_ENABLE_ENDPOINTS_Y  |26
-MOVEMENT_ENABLE_ENDPOINTS_Z  |27
-MOVEMENT_INVERT_MOTOR_X      |31
-MOVEMENT_INVERT_MOTOR_Y      |32
-MOVEMENT_INVERT_MOTOR_Z      |33
-MOVEMENT_SECONDARY_MOTOR_X   |36
-MOVEMENT_SECONDARY_MOTOR_INVERT_X|37
-MOVEMENT_STEPS_ACC_DEC_X     |41
-MOVEMENT_STEPS_ACC_DEC_Y     |42
-MOVEMENT_STEPS_ACC_DEC_Z     |43
-MOVEMENT_HOME_UP_X           |51
-MOVEMENT_HOME_UP_Y           |52
-MOVEMENT_HOME_UP_Z           |53
-MOVEMENT_MIN_SPD_X           |61
-MOVEMENT_MIN_SPD_Y           |62
-MOVEMENT_MIN_SPD_Z           |63
-MOVEMENT_MAX_SPD_X           |71
-MOVEMENT_MAX_SPD_Y           |72
-MOVEMENT_MAX_SPD_Z           |73
-MOVEMENT_MAX_SPD_X           |71
-MOVEMENT_MAX_SPD_Y           |72
-MOVEMENT_MAX_SPD_Z           |73
-ENCODER_ENABLED_X            |101
-ENCODER_ENABLED_Y            |102
-ENCODER_ENABLED_Z            |103
-ENCODER_MISSED_STEPS_MAX_X   |111
-ENCODER_MISSED_STEPS_MAX_Y   |112
-ENCODER_MISSED_STEPS_MAX_Z   |113
-ENCODER_MISSED_STEPS_DECAY_X |121
-ENCODER_MISSED_STEPS_DECAY_Y |122
-ENCODER_MISSED_STEPS_DECAY_Z |123
-MOVEMENT_AXIS_NR_STEPS_X     |141
-MOVEMENT_AXIS_NR_STEPS_Y     |142
-MOVEMENT_AXIS_NR_STEPS_Z     |143
-PIN_GUARD_1_PIN_NR           |201
-PIN_GUARD_1_TIME_OUT         |202
-PIN_GUARD_1_ACTIVE_STATE     |203
-PIN_GUARD_2_PIN_NR           |205
-PIN_GUARD_2_TIME_OUT         |206
-PIN_GUARD_2_ACTIVE_STATE     |207
-PIN_GUARD_3_PIN_NR           |211
-PIN_GUARD_3_TIME_OUT         |212
-PIN_GUARD_3_ACTIVE_STATE     |213
-PIN_GUARD_4_PIN_NR           |215
-PIN_GUARD_4_TIME_OUT         |216
-PIN_GUARD_4_ACTIVE_STATE     |217
-PIN_GUARD_5_PIN_NR           |221
-PIN_GUARD_5_TIME_OUT         |222
-PIN_GUARD_5_ACTIVE_STATE     |223
+ID  | Name                              | Unit      | Notes
+----| ----------------------------------| ----------| ---------------------------------------
+2   | PARAM_CONFIG_OK                   | 0 / 1     | 
+3   | PARAM_USE_EEPROM                  | 0 / 1     | 
+4   | PARAM_E_STOP_ON_MOV_ERR           | 0 / 1     | 
+5   | PARAM_MOV_NR_RETRY                | integer   | 
+11  | MOVEMENT_TIMEOUT_X                | seconds   | 
+12  | MOVEMENT_TIMEOUT_Y                | seconds   | 
+13  | MOVEMENT_TIMEOUT_Z                | seconds   | 
+15  | MOVEMENT_KEEP_ACTIVE_X            | 0 / 1     | 
+16  | MOVEMENT_KEEP_ACTIVE_Y            | 0 / 1     | 
+17  | MOVEMENT_KEEP_ACTIVE_Z            | 0 / 1     | 
+18  | MOVEMENT_HOME_AT_BOOT_X           | 0 / 1     | 
+19  | MOVEMENT_HOME_AT_BOOT_Y           | 0 / 1     | 
+20  | MOVEMENT_HOME_AT_BOOT_Z           | 0 / 1     | 
+21  | MOVEMENT_INVERT_ENDPOINTS_X       | 0 / 1     | switch ends
+22  | MOVEMENT_INVERT_ENDPOINTS_Y       | 0 / 1     | switch ends
+23  | MOVEMENT_INVERT_ENDPOINTS_Z       | 0 / 1     | switch ends
+25  | MOVEMENT_ENABLE_ENDPOINTS_X       | 0 / 1     | 
+26  | MOVEMENT_ENABLE_ENDPOINTS_Y       | 0 / 1     | 
+27  | MOVEMENT_ENABLE_ENDPOINTS_Z       | 0 / 1     | 
+31  | MOVEMENT_INVERT_MOTOR_X           | 0 / 1     | 
+32  | MOVEMENT_INVERT_MOTOR_Y           | 0 / 1     | 
+33  | MOVEMENT_INVERT_MOTOR_Z           | 0 / 1     | 
+36  | MOVEMENT_SECONDARY_MOTOR_X        | 0 / 1     | 
+37  | MOVEMENT_SECONDARY_MOTOR_INVERT_X | 0 / 1     | 
+41  | MOVEMENT_STEPS_ACC_DEC_X          | steps     | 
+42  | MOVEMENT_STEPS_ACC_DEC_Y          | steps     | 
+43  | MOVEMENT_STEPS_ACC_DEC_Z          | steps     | 
+45  | MOVEMENT_STOP_AT_HOME_X           | 0 / 1     | 
+46  | MOVEMENT_STOP_AT_HOME_Y           | 0 / 1     | 
+47  | MOVEMENT_STOP_AT_HOME_Z           | 0 / 1     | 
+51  | MOVEMENT_HOME_UP_X                | 0 / 1     | 
+52  | MOVEMENT_HOME_UP_Y                | 0 / 1     | 
+53  | MOVEMENT_HOME_UP_Z                | 0 / 1     | 
+55  | MOVEMENT_STEP_PER_MM_X            | steps     | 
+56  | MOVEMENT_STEP_PER_MM_Y            | steps     | 
+57  | MOVEMENT_STEP_PER_MM_Z            | steps     | 
+61  | MOVEMENT_MIN_SPD_X                | steps/s   | 
+62  | MOVEMENT_MIN_SPD_Y                | steps/s   | 
+63  | MOVEMENT_MIN_SPD_Z                | steps/s   | 
+65  | MOVEMENT_HOME_SPD_X               | steps/s   | 
+66  | MOVEMENT_HOME_SPD_Y               | steps/s   | 
+67  | MOVEMENT_HOME_SPD_Z               | steps/s   | 
+71  | MOVEMENT_MAX_SPD_X                | steps/s   | 
+72  | MOVEMENT_MAX_SPD_Y                | steps/s   | 
+73  | MOVEMENT_MAX_SPD_Z                | steps/s   | 
+75  | MOVEMENT_INVERT_2_ENDPOINTS_X     | 0 / 1     | switch NO and NC
+76  | MOVEMENT_INVERT_2_ENDPOINTS_Y     | 0 / 1     | switch NO and NC
+77  | MOVEMENT_INVERT_2_ENDPOINTS_Z     | 0 / 1     | switch NO and NC
+81  | MOVEMENT_MOTOR_CURRENT_X          | milliamps | TMC2130 only
+82  | MOVEMENT_MOTOR_CURRENT_Y          | milliamps | TMC2130 only
+83  | MOVEMENT_MOTOR_CURRENT_Z          | milliamps | TMC2130 only
+85  | MOVEMENT_STALL_SENSITIVITY_X      | integer   | -63 (high) to +63 (low), Express only
+86  | MOVEMENT_STALL_SENSITIVITY_Y      | integer   | -63 (high) to +63 (low), Express only
+87  | MOVEMENT_STALL_SENSITIVITY_Z      | integer   | -63 (high) to +63 (low), Express only
+91  | MOVEMENT_MICROSTEPS_X             | integer   | TMC2130 only
+92  | MOVEMENT_MICROSTEPS_Y             | integer   | TMC2130 only
+93  | MOVEMENT_MICROSTEPS_Z             | integer   | TMC2130 only
+101 | ENCODER_ENABLED_X                 | 0 / 1     | enables stall detection on Express
+102 | ENCODER_ENABLED_Y                 | 0 / 1     | enables stall detection on Express
+103 | ENCODER_ENABLED_Z                 | 0 / 1     | enables stall detection on Express
+105 | ENCODER_TYPE_X                    | 0         | differential channels disabled
+106 | ENCODER_TYPE_Y                    | 0         | differential channels disabled
+107 | ENCODER_TYPE_Z                    | 0         | differential channels disabled
+111 | ENCODER_MISSED_STEPS_MAX_X        | steps     | 
+112 | ENCODER_MISSED_STEPS_MAX_Y        | steps     | 
+113 | ENCODER_MISSED_STEPS_MAX_Z        | steps     | 
+115 | ENCODER_SCALING_X                 | integer   | `10000*motor/encoder` (except Express)
+116 | ENCODER_SCALING_Y                 | integer   | `10000*motor/encoder` (except Express)
+117 | ENCODER_SCALING_Z                 | integer   | `10000*motor/encoder` (except Express)
+121 | ENCODER_MISSED_STEPS_DECAY_X      | steps     | 1-99
+122 | ENCODER_MISSED_STEPS_DECAY_Y      | steps     | 1-99
+123 | ENCODER_MISSED_STEPS_DECAY_Z      | steps     | 1-99
+125 | ENCODER_USE_FOR_POS_X             | 0 / 1     | except Express
+126 | ENCODER_USE_FOR_POS_Y             | 0 / 1     | except Express
+127 | ENCODER_USE_FOR_POS_Z             | 0 / 1     | except Express
+131 | ENCODER_INVERT_X                  | 0 / 1     | except Express
+132 | ENCODER_INVERT_Y                  | 0 / 1     | except Express
+133 | ENCODER_INVERT_Z                  | 0 / 1     | except Express
+141 | MOVEMENT_AXIS_NR_STEPS_X          | steps     | 0 = limit disabled
+142 | MOVEMENT_AXIS_NR_STEPS_Y          | steps     | 0 = limit disabled
+143 | MOVEMENT_AXIS_NR_STEPS_Z          | steps     | 0 = limit disabled
+145 | MOVEMENT_STOP_AT_MAX_X            | 0 / 1     | 
+146 | MOVEMENT_STOP_AT_MAX_Y            | 0 / 1     | 
+147 | MOVEMENT_STOP_AT_MAX_Z            | 0 / 1     | 
+201 | PIN_GUARD_1_PIN_NR                | integer   | 
+202 | PIN_GUARD_1_TIME_OUT              | seconds   | 
+203 | PIN_GUARD_1_ACTIVE_STATE          | 0 / 1     | 
+205 | PIN_GUARD_2_PIN_NR                | integer   | 
+206 | PIN_GUARD_2_TIME_OUT              | seconds   | 
+207 | PIN_GUARD_2_ACTIVE_STATE          | 0 / 1     | 
+211 | PIN_GUARD_3_PIN_NR                | integer   | 
+212 | PIN_GUARD_3_TIME_OUT              | seconds   | 
+213 | PIN_GUARD_3_ACTIVE_STATE          | 0 / 1     | 
+215 | PIN_GUARD_4_PIN_NR                | integer   | 
+216 | PIN_GUARD_4_TIME_OUT              | seconds   | 
+217 | PIN_GUARD_4_ACTIVE_STATE          | 0 / 1     | 
+221 | PIN_GUARD_5_PIN_NR                | integer   | 
+222 | PIN_GUARD_5_TIME_OUT              | seconds   | 
+223 | PIN_GUARD_5_ACTIVE_STATE          | 0 / 1     | 
+
+Pin Numbering
+-------------
+
+### RAMPS 1.4 (for other boards, see [/src/pins.h](/src/pins.h))
+
+Tag              |Pin Nr|Comment
+-----------------|------|-------
+X_STEP_PIN       |  54  | X axis step signal
+X_DIR_PIN        |  55  | X axis direction choice
+X_ENABLE_PIN     |  38  | X axis enable
+X_MIN_PIN        |   3  | X axis end stop at home position
+X_MAX_PIN        |   2  | X axis end stop at far position
+X_ENCDR_A        |  16  | X axis encoder A channel
+X_ENCDR_B        |  17  | X axis encoder B channel
+X_ENCDR_A_Q      |  31  | X axis encoder A channel for quarature (not implemented)
+X_ENCDR_B_Q      |  33  | X axis encoder B channel for quarature (not implemented)
+Y_STEP_PIN       |  60  | Y axis step signal
+Y_DIR_PIN        |  61  | Y axis direction choice
+Y_ENABLE_PIN     |  56  | Y axis enable
+Y_MIN_PIN        |  14  | Y axis end stop at home position
+Y_MAX_PIN        |  15  | Y axis end stop at far position
+Y_ENCDR_A        |  23  | Y axis encoder A channel
+Y_ENCDR_B        |  25  | Y axis encoder B channel
+Y_ENCDR_A_Q      |  35  | Y axis encoder A channel for quarature (not implemented)
+Y_ENCDR_B_Q      |  37  | Y axis encoder B channel for quarature (not implemented)
+Z_STEP_PIN       |  46  | Z axis step signal
+Z_DIR_PIN        |  48  | Z axis direction choice
+Z_ENABLE_PIN     |  62  | Z axis enable
+Z_MIN_PIN        |  18  | Z axis end stop at home position
+Z_MAX_PIN        |  19  | Z axis end stop at far position
+Z_ENCDR_A        |  27  | Z axis encoder A channel
+Z_ENCDR_B        |  29  | Z axis encoder B channel
+Z_ENCDR_A_Q      |  39  | Z axis encoder A channel for quarature (not implemented)
+Z_ENCDR_B_Q      |  41  | Z axis encoder B channel for quarature (not implemented)
+LED_PIN          |  13  | on board LED
+FAN_PIN          |   9  | RAMPS board fan pin
+HEATER_0_PIN     |  10  | RAMPS board heating pin 0
+HEATER_1_PIN     |   8  | RAMPS board heating pin 1
+SERVO_0_PIN      |   4  | Servo motor 0 signal pin
+SERVO_1_PIN      |   5  | Servo motor 1 signal pin
+SERVO_2_PIN      |   6  | Servo motor 2 signal pin
+SERVO_3_PIN      |  11  | Servo motor 3 signal pin

SECURITY.md

@@ -0,0 +1,9 @@
+# Security Policy
+
+## Supported Versions
+
+Please see our [official support policy](http://support-policy.farm.bot).
+
+## Reporting a Vulnerability
+
+Please see our [guidlines for responsibly disclosing security vulnerabilities](http://vulnerabilities.farm.bot/).

lib/EEPROM.Makefile

@@ -0,0 +1,40 @@
+DEP_EEPROM_BUILD_DIR := $(FBARDUINO_FIRMWARE_LIB_BUILD_DIR)/EEPROM
+DEP_EEPROM := $(DEP_EEPROM_BUILD_DIR)/EEPROM.a
+DEP_EEPROM_SRC_DIR := $(ARDUINO_INSTALL_DIR)/hardware/arduino/avr/libraries/EEPROM/src
+
+DEP_EEPROM_CFLAGS := \
+	-I$(DEP_EEPROM_SRC_DIR)
+
+DEP_EEPROM_CFLAGS := -I$(DEP_EEPROM_SRC_DIR)
+DEP_EEPROM_LDFLAGS := $(DEP_EEPROM_BUILD_DIR)/EEPROM.a -L$(DEP_EEPROM_BUILD_DIR) -lm
+
+DEP_EEPROM_ASM_SRC := $(call rwildcard, $(DEP_EEPROM_SRC_DIR), *.S)
+DEP_EEPROM_ASM_OBJ := $(DEP_EEPROM_ASM_SRC:.S=.o)
+
+DEP_EEPROM_C_SRC   := $(call rwildcard, $(DEP_EEPROM_SRC_DIR), *.c)
+DEP_EEPROM_C_OBJ   := $(DEP_EEPROM_C_SRC:.c=.o)
+
+DEP_EEPROM_CXX_SRC := $(call rwildcard, $(DEP_EEPROM_SRC_DIR), *.cpp)
+DEP_EEPROM_CXX_OBJ := $(DEP_EEPROM_CXX_SRC:.cpp=.o)
+
+DEP_EEPROM_ALL_OBJ := $(DEP_EEPROM_ASM_OBJ) $(DEP_EEPROM_C_SRC) $(DEP_EEPROM_CXX_OBJ)
+
+DEP_EEPROM_SRC := $(DEP_SERVO_ASM_SRC) $(DEP_SERVO_C_SRC) $(CXX_SRC)
+DEP_EEPROM_OBJ := $(patsubst $(DEP_EEPROM_SRC_DIR)/%,$(DEP_EEPROM_BUILD_DIR)/%,$(DEP_EEPROM_ALL_OBJ))
+
+ARDUINO_DEP_EEPROM_CXX_FLAGS_P := $(DEP_CORE_CXX_FLAGS_P) $(DEP_EEPROM_CFLAGS)
+
+$(DEP_EEPROM): $(DEP_CORE) $(DEP_EEPROM_BUILD_DIR) $(DEP_EEPROM_OBJ)
+	$(AR) rcs $(DEP_EEPROM) $(DEP_EEPROM_OBJ)
+
+$(DEP_EEPROM_BUILD_DIR)/%.o: $(DEP_EEPROM_SRC_DIR)/%.cpp
+	$(CXX) $(ARDUINO_DEP_EEPROM_CXX_FLAGS_P) $< -o $@
+
+$(DEP_EEPROM_BUILD_DIR):
+	$(MKDIR_P) $(DEP_EEPROM_BUILD_DIR)
+
+dep_EEPROM: $(DEP_EEPROM)
+
+dep_EEPROM_clean:
+	$(RM) $(DEP_EEPROM_OBJ)
+	$(RM) $(DEP_EEPROM)

lib/SPI.Makefile

@@ -0,0 +1,42 @@
+DEP_SPI_BUILD_DIR := $(FBARDUINO_FIRMWARE_LIB_BUILD_DIR)/SPI
+DEP_SPI := $(DEP_SPI_BUILD_DIR)/SPI.a
+DEP_SPI_SRC_DIR := $(ARDUINO_INSTALL_DIR)/hardware/arduino/avr/libraries/SPI/src
+
+DEP_SPI_CFLAGS := \
+	-I$(DEP_SPI_SRC_DIR)
+
+DEP_SPI_CFLAGS := -I$(DEP_SPI_SRC_DIR)
+DEP_SPI_LDFLAGS := $(DEP_SPI_BUILD_DIR)/SPI.a -L$(DEP_SPI_BUILD_DIR) -lm
+
+DEP_SPI_ASM_SRC := $(call rwildcard, $(DEP_SPI_SRC_DIR), *.S)
+DEP_SPI_ASM_OBJ := $(DEP_SPI_ASM_SRC:.S=.o)
+
+DEP_SPI_C_SRC   := $(call rwildcard, $(DEP_SPI_SRC_DIR), *.c)
+DEP_SPI_C_OBJ   := $(DEP_SPI_C_SRC:.c=.o)
+
+DEP_SPI_CXX_SRC := $(call rwildcard, $(DEP_SPI_SRC_DIR), *.cpp)
+DEP_SPI_CXX_OBJ := $(DEP_SPI_CXX_SRC:.cpp=.o)
+
+DEP_SPI_ALL_OBJ := $(DEP_SPI_ASM_OBJ) $(DEP_SPI_C_SRC) $(DEP_SPI_CXX_OBJ)
+
+DEP_SPI_SRC := $(DEP_SERVO_ASM_SRC) $(DEP_SERVO_C_SRC) $(CXX_SRC)
+DEP_SPI_OBJ := $(patsubst $(DEP_SPI_SRC_DIR)/%,$(DEP_SPI_BUILD_DIR)/%,$(DEP_SPI_ALL_OBJ))
+
+DEP_SPI_DIRS := $(sort $(dir $(DEP_SPI_OBJ)))
+
+ARDUINO_DEP_SPI_CXX_FLAGS_P := $(DEP_CORE_CXX_FLAGS_P) $(DEP_SPI_CFLAGS)
+
+$(DEP_SPI): $(DEP_CORE) $(DEP_SPI_BUILD_DIR) $(DEP_SPI_OBJ)
+	$(AR) rcs $(DEP_SPI) $(DEP_SPI_OBJ)
+
+$(DEP_SPI_BUILD_DIR)/%.o: $(DEP_SPI_SRC_DIR)/%.cpp
+	$(CXX) $(ARDUINO_DEP_SPI_CXX_FLAGS_P) $< -o $@
+
+$(DEP_SPI_BUILD_DIR):
+	$(MKDIR_P) $(DEP_SPI_DIRS)
+
+dep_SPI: $(DEP_SPI)
+
+dep_SPI_clean:
+	$(RM) $(DEP_SPI_OBJ)
+	$(RM) $(DEP_SPI)

lib/Servo.Makefile

@@ -0,0 +1,42 @@
+DEP_Servo_BUILD_DIR := $(FBARDUINO_FIRMWARE_LIB_BUILD_DIR)/Servo
+DEP_Servo := $(DEP_Servo_BUILD_DIR)/Servo.a
+DEP_Servo_SRC_DIR := $(ARDUINO_INSTALL_DIR)/libraries/Servo/src
+
+DEP_Servo_CFLAGS := \
+	-I$(DEP_Servo_SRC_DIR)
+
+DEP_Servo_CFLAGS := -I$(DEP_Servo_SRC_DIR)
+DEP_Servo_LDFLAGS := $(DEP_Servo_BUILD_DIR)/Servo.a -L$(DEP_Servo_BUILD_DIR) -lm
+
+DEP_Servo_ASM_SRC := $(call rwildcard, $(DEP_Servo_SRC_DIR), *.S)
+DEP_Servo_ASM_OBJ := $(DEP_Servo_ASM_SRC:.S=.o)
+
+DEP_Servo_C_SRC   := $(call rwildcard, $(DEP_Servo_SRC_DIR), *.c)
+DEP_Servo_C_OBJ   := $(DEP_Servo_C_SRC:.c=.o)
+
+DEP_Servo_CXX_SRC := $(call rwildcard, $(DEP_Servo_SRC_DIR), *.cpp)
+DEP_Servo_CXX_OBJ := $(DEP_Servo_CXX_SRC:.cpp=.o)
+
+DEP_Servo_ALL_OBJ := $(DEP_Servo_ASM_OBJ) $(DEP_Servo_C_SRC) $(DEP_Servo_CXX_OBJ)
+
+DEP_Servo_SRC := $(DEP_SERVO_ASM_SRC) $(DEP_SERVO_C_SRC) $(CXX_SRC)
+DEP_Servo_OBJ := $(patsubst $(DEP_Servo_SRC_DIR)/%,$(DEP_Servo_BUILD_DIR)/%,$(DEP_Servo_ALL_OBJ))
+
+DEP_Servo_DIRS := $(sort $(dir $(DEP_Servo_OBJ)))
+
+ARDUINO_DEP_Servo_CXX_FLAGS_P := $(DEP_CORE_CXX_FLAGS_P) $(DEP_Servo_CFLAGS)
+
+$(DEP_Servo): $(DEP_CORE) $(DEP_Servo_BUILD_DIR) $(DEP_Servo_OBJ)
+	$(AR) rcs $(DEP_Servo) $(DEP_Servo_OBJ)
+
+$(DEP_Servo_BUILD_DIR)/%.o: $(DEP_Servo_SRC_DIR)/%.cpp
+	$(CXX) $(ARDUINO_DEP_Servo_CXX_FLAGS_P) $< -o $@
+
+$(DEP_Servo_BUILD_DIR):
+	$(MKDIR_P) $(DEP_Servo_DIRS)
+
+dep_Servo: $(DEP_Servo)
+
+dep_Servo_clean:
+	$(RM) $(DEP_Servo_OBJ)
+	$(RM) $(DEP_Servo)

lib/core.Makefile

@@ -0,0 +1,100 @@
+DEP_CORE_BUILD_DIR := $(BUILD_DIR)/core
+DEP_CORE := $(DEP_CORE_BUILD_DIR)/core.a
+
+DEP_CORE_CFLAGS := \
+	-I$(ARDUINO_INSTALL_DIR)/hardware/arduino/avr/cores/arduino \
+	-I$(ARDUINO_INSTALL_DIR)/hardware/arduino/avr/variants/mega \
+
+DEP_CORE_LDFLAGS := \
+	 $(DEP_CORE_BUILD_DIR)/core.a -L$(DEP_CORE_BUILD_DIR) -lm
+
+DEP_CORE_ASM_SRC := $(ARDUINO_INSTALL_DIR)/hardware/arduino/avr/cores/arduino/wiring_pulse.S
+
+DEP_CORE_C_SRC := \
+	$(ARDUINO_INSTALL_DIR)/hardware/arduino/avr/cores/arduino/WInterrupts.c \
+	$(ARDUINO_INSTALL_DIR)/hardware/arduino/avr/cores/arduino/hooks.c \
+	$(ARDUINO_INSTALL_DIR)/hardware/arduino/avr/cores/arduino/wiring.c \
+	$(ARDUINO_INSTALL_DIR)/hardware/arduino/avr/cores/arduino/wiring_analog.c \
+	$(ARDUINO_INSTALL_DIR)/hardware/arduino/avr/cores/arduino/wiring_digital.c \
+	$(ARDUINO_INSTALL_DIR)/hardware/arduino/avr/cores/arduino/wiring_pulse.c \
+	$(ARDUINO_INSTALL_DIR)/hardware/arduino/avr/cores/arduino/wiring_shift.c
+
+DEP_CORE_CXX_SRC := \
+	$(ARDUINO_INSTALL_DIR)/hardware/arduino/avr/cores/arduino/CDC.cpp \
+	$(ARDUINO_INSTALL_DIR)/hardware/arduino/avr/cores/arduino/HardwareSerial.cpp \
+	$(ARDUINO_INSTALL_DIR)/hardware/arduino/avr/cores/arduino/HardwareSerial0.cpp \
+	$(ARDUINO_INSTALL_DIR)/hardware/arduino/avr/cores/arduino/HardwareSerial1.cpp \
+	$(ARDUINO_INSTALL_DIR)/hardware/arduino/avr/cores/arduino/HardwareSerial2.cpp \
+	$(ARDUINO_INSTALL_DIR)/hardware/arduino/avr/cores/arduino/HardwareSerial3.cpp \
+	$(ARDUINO_INSTALL_DIR)/hardware/arduino/avr/cores/arduino/IPAddress.cpp \
+	$(ARDUINO_INSTALL_DIR)/hardware/arduino/avr/cores/arduino/PluggableUSB.cpp \
+	$(ARDUINO_INSTALL_DIR)/hardware/arduino/avr/cores/arduino/Print.cpp \
+	$(ARDUINO_INSTALL_DIR)/hardware/arduino/avr/cores/arduino/Stream.cpp \
+	$(ARDUINO_INSTALL_DIR)/hardware/arduino/avr/cores/arduino/Tone.cpp \
+	$(ARDUINO_INSTALL_DIR)/hardware/arduino/avr/cores/arduino/USBCore.cpp \
+	$(ARDUINO_INSTALL_DIR)/hardware/arduino/avr/cores/arduino/WMath.cpp \
+	$(ARDUINO_INSTALL_DIR)/hardware/arduino/avr/cores/arduino/WString.cpp \
+	$(ARDUINO_INSTALL_DIR)/hardware/arduino/avr/cores/arduino/abi.cpp \
+	$(ARDUINO_INSTALL_DIR)/hardware/arduino/avr/cores/arduino/main.cpp \
+	$(ARDUINO_INSTALL_DIR)/hardware/arduino/avr/cores/arduino/new.cpp
+
+DEP_CORE_OBJ := \
+	$(DEP_CORE_BUILD_DIR)/wiring_pulse.S.o \
+	$(DEP_CORE_BUILD_DIR)/WInterrupts.c.o \
+	$(DEP_CORE_BUILD_DIR)/hooks.c.o \
+	$(DEP_CORE_BUILD_DIR)/wiring.c.o \
+	$(DEP_CORE_BUILD_DIR)/wiring_analog.c.o \
+	$(DEP_CORE_BUILD_DIR)/wiring_digital.c.o \
+	$(DEP_CORE_BUILD_DIR)/wiring_pulse.c.o \
+	$(DEP_CORE_BUILD_DIR)/wiring_shift.c.o \
+	$(DEP_CORE_BUILD_DIR)/CDC.cpp.o \
+	$(DEP_CORE_BUILD_DIR)/HardwareSerial.cpp.o \
+	$(DEP_CORE_BUILD_DIR)/HardwareSerial0.cpp.o \
+	$(DEP_CORE_BUILD_DIR)/HardwareSerial1.cpp.o \
+	$(DEP_CORE_BUILD_DIR)/HardwareSerial2.cpp.o \
+	$(DEP_CORE_BUILD_DIR)/HardwareSerial3.cpp.o \
+	$(DEP_CORE_BUILD_DIR)/IPAddress.cpp.o \
+	$(DEP_CORE_BUILD_DIR)/PluggableUSB.cpp.o \
+	$(DEP_CORE_BUILD_DIR)/Print.cpp.o \
+	$(DEP_CORE_BUILD_DIR)/Stream.cpp.o \
+	$(DEP_CORE_BUILD_DIR)/Tone.cpp.o \
+	$(DEP_CORE_BUILD_DIR)/USBCore.cpp.o \
+	$(DEP_CORE_BUILD_DIR)/WMath.cpp.o \
+	$(DEP_CORE_BUILD_DIR)/WString.cpp.o \
+	$(DEP_CORE_BUILD_DIR)/abi.cpp.o \
+	$(DEP_CORE_BUILD_DIR)/main.cpp.o \
+	$(DEP_CORE_BUILD_DIR)/new.cpp.o
+
+DEP_CORE_D := $(DEP_CORE_OBJ:.cpp=.d)
+
+DEP_CORE_CFLAGS_P := -c -g -Os -w -std=gnu11 -ffunction-sections \
+-fdata-sections -MMD -flto -fno-fat-lto-objects -mmcu=atmega2560 \
+-DF_CPU=16000000L -DARDUINO=10600 -DARDUINO_AVR_MEGA2560 -DARDUINO_ARCH_AVR \
+$(DEP_CORE_CFLAGS)
+
+DEP_CORE_CXX_FLAGS_P := -c -g -Os -w -std=gnu++11 -fpermissive -fno-exceptions -ffunction-sections \
+-fdata-sections -fno-threadsafe-statics -MMD -flto -mmcu=atmega2560 \
+-DF_CPU=16000000L -DARDUINO=10600 -DARDUINO_AVR_MEGA2560 -DARDUINO_ARCH_AVR \
+$(DEP_CORE_CFLAGS)
+
+$(DEP_CORE): $(DEP_CORE_BUILD_DIR) $(DEP_CORE_OBJ)
+	$(AR) rcs $(DEP_CORE) $(DEP_CORE_OBJ)
+
+$(DEP_CORE_BUILD_DIR)/%.S.o: $(ARDUINO_INSTALL_DIR)/hardware/arduino/avr/cores/arduino/%.S
+	$(CC) -c -g -x assembler-with-cpp -flto -MMD -mmcu=atmega2560 -DF_CPU=16000000L -DARDUINO=10600 -DARDUINO_AVR_MEGA2560 -DARDUINO_ARCH_AV \
+	-I$(ARDUINO_INSTALL_DIR)/hardware/arduino/avr/cores/arduino -I$(ARDUINO_INSTALL_DIR)/hardware/arduino/avr/variants/mega \
+	$< -o $@
+
+$(DEP_CORE_BUILD_DIR)/%.c.o: $(ARDUINO_INSTALL_DIR)/hardware/arduino/avr/cores/arduino/%.c
+	$(CC) $(DEP_CORE_CFLAGS_P) $< -o $@
+
+$(DEP_CORE_BUILD_DIR)/%.cpp.o: $(ARDUINO_INSTALL_DIR)/hardware/arduino/avr/cores/arduino/%.cpp
+	$(CXX) $(DEP_CORE_CXX_FLAGS_P) $< -o $@
+
+$(DEP_CORE_BUILD_DIR):
+	$(MKDIR_P) $(DEP_CORE_BUILD_DIR)
+
+dep_core_clean:
+	$(RM) $(DEP_CORE_OBJ)
+	$(RM) $(DEP_CORE_D)
+	$(RM) $(DEP_CORE)

lib/targets/express_k10.Makefile

@@ -0,0 +1,25 @@
+TARGET_express_k10_BUILD_DIR := $(BUILD_DIR)/express_k10
+TARGET_express_k10_HEX := $(BIN_DIR)/express_k10.hex
+
+TARGET_express_k10_OBJ := $(patsubst $(FBARDUINO_FIRMWARE_SRC_DIR)/%,$(TARGET_express_k10_BUILD_DIR)/%,$(CXX_OBJ))
+
+$(TARGET_express_k10_HEX): $(TARGET_express_k10_BUILD_DIR) $(TARGET_express_k10_BUILD_DIR)/express_k10.eep $(TARGET_express_k10_BUILD_DIR)/express_k10.elf
+	$(OBJ_COPY) -O ihex -R .eeprom  $(TARGET_express_k10_BUILD_DIR)/express_k10.elf $@
+
+$(TARGET_express_k10_BUILD_DIR)/express_k10.eep: $(TARGET_express_k10_BUILD_DIR)/express_k10.elf
+	$(OBJ_COPY) -O ihex -j .eeprom --set-section-flags=.eeprom=alloc,load --no-change-warnings --change-section-lma .eeprom=0  $< $@
+
+$(TARGET_express_k10_BUILD_DIR)/express_k10.elf: $(TARGET_express_k10_OBJ)
+	$(CC) -w -Os -g -flto -fuse-linker-plugin -Wl,--gc-sections,--relax -mmcu=atmega2560 -o $@ $(TARGET_express_k10_OBJ) $(DEPS_OBJ) $(DEP_CORE_LDFLAGS)
+
+$(TARGET_express_k10_BUILD_DIR)/%.o: $(FBARDUINO_FIRMWARE_SRC_DIR)/%.cpp $(HEADERS)
+	$(CXX) $(CXX_FLAGS) -DFARMBOT_BOARD_ID=3 $(DEPS_CFLAGS) $< -o $@
+
+$(TARGET_express_k10_BUILD_DIR):
+	$(MKDIR_P) $(TARGET_express_k10_BUILD_DIR)
+
+target_express_k10: $(TARGET_express_k10_HEX)
+
+target_express_k10_clean:
+	$(RM) -r $(TARGET_express_k10_BUILD_DIR)
+	$(RM) $(TARGET_express_k10_HEX)

lib/targets/farmduino_k14.Makefile

@@ -0,0 +1,25 @@
+TARGET_farmduino_k14_BUILD_DIR := $(BUILD_DIR)/farmduino_k14
+TARGET_farmduino_k14_HEX := $(BIN_DIR)/farmduino_k14.hex
+
+TARGET_farmduino_k14_OBJ := $(patsubst $(FBARDUINO_FIRMWARE_SRC_DIR)/%,$(TARGET_farmduino_k14_BUILD_DIR)/%,$(CXX_OBJ))
+
+$(TARGET_farmduino_k14_HEX): $(TARGET_farmduino_k14_BUILD_DIR) $(TARGET_farmduino_k14_BUILD_DIR)/farmduino_k14.eep $(TARGET_farmduino_k14_BUILD_DIR)/farmduino_k14.elf
+	$(OBJ_COPY) -O ihex -R .eeprom  $(TARGET_farmduino_k14_BUILD_DIR)/farmduino_k14.elf $@
+
+$(TARGET_farmduino_k14_BUILD_DIR)/farmduino_k14.eep: $(TARGET_farmduino_k14_BUILD_DIR)/farmduino_k14.elf
+	$(OBJ_COPY) -O ihex -j .eeprom --set-section-flags=.eeprom=alloc,load --no-change-warnings --change-section-lma .eeprom=0  $< $@
+
+$(TARGET_farmduino_k14_BUILD_DIR)/farmduino_k14.elf: $(TARGET_farmduino_k14_OBJ)
+	$(CC) -w -Os -g -flto -fuse-linker-plugin -Wl,--gc-sections,--relax -mmcu=atmega2560 -o $@ $(TARGET_farmduino_k14_OBJ) $(DEPS_OBJ) $(DEP_CORE_LDFLAGS)
+
+$(TARGET_farmduino_k14_BUILD_DIR)/%.o: $(FBARDUINO_FIRMWARE_SRC_DIR)/%.cpp $(HEADERS)
+	$(CXX) $(CXX_FLAGS) -DFARMBOT_BOARD_ID=2 $(DEPS_CFLAGS) $< -o $@
+
+$(TARGET_farmduino_k14_BUILD_DIR):
+	$(MKDIR_P) $(TARGET_farmduino_k14_BUILD_DIR)
+
+target_farmduino_k14: $(TARGET_farmduino_k14_HEX)
+
+target_farmduino_k14_clean:
+	$(RM) -r $(TARGET_farmduino_k14_BUILD_DIR)
+	$(RM) $(TARGET_farmduino_k14_HEX)

lib/targets/farmduino_k15.Makefile

@@ -0,0 +1,25 @@
+TARGET_farmduino_k15_BUILD_DIR := $(BUILD_DIR)/farmduino_k15
+TARGET_farmduino_k15_HEX := $(BIN_DIR)/farmduino_k15.hex
+
+TARGET_farmduino_k15_OBJ := $(patsubst $(FBARDUINO_FIRMWARE_SRC_DIR)/%,$(TARGET_farmduino_k15_BUILD_DIR)/%,$(CXX_OBJ))
+
+$(TARGET_farmduino_k15_HEX): $(TARGET_farmduino_k15_BUILD_DIR) $(TARGET_farmduino_k15_BUILD_DIR)/farmduino_k15.eep $(TARGET_farmduino_k15_BUILD_DIR)/farmduino_k15.elf
+	$(OBJ_COPY) -O ihex -R .eeprom  $(TARGET_farmduino_k15_BUILD_DIR)/farmduino_k15.elf $@
+
+$(TARGET_farmduino_k15_BUILD_DIR)/farmduino_k15.eep: $(TARGET_farmduino_k15_BUILD_DIR)/farmduino_k15.elf
+	$(OBJ_COPY) -O ihex -j .eeprom --set-section-flags=.eeprom=alloc,load --no-change-warnings --change-section-lma .eeprom=0  $< $@
+
+$(TARGET_farmduino_k15_BUILD_DIR)/farmduino_k15.elf: $(TARGET_farmduino_k15_OBJ)
+	$(CC) -w -Os -g -flto -fuse-linker-plugin -Wl,--gc-sections,--relax -mmcu=atmega2560 -o $@ $(TARGET_farmduino_k15_OBJ) $(DEPS_OBJ) $(DEP_CORE_LDFLAGS)
+
+$(TARGET_farmduino_k15_BUILD_DIR)/%.o: $(FBARDUINO_FIRMWARE_SRC_DIR)/%.cpp $(HEADERS)
+	$(CXX) $(CXX_FLAGS) -DFARMBOT_BOARD_ID=4 $(DEPS_CFLAGS) $< -o $@
+
+$(TARGET_farmduino_k15_BUILD_DIR):
+	$(MKDIR_P) $(TARGET_farmduino_k15_BUILD_DIR)
+
+target_farmduino_k15: $(TARGET_farmduino_k15_HEX)
+
+target_farmduino_k15_clean:
+	$(RM) -r $(TARGET_farmduino_k15_BUILD_DIR)
+	$(RM) $(TARGET_farmduino_k15_HEX)

lib/targets/farmduino_v10.Makefile

@@ -0,0 +1,25 @@
+TARGET_farmduino_v10_BUILD_DIR := $(BUILD_DIR)/farmduino_v10
+TARGET_farmduino_v10_HEX := $(BIN_DIR)/farmduino.hex
+
+TARGET_farmduino_v10_OBJ := $(patsubst $(FBARDUINO_FIRMWARE_SRC_DIR)/%,$(TARGET_farmduino_v10_BUILD_DIR)/%,$(CXX_OBJ))
+
+$(TARGET_farmduino_v10_HEX): $(TARGET_farmduino_v10_BUILD_DIR) $(TARGET_farmduino_v10_BUILD_DIR)/farmduino_v10.eep $(TARGET_farmduino_v10_BUILD_DIR)/farmduino_v10.elf
+	$(OBJ_COPY) -O ihex -R .eeprom  $(TARGET_farmduino_v10_BUILD_DIR)/farmduino_v10.elf $@
+
+$(TARGET_farmduino_v10_BUILD_DIR)/farmduino_v10.eep: $(TARGET_farmduino_v10_BUILD_DIR)/farmduino_v10.elf
+	$(OBJ_COPY) -O ihex -j .eeprom --set-section-flags=.eeprom=alloc,load --no-change-warnings --change-section-lma .eeprom=0  $< $@
+
+$(TARGET_farmduino_v10_BUILD_DIR)/farmduino_v10.elf: $(TARGET_farmduino_v10_OBJ)
+	$(CC) -w -Os -g -flto -fuse-linker-plugin -Wl,--gc-sections,--relax -mmcu=atmega2560 -o $@ $(TARGET_farmduino_v10_OBJ) $(DEPS_OBJ) $(DEP_CORE_LDFLAGS)
+
+$(TARGET_farmduino_v10_BUILD_DIR)/%.o: $(FBARDUINO_FIRMWARE_SRC_DIR)/%.cpp $(HEADERS)
+	$(CXX) $(CXX_FLAGS) -DFARMBOT_BOARD_ID=1 $(DEPS_CFLAGS) $< -o $@
+
+$(TARGET_farmduino_v10_BUILD_DIR):
+	$(MKDIR_P) $(TARGET_farmduino_v10_BUILD_DIR)
+
+target_farmduino_v10: $(TARGET_farmduino_v10_HEX)
+
+target_farmduino_v10_clean:
+	$(RM) -r $(TARGET_farmduino_v10_OBJ)
+	$(RM) $(TARGET_farmduino_v10_HEX)

lib/targets/ramps_v14.Makefile

@@ -0,0 +1,25 @@
+TARGET_ramps_v14_BUILD_DIR := $(BUILD_DIR)/ramps_v14
+TARGET_ramps_v14_HEX := $(BIN_DIR)/arduino_firmware.hex
+
+TARGET_ramps_v14_OBJ := $(patsubst $(FBARDUINO_FIRMWARE_SRC_DIR)/%,$(TARGET_ramps_v14_BUILD_DIR)/%,$(CXX_OBJ))
+
+$(TARGET_ramps_v14_HEX): $(TARGET_ramps_v14_BUILD_DIR) $(TARGET_ramps_v14_BUILD_DIR)/arduino_firmware.eep $(TARGET_ramps_v14_BUILD_DIR)/arduino_firmware.elf
+	$(OBJ_COPY) -O ihex -R .eeprom  $(TARGET_ramps_v14_BUILD_DIR)/arduino_firmware.elf $@
+
+$(TARGET_ramps_v14_BUILD_DIR)/arduino_firmware.eep: $(TARGET_ramps_v14_BUILD_DIR)/arduino_firmware.elf
+	$(OBJ_COPY) -O ihex -j .eeprom --set-section-flags=.eeprom=alloc,load --no-change-warnings --change-section-lma .eeprom=0  $< $@
+
+$(TARGET_ramps_v14_BUILD_DIR)/arduino_firmware.elf: $(TARGET_ramps_v14_OBJ)
+	$(CC) -w -Os -g -flto -fuse-linker-plugin -Wl,--gc-sections,--relax -mmcu=atmega2560 -o $@ $(TARGET_ramps_v14_OBJ) $(DEPS_OBJ) $(DEP_CORE_LDFLAGS)
+
+$(TARGET_ramps_v14_BUILD_DIR)/%.o: $(FBARDUINO_FIRMWARE_SRC_DIR)/%.cpp $(HEADERS)
+	$(CXX) $(CXX_FLAGS) -DFARMBOT_BOARD_ID=0 $(DEPS_CFLAGS) $< -o $@
+
+$(TARGET_ramps_v14_BUILD_DIR):
+	$(MKDIR_P) $(TARGET_ramps_v14_BUILD_DIR)
+
+target_ramps_v14: $(TARGET_ramps_v14_HEX)
+
+target_ramps_v14_clean:
+	$(RM) -r $(TARGET_ramps_v14_OBJ)
+	$(RM) $(TARGET_ramps_v14_HEX)

lib/template.Makefile

@@ -0,0 +1,45 @@
+# Someone will thank me later for this.
+# Find and replace DEP_NAME with the name of your dependency.
+
+DEP_DEP_NAME_BUILD_DIR := $(FBARDUINO_FIRMWARE_LIB_BUILD_DIR)/DEP_NAME
+DEP_DEP_NAME := $(DEP_DEP_NAME_BUILD_DIR)/DEP_NAME.a
+DEP_DEP_NAME_SRC_DIR := $(ARDUINO_INSTALL_DIR)/libraries/DEP_NAME/src
+
+DEP_DEP_NAME_CFLAGS := \
+	-I$(DEP_DEP_NAME_SRC_DIR)
+
+DEP_DEP_NAME_CFLAGS := -I$(DEP_DEP_NAME_SRC_DIR)
+DEP_DEP_NAME_LDFLAGS := $(DEP_DEP_NAME_BUILD_DIR)/DEP_NAME.a -L$(DEP_DEP_NAME_BUILD_DIR) -lm
+
+DEP_DEP_NAME_ASM_SRC := $(call rwildcard, $(DEP_DEP_NAME_SRC_DIR), *.S)
+DEP_DEP_NAME_ASM_OBJ := $(DEP_DEP_NAME_ASM_SRC:.S=.o)
+
+DEP_DEP_NAME_C_SRC   := $(call rwildcard, $(DEP_DEP_NAME_SRC_DIR), *.c)
+DEP_DEP_NAME_C_OBJ   := $(DEP_DEP_NAME_C_SRC:.c=.o)
+
+DEP_DEP_NAME_CXX_SRC := $(call rwildcard, $(DEP_DEP_NAME_SRC_DIR), *.cpp)
+DEP_DEP_NAME_CXX_OBJ := $(DEP_DEP_NAME_CXX_SRC:.cpp=.o)
+
+DEP_DEP_NAME_ALL_OBJ := $(DEP_DEP_NAME_ASM_OBJ) $(DEP_DEP_NAME_C_SRC) $(DEP_DEP_NAME_CXX_OBJ)
+
+DEP_DEP_NAME_SRC := $(DEP_SERVO_ASM_SRC) $(DEP_SERVO_C_SRC) $(CXX_SRC)
+DEP_DEP_NAME_OBJ := $(patsubst $(DEP_DEP_NAME_SRC_DIR)/%,$(DEP_DEP_NAME_BUILD_DIR)/%,$(DEP_DEP_NAME_ALL_OBJ))
+
+DEP_DEP_NAME_DIRS := $(sort $(dir $(DEP_DEP_NAME_OBJ)))
+
+ARDUINO_DEP_DEP_NAME_CXX_FLAGS_P := $(DEP_CORE_CXX_FLAGS_P) $(DEP_DEP_NAME_CFLAGS)
+
+$(DEP_DEP_NAME): $(DEP_CORE) $(DEP_DEP_NAME_BUILD_DIR) $(DEP_DEP_NAME_OBJ)
+	$(AR) rcs $(DEP_DEP_NAME) $(DEP_DEP_NAME_OBJ)
+
+$(DEP_DEP_NAME_BUILD_DIR)/%.o: $(DEP_DEP_NAME_SRC_DIR)/%.cpp
+	$(CXX) $(ARDUINO_DEP_DEP_NAME_CXX_FLAGS_P) $< -o $@
+
+$(DEP_DEP_NAME_BUILD_DIR):
+	$(MKDIR_P) $(DEP_DEP_NAME_DIRS)
+
+dep_DEP_NAME: $(DEP_DEP_NAME)
+
+dep_DEP_NAME_clean:
+	$(RM) $(DEP_DEP_NAME_OBJ)
+	$(RM) $(DEP_DEP_NAME)

src.sln

@@ -0,0 +1,22 @@
+
+Microsoft Visual Studio Solution File, Format Version 12.00
+# Visual Studio 15
+VisualStudioVersion = 15.0.26403.3
+MinimumVisualStudioVersion = 10.0.40219.1
+Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "src", "src\src.vcxproj", "{C5F80730-F44F-4478-BDAE-6634EFC2CA88}"
+EndProject
+Global
+	GlobalSection(SolutionConfigurationPlatforms) = preSolution
+		Debug|x86 = Debug|x86
+		Release|x86 = Release|x86
+	EndGlobalSection
+	GlobalSection(ProjectConfigurationPlatforms) = postSolution
+		{C5F80730-F44F-4478-BDAE-6634EFC2CA88}.Debug|x86.ActiveCfg = Debug|Win32
+		{C5F80730-F44F-4478-BDAE-6634EFC2CA88}.Debug|x86.Build.0 = Debug|Win32
+		{C5F80730-F44F-4478-BDAE-6634EFC2CA88}.Release|x86.ActiveCfg = Release|Win32
+		{C5F80730-F44F-4478-BDAE-6634EFC2CA88}.Release|x86.Build.0 = Release|Win32
+	EndGlobalSection
+	GlobalSection(SolutionProperties) = preSolution
+		HideSolutionNode = FALSE
+	EndGlobalSection
+EndGlobal

src/Board.h

@@ -0,0 +1,35 @@
+#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
+
+  #undef RAMPS_V14
+  #undef FARMDUINO_V10
+  #undef FARMDUINO_V14
+  #undef FARMDUINO_V30
+  #undef FARMDUINO_EXP_V20
+
+  #if FARMBOT_BOARD_ID == 0
+    #define RAMPS_V14
+  #elif FARMBOT_BOARD_ID == 1
+    #define FARMDUINO_V10
+  #elif FARMBOT_BOARD_ID == 2
+    #define FARMDUINO_V14
+  #elif FARMBOT_BOARD_ID == 3
+    #define FARMDUINO_EXP_V20
+  #elif FARMBOT_BOARD_ID == 4
+    #define FARMDUINO_V30
+  #endif
+
+#endif

src/Command.cpp

@@ -1,284 +1,353 @@
 #include "Command.h"
 
-const char axisCodes[3]       = { 'X', 'Y', 'Z' };
-const char axisSpeedCodes[3]  = { 'A', 'B', 'C' };
-const char speedCode          = 'S';
-const char parameterIdCode    = 'P';
+const char axisCodes[3] = {'X', 'Y', 'Z'};
+const char axisSpeedCodes[3] = {'A', 'B', 'C'};
+const char speedCode = 'S';
+const char parameterIdCode = 'P';
 const char parameterValueCode = 'V';
-const char parameterValue2Code= 'W';
-const char elementCode        = 'E';
-const char timeCode           = 'T';
-const char modeCode           = 'M';
-const char msgQueueCode       = 'Q';
-
-double axisValue[3] 		= { 0.0, 0.0, 0.0 };
-long   axisSpeedValue[3] 	= { 0, 0, 0 };
-double speedValue   		= 0.0;
-long   parameterId    		= 0;
-long   parameterValue 		= 0;
-long   parameterValue2          = 0;
-long   element                  = 0;
-long   time                     = 0;
-long   mode                     = 0;
-long   msgQueue                 = 0;
+const char parameterValue2Code = 'W';
+const char elementCode = 'E';
+const char timeCode = 'T';
+const char modeCode = 'M';
+const char msgQueueCode = 'Q';
 
 CommandCodeEnum commandCodeEnum = CODE_UNDEFINED;
 
-Command::Command(char * commandChar) {
+Command::Command(char *commandChar)
+{
 
-	char * charBuf = commandChar;
-	char* charPointer;
-	bool invalidCommand = false;
+  char *charBuf = commandChar;
+  char *charPointer;
+  bool invalidCommand = false;
 
-	charPointer = strtok(charBuf, " ");
+  charPointer = strtok(charBuf, " \n\r\0");
 
-	if (charPointer[0] == 'G' || charPointer[0] == 'F') {
-		commandCodeEnum = getGCodeEnum(charPointer);
-	} else {
-		invalidCommand = true;
-		return;
-	}
+  if (charPointer[0] == 'G' || charPointer[0] == 'F')
+  {
+    commandCodeEnum = getGCodeEnum(charPointer);
+  }
+  else
+  {
+    invalidCommand = true;
+    commandCodeEnum = CODE_UNDEFINED;
+    return;
+  }
 
-	while (charPointer != NULL) {
-		getParameter(charPointer);
+  while (charPointer != NULL)
+  {
+    getParameter(charPointer);
 
-		charPointer = strtok(NULL, " \n\r");
-
-	}
+    charPointer = strtok(NULL, " \n\r");
+  }
 }
 
-CommandCodeEnum Command::getGCodeEnum(char* code) {
+CommandCodeEnum Command::getGCodeEnum(char *code)
+{
 
+  if (strcmp(code, "G0") == 0 || strcmp(code, "G00") == 0)
+  {
+    return G00;
+  }
+  if (strcmp(code, "G1") == 0 || strcmp(code, "G01") == 0)
+  {
+    return G01;
+  }
+    if (strcmp(code, "G28") == 0)
+  {
+    return G28;
+  }
 
-	if (strcmp(code, "G0") == 0 || strcmp(code, "G00") == 0) {
-		return G00;
-	}
-	if (strcmp(code, "G1") == 0 || strcmp(code, "G01") == 0) {
-		return G01;
-	}
-	//if (strcmp(code, "F3") == 0 || strcmp(code, "F03") == 0) {
-	//	return F03;
-	//}
+  //if (strcmp(code, "F3") == 0 || strcmp(code, "F03") == 0) {
+  //	return F03;
+  //}
 
-	if (strcmp(code, "F11") == 0) {
-		return F11;
-	}
-	if (strcmp(code, "F12") == 0) {
-		return F12;
-	}
-	if (strcmp(code, "F13") == 0) {
-		return F13;
-	}
-	if (strcmp(code, "F14") == 0) {
-		return F14;
-	}
-	if (strcmp(code, "F15") == 0) {
-		return F15;
-	}
-	if (strcmp(code, "F16") == 0) {
-		return F16;
-	}
+  if (strcmp(code, "F09") == 0 || strcmp(code, "F9") == 0)
+  {
+    return F09;
+  }
 
-	if (strcmp(code, "F20") == 0) {
-		return F20;
-	}
-	if (strcmp(code, "F21") == 0) {
-		return F21;
-	}
-	if (strcmp(code, "F22") == 0) {
-		return F22;
-	}
+  if (strcmp(code, "F11") == 0)
+  {
+    return F11;
+  }
+  if (strcmp(code, "F12") == 0)
+  {
+    return F12;
+  }
+  if (strcmp(code, "F13") == 0)
+  {
+    return F13;
+  }
+  if (strcmp(code, "F14") == 0)
+  {
+    return F14;
+  }
+  if (strcmp(code, "F15") == 0)
+  {
+    return F15;
+  }
+  if (strcmp(code, "F16") == 0)
+  {
+    return F16;
+  }
 
-	if (strcmp(code, "F31") == 0) {
-		return F31;
-	}
-	if (strcmp(code, "F32") == 0) {
-		return F32;
-	}
+  if (strcmp(code, "F20") == 0)
+  {
+    return F20;
+  }
+  if (strcmp(code, "F21") == 0)
+  {
+    return F21;
+  }
+  if (strcmp(code, "F22") == 0)
+  {
+    return F22;
+  }
 
-	if (strcmp(code, "F41") == 0) {
-		return F41;
-	}
-	if (strcmp(code, "F42") == 0) {
-		return F42;
-	}
-	if (strcmp(code, "F43") == 0) {
-		return F43;
-	}
-	if (strcmp(code, "F44") == 0) {
-		return F44;
-	}
+  if (strcmp(code, "F31") == 0)
+  {
+    return F31;
+  }
+  if (strcmp(code, "F32") == 0)
+  {
+    return F32;
+  }
 
-	if (strcmp(code, "F61") == 0) {
-		return F61;
-	}
+  if (strcmp(code, "F41") == 0)
+  {
+    return F41;
+  }
+  if (strcmp(code, "F42") == 0)
+  {
+    return F42;
+  }
+  if (strcmp(code, "F43") == 0)
+  {
+    return F43;
+  }
+  if (strcmp(code, "F44") == 0)
+  {
+    return F44;
+  }
 
-	if (strcmp(code, "F81") == 0) {
-		return F81;
-	}
-	if (strcmp(code, "F82") == 0) {
-		return F82;
-	}
-	if (strcmp(code, "F83") == 0) {
-		return F83;
-	}
+  if (strcmp(code, "F61") == 0)
+  {
+    return F61;
+  }
 
-	return CODE_UNDEFINED;
+  if (strcmp(code, "F81") == 0)
+  {
+    return F81;
+  }
+  if (strcmp(code, "F82") == 0)
+  {
+    return F82;
+  }
+  if (strcmp(code, "F83") == 0)
+  {
+    return F83;
+  }
+  if (strcmp(code, "F84") == 0)
+  {
+    return F84;
+  }
+
+  return CODE_UNDEFINED;
 }
 
-double minusNotAllowed(double value) {
-	if(value < 0) {
-		return 0;
-	}
-	return value;
+double minusNotAllowed(double value)
+{
+  if (value < 0)
+  {
+    return 0;
+  }
+  return value;
 }
 
-void Command::getParameter(char* charPointer) {
+void Command::getParameter(char *charPointer)
+{
 
-	//msgQueue          =  24;
+  //msgQueue          =  24;
 
-	if (charPointer[0]      == axisCodes[0]         ){
-		axisValue[0]      =  atof(charPointer + 1 );
-		//msgQueue	= 77;
-	}
+  if (charPointer[0] == axisCodes[0])
+  {
+    axisValue[0] = atof(charPointer + 1);
+    //msgQueue	= 77;
+  }
 
-	if (charPointer[0] 	== axisCodes[1]         ){
-		axisValue[1]      =  atof(charPointer + 1 );
-	}
+  if (charPointer[0] == axisCodes[1])
+  {
+    axisValue[1] = atof(charPointer + 1);
+  }
 
-	if (charPointer[0] 	== axisCodes[2]         ){
-		axisValue[2]      =  atof(charPointer + 1 );
-	}
+  if (charPointer[0] == axisCodes[2])
+  {
+    axisValue[2] = atof(charPointer + 1);
+  }
 
-	if (charPointer[0] 	== axisSpeedCodes[0]    ){
-		axisSpeedValue[0] =  atof(charPointer + 1 );
-	}
+  if (charPointer[0] == axisSpeedCodes[0])
+  {
+    axisSpeedValue[0] = atof(charPointer + 1);
+  }
 
-	if (charPointer[0] 	== axisSpeedCodes[1]    ){
-		axisSpeedValue[1] =  atof(charPointer + 1 );
-	}
+  if (charPointer[0] == axisSpeedCodes[1])
+  {
+    axisSpeedValue[1] = atof(charPointer + 1);
+  }
 
-	if (charPointer[0] 	== axisSpeedCodes[2]    ){
-		axisSpeedValue[2] =  atof(charPointer + 1 );
-	}
+  if (charPointer[0] == axisSpeedCodes[2])
+  {
+    axisSpeedValue[2] = atof(charPointer + 1);
+  }
 
-	if (charPointer[0] 	== speedCode            ){
-		speedValue      =  atof(charPointer + 1 );
-	}
+  if (charPointer[0] == speedCode)
+  {
+    speedValue = atof(charPointer + 1);
+  }
 
-	if (charPointer[0] 	== parameterIdCode      ){
-		parameterId     =  atof(charPointer + 1 );
-	}
+  if (charPointer[0] == parameterIdCode)
+  {
+    parameterId = atof(charPointer + 1);
+  }
 
-	if (charPointer[0] 	== parameterValueCode   ){
-		parameterValue  =  atof(charPointer + 1 );
+  if (charPointer[0] == parameterValueCode)
+  {
+    parameterValue = atof(charPointer + 1);
+  }
 
-	}
+  if (charPointer[0] == parameterValue2Code)
+  {
+    parameterValue2 = atof(charPointer + 1);
+  }
 
-	if (charPointer[0] 	== parameterValue2Code  ){
-		parameterValue2 =  atof(charPointer + 1 );
-	}
+  if (charPointer[0] == elementCode)
+  {
+    element = atof(charPointer + 1);
+  }
 
-	if (charPointer[0] == elementCode          ){
-		element    =  atof(charPointer + 1 );
-	}
+  if (charPointer[0] == timeCode)
+  {
+    time = atof(charPointer + 1);
+  }
 
-	if (charPointer[0] == timeCode             ){
-		time       =  atof(charPointer + 1 );
-	}
+  if (charPointer[0] == modeCode)
+  {
+    mode = atof(charPointer + 1);
+  }
 
-	if (charPointer[0] == modeCode             ){
-		mode       =  atof(charPointer + 1 );
-	}
-
-	if (charPointer[0] == msgQueueCode         ){
-		msgQueue   =  atof(charPointer + 1 );
-		//msgQueue   =  5;
-	}
+  if (charPointer[0] == msgQueueCode)
+  {
+    msgQueue = atof(charPointer + 1);
+    //msgQueue   =  5;
+  }
 }
 
-void Command::print() {
-	Serial.print("R99 Command with code: ");
-	Serial.print(commandCodeEnum);
-	Serial.print(", X: ");
-	Serial.print(axisValue[0]);
-	Serial.print(", Y: ");
-	Serial.print(axisValue[1]);
-	Serial.print(", Z: ");
-	Serial.print(axisValue[2]);
-	Serial.print(", S: ");
-	Serial.print(speedValue);
-	Serial.print(", P: ");
-	Serial.print(parameterId);
-	Serial.print(", V: ");
-	Serial.print(parameterValue);
+void Command::print()
+{
+  Serial.print("R99 Command with code: ");
+  Serial.print(commandCodeEnum);
+  Serial.print(", X: ");
+  Serial.print(axisValue[0]);
+  Serial.print(", Y: ");
+  Serial.print(axisValue[1]);
+  Serial.print(", Z: ");
+  Serial.print(axisValue[2]);
+  Serial.print(", S: ");
+  Serial.print(speedValue);
+  Serial.print(", P: ");
+  Serial.print(parameterId);
+  Serial.print(", V: ");
+  Serial.print(parameterValue);
 
-	Serial.print(", W: ");
-	Serial.print(parameterValue2);
-	Serial.print(", T: ");
-	Serial.print(time);
-	Serial.print(", E: ");
-	Serial.print(element);
-	Serial.print(", M: ");
-	Serial.print(mode);
-	Serial.print(", Q: ");
-	Serial.print(msgQueue);
-	Serial.print("\r\n");
+  Serial.print(", W: ");
+  Serial.print(parameterValue2);
+  Serial.print(", T: ");
+  Serial.print(time);
+  Serial.print(", E: ");
+  Serial.print(element);
+  Serial.print(", M: ");
+  Serial.print(mode);
+
+  Serial.print(", A: ");
+  Serial.print(axisSpeedValue[0]);
+  Serial.print(", B: ");
+  Serial.print(axisSpeedValue[1]);
+  Serial.print(", C: ");
+  Serial.print(axisSpeedValue[2]);
+
+  Serial.print(", Q: ");
+  Serial.print(msgQueue);
+
+  Serial.print("\r\n");
 }
 
-CommandCodeEnum Command::getCodeEnum() {
-	return commandCodeEnum;
+CommandCodeEnum Command::getCodeEnum()
+{
+  return commandCodeEnum;
 }
 
-double Command::getX() {
-	return axisValue[0];
+double Command::getX()
+{
+  return axisValue[0];
 }
 
-double Command::getY() {
-	return axisValue[1];
+double Command::getY()
+{
+  return axisValue[1];
 }
 
-double Command::getZ() {
-	return axisValue[2];
+double Command::getZ()
+{
+  return axisValue[2];
 }
 
-long Command::getA() {
-	return axisSpeedValue[0];
+long Command::getA()
+{
+  return axisSpeedValue[0];
 }
 
-long Command::getB() {
-	return axisSpeedValue[1];
+long Command::getB()
+{
+  return axisSpeedValue[1];
 }
 
-long Command::getC() {
-	return axisSpeedValue[2];
+long Command::getC()
+{
+  return axisSpeedValue[2];
 }
 
-long Command::getP() {
-	return parameterId;
+long Command::getP()
+{
+  return parameterId;
 }
 
-long Command::getV() {
-	return parameterValue;
+long Command::getV()
+{
+  return parameterValue;
 }
 
-long Command::getW() {
-	return parameterValue2;
+long Command::getW()
+{
+  return parameterValue2;
 }
 
-long Command::getT() {
-	return time;
+long Command::getT()
+{
+  return time;
 }
 
-long Command::getE() {
-	return element;
+long Command::getE()
+{
+  return element;
 }
 
-long Command::getM() {
-	return mode;
+long Command::getM()
+{
+  return mode;
 }
 
-long Command::getQ() {
-	//msgQueue = 9876;
-	return msgQueue;
+long Command::getQ()
+{
+  //msgQueue = 9876;
+  return msgQueue;
 }

src/Command.h

@@ -7,12 +7,13 @@
 enum CommandCodeEnum
 {
   CODE_UNDEFINED = -1,
-  G00 =   0,
-  G01 =   1,
-  G28 =  28,
+  G00 = 0,
+  G01 = 1,
+  G28 = 28,
   F01 = 101,
   F02 = 102,
   F03 = 103,
+  F09 = 109,
   F11 = 111,
   F12 = 112,
   F13 = 113,
@@ -31,37 +32,52 @@ enum CommandCodeEnum
   F61 = 161,
   F81 = 181,
   F82 = 182,
-  F83 = 183
+  F83 = 183,
+  F84 = 184
 };
 
 //#define NULL 0
 
-class Command {
-	CommandCodeEnum codeEnum;
-public:
-//	Command(String);
-	Command(char * commandChar);
-	void print();
-	CommandCodeEnum getCodeEnum();
-	double getX();
-	double getY();
-	double getZ();
-	double getS();
-	long getP();
-	long getV();
-	long getA();
-	long getB();
-	long getC();
-	long getW();
-	long getT();
-	long getE();
-	long getM();
-	long getQ();
+class Command
+{
+  CommandCodeEnum codeEnum;
+
+public:
+  //	Command(String);
+  Command(char *commandChar);
+  void print();
+  CommandCodeEnum getCodeEnum();
+  double getX();
+  double getY();
+  double getZ();
+  double getS();
+  long getP();
+  long getV();
+  long getA();
+  long getB();
+  long getC();
+  long getW();
+  long getT();
+  long getE();
+  long getM();
+  long getQ();
+
+  void printQAndNewLine();
 
-	void printQAndNewLine();
 private:
-	CommandCodeEnum getGCodeEnum(char* code);
-	void getParameter(char* charPointer);
+  CommandCodeEnum getGCodeEnum(char *code);
+  void getParameter(char *charPointer);
+
+  double axisValue[3] = {0.0, 0.0, 0.0};
+  long axisSpeedValue[3] = {0, 0, 0};
+  double speedValue = 0.0;
+  long parameterId = 0;
+  long parameterValue = 0;
+  long parameterValue2 = 0;
+  long element = 0;
+  long time = 0;
+  long mode = 0;
+  long msgQueue = 0;
 };
 
 #endif /* COMMAND_H_ */

src/Config.h

@@ -3,136 +3,265 @@
  *
  *  Created on: 16 maj 2014
  *      Author: MattLech
+ *      Author: Tim Evers
  */
 
 #ifndef CONFIG_H_
 #define CONFIG_H_
 
-const int LOGGING = 0;
+  const char SOFTWARE_VERSION[] = "6.5.24\0";
 
-const int INCOMING_CMD_BUF_SIZE = 50;
+  const int LOGGING = 0;
 
-//const unsigned long STEPS_FOR_ACC_DEC = 20;
-//const unsigned int MAX_ACCELERATION_STEPS_PER_SECOND = 2;
+  const int INCOMING_CMD_BUF_SIZE = 100;
 
-//const unsigned int MOVEMENT_STEPS_ACC_DEC        = 100;
-//const unsigned int MOVEMENT_MAX_STEPS_PER_SECOND = 1000;
-//const unsigned int MOVEMENT_HOME_SPEED_S_P_S     = 200;
-//const unsigned int MOVEMENT_TIMEOUT              = 30;
-//const unsigned int INVERT_ENDSTOPS = 1;
-//const bool AXIS_HOME_UP_X = false;
-//const bool AXIS_HOME_UP_Y = false;
-//const bool AXIS_HOME_UP_Z = true;
+  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'};
+  const char COMM_REPORT_CMD_ERROR[4] = {'R', '0', '3', '\0'};
+  const char COMM_REPORT_CMD_BUSY[4] = {'R', '0', '4', '\0'};
+  const char COMM_REPORT_CMD_STATUS[4] = {'R', '0', '5', '\0'};
+  const char COMM_REPORT_CALIB_STATUS[4] = {'R', '0', '6', '\0'};
+  const char COMM_REPORT_CMD_RETRY[4] = { 'R', '0', '7', '\0' };
+  const char COMM_REPORT_CMD_ECHO[4] = { 'R', '0', '8', '\0' };
+  const char COMM_REPORT_BAD_CMD[4] = { 'R', '0', '9', '\0' };
 
-const String COMM_REPORT_CMD_START    			= "R01";
-const String COMM_REPORT_CMD_DONE     			= "R02";
-const String COMM_REPORT_CMD_ERROR    			= "R03";
-const String COMM_REPORT_CMD_BUSY     			= "R04";
-const String COMM_REPORT_CMD_STATUS   			= "R05";
-const String COMM_REPORT_CALIB_STATUS 			= "R06";
-const String COMM_REPORT_COMMENT     		 	= "R99";
+  const char COMM_REPORT_HOMED_X[4] = { 'R', '1', '1', '\0' };
+  const char COMM_REPORT_HOMED_Y[4] = { 'R', '1', '2', '\0' };
+  const char COMM_REPORT_HOMED_Z[4] = { 'R', '1', '3', '\0' };
 
-const int COMM_REPORT_MOVE_STATUS_IDLE         		= 0;
-const int COMM_REPORT_MOVE_STATUS_START_MOTOR  		= 1;
-const int COMM_REPORT_MOVE_STATUS_ACCELERATING 		= 2;
-const int COMM_REPORT_MOVE_STATUS_CRUISING     		= 3;
-const int COMM_REPORT_MOVE_STATUS_DECELERATING 		= 4;
-const int COMM_REPORT_MOVE_STATUS_STOP_MOTOR   		= 5;
-const int COMM_REPORT_MOVE_STATUS_CRAWLING     		= 6;
-const int COMM_REPORT_MOVE_STATUS_ERROR        		= -1;
+  const char COMM_REPORT_COORD_CHANGED_X[4] = { 'R', '1', '5', '\0' };
+  const char COMM_REPORT_COORD_CHANGED_Y[4] = { 'R', '1', '6', '\0' };
+  const char COMM_REPORT_COORD_CHANGED_Z[4] = { 'R', '1', '7', '\0' };
 
-const int COMM_REPORT_CALIBRATE_STATUS_IDLE    		= 0;
-const int COMM_REPORT_CALIBRATE_STATUS_TO_HOME 		= 1;
-const int COMM_REPORT_CALIBRATE_STATUS_TO_END  		= 2;
-const int COMM_REPORT_CALIBRATE_STATUS_ERROR   		= -1;
+  const char COMM_REPORT_TIMEOUT_X[4] = { 'R', '7', '1', '\0' };
+  const char COMM_REPORT_TIMEOUT_Y[4] = { 'R', '7', '2', '\0' };
+  const char COMM_REPORT_TIMEOUT_Z[4] = { 'R', '7', '3', '\0' };
 
+  const char COMM_REPORT_ENCODER_SCALED[4] = { 'R', '8', '4', '\0' };
+  const char COMM_REPORT_ENCODER_RAW[4] = { 'R', '8', '5', '\0' };
 
-const int MOVEMENT_INTERRUPT_SPEED = 100; // Interrupt cycle in micro seconds
+  const char COMM_REPORT_EMERGENCY_STOP[4] = { 'R', '8', '7', '\0' };
+  const char COMM_REPORT_NO_CONFIG[4] = {'R', '8', '8', '\0'};
+  const char COMM_REPORT_COMMENT[4] = {'R', '9', '9', '\0'};
 
-const unsigned int MOVEMENT_SPEED_BASE_TIME      	= 2000;
-const unsigned int MOVEMENT_DELAY                	= 500;
+  const int COMM_REPORT_MOVE_STATUS_IDLE = 0;
+  const int COMM_REPORT_MOVE_STATUS_START_MOTOR = 1;
+  const int COMM_REPORT_MOVE_STATUS_ACCELERATING = 2;
+  const int COMM_REPORT_MOVE_STATUS_CRUISING = 3;
+  const int COMM_REPORT_MOVE_STATUS_DECELERATING = 4;
+  const int COMM_REPORT_MOVE_STATUS_STOP_MOTOR = 5;
+  const int COMM_REPORT_MOVE_STATUS_CRAWLING = 6;
+  const int COMM_REPORT_MOVE_STATUS_ERROR = -1;
 
-const long PARAM_VERSION_DEFAULT                	= 1;
-const long PARAM_TEST_DEFAULT                   	= 0;
+  const int COMM_REPORT_CALIBRATE_STATUS_IDLE = 0;
+  const int COMM_REPORT_CALIBRATE_STATUS_TO_HOME = 1;
+  const int COMM_REPORT_CALIBRATE_STATUS_TO_END = 2;
+  const int COMM_REPORT_CALIBRATE_STATUS_ERROR = -1;
 
-const long MOVEMENT_TIMEOUT_X_DEFAULT           	= 120;
-const long MOVEMENT_TIMEOUT_Y_DEFAULT           	= 120;
-const long MOVEMENT_TIMEOUT_Z_DEFAULT           	= 120;
+  const int MOVEMENT_INTERRUPT_SPEED = 64; // Interrupt cycle in micro seconds
+  //const int MOVEMENT_INTERRUPT_SPEED = 40; // Interrupt cycle in micro seconds
 
-const long MOVEMENT_ENABLE_ENDPOINTS_X_DEFAULT  	= 0;
-const long MOVEMENT_ENABLE_ENDPOINTS_Y_DEFAULT  	= 0;
-const long MOVEMENT_ENABLE_ENDPOINTS_Z_DEFAULT  	= 0;
+  const unsigned int MOVEMENT_SPEED_BASE_TIME = 2000;
+  const unsigned int MOVEMENT_DELAY = 250;
 
-const long MOVEMENT_INVERT_ENDPOINTS_X_DEFAULT  	= 0;
-const long MOVEMENT_INVERT_ENDPOINTS_Y_DEFAULT  	= 0;
-const long MOVEMENT_INVERT_ENDPOINTS_Z_DEFAULT  	= 0;
+  const long PARAM_VERSION_DEFAULT = 1;
+  const long PARAM_TEST_DEFAULT = 0;
 
-const long MOVEMENT_INVERT_MOTOR_X_DEFAULT      	= 0;
-const long MOVEMENT_INVERT_MOTOR_Y_DEFAULT		= 0;
-const long MOVEMENT_INVERT_MOTOR_Z_DEFAULT      	= 0;
+  const long PARAM_CONFIG_OK_DEFAULT = 0;
+  const long PARAM_USE_EEPROM_DEFAULT = 1;
+  const long PARAM_E_STOP_ON_MOV_ERR_DEFAULT = 0;
+  const long PARAM_MOV_NR_RETRY_DEFAULT = 3;
 
-const long MOVEMENT_SECONDARY_MOTOR_X_DEFAULT    	= 1;
-const long MOVEMENT_SECONDARY_MOTOR_INVERT_X_DEFAULT	= 0;
+  const long MOVEMENT_TIMEOUT_X_DEFAULT = 120;
+  const long MOVEMENT_TIMEOUT_Y_DEFAULT = 120;
+  const long MOVEMENT_TIMEOUT_Z_DEFAULT = 120;
 
-const long MOVEMENT_HOME_UP_X_DEFAULT           	= 0;
-const long MOVEMENT_HOME_UP_Y_DEFAULT           	= 0;
-const long MOVEMENT_HOME_UP_Z_DEFAULT           	= 1;
+  const long MOVEMENT_KEEP_ACTIVE_X_DEFAULT = 0;
+  const long MOVEMENT_KEEP_ACTIVE_Y_DEFAULT = 0;
+  const long MOVEMENT_KEEP_ACTIVE_Z_DEFAULT = 1;
 
-// numver of steps used for acceleration or deceleration
-const long MOVEMENT_STEPS_ACC_DEC_X_DEFAULT     	= 500;
-const long MOVEMENT_STEPS_ACC_DEC_Y_DEFAULT     	= 500;
-const long MOVEMENT_STEPS_ACC_DEC_Z_DEFAULT     	= 500;
+  const long MOVEMENT_HOME_AT_BOOT_X_DEFAULT = 0;
+  const long MOVEMENT_HOME_AT_BOOT_Y_DEFAULT = 0;
+  const long MOVEMENT_HOME_AT_BOOT_Z_DEFAULT = 0;
 
-// Minimum speed in steps per second
-const long MOVEMENT_MIN_SPD_X_DEFAULT           	= 50;
-const long MOVEMENT_MIN_SPD_Y_DEFAULT           	= 50;
-const long MOVEMENT_MIN_SPD_Z_DEFAULT           	= 50;
+  const long MOVEMENT_ENABLE_ENDPOINTS_X_DEFAULT = 0;
+  const long MOVEMENT_ENABLE_ENDPOINTS_Y_DEFAULT = 0;
+  const long MOVEMENT_ENABLE_ENDPOINTS_Z_DEFAULT = 0;
 
-// Maxumum speed in steps per second
-const long MOVEMENT_MAX_SPD_X_DEFAULT           	= 1500;
-const long MOVEMENT_MAX_SPD_Y_DEFAULT           	= 1500;
-const long MOVEMENT_MAX_SPD_Z_DEFAULT           	= 1500;
+  const long MOVEMENT_INVERT_ENDPOINTS_X_DEFAULT = 0;
+  const long MOVEMENT_INVERT_ENDPOINTS_Y_DEFAULT = 0;
+  const long MOVEMENT_INVERT_ENDPOINTS_Z_DEFAULT = 0;
 
-// Use encoder (0 or 1)
-const long ENCODER_ENABLED_X_DEFAULT			= 0;
-const long ENCODER_ENABLED_Y_DEFAULT			= 0;
-const long ENCODER_ENABLED_Z_DEFAULT			= 0;
+  const long MOVEMENT_INVERT_MOTOR_X_DEFAULT = 0;
+  const long MOVEMENT_INVERT_MOTOR_Y_DEFAULT = 0;
+  const long MOVEMENT_INVERT_MOTOR_Z_DEFAULT = 0;
 
-// Number of stes missed before motor is seen as not moving
-const long ENCODER_MISSED_STEPS_MAX_X_DEFAULT		= 10;
-const long ENCODER_MISSED_STEPS_MAX_Y_DEFAULT		= 10;
-const long ENCODER_MISSED_STEPS_MAX_Z_DEFAULT		= 10;
+  const long MOVEMENT_SECONDARY_MOTOR_X_DEFAULT = 1;
+  const long MOVEMENT_SECONDARY_MOTOR_INVERT_X_DEFAULT = 1;
 
-// How much a good step is substracted from the missed step total (1-99)
-const long ENCODER_MISSED_STEPS_DECAY_X_DEFAULT 	= 10;
-const long ENCODER_MISSED_STEPS_DECAY_Y_DEFAULT 	= 10;
-const long ENCODER_MISSED_STEPS_DECAY_Z_DEFAULT 	= 10;
+  const long MOVEMENT_HOME_UP_X_DEFAULT = 0;
+  const long MOVEMENT_HOME_UP_Y_DEFAULT = 0;
+  const long MOVEMENT_HOME_UP_Z_DEFAULT = 1;
 
-// pin guard default settings
-const long PIN_GUARD_1_PIN_NR_DEFAULT           	= 0;
-const long PIN_GUARD_1_TIME_OUT_DEFAULT         	= 60;
-const long PIN_GUARD_1_ACTIVE_STATE_DEFAULT     	= 1;
+  const long MOVEMENT_STEP_PER_MM_X_DEFAULT = 5;
+  const long MOVEMENT_STEP_PER_MM_Y_DEFAULT = 5;
+  const long MOVEMENT_STEP_PER_MM_Z_DEFAULT = 25;
 
-const long PIN_GUARD_2_PIN_NR_DEFAULT           	= 0;
-const long PIN_GUARD_2_TIME_OUT_DEFAULT         	= 60;
-const long PIN_GUARD_2_ACTIVE_STATE_DEFAULT     	= 1;
+  // Number of steps used for acceleration or deceleration
+  const long MOVEMENT_STEPS_ACC_DEC_X_DEFAULT = 300;
+  const long MOVEMENT_STEPS_ACC_DEC_Y_DEFAULT = 300;
+  const long MOVEMENT_STEPS_ACC_DEC_Z_DEFAULT = 300;
 
-const long PIN_GUARD_3_PIN_NR_DEFAULT           	= 0;
-const long PIN_GUARD_3_TIME_OUT_DEFAULT         	= 60;
-const long PIN_GUARD_3_ACTIVE_STATE_DEFAULT     	= 1;
+  // Minimum speed in steps per second
+  const long MOVEMENT_MIN_SPD_X_DEFAULT = 50;
+  const long MOVEMENT_MIN_SPD_Y_DEFAULT = 50;
+  const long MOVEMENT_MIN_SPD_Z_DEFAULT = 50;
 
-const long PIN_GUARD_4_PIN_NR_DEFAULT           	= 0;
-const long PIN_GUARD_4_TIME_OUT_DEFAULT         	= 60;
-const long PIN_GUARD_4_ACTIVE_STATE_DEFAULT     	= 1;
+  // Speed used for homing and calibration
+  const long MOVEMENT_HOME_SPEED_X_DEFAULT = 50;
+  const long MOVEMENT_HOME_SPEED_Y_DEFAULT = 50;
+  const long MOVEMENT_HOME_SPEED_Z_DEFAULT = 50;
 
-const long PIN_GUARD_5_PIN_NR_DEFAULT           	= 0;
-const long PIN_GUARD_5_TIME_OUT_DEFAULT         	= 60;
-const long PIN_GUARD_5_ACTIVE_STATE_DEFAULT     	= 1;
+  // Maximum speed in steps per second
+  const long MOVEMENT_MAX_SPD_X_DEFAULT = 400;
+  const long MOVEMENT_MAX_SPD_Y_DEFAULT = 400;
+  const long MOVEMENT_MAX_SPD_Z_DEFAULT = 400;
 
+  // switch the end contacts from NO to NC
+  const long MOVEMENT_INVERT_2_ENDPOINTS_X_DEFAULT = 0;
+  const long MOVEMENT_INVERT_2_ENDPOINTS_Y_DEFAULT = 0;
+  const long MOVEMENT_INVERT_2_ENDPOINTS_Z_DEFAULT = 0;
 
-const long STATUS_GENERAL_DEFAULT               	= 0;
+  // Stop at the home position or continue to other size of axis
+  const long MOVEMENT_STOP_AT_HOME_X_DEFAULT = 0;
+  const long MOVEMENT_STOP_AT_HOME_Y_DEFAULT = 0;
+  const long MOVEMENT_STOP_AT_HOME_Z_DEFAULT = 0;
 
+  // Stop at the maximum size of the axis
+  const long MOVEMENT_STOP_AT_MAX_X_DEFAULT = 0;
+  const long MOVEMENT_STOP_AT_MAX_Y_DEFAULT = 0;
+  const long MOVEMENT_STOP_AT_MAX_Z_DEFAULT = 0;
 
-const String SOFTWARE_VERSION = "GENESIS V.01.07.EXPERIMENTAL";
+  // 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;
+  const long ENCODER_ENABLED_Z_DEFAULT = 0;
+
+  // Type of enocder.
+  // 0 = non-differential encoder, channel A,B
+  // 1 = differenttial encoder, channel A, A*, B, B*
+  const long ENCODER_TYPE_X_DEFAULT = 0;
+  const long ENCODER_TYPE_Y_DEFAULT = 0;
+  const long ENCODER_TYPE_Z_DEFAULT = 0;
+
+  // Position = encoder position * scaling / 10000
+  const long ENCODER_SCALING_X_DEFAULT = 5556;
+  const long ENCODER_SCALING_Y_DEFAULT = 5556;
+  const long ENCODER_SCALING_Z_DEFAULT = 5556;
+
+  // Number of steps missed before motor is seen as not moving
+  const long ENCODER_MISSED_STEPS_MAX_X_DEFAULT = 5;
+  const long ENCODER_MISSED_STEPS_MAX_Y_DEFAULT = 5;
+  const long ENCODER_MISSED_STEPS_MAX_Z_DEFAULT = 5;
+
+  // How much a good step is substracted from the missed step total (1-99)
+  // 10 means it ignores 10 steps in 100. This is normal because of jerkiness while moving
+  const long ENCODER_MISSED_STEPS_DECAY_X_DEFAULT = 5;
+  const long ENCODER_MISSED_STEPS_DECAY_Y_DEFAULT = 5;
+  const long ENCODER_MISSED_STEPS_DECAY_Z_DEFAULT = 5;
+
+  // Use the encoder for positioning
+  const long ENCODER_USE_FOR_POS_X_DEFAULT = 0;
+  const long ENCODER_USE_FOR_POS_Y_DEFAULT = 0;
+  const long ENCODER_USE_FOR_POS_Z_DEFAULT = 0;
+
+  // Invert the encoder position sign
+  const long ENCODER_INVERT_X_DEFAULT = 0;
+  const long ENCODER_INVERT_Y_DEFAULT = 0;
+  const long ENCODER_INVERT_Z_DEFAULT = 0;
+
+  // Length of axis in steps. Zero means don't care
+  const long MOVEMENT_AXIS_NR_STEPS_X_DEFAULT = 0;
+  const long MOVEMENT_AXIS_NR_STEPS_Y_DEFAULT = 0;
+  const long MOVEMENT_AXIS_NR_STEPS_Z_DEFAULT = 0;
+
+  // Pin guard default settings
+  const long PIN_GUARD_1_PIN_NR_DEFAULT = 0;
+  const long PIN_GUARD_1_TIME_OUT_DEFAULT = 60;
+  const long PIN_GUARD_1_ACTIVE_STATE_DEFAULT = 1;
+
+  const long PIN_GUARD_2_PIN_NR_DEFAULT = 0;
+  const long PIN_GUARD_2_TIME_OUT_DEFAULT = 60;
+  const long PIN_GUARD_2_ACTIVE_STATE_DEFAULT = 1;
+
+  const long PIN_GUARD_3_PIN_NR_DEFAULT = 0;
+  const long PIN_GUARD_3_TIME_OUT_DEFAULT = 60;
+  const long PIN_GUARD_3_ACTIVE_STATE_DEFAULT = 1;
+
+  const long PIN_GUARD_4_PIN_NR_DEFAULT = 0;
+  const long PIN_GUARD_4_TIME_OUT_DEFAULT = 60;
+  const long PIN_GUARD_4_ACTIVE_STATE_DEFAULT = 1;
+
+  const long PIN_GUARD_5_PIN_NR_DEFAULT = 0;
+  const long PIN_GUARD_5_TIME_OUT_DEFAULT = 60;
+  const long PIN_GUARD_5_ACTIVE_STATE_DEFAULT = 1;
+
+  const long STATUS_GENERAL_DEFAULT = 0;
+
+#define NSS_PIN 22 
+#define READ_ENA_PIN 49
+#define NULL 0
+
+static const int mdl_spi_encoder_offset = 4;
+enum MdlSpiEncoders
+{
+  _MDL_X1 = 0b0001,
+  _MDL_X2 = 0b0010,
+  _MDL_Y = 0b0100,
+  _MDL_Z = 0b1000,
+};
 #endif /* CONFIG_H_ */
+
+#if defined(RAMPS_V14) && !defined(SOFTWARE_VERSION_SUFFIX)
+#define SOFTWARE_VERSION_SUFFIX ".R.genesisK12\0"
+#endif
+
+#if defined(FARMDUINO_V10) && !defined(SOFTWARE_VERSION_SUFFIX)
+#define SOFTWARE_VERSION_SUFFIX ".F.genesisK13\0"
+#endif
+
+#if defined(FARMDUINO_V14) && !defined(SOFTWARE_VERSION_SUFFIX)
+#define SOFTWARE_VERSION_SUFFIX ".G.genesisK14\0"
+#endif
+
+#if defined(FARMDUINO_V30) && !defined(SOFTWARE_VERSION_SUFFIX)
+#define SOFTWARE_VERSION_SUFFIX ".H.genesisK15\0"
+#endif
+
+#if defined(FARMDUINO_EXP_V20) && !defined(SOFTWARE_VERSION_SUFFIX)
+#define SOFTWARE_VERSION_SUFFIX ".E.expressK10\0"
+#endif
+
+#ifndef FARMBOT_BOARD_ID
+#define SOFTWARE_COMMIT ""
+#else
+#include "CommitSHA.h"
+#endif

src/CurrentState.cpp

@@ -7,128 +7,276 @@
 
 #include "CurrentState.h"
 
-static CurrentState* instance;
-long x = 0;
-long y = 0;
-long z = 0;
-unsigned int speed = 0;
-bool endStopState[3][2];
-long Q = 0;
+static CurrentState *instance;
 
-CurrentState * CurrentState::getInstance() {
-	if (!instance) {
-		instance = new CurrentState();
-	};
-	return instance;
+CurrentState *CurrentState::getInstance()
+{
+  if (!instance)
+  {
+    instance = new CurrentState();
+  };
+  return instance;
 };
 
-CurrentState::CurrentState() {
-	x = 0;
-	y = 0;
-	z = 0;
-	speed = 0;
-	Q = 0;
+CurrentState::CurrentState()
+{
+  x = 0;
+  y = 0;
+  z = 0;
+
+  homeMissedStepX = 0;
+  homeMissedStepY = 0;
+  homeMissedStepZ = 0;
+
+  speed = 0;
+  Q = 0;
+  lastError = 0;
 }
 
-long CurrentState::getX() {
-	return x;
+long CurrentState::getX()
+{
+  return x;
 }
 
-long CurrentState::getY() {
-	return y;
+long CurrentState::getY()
+{
+  return y;
 }
 
-long CurrentState::getZ() {
-	return z;
+long CurrentState::getZ()
+{
+  return z;
 }
 
-long* CurrentState::getPoint() {
-	static long currentPoint[3] = {x, y, z};
-	return currentPoint;
+long *CurrentState::getPoint()
+{
+  static long currentPoint[3] = {x, y, z};
+  return currentPoint;
 }
 
-void CurrentState::setX(long newX) {
-	x = newX;
+void CurrentState::setX(long newX)
+{
+  x = newX;
 }
 
-void CurrentState::setY(long newY) {
-	y = newY;
+void CurrentState::setY(long newY)
+{
+  y = newY;
 }
 
-void CurrentState::setZ(long newZ) {
-	z = newZ;
+void CurrentState::setZ(long newZ)
+{
+  z = newZ;
 }
 
-void CurrentState::setEndStopState(unsigned int axis, unsigned int position, bool state) {
-	endStopState[axis][position] = state;
+long CurrentState::getHomeMissedStepsX()
+{
+  return homeMissedStepX;
 }
 
-void CurrentState::storeEndStops() {
-        CurrentState::getInstance()->setEndStopState(0,0,digitalRead(X_MIN_PIN));
-        CurrentState::getInstance()->setEndStopState(0,1,digitalRead(X_MAX_PIN));
-        CurrentState::getInstance()->setEndStopState(1,0,digitalRead(Y_MIN_PIN));
-        CurrentState::getInstance()->setEndStopState(1,1,digitalRead(Y_MAX_PIN));
-        CurrentState::getInstance()->setEndStopState(2,0,digitalRead(Z_MIN_PIN));
-        CurrentState::getInstance()->setEndStopState(2,1,digitalRead(Z_MAX_PIN));
+long CurrentState::getHomeMissedStepsXscaled()
+{
+  return homeMissedStepX / stepsPerMmX;
 }
 
-void CurrentState::printPosition() {
-	Serial.print("R82");
-	Serial.print(" X");
-	Serial.print(x);
-	Serial.print(" Y");
-	Serial.print(y);
-	Serial.print(" Z");
-	Serial.print(z);
-//	Serial.print("\r\n");
-	printQAndNewLine();
+long CurrentState::getHomeMissedStepsY()
+{
+  return homeMissedStepY;
+}
+
+long CurrentState::getHomeMissedStepsYscaled()
+{
+  return homeMissedStepY / stepsPerMmY;
+}
+
+long CurrentState::getHomeMissedStepsZ()
+{
+  return homeMissedStepZ;
+}
+
+long CurrentState::getHomeMissedStepsZscaled()
+{
+  return homeMissedStepZ / stepsPerMmZ;
+}
+
+void CurrentState::setHomeMissedStepsX(long newX)
+{
+  homeMissedStepX = newX;
+}
+
+void CurrentState::setHomeMissedStepsY(long newY)
+{
+  homeMissedStepY = newY;
+}
+
+void CurrentState::setHomeMissedStepsZ(long newZ)
+{
+  homeMissedStepZ = newZ;
+}
+
+
+
+
+int CurrentState::getLastError()
+{
+  return lastError;
+}
+
+void CurrentState::setLastError(int error)
+{
+  lastError = error;
+}
+
+void CurrentState::setEndStopState(unsigned int axis, unsigned int position, bool state)
+{
+  endStopState[axis][position] = state;
+}
+
+void CurrentState::setStepsPerMm(long stepsX, long stepsY, long stepsZ)
+{
+  stepsPerMmX = stepsX;
+  stepsPerMmY = stepsY;
+  stepsPerMmZ = stepsZ;
+}
+
+long CurrentState::getStepsPerMmX()
+{
+  return stepsPerMmX;
+}
+
+long CurrentState::getStepsPerMmY()
+{
+  return stepsPerMmY;
+}
+
+long CurrentState::getStepsPerMmZ()
+{
+  return stepsPerMmZ;
+}
+
+void CurrentState::storeEndStops()
+{
+  CurrentState::getInstance()->setEndStopState(0, 0, digitalRead(X_MIN_PIN));
+  CurrentState::getInstance()->setEndStopState(0, 1, digitalRead(X_MAX_PIN));
+  CurrentState::getInstance()->setEndStopState(1, 0, digitalRead(Y_MIN_PIN));
+  CurrentState::getInstance()->setEndStopState(1, 1, digitalRead(Y_MAX_PIN));
+  CurrentState::getInstance()->setEndStopState(2, 0, digitalRead(Z_MIN_PIN));
+  CurrentState::getInstance()->setEndStopState(2, 1, digitalRead(Z_MAX_PIN));
+}
+
+void CurrentState::printPosition()
+{
+
+  if (stepsPerMmX <= 0) { stepsPerMmX = 1; }
+  if (stepsPerMmY <= 0) { stepsPerMmY = 1; }
+  if (stepsPerMmZ <= 0) { stepsPerMmZ = 1; }
+
+  Serial.print("R82");
+  Serial.print(" X");
+  Serial.print((float)x / (float)stepsPerMmX);
+  Serial.print(" Y");
+  Serial.print((float)y / (float)stepsPerMmY );
+  Serial.print(" Z");
+  Serial.print((float)z / (float)stepsPerMmZ * 1.0);
+  printQAndNewLine();
+}
+
+String CurrentState::getPosition()
+{
+
+  if (stepsPerMmX <= 0) { stepsPerMmX = 1; }
+  if (stepsPerMmY <= 0) { stepsPerMmY = 1; }
+  if (stepsPerMmZ <= 0) { stepsPerMmZ = 1; }
+
+  String output = "";
+
+  output += "R82";
+  output += " X";
+  output += (float)x / (float)stepsPerMmX * 1.0;
+  output += " Y";
+  output += (float)y / (float)stepsPerMmY * 1.0;
+  output += " Z";
+  output += (float)z / (float)stepsPerMmZ * 1.0;
+
+  return output;
 }
 
 void CurrentState::printBool(bool value)
 {
-	if (value)
-	{
-		Serial.print("1");
-	}
-	else
-	{
-		Serial.print("0");
-	}
+  if (value)
+  {
+    Serial.print("1");
+  }
+  else
+  {
+    Serial.print("0");
+  }
 }
 
-void CurrentState::printEndStops() {
-	Serial.print("R81");
-	Serial.print(" XA");
-	printBool(endStopState[0][0]);
-	Serial.print(" XB");
-	printBool(endStopState[0][1]);
-	Serial.print(" YA");
-	printBool(endStopState[1][0]);
-	Serial.print(" YB");
-	printBool(endStopState[1][1]);
-	Serial.print(" ZA");
-	printBool(endStopState[2][0]);
-	Serial.print(" ZB");
-	printBool(endStopState[2][1]);
-	//Serial.print("\r\n");
-	printQAndNewLine();
+void CurrentState::printEndStops()
+{
+  Serial.print("R81");
+  Serial.print(" XA");
+  printBool(endStopState[0][0]);
+  Serial.print(" XB");
+  printBool(endStopState[0][1]);
+  Serial.print(" YA");
+  printBool(endStopState[1][0]);
+  Serial.print(" YB");
+  printBool(endStopState[1][1]);
+  Serial.print(" ZA");
+  printBool(endStopState[2][0]);
+  Serial.print(" ZB");
+  printBool(endStopState[2][1]);
+  //Serial.print("\r\n");
+  printQAndNewLine();
 }
 
-void CurrentState::print() {
-	printPosition();
-	printEndStops();
+void CurrentState::print()
+{
+  printPosition();
+  printEndStops();
 }
 
-void CurrentState::setQ(long q) {
-	Q = q;
+void CurrentState::setQ(long q)
+{
+  Q = q;
 }
 
-void CurrentState::resetQ() {
-	Q = 0;
+void CurrentState::resetQ()
+{
+  Q = 0;
 }
 
-void CurrentState::printQAndNewLine() {
-        Serial.print(" Q");
-        Serial.print(Q);
-        Serial.print("\r\n");
+void CurrentState::printQAndNewLine()
+{
+  Serial.print(" Q");
+  Serial.print(Q);
+  Serial.print("\r\n");
+}
+
+String CurrentState::getQAndNewLine()
+{
+  String output = "";
+
+  output += " Q";
+  output += Q;
+  output += "\r\n";
+
+  return output;
+}
+
+void CurrentState::setEmergencyStop()
+{
+  emergencyStop = true;
+}
+
+void CurrentState::resetEmergencyStop()
+{
+  emergencyStop = false;
+}
+
+bool CurrentState::isEmergencyStop()
+{
+  return emergencyStop;
 }

src/CurrentState.h

@@ -10,32 +10,97 @@
 #include "Arduino.h"
 #include "pins.h"
 
-class CurrentState {
-public:
-	static CurrentState* getInstance();
-	long getX();
-	long getY();
-	long getZ();
-	long* getPoint();
-	void setX(long);
-	void setY(long);
-	void setZ(long);
-	void setEndStopState(unsigned int, unsigned int, bool);
-	void printPosition();
-	void storeEndStops();
-	void printEndStops();
-	void print();
-	void printBool(bool);
+enum ErrorListEnum
+{
+  ERR_NO_ERROR = 0,
+  ERR_EMERGENCY_STOP = 1,
+  ERR_TIMEOUT = 2,
+  ERR_STALL_DETECTED = 3,
+  ERR_CALIBRATION_ERROR = 4,
+
+  ERR_INVALID_COMMAND = 14,
+  ERR_PARAMS_NOT_OK = 15,
+
+  ERR_STALL_DETECTED_X = 31,
+  ERR_STALL_DETECTED_Y = 32,
+  ERR_STALL_DETECTED_Z = 33,
+};
+
+class CurrentState
+{
+public:
+  static CurrentState *getInstance();
+
+  long getX();
+  long getY();
+  long getZ();
+  long *getPoint();
+  void setX(long);
+  void setY(long);
+  void setZ(long);
+
+  long getHomeMissedStepsX();
+  long getHomeMissedStepsY();
+  long getHomeMissedStepsZ();
+  long getHomeMissedStepsXscaled();
+  long getHomeMissedStepsYscaled();
+  long getHomeMissedStepsZscaled();
+  void setHomeMissedStepsX(long);
+  void setHomeMissedStepsY(long);
+  void setHomeMissedStepsZ(long);
+
+
+  int getLastError();
+  void setLastError(int error);
+
+  void setEndStopState(unsigned int, unsigned int, bool);
+  void printPosition();
+  String getPosition();
+  void storeEndStops();
+  void printEndStops();
+  void print();
+  void printBool(bool);
+
+  void setQ(long);
+  void resetQ();
+  void printQAndNewLine();
+  String getQAndNewLine();
+
+  void setEmergencyStop();
+  void resetEmergencyStop();
+  bool isEmergencyStop();
+
+  void setStepsPerMm(long stepsX, long stepsY, long stepsZ);
+  long getStepsPerMmX();
+  long getStepsPerMmY();
+  long getStepsPerMmZ();
 
-	void setQ(long);
-	void resetQ();
-	void printQAndNewLine();
 
 private:
-	CurrentState();
-	CurrentState(CurrentState const&);
-	void operator=(CurrentState const&);
+  CurrentState();
+  CurrentState(CurrentState const &);
+  void operator=(CurrentState const &);
 
+  long stepsPerMmX = 1;
+  long stepsPerMmY = 1;
+  long stepsPerMmZ = 1;
+
+  int errorCode = 0;
+
+  long x = 0;
+  long y = 0;
+  long z = 0;
+  unsigned int speed = 0;
+  bool endStopState[3][2];
+  long Q = 0;
+  int lastError = 0;
+
+  long homeMissedStepX = 0;
+  long homeMissedStepY = 0;
+  long homeMissedStepZ = 0;
+
+
+  bool emergencyStop = false;
 };
 
 #endif /* CURRENTSTATE_H_ */

src/Debug.h

@@ -0,0 +1,11 @@
+/*
+* Debug.h
+*
+*  Created on: 2017/04/20
+*      Author: Tim Evers
+*/
+
+#ifndef DEBUG_H_
+#define debugMessages   false
+#define debugInterrupt  false
+#endif /* DEBUG_H_ */

src/F09Handler.cpp

@@ -0,0 +1,37 @@
+
+/*
+ * F09Handler.cpp
+ *
+ *  Created on: 2017/04/26
+ *      Author: Tim Evers
+ */
+
+#include "F09Handler.h"
+
+static F09Handler *instance;
+
+F09Handler *F09Handler::getInstance()
+{
+  if (!instance)
+  {
+    instance = new F09Handler();
+  };
+  return instance;
+};
+
+F09Handler::F09Handler()
+{
+}
+
+int F09Handler::execute(Command *command)
+{
+
+  if (LOGGING)
+  {
+    Serial.print("R99 Reset emergency stop\r\n");
+  }
+
+  CurrentState::getInstance()->resetEmergencyStop();
+
+  return 0;
+}

src/F09Handler.h

@@ -0,0 +1,26 @@
+/*
+ * F09Handler.h
+ *
+ *  Created on: 2017/04/26
+ *      Author: Tim Evers
+ */
+
+#ifndef F09HANDLER_H_
+#define F09HANDLER_H_
+#include "GCodeHandler.h"
+#include "Config.h"
+#include "CurrentState.h"
+
+class F09Handler : public GCodeHandler
+{
+public:
+  static F09Handler *getInstance();
+  int execute(Command *);
+
+private:
+  F09Handler();
+  F09Handler(F09Handler const &);
+  void operator=(F09Handler const &);
+};
+
+#endif /* F09HANDLER_H_ */

src/F11Handler.cpp

@@ -8,31 +8,129 @@
 
 #include "F11Handler.h"
 
+static F11Handler *instance;
 
-static F11Handler* instance;
+F11Handler *F11Handler::getInstance()
+{
+  if (!instance)
+  {
+    instance = new F11Handler();
+  };
+  return instance;
+};
 
-F11Handler * F11Handler::getInstance() {
-        if (!instance) {
-                instance = new F11Handler();
-        };
-        return instance;
-}
-;
-
-F11Handler::F11Handler() {
+F11Handler::F11Handler()
+{
 }
 
-int F11Handler::execute(Command* command) {
+int F11Handler::execute(Command *command)
+{
 
-        if (LOGGING) {
-		Serial.print("R99 HOME X\r\n");
-        }
+  if (LOGGING)
+  {
+    Serial.print("R99 HOME X\r\n");
+  }
 
-	StepperControl::getInstance()->moveToCoords(0,0,0, 0,0,0, true, false, false);
+  int homeIsUp = ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_X);
+  int stepsPerMM = ParameterList::getInstance()->getValue(MOVEMENT_STEP_PER_MM_X);
+  int missedStepsMax = ParameterList::getInstance()->getValue(ENCODER_MISSED_STEPS_MAX_X);
 
-        if (LOGGING) {
-                CurrentState::getInstance()->print();
-        }
-        return 0;
+  #if defined(FARMDUINO_EXP_V20)
+    missedStepsMax += ParameterList::getInstance()->getValue(ENCODER_MISSED_STEPS_DECAY_X);
+  #endif
+
+  if (stepsPerMM <= 0)
+  {
+    missedStepsMax = 0;
+    stepsPerMM = 1;
+  }
+
+  int A = 10; // move away coordinates
+  int execution;
+  bool emergencyStop;
+
+  bool homingComplete = false;
+
+  if (homeIsUp == 1)
+  {
+    A = -A;
+  }
+
+  int stepNr;
+
+  double X = CurrentState::getInstance()->getX() / (float)ParameterList::getInstance()->getValue(MOVEMENT_STEP_PER_MM_X);
+  double Y = CurrentState::getInstance()->getY() / (float)ParameterList::getInstance()->getValue(MOVEMENT_STEP_PER_MM_Y);
+  double Z = CurrentState::getInstance()->getZ() / (float)ParameterList::getInstance()->getValue(MOVEMENT_STEP_PER_MM_Z);
+
+  bool firstMove = true;
+  int goodConsecutiveHomings = 0;
+
+  // After the first home, keep moving away and home back
+  // until there is no deviation in positions
+  while (!homingComplete)
+  {
+    if (firstMove)
+    {
+      firstMove = false;
+
+      // Move to home position
+      Movement::getInstance()->moveToCoords(0, Y, Z, 0, 0, 0, false, false, false);
+      //execution = CurrentState::getInstance()->getLastError();
+      execution = 0;
+      emergencyStop = CurrentState::getInstance()->isEmergencyStop();
+      if (emergencyStop || execution != 0) { homingComplete = true; }
+    }
+
+    // Move away from the home position
+    Movement::getInstance()->moveToCoords(A, Y, Z, 0, 0, 0, false, false, false);
+    //execution = CurrentState::getInstance()->getLastError();
+    execution = 0;
+    emergencyStop = CurrentState::getInstance()->isEmergencyStop();
+    if (emergencyStop || execution != 0) { break; }
+
+    // Home again
+    Movement::getInstance()->moveToCoords(0, Y, Z, 0, 0, 0, true, false, false);
+    //execution = CurrentState::getInstance()->getLastError();
+    execution = 0;
+    emergencyStop = CurrentState::getInstance()->isEmergencyStop();
+    if (emergencyStop || execution != 0) { break; }
+
+    Serial.print("R99 homing displaced");
+    Serial.print(" X ");
+    Serial.print(CurrentState::getInstance()->getHomeMissedStepsX());
+    Serial.print(" / ");
+    Serial.print(CurrentState::getInstance()->getHomeMissedStepsXscaled());
+    Serial.print(" Y ");
+    Serial.print(CurrentState::getInstance()->getHomeMissedStepsY());
+    Serial.print(" / ");
+    Serial.print(CurrentState::getInstance()->getHomeMissedStepsYscaled());
+    Serial.print(" Z ");
+    Serial.print(CurrentState::getInstance()->getHomeMissedStepsZ());
+    Serial.print(" / ");
+    Serial.print(CurrentState::getInstance()->getHomeMissedStepsZscaled());
+    Serial.print("\r\n");
+
+    // Home position cannot drift more than 5 milimeter otherwise no valid home pos
+    if (CurrentState::getInstance()->getHomeMissedStepsXscaled() < (20 + (missedStepsMax * 3) / stepsPerMM))
+    {
+      goodConsecutiveHomings++;
+      if (goodConsecutiveHomings >= 3)
+      {
+        homingComplete = true;
+        CurrentState::getInstance()->setX(0);
+      }
+    }
+    else
+    {
+      delay(500);
+      goodConsecutiveHomings = 0;
+    }
+  }
+
+  if (LOGGING)
+  {
+    CurrentState::getInstance()->print();
+  }
+
+  return 0;
 }
-

src/F11Handler.h

@@ -12,19 +12,20 @@
 #include "CurrentState.h"
 #include "pins.h"
 #include "Config.h"
-#include "StepperControl.h"
+#include "Movement.h"
 
-class F11Handler : public GCodeHandler {
+class F11Handler : public GCodeHandler
+{
 public:
-        static F11Handler* getInstance();
-        int execute(Command*);
+  static F11Handler *getInstance();
+  int execute(Command *);
+
 private:
-        F11Handler();
-        F11Handler(F11Handler const&);
-        void operator=(F11Handler const&);
-        long adjustStepAmount(long);
-        long getNumberOfSteps(double, double);
+  F11Handler();
+  F11Handler(F11Handler const &);
+  void operator=(F11Handler const &);
+  long adjustStepAmount(long);
+  long getNumberOfSteps(double, double);
 };
 
 #endif /* F11HANDLER_H_ */
-

src/F12Handler.cpp

@@ -8,31 +8,127 @@
 
 #include "F12Handler.h"
 
+static F12Handler *instance;
 
-static F12Handler* instance;
+F12Handler *F12Handler::getInstance()
+{
+  if (!instance)
+  {
+    instance = new F12Handler();
+  };
+  return instance;
+};
 
-F12Handler * F12Handler::getInstance() {
-        if (!instance) {
-                instance = new F12Handler();
-        };
-        return instance;
-}
-;
-
-F12Handler::F12Handler() {
+F12Handler::F12Handler()
+{
 }
 
-int F12Handler::execute(Command* command) {
+int F12Handler::execute(Command *command)
+{
 
-        if (LOGGING) {
-                Serial.print("R99 HOME Y\r\n");
-        }
+  if (LOGGING)
+  {
+    Serial.print("R99 HOME Y\r\n");
+  }
 
-        StepperControl::getInstance()->moveToCoords(0,0,0, 0,0,0, false, true, false);
+  int homeIsUp = ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_Y);
+  int stepsPerMM = ParameterList::getInstance()->getValue(MOVEMENT_STEP_PER_MM_Y);
+  int missedStepsMax = ParameterList::getInstance()->getValue(ENCODER_MISSED_STEPS_MAX_Y);
 
-        if (LOGGING) {
-                CurrentState::getInstance()->print();
-        }
-        return 0;
+  #if defined(FARMDUINO_EXP_V20)
+    missedStepsMax += ParameterList::getInstance()->getValue(ENCODER_MISSED_STEPS_DECAY_Y);
+  #endif
+
+  if (stepsPerMM <= 0)
+  {
+    missedStepsMax = 0;
+    stepsPerMM = 1;
+  }
+
+  int A = 10; // move away coordinates
+  int execution;
+  bool emergencyStop;
+  bool homingComplete = false;
+
+  if (homeIsUp == 1)
+  {
+    A = -A;
+  }
+
+  int stepNr;
+
+  double X = CurrentState::getInstance()->getX() / (float)ParameterList::getInstance()->getValue(MOVEMENT_STEP_PER_MM_X);
+  double Y = CurrentState::getInstance()->getY() / (float)ParameterList::getInstance()->getValue(MOVEMENT_STEP_PER_MM_Y);
+  double Z = CurrentState::getInstance()->getZ() / (float)ParameterList::getInstance()->getValue(MOVEMENT_STEP_PER_MM_Z);
+
+  bool firstMove = true;
+  int goodConsecutiveHomings = 0;
+
+  // After the first home, keep moving away and home back
+  // until there is no deviation in positions
+  while (!homingComplete)
+  {
+
+    if (firstMove)
+    {
+      firstMove = false;
+
+      // Move to home position. s
+      Movement::getInstance()->moveToCoords(X, 0, Z, 0, 0, 0, false, false, false);
+      //execution = CurrentState::getInstance()->getLastError();
+      execution = 0;
+      emergencyStop = CurrentState::getInstance()->isEmergencyStop();
+      if (emergencyStop || execution != 0) { homingComplete = true; }
+    }
+
+    // Move away from the home position
+    //posBeforeVerify = CurrentState::getInstance()->getY();
+    Movement::getInstance()->moveToCoords(X, A, Z, 0, 0, 0, false, false, false);
+    //execution = CurrentState::getInstance()->getLastError();
+    execution = 0;
+    emergencyStop = CurrentState::getInstance()->isEmergencyStop();
+    if (emergencyStop || execution != 0) { break; }
+
+    // Home again
+    Movement::getInstance()->moveToCoords(X, 0, Z, 0, 0, 0, false, true, false);
+    //execution = CurrentState::getInstance()->getLastError();
+    execution = 0;
+    emergencyStop = CurrentState::getInstance()->isEmergencyStop();
+    if (emergencyStop || execution != 0) { break; }
+
+    Serial.print("R99 homing displaced");
+    Serial.print(" X ");
+    Serial.print(CurrentState::getInstance()->getHomeMissedStepsXscaled());
+    Serial.print(" Y ");
+    Serial.print(CurrentState::getInstance()->getHomeMissedStepsYscaled());
+    Serial.print(" Z ");
+    Serial.print(CurrentState::getInstance()->getHomeMissedStepsZscaled());
+    Serial.print(" M ");
+    Serial.print(missedStepsMax);
+    Serial.print(" M ");
+    Serial.print(stepsPerMM);
+    Serial.print("\r\n");
+
+    // Compare postition before and after verify homing, accounting for missed steps detecting stall
+    if (CurrentState::getInstance()->getHomeMissedStepsYscaled() <= (20 + (missedStepsMax * 3) / stepsPerMM))
+    {
+      goodConsecutiveHomings++;
+      if (goodConsecutiveHomings >= 3)
+      {
+        homingComplete = true;
+        CurrentState::getInstance()->setY(0);
+      }      
+    }
+    else
+    {
+      delay(500);
+      goodConsecutiveHomings = 0;
+    }
+  }
+
+  if (LOGGING)
+  {
+    CurrentState::getInstance()->print();
+  }
+  return 0;
 }
-

src/F12Handler.h

@@ -12,19 +12,20 @@
 #include "CurrentState.h"
 #include "pins.h"
 #include "Config.h"
-#include "StepperControl.h"
+#include "Movement.h"
 
-class F12Handler : public GCodeHandler {
+class F12Handler : public GCodeHandler
+{
 public:
-        static F12Handler* getInstance();
-        int execute(Command*);
+  static F12Handler *getInstance();
+  int execute(Command *);
+
 private:
-        F12Handler();
-        F12Handler(F12Handler const&);
-        void operator=(F12Handler const&);
-        long adjustStepAmount(long);
-        long getNumberOfSteps(double, double);
+  F12Handler();
+  F12Handler(F12Handler const &);
+  void operator=(F12Handler const &);
+  long adjustStepAmount(long);
+  long getNumberOfSteps(double, double);
 };
 
 #endif /* F12HANDLER_H_ */
-

src/F13Handler.cpp

@@ -8,31 +8,131 @@
 
 #include "F13Handler.h"
 
+static F13Handler *instance;
 
-static F13Handler* instance;
+F13Handler *F13Handler::getInstance()
+{
+  if (!instance)
+  {
+    instance = new F13Handler();
+  };
+  return instance;
+};
 
-F13Handler * F13Handler::getInstance() {
-        if (!instance) {
-                instance = new F13Handler();
-        };
-        return instance;
-}
-;
-
-F13Handler::F13Handler() {
+F13Handler::F13Handler()
+{
 }
 
-int F13Handler::execute(Command* command) {
+int F13Handler::execute(Command *command)
+{
 
-        if (LOGGING) {
-                Serial.print("R99 HOME Z\r\n");
-        }
+  if (LOGGING)
+  {
+    Serial.print("R99 HOME Z\r\n");
+  }
 
-        StepperControl::getInstance()->moveToCoords(0,0,0, 0,0,0, false, false, true);
+  Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, false, true);
 
-        if (LOGGING) {
-                CurrentState::getInstance()->print();
-        }
-        return 0;
+  int homeIsUp = ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_Z);
+  int stepsPerMM = ParameterList::getInstance()->getValue(MOVEMENT_STEP_PER_MM_Z);
+  int missedStepsMax = ParameterList::getInstance()->getValue(ENCODER_MISSED_STEPS_MAX_Z);
+
+  #if defined(FARMDUINO_EXP_V20)
+    missedStepsMax += ParameterList::getInstance()->getValue(ENCODER_MISSED_STEPS_DECAY_Z);
+  #endif
+
+
+  if (stepsPerMM <= 0)
+  {
+    missedStepsMax = 0;
+    stepsPerMM = 1;
+  }
+
+  int A = 10; // move away coordinates
+  int execution;
+  bool emergencyStop;
+
+  bool homingComplete = false;
+
+  if (homeIsUp == 1)
+  {
+    A = -A;
+  }
+
+  int stepNr;
+
+  double X = CurrentState::getInstance()->getX() / (float)ParameterList::getInstance()->getValue(MOVEMENT_STEP_PER_MM_X);
+  double Y = CurrentState::getInstance()->getY() / (float)ParameterList::getInstance()->getValue(MOVEMENT_STEP_PER_MM_Y);
+  double Z = CurrentState::getInstance()->getZ() / (float)ParameterList::getInstance()->getValue(MOVEMENT_STEP_PER_MM_Z);
+
+  bool firstMove = true;
+  int goodConsecutiveHomings = 0;
+
+  // After the first home, keep moving away and home back
+  // until there is no deviation in positions
+  while (!homingComplete)
+  {
+    if (firstMove)
+    {
+      firstMove = false;
+
+      // Move to home position. 
+      Movement::getInstance()->moveToCoords(X, Y, 0, 0, 0, 0, false, false, false);
+      //execution = CurrentState::getInstance()->getLastError();
+      execution = 0;
+      emergencyStop = CurrentState::getInstance()->isEmergencyStop();
+      if (emergencyStop || execution != 0) { homingComplete = true; }
+    }
+
+    // Move away from the home position
+    Movement::getInstance()->moveToCoords(X, Y, A, 0, 0, 0, false, false, false);
+    //execution = CurrentState::getInstance()->getLastError();
+    execution = 0;
+    emergencyStop = CurrentState::getInstance()->isEmergencyStop();
+    if (emergencyStop || execution != 0) { break; }
+
+    // Home again
+    Movement::getInstance()->moveToCoords(X, Y, 0, 0, 0, 0, false, false, true);
+    //execution = CurrentState::getInstance()->getLastError();
+    execution = 0;
+    emergencyStop = CurrentState::getInstance()->isEmergencyStop();
+    if (emergencyStop || execution != 0) { break; }
+
+    Serial.print("R99 homing displaced");
+    Serial.print(" X ");
+    Serial.print(CurrentState::getInstance()->getHomeMissedStepsX());
+    Serial.print(" / ");
+    Serial.print(CurrentState::getInstance()->getHomeMissedStepsXscaled());
+    Serial.print(" Y ");
+    Serial.print(CurrentState::getInstance()->getHomeMissedStepsY());
+    Serial.print(" / ");
+    Serial.print(CurrentState::getInstance()->getHomeMissedStepsYscaled());
+    Serial.print(" Z ");
+    Serial.print(CurrentState::getInstance()->getHomeMissedStepsZ());
+    Serial.print(" / ");
+    Serial.print(CurrentState::getInstance()->getHomeMissedStepsZscaled());
+    Serial.print("\r\n");
+
+    // Compare postition before and after verify homing
+    if (CurrentState::getInstance()->getHomeMissedStepsZscaled() < (20 + (missedStepsMax * 3) / stepsPerMM))
+    {
+      goodConsecutiveHomings++;
+      if (goodConsecutiveHomings >= 3)
+      {
+        homingComplete = true;
+        CurrentState::getInstance()->setZ(0);
+      }
+    }
+    else
+    {
+      delay(500);
+      goodConsecutiveHomings = 0;
+    }
+  }
+
+  if (LOGGING)
+  {
+    CurrentState::getInstance()->print();
+  }
+  return 0;
 }
-

src/F13Handler.h

@@ -12,19 +12,20 @@
 #include "CurrentState.h"
 #include "pins.h"
 #include "Config.h"
-#include "StepperControl.h"
+#include "Movement.h"
 
-class F13Handler : public GCodeHandler {
+class F13Handler : public GCodeHandler
+{
 public:
-        static F13Handler* getInstance();
-        int execute(Command*);
+  static F13Handler *getInstance();
+  int execute(Command *);
+
 private:
-        F13Handler();
-        F13Handler(F13Handler const&);
-        void operator=(F13Handler const&);
-        long adjustStepAmount(long);
-        long getNumberOfSteps(double, double);
+  F13Handler();
+  F13Handler(F13Handler const &);
+  void operator=(F13Handler const &);
+  long adjustStepAmount(long);
+  long getNumberOfSteps(double, double);
 };
 
 #endif /* F13HANDLER_H_ */
-

src/F14Handler.cpp

@@ -8,41 +8,43 @@
 
 #include "F14Handler.h"
 
+static F14Handler *instance;
 
-static F14Handler* instance;
+F14Handler *F14Handler::getInstance()
+{
+  if (!instance)
+  {
+    instance = new F14Handler();
+  };
+  return instance;
+};
 
-F14Handler * F14Handler::getInstance() {
-        if (!instance) {
-                instance = new F14Handler();
-        };
-        return instance;
-}
-;
-
-F14Handler::F14Handler() {
+F14Handler::F14Handler()
+{
 }
 
-int F14Handler::execute(Command* command) {
+int F14Handler::execute(Command *command)
+{
 
-	int ret = 0;
+  int ret = 0;
 
-        if (LOGGING) {
-                Serial.print("R99 CALIBRATE X\r\n");
-        }
+  if (LOGGING)
+  {
+    Serial.print("R99 CALIBRATE X\r\n");
+  }
 
-        ret = StepperControl::getInstance()->calibrateAxis(0);
+  ret = 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);
 	}
-	*/
+	
 
-        if (LOGGING) {
-                CurrentState::getInstance()->print();
-        }
+  if (LOGGING)
+  {
+    CurrentState::getInstance()->print();
+  }
 
-        return 0;
+  return 0;
 }
-
-

src/F14Handler.h

@@ -13,20 +13,20 @@
 #include "CurrentState.h"
 #include "pins.h"
 #include "Config.h"
-#include "StepperControl.h"
+#include "Movement.h"
 
-class F14Handler : public GCodeHandler {
+class F14Handler : public GCodeHandler
+{
 public:
-        static F14Handler* getInstance();
-        int execute(Command*);
+  static F14Handler *getInstance();
+  int execute(Command *);
+
 private:
-        F14Handler();
-        F14Handler(F14Handler const&);
-        void operator=(F14Handler const&);
-        long adjustStepAmount(long);
-        long getNumberOfSteps(double, double);
+  F14Handler();
+  F14Handler(F14Handler const &);
+  void operator=(F14Handler const &);
+  long adjustStepAmount(long);
+  long getNumberOfSteps(double, double);
 };
 
 #endif /* F14HANDLER_H_ */
-
-

src/F15Handler.cpp

@@ -8,31 +8,40 @@
 
 #include "F15Handler.h"
 
+static F15Handler *instance;
 
-static F15Handler* instance;
+F15Handler *F15Handler::getInstance()
+{
+  if (!instance)
+  {
+    instance = new F15Handler();
+  };
+  return instance;
+};
 
-F15Handler * F15Handler::getInstance() {
-        if (!instance) {
-                instance = new F15Handler();
-        };
-        return instance;
-}
-;
-
-F15Handler::F15Handler() {
+F15Handler::F15Handler()
+{
 }
 
-int F15Handler::execute(Command* command) {
+int F15Handler::execute(Command *command)
+{
+  int ret = 0;
 
-        if (LOGGING) {
-                Serial.print("R99 HOME Z\r\n");
-        }
+  if (LOGGING)
+  {
+    Serial.print("R99 HOME Z\r\n");
+  }
 
-	StepperControl::getInstance()->calibrateAxis(1);
+  ret = Movement::getInstance()->calibrateAxis(1);
 
-        if (LOGGING) {
-                CurrentState::getInstance()->print();
-        }
+  if (ret == 0) {
+    Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, true, false);
+  }
 
-        return 0;
+  if (LOGGING)
+  {
+    CurrentState::getInstance()->print();
+  }
+
+  return 0;
 }

src/F15Handler.h

@@ -13,20 +13,20 @@
 #include "CurrentState.h"
 #include "pins.h"
 #include "Config.h"
-#include "StepperControl.h"
+#include "Movement.h"
 
-class F15Handler : public GCodeHandler {
+class F15Handler : public GCodeHandler
+{
 public:
-        static F15Handler* getInstance();
-        int execute(Command*);
+  static F15Handler *getInstance();
+  int execute(Command *);
+
 private:
-        F15Handler();
-        F15Handler(F15Handler const&);
-        void operator=(F15Handler const&);
-        long adjustStepAmount(long);
-        long getNumberOfSteps(double, double);
+  F15Handler();
+  F15Handler(F15Handler const &);
+  void operator=(F15Handler const &);
+  long adjustStepAmount(long);
+  long getNumberOfSteps(double, double);
 };
 
 #endif /* F15HANDLER_H_ */
-
-

src/F16Handler.cpp

@@ -8,33 +8,41 @@
 
 #include "F16Handler.h"
 
+static F16Handler *instance;
 
-static F16Handler* instance;
+F16Handler *F16Handler::getInstance()
+{
+  if (!instance)
+  {
+    instance = new F16Handler();
+  };
+  return instance;
+};
 
-F16Handler * F16Handler::getInstance() {
-        if (!instance) {
-                instance = new F16Handler();
-        };
-        return instance;
-}
-;
-
-F16Handler::F16Handler() {
+F16Handler::F16Handler()
+{
 }
 
-int F16Handler::execute(Command* command) {
+int F16Handler::execute(Command *command)
+{
+  int ret = 0;
 
-        if (LOGGING) {
-                Serial.print("R99 HOME Z\r\n");
-        }
+  if (LOGGING)
+  {
+    Serial.print("R99 HOME Z\r\n");
+  }
 
-	StepperControl::getInstance()->calibrateAxis(2);
+  ret = Movement::getInstance()->calibrateAxis(2);
 
-        if (LOGGING) {
-                CurrentState::getInstance()->print();
-        }
+  if (ret == 0) {
+    Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, false, true);
+  }
 
-        return 0;
+
+  if (LOGGING)
+  {
+    CurrentState::getInstance()->print();
+  }
+
+  return 0;
 }
-
-

src/F16Handler.h

@@ -13,20 +13,20 @@
 #include "CurrentState.h"
 #include "pins.h"
 #include "Config.h"
-#include "StepperControl.h"
+#include "Movement.h"
 
-class F16Handler : public GCodeHandler {
+class F16Handler : public GCodeHandler
+{
 public:
-        static F16Handler* getInstance();
-        int execute(Command*);
+  static F16Handler *getInstance();
+  int execute(Command *);
+
 private:
-        F16Handler();
-        F16Handler(F16Handler const&);
-        void operator=(F16Handler const&);
-        long adjustStepAmount(long);
-        long getNumberOfSteps(double, double);
+  F16Handler();
+  F16Handler(F16Handler const &);
+  void operator=(F16Handler const &);
+  long adjustStepAmount(long);
+  long getNumberOfSteps(double, double);
 };
 
 #endif /* F16HANDLER_H_ */
-
-

src/F20Handler.cpp

@@ -7,23 +7,27 @@
 
 #include "F20Handler.h"
 
+static F20Handler *instance;
 
-static F20Handler* instance;
+F20Handler *F20Handler::getInstance()
+{
+  if (!instance)
+  {
+    instance = new F20Handler();
+  };
+  return instance;
+};
 
-F20Handler * F20Handler::getInstance() {
-	if (!instance) {
-		instance = new F20Handler();
-	};
-	return instance;
-}
-;
-
-F20Handler::F20Handler() {
+F20Handler::F20Handler()
+{
 }
 
-int F20Handler::execute(Command* command) {
+int F20Handler::execute(Command *command)
+{
 
-	ParameterList::getInstance()->readAllValues();
+  ParameterList::getInstance()->readAllValues();
+  Serial.print("R20");
+  CurrentState::getInstance()->printQAndNewLine();
 
-	return 1;
+  return 0;
 }

src/F20Handler.h

@@ -12,18 +12,20 @@
 #include "CurrentState.h"
 #include "pins.h"
 #include "Config.h"
-#include "StepperControl.h"
+#include "Movement.h"
 
-class F20Handler : public GCodeHandler {
+class F20Handler : public GCodeHandler
+{
 public:
-	static F20Handler* getInstance();
-	int execute(Command*);
+  static F20Handler *getInstance();
+  int execute(Command *);
+
 private:
-	F20Handler();
-	F20Handler(F20Handler const&);
-	void operator=(F20Handler const&);
-	long adjustStepAmount(long);
-	long getNumberOfSteps(double, double);
+  F20Handler();
+  F20Handler(F20Handler const &);
+  void operator=(F20Handler const &);
+  long adjustStepAmount(long);
+  long getNumberOfSteps(double, double);
 };
 
 #endif /* F20HANDLER_H_ */

src/F21Handler.cpp

@@ -7,23 +7,25 @@
 
 #include "F21Handler.h"
 
+static F21Handler *instance;
 
-static F21Handler* instance;
+F21Handler *F21Handler::getInstance()
+{
+  if (!instance)
+  {
+    instance = new F21Handler();
+  };
+  return instance;
+};
 
-F21Handler * F21Handler::getInstance() {
-	if (!instance) {
-		instance = new F21Handler();
-	};
-	return instance;
-}
-;
-
-F21Handler::F21Handler() {
+F21Handler::F21Handler()
+{
 }
 
-int F21Handler::execute(Command* command) {
+int F21Handler::execute(Command *command)
+{
 
-	ParameterList::getInstance()->readValue(command->getP());
+  ParameterList::getInstance()->readValue(command->getP());
 
-	return 0;
+  return 0;
 }

src/F21Handler.h

@@ -12,19 +12,21 @@
 #include "CurrentState.h"
 #include "pins.h"
 #include "Config.h"
-#include "StepperControl.h"
+#include "Movement.h"
 #include "ParameterList.h"
 
-class F21Handler : public GCodeHandler {
+class F21Handler : public GCodeHandler
+{
 public:
-	static F21Handler* getInstance();
-	int execute(Command*);
+  static F21Handler *getInstance();
+  int execute(Command *);
+
 private:
-	F21Handler();
-	F21Handler(F21Handler const&);
-	void operator=(F21Handler const&);
-	long adjustStepAmount(long);
-	long getNumberOfSteps(double, double);
+  F21Handler();
+  F21Handler(F21Handler const &);
+  void operator=(F21Handler const &);
+  long adjustStepAmount(long);
+  long getNumberOfSteps(double, double);
 };
 
 #endif /* F21HANDLER_H_ */

src/F22Handler.cpp

@@ -7,23 +7,25 @@
 
 #include "F22Handler.h"
 
+static F22Handler *instance;
 
-static F22Handler* instance;
+F22Handler *F22Handler::getInstance()
+{
+  if (!instance)
+  {
+    instance = new F22Handler();
+  };
+  return instance;
+};
 
-F22Handler * F22Handler::getInstance() {
-	if (!instance) {
-		instance = new F22Handler();
-	};
-	return instance;
-}
-;
-
-F22Handler::F22Handler() {
+F22Handler::F22Handler()
+{
 }
 
-int F22Handler::execute(Command* command) {
+int F22Handler::execute(Command *command)
+{
 
-/*
+  /*
 Serial.print("R99");
 Serial.print(" ");
 Serial.print("write value");
@@ -42,7 +44,7 @@ Serial.print(" ");
 Serial.print("\r\n");
 */
 
-	ParameterList::getInstance()->writeValue(command->getP(), command->getV());
+  ParameterList::getInstance()->writeValue(command->getP(), command->getV());
 
-	return 0;
+  return 0;
 }

src/F22Handler.h

@@ -12,19 +12,21 @@
 #include "CurrentState.h"
 #include "pins.h"
 #include "Config.h"
-#include "StepperControl.h"
+#include "Movement.h"
 #include "ParameterList.h"
 
-class F22Handler : public GCodeHandler {
+class F22Handler : public GCodeHandler
+{
 public:
-	static F22Handler* getInstance();
-	int execute(Command*);
+  static F22Handler *getInstance();
+  int execute(Command *);
+
 private:
-	F22Handler();
-	F22Handler(F22Handler const&);
-	void operator=(F22Handler const&);
-	//long adjustStepAmount(long);
-	//long getNumberOfSteps(double, double);
+  F22Handler();
+  F22Handler(F22Handler const &);
+  void operator=(F22Handler const &);
+  //long adjustStepAmount(long);
+  //long getNumberOfSteps(double, double);
 };
 
 #endif /* F22HANDLER_H_ */

src/F31Handler.cpp

@@ -8,25 +8,25 @@
 
 #include "F31Handler.h"
 
+static F31Handler *instance;
 
-static F31Handler* instance;
+F31Handler *F31Handler::getInstance()
+{
+  if (!instance)
+  {
+    instance = new F31Handler();
+  };
+  return instance;
+};
 
-F31Handler * F31Handler::getInstance() {
-        if (!instance) {
-                instance = new F31Handler();
-        };
-        return instance;
-}
-;
-
-F31Handler::F31Handler() {
+F31Handler::F31Handler()
+{
 }
 
-int F31Handler::execute(Command* command) {
+int F31Handler::execute(Command *command)
+{
 
-        StatusList::getInstance()->readValue(command->getP());
+  StatusList::getInstance()->readValue(command->getP());
 
-        return 0;
+  return 0;
 }
-
-

src/F31Handler.h

@@ -13,22 +13,21 @@
 #include "CurrentState.h"
 #include "pins.h"
 #include "Config.h"
-#include "StepperControl.h"
+#include "Movement.h"
 #include "StatusList.h"
 
-class F31Handler : public GCodeHandler {
+class F31Handler : public GCodeHandler
+{
 public:
-        static F31Handler* getInstance();
-        int execute(Command*);
+  static F31Handler *getInstance();
+  int execute(Command *);
+
 private:
-        F31Handler();
-        F31Handler(F31Handler const&);
-        void operator=(F31Handler const&);
-        //long adjustStepAmount(long);
-        //long getNumberOfSteps(double, double);
+  F31Handler();
+  F31Handler(F31Handler const &);
+  void operator=(F31Handler const &);
+  //long adjustStepAmount(long);
+  //long getNumberOfSteps(double, double);
 };
 
 #endif /* F31HANDLER_H_ */
-
-
-

src/F32Handler.cpp

@@ -8,25 +8,25 @@
 
 #include "F32Handler.h"
 
+static F32Handler *instance;
 
-static F32Handler* instance;
+F32Handler *F32Handler::getInstance()
+{
+  if (!instance)
+  {
+    instance = new F32Handler();
+  };
+  return instance;
+};
 
-F32Handler * F32Handler::getInstance() {
-        if (!instance) {
-                instance = new F32Handler();
-        };
-        return instance;
-}
-;
-
-F32Handler::F32Handler() {
+F32Handler::F32Handler()
+{
 }
 
-int F32Handler::execute(Command* command) {
+int F32Handler::execute(Command *command)
+{
 
-        StatusList::getInstance()->readValue(command->getP());
+  StatusList::getInstance()->readValue(command->getP());
 
-        return 0;
+  return 0;
 }
-
-

src/F32Handler.h

@@ -13,22 +13,21 @@
 #include "CurrentState.h"
 #include "pins.h"
 #include "Config.h"
-#include "StepperControl.h"
+#include "Movement.h"
 #include "StatusList.h"
 
-class F32Handler : public GCodeHandler {
+class F32Handler : public GCodeHandler
+{
 public:
-        static F32Handler* getInstance();
-        int execute(Command*);
+  static F32Handler *getInstance();
+  int execute(Command *);
+
 private:
-        F32Handler();
-        F32Handler(F32Handler const&);
-        void operator=(F32Handler const&);
-        long adjustStepAmount(long);
-        long getNumberOfSteps(double, double);
+  F32Handler();
+  F32Handler(F32Handler const &);
+  void operator=(F32Handler const &);
+  long adjustStepAmount(long);
+  long getNumberOfSteps(double, double);
 };
 
 #endif /* F32HANDLER_H_ */
-
-
-

src/F41Handler.cpp

@@ -9,23 +9,25 @@
 
 #include "F41Handler.h"
 
+static F41Handler *instance;
 
-static F41Handler* instance;
+F41Handler *F41Handler::getInstance()
+{
+  if (!instance)
+  {
+    instance = new F41Handler();
+  };
+  return instance;
+};
 
-F41Handler * F41Handler::getInstance() {
-        if (!instance) {
-                instance = new F41Handler();
-        };
-        return instance;
-}
-;
-
-F41Handler::F41Handler() {
+F41Handler::F41Handler()
+{
 }
 
-int F41Handler::execute(Command* command) {
+int F41Handler::execute(Command *command)
+{
 
-        PinControl::getInstance()->writeValue(command->getP(),command->getV(), command->getM());
+  PinControl::getInstance()->writeValue(command->getP(), command->getV(), command->getM());
 
-        return 0;
+  return 0;
 }

src/F41Handler.h

@@ -14,20 +14,16 @@
 #include "Config.h"
 #include "PinControl.h"
 
-class F41Handler : public GCodeHandler {
+class F41Handler : public GCodeHandler
+{
 public:
-        static F41Handler* getInstance();
-        int execute(Command*);
+  static F41Handler *getInstance();
+  int execute(Command *);
+
 private:
-        F41Handler();
-        F41Handler(F41Handler const&);
-        void operator=(F41Handler const&);
+  F41Handler();
+  F41Handler(F41Handler const &);
+  void operator=(F41Handler const &);
 };
 
 #endif /* F41HANDLER_H_ */
-
-
-
-
-
-

src/F42Handler.cpp

@@ -7,23 +7,25 @@
 
 #include "F42Handler.h"
 
+static F42Handler *instance;
 
-static F42Handler* instance;
+F42Handler *F42Handler::getInstance()
+{
+  if (!instance)
+  {
+    instance = new F42Handler();
+  };
+  return instance;
+};
 
-F42Handler * F42Handler::getInstance() {
-        if (!instance) {
-                instance = new F42Handler();
-        };
-        return instance;
-}
-;
-
-F42Handler::F42Handler() {
+F42Handler::F42Handler()
+{
 }
 
-int F42Handler::execute(Command* command) {
+int F42Handler::execute(Command *command)
+{
 
-        PinControl::getInstance()->readValue(command->getP(), command->getM());
+  PinControl::getInstance()->readValue(command->getP(), command->getM());
 
-        return 0;
+  return 0;
 }

src/F42Handler.h

@@ -14,20 +14,16 @@
 #include "Config.h"
 #include "PinControl.h"
 
-class F42Handler : public GCodeHandler {
+class F42Handler : public GCodeHandler
+{
 public:
-        static F42Handler* getInstance();
-        int execute(Command*);
+  static F42Handler *getInstance();
+  int execute(Command *);
+
 private:
-        F42Handler();
-        F42Handler(F42Handler const&);
-        void operator=(F42Handler const&);
+  F42Handler();
+  F42Handler(F42Handler const &);
+  void operator=(F42Handler const &);
 };
 
 #endif /* F42HANDLER_H_ */
-
-
-
-
-
-

src/F43Handler.cpp

@@ -9,30 +9,25 @@
 
 #include "F43Handler.h"
 
+static F43Handler *instance;
 
-static F43Handler* instance;
+F43Handler *F43Handler::getInstance()
+{
+  if (!instance)
+  {
+    instance = new F43Handler();
+  };
+  return instance;
+};
 
-F43Handler * F43Handler::getInstance() {
-        if (!instance) {
-                instance = new F43Handler();
-        };
-        return instance;
-}
-;
-
-F43Handler::F43Handler() {
+F43Handler::F43Handler()
+{
 }
 
-int F43Handler::execute(Command* command) {
+int F43Handler::execute(Command *command)
+{
 
-        PinControl::getInstance()->setMode(command->getP(),command->getM());
+  PinControl::getInstance()->setMode(command->getP(), command->getM());
 
-        return 0;
+  return 0;
 }
-
-
-
-
-
-
-

src/F43Handler.h

@@ -14,14 +14,16 @@
 #include "Config.h"
 #include "PinControl.h"
 
-class F43Handler : public GCodeHandler {
+class F43Handler : public GCodeHandler
+{
 public:
-        static F43Handler* getInstance();
-        int execute(Command*);
+  static F43Handler *getInstance();
+  int execute(Command *);
+
 private:
-        F43Handler();
-        F43Handler(F43Handler const&);
-        void operator=(F43Handler const&);
+  F43Handler();
+  F43Handler(F43Handler const &);
+  void operator=(F43Handler const &);
 };
 
 #endif /* F43HANDLER_H_ */

src/F44Handler.cpp

@@ -9,23 +9,25 @@
 
 #include "F44Handler.h"
 
+static F44Handler *instance;
 
-static F44Handler* instance;
+F44Handler *F44Handler::getInstance()
+{
+  if (!instance)
+  {
+    instance = new F44Handler();
+  };
+  return instance;
+};
 
-F44Handler * F44Handler::getInstance() {
-        if (!instance) {
-                instance = new F44Handler();
-        };
-        return instance;
-}
-;
-
-F44Handler::F44Handler() {
+F44Handler::F44Handler()
+{
 }
 
-int F44Handler::execute(Command* command) {
+int F44Handler::execute(Command *command)
+{
 
-        PinControl::getInstance()->writePulse(command->getP(),command->getV(),command->getW(),command->getT(), command->getM());
+  PinControl::getInstance()->writePulse(command->getP(), command->getV(), command->getW(), command->getT(), command->getM());
 
-        return 0;
+  return 0;
 }

src/F44Handler.h

@@ -14,17 +14,16 @@
 #include "Config.h"
 #include "PinControl.h"
 
-class F44Handler : public GCodeHandler {
+class F44Handler : public GCodeHandler
+{
 public:
-        static F44Handler* getInstance();
-        int execute(Command*);
+  static F44Handler *getInstance();
+  int execute(Command *);
+
 private:
-        F44Handler();
-        F44Handler(F44Handler const&);
-        void operator=(F44Handler const&);
+  F44Handler();
+  F44Handler(F44Handler const &);
+  void operator=(F44Handler const &);
 };
 
 #endif /* F44HANDLER_H_ */
-
-
-

src/F61Handler.cpp

@@ -9,23 +9,25 @@
 
 #include "F61Handler.h"
 
+static F61Handler *instance;
 
-static F61Handler* instance;
+F61Handler *F61Handler::getInstance()
+{
+  if (!instance)
+  {
+    instance = new F61Handler();
+  };
+  return instance;
+};
 
-F61Handler * F61Handler::getInstance() {
-        if (!instance) {
-                instance = new F61Handler();
-        };
-        return instance;
-}
-;
-
-F61Handler::F61Handler() {
+F61Handler::F61Handler()
+{
 }
 
-int F61Handler::execute(Command* command) {
+int F61Handler::execute(Command *command)
+{
 
-        ServoControl::getInstance()->setAngle(command->getP(),command->getV());
+  ServoControl::getInstance()->setAngle(command->getP(), command->getV());
 
-        return 0;
+  return 0;
 }

src/F61Handler.h

@@ -14,20 +14,16 @@
 #include "Config.h"
 #include "ServoControl.h"
 
-class F61Handler : public GCodeHandler {
+class F61Handler : public GCodeHandler
+{
 public:
-        static F61Handler* getInstance();
-        int execute(Command*);
+  static F61Handler *getInstance();
+  int execute(Command *);
+
 private:
-        F61Handler();
-        F61Handler(F61Handler const&);
-        void operator=(F61Handler const&);
+  F61Handler();
+  F61Handler(F61Handler const &);
+  void operator=(F61Handler const &);
 };
 
 #endif /* F61HANDLER_H_ */
-
-
-
-
-
-

src/F81Handler.cpp

@@ -9,40 +9,40 @@
 
 #include "F81Handler.h"
 
+static F81Handler *instance;
 
-static F81Handler* instance;
+F81Handler *F81Handler::getInstance()
+{
+  if (!instance)
+  {
+    instance = new F81Handler();
+  };
+  return instance;
+};
 
-F81Handler * F81Handler::getInstance() {
-        if (!instance) {
-                instance = new F81Handler();
-        };
-        return instance;
-}
-;
-
-F81Handler::F81Handler() {
+F81Handler::F81Handler()
+{
 }
 
-int F81Handler::execute(Command* command) {
+int F81Handler::execute(Command *command)
+{
 
-Serial.print("home\r\n");
+  Serial.print("home\r\n");
 
-        if (LOGGING) {
-                Serial.print("R99 Report end stops\r\n");
-        }
+  if (LOGGING)
+  {
+    Serial.print("R99 Report end stops\r\n");
+  }
 
-	// Report back the end stops
-        CurrentState::getInstance()->storeEndStops();
-        CurrentState::getInstance()->printEndStops();
+  // Report back the end stops
+  CurrentState::getInstance()->storeEndStops();
+  CurrentState::getInstance()->printEndStops();
 
-	// Also report back some selected pin numbers: 8, 9, 10, 13
-	PinControl::getInstance()->readValue( 8, 0);
-	PinControl::getInstance()->readValue( 9, 0);
-	PinControl::getInstance()->readValue(10, 0);
-	PinControl::getInstance()->readValue(13, 0);
+  // Also report back some selected pin numbers: 8, 9, 10, 13
+  PinControl::getInstance()->readValue(8, 0);
+  PinControl::getInstance()->readValue(9, 0);
+  PinControl::getInstance()->readValue(10, 0);
+  PinControl::getInstance()->readValue(13, 0);
 
-
-
-        return 0;
+  return 0;
 }
-

src/F81Handler.h

@@ -12,20 +12,21 @@
 #include "CurrentState.h"
 #include "pins.h"
 #include "Config.h"
-#include "StepperControl.h"
+#include "Movement.h"
 #include "PinControl.h"
 
-class F81Handler : public GCodeHandler {
+class F81Handler : public GCodeHandler
+{
 public:
-        static F81Handler* getInstance();
-        int execute(Command*);
+  static F81Handler *getInstance();
+  int execute(Command *);
+
 private:
-        F81Handler();
-        F81Handler(F81Handler const&);
-        void operator=(F81Handler const&);
-        long adjustStepAmount(long);
-        long getNumberOfSteps(double, double);
+  F81Handler();
+  F81Handler(F81Handler const &);
+  void operator=(F81Handler const &);
+  long adjustStepAmount(long);
+  long getNumberOfSteps(double, double);
 };
 
 #endif /* F81HANDLER_H_ */
-

src/F82Handler.cpp

@@ -9,30 +9,30 @@
 
 #include "F82Handler.h"
 
+static F82Handler *instance;
 
-static F82Handler* instance;
+F82Handler *F82Handler::getInstance()
+{
+  if (!instance)
+  {
+    instance = new F82Handler();
+  };
+  return instance;
+};
 
-F82Handler * F82Handler::getInstance() {
-        if (!instance) {
-                instance = new F82Handler();
-        };
-        return instance;
-}
-;
-
-F82Handler::F82Handler() {
+F82Handler::F82Handler()
+{
 }
 
-int F82Handler::execute(Command* command) {
+int F82Handler::execute(Command *command)
+{
 
-        if (LOGGING) {
-                Serial.print("R99 Report current position\r\n");
-        }
+  if (LOGGING)
+  {
+    Serial.print("R99 Report current position\r\n");
+  }
 
-        CurrentState::getInstance()->printPosition();
+  CurrentState::getInstance()->printPosition();
 
-        return 0;
+  return 0;
 }
-
-
-

src/F82Handler.h

@@ -12,19 +12,20 @@
 #include "CurrentState.h"
 #include "pins.h"
 #include "Config.h"
-#include "StepperControl.h"
+#include "Movement.h"
 
-class F82Handler : public GCodeHandler {
+class F82Handler : public GCodeHandler
+{
 public:
-        static F82Handler* getInstance();
-        int execute(Command*);
+  static F82Handler *getInstance();
+  int execute(Command *);
+
 private:
-        F82Handler();
-        F82Handler(F82Handler const&);
-        void operator=(F82Handler const&);
-        long adjustStepAmount(long);
-        long getNumberOfSteps(double, double);
+  F82Handler();
+  F82Handler(F82Handler const &);
+  void operator=(F82Handler const &);
+  long adjustStepAmount(long);
+  long getNumberOfSteps(double, double);
 };
 
 #endif /* F82HANDLER_H_ */
-

src/F83Handler.cpp

@@ -9,33 +9,35 @@
 
 #include "F83Handler.h"
 
+static F83Handler *instance;
 
-static F83Handler* instance;
+F83Handler *F83Handler::getInstance()
+{
+  if (!instance)
+  {
+    instance = new F83Handler();
+  };
+  return instance;
+};
 
-F83Handler * F83Handler::getInstance() {
-        if (!instance) {
-                instance = new F83Handler();
-        };
-        return instance;
-}
-;
-
-F83Handler::F83Handler() {
+F83Handler::F83Handler()
+{
 }
 
-int F83Handler::execute(Command* command) {
+int F83Handler::execute(Command *command)
+{
 
-        if (LOGGING) {
-                Serial.print("R99 Report server version\r\n");
-        }
+  if (LOGGING)
+  {
+    Serial.print("R99 Report server version\r\n");
+  }
 
-	Serial.print("R83 ");
-	Serial.print(SOFTWARE_VERSION);
-	//Serial.print("\r\n");
-	CurrentState::getInstance()->printQAndNewLine();
+  Serial.print("R83 ");
+  Serial.print(SOFTWARE_VERSION);
+  Serial.print(SOFTWARE_VERSION_SUFFIX);
+  Serial.print(SOFTWARE_COMMIT);
+  //Serial.print("\r\n");
+  CurrentState::getInstance()->printQAndNewLine();
 
-        return 0;
+  return 0;
 }
-
-
-

src/F83Handler.h

@@ -12,19 +12,20 @@
 #include "CurrentState.h"
 #include "pins.h"
 #include "Config.h"
-#include "StepperControl.h"
+#include "Movement.h"
 
-class F83Handler : public GCodeHandler {
+class F83Handler : public GCodeHandler
+{
 public:
-        static F83Handler* getInstance();
-        int execute(Command*);
+  static F83Handler *getInstance();
+  int execute(Command *);
+
 private:
-        F83Handler();
-        F83Handler(F83Handler const&);
-        void operator=(F83Handler const&);
-        long adjustStepAmount(long);
-        long getNumberOfSteps(double, double);
+  F83Handler();
+  F83Handler(F83Handler const &);
+  void operator=(F83Handler const &);
+  long adjustStepAmount(long);
+  long getNumberOfSteps(double, double);
 };
 
 #endif /* F83HANDLER_H_ */
-

src/F84Handler.cpp

@@ -0,0 +1,49 @@
+/*
+ * F84Handler.cpp
+ *
+ *  Created on: 2017/04/13
+ *      Author: Rick Carlino
+ */
+
+#include "F84Handler.h"
+
+static F84Handler *instance;
+const double DO_RESET = 1;
+
+F84Handler *F84Handler::getInstance()
+{
+  if (!instance)
+  {
+    instance = new F84Handler();
+  };
+  return instance;
+};
+
+F84Handler::F84Handler()
+{
+}
+
+int F84Handler::execute(Command *command)
+{
+
+  if (command->getX() == DO_RESET)
+  {
+    Serial.print("R99 Will zero X");
+    Movement::getInstance()->setPositionX(0);
+  }
+
+  if (command->getY() == DO_RESET)
+  {
+    Serial.print("R99 Will zero Y");
+    Movement::getInstance()->setPositionY(0);
+  }
+
+  if (command->getZ() == DO_RESET)
+  {
+    Serial.print("R99 Will zero Z");
+    Movement::getInstance()->setPositionZ(0);
+  }
+
+  CurrentState::getInstance()->printQAndNewLine();
+  return 0;
+}

src/F84Handler.h

@@ -0,0 +1,31 @@
+/*
+ * F84Handler.h
+ *
+ *  Created on: 2017/4/13
+ *      Author: Rick Carlino
+ */
+
+#ifndef F84HANDLER_H_
+#define F84HANDLER_H_
+#include "GCodeHandler.h"
+#include "Config.h"
+#include "CurrentState.h"
+#include "pins.h"
+#include "Config.h"
+#include "Movement.h"
+
+class F84Handler : public GCodeHandler
+{
+public:
+  static F84Handler *getInstance();
+  int execute(Command *);
+
+private:
+  F84Handler();
+  F84Handler(F84Handler const &);
+  void operator=(F84Handler const &);
+  long adjustStepAmount(long);
+  long getNumberOfSteps(double, double);
+};
+
+#endif /* F84HANDLER_H_ */

src/G00Handler.cpp

@@ -7,51 +7,50 @@
 
 #include "G00Handler.h"
 
+static G00Handler *instance;
 
-static G00Handler* instance;
+G00Handler *G00Handler::getInstance()
+{
+  if (!instance)
+  {
+    instance = new G00Handler();
+  };
+  return instance;
+};
 
-G00Handler * G00Handler::getInstance() {
-	if (!instance) {
-		instance = new G00Handler();
-	};
-	return instance;
-}
-;
-
-G00Handler::G00Handler() {
+G00Handler::G00Handler()
+{
 }
 
-int G00Handler::execute(Command* command) {
+int G00Handler::execute(Command *command)
+{
+  
+ /* 	Serial.print("G00 was here\r\n");
 
+  	Serial.print("R99");
+  	Serial.print(" X ");
+  	Serial.print(command->getX());
+  	Serial.print(" Y ");
+  	Serial.print(command->getY());
+  	Serial.print(" Z ");
+  	Serial.print(command->getZ());
+  	Serial.print(" A ");
+  	Serial.print(command->getA());
+  	Serial.print(" B ");
+  	Serial.print(command->getB());
+  	Serial.print(" C ");
+  	Serial.print(command->getC());
+  	Serial.print("\r\n");*/
+  
 
-//	Serial.print("G00 was here\r\n");
+  Movement::getInstance()->moveToCoords(
+      command->getX(), command->getY(), command->getZ(),
+      command->getA(), command->getB(), command->getC(),
+      false, false, false);
 
-
-//	Serial.print("R99");
-//	Serial.print(" X ");
-//	Serial.print(command->getX());
-//	Serial.print(" Y ");
-//	Serial.print(command->getY());
-//	Serial.print(" Z ");
-//	Serial.print(command->getZ());
-//	Serial.print(" A ");
-//	Serial.print(command->getA());
-//	Serial.print(" B ");
-//	Serial.print(command->getB());
-//	Serial.print(" C ");
-//	Serial.print(command->getC());
-//	Serial.print("\r\n");
-
-
-	StepperControl::getInstance()->moveToCoords
-		(
-			command->getX(), command->getY(), command->getZ(),
-			command->getA(), command->getB(), command->getC(),
-			false, false, false
-		);
-
-	if (LOGGING) {
-		CurrentState::getInstance()->print();
-	}
-	return 0;
+  if (LOGGING)
+  {
+    CurrentState::getInstance()->print();
+  }
+  return 0;
 }

src/G00Handler.h

@@ -12,18 +12,20 @@
 #include "CurrentState.h"
 #include "pins.h"
 #include "Config.h"
-#include "StepperControl.h"
+#include "Movement.h"
 
-class G00Handler : public GCodeHandler {
+class G00Handler : public GCodeHandler
+{
 public:
-	static G00Handler* getInstance();
-	int execute(Command*);
+  static G00Handler *getInstance();
+  int execute(Command *);
+
 private:
-	G00Handler();
-	G00Handler(G00Handler const&);
-	void operator=(G00Handler const&);
-	long adjustStepAmount(long);
-	long getNumberOfSteps(double, double);
+  G00Handler();
+  G00Handler(G00Handler const &);
+  void operator=(G00Handler const &);
+  long adjustStepAmount(long);
+  long getNumberOfSteps(double, double);
 };
 
 #endif /* G00HANDLER_H_ */

src/G28Handler.cpp

@@ -7,29 +7,45 @@
 
 #include "G28Handler.h"
 
+static G28Handler *instance;
 
-static G28Handler* instance;
+G28Handler *G28Handler::getInstance()
+{
+  if (!instance)
+  {
+    instance = new G28Handler();
+  };
+  return instance;
+};
 
-G28Handler * G28Handler::getInstance() {
-	if (!instance) {
-		instance = new G28Handler();
-	};
-	return instance;
-}
-;
-
-G28Handler::G28Handler() {
+G28Handler::G28Handler()
+{
 }
 
-int G28Handler::execute(Command* command) {
+int G28Handler::execute(Command *command)
+{
 
-//Serial.print("home\r\n");
+  //Serial.print("home\r\n");
 
-	StepperControl::getInstance()->moveToCoords(0,0,0, 0,0,0, false, false, true);
-	StepperControl::getInstance()->moveToCoords(0,0,0, 0,0,0, true,  true,  false);
-	//StepperControl::getInstance()->moveAbsoluteConstant(0,0,0,HOME_MOVEMENT_SPEED_S_P_S,true);
-	if (LOGGING) {
-		CurrentState::getInstance()->print();
-	}
-	return 0;
+  long stepsPerMm[2] = {0, 0};
+  stepsPerMm[0] = ParameterList::getInstance()->getValue(MOVEMENT_STEP_PER_MM_X);
+  stepsPerMm[1] = ParameterList::getInstance()->getValue(MOVEMENT_STEP_PER_MM_Y);
+
+  long sourcePoint[2] = {0.0, 0.0};
+  sourcePoint[0] = CurrentState::getInstance()->getX();
+  sourcePoint[1] = CurrentState::getInstance()->getY();
+
+  double currentPoint[2] =  {0.0, 0.0};
+  currentPoint[0] = sourcePoint[0] / (float)stepsPerMm[0];
+  currentPoint[1] = sourcePoint[1] / (float)stepsPerMm[1];
+
+  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)
+  {
+    CurrentState::getInstance()->print();
+  }
+  return 0;
 }

src/G28Handler.h

@@ -12,18 +12,20 @@
 #include "CurrentState.h"
 #include "pins.h"
 #include "Config.h"
-#include "StepperControl.h"
+#include "Movement.h"
 
-class G28Handler : public GCodeHandler {
+class G28Handler : public GCodeHandler
+{
 public:
-	static G28Handler* getInstance();
-	int execute(Command*);
+  static G28Handler *getInstance();
+  int execute(Command *);
+
 private:
-	G28Handler();
-	G28Handler(G28Handler const&);
-	void operator=(G28Handler const&);
-	long adjustStepAmount(long);
-	long getNumberOfSteps(double, double);
+  G28Handler();
+  G28Handler(G28Handler const &);
+  void operator=(G28Handler const &);
+  long adjustStepAmount(long);
+  long getNumberOfSteps(double, double);
 };
 
 #endif /* G28HANDLER_H_ */

src/GCodeHandler.cpp

@@ -7,13 +7,15 @@
 
 #include "GCodeHandler.h"
 
-GCodeHandler::GCodeHandler() {
-
+GCodeHandler::GCodeHandler()
+{
 }
 
-GCodeHandler::~GCodeHandler() {
+GCodeHandler::~GCodeHandler()
+{
 }
 
-int GCodeHandler::execute(Command*) {
-	return -1;
+int GCodeHandler::execute(Command *)
+{
+  return -1;
 }

src/GCodeHandler.h

@@ -9,11 +9,12 @@
 #define GCODEHANDLER_H_
 #include "Command.h"
 
-class GCodeHandler {
+class GCodeHandler
+{
 public:
-	GCodeHandler();
-	virtual ~GCodeHandler();
-	virtual int execute(Command*);
+  GCodeHandler();
+  virtual ~GCodeHandler();
+  virtual int execute(Command *);
 };
 
 #endif /* GCODEHANDLER_H_ */

src/GCodeProcessor.cpp

@@ -9,155 +9,310 @@
 #include "GCodeProcessor.h"
 #include "CurrentState.h"
 
-GCodeProcessor::GCodeProcessor() {
-
+GCodeProcessor::GCodeProcessor()
+{
 }
 
-GCodeProcessor::~GCodeProcessor() {
+GCodeProcessor::~GCodeProcessor()
+{
 }
 
-void GCodeProcessor::printCommandLog(Command* command) {
-	Serial.print("command == NULL: ");
-	Serial.println("\r\n");
+void GCodeProcessor::printCommandLog(Command *command)
+{
+  Serial.print("command == NULL: ");
+  Serial.println("\r\n");
 }
 
-int GCodeProcessor::execute(Command* command) {
+int GCodeProcessor::execute(Command *command)
+{
 
-	int execution = 0;
+  int execution = 0;
+  bool isMovement = false;
+
+  bool emergencyStop = false;
+  emergencyStop = CurrentState::getInstance()->isEmergencyStop();
+
+  int attempt = 0;
+  int maximumAttempts = ParameterList::getInstance()->getValue(PARAM_MOV_NR_RETRY);
+
+  long Q = command->getQ();
+  CurrentState::getInstance()->setQ(Q);
+
+  if
+  (
+    command->getCodeEnum() == G00 ||
+    command->getCodeEnum() == G01 ||
+    command->getCodeEnum() == F11 ||
+    command->getCodeEnum() == F12 ||
+    command->getCodeEnum() == F13 ||
+    command->getCodeEnum() == F14 ||
+    command->getCodeEnum() == F15 ||
+    command->getCodeEnum() == F16
+  )
+  {
+    isMovement = true;
+  }
 
 
-	long Q = command->getQ();
-	CurrentState::getInstance()->setQ(Q);
 
-	if(command == NULL) {
-		if(LOGGING) {
-			printCommandLog(command);
-		}
-		return -1;
-	}
+  //Only allow reset of emergency stop when emergency stop is engaged
 
-	if(command->getCodeEnum() == CODE_UNDEFINED) {
-		if(LOGGING) {
-			printCommandLog(command);
-		}
-		return -1;
-	}
+  if (emergencyStop)
+  {
+    if (!(
+      command->getCodeEnum() == F09 ||
+      command->getCodeEnum() == F20 ||
+      command->getCodeEnum() == F21 ||
+      command->getCodeEnum() == F22 ||
+      command->getCodeEnum() == F81 ||
+      command->getCodeEnum() == F82 ||
+      command->getCodeEnum() == F83 ))
+    {
 
-	GCodeHandler* handler = getGCodeHandler(command->getCodeEnum());
+    Serial.print(COMM_REPORT_EMERGENCY_STOP);
+    CurrentState::getInstance()->printQAndNewLine();
+    return -1;
+    }
+  }
 
-	if(handler == NULL) {
-		Serial.println("R99 handler == NULL\r\n");
-		return -1;
-	}
+  // Do not execute the command when the config complete parameter is not
+  // set by the raspberry pi and it's asked to do a move command
 
-	Serial.print(COMM_REPORT_CMD_START);
-	CurrentState::getInstance()->printQAndNewLine();
+  if (ParameterList::getInstance()->getValue(PARAM_CONFIG_OK) != 1)
+  {
+    if (isMovement)
+    {
+      Serial.print(COMM_REPORT_NO_CONFIG);
+      CurrentState::getInstance()->printQAndNewLine();
+      CurrentState::getInstance()->setLastError(ERR_PARAMS_NOT_OK);
+      return -1;
+    }
+  }
 
-	execution = handler->execute(command);
-	if(execution == 0) {
-		Serial.print(COMM_REPORT_CMD_DONE);
-		CurrentState::getInstance()->printQAndNewLine();
-	} else {
-		Serial.print(COMM_REPORT_CMD_ERROR);
-		CurrentState::getInstance()->printQAndNewLine();
-	}
+  // Return error when no command or invalid command is found
 
-	CurrentState::getInstance()->resetQ();
-	return execution;
+  if (command == NULL)
+  {
+
+    Serial.print(COMM_REPORT_BAD_CMD);
+    CurrentState::getInstance()->printQAndNewLine();
+    CurrentState::getInstance()->setLastError(ERR_INVALID_COMMAND);
+
+    if (LOGGING)
+    {
+      printCommandLog(command);
+    }
+    return -1;
+  }
+
+  if (command->getCodeEnum() == CODE_UNDEFINED)
+  {
+    Serial.print(COMM_REPORT_BAD_CMD);
+    CurrentState::getInstance()->printQAndNewLine();
+    CurrentState::getInstance()->setLastError(ERR_INVALID_COMMAND);
+
+    if (LOGGING)
+    {
+      printCommandLog(command);
+    }
+    return -1;
+  }
+
+  // Get the right handler for this command
+
+  GCodeHandler *handler = getGCodeHandler(command->getCodeEnum());
+
+  if (handler == NULL)
+  {
+    Serial.print(COMM_REPORT_BAD_CMD);
+    CurrentState::getInstance()->printQAndNewLine();
+    CurrentState::getInstance()->setLastError(ERR_INVALID_COMMAND);
+
+    Serial.println("R99 handler == NULL\r\n");
+    return -1;
+  }
+
+  // Report start of command
+
+  Serial.print(COMM_REPORT_CMD_START);
+  CurrentState::getInstance()->printQAndNewLine();
+
+  // Execute command with retry
+  CurrentState::getInstance()->setLastError(0);
+  while (attempt < 1 || (!emergencyStop && attempt < maximumAttempts && execution != 0 && execution != 2))
+  // error 2 is timeout error: stop movement retries
+  {
+
+    if (LOGGING || debugMessages)
+    {
+      Serial.print("R99 attempt ");
+      Serial.print(attempt);
+      Serial.print(" from ");
+      Serial.print(maximumAttempts);
+      Serial.print("\r\n");
+    }
+
+    attempt++;
+    if (attempt > 1)
+    {
+      Serial.print(COMM_REPORT_CMD_RETRY);
+      CurrentState::getInstance()->printQAndNewLine();
+    }
+    
+    handler->execute(command);
+    execution = CurrentState::getInstance()->getLastError();
+    emergencyStop = CurrentState::getInstance()->isEmergencyStop();
+
+    if (LOGGING || debugMessages)
+    {
+      Serial.print("R99 execution ");
+      Serial.print(execution);
+      Serial.print("\r\n");
+    }
+  }
+
+  // Clean serial buffer
+  while (Serial.available() > 0)
+  {
+    Serial.read();
+  }
+
+  // if movemement failed after retry
+  // and parameter for emergency stop is set
+  // set the emergency stop
+
+  if (execution != 0)
+  {
+    if (isMovement)
+    {
+      if (ParameterList::getInstance()->getValue(PARAM_E_STOP_ON_MOV_ERR) == 1)
+      {
+        CurrentState::getInstance()->setEmergencyStop();
+      }
+    }
+  }
+
+  // Report back result of execution
+  if (execution == 0)
+  {
+    Serial.print(COMM_REPORT_CMD_DONE);
+    CurrentState::getInstance()->printQAndNewLine();
+  }
+  else
+  {
+    Serial.print(COMM_REPORT_CMD_ERROR);
+    Serial.print(" V");
+    Serial.print(CurrentState::getInstance()->getLastError());
+    CurrentState::getInstance()->printQAndNewLine();
+  }
+
+  CurrentState::getInstance()->resetQ();
+  return execution;
 };
 
-GCodeHandler* GCodeProcessor::getGCodeHandler(CommandCodeEnum codeEnum) {
+GCodeHandler *GCodeProcessor::getGCodeHandler(CommandCodeEnum codeEnum)
+{
 
-	GCodeHandler* handler = NULL;
+  GCodeHandler *handler = NULL;
 
-	if (codeEnum == G00) {handler = G00Handler::getInstance();}
+  // These are if statements so they can be disabled as test
+  // Usefull when running into memory issues again
 
-	if (codeEnum == G28) {handler = G28Handler::getInstance();}
+  if (codeEnum == G00)
+  {
+    handler = G00Handler::getInstance();
+  }
 
-	if (codeEnum == F11) {handler = F11Handler::getInstance();}
-	if (codeEnum == F12) {handler = F12Handler::getInstance();}
-	if (codeEnum == F13) {handler = F13Handler::getInstance();}
+  if (codeEnum == G28)
+  {
+    handler = G28Handler::getInstance();
+  }
 
-	if (codeEnum == F14) {handler = F14Handler::getInstance();}
-	if (codeEnum == F15) {handler = F15Handler::getInstance();}
-	if (codeEnum == F16) {handler = F16Handler::getInstance();}
+  if (codeEnum == F09)
+  {
+    handler = F09Handler::getInstance();
+  }
 
-	if (codeEnum == F20) {handler = F20Handler::getInstance();}
-	if (codeEnum == F21) {handler = F21Handler::getInstance();}
-	if (codeEnum == F22) {handler = F22Handler::getInstance();}
+  if (codeEnum == F11)
+  {
+    handler = F11Handler::getInstance();
+  }
+  if (codeEnum == F12)
+  {
+    handler = F12Handler::getInstance();
+  }
+  if (codeEnum == F13)
+  {
+    handler = F13Handler::getInstance();
+  }
 
-//	if (codeEnum == F31) {handler = F31Handler::getInstance();}
-//	if (codeEnum == F32) {handler = F32Handler::getInstance();}
+  if (codeEnum == F14)
+  {
+    handler = F14Handler::getInstance();
+  }
+  if (codeEnum == F15)
+  {
+    handler = F15Handler::getInstance();
+  }
+  if (codeEnum == F16)
+  {
+    handler = F16Handler::getInstance();
+  }
 
-	if (codeEnum == F41) {handler = F41Handler::getInstance();}
-	if (codeEnum == F42) {handler = F42Handler::getInstance();}
-	if (codeEnum == F43) {handler = F43Handler::getInstance();}
-	if (codeEnum == F44) {handler = F44Handler::getInstance();}
+  if (codeEnum == F20)
+  {
+    handler = F20Handler::getInstance();
+  }
+  if (codeEnum == F21)
+  {
+    handler = F21Handler::getInstance();
+  }
+  if (codeEnum == F22)
+  {
+    handler = F22Handler::getInstance();
+  }
 
-//	if (codeEnum == F61) {handler = F61Handler::getInstance();}
+  //	if (codeEnum == F31) {handler = F31Handler::getInstance();}
+  //	if (codeEnum == F32) {handler = F32Handler::getInstance();}
 
-	if (codeEnum == F81) {handler = F81Handler::getInstance();}
-	if (codeEnum == F82) {handler = F82Handler::getInstance();}
-	if (codeEnum == F83) {handler = F83Handler::getInstance();}
+  if (codeEnum == F41)
+  {
+    handler = F41Handler::getInstance();
+  }
+  if (codeEnum == F42)
+  {
+    handler = F42Handler::getInstance();
+  }
+  if (codeEnum == F43)
+  {
+    handler = F43Handler::getInstance();
+  }
+  if (codeEnum == F44)
+  {
+    handler = F44Handler::getInstance();
+  }
 
+  if (codeEnum == F61)
+  {
+    handler = F61Handler::getInstance();
+  }
 
-/*
-	switch(codeEnum) {
-	case G00:
-		return G00Handler::getInstance();
-	case G28:
-		return G28Handler::getInstance();
+  if (codeEnum == F81)
+  {
+    handler = F81Handler::getInstance();
+  }
+  if (codeEnum == F82)
+  {
+    handler = F82Handler::getInstance();
+  }
+  if (codeEnum == F83)
+  {
+    handler = F83Handler::getInstance();
+  }
+  if (codeEnum == F84)
+  {
+    handler = F84Handler::getInstance();
+  }
 
-	case F11:
-		return F11Handler::getInstance();
-	case F12:
-		return F12Handler::getInstance();
-	case F13:
-		return F13Handler::getInstance();
-
-	case F14:
-		return F14Handler::getInstance();
-	case F15:
-		return F15Handler::getInstance();
-	case F16:
-		return F16Handler::getInstance();
-
-	case F20:
-		return F20Handler::getInstance();
-	case F21:
-		return F21Handler::getInstance();
-	case F22:
-		return F22Handler::getInstance();
-
-	case F31:
-		return F31Handler::getInstance();
-	case F32:
-		return F32Handler::getInstance();
-
-	case F41:
-		return F41Handler::getInstance();
-	case F42:
-		return F42Handler::getInstance();
-	case F43:
-		return F43Handler::getInstance();
-	case F44:
-		return F44Handler::getInstance();
-
-	case F61:
-		return F61Handler::getInstance();
-
-	case F81:
-		return F81Handler::getInstance();
-	case F82:
-		return F82Handler::getInstance();
-	case F83:
-		return F83Handler::getInstance();
-
-	}
-*/
-
-
-	return handler;
+  return handler;
 }
-
-

src/GCodeProcessor.h

@@ -11,10 +11,14 @@
 #include "Command.h"
 #include "Config.h"
 
+#include "Debug.h"
+
 #include "GCodeHandler.h"
 #include "G00Handler.h"
 #include "G28Handler.h"
 
+#include "F09Handler.h"
+
 #include "F11Handler.h"
 #include "F12Handler.h"
 #include "F13Handler.h"
@@ -35,21 +39,25 @@
 #include "F43Handler.h"
 #include "F44Handler.h"
 
-//#include "F61Handler.h"
+#include "F61Handler.h"
 
 #include "F81Handler.h"
 #include "F82Handler.h"
 #include "F83Handler.h"
+#include "F84Handler.h"
 
-class GCodeProcessor {
+class GCodeProcessor
+{
 public:
-	GCodeProcessor();
-	virtual ~GCodeProcessor();
-	int execute(Command* command);
+  GCodeProcessor();
+  virtual ~GCodeProcessor();
+  int execute(Command *command);
+
 protected:
-	GCodeHandler* getGCodeHandler(CommandCodeEnum);
+  GCodeHandler *getGCodeHandler(CommandCodeEnum);
+
 private:
-	void printCommandLog(Command*);
+  void printCommandLog(Command *);
 };
 
 #endif /* GCODEPROCESSOR_H_ */

src/LICENSE

@@ -18,4 +18,4 @@ FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-SOFTWARE.
+SOFTWARE.

src/MemoryFree.cpp

@@ -0,0 +1,52 @@
+#if (ARDUINO >= 100)
+#include <Arduino.h>
+#else
+#include <WProgram.h>
+#endif
+
+extern unsigned int __heap_start;
+extern void *__brkval;
+
+/*
+ * The free list structure as maintained by the
+ * avr-libc memory allocation routines.
+ */
+struct __freelist
+{
+  size_t sz;
+  struct __freelist *nx;
+};
+
+/* The head of the free list structure */
+extern struct __freelist *__flp;
+
+#include "MemoryFree.h"
+
+/* Calculates the size of the free list */
+int freeListSize()
+{
+  struct __freelist *current;
+  int total = 0;
+  for (current = __flp; current; current = current->nx)
+  {
+    total += 2; /* Add two bytes for the memory block's header  */
+    total += (int)current->sz;
+  }
+
+  return total;
+}
+
+int freeMemory()
+{
+  int free_memory;
+  if ((int)__brkval == 0)
+  {
+    free_memory = ((int)&free_memory) - ((int)&__heap_start);
+  }
+  else
+  {
+    free_memory = ((int)&free_memory) - ((int)__brkval);
+    free_memory += freeListSize();
+  }
+  return free_memory;
+}

src/MemoryFree.h

@@ -0,0 +1,18 @@
+// MemoryFree library based on code posted here:
+// http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1213583720/15
+// Extended by Matthew Murdoch to include walking of the free list.
+
+#ifndef MEMORY_FREE_H
+#define MEMORY_FREE_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+int freeMemory();
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

src/Movement.h

@@ -0,0 +1,160 @@
+/*
+ * Movement.h
+ *
+ *  Created on: 16 maj 2014
+ *      Author: MattLech
+ */
+
+#ifndef MOVEMENT_H_
+#define MOVEMENT_H_
+
+#include "Arduino.h"
+#include "CurrentState.h"
+#include "ParameterList.h"
+#include "MovementAxis.h"
+#include "MovementEncoder.h"
+#include "pins.h"
+#include "Config.h"
+#include <stdio.h>
+#include <stdlib.h>
+#include "Command.h"
+
+class Movement
+{
+public:
+  Movement();
+  Movement(Movement const &);
+  void operator=(Movement const &);
+
+  static Movement *getInstance();
+  //int moveAbsolute(	long xDest, long yDest,long zDest,
+  //			unsigned int maxStepsPerSecond,
+  //			unsigned int maxAccelerationStepsPerSecond);
+  int moveToCoords(double xDestScaled, double yDestScaled, double zDestScaled,
+                   unsigned int xMaxSpd, unsigned int yMaxSpd, unsigned int zMaxSpd,
+                   bool homeX, bool homeY, bool homeZ);
+
+  void handleMovementInterrupt();
+  void checkEncoders();
+
+  #if defined(FARMDUINO_EXP_V20) || defined(FARMDUINO_V30)
+    void initTMC2130();
+    void loadSettingsTMC2130();
+    void loadSettingsTMC2130_X();
+    void loadSettingsTMC2130_Y();
+    void loadSettingsTMC2130_Z();
+  #endif
+
+
+  int calibrateAxis(int axis);
+  //void initInterrupt();
+  void enableMotors();
+  void disableMotors();
+  void disableMotorsEmergency();
+  void primeMotors();
+  bool motorsEnabled();
+
+  void storePosition();
+  void loadSettings();
+
+  void setPositionX(long pos);
+  void setPositionY(long pos);
+  void setPositionZ(long pos);
+
+  void reportEncoders();
+  void getEncoderReport();
+
+  void test();
+  void test2();
+	unsigned long i1 = 0;
+	unsigned long i2 = 0;
+  unsigned long i3 = 0;
+  unsigned long i4 = 0;
+
+private:
+
+  MovementAxis axisX;
+  MovementAxis axisY;
+  MovementAxis axisZ;
+
+  MovementEncoder encoderX;
+  MovementEncoder encoderY;
+  MovementEncoder encoderZ;
+
+  //char serialBuffer[100];
+  String serialBuffer;
+  int serialBufferLength = 0;
+  int serialBufferSending = 0;
+  int serialMessageNr = 0;
+  int serialMessageDelay = 0;
+
+  unsigned long calibrationTicks = 0;
+
+  void serialBufferSendNext();
+  void serialBufferEmpty();
+
+  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 };
+
+  void loadMotorSettings();
+  void loadEncoderSettings();
+  bool intToBool(int value);
+
+  void reportPosition();
+  String getReportPosition();
+
+  void storeEndStops();
+  void reportEndStops();
+  void reportStatus(MovementAxis *axis, int axisSubStatus);
+  void reportCalib(MovementAxis *axis, int calibStatus);
+
+  unsigned long getMaxLength(unsigned long lengths[3]);
+  bool endStopsReached();
+
+  bool homeIsUp[3] = {false, false, false};
+  long speedMax[3] = {0, 0, 0 };
+  long commandSpeed[3] = { 0, 0, 0 };
+  long speedMin[3] = { 0, 0, 0 };
+  long speedHome[3] = { 0, 0, 0 };
+  long stepsAcc[3] = { 0, 0, 0 };
+  bool motorInv[3] = { false, false, false };
+  long motorMaxSize[3] = { 0, 0, 0};
+  bool motorStopAtMax[3] = { false, false, false };
+  bool motorKeepActive[3] = { false, false, false };
+  bool motor2Inv[3] = { false, false, false };
+  bool motor2Enbl[3] = { false, false, false };
+  bool motorStopAtHome[3] = { false, false, false };
+  bool endStInv[3] = { false, false, false };
+  bool endStInv2[3] = { false, false, false };
+  bool endStEnbl[3] = { false, false, false };
+  long timeOut[3] = { 0, 0, 0 };
+  long stepsPerMm[3] = { 1.0, 1.0, 1.0 };
+
+  float motorConsMissedSteps[3] = { 0, 0, 0 };
+  int motorConsMissedStepsPrev[3] = { 0, 0, 0 };
+  long motorLastPosition[3] = { 0, 0, 0 };
+  long motorConsEncoderLastPosition[3] = { 0, 0, 0 };
+
+  int motorConsMissedStepsMax[3] = { 0, 0, 0 };
+  float motorConsMissedStepsDecay[3] = { 0, 0, 0 };
+  bool motorConsEncoderEnabled[3] = { false, false, false };
+  int motorConsEncoderType[3] = { 0, 0, 0 };
+  long motorConsEncoderScaling[3] = { 0, 0, 0 };
+  int motorConsEncoderUseForPos[3] = { 0, 0, 0 };
+  int motorConsEncoderInvert[3] = { 0, 0, 0 };
+
+  int axisServiced = 0;
+  int axisServicedNext = 0;
+  bool motorMotorsEnabled = false;
+
+
+  int testA = 0;
+  int testB = 0;
+
+
+};
+
+#endif /* MOVEMENT_H_ */

src/MovementAxis.h

@@ -0,0 +1,225 @@
+/*
+ * MovementAxis.h
+ *
+ *  Created on: 18 juli 2015
+ *      Author: Tim Evers
+ */
+
+#ifndef MovementAXIS_H_
+#define MovementAXIS_H_
+
+#include "Arduino.h"
+#include "CurrentState.h"
+#include "ParameterList.h"
+#include "pins.h"
+#include "MovementEncoder.h"
+#include "Config.h"
+#include <stdio.h>
+#include <stdlib.h>
+#include "Board.h"
+
+//#if defined(FARMDUINO_EXP_V20)
+  #include "TMC2130.h"
+//#endif
+
+
+class MovementAxis
+{
+
+public:
+  MovementAxis();
+
+#if defined(FARMDUINO_EXP_V20) || defined(FARMDUINO_V30)
+  TMC2130_Basics *TMC2130A;
+  TMC2130_Basics *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);
+  bool loadCoordinates(long sourcePoint, long destinationPoint, bool home);
+  void setMaxSpeed(long speed);
+
+  void enableMotor();
+  void disableMotor();
+  void checkMovement();
+  void incrementTick();
+  void checkTiming();
+  void setTicks();
+
+  bool isAxisActive();
+  void deactivateAxis();
+  bool isAccelerating();
+  bool isDecelerating();
+  bool isCruising();
+  bool isCrawling();
+  bool isMotorActive();
+  //bool isMoving();
+
+  bool endStopMin();
+  bool endStopMax();
+  bool endStopsReached();
+  bool endStopAxisReached(bool movement_forward);
+
+  long currentPosition();
+  void setCurrentPosition(long newPos);
+  long destinationPosition();
+
+  void setStepAxis();
+  void setMotorStep();
+  void resetMotorStep();
+
+  void setDirectionUp();
+  void setDirectionDown();
+  void setDirectionHome();
+  void setDirectionAway();
+  void setDirectionAxis();
+
+  void setMovementUp();
+  void setMovementDown();
+
+  long getLastPosition();
+  void setLastPosition(long position);
+
+  bool movingToHome();
+  bool movingUp();
+
+  bool isStepDone();
+  void resetStepDone();
+
+  void activateDebugPrint();
+  void test();
+
+  unsigned long getNrOfSteps();
+  void resetNrOfSteps();
+
+  char channelLabel;
+  bool movementStarted;
+
+#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) || defined(FARMDUINO_V30)
+  void setMotorStepWriteTMC2130();
+  void setMotorStepWriteTMC2130_2();
+  void resetMotorStepWriteTMC2130();
+  void resetMotorStepWriteTMC2130_2();
+
+  uint8_t getStatus();
+
+  void readDriverStatus();
+  bool isStallGuard();
+  bool isStandStill();
+  bool isDriverError();
+
+  unsigned int getLostSteps();
+
+  int missedStepHistory[5] = {0,0,0,0,0};
+
+  bool tmcStep = true;
+  bool tmcStep2 = true;
+#endif
+
+private:
+  int lastCalcLog = 0;
+  bool debugPrint = false;
+
+#if defined(FARMDUINO_EXP_V20) || defined(FARMDUINO_V30)
+  uint32_t driverStatus = 0;
+#endif
+
+  // pin settings primary motor
+  int pinStep;
+  int pinDirection;
+  int pinEnable;
+
+  // pin settings primary motor
+  int pin2Step;
+  int pin2Direction;
+  int pin2Enable;
+
+  // pin settings primary motor
+  int pinMin;
+  int pinMax;
+
+  // motor settings
+  bool motorEndStopInv;  // switch places of end stops
+  bool motorEndStopInv2; // invert input (true/normal open, falce/normal closed) of end stops
+  bool motorEndStopEnbl; // enable the use of the end stops
+
+  // motor settings
+  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     = 0;      // all coordinated in steps
+  long coordCurrentPoint    = 0;
+  long coordDestinationPoint = 0;
+  bool coordHomeAxis        = false; // homing command
+
+  long axisLastPosition     = 0;
+
+  // movement handling
+  unsigned long movementLength  = 0;
+  unsigned long maxLenth        = 0;
+  unsigned long moveTicks       = 0;
+  unsigned long stepOnTick      = 0;
+  unsigned long stepOffTick     = 0;
+  unsigned long axisSpeed       = 0;
+  unsigned long movementSteps   = 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;
+
+  void step(long &currentPoint, 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 (MovementAxis::*setMotorStepWrite)();
+  void (MovementAxis::*setMotorStepWrite2)();
+  void (MovementAxis::*resetMotorStepWrite)();
+  void (MovementAxis::*resetMotorStepWrite2)();
+
+  void setMotorStepWriteDefault();
+  void setMotorStepWriteDefault2();
+  void resetMotorStepWriteDefault();
+  void resetMotorStepWriteDefault2();
+  void setMotorStepWrite54();
+  void resetMotorStepWrite54();
+  void setMotorStepWrite26();
+  void resetMotorStepWrite26();
+  void setMotorStepWrite60();
+  void resetMotorStepWrite60();
+  void setMotorStepWrite46();
+  void resetMotorStepWrite46();
+
+};
+
+#endif /* MovementAXIS_H_ */

src/MovementEncoder.cpp

@@ -0,0 +1,337 @@
+#include "MovementEncoder.h"
+
+MovementEncoder::MovementEncoder()
+{
+  //lastCalcLog	= 0;
+
+  pinChannelA = 0;
+  pinChannelB = 0;
+
+  position = 0;
+  encoderType = 0; // default type
+  scalingFactor = 10000;
+
+  curValChannelA = false;
+  curValChannelA = false;
+  prvValChannelA = false;
+  prvValChannelA = false;
+
+  readChannelA = false;
+  readChannelAQ = false;
+  readChannelB = false;
+  readChannelBQ = false;
+
+  mdlEncoder = _MDL_X1;
+}
+
+void MovementEncoder::test()
+{
+  /*
+                Serial.print("R88 ");
+                Serial.print("position ");
+                Serial.print(position);
+                Serial.print(" channel A ");
+                Serial.print(prvValChannelA);
+                Serial.print(" -> ");
+                Serial.print(curValChannelA);
+                Serial.print(" channel B ");
+                Serial.print(prvValChannelB);
+                Serial.print(" -> ");
+                Serial.print(curValChannelB);
+                Serial.print("\r\n");
+*/
+}
+
+void MovementEncoder::loadPinNumbers(int channelA, int channelB, int channelAQ, int channelBQ)
+{
+  pinChannelA = channelA;
+  pinChannelB = channelB;
+  pinChannelAQ = channelAQ;
+  pinChannelBQ = channelBQ;
+
+  readChannels();
+  shiftChannels();
+}
+
+void MovementEncoder::loadSettings(int encType, long scaling, int invert)
+{
+  encoderType = encType;
+  scalingFactor = scaling;
+  if (invert == 1)
+  {
+    encoderInvert = -1;
+  }
+  else
+  {
+    encoderInvert = 1;
+  }
+
+//  encoderType = 0; // TEVE 2017-04-20 Disabling the differential channels. They take too much time to read.
+}
+
+void MovementEncoder::loadMdlEncoderId(MdlSpiEncoders encoder)
+{
+  mdlEncoder = encoder;
+}
+
+void MovementEncoder::setPosition(long newPosition)
+{
+  #if defined(RAMPS_V14) || defined(FARMDUINO_V10) || defined(FARMDUINO_EXP_V20)
+    position = newPosition;
+  #endif
+
+  #if defined(FARMDUINO_V14) || defined(FARMDUINO_V30)
+    if (newPosition == 0)
+    {
+      position = newPosition;
+
+      const byte reset_cmd = 0x00;
+
+      SPI.beginTransaction(SPISettings(SPI_CLOCK_DIV4, MSBFIRST, SPI_MODE0));
+      digitalWrite(NSS_PIN, LOW);
+      delayMicroseconds(2);
+      SPI.transfer(reset_cmd | (mdlEncoder << mdl_spi_encoder_offset));
+      delayMicroseconds(5);
+      digitalWrite(NSS_PIN, HIGH);
+      SPI.endTransaction();
+    }
+  #endif
+}
+
+long MovementEncoder::currentPosition()
+{
+
+
+  // Apply scaling to the output of the encoder, except when scaling is zero or lower
+  if (scalingFactor == 10000 || scalingFactor <= 0)
+  {
+    return position * encoderInvert;
+  }
+  else
+  {
+    #if defined(FARMDUINO_V14) || defined(FARMDUINO_V30)
+      floatScalingFactor = scalingFactor / 40000.0;
+      return position * floatScalingFactor * encoderInvert;
+    #endif
+    floatScalingFactor = scalingFactor / 10000.0;
+    return position * floatScalingFactor * encoderInvert;
+  }
+}
+
+long MovementEncoder::currentPositionRaw()
+{
+    return position * encoderInvert;
+}
+
+void MovementEncoder::checkEncoder(bool channelA, bool channelB, bool channelAQ, bool channelBQ)
+{
+  #if defined(RAMPS_V14) || defined(FARMDUINO_V10)
+    shiftChannels();
+    setChannels(channelA, channelB, channelAQ, channelBQ);
+    processEncoder();
+  #endif
+
+  #if defined(FARMDUINO_V14) || defined(FARMDUINO_V30)
+    processEncoder();
+  #endif
+
+}
+
+
+/* Check the encoder channels for movement according to this specification
+                    ________            ________
+Channel A          /        \          /        \
+             _____/          \________/          \________
+                         ________            ________
+Channel B               /        \          /        \
+             __________/          \________/          \____
+                                   __
+Channel I                         /  \
+             ____________________/    \___________________
+
+rotation ----------------------------------------------------->
+
+*/
+
+void MovementEncoder::processEncoder()
+{
+
+  #if defined(RAMPS_V14) || defined(FARMDUINO_V10)
+
+    // Detect edges on the A channel when the B channel is high
+    if (curValChannelB == true && prvValChannelA == false && curValChannelA == true)
+    {
+      //delta--;
+      position--;
+    }
+    if (curValChannelB == true && prvValChannelA == true && curValChannelA == false)
+    {
+      //delta++;
+      position++;
+    }
+
+  #endif
+
+  // If using farmduino, revision 1.4, use the SPI interface to read from the Motor Dynamics Lab chip
+  #if defined(FARMDUINO_V14) || defined(FARMDUINO_V30)
+    const byte read_cmd = 0x0F;
+    int readSize = 4;
+    long encoderVal = 0;
+
+    SPI.beginTransaction(SPISettings(SPI_CLOCK_DIV4, MSBFIRST, SPI_MODE0));
+    digitalWrite(NSS_PIN, LOW);
+    delayMicroseconds(2);
+    SPI.transfer(read_cmd | (mdlEncoder << mdl_spi_encoder_offset));
+    delayMicroseconds(5);
+
+    for (int i = 0; i < readSize; ++i)
+    {
+      encoderVal <<= 8;
+      encoderVal |= SPI.transfer(0x01);
+    }
+
+    digitalWrite(NSS_PIN, HIGH);
+    SPI.endTransaction();
+    position = encoderVal;
+  #endif
+
+}
+
+void MovementEncoder::readChannels()
+{
+
+#if defined(RAMPS_V14) || defined(FARMDUINO_V10)
+  // read the new values from the coder
+
+  readChannelA = digitalRead(pinChannelA);
+  readChannelB = digitalRead(pinChannelB);
+
+  if (encoderType == 1)
+  {
+    readChannelAQ = digitalRead(pinChannelAQ);
+    readChannelBQ = digitalRead(pinChannelBQ);
+
+    // differential encoder
+    if ((readChannelA ^ readChannelAQ) && (readChannelB ^ readChannelBQ))
+    {
+      curValChannelA = readChannelA;
+      curValChannelB = readChannelB;
+    }
+  }
+  else
+  {
+
+    // encoderType <= 0
+    // non-differential incremental encoder
+    curValChannelA = readChannelA;
+    curValChannelB = readChannelB;
+  }
+#endif
+
+}
+
+void MovementEncoder::setChannels(bool channelA, bool channelB, bool channelAQ, bool channelBQ)
+{
+  // read the new values from the coder
+
+  if (encoderType == 1)
+  {
+    // differential encoder
+    if ((channelA ^ channelAQ) && (channelB ^ channelBQ))
+    {
+      curValChannelA = channelA;
+      curValChannelB = channelB;
+    }
+  }
+  else
+  {
+    // encoderType <= 0
+    // non-differential incremental encoder
+    curValChannelA = channelA;
+    curValChannelB = channelB;
+  }
+}
+
+void MovementEncoder::shiftChannels()
+{
+
+  // Save the current enoder status to later on compare with new values
+
+  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) || defined(FARMDUINO_V30)
+
+    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
+*/
+}

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 <stdio.h>
+#include <stdlib.h>
+#include <SPI.h>
+#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_ */

src/ParameterList.cpp

@@ -1,68 +1,122 @@
 #include "ParameterList.h"
 #include <EEPROM.h>
 
-static ParameterList* instanceParam;
-int paramValues[PARAM_NR_OF_PARAMS];
+static ParameterList *instanceParam;
+long paramValues[PARAM_NR_OF_PARAMS];
 
-ParameterList * ParameterList::getInstance() {
-        if (!instanceParam) {
-                instanceParam = new ParameterList();
-        };
-        return instanceParam;
+ParameterList *ParameterList::getInstance()
+{
+  if (!instanceParam)
+  {
+    instanceParam = new ParameterList();
+  };
+  return instanceParam;
 }
 
-ParameterList::ParameterList() {
-	// at the first boot, load default parameters and set the parameter version
-	// so during subsequent boots the values are just loaded from eeprom 
-	int paramVersion = readValueEeprom(0);
-	if (paramVersion <= 0) {
-		setAllValuesToDefault();
-		writeAllValuesToEeprom();
-	} else {
-		readAllValuesFromEeprom();
-	}
+ParameterList::ParameterList()
+{
+  // at the first boot, load default parameters and set the parameter version
+  // so during subsequent boots the values are just loaded from eeprom
+  // unless the eeprom is disabled with a parameter
+
+  int paramChangeNr = 0;
+  int tmcParamChangeNr = 0;
+
+  int paramVersion = readValueEeprom(0);
+  if (paramVersion <= 0)
+  {
+    setAllValuesToDefault();
+    writeAllValuesToEeprom();
+  }
+  else
+  {
+    if (readValueEeprom(PARAM_USE_EEPROM) == 1)
+    {
+      readAllValuesFromEeprom();
+    } else {
+      setAllValuesToDefault();
+    }
+  }
 }
 
 // ===== Interface functions for the raspberry pi =====
 
-int ParameterList::readValue(int id) {
+int ParameterList::readValue(int id)
+{
 
-	// Check if the value is an existing parameter
-	if (validParam(id)) {
-		// Retrieve the value from memory
-		int value =  paramValues[id];
+  // Check if the value is an existing parameter
+  if (validParam(id))
+  {
+    // Retrieve the value from memory
+    long value = paramValues[id];
 
-		// Send to the raspberrt pi
-		Serial.print("R21");
-		Serial.print(" ");
-		Serial.print("P");
-		Serial.print(id);
-		Serial.print(" ");
-		Serial.print("V");
-		Serial.print(value);
-		//Serial.print("\r\n");
-		CurrentState::getInstance()->printQAndNewLine();
+    // Send to the raspberry pi
+    Serial.print("R21");
+    Serial.print(" ");
+    Serial.print("P");
+    Serial.print(id);
+    Serial.print(" ");
+    Serial.print("V");
+    Serial.print(value);
+    //Serial.print("\r\n");
+    CurrentState::getInstance()->printQAndNewLine();
+  }
+  else
+  {
+    Serial.print("R99 Error: invalid parameter id\r\n");
+  }
 
-	} else  {
-		Serial.print("R99 Error: invalid parameter id\r\n");
-	}
-
-	return 0;
+  return 0;
 }
 
-int ParameterList::writeValue(int id, int value) {
+int ParameterList::writeValue(int id, long value)
+{
 
-	// Check if the value is a valid parameter
-	if (validParam(id)) {
-		// Store the value in memory
-		paramValues[id] = value;
-		writeValueEeprom(id, value);
-	} else  {
-		Serial.print("R99 Error: invalid parameter id\r\n");
-	}
+  if (paramChangeNr < 9999)
+  {
+    paramChangeNr++;
+  }
+  else
+  {
+    paramChangeNr = 0;
+  }
 
+  if (tmcParamChangeNr < 9999)
+  {
+    if (
+      id == 81 ||
+      id == 82 ||
+      id == 83 ||
+      id == 85 ||
+      id == 86 ||
+      id == 87 ||
+      id == 91 ||
+      id == 92 ||
+      id == 93
+    )
+    {
+      tmcParamChangeNr++;
+    }
+  }
+  else
+  {
+    tmcParamChangeNr = 0;
+  }
 
-/*
+  // Check if the value is a valid parameter
+  if (validParam(id))
+  {
+    // Store the value in memory
+    paramValues[id] = value;
+    writeValueEeprom(id, value);
+    readValue(id);
+  }
+  else
+  {
+    Serial.print("R99 Error: invalid parameter id\r\n");
+  }
+
+  /*
 	// Debugging output
 	Serial.print("R99");
 	Serial.print(" ");
@@ -79,242 +133,604 @@ int ParameterList::writeValue(int id, int value) {
 	CurrentState::getInstance()->printQAndNewLine();
 */
 
-	// If any value is written,
-	// trigger the loading of the new configuration in all other modules
-	sendConfigToModules();
+  // If any value is written,
+  // trigger the loading of the new configuration in all other modules
+  sendConfigToModules();
 
-	return 0;
+  return 0;
 }
 
-
-void ParameterList::sendConfigToModules() {
-	// Trigger other modules to load the new values
-	PinGuard::getInstance()->loadConfig();
+void ParameterList::sendConfigToModules()
+{
+  // Trigger other modules to load the new values
+  PinGuard::getInstance()->loadConfig();
 }
 
-int ParameterList::readAllValues() {
+int ParameterList::readAllValues()
+{
 
-
-	// Make a dump of all values
-	// Check if it's a valid value to keep the junk out of the list
-	for (int i=0; i < PARAM_NR_OF_PARAMS; i++) {
-		if (validParam(i)) {
-			readValue(i);
-		}
-	}
+  // Make a dump of all values
+  // Check if it's a valid value to keep the junk out of the list
+  for (int i = 0; i < PARAM_NR_OF_PARAMS; i++)
+  {
+    if (validParam(i))
+    {
+      readValue(i);
+    }
+  }
 }
 
-int ParameterList::getValue(int id) {
-	return  paramValues[id];
+long ParameterList::getValue(int id)
+{
+  return paramValues[id];
+}
+
+int ParameterList::paramChangeNumber()
+{
+  return paramChangeNr;
+}
+
+int ParameterList::tmcParamChangeNumber()
+{
+  return tmcParamChangeNr;
 }
 
 // ===== 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;
+  // Assume all values are ints and calculate address for that
+  int address = id * 2;
 
-	//Read the 2 bytes from the eeprom memory.
-	long two = EEPROM.read(address);
-	long one = EEPROM.read(address + 1);
+  //Read the 2 bytes from the eeprom memory.
+  long one   = EEPROM.read(address + 0);
+  long two   = EEPROM.read(address + 1);
 
-	//Return the recomposed long by using bitshift.
-	return ((two << 0) & 0xFF) + ((one << 8) & 0xFFFF);
+  long three = 0;
+  long four  = 0;
+
+  // Process 2-byte or 4-byte EEPROM value
+  // Return -1 for negative values (1 in highest bit) to indicate value should be set to default.
+  if (id == 141 || id == 142 || id == 143)
+  {
+    // 4-byte EEPROM value
+    three = EEPROM.read(address + 20);
+    four = EEPROM.read(address + 21);
+    if ((three == 255 && four == 255) && !(one == 255 && two == 255))
+    {
+      // Value may have been recently increased to 4 bytes. Keep only the first two.
+      three = 0;
+      four = 0;
+      if (two > 127) { return -1; }
+    }
+    if (four > 127) { return -1; }
+  }
+  else
+  {
+    // 2-byte EEPROM value
+    if (two > 127) { return -1; }
+  }
+
+  // Return the recomposed long by using bitshift.
+  return ((one & 0xFFl) << 0) + ((two & 0xFFl) << 8) + ((three & 0xFFl) << 16) + ((four & 0xFFl) << 24);
 }
 
-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;
+  // Assume all values are ints and calculate address for that
+  int address = id * 2;
 
-	//Decomposition from a int to 2 bytes by using bitshift.
-	//One = Most significant -> Two = Least significant byte
-	byte two = (value        & 0xFF);
-	byte one = ((value >> 8) & 0xFF);
+  //Decomposition from a int to 2 bytes by using bitshift.
+  //One = 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);
+  //Write the 4 bytes into the eeprom memory.
+  EEPROM.write(address + 0, one);
+  EEPROM.write(address + 1, two);
 
-	return 0;
+  // Only this parameter needs a long value
+  if (id == 141 || id == 142 || id == 143)
+  {
+    EEPROM.write(address + 20, three);
+    EEPROM.write(address + 21, four);
+  }
+  return 0;
 }
 
-int ParameterList::readAllValuesFromEeprom() {
-	// Write all existing values to eeprom
-	for (int i=0; i < PARAM_NR_OF_PARAMS; i++) {
-		if (validParam(i)) {
-			paramValues[i] = readValueEeprom(i);
-			if (paramValues[i] == -1) {
-				// When parameters are still on default,
-				// load a good value and save it
-				loadDefaultValue(i);
-				writeValueEeprom(i,paramValues[i]);
-			}
-		}
-	}
+int ParameterList::readAllValuesFromEeprom()
+{
+  // Write all existing values to eeprom
+  for (int i = 0; i < PARAM_NR_OF_PARAMS; i++)
+  {
+    if (validParam(i))
+    {
+      paramValues[i] = readValueEeprom(i);
+      if (paramValues[i] == -1)
+      {
+        // When parameters are still on default,
+        // load a good value and save it
+        loadDefaultValue(i);
+        writeValueEeprom(i, paramValues[i]);
+      }
+    }
+  }
 }
 
-int ParameterList::writeAllValuesToEeprom() {
-	// Write all existing values to eeprom
-	for (int i=0; i < 150; i++)
-	{
-		if (validParam(i)) {
-			writeValueEeprom(i,paramValues[i]);
-		}
-	}
+int ParameterList::writeAllValuesToEeprom()
+{
+  // Write all existing values to eeprom
+  for (int i = 0; i < 150; i++)
+  {
+    if (validParam(i))
+    {
+      writeValueEeprom(i, paramValues[i]);
+    }
+  }
 }
 
 // ==== parameter valdation and defaults
 
-int ParameterList::setAllValuesToDefault() {
-	// Copy default values to the memory values
-	for (int i=0; i < PARAM_NR_OF_PARAMS; i++)
-	{
-		if (validParam(i)) {
-			loadDefaultValue(i);
-		}
-	}
+int ParameterList::setAllValuesToDefault()
+{
+  // Copy default values to the memory values
+  for (int i = 0; i < PARAM_NR_OF_PARAMS; i++)
+  {
+    if (validParam(i))
+    {
+      loadDefaultValue(i);
+    }
+  }
 }
 
-void ParameterList::loadDefaultValue(int id) {
+void ParameterList::loadDefaultValue(int id)
+{
 
-	switch(id)
-	{
-		case PARAM_VERSION                	: paramValues[id] = PARAM_VERSION_DEFAULT; break;
-		case PARAM_TEST                   	: paramValues[id] = PARAM_TEST_DEFAULT; break;
+  switch (id)
+  {
+  case PARAM_VERSION:
+    paramValues[id] = PARAM_VERSION_DEFAULT;
+    break;
+  case PARAM_TEST:
+    paramValues[id] = PARAM_TEST_DEFAULT;
+    break;
+  case PARAM_CONFIG_OK:
+    paramValues[id] = PARAM_CONFIG_OK_DEFAULT;
+    break;
+  case PARAM_USE_EEPROM:
+    paramValues[id] = PARAM_USE_EEPROM_DEFAULT;
+    break;
+  case PARAM_E_STOP_ON_MOV_ERR:
+    paramValues[id] = PARAM_E_STOP_ON_MOV_ERR_DEFAULT;
+    break;
+  case PARAM_MOV_NR_RETRY:
+    paramValues[id] = PARAM_MOV_NR_RETRY_DEFAULT;
+    break;
+  case MOVEMENT_TIMEOUT_X:
+    paramValues[id] = MOVEMENT_TIMEOUT_X_DEFAULT;
+    break;
+  case MOVEMENT_TIMEOUT_Y:
+    paramValues[id] = MOVEMENT_TIMEOUT_Y_DEFAULT;
+    break;
+  case MOVEMENT_TIMEOUT_Z:
+    paramValues[id] = MOVEMENT_TIMEOUT_Z_DEFAULT;
+    break;
 
-        	case MOVEMENT_TIMEOUT_X           	: paramValues[id] = MOVEMENT_TIMEOUT_X_DEFAULT; break;
-        	case MOVEMENT_TIMEOUT_Y           	: paramValues[id] = MOVEMENT_TIMEOUT_Y_DEFAULT; break;
-	        case MOVEMENT_TIMEOUT_Z           	: paramValues[id] = MOVEMENT_TIMEOUT_Z_DEFAULT; break;
+  case MOVEMENT_KEEP_ACTIVE_X:
+    paramValues[id] = MOVEMENT_KEEP_ACTIVE_X_DEFAULT;
+    break;
+  case MOVEMENT_KEEP_ACTIVE_Y:
+    paramValues[id] = MOVEMENT_KEEP_ACTIVE_Y_DEFAULT;
+    break;
+  case MOVEMENT_KEEP_ACTIVE_Z:
+    paramValues[id] = MOVEMENT_KEEP_ACTIVE_Z_DEFAULT;
+    break;
 
-	        case MOVEMENT_INVERT_ENDPOINTS_X  	: paramValues[id] = MOVEMENT_INVERT_ENDPOINTS_X_DEFAULT; break;
-	        case MOVEMENT_INVERT_ENDPOINTS_Y  	: paramValues[id] = MOVEMENT_INVERT_ENDPOINTS_Y_DEFAULT; break;
-	        case MOVEMENT_INVERT_ENDPOINTS_Z  	: paramValues[id] = MOVEMENT_INVERT_ENDPOINTS_Z_DEFAULT; break;
+  case MOVEMENT_HOME_AT_BOOT_X:
+    paramValues[id] = MOVEMENT_HOME_AT_BOOT_X_DEFAULT;
+    break;
+  case MOVEMENT_HOME_AT_BOOT_Y:
+    paramValues[id] = MOVEMENT_HOME_AT_BOOT_Y_DEFAULT;
+    break;
+  case MOVEMENT_HOME_AT_BOOT_Z:
+    paramValues[id] = MOVEMENT_HOME_AT_BOOT_Z_DEFAULT;
+    break;
 
-                case MOVEMENT_ENABLE_ENDPOINTS_X        : paramValues[id] = MOVEMENT_ENABLE_ENDPOINTS_X_DEFAULT; break;
-                case MOVEMENT_ENABLE_ENDPOINTS_Y        : paramValues[id] = MOVEMENT_ENABLE_ENDPOINTS_Y_DEFAULT; break;
-                case MOVEMENT_ENABLE_ENDPOINTS_Z        : paramValues[id] = MOVEMENT_ENABLE_ENDPOINTS_Z_DEFAULT; break;
+  case MOVEMENT_INVERT_ENDPOINTS_X:
+    paramValues[id] = MOVEMENT_INVERT_ENDPOINTS_X_DEFAULT;
+    break;
+  case MOVEMENT_INVERT_ENDPOINTS_Y:
+    paramValues[id] = MOVEMENT_INVERT_ENDPOINTS_Y_DEFAULT;
+    break;
+  case MOVEMENT_INVERT_ENDPOINTS_Z:
+    paramValues[id] = MOVEMENT_INVERT_ENDPOINTS_Z_DEFAULT;
+    break;
 
-	        case MOVEMENT_INVERT_MOTOR_X      	: paramValues[id] = MOVEMENT_INVERT_MOTOR_X_DEFAULT; break;
-	        case MOVEMENT_INVERT_MOTOR_Y      	: paramValues[id] = MOVEMENT_INVERT_MOTOR_Y_DEFAULT; break;
-	        case MOVEMENT_INVERT_MOTOR_Z      	: paramValues[id] = MOVEMENT_INVERT_MOTOR_Z_DEFAULT; break;
+  case MOVEMENT_ENABLE_ENDPOINTS_X:
+    paramValues[id] = MOVEMENT_ENABLE_ENDPOINTS_X_DEFAULT;
+    break;
+  case MOVEMENT_ENABLE_ENDPOINTS_Y:
+    paramValues[id] = MOVEMENT_ENABLE_ENDPOINTS_Y_DEFAULT;
+    break;
+  case MOVEMENT_ENABLE_ENDPOINTS_Z:
+    paramValues[id] = MOVEMENT_ENABLE_ENDPOINTS_Z_DEFAULT;
+    break;
 
-	        case MOVEMENT_SECONDARY_MOTOR_X      	: paramValues[id] = MOVEMENT_SECONDARY_MOTOR_X_DEFAULT; break;
-	        case MOVEMENT_SECONDARY_MOTOR_INVERT_X  : paramValues[id] = MOVEMENT_SECONDARY_MOTOR_INVERT_X_DEFAULT; break;
+  case MOVEMENT_INVERT_MOTOR_X:
+    paramValues[id] = MOVEMENT_INVERT_MOTOR_X_DEFAULT;
+    break;
+  case MOVEMENT_INVERT_MOTOR_Y:
+    paramValues[id] = MOVEMENT_INVERT_MOTOR_Y_DEFAULT;
+    break;
+  case MOVEMENT_INVERT_MOTOR_Z:
+    paramValues[id] = MOVEMENT_INVERT_MOTOR_Z_DEFAULT;
+    break;
 
-	        case MOVEMENT_STEPS_ACC_DEC_X     	: paramValues[id] = MOVEMENT_STEPS_ACC_DEC_X_DEFAULT; break;
-	        case MOVEMENT_STEPS_ACC_DEC_Y     	: paramValues[id] = MOVEMENT_STEPS_ACC_DEC_Y_DEFAULT; break;
-	        case MOVEMENT_STEPS_ACC_DEC_Z     	: paramValues[id] = MOVEMENT_STEPS_ACC_DEC_Z_DEFAULT; break;
+  case MOVEMENT_SECONDARY_MOTOR_X:
+    paramValues[id] = MOVEMENT_SECONDARY_MOTOR_X_DEFAULT;
+    break;
+  case MOVEMENT_SECONDARY_MOTOR_INVERT_X:
+    paramValues[id] = MOVEMENT_SECONDARY_MOTOR_INVERT_X_DEFAULT;
+    break;
 
-	        case MOVEMENT_HOME_UP_X           	: paramValues[id] = MOVEMENT_HOME_UP_X_DEFAULT; break;
-	        case MOVEMENT_HOME_UP_Y           	: paramValues[id] = MOVEMENT_HOME_UP_Y_DEFAULT; break;
-	        case MOVEMENT_HOME_UP_Z          	: paramValues[id] = MOVEMENT_HOME_UP_Z_DEFAULT; break;
+  case MOVEMENT_STEPS_ACC_DEC_X:
+    paramValues[id] = MOVEMENT_STEPS_ACC_DEC_X_DEFAULT;
+    break;
+  case MOVEMENT_STEPS_ACC_DEC_Y:
+    paramValues[id] = MOVEMENT_STEPS_ACC_DEC_Y_DEFAULT;
+    break;
+  case MOVEMENT_STEPS_ACC_DEC_Z:
+    paramValues[id] = MOVEMENT_STEPS_ACC_DEC_Z_DEFAULT;
+    break;
 
-	        case MOVEMENT_MIN_SPD_X           	: paramValues[id] = MOVEMENT_MIN_SPD_X_DEFAULT; break;
-	        case MOVEMENT_MIN_SPD_Y           	: paramValues[id] = MOVEMENT_MIN_SPD_Y_DEFAULT; break;
-	        case MOVEMENT_MIN_SPD_Z           	: paramValues[id] = MOVEMENT_MIN_SPD_Z_DEFAULT; break;
+  case MOVEMENT_STOP_AT_HOME_X:
+    paramValues[id] = MOVEMENT_STOP_AT_HOME_X_DEFAULT;
+    break;
+  case MOVEMENT_STOP_AT_HOME_Y:
+    paramValues[id] = MOVEMENT_STOP_AT_HOME_Y_DEFAULT;
+    break;
+  case MOVEMENT_STOP_AT_HOME_Z:
+    paramValues[id] = MOVEMENT_STOP_AT_HOME_Z_DEFAULT;
+    break;
 
-	        case MOVEMENT_MAX_SPD_X           	: paramValues[id] = MOVEMENT_MAX_SPD_X_DEFAULT; break;
-	        case MOVEMENT_MAX_SPD_Y           	: paramValues[id] = MOVEMENT_MAX_SPD_Y_DEFAULT; break;
-	        case MOVEMENT_MAX_SPD_Z           	: paramValues[id] = MOVEMENT_MAX_SPD_Z_DEFAULT; break;
+  case MOVEMENT_HOME_UP_X:
+    paramValues[id] = MOVEMENT_HOME_UP_X_DEFAULT;
+    break;
+  case MOVEMENT_HOME_UP_Y:
+    paramValues[id] = MOVEMENT_HOME_UP_Y_DEFAULT;
+    break;
+  case MOVEMENT_HOME_UP_Z:
+    paramValues[id] = MOVEMENT_HOME_UP_Z_DEFAULT;
+    break;
 
-	        case ENCODER_ENABLED_X            	: paramValues[id] = ENCODER_ENABLED_X_DEFAULT; break;
-	        case ENCODER_ENABLED_Y            	: paramValues[id] = ENCODER_ENABLED_Y_DEFAULT; break;
-	        case ENCODER_ENABLED_Z            	: paramValues[id] = ENCODER_ENABLED_Z_DEFAULT; break;
+  case MOVEMENT_STEP_PER_MM_X:
+    paramValues[id] = MOVEMENT_STEP_PER_MM_X_DEFAULT;
+    break;
+  case MOVEMENT_STEP_PER_MM_Y:
+    paramValues[id] = MOVEMENT_STEP_PER_MM_Y_DEFAULT;
+    break;
+  case MOVEMENT_STEP_PER_MM_Z:
+    paramValues[id] = MOVEMENT_STEP_PER_MM_Z_DEFAULT;
+    break;
 
-	        case ENCODER_MISSED_STEPS_MAX_X   	: paramValues[id] = ENCODER_MISSED_STEPS_MAX_X_DEFAULT; break;
-	        case ENCODER_MISSED_STEPS_MAX_Y   	: paramValues[id] = ENCODER_MISSED_STEPS_MAX_Y_DEFAULT; break;
-	        case ENCODER_MISSED_STEPS_MAX_Z   	: paramValues[id] = ENCODER_MISSED_STEPS_MAX_Z_DEFAULT; break;
+  case MOVEMENT_MIN_SPD_X:
+    paramValues[id] = MOVEMENT_MIN_SPD_X_DEFAULT;
+    break;
+  case MOVEMENT_MIN_SPD_Y:
+    paramValues[id] = MOVEMENT_MIN_SPD_Y_DEFAULT;
+    break;
+  case MOVEMENT_MIN_SPD_Z:
+    paramValues[id] = MOVEMENT_MIN_SPD_Z_DEFAULT;
+    break;
 
-	        case ENCODER_MISSED_STEPS_DECAY_X 	: paramValues[id] = ENCODER_MISSED_STEPS_DECAY_X_DEFAULT; break;
-	        case ENCODER_MISSED_STEPS_DECAY_Y 	: paramValues[id] = ENCODER_MISSED_STEPS_DECAY_Y_DEFAULT; break;
-	        case ENCODER_MISSED_STEPS_DECAY_Z 	: paramValues[id] = ENCODER_MISSED_STEPS_DECAY_Z_DEFAULT; break;
+  case MOVEMENT_HOME_SPEED_X:
+    paramValues[id] = MOVEMENT_HOME_SPEED_X_DEFAULT;
+    break;
+  case MOVEMENT_HOME_SPEED_Y:
+    paramValues[id] = MOVEMENT_HOME_SPEED_Y_DEFAULT;
+    break;
+  case MOVEMENT_HOME_SPEED_Z:
+    paramValues[id] = MOVEMENT_HOME_SPEED_Z_DEFAULT;
+    break;
 
+  case MOVEMENT_MAX_SPD_X:
+    paramValues[id] = MOVEMENT_MAX_SPD_X_DEFAULT;
+    break;
+  case MOVEMENT_MAX_SPD_Y:
+    paramValues[id] = MOVEMENT_MAX_SPD_Y_DEFAULT;
+    break;
+  case MOVEMENT_MAX_SPD_Z:
+    paramValues[id] = MOVEMENT_MAX_SPD_Z_DEFAULT;
+    break;
 
-		case PIN_GUARD_1_PIN_NR           	: paramValues[id] = PIN_GUARD_1_PIN_NR_DEFAULT; break;
-		case PIN_GUARD_1_TIME_OUT         	: paramValues[id] = PIN_GUARD_1_TIME_OUT_DEFAULT; break;
-		case PIN_GUARD_1_ACTIVE_STATE     	: paramValues[id] = PIN_GUARD_1_ACTIVE_STATE_DEFAULT; break;
+  case MOVEMENT_INVERT_2_ENDPOINTS_X:
+    paramValues[id] = MOVEMENT_INVERT_2_ENDPOINTS_X_DEFAULT;
+    break;
+  case MOVEMENT_INVERT_2_ENDPOINTS_Y:
+    paramValues[id] = MOVEMENT_INVERT_2_ENDPOINTS_Y_DEFAULT;
+    break;
+  case MOVEMENT_INVERT_2_ENDPOINTS_Z:
+    paramValues[id] = MOVEMENT_INVERT_2_ENDPOINTS_Z_DEFAULT;
+    break;
 
-		case PIN_GUARD_2_PIN_NR           	: paramValues[id] = PIN_GUARD_2_PIN_NR_DEFAULT; break;
-		case PIN_GUARD_2_TIME_OUT         	: paramValues[id] = PIN_GUARD_2_TIME_OUT_DEFAULT; break;
-		case PIN_GUARD_2_ACTIVE_STATE     	: paramValues[id] = PIN_GUARD_2_ACTIVE_STATE_DEFAULT; break;
+  case MOVEMENT_STOP_AT_MAX_X:
+    paramValues[id] = MOVEMENT_STOP_AT_MAX_X_DEFAULT;
+    break;
+  case MOVEMENT_STOP_AT_MAX_Y:
+    paramValues[id] = MOVEMENT_STOP_AT_MAX_Y_DEFAULT;
+    break;
+  case MOVEMENT_STOP_AT_MAX_Z:
+    paramValues[id] = MOVEMENT_STOP_AT_MAX_Z_DEFAULT;
+    break;
 
-		case PIN_GUARD_3_PIN_NR           	: paramValues[id] = PIN_GUARD_3_PIN_NR_DEFAULT; break;
-		case PIN_GUARD_3_TIME_OUT         	: paramValues[id] = PIN_GUARD_3_TIME_OUT_DEFAULT; break;
-		case PIN_GUARD_3_ACTIVE_STATE     	: paramValues[id] = PIN_GUARD_3_ACTIVE_STATE_DEFAULT; break;
+  case 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 PIN_GUARD_4_PIN_NR           	: paramValues[id] = PIN_GUARD_4_PIN_NR_DEFAULT; break;
-		case PIN_GUARD_4_TIME_OUT         	: paramValues[id] = PIN_GUARD_4_TIME_OUT_DEFAULT; break;
-		case PIN_GUARD_4_ACTIVE_STATE     	: paramValues[id] = PIN_GUARD_4_ACTIVE_STATE_DEFAULT; break;
+  case 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 PIN_GUARD_5_PIN_NR           	: paramValues[id] = PIN_GUARD_5_PIN_NR_DEFAULT; break;
-		case PIN_GUARD_5_TIME_OUT         	: paramValues[id] = PIN_GUARD_5_TIME_OUT_DEFAULT; break;
-		case PIN_GUARD_5_ACTIVE_STATE     	: paramValues[id] = PIN_GUARD_5_ACTIVE_STATE_DEFAULT; break;
+  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;
 
-		default 				: paramValues[id] = 0; break;
-	}
+  case ENCODER_ENABLED_X:
+    paramValues[id] = ENCODER_ENABLED_X_DEFAULT;
+    break;
+  case ENCODER_ENABLED_Y:
+    paramValues[id] = ENCODER_ENABLED_Y_DEFAULT;
+    break;
+  case ENCODER_ENABLED_Z:
+    paramValues[id] = ENCODER_ENABLED_Z_DEFAULT;
+    break;
+
+  case ENCODER_TYPE_X:
+    paramValues[id] = ENCODER_TYPE_X_DEFAULT;
+    break;
+  case ENCODER_TYPE_Y:
+    paramValues[id] = ENCODER_TYPE_Y_DEFAULT;
+    break;
+  case ENCODER_TYPE_Z:
+    paramValues[id] = ENCODER_TYPE_Z_DEFAULT;
+    break;
+
+  case ENCODER_MISSED_STEPS_MAX_X:
+    paramValues[id] = ENCODER_MISSED_STEPS_MAX_X_DEFAULT;
+    break;
+  case ENCODER_MISSED_STEPS_MAX_Y:
+    paramValues[id] = ENCODER_MISSED_STEPS_MAX_Y_DEFAULT;
+    break;
+  case ENCODER_MISSED_STEPS_MAX_Z:
+    paramValues[id] = ENCODER_MISSED_STEPS_MAX_Z_DEFAULT;
+    break;
+
+  case ENCODER_SCALING_X:
+    paramValues[id] = ENCODER_SCALING_X_DEFAULT;
+    break;
+  case ENCODER_SCALING_Y:
+    paramValues[id] = ENCODER_SCALING_Y_DEFAULT;
+    break;
+  case ENCODER_SCALING_Z:
+    paramValues[id] = ENCODER_SCALING_Z_DEFAULT;
+    break;
+
+  case ENCODER_MISSED_STEPS_DECAY_X:
+    paramValues[id] = ENCODER_MISSED_STEPS_DECAY_X_DEFAULT;
+    break;
+  case ENCODER_MISSED_STEPS_DECAY_Y:
+    paramValues[id] = ENCODER_MISSED_STEPS_DECAY_Y_DEFAULT;
+    break;
+  case ENCODER_MISSED_STEPS_DECAY_Z:
+    paramValues[id] = ENCODER_MISSED_STEPS_DECAY_Z_DEFAULT;
+    break;
+
+  case ENCODER_USE_FOR_POS_X:
+    paramValues[id] = ENCODER_USE_FOR_POS_X_DEFAULT;
+    break;
+  case ENCODER_USE_FOR_POS_Y:
+    paramValues[id] = ENCODER_USE_FOR_POS_Y_DEFAULT;
+    break;
+  case ENCODER_USE_FOR_POS_Z:
+    paramValues[id] = ENCODER_USE_FOR_POS_Z_DEFAULT;
+    break;
+
+  case ENCODER_INVERT_X:
+    paramValues[id] = ENCODER_INVERT_X_DEFAULT;
+    break;
+  case ENCODER_INVERT_Y:
+    paramValues[id] = ENCODER_INVERT_Y_DEFAULT;
+    break;
+  case ENCODER_INVERT_Z:
+    paramValues[id] = ENCODER_INVERT_Z_DEFAULT;
+    break;
+
+  case PIN_GUARD_1_PIN_NR:
+    paramValues[id] = PIN_GUARD_1_PIN_NR_DEFAULT;
+    break;
+  case PIN_GUARD_1_TIME_OUT:
+    paramValues[id] = PIN_GUARD_1_TIME_OUT_DEFAULT;
+    break;
+  case PIN_GUARD_1_ACTIVE_STATE:
+    paramValues[id] = PIN_GUARD_1_ACTIVE_STATE_DEFAULT;
+    break;
+
+  case PIN_GUARD_2_PIN_NR:
+    paramValues[id] = PIN_GUARD_2_PIN_NR_DEFAULT;
+    break;
+  case PIN_GUARD_2_TIME_OUT:
+    paramValues[id] = PIN_GUARD_2_TIME_OUT_DEFAULT;
+    break;
+  case PIN_GUARD_2_ACTIVE_STATE:
+    paramValues[id] = PIN_GUARD_2_ACTIVE_STATE_DEFAULT;
+    break;
+
+  case PIN_GUARD_3_PIN_NR:
+    paramValues[id] = PIN_GUARD_3_PIN_NR_DEFAULT;
+    break;
+  case PIN_GUARD_3_TIME_OUT:
+    paramValues[id] = PIN_GUARD_3_TIME_OUT_DEFAULT;
+    break;
+  case PIN_GUARD_3_ACTIVE_STATE:
+    paramValues[id] = PIN_GUARD_3_ACTIVE_STATE_DEFAULT;
+    break;
+
+  case PIN_GUARD_4_PIN_NR:
+    paramValues[id] = PIN_GUARD_4_PIN_NR_DEFAULT;
+    break;
+  case PIN_GUARD_4_TIME_OUT:
+    paramValues[id] = PIN_GUARD_4_TIME_OUT_DEFAULT;
+    break;
+  case PIN_GUARD_4_ACTIVE_STATE:
+    paramValues[id] = PIN_GUARD_4_ACTIVE_STATE_DEFAULT;
+    break;
+
+  case PIN_GUARD_5_PIN_NR:
+    paramValues[id] = PIN_GUARD_5_PIN_NR_DEFAULT;
+    break;
+  case PIN_GUARD_5_TIME_OUT:
+    paramValues[id] = PIN_GUARD_5_TIME_OUT_DEFAULT;
+    break;
+  case PIN_GUARD_5_ACTIVE_STATE:
+    paramValues[id] = PIN_GUARD_5_ACTIVE_STATE_DEFAULT;
+    break;
+
+  default:
+    paramValues[id] = 0;
+    break;
+  }
 }
 
-bool ParameterList::validParam(int id) {
+bool ParameterList::validParam(int id)
+{
 
-	// Check if the id is a valid one
-	switch(id)
-	{
-		case PARAM_VERSION:
-		case MOVEMENT_TIMEOUT_X:
-		case MOVEMENT_TIMEOUT_Y:
-		case MOVEMENT_TIMEOUT_Z:
-                case MOVEMENT_ENABLE_ENDPOINTS_X:
-                case MOVEMENT_ENABLE_ENDPOINTS_Y:
-                case MOVEMENT_ENABLE_ENDPOINTS_Z:
-		case MOVEMENT_INVERT_ENDPOINTS_X:
-		case MOVEMENT_INVERT_ENDPOINTS_Y:
-		case MOVEMENT_INVERT_ENDPOINTS_Z:
-		case MOVEMENT_INVERT_MOTOR_X:
-		case MOVEMENT_INVERT_MOTOR_Y:
-		case MOVEMENT_INVERT_MOTOR_Z:
-	        case MOVEMENT_SECONDARY_MOTOR_X:
-	        case MOVEMENT_SECONDARY_MOTOR_INVERT_X:
-		case MOVEMENT_STEPS_ACC_DEC_X:
-		case MOVEMENT_STEPS_ACC_DEC_Y:
-		case MOVEMENT_STEPS_ACC_DEC_Z:
-		case MOVEMENT_HOME_UP_X:
-		case MOVEMENT_HOME_UP_Y:
-		case MOVEMENT_HOME_UP_Z:
-		case MOVEMENT_MIN_SPD_X:
-		case MOVEMENT_MIN_SPD_Y:
-		case MOVEMENT_MIN_SPD_Z:
-		case MOVEMENT_MAX_SPD_X:
-		case MOVEMENT_MAX_SPD_Y:
-		case MOVEMENT_MAX_SPD_Z:
-		case ENCODER_ENABLED_X:
-		case ENCODER_ENABLED_Y:
-		case ENCODER_ENABLED_Z:
-		case ENCODER_MISSED_STEPS_MAX_X:
-		case ENCODER_MISSED_STEPS_MAX_Y:
-		case ENCODER_MISSED_STEPS_MAX_Z:
-		case ENCODER_MISSED_STEPS_DECAY_X:
-		case ENCODER_MISSED_STEPS_DECAY_Y:
-		case ENCODER_MISSED_STEPS_DECAY_Z:
-                case PIN_GUARD_1_PIN_NR:
-                case PIN_GUARD_1_TIME_OUT:
-                case PIN_GUARD_1_ACTIVE_STATE:
-                case PIN_GUARD_2_PIN_NR:
-                case PIN_GUARD_2_TIME_OUT:
-                case PIN_GUARD_2_ACTIVE_STATE:
-                case PIN_GUARD_3_PIN_NR:
-                case PIN_GUARD_3_TIME_OUT:
-                case PIN_GUARD_3_ACTIVE_STATE:
-                case PIN_GUARD_4_PIN_NR:
-                case PIN_GUARD_4_TIME_OUT:
-                case PIN_GUARD_4_ACTIVE_STATE:
-                case PIN_GUARD_5_PIN_NR:
-                case PIN_GUARD_5_TIME_OUT:
-                case PIN_GUARD_5_ACTIVE_STATE:
-			return true;
-		default:
-			return false;
-	}
+  // Check if the id is a valid one
+  switch (id)
+  {
+  case PARAM_VERSION:
+  case PARAM_CONFIG_OK:
+  case PARAM_USE_EEPROM:
+  case PARAM_E_STOP_ON_MOV_ERR:
+  case PARAM_MOV_NR_RETRY:
+  case MOVEMENT_TIMEOUT_X:
+  case MOVEMENT_TIMEOUT_Y:
+  case MOVEMENT_TIMEOUT_Z:
+  case MOVEMENT_KEEP_ACTIVE_X:
+  case MOVEMENT_KEEP_ACTIVE_Y:
+  case MOVEMENT_KEEP_ACTIVE_Z:
+  case MOVEMENT_HOME_AT_BOOT_X:
+  case MOVEMENT_HOME_AT_BOOT_Y:
+  case MOVEMENT_HOME_AT_BOOT_Z:
+  case MOVEMENT_ENABLE_ENDPOINTS_X:
+  case MOVEMENT_ENABLE_ENDPOINTS_Y:
+  case MOVEMENT_ENABLE_ENDPOINTS_Z:
+  case MOVEMENT_INVERT_ENDPOINTS_X:
+  case MOVEMENT_INVERT_ENDPOINTS_Y:
+  case MOVEMENT_INVERT_ENDPOINTS_Z:
+  case MOVEMENT_INVERT_2_ENDPOINTS_X:
+  case MOVEMENT_INVERT_2_ENDPOINTS_Y:
+  case MOVEMENT_INVERT_2_ENDPOINTS_Z:
+  case MOVEMENT_INVERT_MOTOR_X:
+  case MOVEMENT_INVERT_MOTOR_Y:
+  case MOVEMENT_INVERT_MOTOR_Z:
+  case MOVEMENT_SECONDARY_MOTOR_X:
+  case MOVEMENT_SECONDARY_MOTOR_INVERT_X:
+  case MOVEMENT_STEPS_ACC_DEC_X:
+  case MOVEMENT_STEPS_ACC_DEC_Y:
+  case MOVEMENT_STEPS_ACC_DEC_Z:
+  case MOVEMENT_STOP_AT_HOME_X:
+  case MOVEMENT_STOP_AT_HOME_Y:
+  case MOVEMENT_STOP_AT_HOME_Z:
+  case MOVEMENT_HOME_UP_X:
+  case MOVEMENT_HOME_UP_Y:
+  case MOVEMENT_HOME_UP_Z:
+  case MOVEMENT_STEP_PER_MM_X:
+  case MOVEMENT_STEP_PER_MM_Y:
+  case MOVEMENT_STEP_PER_MM_Z:
+  case MOVEMENT_MIN_SPD_X:
+  case MOVEMENT_MIN_SPD_Y:
+  case MOVEMENT_MIN_SPD_Z:
+  case MOVEMENT_HOME_SPEED_X:
+  case MOVEMENT_HOME_SPEED_Y:
+  case MOVEMENT_HOME_SPEED_Z:
+  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:
+  case ENCODER_TYPE_X:
+  case ENCODER_TYPE_Y:
+  case ENCODER_TYPE_Z:
+  case ENCODER_MISSED_STEPS_MAX_X:
+  case ENCODER_MISSED_STEPS_MAX_Y:
+  case ENCODER_MISSED_STEPS_MAX_Z:
+  case ENCODER_SCALING_X:
+  case ENCODER_SCALING_Y:
+  case ENCODER_SCALING_Z:
+  case ENCODER_MISSED_STEPS_DECAY_X:
+  case ENCODER_MISSED_STEPS_DECAY_Y:
+  case ENCODER_MISSED_STEPS_DECAY_Z:
+  case ENCODER_USE_FOR_POS_X:
+  case ENCODER_USE_FOR_POS_Y:
+  case ENCODER_USE_FOR_POS_Z:
+  case ENCODER_INVERT_X:
+  case ENCODER_INVERT_Y:
+  case ENCODER_INVERT_Z:
+  case MOVEMENT_AXIS_NR_STEPS_X:
+  case MOVEMENT_AXIS_NR_STEPS_Y:
+  case MOVEMENT_AXIS_NR_STEPS_Z:
+  case MOVEMENT_STOP_AT_MAX_X:
+  case MOVEMENT_STOP_AT_MAX_Y:
+  case MOVEMENT_STOP_AT_MAX_Z:
+  case PIN_GUARD_1_PIN_NR:
+  case PIN_GUARD_1_TIME_OUT:
+  case PIN_GUARD_1_ACTIVE_STATE:
+  case PIN_GUARD_2_PIN_NR:
+  case PIN_GUARD_2_TIME_OUT:
+  case PIN_GUARD_2_ACTIVE_STATE:
+  case PIN_GUARD_3_PIN_NR:
+  case PIN_GUARD_3_TIME_OUT:
+  case PIN_GUARD_3_ACTIVE_STATE:
+  case PIN_GUARD_4_PIN_NR:
+  case PIN_GUARD_4_TIME_OUT:
+  case PIN_GUARD_4_ACTIVE_STATE:
+  case PIN_GUARD_5_PIN_NR:
+  case PIN_GUARD_5_TIME_OUT:
+  case PIN_GUARD_5_ACTIVE_STATE:
+    return true;
+  default:
+    return false;
+  }
 }
-

src/ParameterList.h

@@ -7,90 +7,158 @@
 #include "CurrentState.h"
 
 //#define NULL 0
-const int PARAM_NR_OF_PARAMS = 300;
-
+const int PARAM_NR_OF_PARAMS = 225;
 
 enum ParamListEnum
 {
-	PARAM_VERSION 				= 0,
-	PARAM_TEST 				= 1,
+  PARAM_VERSION = 0,
+  PARAM_TEST = 1,
+  PARAM_CONFIG_OK = 2,
+  PARAM_USE_EEPROM = 3,
+  PARAM_E_STOP_ON_MOV_ERR = 4,
 
-	// stepper motor settings
+  PARAM_MOV_NR_RETRY = 5,
 
-	MOVEMENT_TIMEOUT_X			= 11,
-	MOVEMENT_TIMEOUT_Y			= 12,
-	MOVEMENT_TIMEOUT_Z			= 13,
+  // stepper motor settings
 
-	MOVEMENT_INVERT_ENDPOINTS_X		= 21,
-	MOVEMENT_INVERT_ENDPOINTS_Y		= 22,
-	MOVEMENT_INVERT_ENDPOINTS_Z		= 23,
+  MOVEMENT_TIMEOUT_X = 11,
+  MOVEMENT_TIMEOUT_Y = 12,
+  MOVEMENT_TIMEOUT_Z = 13,
 
-	MOVEMENT_ENABLE_ENDPOINTS_X		= 25,
-	MOVEMENT_ENABLE_ENDPOINTS_Y		= 26,
-	MOVEMENT_ENABLE_ENDPOINTS_Z		= 27,
+  MOVEMENT_KEEP_ACTIVE_X = 15,
+  MOVEMENT_KEEP_ACTIVE_Y = 16,
+  MOVEMENT_KEEP_ACTIVE_Z = 17,
 
-	MOVEMENT_INVERT_MOTOR_X			= 31,
-	MOVEMENT_INVERT_MOTOR_Y			= 32,
-	MOVEMENT_INVERT_MOTOR_Z			= 33,
+  MOVEMENT_HOME_AT_BOOT_X = 18,
+  MOVEMENT_HOME_AT_BOOT_Y = 19,
+  MOVEMENT_HOME_AT_BOOT_Z = 20,
 
-	MOVEMENT_SECONDARY_MOTOR_X		= 36,
-	MOVEMENT_SECONDARY_MOTOR_INVERT_X	= 37,
+  MOVEMENT_INVERT_ENDPOINTS_X = 21,
+  MOVEMENT_INVERT_ENDPOINTS_Y = 22,
+  MOVEMENT_INVERT_ENDPOINTS_Z = 23,
 
-	MOVEMENT_STEPS_ACC_DEC_X		= 41,
-	MOVEMENT_STEPS_ACC_DEC_Y		= 42,
-	MOVEMENT_STEPS_ACC_DEC_Z		= 43,
+  MOVEMENT_ENABLE_ENDPOINTS_X = 25,
+  MOVEMENT_ENABLE_ENDPOINTS_Y = 26,
+  MOVEMENT_ENABLE_ENDPOINTS_Z = 27,
 
-	MOVEMENT_HOME_UP_X			= 51,
-	MOVEMENT_HOME_UP_Y			= 52,
-	MOVEMENT_HOME_UP_Z			= 53,
+  MOVEMENT_INVERT_MOTOR_X = 31,
+  MOVEMENT_INVERT_MOTOR_Y = 32,
+  MOVEMENT_INVERT_MOTOR_Z = 33,
 
-	MOVEMENT_MIN_SPD_X			= 61,
-	MOVEMENT_MIN_SPD_Y			= 62,
-	MOVEMENT_MIN_SPD_Z			= 63,
+  MOVEMENT_SECONDARY_MOTOR_X = 36,
+  MOVEMENT_SECONDARY_MOTOR_INVERT_X = 37,
 
-	MOVEMENT_MAX_SPD_X			= 71,
-	MOVEMENT_MAX_SPD_Y			= 72,
-	MOVEMENT_MAX_SPD_Z			= 73,
+  MOVEMENT_STEPS_ACC_DEC_X = 41,
+  MOVEMENT_STEPS_ACC_DEC_Y = 42,
+  MOVEMENT_STEPS_ACC_DEC_Z = 43,
 
-	// encoder settings
-	ENCODER_ENABLED_X			= 101,
-	ENCODER_ENABLED_Y			= 102,
-	ENCODER_ENABLED_Z			= 103,
+  MOVEMENT_STOP_AT_HOME_X = 45,
+  MOVEMENT_STOP_AT_HOME_Y = 46,
+  MOVEMENT_STOP_AT_HOME_Z = 47,
 
-	ENCODER_MISSED_STEPS_MAX_X		= 111,
-	ENCODER_MISSED_STEPS_MAX_Y		= 112,
-	ENCODER_MISSED_STEPS_MAX_Z		= 113,
+  MOVEMENT_HOME_UP_X = 51,
+  MOVEMENT_HOME_UP_Y = 52,
+  MOVEMENT_HOME_UP_Z = 53,
 
-	ENCODER_MISSED_STEPS_DECAY_X		= 121,
-	ENCODER_MISSED_STEPS_DECAY_Y		= 122,
-	ENCODER_MISSED_STEPS_DECAY_Z		= 123,
+  MOVEMENT_STEP_PER_MM_X = 55,
+  MOVEMENT_STEP_PER_MM_Y = 56,
+  MOVEMENT_STEP_PER_MM_Z = 57,
 
-	// not used in software at this time
-	MOVEMENT_AXIS_NR_STEPS_X		= 141,
-	MOVEMENT_AXIS_NR_STEPS_Y		= 142,
-	MOVEMENT_AXIS_NR_STEPS_Z		= 143,
+  MOVEMENT_MIN_SPD_X = 61,
+  MOVEMENT_MIN_SPD_Y = 62,
+  MOVEMENT_MIN_SPD_Z = 63,
 
+  MOVEMENT_HOME_SPEED_X = 65,
+  MOVEMENT_HOME_SPEED_Y = 66,
+  MOVEMENT_HOME_SPEED_Z = 67,
 
-	// pin guard settings
-	PIN_GUARD_1_PIN_NR			= 201,
-	PIN_GUARD_1_TIME_OUT			= 202,
-	PIN_GUARD_1_ACTIVE_STATE		= 203,
+  MOVEMENT_MAX_SPD_X = 71,
+  MOVEMENT_MAX_SPD_Y = 72,
+  MOVEMENT_MAX_SPD_Z = 73,
 
-	PIN_GUARD_2_PIN_NR			= 205,
-	PIN_GUARD_2_TIME_OUT			= 206,
-	PIN_GUARD_2_ACTIVE_STATE		= 207,
+  // switch the end contacts from NO to NC
+  MOVEMENT_INVERT_2_ENDPOINTS_X = 75,
+  MOVEMENT_INVERT_2_ENDPOINTS_Y = 76,
+  MOVEMENT_INVERT_2_ENDPOINTS_Z = 77,
 
-	PIN_GUARD_3_PIN_NR			= 211,
-	PIN_GUARD_3_TIME_OUT			= 212,
-	PIN_GUARD_3_ACTIVE_STATE		= 213,
+  // motor current (used with TMC2130)
+  MOVEMENT_MOTOR_CURRENT_X = 81,
+  MOVEMENT_MOTOR_CURRENT_Y = 82,
+  MOVEMENT_MOTOR_CURRENT_Z = 83,
 
-	PIN_GUARD_4_PIN_NR			= 215,
-	PIN_GUARD_4_TIME_OUT			= 216,
-	PIN_GUARD_4_ACTIVE_STATE		= 217,
+  // stall sensitivity (used with TMC2130)
+  MOVEMENT_STALL_SENSITIVITY_X = 85,
+  MOVEMENT_STALL_SENSITIVITY_Y = 86,
+  MOVEMENT_STALL_SENSITIVITY_Z = 87,
 
-	PIN_GUARD_5_PIN_NR			= 221,
-	PIN_GUARD_5_TIME_OUT			= 222,
-	PIN_GUARD_5_ACTIVE_STATE		= 223
+  // 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,
+  ENCODER_ENABLED_Z = 103,
+
+  ENCODER_TYPE_X = 105,
+  ENCODER_TYPE_Y = 106,
+  ENCODER_TYPE_Z = 107,
+
+  ENCODER_MISSED_STEPS_MAX_X = 111,
+  ENCODER_MISSED_STEPS_MAX_Y = 112,
+  ENCODER_MISSED_STEPS_MAX_Z = 113,
+
+  ENCODER_SCALING_X = 115,
+  ENCODER_SCALING_Y = 116,
+  ENCODER_SCALING_Z = 117,
+
+  ENCODER_MISSED_STEPS_DECAY_X = 121,
+  ENCODER_MISSED_STEPS_DECAY_Y = 122,
+  ENCODER_MISSED_STEPS_DECAY_Z = 123,
+
+  ENCODER_USE_FOR_POS_X = 125,
+  ENCODER_USE_FOR_POS_Y = 126,
+  ENCODER_USE_FOR_POS_Z = 127,
+
+  ENCODER_INVERT_X = 131,
+  ENCODER_INVERT_Y = 132,
+  ENCODER_INVERT_Z = 133,
+
+  // 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,
+  MOVEMENT_STOP_AT_MAX_Z = 147,
+
+  // pin guard settings
+  PIN_GUARD_1_PIN_NR = 201,
+  PIN_GUARD_1_TIME_OUT = 202,
+  PIN_GUARD_1_ACTIVE_STATE = 203,
+
+  PIN_GUARD_2_PIN_NR = 205,
+  PIN_GUARD_2_TIME_OUT = 206,
+  PIN_GUARD_2_ACTIVE_STATE = 207,
+
+  PIN_GUARD_3_PIN_NR = 211,
+  PIN_GUARD_3_TIME_OUT = 212,
+  PIN_GUARD_3_ACTIVE_STATE = 213,
+
+  PIN_GUARD_4_PIN_NR = 215,
+  PIN_GUARD_4_TIME_OUT = 216,
+  PIN_GUARD_4_ACTIVE_STATE = 217,
+
+  PIN_GUARD_5_PIN_NR = 221,
+  PIN_GUARD_5_TIME_OUT = 222,
+  PIN_GUARD_5_ACTIVE_STATE = 223
 
 };
 
@@ -98,32 +166,39 @@ enum ParamListEnum
 #define NULL 0
 */
 
-class ParameterList {
-        ParamListEnum paramListEnum;
+class ParameterList
+{
+  ParamListEnum paramListEnum;
+
 public:
-        static ParameterList* getInstance();
-	int writeValue(int id, int value);
-	int readValue(int id);
-	int getValue(int id);
+  static ParameterList *getInstance();
+  int writeValue(int id, long value);
+  int readValue(int id);
+  long getValue(int id);
 
-	bool validParam(int id);
-	void loadDefaultValue(int id);
+  bool validParam(int id);
+  void loadDefaultValue(int id);
 
-	int readAllValues();
-	int readAllValuesFromEeprom();
-	int writeAllValuesToEeprom();
-	int setAllValuesToDefault();
+  int readAllValues();
+  int readAllValuesFromEeprom();
+  int writeAllValuesToEeprom();
+  int setAllValuesToDefault();
 
-	int readValueEeprom(int id);
-	int writeValueEeprom(int id, int value);
+  long readValueEeprom(int id);
+  int writeValueEeprom(int id, long value);
 
-	void sendConfigToModules();
+  void sendConfigToModules();
+
+  int paramChangeNumber();
+  int tmcParamChangeNumber();
 
 private:
-	ParameterList();
-        ParameterList(ParameterList const&);
-        void operator=(ParameterList const&);
+  ParameterList();
+  ParameterList(ParameterList const &);
+  void operator=(ParameterList const &);
+
+  int paramChangeNr;
+  int tmcParamChangeNr;
 };
 
-
 #endif /* PARAMETERLIST_H_ */

src/PinControl.cpp

@@ -1,71 +1,125 @@
 
 #include "PinControl.h"
 
-static PinControl* instance;
+static PinControl *instance;
 
-PinControl * PinControl::getInstance() {
-        if (!instance) {
-                instance = new PinControl();
-        };
-        return instance;
-}
-;
+PinControl *PinControl::getInstance()
+{
+  if (!instance)
+  {
+    instance = new PinControl();
+  };
+  return instance;
+};
+
+PinControl::PinControl()
+{
+  for (int pinNr = 1; pinNr <= 52; pinNr++)
+  {
+    pinWritten[0][pinNr] = false;
+    pinWritten[1][pinNr] = false;
+  }
 
-PinControl::PinControl() {
 }
 
-int PinControl::setMode(int pinNr, int mode) {
-	pinMode(pinNr   , mode );
-	return 0;
+int PinControl::setMode(int pinNr, int mode)
+{
+  pinMode(pinNr, mode);
+  return 0;
 }
 
-int PinControl::writeValue(int pinNr, int value, int mode) {
-	if (mode == 0) {
-		digitalWrite(pinNr, value);
-		return 0;
-	}
-	if (mode == 1) {
-		analogWrite(pinNr, value);
-		return 0;
-	}
-	return 1;
+int PinControl::writeValue(int pinNr, int value, int mode)
+{
+  if (pinNr > 0 && pinNr <= 52 && (mode == 0 || mode == 1))
+  {
+    pinWritten[mode][pinNr] = true;
+
+    if (mode == 0)
+    {
+      digitalWrite(pinNr, value);
+      return 0;
+    }
+    if (mode == 1)
+    {
+      analogWrite(pinNr, value);
+      return 0;
+    }
+  }
+  return 1;
 }
 
-int PinControl::readValue(int pinNr, int mode) {
+// Set all pins that were once used for writing to zero
+void PinControl::resetPinsUsed()
+{
+  for (int pinNr = 1; pinNr <= 52; pinNr++)
+  {
+    if (pinWritten[0][pinNr])
+    {
+      Serial.print("R99");
+      Serial.print(" resetting digital pin");
+      Serial.print(pinNr);
+      Serial.print("\r\n");
 
-	int value = 0;
+      digitalWrite(pinNr, false);
+      readValue(pinNr, 0);
+      pinWritten[0][pinNr] = false;
+    }
+    if (pinWritten[1][pinNr])
+    {
+      Serial.print("R99");
+      Serial.print(" resetting analog pin");
+      Serial.print(pinNr);
+      Serial.print("\r\n");
 
-	if (mode == 0) {
-		if (digitalRead(pinNr) == 1){
-			value = 1;
-		}
-	}
-	if (mode == 1) {
-		value = analogRead(pinNr);
-	}
-
-	if (mode == 0 || mode == 1) {
-
-		Serial.print("R41");
-		Serial.print(" ");
-		Serial.print("P");
-		Serial.print(pinNr);
-		Serial.print(" ");
-		Serial.print("V");
-		Serial.print(value);
-		//Serial.print("\r\n");
-		CurrentState::getInstance()->printQAndNewLine();
-
-		return 0;
-	}
-	else {
-		return 1;
-	}
+      analogWrite(pinNr, 0);
+      readValue(pinNr, 1);
+      pinWritten[1][pinNr] = false;
+    }
+  }
 }
 
-int  PinControl::writePulse(int pinNr, int valueOne, int valueTwo, long time, int mode) {
-	writeValue( pinNr, valueOne, mode);
-	delay(time);
-	writeValue( pinNr, valueTwo, mode);
-	return 0;
+int PinControl::readValue(int pinNr, int mode)
+{
+
+  int value = 0;
+
+  if (mode == 0)
+  {
+    if (digitalRead(pinNr) == 1)
+    {
+      value = 1;
+    }
+  }
+  if (mode == 1)
+  {
+    value = analogRead(pinNr);
+  }
+
+  if (mode == 0 || mode == 1)
+  {
+
+    Serial.print("R41");
+    Serial.print(" ");
+    Serial.print("P");
+    Serial.print(pinNr);
+    Serial.print(" ");
+    Serial.print("V");
+    Serial.print(value);
+    //Serial.print("\r\n");
+    CurrentState::getInstance()->printQAndNewLine();
+
+    return 0;
+  }
+  else
+  {
+    return 1;
+  }
+}
+
+int PinControl::writePulse(int pinNr, int valueOne, int valueTwo, long time, int mode)
+{
+  writeValue(pinNr, valueOne, mode);
+  delay(time);
+  writeValue(pinNr, valueTwo, mode);
+  return 0;
 }

src/PinControl.h

@@ -16,19 +16,24 @@
 #include <stdlib.h>
 #include "CurrentState.h"
 
-class PinControl {
+class PinControl
+{
 public:
-        static PinControl* getInstance();
+  static PinControl *getInstance();
 
-	int setMode(int pinNr, int mode);
-	int writeValue(int pinNr, int value, int mode);
-	int readValue(int pinNr, int mode);
-	int writePulse(int pinNr, int valueOne, int valueTwo, long time, int mode);
+  int setMode(int pinNr, int mode);
+  int writeValue(int pinNr, int value, int mode);
+  int readValue(int pinNr, int mode);
+  int writePulse(int pinNr, int valueOne, int valueTwo, long time, int mode);
+  void resetPinsUsed();
 
 private:
-        PinControl();
-        PinControl(PinControl const&);
-        void operator=(PinControl const&);
+  PinControl();
+  PinControl(PinControl const &);
+  void operator=(PinControl const &);
+
+  bool pinWritten[2][56];
+
 };
 
 #endif /* PINCONTROL_H_ */

src/PinGuard.cpp

@@ -1,39 +1,42 @@
 
 #include "PinGuard.h"
 
-static PinGuard* instance;
+static PinGuard *instance;
 
-PinGuard * PinGuard::getInstance() {
-        if (!instance) {
-                instance = new PinGuard();
-        };
-        return instance;
-}
-;
+PinGuard *PinGuard::getInstance()
+{
+  if (!instance)
+  {
+    instance = new PinGuard();
+  };
+  return instance;
+};
 
-PinGuard::PinGuard() {
-
-	pinGuardPin[0] = PinGuardPin();
-	pinGuardPin[1] = PinGuardPin();
-	pinGuardPin[2] = PinGuardPin();
-	pinGuardPin[3] = PinGuardPin();
-	pinGuardPin[4] = PinGuardPin();
-	loadConfig();
+PinGuard::PinGuard()
+{
 
+  pinGuardPin[0] = PinGuardPin();
+  pinGuardPin[1] = PinGuardPin();
+  pinGuardPin[2] = PinGuardPin();
+  pinGuardPin[3] = PinGuardPin();
+  pinGuardPin[4] = PinGuardPin();
+  loadConfig();
 }
 
-void PinGuard::loadConfig() {
-	pinGuardPin[0].loadPinConfig(PIN_GUARD_1_PIN_NR, PIN_GUARD_1_ACTIVE_STATE, PIN_GUARD_1_TIME_OUT);
-	pinGuardPin[1].loadPinConfig(PIN_GUARD_2_PIN_NR, PIN_GUARD_2_ACTIVE_STATE, PIN_GUARD_2_TIME_OUT);
-	pinGuardPin[2].loadPinConfig(PIN_GUARD_3_PIN_NR, PIN_GUARD_3_ACTIVE_STATE, PIN_GUARD_3_TIME_OUT);
-	pinGuardPin[3].loadPinConfig(PIN_GUARD_4_PIN_NR, PIN_GUARD_4_ACTIVE_STATE, PIN_GUARD_4_TIME_OUT);
-	pinGuardPin[4].loadPinConfig(PIN_GUARD_5_PIN_NR, PIN_GUARD_5_ACTIVE_STATE, PIN_GUARD_5_TIME_OUT);
+void PinGuard::loadConfig()
+{
+  pinGuardPin[0].loadPinConfig(PIN_GUARD_1_PIN_NR, PIN_GUARD_1_ACTIVE_STATE, PIN_GUARD_1_TIME_OUT);
+  pinGuardPin[1].loadPinConfig(PIN_GUARD_2_PIN_NR, PIN_GUARD_2_ACTIVE_STATE, PIN_GUARD_2_TIME_OUT);
+  pinGuardPin[2].loadPinConfig(PIN_GUARD_3_PIN_NR, PIN_GUARD_3_ACTIVE_STATE, PIN_GUARD_3_TIME_OUT);
+  pinGuardPin[3].loadPinConfig(PIN_GUARD_4_PIN_NR, PIN_GUARD_4_ACTIVE_STATE, PIN_GUARD_4_TIME_OUT);
+  pinGuardPin[4].loadPinConfig(PIN_GUARD_5_PIN_NR, PIN_GUARD_5_ACTIVE_STATE, PIN_GUARD_5_TIME_OUT);
 }
 
-void PinGuard::checkPins() {
-	pinGuardPin[0].processTick();
-	pinGuardPin[1].processTick();
-	pinGuardPin[2].processTick();
-	pinGuardPin[3].processTick();
-	pinGuardPin[4].processTick();
+void PinGuard::checkPins()
+{
+  pinGuardPin[0].processTick();
+  pinGuardPin[1].processTick();
+  pinGuardPin[2].processTick();
+  pinGuardPin[3].processTick();
+  pinGuardPin[4].processTick();
 }

src/PinGuard.h

@@ -17,27 +17,26 @@
 #include "PinGuardPin.h"
 #include "ParameterList.h"
 
-class PinGuard {
+class PinGuard
+{
 public:
-        static PinGuard* getInstance();
+  static PinGuard *getInstance();
 
-	void loadConfig();
-	void checkPins();
+  void loadConfig();
+  void checkPins();
 
 private:
+  //long pinTimeOut[100];
+  //long pinCurrentTime[100];
+  //void checkPin;
+  //bool pinSafeState[100];
 
+  PinGuardPin pinGuardPin[5];
+  //PinGuardPin test;
 
-	//long pinTimeOut[100];
-	//long pinCurrentTime[100];
-	//void checkPin;
-	//bool pinSafeState[100];
-
-	PinGuardPin pinGuardPin[5];
-	//PinGuardPin test;
-
-        PinGuard();
-        PinGuard(PinGuard const&);
-        void operator=(PinGuard const&);
+  PinGuard();
+  PinGuard(PinGuard const &);
+  void operator=(PinGuard const &);
 };
 
 #endif /* PINGUARD_H_ */

src/PinGuardPin.cpp

@@ -1,39 +1,47 @@
 
 #include "PinGuardPin.h"
 #include "ParameterList.h"
+#include "PinControl.h"
 
-PinGuardPin::PinGuardPin() {
-	pinNr = 0;
+PinGuardPin::PinGuardPin()
+{
+  pinNr = 0;
 }
 
 // Set the initial settings for what pin to check
-void PinGuardPin::loadPinConfig(int pinNrParamNr, int activeStateParamNr, int timeOutParamNr) {
+void PinGuardPin::loadPinConfig(int pinNrParamNr, int activeStateParamNr, int timeOutParamNr)
+{
 
-        pinNr       =  ParameterList::getInstance()->getValue(pinNrParamNr);
-        activeState = (ParameterList::getInstance()->getValue(activeStateParamNr)== 1);
-        timeOut     =  ParameterList::getInstance()->getValue(timeOutParamNr);
+  pinNr = ParameterList::getInstance()->getValue(pinNrParamNr);
+  activeState = (ParameterList::getInstance()->getValue(activeStateParamNr) == 1);
+  timeOut = ParameterList::getInstance()->getValue(timeOutParamNr);
 
-	timeOutCount	= 0;
+  timeOutCount = 0;
 }
 
 // Check each second if the time out is lapsed or the value has changed
-void PinGuardPin::processTick() {
+void PinGuardPin::processTick()
+{
 
-	if (pinNr==0) {
-		return;
-	}
+  if (pinNr == 0)
+  {
+    return;
+  }
 
-	currentStatePin = digitalRead(pinNr);
+  currentStatePin = digitalRead(pinNr);
 
-	if (currentStatePin != activeState) {
-		timeOutCount = 0;
-	} else {
-		timeOutCount++;
-	}
+  if (currentStatePin != activeState)
+  {
+    timeOutCount = 0;
+  }
+  else
+  {
+    timeOutCount++;
+  }
 
-	if (timeOutCount >= timeOut) {
-		digitalWrite(pinNr, !activeState);
-	}
+  if (timeOutCount >= timeOut)
+  {
+    digitalWrite(pinNr, !activeState);
+    PinControl::getInstance()->readValue(pinNr, 0);
+  }
 }
-
-

src/PinGuardPin.h

@@ -16,23 +16,23 @@
 #include <stdlib.h>
 //#include "ParameterList.h"
 
-class PinGuardPin {
+class PinGuardPin
+{
 public:
+  PinGuardPin();
 
-        PinGuardPin();
+  void processTick();
+  void loadPinConfig(int pinNrParamNr, int activeStateParamNr, int timeOutParamNr);
 
-	void processTick();
-	void loadPinConfig(int pinNrParamNr, int activeStateParamNr, int timeOutParamNr);
 private:
-        //PinControlPin(PinControlPin const&);
-        ///void operator=(PinControlPin const&);
-
-	int pinNr;
-	long timeOut;
-	long timeOutCount;
-	bool activeState;
-	bool currentStatePin;
+  //PinControlPin(PinControlPin const&);
+  ///void operator=(PinControlPin const&);
 
+  int pinNr;
+  long timeOut;
+  long timeOutCount;
+  bool activeState;
+  bool currentStatePin;
 };
 
 #endif /* PINGUARDPIN_H_ */

src/README.md

@@ -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/

src/ServoControl.cpp

@@ -7,29 +7,43 @@ Servo pin layout
 D11 D6 D5 D4
 */
 
-static ServoControl* instance;
+static ServoControl *instance;
 
-Servo servos[2];
+Servo servos[4];
 
-ServoControl * ServoControl::getInstance() {
-	if (!instance) {
-		instance = new ServoControl();
-	};
-	return instance;
-}
-;
+ServoControl *ServoControl::getInstance()
+{
+  if (!instance)
+  {
+    instance = new ServoControl();
+  };
+  return instance;
+};
 
-ServoControl::ServoControl() {
+ServoControl::ServoControl()
+{
 }
 
-int ServoControl::attach() {
-	servos[0].attach(SERVO_0_PIN);
-	servos[1].attach(SERVO_1_PIN);
+int ServoControl::attach()
+{
+  servos[0].attach(SERVO_0_PIN);
+  servos[1].attach(SERVO_1_PIN);
+  servos[2].attach(SERVO_2_PIN);
+  servos[3].attach(SERVO_3_PIN);
 }
 
-int ServoControl::setAngle(int pin, int angle) {
+void ServoControl::detachServos()
+{
+  servos[0].detach();
+  servos[1].detach();
+  servos[2].detach();
+  servos[3].detach();
+}
 
-/*
+int ServoControl::setAngle(int pin, int angle)
+{
+
+  /*
 	Serial.print("R99");
 	Serial.print(" ");
 	Serial.print("SERVO");
@@ -45,19 +59,27 @@ int ServoControl::setAngle(int pin, int angle) {
 	Serial.print("\r\n");
 */
 
-	switch(pin) {
-		case 4:
-		        servos[0].write(angle);
-			break;
-		case 5:
-		        servos[1].write(angle);
-			break;
-		default:  
-			return 1;
-	}
+  switch (pin)
+  {
+  case 4:
+    servos[0].attach(SERVO_0_PIN);
+    servos[0].write(angle);
+    break;
+  case 5:
+    servos[1].attach(SERVO_1_PIN);
+    servos[1].write(angle);
+    break;
+  case 6:
+    servos[2].attach(SERVO_2_PIN);
+    servos[2].write(angle);
+    break;
+  case 11:
+    servos[3].attach(SERVO_3_PIN);
+    servos[3].write(angle);
+    break;
+  default:
+    return 1;
+  }
 
-	return 0;
+  return 0;
 }
-
-
-

src/ServoControl.h

@@ -14,16 +14,19 @@
 #include <stdio.h>
 #include <stdlib.h>
 
-class ServoControl {
+class ServoControl
+{
 public:
-	static ServoControl* getInstance();
+  static ServoControl *getInstance();
+
+  int attach();
+  void detachServos();
+  int setAngle(int pin, int angle);
 
-	int attach();
-	int setAngle(int pin, int angle);
 private:
-	ServoControl();
-	ServoControl(ServoControl const&);
-	void operator=(ServoControl const&);
+  ServoControl();
+  ServoControl(ServoControl const &);
+  void operator=(ServoControl const &);
 };
 
 #endif /* SERVOCONTROL_H_ */

src/StatusList.cpp

@@ -1,50 +1,49 @@
 #include "StatusList.h"
 
-static StatusList* instanceParam;
+static StatusList *instanceParam;
 long statusValues[150];
 
-StatusList * StatusList::getInstance() {
-        if (!instanceParam) {
-                instanceParam = new StatusList();
-        };
-        return instanceParam;
+StatusList *StatusList::getInstance()
+{
+  if (!instanceParam)
+  {
+    instanceParam = new StatusList();
+  };
+  return instanceParam;
 }
 
-StatusList::StatusList() {
+StatusList::StatusList()
+{
 
+  statusValues[STATUS_GENERAL] = STATUS_GENERAL_DEFAULT;
 
-	statusValues[STATUS_GENERAL]               = STATUS_GENERAL_DEFAULT;
-
-	//paramValues[MOVEMENT_MAX_SPD_X]          = MOVEMENT_MAX_SPD_X_DEFAULT;
-	//paramValues[MOVEMENT_MAX_SPD_Y]          = MOVEMENT_MAX_SPD_Y_DEFAULT;
-	//paramValues[MOVEMENT_MAX_SPD_Z]          = MOVEMENT_MAX_SPD_Z_DEFAULT;
-
+  //paramValues[MOVEMENT_MAX_SPD_X]          = MOVEMENT_MAX_SPD_X_DEFAULT;
+  //paramValues[MOVEMENT_MAX_SPD_Y]          = MOVEMENT_MAX_SPD_Y_DEFAULT;
+  //paramValues[MOVEMENT_MAX_SPD_Z]          = MOVEMENT_MAX_SPD_Z_DEFAULT;
 }
 
+int StatusList::readValue(int id)
+{
 
-int StatusList::readValue(int id) {
+  long value = statusValues[id];
 
-	long value =  statusValues[id];
+  Serial.print("R31");
+  Serial.print(" ");
+  Serial.print("P");
+  Serial.print(id);
+  Serial.print(" ");
+  Serial.print("V");
+  Serial.print(value);
+  //Serial.print("\r\n");
+  CurrentState::getInstance()->printQAndNewLine();
 
-
-	Serial.print("R31");
-	Serial.print(" ");
-	Serial.print("P");
-	Serial.print(id);
-	Serial.print(" ");
-	Serial.print("V");
-	Serial.print(value);
-	//Serial.print("\r\n");
-	CurrentState::getInstance()->printQAndNewLine();
-
-
-	return 0;
+  return 0;
 }
 
+long StatusList::getValue(int id)
+{
 
-long StatusList::getValue(int id) {
-
-/*
+  /*
 	Serial.print("R99");
 	Serial.print(" ");
 	Serial.print("getValue");
@@ -55,13 +54,13 @@ long StatusList::getValue(int id) {
 	Serial.print("\r\n");
 */
 
-	return  statusValues[id];
+  return statusValues[id];
 }
 
-int StatusList::setValue(int id, long value) {
+int StatusList::setValue(int id, long value)
+{
 
-	statusValues[id] = value;
+  statusValues[id] = value;
 
-	return 0;
+  return 0;
 }
-

src/StatusList.h

@@ -7,14 +7,13 @@
 
 //#define NULL 0
 
-
 enum StatusListEnum
 {
-	STATUS_GENERAL			= 0,
+  STATUS_GENERAL = 0,
 
-	//MOVEMENT_MAX_SPD_X		= 71,
-	//MOVEMENT_MAX_SPD_Y		= 72,
-	//MOVEMENT_MAX_SPD_Z		= 73
+  //MOVEMENT_MAX_SPD_X		= 71,
+  //MOVEMENT_MAX_SPD_Y		= 72,
+  //MOVEMENT_MAX_SPD_Z		= 73
 
 };
 
@@ -22,19 +21,21 @@ enum StatusListEnum
 #define NULL 0
 */
 
-class StatusList {
-        StatusListEnum statusListEnum;
+class StatusList
+{
+  StatusListEnum statusListEnum;
+
 public:
-        static StatusList* getInstance();
-	int  writeValue(int id, long value);
-	int  readValue(int id);
-	long getValue(int id);
-	int  setValue(int id, long value);
+  static StatusList *getInstance();
+  int writeValue(int id, long value);
+  int readValue(int id);
+  long getValue(int id);
+  int setValue(int id, long value);
+
 private:
-	StatusList();
-        StatusList(StatusList const&);
-        void operator=(StatusList const&);
+  StatusList();
+  StatusList(StatusList const &);
+  void operator=(StatusList const &);
 };
 
-
 #endif /* STATUSLIST_H_ */