diff --git a/.gitignore b/.gitignore index d9256042..a0b3a9cd 100644 --- a/.gitignore +++ b/.gitignore @@ -18,4 +18,17 @@ #eclipse project files .cproject .project - +.settings/ +gdbserver.log +src/Segger_SystemView/Global.h +src/Segger_SystemView/SEGGER.h +src/Segger_SystemView/SEGGER_RTT.c +src/Segger_SystemView/SEGGER_RTT.h +src/Segger_SystemView/SEGGER_RTT_Conf.h +src/Segger_SystemView/SEGGER_SYSVIEW.c +src/Segger_SystemView/SEGGER_SYSVIEW.h +src/Segger_SystemView/SEGGER_SYSVIEW_Conf.h +src/Segger_SystemView/SEGGER_SYSVIEW_ConfDefaults.h +src/Segger_SystemView/SEGGER_SYSVIEW_Config_NoOS.c +src/Segger_SystemView/SEGGER_SYSVIEW_Int.h +README.md diff --git a/README.md b/README.md index 5de28e1a..481ade0c 100644 --- a/README.md +++ b/README.md @@ -1,8 +1,29 @@ # Smoothie2 -A Smoothie firmware port to the Smoothieboard v2 boards. +A Smoothie firmware port to the Bambino210E board ahead of making changes to support the SmoothieV2 hardware. -Current status : Gcode processing, motion planning, and step generation work. Many secondary features need porting, and some major lower level things need implementation ( USB, Ethernet, SDIO ). +There are no guarantees that this code works, won't burn down your house or destroy your electronics or hardware, use entirely at your own risk. + +There are a lot of bugs in the code and it is very much a work in progress, the reason it is published is to allow others to contribute before it is folded back into the Smoothie2 main repo. + +#IF YOU ARE NOT A CODER / WORKING ON THIS CODE BASE DO NOT TRY TO USE THIS UNTIL THE CODE HAS BEEN FULLY PORTED AND TESTED + +Current status as at 2016-11-08: +* SlowTicker code finally ported, but there are timing issues that need to be solved, including interrupt priorities to get it working properly (for end-stops for instance) +* The current focus is getting all the timing and interrupts and interrupt priorities sorted, then continue testing of ported code. +* Merged changes 65fb13e and c3f254c to bring codebase current with Smoothie V2 main repo +* Moved open issues to issue tracker +* This code base is equivalent to Smoothie V1 as at 2016-10-18 and Smoothie V2 as at 2016-11-08 +* The code that relies on the priority setting of GPIO interrupts has not had this feature migrated (e.g. laser) +* Some major lower level things need implementation ( USB, Ethernet, SDIO ). +* There some changes in `gcc4mbed/external/mbed/libraries/mbed/hal/sleep_api.h` which appear to be driven by Silicon Labs changes to support their EFM32 range of ARM processor's lower power modes. This causes conflicts with `sleep` and I have had to slightly modify this file to get our code to compile and work properly. I don't have time to go back and investigate this at this time. + +There are a lot of bugs in the code and it is very much a work in progress, the reason it is published is to allow others to contriute before it is folded back into the Smoothie2 main repo. + +Current status : +* This code base is equivalent to Smoothie V1 as at 2016-10-18. +* The Extruder code is being worked on and as of 2016-10-22 and does not yet compile. +* Some major lower level things need implementation ( USB, Ethernet, SDIO ). # Compiling @@ -27,50 +48,43 @@ And finally compile the code make # Board Specific Programming/Debugging Notes -* [Smoothie2 Pro Proto1 Boards](https://github.com/Smoothieware/Smoothie2/blob/master/notes/Smoothie2Proto1-board.creole#smoothie2-board-programmingdebugging-notes) * [Bambino210E Boards](https://github.com/Smoothieware/Smoothie2/blob/master/notes/Bambino210E-board.creole#bambino210e-board-programmingdebugging-notes) # TODO :  Current major TODOs :  - * Porting of existing functionality that hasn't been ported yet ( anything in https://github.com/Smoothieware/Smoothieware/tree/edge/src/modules that isn't ported yet ) - * Some modules have been ported, but the underlying low-level/HAL stuff to talk to the peripherals isn't ( like SPI, or ADCs etc ), it's just dummy objects right now. So that needs porting too. - * USB and Ethernet need to be implemented. - * Doing the step generation on the M0 co-processor instead of on the M4 main core. - * This port was forked from Smoothie months ago, it should be updated to incoprorate changes to Smoothie done in the meantime. - * Explore using http://nuttx.org/ for a RTOS ( major rewrite ) + * SD-Card, USB and Ethernet need to be implemented, awaiting base libraries from Micromint.com. + * Move the step generation to the M0 co-processor instead of on the M4 main core OR into a FPGA. TODO functionality to port from v1 in more detail : * libs/Adc.cpp : Port low level ADC functionality, then re-enable it in TemperatureControl ( note : there is some weird pin configuration surrounding this that makes it more complicated than it seems, but I don't remember what it is. Logxen@gmail.com probably remembers what it is ). * libs/md5.cpp : Port and re-enable in SimpleShell - * libs/Network : Port and enable + * libs/Network : Port and enable once base libraries are available * [@adamgreen](https://github.com/adamgreen) - libs/SDFAT.cpp : Port for SPI SD card access, not SDIO, and re-enable. Then add SDIO support. * [@adamgreen](https://github.com/adamgreen) - libs/SPI.cpp : Port and make to use the mBed library instead of registers - * libs/USBDevice : Port and enable + * libs/USBDevice : Port and enable once base libraries are available * libs/Watchdog : Port ( no mBed libraries available that I could find ) * libs/Hook.h : Uses doubles instead of ints ( in v1 ) for some reason, should be fixed - * modules/tools/drillingcycles : Port ( should be fairly simple ) - * modules/tools/extruder : Port + * modules/tools/filamentdetector : Get PWM to work + * modules/tools/spindle : Get PWM to work + * modules/tools/temperaturecontrol : Finish porting, get the ADC to actually work, I cannot test the AD8495 temperature inputs as I don't have one of these! + * modules/tools/zprobe : Ported Needs testing (2016-11-08) + * modules/tools/drillingcycles : Ported Needs testing (2016-11-08) + * modules/tools/extruder : Ported Needs testing (2016-11-08) * modules/tools/filamentdetector : Port * modules/tools/filamentdetector : Get PWM to work - * modules/tools/scaracal : Port - * modules/tools/spindle : Port and get PWM to work + * modules/tools/spindle : Get PWM to work * modules/tools/temperaturecontrol : Finish porting, get the ADC to actually work, port the AD8495 temperature input - * modules/tools/temperatureswitch : Port ( should be fairly easy ) - * modules/tools/toolmanager : Port ( should be trivial, is needed for Extruder ) - * modules/tools/touchprobe : Port ( could be quite easy ) - * modules/tools/zprobe : Port ( could be quite easy ) - * modules/tools/simpleshell : Port + * modules/tools/zprobe : Ported Needs testing (2016-11-08) + * modules/tools/simpleshell : Port once USB ported * The FPGA requires a SGPIO spi library * The FPGA requires a way to flash a .bin to it over JTAG from the M4 Things that are broken during the port and should be added back as things progress ( some things might be missing here. I try to use the TOADDBACK label when commenting things so search for that too ) :  -* In GcodeDispatch.cpp : Removed dependency to Pauser.h -* In Switch.cpp : Commented out all of the set_low_on_debug stuff +* In Switch.cpp : Commented out all of the set_low_on_debug stuff; do we still need this? * In SlowTicker.h and Hook.h : Using doubles instead of ints for intervals/frequencies and counting down. Done to accomodate mBed, but dirty costly hack -* In SlowTicker.h : Assuming it's fine to use mBed for this, we can remove a lot of code and just rely fully on mBed * In RingBuffer.h : Removed the irq stuff * In Laser.cpp : PWM pin is broken, needs to be fixed @@ -87,4 +101,4 @@ Planned refactors ( only to be done when the basic port is done ) :  * Try to read config directly from SD card without cache ( as smoothie used to do ) now that we have SDIO * Queue refactor ( see wiki ) * Adding MTP and removing MSD for USB -* Full rewrite based on a RTOS +* Explore using http://nuttx.org/ for a RTOS ( major rewrite ) diff --git a/gcc4mbed/external/mbed/libraries/mbed/hal/sleep_api.h b/gcc4mbed/external/mbed/libraries/mbed/hal/sleep_api.h index c8cf3b6f..3269c08a 100644 --- a/gcc4mbed/external/mbed/libraries/mbed/hal/sleep_api.h +++ b/gcc4mbed/external/mbed/libraries/mbed/hal/sleep_api.h @@ -18,7 +18,8 @@ #include "device.h" -#if DEVICE_SLEEP +//#if DEVICE_SLEEP //TODO THIS WAS THE ORIGINAL BUT CONFLICTS WITH OTHER DEFINITION OF SLEEP +#ifndef DEVICE_SLEEP #ifdef __cplusplus extern "C" { diff --git a/makefile b/makefile new file mode 100644 index 00000000..3fbd2885 --- /dev/null +++ b/makefile @@ -0,0 +1,35 @@ + +#DIRS = gcc4mbed src +DIRS = src +DIRSCLEAN = $(addsuffix .clean,$(DIRS)) + +all: +# @ $(MAKE) -C gcc4mbed + @echo Building Smoothie + @ $(MAKE) -C src + +clean: $(DIRSCLEAN) + +$(DIRSCLEAN): %.clean: + @echo Cleaning $* + @ $(MAKE) -C $* clean + +debug-store: + @ $(MAKE) -C src debug-store + +flash: + @ $(MAKE) -C src flash + +dfu: + @ $(MAKE) -C src dfu + +upload: + @ $(MAKE) -C src upload + +debug: + @ $(MAKE) -C src debug + +console: + @ $(MAKE) -C src console + +.PHONY: all $(DIRS) $(DIRSCLEAN) debug-store flash upload debug console dfu diff --git a/src/Segger_SystemView/README.md b/src/Segger_SystemView/README.md new file mode 100644 index 00000000..1d0390a1 --- /dev/null +++ b/src/Segger_SystemView/README.md @@ -0,0 +1,50 @@ +#Segger JTrace SystemView debugging and integration + +Go to http://www.segger.com and download their SystemView free tool and copy only some of the source code files so you end up with the following files: +``` +src/Segger_SystemView/SEGGER_SYSVIEW_Int.h +src/Segger_SystemView/Global.h +src/Segger_SystemView/SEGGER.h +src/Segger_SystemView/SEGGER_RTT.c +src/Segger_SystemView/SEGGER_RTT.h +src/Segger_SystemView/SEGGER_RTT_Conf.h +src/Segger_SystemView/SEGGER_SYSVIEW.c +src/Segger_SystemView/SEGGER_SYSVIEW.h +src/Segger_SystemView/SEGGER_SYSVIEW_Conf.h +src/Segger_SystemView/SEGGER_SYSVIEW_ConfDefaults.h +src/Segger_SystemView/SEGGER_SYSVIEW_Config_NoOS.c +``` + +You will need to make the following changes in src/Segger_SystemView/SEGGER_SYSVIEW_Config_NoOS.c + +old +``` +#define SYSVIEW_APP_NAME "Demo Application" +``` +new +``` +#define SYSVIEW_APP_NAME "Smoothie2" +``` +old +``` +SEGGER_SYSVIEW_SendSysDesc("I#15=SysTick"); +``` +new (note the line is long and scrolls off the right hand side of the page, make sure you copy it all) +``` +SEGGER_SYSVIEW_SendSysDesc("I#15=SysTick,I#27=acceleration_tick(),I#28=step_tick(),I#29=unstep_tick(),I#30=Timer2,I#100=Endstops::Endstops()"); +``` +If you find that you are running out of buffering between the CPU and your PC, then change src/Segger_SystemView/SEGGER_SYSVIEW_ConfDefaults.h as follows + +old +``` +#define SEGGER_SYSVIEW_RTT_BUFFER_SIZE 1024 +``` +new +``` +#define SEGGER_SYSVIEW_RTT_BUFFER_SIZE 4096 +``` +Note: 4096 is the largest buffer size supported; you can also increase the speed of the JTAG interface to 15MHz when you start your JLinkGDBServer: + +JLinkGDBServer -if jtag -device LPC4330 -speed 15000 + +Note: I run my JLinkGDBServer on a separate Ubuntu machine onto which my Firepick is attached and use TCP/IP to connect to it. diff --git a/src/config.default b/src/config.default index 2203980d..b513db15 100644 --- a/src/config.default +++ b/src/config.default @@ -1,7 +1,20 @@ +# NOTE Lines must not exceed 132 characters +# Robot module configurations : general handling of movement G-codes and slicing into moves +default_feed_rate 4000 # Default rate ( mm/minute ) for G1/G2/G3 moves +default_seek_rate 4000 # Default rate ( mm/minute ) for G0 moves +mm_per_arc_segment 0.0 # Fixed length for line segments that divide arcs 0 to disable +mm_max_arc_error 0.01 # The maximum error for line segments that divide arcs 0 to disable + # note it is invalid for both the above be 0 + # if both are used, will use largest segment length based on radius +#mm_per_line_segment 0.5 # Lines can be cut into segments ( not useful with cartesian + # coordinates robots ). +delta_segments_per_second 100 # for deltas only same as in Marlin/Delta, set to 0 to disable + # and use mm_per_line_segment + # Arm solution configuration : Cartesian robot. Translates mm positions into stepper positions -alpha_steps_per_mm 80 # Steps per mm for alpha stepper -beta_steps_per_mm 80 # Steps per mm for beta stepper -gamma_steps_per_mm 80 # Steps per mm for gamma stepper +#alpha_steps_per_mm 80 # Steps per mm for alpha stepper +#beta_steps_per_mm 80 # Steps per mm for beta stepper +#gamma_steps_per_mm 80 # Steps per mm for gamma stepper # Planner module configuration : Look-ahead and acceleration configuration planner_queue_size 32 # DO NOT CHANGE THIS UNLESS YOU KNOW EXACTLY WHAT YOU ARE DOING @@ -25,24 +38,105 @@ x_axis_max_speed 60000 # mm/min y_axis_max_speed 60000 # mm/min z_axis_max_speed 60000 # mm/min +# Arm solution configuration : Rotatable Delta robot. Translates mm positions into stepper positions +arm_solution rotary_delta # selects the delta arm solution + +delta_e 131.636 # End effector length, triangle side length in mm +delta_f 190.526 # Base length, triangle side length in mm +delta_re 270.000 # Carbon rod length +delta_rf 90.000 # Servo horn length + +delta_z_offset 268.0 # Distance from big pulley shaft to table/bed +delta_ee_offs 15.000 # Ball joint plane to bottom of end effector surface +delta_tool_offset 30.500 # Distance between end effector ball joint plane and tip of tool (PnP) + +delta_mirror_xy true # true for firepick + +# For a FirePick Rotary Delta, the steps per degree are calculated as: +# +# (1) First determine the circumference of the big pulley, which has a smooth surface, and calculate the 'teeth it would have' +# (2) Determine the GT2 belt thickness and tooth depth +# (3) Calculate the ACTUAL big pulley circumference taking into account that the GT2 belt affects this as +# it rests on the big pulley and not into teeth cut into the pulley, the formula is: +# BIG_PULLEY_CIRCUMFERENCE = BIG_PULLEY_TEETH * TOOTH_SPACING + (BELT_THICKNESS - TOOTH_DEPTH)*6.283185 +# (4) Calculate the ACTUAL small pulley circumference taking into account that the GT2 belt affects this as +# rests into the small pulley as it has teeth cut into the pulley, the formula is: +# SMALL_PULLEY_CIRCUMFERENCE = SMALL_PULLEY_TEETH * TOOTH_SPACING +# (5) Now determine the pulley reduction which is: +# PULLEY_REDUCTION = BIG_PULLEY_CIRCUMFERENCE / SMALL_PULLEY_CIRCUMFERENCE +# (6) Finally, given the number of steps a stepper motor needs for a full 360 degree rotation, and the number of microsteps your +# stepper motor driver chip is set for, you can calculate the STEPS_PER_MM for each axis, which for a Rotary Delta, are the same: +# [ALPHA/BETA/GAMMA]_STEPS_PER_MM = (XYZ_STEPS_PER_ROTATION*XYZ_MICROSTEPS*PULLEY_REDUCTION)/360 +# e.g. a FirePick Delta mechanism, we can calculate: +# BIG_PULLEY_CIRCUMFERENCE = 150 * 2 + (1.49 - 0.74) * 6.283185 = 304.712389 +# SMALL_PULLEY_CIRCUMFERENCE = 16 * 2 = 32 +# PULLEY_REDUCTION = 304.712389 / 32 = 9.52226215 +# For a 0.9 degree stepper motor and an a4988 driver set for 16 microsteps = (400 * 16 * 9.52226215)/360 = 169.2846604 +# For a 1.8 degree stepper motor and an a4988 driver set for 16 microsteps = (200 * 16 * 9.52226215)/360 = 84.6423302 +# For a 0.9 degree stepper motor and an drv8825 driver set for 32 microsteps = (400 * 32 * 9.52226215)/360 = 338.5693208 +# For a 1.8 degree stepper motor and an drv8825 driver set for 32 microsteps = (200 * 32 * 9.52226215)/360 = 169.2846604 +# For a 0.9 degree stepper motor and an TMC2130 driver set for 256 microsteps = (400 * 256 * 9.52226215)/360 = 2708.554567 +# For a 1.8 degree stepper motor and an TMC2130 driver set for 256 microsteps = (200 * 256 * 9.52226215)/360 = 1354.277284 + +alpha_steps_per_mm 2708.554567 # Steps per mm for alpha stepper +beta_steps_per_mm 2708.554567 # Steps per mm for beta stepper +gamma_steps_per_mm 2708.554567 # Steps per mm for gamma stepper + # Stepper module pins ( ports, and pin numbers, appending "!" to the number will invert a pin ) -alpha_step_pin 3.2 # Pin for alpha stepper step signal -alpha_dir_pin 7.3 # Pin for alpha stepper direction -alpha_en_pin 7.2 # Pin for alpha enable pin +alpha_step_pin 5.0 # Pin for alpha stepper step signal +alpha_dir_pin 6.12! # Pin for alpha stepper direction +alpha_en_pin 4.10 # Pin for alpha enable pin alpha_current 1.5 # X stepper motor current -alpha_max_rate 60000.0 # mm/min +alpha_max_rate 600.0 # mm/min -beta_step_pin 4.2 # Pin for beta stepper step signal -beta_dir_pin 7.0 # Pin for beta stepper direction -beta_en_pin 4.5 # Pin for beta enable +beta_step_pin 5.7 # Pin for beta stepper step signal +beta_dir_pin 5.5! # Pin for beta stepper direction +beta_en_pin 7.6 # Pin for beta enable beta_current 1.5 # Y stepper motor current -beta_max_rate 60000.0 # mm/min +beta_max_rate 600.0 # mm/min -gamma_step_pin 5.7 # Pin for gamma stepper step signal -gamma_dir_pin 5.5 # Pin for gamma stepper direction -gamma_en_pin 7.6 # Pin for gamma enable +gamma_step_pin 4.0 # Pin for gamma stepper step signal +gamma_dir_pin 1.7! # Pin for gamma stepper direction +gamma_en_pin 6.9 # Pin for gamma enable gamma_current 1.5 # Z stepper motor current -gamma_max_rate 60000.0 # mm/min +gamma_max_rate 600.0 # mm/min + +#SilentStepStick TMC2130 setup +motor_driver_control.alpha.enable true # alpha (X) is a TMC2130 +motor_driver_control.alpha.designator A # A to set the settings +motor_driver_control.alpha.chip TMC2130 # chip name +motor_driver_control.alpha.current 31 # current in 0..31 representing a ratio of 1/32 .. 32/32 +motor_driver_control.alpha.max_current 31 # max current in 0..31 representing a ratio of 1/32 .. 32/32 +motor_driver_control.alpha.microsteps 256 # microsteps +motor_driver_control.alpha.alarm true # set to true means the error bits are checked +motor_driver_control.alpha.halt_on_alarm false # if set to true means ON_HALT is entered on any error bits being set +motor_driver_control.alpha.spi_channel 0 # SPI channel 1 is sdcard channel +motor_driver_control.alpha.spi_cs_pin 7.4 # SPI CS pin +#motor_driver_control.alpha.spi_frequency 1000000 # SPI frequency + +motor_driver_control.beta.enable true # beta (Y) is a TMC2130 +motor_driver_control.beta.designator B # B to set the settings +motor_driver_control.beta.chip TMC2130 # chip name +motor_driver_control.beta.current 31 # current in 0..31 representing a ratio of 1/32 .. 32/32 +motor_driver_control.beta.max_current 31 # max current in 0..31 representing a ratio of 1/32 .. 32/32 +motor_driver_control.beta.microsteps 256 # microsteps +motor_driver_control.beta.alarm true # set to true means the error bits are checked +motor_driver_control.beta.halt_on_alarm false # if set to true means ON_HALT is entered on any error bits being set +motor_driver_control.beta.spi_channel 0 # SPI channel 1 is sdcard channel +motor_driver_control.beta.spi_cs_pin 7.5 # SPI CS pin +#motor_driver_control.beta.spi_frequency 1000000 # SPI frequency + +motor_driver_control.gamma.enable true # gamma (Z) is a TMC2130 +motor_driver_control.gamma.designator C # C to set the settings +motor_driver_control.gamma.chip TMC2130 # chip name +motor_driver_control.gamma.current 31 # current in 0..31 representing a ratio of 1/32 .. 32/32 +motor_driver_control.gamma.max_current 31 # max current in 0..31 representing a ratio of 1/32 .. 32/32 +motor_driver_control.gamma.microsteps 256 # microsteps +motor_driver_control.gamma.alarm true # set to true means the error bits are checked +motor_driver_control.gamma.halt_on_alarm false # if set to true means ON_HALT is entered on any error bits being set +motor_driver_control.gamma.spi_channel 0 # SPI channel 1 is sdcard channel ??? +motor_driver_control.gamma.spi_cs_pin 1.0 # SPI CS pin +#motor_driver_control.gamma.spi_frequency 1000000 # SPI frequency ## System configuration # Serial communications configuration ( baud rate defaults to 9600 if undefined ) @@ -53,7 +147,7 @@ second_usb_serial_enable false # This enables a s #play_led_disable true # disable the play led # Kill button (used to be called pause) maybe assigned to a different pin, set to the onboard pin by default -kill_button_enable false # set to true to enable a kill button +kill_button_enable false # set to true to enable a kill button kill_button_pin 2.12 # kill button pin. default is same as pause button 2.12 (2.11 is another good choice) #msd_disable false # disable the MSD (USB SDCARD) when set to true (needs special binary) @@ -61,30 +155,30 @@ kill_button_pin 2.12 # kill button pin. #watchdog_timeout 10 # watchdog timeout in seconds, default is 10, set to 0 to disable the watchdog # Only needed on a smoothieboard -currentcontrol_module_enable false # +currentcontrol_module_enable false # ## Extruder module configuration -extruder.hotend.enable true # Whether to activate the extruder module at all. All configuration is ignored if false -extruder.hotend.steps_per_mm 140 # Steps per mm for extruder stepper -extruder.hotend.default_feed_rate 600 # Default rate ( mm/minute ) for moves where only the extruder moves -extruder.hotend.acceleration 500 # Acceleration for the stepper motor mm/sec² -extruder.hotend.max_speed 50 # mm/s +extruder.hotend.enable false # Whether to activate the extruder module at all. All configuration is ignored if false +extruder.hotend.steps_per_mm 140 # Steps per mm for extruder stepper +extruder.hotend.default_feed_rate 600 # Default rate ( mm/minute ) for moves where only the extruder moves +extruder.hotend.acceleration 500 # Acceleration for the stepper motor mm/sec² +extruder.hotend.max_speed 50 # mm/s -extruder.hotend.step_pin 6.4 # Pin for extruder step signal -extruder.hotend.dir_pin 6.5 # Pin for extruder dir signal -extruder.hotend.en_pin 1.7 # Pin for extruder enable signal +extruder.hotend.step_pin 6.4 # Pin for extruder step signal +extruder.hotend.dir_pin 6.5 # Pin for extruder dir signal +extruder.hotend.en_pin 1.7 # Pin for extruder enable signal # Second extruder module configuration -extruder.hotend2.enable false # Whether to activate the extruder module at all. All configuration is ignored if false -extruder.hotend2.steps_per_mm 140 # Steps per mm for extruder stepper -extruder.hotend2.default_feed_rate 600 # Default rate ( mm/minute ) for moves where only the extruder moves -extruder.hotend2.acceleration 500 # Acceleration for the stepper motor, as of 0.6, arbitrary ratio -extruder.hotend2.max_speed 50 # mm/s +extruder.hotend2.enable false # Whether to activate the extruder module at all. All configuration is ignored if false +extruder.hotend2.steps_per_mm 140 # Steps per mm for extruder stepper +extruder.hotend2.default_feed_rate 600 # Default rate ( mm/minute ) for moves where only the extruder moves +extruder.hotend2.acceleration 500 # Acceleration for the stepper motor, as of 0.6, arbitrary ratio +extruder.hotend2.max_speed 50 # mm/s -extruder.hotend2.step_pin 6.10 # Pin for extruder step signal -extruder.hotend2.dir_pin 2.9 # Pin for extruder dir signal -extruder.hotend2.en_pin 2.8 # Pin for extruder enable signal +extruder.hotend2.step_pin 6.10 # Pin for extruder step signal +extruder.hotend2.dir_pin 2.9 # Pin for extruder dir signal +extruder.hotend2.en_pin 2.8 # Pin for extruder enable signal ## Laser module configuration @@ -92,9 +186,9 @@ laser_module_enable false # Whether to activ # ignored if false. ## Temperature control configuration # First hotend configuration -temperature_control.hotend.enable true # Whether to activate this ( "hotend" ) module at all. +temperature_control.hotend.enable false # Whether to activate this ( "hotend" ) module at all. # All configuration is ignored if false. -temperature_control.hotend.thermistor_pin adc.5 # Pin for the thermistor to read +temperature_control.hotend.thermistor_pin adc.5 # Pin for the thermistor to read temperature_control.hotend.heater_pin 7.5 # Pin that controls the heater, set to nc if a readonly thermistor is being defined temperature_control.hotend.thermistor EPCOS100K # see http://smoothieware.org/temperaturecontrol#toc5 #temperature_control.hotend.beta 4066 # or set the beta value @@ -114,22 +208,22 @@ temperature_control.hotend.designator T # temperature_control.hotend2.enable false # Whether to activate this ( "hotend" ) module at all. # All configuration is ignored if false. -temperature_control.hotend2.thermistor_pin adc.7 # Pin for the thermistor to read -temperature_control.hotend2.heater_pin 7.7 # Pin that controls the heater -temperature_control.hotend2.thermistor EPCOS100K # see http://smoothieware.org/temperaturecontrol#toc5 -#temperature_control.hotend2.beta 4066 # or set the beta value -temperature_control.hotend2.set_m_code 104 # -temperature_control.hotend2.set_and_wait_m_code 109 # -temperature_control.hotend2.designator T1 # +temperature_control.hotend2.thermistor_pin adc.7 # Pin for the thermistor to read +temperature_control.hotend2.heater_pin 7.7 # Pin that controls the heater +temperature_control.hotend2.thermistor EPCOS100K # see http://smoothieware.org/temperaturecontrol#toc5 +#temperature_control.hotend2.beta 4066 # or set the beta value +temperature_control.hotend2.set_m_code 104 # +temperature_control.hotend2.set_and_wait_m_code 109 # +temperature_control.hotend2.designator T1 # #temperature_control.hotend2.p_factor 13.7 # permanently set the PID values after an auto pid #temperature_control.hotend2.i_factor 0.097 # #temperature_control.hotend2.d_factor 24 # -#temperature_control.hotend2.max_pwm 64 # max pwm, 64 is a good value if driving a 12v resistor with 24v. +#temperature_control.hotend2.max_pwm 64 # max pwm, 64 is a good value if driving a 12v resistor with 24v. -temperature_control.bed.enable false # -temperature_control.bed.thermistor_pin adc.2 # +temperature_control.bed.enable false # +temperature_control.bed.thermistor_pin adc.2 # temperature_control.bed.heater_pin 7.7 # temperature_control.bed.thermistor Honeywell100K # see http://smoothieware.org/temperaturecontrol#toc5 #temperature_control.bed.beta 3974 # or set the beta value @@ -143,52 +237,69 @@ temperature_control.bed.designator B # # when using bang bang ## Switch module for fan control -switch.fan.enable true # +switch.fan.enable false # switch.fan.input_on_command M106 # switch.fan.input_off_command M107 # switch.fan.output_pin 4.1 # switch.fan.output_type pwm # pwm output settable with S parameter in the input_on_comand switch.fan.max_pwm 255 # set max pwm for the pin default is 255 -switch.misc.enable true # +switch.misc.enable false # switch.misc.input_on_command M42 # switch.misc.input_off_command M43 # switch.misc.output_pin 7.7 # -switch.misc.output_type pwm # just an on or off pin +switch.misc.output_type pwm # just an on or off pin ## Endstops endstops_enable true # the endstop module is enabled by default and can be disabled here -#corexy_homing false # set to true if homing on a hbot or corexy -alpha_min_endstop 5.3!^ # add a ! to invert if endstop is NO connected to ground -alpha_max_endstop 1.8^ # NOTE set to nc if this is not installed -alpha_homing_direction home_to_min # or set to home_to_max and set alpha_max -alpha_min 0 # this gets loaded after homing when home_to_min is set -alpha_max 200 # this gets loaded after homing when home_to_max is set -beta_min_endstop 9.6!^ # -beta_max_endstop 9.5^ # -beta_homing_direction home_to_min # -beta_min 0 # -beta_max 200 # -gamma_min_endstop 2.13!^ # -gamma_max_endstop 2.12^ # -gamma_homing_direction home_to_min # -gamma_min 0 # -gamma_max 200 # +rdelta_homing true # forces all three axis to home a the same time regardless of + # what is specified in G28 +alpha_min_endstop 7.4!^ # +alpha_max_endstop 7.7^ # add ! to invert pullup if switch is NO to ground +alpha_homing_direction home_to_max # Home up + +alpha_max 300 # +beta_min_endstop 7.5!^ # +beta_max_endstop 4.3^ # +beta_homing_direction home_to_max # +beta_max 300 # +gamma_min_endstop 4.1!^ # +gamma_max_endstop 4.4^ # +gamma_homing_direction home_to_max # +gamma_max 300 # # optional enable limit switches, actions will stop if any enabled limit switch is triggered -#alpha_limit_enable false # set to true to enable X min and max limit switches -#beta_limit_enable false # set to true to enable Y min and max limit switches -#gamma_limit_enable false # set to true to enable Z min and max limit switches +alpha_limit_enable true # set to true to enable X min and max limit switches +beta_limit_enable true # set to true to enable Y min and max limit switches +gamma_limit_enable true # set to true to enable Z min and max limit switches -alpha_fast_homing_rate_mm_s 500 # feedrates in mm/second -beta_fast_homing_rate_mm_s 500 # " -gamma_fast_homing_rate_mm_s 40 # " -alpha_slow_homing_rate_mm_s 250 # " -beta_slow_homing_rate_mm_s 250 # " -gamma_slow_homing_rate_mm_s 20 # " +alpha_fast_homing_rate_mm_s 500 # feedrates in mm/second +beta_fast_homing_rate_mm_s 500 # " +gamma_fast_homing_rate_mm_s 500 # " + +alpha_slow_homing_rate_mm_s 50 # " +beta_slow_homing_rate_mm_s 50 # " +gamma_slow_homing_rate_mm_s 50 # " alpha_homing_retract_mm 5 # distance in mm beta_homing_retract_mm 5 # " -gamma_homing_retract_mm 1 # " +gamma_homing_retract_mm 5 # " + +endstop_debounce_count 100 # uncomment if you get noise on your endstops, default is 100 + +alpha_trim 0 # software trim for alpha stepper endstop (in mm) +beta_trim 0 # software trim for beta stepper endstop (in mm) +gamma_trim 0 # software trim for gamma stepper endstop (in mm) + +# optional Z probe +zprobe.enable true # set to true to enable a zprobe +zprobe.probe_pin 7.3^ # pin probe is attached to if NC remove the ! +zprobe.slow_feedrate 50 #5 # mm/sec probe feed rate +#zprobe.debounce_count 100 # set if noisy +zprobe.fast_feedrate 5000 # move feedrate mm/sec +zprobe.probe_height 15 # how much above bed to start probe + +# DistanceProbes +distanceprobes_enable true +#rdelta_homing true # forces all three axis to home a the same time regardless of -#endstop_debounce_count 100 # uncomment if you get noise on your endstops, default is 100 diff --git a/src/config.default.cartesian b/src/config.default.cartesian new file mode 100644 index 00000000..2d4a4418 --- /dev/null +++ b/src/config.default.cartesian @@ -0,0 +1,195 @@ +# Arm solution configuration : Cartesian robot. Translates mm positions into stepper positions +alpha_steps_per_mm 80 # Steps per mm for alpha stepper +beta_steps_per_mm 80 # Steps per mm for beta stepper +gamma_steps_per_mm 80 # Steps per mm for gamma stepper + +# Planner module configuration : Look-ahead and acceleration configuration +planner_queue_size 32 # DO NOT CHANGE THIS UNLESS YOU KNOW EXACTLY WHAT YOU ARE DOING +acceleration 3000 # Acceleration in mm/second/second. +#z_acceleration 500 # Acceleration for Z only moves in mm/s^2, 0 uses acceleration which is the default. DO NOT SET ON A DELTA +acceleration_ticks_per_second 1000 # Number of times per second the speed is updated +junction_deviation 0.05 # Similar to the old "max_jerk", in millimeters, + # see https://github.com/grbl/grbl/blob/master/planner.c + # and https://github.com/grbl/grbl/wiki/Configuring-Grbl-v0.8 + # Lower values mean being more careful, higher values means being + # faster and have more jerk +#z_junction_deviation 0.0 # for Z only moves, -1 uses junction_deviation, zero disables junction_deviation on z moves DO NOT SET ON A DELTA +#minimum_planner_speed 0.0 # sets the minimum planner speed in mm/sec + +# Stepper module configuration +microseconds_per_step_pulse 5 # Duration of step pulses to stepper drivers, in microseconds +base_stepping_frequency 100000 # Base frequency for stepping + +# Cartesian axis speed limits +x_axis_max_speed 30000 # mm/min +y_axis_max_speed 30000 # mm/min +z_axis_max_speed 30000 # mm/min + +# Stepper module pins ( ports, and pin numbers, appending "!" to the number will invert a pin ) +alpha_step_pin 3.2 # Pin for alpha stepper step signal +alpha_dir_pin 7.3 # Pin for alpha stepper direction +alpha_en_pin 7.2 # Pin for alpha enable pin +alpha_current 1.5 # X stepper motor current +alpha_max_rate 30000.0 # mm/min + +beta_step_pin 4.2 # Pin for beta stepper step signal +beta_dir_pin 7.0 # Pin for beta stepper direction +beta_en_pin 4.5 # Pin for beta enable +beta_current 1.5 # Y stepper motor current +beta_max_rate 30000.0 # mm/min + +gamma_step_pin 5.7 # Pin for gamma stepper step signal +gamma_dir_pin 5.5 # Pin for gamma stepper direction +gamma_en_pin 7.6 # Pin for gamma enable +gamma_current 1.5 # Z stepper motor current +gamma_max_rate 30000.0 # mm/min + +## System configuration +# Serial communications configuration ( baud rate defaults to 9600 if undefined ) +uart0.baud_rate 115200 # Baud rate for the default hardware serial port +second_usb_serial_enable false # This enables a second usb serial port (to have both pronterface + # and a terminal connected) +#leds_disable true # disable using leds after config loaded +#play_led_disable true # disable the play led + +# Kill button (used to be called pause) maybe assigned to a different pin, set to the onboard pin by default +kill_button_enable false # set to true to enable a kill button +kill_button_pin 2.12 # kill button pin. default is same as pause button 2.12 (2.11 is another good choice) + +#msd_disable false # disable the MSD (USB SDCARD) when set to true (needs special binary) +#dfu_enable false # for linux developers, set to true to enable DFU +#watchdog_timeout 10 # watchdog timeout in seconds, default is 10, set to 0 to disable the watchdog + +# Only needed on a smoothieboard +currentcontrol_module_enable false # + +## Extruder module configuration +extruder.hotend.enable true # Whether to activate the extruder module at all. All configuration is ignored if false +extruder.hotend.steps_per_mm 140 # Steps per mm for extruder stepper +extruder.hotend.default_feed_rate 600 # Default rate ( mm/minute ) for moves where only the extruder moves +extruder.hotend.acceleration 500 # Acceleration for the stepper motor mm/sec² +extruder.hotend.max_speed 50 # mm/s + +extruder.hotend.step_pin 6.4 # Pin for extruder step signal +extruder.hotend.dir_pin 6.5 # Pin for extruder dir signal +extruder.hotend.en_pin 1.7 # Pin for extruder enable signal + + +# Second extruder module configuration +extruder.hotend2.enable false # Whether to activate the extruder module at all. All configuration is ignored if false +extruder.hotend2.steps_per_mm 140 # Steps per mm for extruder stepper +extruder.hotend2.default_feed_rate 600 # Default rate ( mm/minute ) for moves where only the extruder moves +extruder.hotend2.acceleration 500 # Acceleration for the stepper motor, as of 0.6, arbitrary ratio +extruder.hotend2.max_speed 50 # mm/s + +extruder.hotend2.step_pin 6.10 # Pin for extruder step signal +extruder.hotend2.dir_pin 2.9 # Pin for extruder dir signal +extruder.hotend2.en_pin 2.8 # Pin for extruder enable signal + + +## Laser module configuration +laser_module_enable false # Whether to activate the laser module at all. All configuration is + # ignored if false. +## Temperature control configuration +# First hotend configuration +temperature_control.hotend.enable true # Whether to activate this ( "hotend" ) module at all. + # All configuration is ignored if false. +temperature_control.hotend.thermistor_pin adc.5 # Pin for the thermistor to read +temperature_control.hotend.heater_pin 7.5 # Pin that controls the heater, set to nc if a readonly thermistor is being defined +temperature_control.hotend.thermistor EPCOS100K # see http://smoothieware.org/temperaturecontrol#toc5 +#temperature_control.hotend.beta 4066 # or set the beta value +temperature_control.hotend.set_m_code 104 # +temperature_control.hotend.set_and_wait_m_code 109 # +temperature_control.hotend.designator T # +#temperature_control.hotend.max_temp 300 # Set maximum temperature - Will prevent heating above 300 by default +#temperature_control.hotend.min_temp 0 # Set minimum temperature - Will prevent heating below if set + +#temperature_control.hotend.p_factor 13.7 # permanently set the PID values after an auto pid +#temperature_control.hotend.i_factor 0.097 # +#temperature_control.hotend.d_factor 24 # + +#temperature_control.hotend.max_pwm 64 # max pwm, 64 is a good value if driving a 12v resistor with 24v. + +# Second hotend configuration +temperature_control.hotend2.enable false # Whether to activate this ( "hotend" ) module at all. + # All configuration is ignored if false. + +temperature_control.hotend2.thermistor_pin adc.7 # Pin for the thermistor to read +temperature_control.hotend2.heater_pin 7.7 # Pin that controls the heater +temperature_control.hotend2.thermistor EPCOS100K # see http://smoothieware.org/temperaturecontrol#toc5 +#temperature_control.hotend2.beta 4066 # or set the beta value +temperature_control.hotend2.set_m_code 104 # +temperature_control.hotend2.set_and_wait_m_code 109 # +temperature_control.hotend2.designator T1 # + +#temperature_control.hotend2.p_factor 13.7 # permanently set the PID values after an auto pid +#temperature_control.hotend2.i_factor 0.097 # +#temperature_control.hotend2.d_factor 24 # + +#temperature_control.hotend2.max_pwm 64 # max pwm, 64 is a good value if driving a 12v resistor with 24v. + +temperature_control.bed.enable false # +temperature_control.bed.thermistor_pin adc.2 # +temperature_control.bed.heater_pin 7.7 # +temperature_control.bed.thermistor Honeywell100K # see http://smoothieware.org/temperaturecontrol#toc5 +#temperature_control.bed.beta 3974 # or set the beta value + +temperature_control.bed.set_m_code 140 # +temperature_control.bed.set_and_wait_m_code 190 # +temperature_control.bed.designator B # + +#temperature_control.bed.bang_bang false # set to true to use bang bang control rather than PID +#temperature_control.bed.hysteresis 2.0 # set to the temperature in degrees C to use as hysteresis + # when using bang bang + +## Switch module for fan control +switch.fan.enable true # +switch.fan.input_on_command M106 # +switch.fan.input_off_command M107 # +switch.fan.output_pin 4.1 # +switch.fan.output_type pwm # pwm output settable with S parameter in the input_on_comand +switch.fan.max_pwm 255 # set max pwm for the pin default is 255 + +switch.misc.enable true # +switch.misc.input_on_command M42 # +switch.misc.input_off_command M43 # +switch.misc.output_pin 7.7 # +switch.misc.output_type pwm # just an on or off pin + +## Endstops +endstops_enable true # the endstop module is enabled by default and can be disabled here +#corexy_homing false # set to true if homing on a hbot or corexy +alpha_min_endstop 5.3!^ # add a ! to invert if endstop is NO connected to ground +alpha_max_endstop 1.8^ # NOTE set to nc if this is not installed +alpha_homing_direction home_to_min # or set to home_to_max and set alpha_max +alpha_min 0 # this gets loaded after homing when home_to_min is set +alpha_max 200 # this gets loaded after homing when home_to_max is set +beta_min_endstop 9.6!^ # +beta_max_endstop 9.5^ # +beta_homing_direction home_to_min # +beta_min 0 # +beta_max 200 # +gamma_min_endstop 2.13!^ # +gamma_max_endstop 2.12^ # +gamma_homing_direction home_to_min # +gamma_min 0 # +gamma_max 200 # + +# optional enable limit switches, actions will stop if any enabled limit switch is triggered +#alpha_limit_enable false # set to true to enable X min and max limit switches +#beta_limit_enable false # set to true to enable Y min and max limit switches +#gamma_limit_enable false # set to true to enable Z min and max limit switches + +alpha_fast_homing_rate_mm_s 500 # feedrates in mm/second +beta_fast_homing_rate_mm_s 500 # " +gamma_fast_homing_rate_mm_s 40 # " +alpha_slow_homing_rate_mm_s 250 # " +beta_slow_homing_rate_mm_s 250 # " +gamma_slow_homing_rate_mm_s 20 # " + +alpha_homing_retract_mm 5 # distance in mm +beta_homing_retract_mm 5 # " +gamma_homing_retract_mm 1 # " + +#endstop_debounce_count 100 # uncomment if you get noise on your endstops, default is 100 + diff --git a/src/copy.default.rotarydelta.default b/src/copy.default.rotarydelta.default new file mode 100644 index 00000000..e46aa2d2 --- /dev/null +++ b/src/copy.default.rotarydelta.default @@ -0,0 +1,305 @@ +# NOTE Lines must not exceed 132 characters +# Robot module configurations : general handling of movement G-codes and slicing into moves +default_feed_rate 4000 # Default rate ( mm/minute ) for G1/G2/G3 moves +default_seek_rate 4000 # Default rate ( mm/minute ) for G0 moves +mm_per_arc_segment 0.0 # Fixed length for line segments that divide arcs 0 to disable +mm_max_arc_error 0.01 # The maximum error for line segments that divide arcs 0 to disable + # note it is invalid for both the above be 0 + # if both are used, will use largest segment length based on radius +#mm_per_line_segment 0.5 # Lines can be cut into segments ( not useful with cartesian + # coordinates robots ). +delta_segments_per_second 100 # for deltas only same as in Marlin/Delta, set to 0 to disable + # and use mm_per_line_segment + +# Arm solution configuration : Cartesian robot. Translates mm positions into stepper positions +#alpha_steps_per_mm 80 # Steps per mm for alpha stepper +#beta_steps_per_mm 80 # Steps per mm for beta stepper +#gamma_steps_per_mm 80 # Steps per mm for gamma stepper + +# Planner module configuration : Look-ahead and acceleration configuration +planner_queue_size 32 # DO NOT CHANGE THIS UNLESS YOU KNOW EXACTLY WHAT YOU ARE DOING +acceleration 3000 # Acceleration in mm/second/second. +#z_acceleration 500 # Acceleration for Z only moves in mm/s^2, 0 uses acceleration which is the default. DO NOT SET ON A DELTA +acceleration_ticks_per_second 1000 # Number of times per second the speed is updated +junction_deviation 0.05 # Similar to the old "max_jerk", in millimeters, + # see https://github.com/grbl/grbl/blob/master/planner.c + # and https://github.com/grbl/grbl/wiki/Configuring-Grbl-v0.8 + # Lower values mean being more careful, higher values means being + # faster and have more jerk +#z_junction_deviation 0.0 # for Z only moves, -1 uses junction_deviation, zero disables junction_deviation on z moves DO NOT SET ON A DELTA +#minimum_planner_speed 0.0 # sets the minimum planner speed in mm/sec + +# Stepper module configuration +microseconds_per_step_pulse 5 # Duration of step pulses to stepper drivers, in microseconds +base_stepping_frequency 100000 # Base frequency for stepping + +# Cartesian axis speed limits +x_axis_max_speed 60000 # mm/min +y_axis_max_speed 60000 # mm/min +z_axis_max_speed 60000 # mm/min + +# Arm solution configuration : Rotatable Delta robot. Translates mm positions into stepper positions +arm_solution rotary_delta # selects the delta arm solution + +delta_e 131.636 # End effector length, triangle side length in mm +delta_f 190.526 # Base length, triangle side length in mm +delta_re 270.000 # Carbon rod length +delta_rf 90.000 # Servo horn length + +delta_z_offset 268.0 # Distance from big pulley shaft to table/bed +delta_ee_offs 15.000 # Ball joint plane to bottom of end effector surface +delta_tool_offset 30.500 # Distance between end effector ball joint plane and tip of tool (PnP) + +delta_mirror_xy true # true for firepick + +# For a FirePick Rotary Delta, the steps per degree are calculated as: +# +# (1) First determine the circumference of the big pulley, which has a smooth surface, and calculate the 'teeth it would have' +# (2) Determine the GT2 belt thickness and tooth depth +# (3) Calculate the ACTUAL big pulley circumference taking into account that the GT2 belt affects this as +# it rests on the big pulley and not into teeth cut into the pulley, the formula is: +# BIG_PULLEY_CIRCUMFERENCE = BIG_PULLEY_TEETH * TOOTH_SPACING + (BELT_THICKNESS - TOOTH_DEPTH)*6.283185 +# (4) Calculate the ACTUAL small pulley circumference taking into account that the GT2 belt affects this as +# rests into the small pulley as it has teeth cut into the pulley, the formula is: +# SMALL_PULLEY_CIRCUMFERENCE = SMALL_PULLEY_TEETH * TOOTH_SPACING +# (5) Now determine the pulley reduction which is: +# PULLEY_REDUCTION = BIG_PULLEY_CIRCUMFERENCE / SMALL_PULLEY_CIRCUMFERENCE +# (6) Finally, given the number of steps a stepper motor needs for a full 360 degree rotation, and the number of microsteps your +# stepper motor driver chip is set for, you can calculate the STEPS_PER_MM for each axis, which for a Rotary Delta, are the same: +# [ALPHA/BETA/GAMMA]_STEPS_PER_MM = (XYZ_STEPS_PER_ROTATION*XYZ_MICROSTEPS*PULLEY_REDUCTION)/360 +# e.g. a FirePick Delta mechanism, we can calculate: +# BIG_PULLEY_CIRCUMFERENCE = 150 * 2 + (1.49 - 0.74) * 6.283185 = 304.712389 +# SMALL_PULLEY_CIRCUMFERENCE = 16 * 2 = 32 +# PULLEY_REDUCTION = 304.712389 / 32 = 9.52226215 +# For a 0.9 degree stepper motor and an a4988 driver set for 16 microsteps = (400 * 16 * 9.52226215)/360 = 169.2846604 +# For a 1.8 degree stepper motor and an a4988 driver set for 16 microsteps = (200 * 16 * 9.52226215)/360 = 84.6423302 +# For a 0.9 degree stepper motor and an drv8825 driver set for 32 microsteps = (400 * 32 * 9.52226215)/360 = 338.5693208 +# For a 1.8 degree stepper motor and an drv8825 driver set for 32 microsteps = (200 * 32 * 9.52226215)/360 = 169.2846604 +# For a 0.9 degree stepper motor and an TMC2130 driver set for 256 microsteps = (400 * 256 * 9.52226215)/360 = 2708.554567 +# For a 1.8 degree stepper motor and an TMC2130 driver set for 256 microsteps = (200 * 256 * 9.52226215)/360 = 1354.277284 + +alpha_steps_per_mm 2708.554567 # Steps per mm for alpha stepper +beta_steps_per_mm 2708.554567 # Steps per mm for beta stepper +gamma_steps_per_mm 2708.554567 # Steps per mm for gamma stepper + +# Stepper module pins ( ports, and pin numbers, appending "!" to the number will invert a pin ) +alpha_step_pin 5.0 # Pin for alpha stepper step signal +alpha_dir_pin 6.12 # Pin for alpha stepper direction +alpha_en_pin 4.10 # Pin for alpha enable pin +alpha_current 1.5 # X stepper motor current +alpha_max_rate 60000.0 # mm/min + +beta_step_pin 5.7 # Pin for beta stepper step signal +beta_dir_pin 5.5 # Pin for beta stepper direction +beta_en_pin 7.6 # Pin for beta enable +beta_current 1.5 # Y stepper motor current +beta_max_rate 60000.0 # mm/min + +gamma_step_pin 4.0 # Pin for gamma stepper step signal +gamma_dir_pin 1.7 # Pin for gamma stepper direction +gamma_en_pin 6.9 # Pin for gamma enable +gamma_current 1.5 # Z stepper motor current +gamma_max_rate 60000.0 # mm/min + +#SilentStepStick TMC2130 setup +motor_driver_control.alpha.enable true # alpha (X) is a TMC2130 +motor_driver_control.alpha.designator A # A to set the settings +motor_driver_control.alpha.chip TMC2130 # chip name +motor_driver_control.alpha.current 31 # current in 0..31 representing a ratio of 1/32 .. 32/32 +motor_driver_control.alpha.max_current 31 # max current in 0..31 representing a ratio of 1/32 .. 32/32 +motor_driver_control.alpha.microsteps 256 # microsteps +motor_driver_control.alpha.alarm true # set to true means the error bits are checked +motor_driver_control.alpha.halt_on_alarm false # if set to true means ON_HALT is entered on any error bits being set +motor_driver_control.alpha.spi_channel 0 # SPI channel 1 is sdcard channel +motor_driver_control.alpha.spi_cs_pin 7.4 # SPI CS pin +#motor_driver_control.alpha.spi_frequency 1000000 # SPI frequency + +motor_driver_control.beta.enable true # beta (Y) is a TMC2130 +motor_driver_control.beta.designator B # B to set the settings +motor_driver_control.beta.chip TMC2130 # chip name +motor_driver_control.beta.current 31 # current in 0..31 representing a ratio of 1/32 .. 32/32 +motor_driver_control.beta.max_current 31 # max current in 0..31 representing a ratio of 1/32 .. 32/32 +motor_driver_control.beta.microsteps 256 # microsteps +motor_driver_control.beta.alarm true # set to true means the error bits are checked +motor_driver_control.beta.halt_on_alarm false # if set to true means ON_HALT is entered on any error bits being set +motor_driver_control.beta.spi_channel 0 # SPI channel 1 is sdcard channel +motor_driver_control.beta.spi_cs_pin 7.5 # SPI CS pin +#motor_driver_control.beta.spi_frequency 1000000 # SPI frequency + +motor_driver_control.gamma.enable true # gamma (Z) is a TMC2130 +motor_driver_control.gamma.designator C # C to set the settings +motor_driver_control.gamma.chip TMC2130 # chip name +motor_driver_control.gamma.current 31 # current in 0..31 representing a ratio of 1/32 .. 32/32 +motor_driver_control.gamma.max_current 31 # max current in 0..31 representing a ratio of 1/32 .. 32/32 +motor_driver_control.gamma.microsteps 256 # microsteps +motor_driver_control.gamma.alarm true # set to true means the error bits are checked +motor_driver_control.gamma.halt_on_alarm false # if set to true means ON_HALT is entered on any error bits being set +motor_driver_control.gamma.spi_channel 0 # SPI channel 1 is sdcard channel ??? +motor_driver_control.gamma.spi_cs_pin 1.0 # SPI CS pin +#motor_driver_control.gamma.spi_frequency 1000000 # SPI frequency + +## System configuration +# Serial communications configuration ( baud rate defaults to 9600 if undefined ) +uart0.baud_rate 115200 # Baud rate for the default hardware serial port +second_usb_serial_enable false # This enables a second usb serial port (to have both pronterface + # and a terminal connected) +#leds_disable true # disable using leds after config loaded +#play_led_disable true # disable the play led + +# Kill button (used to be called pause) maybe assigned to a different pin, set to the onboard pin by default +kill_button_enable false # set to true to enable a kill button +kill_button_pin 2.12 # kill button pin. default is same as pause button 2.12 (2.11 is another good choice) + +#msd_disable false # disable the MSD (USB SDCARD) when set to true (needs special binary) +#dfu_enable false # for linux developers, set to true to enable DFU +#watchdog_timeout 10 # watchdog timeout in seconds, default is 10, set to 0 to disable the watchdog + +# Only needed on a smoothieboard +currentcontrol_module_enable false # + +## Extruder module configuration +extruder.hotend.enable false # Whether to activate the extruder module at all. All configuration is ignored if false +extruder.hotend.steps_per_mm 140 # Steps per mm for extruder stepper +extruder.hotend.default_feed_rate 600 # Default rate ( mm/minute ) for moves where only the extruder moves +extruder.hotend.acceleration 500 # Acceleration for the stepper motor mm/sec² +extruder.hotend.max_speed 50 # mm/s + +extruder.hotend.step_pin 6.4 # Pin for extruder step signal +extruder.hotend.dir_pin 6.5 # Pin for extruder dir signal +extruder.hotend.en_pin 1.7 # Pin for extruder enable signal + + +# Second extruder module configuration +extruder.hotend2.enable false # Whether to activate the extruder module at all. All configuration is ignored if false +extruder.hotend2.steps_per_mm 140 # Steps per mm for extruder stepper +extruder.hotend2.default_feed_rate 600 # Default rate ( mm/minute ) for moves where only the extruder moves +extruder.hotend2.acceleration 500 # Acceleration for the stepper motor, as of 0.6, arbitrary ratio +extruder.hotend2.max_speed 50 # mm/s + +extruder.hotend2.step_pin 6.10 # Pin for extruder step signal +extruder.hotend2.dir_pin 2.9 # Pin for extruder dir signal +extruder.hotend2.en_pin 2.8 # Pin for extruder enable signal + + +## Laser module configuration +laser_module_enable false # Whether to activate the laser module at all. All configuration is + # ignored if false. +## Temperature control configuration +# First hotend configuration +temperature_control.hotend.enable false # Whether to activate this ( "hotend" ) module at all. + # All configuration is ignored if false. +temperature_control.hotend.thermistor_pin adc.5 # Pin for the thermistor to read +temperature_control.hotend.heater_pin 7.5 # Pin that controls the heater, set to nc if a readonly thermistor is being defined +temperature_control.hotend.thermistor EPCOS100K # see http://smoothieware.org/temperaturecontrol#toc5 +#temperature_control.hotend.beta 4066 # or set the beta value +temperature_control.hotend.set_m_code 104 # +temperature_control.hotend.set_and_wait_m_code 109 # +temperature_control.hotend.designator T # +#temperature_control.hotend.max_temp 300 # Set maximum temperature - Will prevent heating above 300 by default +#temperature_control.hotend.min_temp 0 # Set minimum temperature - Will prevent heating below if set + +#temperature_control.hotend.p_factor 13.7 # permanently set the PID values after an auto pid +#temperature_control.hotend.i_factor 0.097 # +#temperature_control.hotend.d_factor 24 # + +#temperature_control.hotend.max_pwm 64 # max pwm, 64 is a good value if driving a 12v resistor with 24v. + +# Second hotend configuration +temperature_control.hotend2.enable false # Whether to activate this ( "hotend" ) module at all. + # All configuration is ignored if false. + +temperature_control.hotend2.thermistor_pin adc.7 # Pin for the thermistor to read +temperature_control.hotend2.heater_pin 7.7 # Pin that controls the heater +temperature_control.hotend2.thermistor EPCOS100K # see http://smoothieware.org/temperaturecontrol#toc5 +#temperature_control.hotend2.beta 4066 # or set the beta value +temperature_control.hotend2.set_m_code 104 # +temperature_control.hotend2.set_and_wait_m_code 109 # +temperature_control.hotend2.designator T1 # + +#temperature_control.hotend2.p_factor 13.7 # permanently set the PID values after an auto pid +#temperature_control.hotend2.i_factor 0.097 # +#temperature_control.hotend2.d_factor 24 # + +#temperature_control.hotend2.max_pwm 64 # max pwm, 64 is a good value if driving a 12v resistor with 24v. + +temperature_control.bed.enable false # +temperature_control.bed.thermistor_pin adc.2 # +temperature_control.bed.heater_pin 7.7 # +temperature_control.bed.thermistor Honeywell100K # see http://smoothieware.org/temperaturecontrol#toc5 +#temperature_control.bed.beta 3974 # or set the beta value + +temperature_control.bed.set_m_code 140 # +temperature_control.bed.set_and_wait_m_code 190 # +temperature_control.bed.designator B # + +#temperature_control.bed.bang_bang false # set to true to use bang bang control rather than PID +#temperature_control.bed.hysteresis 2.0 # set to the temperature in degrees C to use as hysteresis + # when using bang bang + +## Switch module for fan control +switch.fan.enable false # +switch.fan.input_on_command M106 # +switch.fan.input_off_command M107 # +switch.fan.output_pin 4.1 # +switch.fan.output_type pwm # pwm output settable with S parameter in the input_on_comand +switch.fan.max_pwm 255 # set max pwm for the pin default is 255 + +switch.misc.enable false # +switch.misc.input_on_command M42 # +switch.misc.input_off_command M43 # +switch.misc.output_pin 7.7 # +switch.misc.output_type pwm # just an on or off pin + +## Endstops +endstops_enable true # the endstop module is enabled by default and can be disabled here +rdelta_homing true # forces all three axis to home a the same time regardless of + # what is specified in G28 +alpha_min_endstop 7.4!^ # +alpha_max_endstop 7.7^ # add ! to invert pullup if switch is NO to ground +alpha_homing_direction home_to_max # Home up + +alpha_max 300 # +beta_min_endstop 7.5!^ # +beta_max_endstop 4.3^ # +beta_homing_direction home_to_max # +beta_max 300 # +gamma_min_endstop 4.1!^ # +gamma_max_endstop 4.4^ # +gamma_homing_direction home_to_max # +gamma_max 300 # + +# optional enable limit switches, actions will stop if any enabled limit switch is triggered +alpha_limit_enable true # set to true to enable X min and max limit switches +beta_limit_enable true # set to true to enable Y min and max limit switches +gamma_limit_enable true # set to true to enable Z min and max limit switches + +alpha_fast_homing_rate_mm_s 500 # feedrates in mm/second +beta_fast_homing_rate_mm_s 500 # " +gamma_fast_homing_rate_mm_s 500 # " + +alpha_slow_homing_rate_mm_s 50 # " +beta_slow_homing_rate_mm_s 50 # " +gamma_slow_homing_rate_mm_s 50 # " + +alpha_homing_retract_mm 5 # distance in mm +beta_homing_retract_mm 5 # " +gamma_homing_retract_mm 5 # " + +endstop_debounce_count 100 # uncomment if you get noise on your endstops, default is 100 + +alpha_trim 0 # software trim for alpha stepper endstop (in mm) +beta_trim 0 # software trim for beta stepper endstop (in mm) +gamma_trim 0 # software trim for gamma stepper endstop (in mm) + +# optional Z probe +zprobe.enable true # set to true to enable a zprobe +#zprobe.probe_pin 4.4!^ # pin probe is attached to if NC remove the ! +zprobe.slow_feedrate 5 # mm/sec probe feed rate +#zprobe.debounce_count 100 # set if noisy +zprobe.fast_feedrate 100 # move feedrate mm/sec +zprobe.probe_height 15 # how much above bed to start probe + +# DistanceProbes +distanceprobes_enable true +#rdelta_homing true # forces all three axis to home a the same time regardless of + diff --git a/src/generate-version.bat b/src/generate-version.bat new file mode 100644 index 00000000..0ebf0b8b --- /dev/null +++ b/src/generate-version.bat @@ -0,0 +1,6 @@ +@for /f "delims=" %%a in ('git symbolic-ref HEAD') do @set myvar=%%a +@for /f "delims=" %%b in ('git log "--pretty=format:%%h" -1') do @set myvar2=%%b +@REM the 11 is derived from generate-version.sh "cut -b 12" by subtracting 1 +@echo %myvar:~11%-%myvar2% +@copy /b version.cpp+,, >NUL: + diff --git a/src/generate-version.sh b/src/generate-version.sh old mode 100644 new mode 100755 diff --git a/src/libs/Debug/HardFault_HandlerC.c b/src/libs/Debug/HardFault_HandlerC.c new file mode 100644 index 00000000..4377de89 --- /dev/null +++ b/src/libs/Debug/HardFault_HandlerC.c @@ -0,0 +1,142 @@ +/** + * HardFaultHandler_C: + * + * This is called from the HardFault_Handler which over-rides the WEAK definition of HardFault_Handler with a pointer the + * Fault Stack as the parameter. + * + * Then this code reads the values from the stack and places them into local variables so they can be examined. + * + * There are various Fault Status and Address Registers to help decode the cause of the fault. + * + * The function ends with a BKPT instruction to force control back into the debugger + * + * Refer to http://support.code-red-tech.com/CodeRedWiki/DebugHardFault + * + * And to http://www.freertos.org/Debugging-Hard-Faults-On-Cortex-M-Microcontrollers.html + * + * Also refer to: + * Cortex M4 exception handling: http://infocenter.arm.com/help/index.jsp?topic=/com.arm.doc.ddi0439b/ch03s09s01.html + * Cortex M0 exception handing: http://infocenter.arm.com/help/index.jsp?topic=/com.arm.doc.dui0497a/BABBFABJ.html + * + * The first register of interest is the program counter. + * + * In the code, below the variable stacked_pc contains the program counter value. + * + * When the fault is a precise fault, the stacked_pc holds the address of the instruction that was executing when the hard fault + * (or other fault) occurred. When the fault is an imprecise fault, then additional steps are required to find the address of the + * instruction that caused the fault. + * + * To find the instruction at the address held in the stacked_pc variable, either... + * Open an assembly code window in the debugger, and manually enter the address to view the assembly instructions at that address, or + * Open the break point window in the debugger, and manually define an execution or access break point at that address. + * With the break point set, restart the application to see which line of code the instruction relates to. + * + * Knowing the instruction that was being executed when the fault occurred allows you to determine which other register values are + * also of interest. + * + * For example, if the instruction was using the value of R7 as an address, then the value of R7 needs to be know. + * Further, examining the assembly code, and the C code that generated the assembly code, will show what R7 actually holds + * (it might be the value of a variable, for example). + * + * Handling Imprecise Faults: + * + * ARM Cortex-M faults can be precise or imprecise. + * + * If the IMPRECISERR bit (bit 2) is set in the Bus Fault Status Register (or BFSR, which is at address 0xE000ED29), then the fault + * is imprecise. + * + * It is harder to determine the cause of an imprecise fault because the fault will not necessarily occur concurrently with the + * instruction that caused the fault. + * + * For example, if writes to memory are cached then there might be a delay between an assembly instruction initiating a write to + * memory and the write to memory actually occurring. + * + * If such a delayed write operation is invalid (for example, a write is being attempted to an invalid memory location) then an + * imprecise fault will occur, and the program counter value obtained using the code above will not be the address of the assembly + * instruction that initiated the write operation. + * + * Turn off write buffering by setting the DISDEFWBUF bit (bit 1) in the Auxiliary Control Register (or ACTLR) compile and + * re-run the test and then the imprecise fault becoming a precise fault, which makes the fault easier to debug. + * + * Note: this will come at the cost of slower program execution. + * + * I hope this helps you track down this hard to find HardFaults + * + * Douglas.Pearless@gmail.com + * + */ + +// This overrides the WEAK definition of HardFault_Handler +void HardFault_HandlerC(unsigned long *hardfault_args){ + volatile unsigned long stacked_r0 ; + volatile unsigned long stacked_r1 ; + volatile unsigned long stacked_r2 ; + volatile unsigned long stacked_r3 ; + volatile unsigned long stacked_r12 ; + volatile unsigned long stacked_lr ; // Link register + volatile unsigned long stacked_pc ; // Program counter + volatile unsigned long stacked_psr ; // Program status register + volatile unsigned long _CFSR ; + volatile unsigned long _HFSR ; + volatile unsigned long _DFSR ; + volatile unsigned long _AFSR ; + volatile unsigned long _BFAR ; + volatile unsigned long _MMAR ; + volatile unsigned long _BFSR ; + + stacked_r0 = ((unsigned long)hardfault_args[0]) ; + stacked_r1 = ((unsigned long)hardfault_args[1]) ; + stacked_r2 = ((unsigned long)hardfault_args[2]) ; + stacked_r3 = ((unsigned long)hardfault_args[3]) ; + stacked_r12 = ((unsigned long)hardfault_args[4]) ; + stacked_lr = ((unsigned long)hardfault_args[5]) ; + stacked_pc = ((unsigned long)hardfault_args[6]) ; + stacked_psr = ((unsigned long)hardfault_args[7]) ; + + // Configurable Fault Status Register + // Consists of MMSR, BFSR and UFSR + _CFSR = (*((volatile unsigned long *)(0xE000ED28))) ; + + // Hard Fault Status Register + _HFSR = (*((volatile unsigned long *)(0xE000ED2C))) ; + + // Debug Fault Status Register + _DFSR = (*((volatile unsigned long *)(0xE000ED30))) ; + + // Auxiliary Fault Status Register + _AFSR = (*((volatile unsigned long *)(0xE000ED3C))) ; + + // Read the Fault Address Registers. These may not contain valid values. + // Check BFARVALID/MMARVALID to see if they are valid values + // MemManage Fault Address Register + _MMAR = (*((volatile unsigned long *)(0xE000ED34))) ; + + // Bus Fault Address Register + _BFAR = (*((volatile unsigned long *)(0xE000ED38))) ; + + // Bus Fault Status Register + _BFSR = (*((volatile unsigned long *)(0xE000ED29))) ; + + __asm("BKPT #0\n") ; // Break into the debugger +} + +/* The prototype shows it is a naked function - in effect this is just an +assembly function. */ +static void HardFault_Handler( void ) __attribute__( ( naked ) ); + +/* The fault handler implementation calls a function called +prvGetRegistersFromStack(). */ +static void HardFault_Handler(void) +{ + __asm volatile + ( + " tst lr, #4 \n" + " ite eq \n" + " mrseq r0, msp \n" + " mrsne r0, psp \n" + " ldr r1, [r0, #24] \n" + " ldr r2, handler2_address_const \n" + " bx r2 \n" + " handler2_address_const: .word HardFault_HandlerCk \n" + ); +} diff --git a/src/libs/Kernel.cpp b/src/libs/Kernel.cpp index 374fbbd3..71c85911 100644 --- a/src/libs/Kernel.cpp +++ b/src/libs/Kernel.cpp @@ -26,7 +26,7 @@ #include "StepperMotor.h" #include "BaseSolution.h" #include "EndstopsPublicAccess.h" -//#include "Configurator.h" +#include "Configurator.h" //#include "SimpleShell.h" #include "platform_memory.h" @@ -58,11 +58,11 @@ Kernel::Kernel(){ // Create the default UART Serial Console interface if (SMOOTHIE_UART_PRIMARY_ENABLE) { - this->serial = new SerialConsole(SMOOTHIE_UART_PRIMARY_TX, SMOOTHIE_UART_PRIMARY_RX, 9600); + this->serial = new SerialConsole(SMOOTHIE_UART_PRIMARY_TX, SMOOTHIE_UART_PRIMARY_RX, DEFAULT_SERIAL_BAUD_RATE); this->add_module( this->serial ); } if (SMOOTHIE_UART_SECONDARY_ENABLE) { - this->secondary_serial = new SerialConsole(SMOOTHIE_UART_SECONDARY_TX, SMOOTHIE_UART_SECONDARY_RX, 9600); + this->secondary_serial = new SerialConsole(SMOOTHIE_UART_SECONDARY_TX, SMOOTHIE_UART_SECONDARY_RX, DEFAULT_SERIAL_BAUD_RATE); this->add_module( this->secondary_serial ); } @@ -74,18 +74,57 @@ Kernel::Kernel(){ // For slow repeteative tasks this->add_module( this->slow_ticker = new SlowTicker()); + // now config is loaded we can do normal setup for serial based on config +//TOADDBACK when the USB libraries are ported +// delete this->serial; +// this->serial= NULL; +// +// this->streams = new StreamOutputPool(); this->current_path = "/"; + // Configure UART depending on MRI config + // Match up the SerialConsole to MRI UART. This makes it easy to use only one UART for both debug and actual commands. + NVIC_SetPriorityGrouping(0); + +#if MRI_ENABLE != 0 + switch( __mriPlatform_CommUartIndex() ) { + case 0: + this->serial = new (SerialConsole(USBTX, USBRX, this->config->value(uart0_checksum,baud_rate_setting_checksum)->by_default(DEFAULT_SERIAL_BAUD_RATE)->as_number()); + break; + case 1: + this->serial = new SerialConsole( p13, p14, this->config->value(uart0_checksum,baud_rate_setting_checksum)->by_default(DEFAULT_SERIAL_BAUD_RATE)->as_number()); + break; + case 2: + this->serial = new SerialConsole( p28, p27, this->config->value(uart0_checksum,baud_rate_setting_checksum)->by_default(DEFAULT_SERIAL_BAUD_RATE)->as_number()); + break; + case 3: + this->serial = new SerialConsole( p9, p10, this->config->value(uart0_checksum,baud_rate_setting_checksum)->by_default(DEFAULT_SERIAL_BAUD_RATE)->as_number()); + break; + } +#endif + // default + if(this->serial == nullptr) { + this->serial = new SerialConsole(USBTX, USBRX, this->config->value(uart0_checksum,baud_rate_setting_checksum)->by_default(DEFAULT_SERIAL_BAUD_RATE)->as_number()); + } + //some boards don't have leds.. TOO BAD! this->use_leds= !this->config->value( disable_leds_checksum )->by_default(false)->as_bool(); + + #ifdef CNC + this->grbl_mode= this->config->value( grbl_mode_checksum )->by_default(true)->as_bool(); + #else this->grbl_mode= this->config->value( grbl_mode_checksum )->by_default(false)->as_bool(); + #endif + + // we exepct ok per line now not per G code, setting this to false will return to the old (incorrect) way of ok per G code this->ok_per_line= this->config->value( ok_per_line_checksum )->by_default(true)->as_bool(); // Configure the step ticker this->base_stepping_frequency = this->config->value(base_stepping_frequency_checksum)->by_default(100000)->as_number(); float microseconds_per_step_pulse = this->config->value(microseconds_per_step_pulse_checksum)->by_default(5)->as_number(); - // REMOVE this->acceleration_ticks_per_second = THEKERNEL->config->value(acceleration_ticks_per_second_checksum)->by_default(1000)->as_number(); + + this->add_module( this->serial ); // HAL stuff add_module( this->slow_ticker = new SlowTicker()); @@ -97,12 +136,26 @@ Kernel::Kernel(){ NVIC_SetPriorityGrouping(0); NVIC_SetPriority(TIMER0_IRQn, 2); NVIC_SetPriority(TIMER1_IRQn, 1); - NVIC_SetPriority(TIMER2_IRQn, 4); + NVIC_SetPriority(TIMER2_IRQn, 1); NVIC_SetPriority(PendSV_IRQn, 3); // Set other priorities lower than the timers NVIC_SetPriority(ADC0_IRQn, 5); NVIC_SetPriority(ADC1_IRQn, 5); +// NVIC_SetPriority(USB_IRQn, 5); +// +// // If MRI is enabled +// if( MRI_ENABLE ){ +// if( NVIC_GetPriority(UART0_IRQn) > 0 ){ NVIC_SetPriority(UART0_IRQn, 5); } +// if( NVIC_GetPriority(UART1_IRQn) > 0 ){ NVIC_SetPriority(UART1_IRQn, 5); } +// if( NVIC_GetPriority(UART2_IRQn) > 0 ){ NVIC_SetPriority(UART2_IRQn, 5); } +// if( NVIC_GetPriority(UART3_IRQn) > 0 ){ NVIC_SetPriority(UART3_IRQn, 5); } +// }else{ +// NVIC_SetPriority(UART0_IRQn, 5); +// NVIC_SetPriority(UART1_IRQn, 5); +// NVIC_SetPriority(UART2_IRQn, 5); +// NVIC_SetPriority(UART3_IRQn, 5); +// } // Configure the step ticker this->step_ticker->set_frequency( this->base_stepping_frequency ); @@ -115,7 +168,7 @@ Kernel::Kernel(){ // TOADDBACK this->add_module( this->simpleshell = new SimpleShell() ); this->planner = new Planner(); - // TOADDBACK this->configurator = new Configurator(); + this->configurator = new Configurator(); } // return a GRBL-like query string for serial ? diff --git a/src/libs/Kernel.h b/src/libs/Kernel.h index 0f4711f8..68c1f8d3 100644 --- a/src/libs/Kernel.h +++ b/src/libs/Kernel.h @@ -31,8 +31,8 @@ class Planner; class StepTicker; class Adc; class PublicData; -//TOADDBACK class SimpleShell; -//TOADDBACK class Configurator; +//TOADDBACK class SimpleShell; +class Configurator; class Kernel { public: @@ -63,12 +63,11 @@ class Kernel { StreamOutputPool* streams; GcodeDispatch* gcode_dispatch; Robot* robot; - Stepper* stepper; Planner* planner; Config* config; Conveyor* conveyor; - // TOADDBACK Configurator* configurator; - // TOADDBACK SimpleShell* simpleshell; + Configurator* configurator; + // TOADDBACK SimpleShell* simpleshell; int debug; SlowTicker* slow_ticker; @@ -76,7 +75,6 @@ class Kernel { Adc* adc; std::string current_path; uint32_t base_stepping_frequency; - uint32_t acceleration_ticks_per_second; private: // When a module asks to be called for a specific event ( a hook ), this is where that request is remembered diff --git a/src/libs/Pin.cpp b/src/libs/Pin.cpp index 593cab4b..bc2ec29d 100644 --- a/src/libs/Pin.cpp +++ b/src/libs/Pin.cpp @@ -18,9 +18,6 @@ Pin::Pin(){ // Make a new pin object from a string Pin* Pin::from_string(std::string value){ - // Port and pin for when we find them - int port_number; - int pin; // cs is the current position in the string const char* cs = value.c_str(); @@ -60,18 +57,36 @@ Pin* Pin::from_string(std::string value){ cs = ++cn; // grab pin index. - pin = strtol(cs, &cn, 10); - - map pins = {{(0x00<<8)+0,P0_0},{(0x00<<8)+1,P0_1},{(0x01<<8)+0,P1_0},{(0x01<<8)+1,P1_1},{(0x01<<8)+2,P1_2},{(0x01<<8)+3,P1_3},{(0x01<<8)+4,P1_4},{(0x01<<8)+5,P1_5},{(0x01<<8)+6,P1_6},{(0x01<<8)+7,P1_7},{(0x01<<8)+8,P1_8},{(0x01<<8)+9,P1_9},{(0x01<<8)+10,P1_10},{(0x01<<8)+11,P1_11},{(0x01<<8)+12,P1_12},{(0x01<<8)+13,P1_13},{(0x01<<8)+14,P1_14},{(0x01<<8)+15,P1_15},{(0x01<<8)+16,P1_16},{(0x01<<8)+17,P1_17},{(0x01<<8)+18,P1_18},{(0x01<<8)+20,P1_20},{(0x02<<8)+0,P2_0},{(0x02<<8)+1,P2_1},{(0x02<<8)+2,P2_2},{(0x02<<8)+3,P2_3},{(0x02<<8)+4,P2_4},{(0x02<<8)+5,P2_5},{(0x02<<8)+6,P2_6},{(0x02<<8)+7,P2_7},{(0x02<<8)+8,P2_8},{(0x02<<8)+9,P2_9},{(0x02<<8)+10,P2_10},{(0x02<<8)+11,P2_11},{(0x02<<8)+12,P2_12},{(0x02<<8)+13,P2_13},{(0x03<<8)+1,P3_1},{(0x03<<8)+2,P3_2},{(0x03<<8)+4,P3_4},{(0x03<<8)+5,P3_5},{(0x03<<8)+6,P3_6},{(0x03<<8)+7,P3_7},{(0x03<<8)+8,P3_8},{(0x04<<8)+0,P4_0},{(0x04<<8)+1,P4_1},{(0x04<<8)+2,P4_2},{(0x04<<8)+3,P4_3},{(0x04<<8)+4,P4_4},{(0x04<<8)+5,P4_5},{(0x04<<8)+6,P4_6},{(0x04<<8)+8,P4_8},{(0x04<<8)+9,P4_9},{(0x04<<8)+10,P4_10},{(0x05<<8)+0,P5_0},{(0x05<<8)+1,P5_1},{(0x05<<8)+2,P5_2},{(0x05<<8)+3,P5_3},{(0x05<<8)+4,P5_4},{(0x05<<8)+5,P5_5},{(0x05<<8)+6,P5_6},{(0x05<<8)+7,P5_7},{(0x06<<8)+1,P6_1},{(0x06<<8)+2,P6_2},{(0x06<<8)+3,P6_3},{(0x06<<8)+4,P6_4},{(0x06<<8)+5,P6_5},{(0x06<<8)+6,P6_6},{(0x06<<8)+7,P6_7},{(0x06<<8)+8,P6_8},{(0x06<<8)+9,P6_9},{(0x06<<8)+10,P6_10},{(0x06<<8)+11,P6_11},{(0x06<<8)+12,P6_12},{(0x07<<8)+0,P7_0},{(0x07<<8)+1,P7_1},{(0x07<<8)+2,P7_2},{(0x07<<8)+3,P7_3},{(0x07<<8)+4,P7_4},{(0x07<<8)+5,P7_5},{(0x07<<8)+6,P7_6},{(0x07<<8)+7,P7_7},{(0x08<<8)+8,P8_0},{(0x09<<8)+0,P8_1},{(0x09<<8)+1,P8_2},{(0x09<<8)+2,P8_3},{(0x08<<8)+4,P8_4},{(0x08<<8)+5,P8_5},{(0x08<<8)+6,P8_6},{(0x08<<8)+7,P8_7},{(0x09<<8)+0,P9_0},{(0x09<<8)+1,P9_1},{(0x09<<8)+2,P9_2},{(0x09<<8)+3,P9_3},{(0x09<<8)+4,P9_4},{(0x09<<8)+5,P9_5},{(0x09<<8)+6,P9_6},{(0x0A<<8)+1,PA_1},{(0x0A<<8)+2,PA_2},{(0x0A<<8)+3,PA_3},{(0x0A<<8)+4,PA_4},{(0x0B<<8)+0,PB_0},{(0x0B<<8)+1,PB_1},{(0x0B<<8)+2,PB_2},{(0x0B<<8)+3,PB_3},{(0x0B<<8)+4,PB_4},{(0x0B<<8)+5,PB_5},{(0x0B<<8)+6,PB_6},{(0x0C<<8)+1,PC_1},{(0x0C<<8)+2,PC_2},{(0x0C<<8)+3,PC_3},{(0x0C<<8)+4,PC_4},{(0x0C<<8)+5,PC_5},{(0x0C<<8)+6,PC_6},{(0x0C<<8)+7,PC_7},{(0x0C<<8)+8,PC_8},{(0x0C<<8)+9,PC_9},{(0x0C<<8)+10,PC_10},{(0x0C<<8)+11,PC_11},{(0x0C<<8)+12,PC_12},{(0x0C<<8)+13,PC_13},{(0x0C<<8)+14,PC_14},{(0x0D<<8)+0,PD_0},{(0x0D<<8)+1,PD_1},{(0x0D<<8)+2,PD_2},{(0x0D<<8)+3,PD_3},{(0x0D<<8)+4,PD_4},{(0x0D<<8)+5,PD_5},{(0x0D<<8)+6,PD_6},{(0x0D<<8)+7,PD_7},{(0x0D<<8)+8,PD_8},{(0x0D<<8)+9,PD_9},{(0x0D<<8)+10,PD_10},{(0x0D<<8)+11,PD_11},{(0x0D<<8)+12,PD_12},{(0x0D<<8)+13,PD_13},{(0x0D<<8)+14,PD_14},{(0x0D<<8)+15,PD_15},{(0x0D<<8)+16,PD_16},{(0x0E<<8)+0,PE_0},{(0x0E<<8)+1,PE_1},{(0x0E<<8)+2,PE_2},{(0x0E<<8)+3,PE_3},{(0x0E<<8)+4,PE_4},{(0x0E<<8)+5,PE_5},{(0x0E<<8)+6,PE_6},{(0x0E<<8)+7,PE_7},{(0x0E<<8)+8,PE_8},{(0x0E<<8)+9,PE_9},{(0x0E<<8)+10,PE_10},{(0x0E<<8)+11,PE_11},{(0x0E<<8)+12,PE_12},{(0x0E<<8)+13,PE_13},{(0x0E<<8)+14,PE_14},{(0x0E<<8)+15,PE_15},{(0x0F<<8)+1,PF_1},{(0x0F<<8)+2,PF_2},{(0x0F<<8)+3,PF_3},{(0x0F<<8)+5,PF_5},{(0x0F<<8)+6,PF_6},{(0x0F<<8)+7,PF_7},{(0x0F<<8)+8,PF_8},{(0x0F<<8)+9,PF_9},{(0x0F<<8)+10,PF_10},{(0x0F<<8)+11,PF_11}}; + pin_number = strtol(cs, &cn, 10); +// map pins = {{(0x00<<8)+0,P0_0},{(0x00<<8)+1,P0_1},{(0x01<<8)+0,P1_0},{(0x01<<8)+1,P1_1},{(0x01<<8)+2,P1_2},{(0x01<<8)+3,P1_3},{(0x01<<8)+4,P1_4},{(0x01<<8)+5,P1_5},{(0x01<<8)+6,P1_6},{(0x01<<8)+7,P1_7},{(0x01<<8)+8,P1_8},{(0x01<<8)+9,P1_9},{(0x01<<8)+10,P1_10},{(0x01<<8)+11,P1_11},{(0x01<<8)+12,P1_12},{(0x01<<8)+13,P1_13},{(0x01<<8)+14,P1_14},{(0x01<<8)+15,P1_15},{(0x01<<8)+16,P1_16},{(0x01<<8)+17,P1_17},{(0x01<<8)+18,P1_18},{(0x01<<8)+20,P1_20},{(0x02<<8)+0,P2_0},{(0x02<<8)+1,P2_1},{(0x02<<8)+2,P2_2},{(0x02<<8)+3,P2_3},{(0x02<<8)+4,P2_4},{(0x02<<8)+5,P2_5},{(0x02<<8)+6,P2_6},{(0x02<<8)+7,P2_7},{(0x02<<8)+8,P2_8},{(0x02<<8)+9,P2_9},{(0x02<<8)+10,P2_10},{(0x02<<8)+11,P2_11},{(0x02<<8)+12,P2_12},{(0x02<<8)+13,P2_13},{(0x03<<8)+1,P3_1},{(0x03<<8)+2,P3_2},{(0x03<<8)+4,P3_4},{(0x03<<8)+5,P3_5},{(0x03<<8)+6,P3_6},{(0x03<<8)+7,P3_7},{(0x03<<8)+8,P3_8},{(0x04<<8)+0,P4_0},{(0x04<<8)+1,P4_1},{(0x04<<8)+2,P4_2},{(0x04<<8)+3,P4_3},{(0x04<<8)+4,P4_4},{(0x04<<8)+5,P4_5},{(0x04<<8)+6,P4_6},{(0x04<<8)+8,P4_8},{(0x04<<8)+9,P4_9},{(0x04<<8)+10,P4_10},{(0x05<<8)+0,P5_0},{(0x05<<8)+1,P5_1},{(0x05<<8)+2,P5_2},{(0x05<<8)+3,P5_3},{(0x05<<8)+4,P5_4},{(0x05<<8)+5,P5_5},{(0x05<<8)+6,P5_6},{(0x05<<8)+7,P5_7},{(0x06<<8)+1,P6_1},{(0x06<<8)+2,P6_2},{(0x06<<8)+3,P6_3},{(0x06<<8)+4,P6_4},{(0x06<<8)+5,P6_5},{(0x06<<8)+6,P6_6},{(0x06<<8)+7,P6_7},{(0x06<<8)+8,P6_8},{(0x06<<8)+9,P6_9},{(0x06<<8)+10,P6_10},{(0x06<<8)+11,P6_11},{(0x06<<8)+12,P6_12},{(0x07<<8)+0,P7_0},{(0x07<<8)+1,P7_1},{(0x07<<8)+2,P7_2},{(0x07<<8)+3,P7_3},{(0x07<<8)+4,P7_4},{(0x07<<8)+5,P7_5},{(0x07<<8)+6,P7_6},{(0x07<<8)+7,P7_7},{(0x08<<8)+8,P8_0},{(0x09<<8)+0,P8_1},{(0x09<<8)+1,P8_2},{(0x09<<8)+2,P8_3},{(0x08<<8)+4,P8_4},{(0x08<<8)+5,P8_5},{(0x08<<8)+6,P8_6},{(0x08<<8)+7,P8_7},{(0x09<<8)+0,P9_0},{(0x09<<8)+1,P9_1},{(0x09<<8)+2,P9_2},{(0x09<<8)+3,P9_3},{(0x09<<8)+4,P9_4},{(0x09<<8)+5,P9_5},{(0x09<<8)+6,P9_6},{(0x0A<<8)+1,PA_1},{(0x0A<<8)+2,PA_2},{(0x0A<<8)+3,PA_3},{(0x0A<<8)+4,PA_4},{(0x0B<<8)+0,PB_0},{(0x0B<<8)+1,PB_1},{(0x0B<<8)+2,PB_2},{(0x0B<<8)+3,PB_3},{(0x0B<<8)+4,PB_4},{(0x0B<<8)+5,PB_5},{(0x0B<<8)+6,PB_6},{(0x0C<<8)+1,PC_1},{(0x0C<<8)+2,PC_2},{(0x0C<<8)+3,PC_3},{(0x0C<<8)+4,PC_4},{(0x0C<<8)+5,PC_5},{(0x0C<<8)+6,PC_6},{(0x0C<<8)+7,PC_7},{(0x0C<<8)+8,PC_8},{(0x0C<<8)+9,PC_9},{(0x0C<<8)+10,PC_10},{(0x0C<<8)+11,PC_11},{(0x0C<<8)+12,PC_12},{(0x0C<<8)+13,PC_13},{(0x0C<<8)+14,PC_14},{(0x0D<<8)+0,PD_0},{(0x0D<<8)+1,PD_1},{(0x0D<<8)+2,PD_2},{(0x0D<<8)+3,PD_3},{(0x0D<<8)+4,PD_4},{(0x0D<<8)+5,PD_5},{(0x0D<<8)+6,PD_6},{(0x0D<<8)+7,PD_7},{(0x0D<<8)+8,PD_8},{(0x0D<<8)+9,PD_9},{(0x0D<<8)+10,PD_10},{(0x0D<<8)+11,PD_11},{(0x0D<<8)+12,PD_12},{(0x0D<<8)+13,PD_13},{(0x0D<<8)+14,PD_14},{(0x0D<<8)+15,PD_15},{(0x0D<<8)+16,PD_16},{(0x0E<<8)+0,PE_0},{(0x0E<<8)+1,PE_1},{(0x0E<<8)+2,PE_2},{(0x0E<<8)+3,PE_3},{(0x0E<<8)+4,PE_4},{(0x0E<<8)+5,PE_5},{(0x0E<<8)+6,PE_6},{(0x0E<<8)+7,PE_7},{(0x0E<<8)+8,PE_8},{(0x0E<<8)+9,PE_9},{(0x0E<<8)+10,PE_10},{(0x0E<<8)+11,PE_11},{(0x0E<<8)+12,PE_12},{(0x0E<<8)+13,PE_13},{(0x0E<<8)+14,PE_14},{(0x0E<<8)+15,PE_15},{(0x0F<<8)+1,PF_1},{(0x0F<<8)+2,PF_2},{(0x0F<<8)+3,PF_3},{(0x0F<<8)+5,PF_5},{(0x0F<<8)+6,PF_6},{(0x0F<<8)+7,PF_7},{(0x0F<<8)+8,PF_8},{(0x0F<<8)+9,PF_9},{(0x0F<<8)+10,PF_10},{(0x0F<<8)+11,PF_11}}; + map pins = { + {(0x00<<8)+0,P0_0},{(0x00<<8)+1,P0_1}, + {(0x01<<8)+0,P1_0},{(0x01<<8)+1,P1_1},{(0x01<<8)+2,P1_2},{(0x01<<8)+3,P1_3},{(0x01<<8)+4,P1_4},{(0x01<<8)+5,P1_5},{(0x01<<8)+6,P1_6},{(0x01<<8)+7,P1_7},{(0x01<<8)+8,P1_8},{(0x01<<8)+9,P1_9},{(0x01<<8)+10,P1_10},{(0x01<<8)+11,P1_11},{(0x01<<8)+12,P1_12},{(0x01<<8)+13,P1_13},{(0x01<<8)+14,P1_14},{(0x01<<8)+15,P1_15},{(0x01<<8)+16,P1_16},{(0x01<<8)+17,P1_17},{(0x01<<8)+18,P1_18},{(0x01<<8)+19,P1_19},{(0x01<<8)+20,P1_20}, + {(0x02<<8)+0,P2_0},{(0x02<<8)+1,P2_1},{(0x02<<8)+2,P2_2},{(0x02<<8)+3,P2_3},{(0x02<<8)+4,P2_4},{(0x02<<8)+5,P2_5},{(0x02<<8)+6,P2_6},{(0x02<<8)+7,P2_7},{(0x02<<8)+8,P2_8},{(0x02<<8)+9,P2_9},{(0x02<<8)+10,P2_10},{(0x02<<8)+11,P2_11},{(0x02<<8)+12,P2_12},{(0x02<<8)+13,P2_13}, + {(0x03<<8)+0,P3_0},{(0x03<<8)+1,P3_1},{(0x03<<8)+2,P3_2}, {(0x03<<8)+4,P3_4},{(0x03<<8)+5,P3_5},{(0x03<<8)+6,P3_6},{(0x03<<8)+7,P3_7},{(0x03<<8)+8,P3_8}, + {(0x04<<8)+0,P4_0},{(0x04<<8)+1,P4_1},{(0x04<<8)+2,P4_2},{(0x04<<8)+3,P4_3},{(0x04<<8)+4,P4_4},{(0x04<<8)+5,P4_5},{(0x04<<8)+6,P4_6}, {(0x04<<8)+8,P4_8},{(0x04<<8)+9,P4_9},{(0x04<<8)+10,P4_10}, + {(0x05<<8)+0,P5_0},{(0x05<<8)+1,P5_1},{(0x05<<8)+2,P5_2},{(0x05<<8)+3,P5_3},{(0x05<<8)+4,P5_4},{(0x05<<8)+5,P5_5},{(0x05<<8)+6,P5_6},{(0x05<<8)+7,P5_7}, + {(0x06<<8)+1,P6_1},{(0x06<<8)+2,P6_2},{(0x06<<8)+3,P6_3},{(0x06<<8)+4,P6_4},{(0x06<<8)+5,P6_5},{(0x06<<8)+6,P6_6},{(0x06<<8)+7,P6_7},{(0x06<<8)+8,P6_8},{(0x06<<8)+9,P6_9},{(0x06<<8)+10,P6_10},{(0x06<<8)+11,P6_11},{(0x06<<8)+12,P6_12}, + {(0x07<<8)+0,P7_0},{(0x07<<8)+1,P7_1},{(0x07<<8)+2,P7_2},{(0x07<<8)+3,P7_3},{(0x07<<8)+4,P7_4},{(0x07<<8)+5,P7_5},{(0x07<<8)+6,P7_6},{(0x07<<8)+7,P7_7}, + {(0x08<<8)+0,P8_0},{(0x09<<8)+1,P8_1},{(0x08<<8)+2,P8_2},{(0x09<<8)+3,P8_3},{(0x08<<8)+4,P8_4},{(0x08<<8)+5,P8_5},{(0x08<<8)+6,P8_6},{(0x08<<8)+7,P8_7}, + {(0x09<<8)+0,P9_0},{(0x09<<8)+1,P9_1},{(0x09<<8)+2,P9_2},{(0x09<<8)+3,P9_3},{(0x09<<8)+4,P9_4},{(0x09<<8)+5,P9_5},{(0x09<<8)+6,P9_6}, + {(0x0A<<8)+1,PA_1},{(0x0A<<8)+2,PA_2},{(0x0A<<8)+3,PA_3},{(0x0A<<8)+4,PA_4}, + {(0x0B<<8)+0,PB_0},{(0x0B<<8)+1,PB_1},{(0x0B<<8)+2,PB_2},{(0x0B<<8)+3,PB_3},{(0x0B<<8)+4,PB_4},{(0x0B<<8)+5,PB_5},{(0x0B<<8)+6,PB_6}, + {(0x0C<<8)+1,PC_1},{(0x0C<<8)+2,PC_2},{(0x0C<<8)+3,PC_3},{(0x0C<<8)+4,PC_4},{(0x0C<<8)+5,PC_5},{(0x0C<<8)+6,PC_6},{(0x0C<<8)+7,PC_7},{(0x0C<<8)+8,PC_8},{(0x0C<<8)+9,PC_9},{(0x0C<<8)+10,PC_10},{(0x0C<<8)+11,PC_11},{(0x0C<<8)+12,PC_12},{(0x0C<<8)+13,PC_13},{(0x0C<<8)+14,PC_14}, + {(0x0D<<8)+0,PD_0},{(0x0D<<8)+1,PD_1},{(0x0D<<8)+2,PD_2},{(0x0D<<8)+3,PD_3},{(0x0D<<8)+4,PD_4},{(0x0D<<8)+5,PD_5},{(0x0D<<8)+6,PD_6},{(0x0D<<8)+7,PD_7},{(0x0D<<8)+8,PD_8},{(0x0D<<8)+9,PD_9},{(0x0D<<8)+10,PD_10},{(0x0D<<8)+11,PD_11},{(0x0D<<8)+12,PD_12},{(0x0D<<8)+13,PD_13},{(0x0D<<8)+14,PD_14},{(0x0D<<8)+15,PD_15},{(0x0D<<8)+16,PD_16}, + {(0x0E<<8)+0,PE_0},{(0x0E<<8)+1,PE_1},{(0x0E<<8)+2,PE_2},{(0x0E<<8)+3,PE_3},{(0x0E<<8)+4,PE_4},{(0x0E<<8)+5,PE_5},{(0x0E<<8)+6,PE_6},{(0x0E<<8)+7,PE_7},{(0x0E<<8)+8,PE_8},{(0x0E<<8)+9,PE_9},{(0x0E<<8)+10,PE_10},{(0x0E<<8)+11,PE_11},{(0x0E<<8)+12,PE_12},{(0x0E<<8)+13,PE_13},{(0x0E<<8)+14,PE_14},{(0x0E<<8)+15,PE_15}, + {(0x0F<<8)+1,PF_1},{(0x0F<<8)+2,PF_2},{(0x0F<<8)+3,PF_3},{(0x0F<<8)+4,PF_4},{(0x0F<<8)+5,PF_5},{(0x0F<<8)+6,PF_6},{(0x0F<<8)+7,PF_7},{(0x0F<<8)+8,PF_8},{(0x0F<<8)+9,PF_9},{(0x0F<<8)+10,PF_10},{(0x0F<<8)+11,PF_11} + }; + // Find the pin - if( pins.count((port_number<<8)+pin) > 0 ){ - this->mbed_pin = new DigitalInOut( pins[(port_number<<8)+pin] ); - this->pinName = pins[(port_number<<8)+pin]; + if( pins.count((port_number<<8)+pin_number) > 0 ){ + this->mbed_pin = new DigitalInOut( pins[(port_number<<8)+pin_number] ); + this->pinName = pins[(port_number<<8)+pin_number]; } // if strtol read some numbers, cn will point to the first non-digit - if ((cn > cs) && (pin < 32)){ + if ((cn > cs) && (pin_number < 32)){ // now check for modifiers:- // ! = invert pin @@ -163,18 +178,17 @@ mbed::PwmOut* Pin::hardware_pwm() mbed::InterruptIn* Pin::interrupt_pin() { if(!this->valid) return nullptr; -/* + // set as input as_input(); - if (port_number == 0 || port_number == 2) { - PinName pinname = port_pin((PortName)port_number, pin); + if (port_number >= 0 && port_number <= 7 && pin_number >= 0 && pin_number <= 7) { //LPC4330 allows bits 0..7 on ports 0..7 + PinName pinname = port_pin((PortName)port_number, pin_number); return new mbed::InterruptIn(pinname); - }else{ this->valid= false; return nullptr; } -*/ + return nullptr; } diff --git a/src/libs/Pin.h b/src/libs/Pin.h index 49ca89e0..d4a01019 100644 --- a/src/libs/Pin.h +++ b/src/libs/Pin.h @@ -5,7 +5,6 @@ #include #include -//#include "libs/LPC17xx/sLPC17xx.h" // smoothed mbed.h lib #include "PinNames.h" namespace mbed { @@ -26,7 +25,7 @@ class Pin { } inline bool equals(const Pin& other) const { - //return (this->pin == other.pin) && (this->port == other.port); + return (this->pin_number == other.pin_number) && (this->port_number == other.port_number); return false; } @@ -87,6 +86,9 @@ class Pin { //getter to access to mbed pinName PinName getPinName() {return pinName; } + int port_number; + int pin_number; + private: //Added pinName PinName pinName; diff --git a/src/libs/Pwm.cpp b/src/libs/Pwm.cpp index 744ee9e4..de30e9a0 100644 --- a/src/libs/Pwm.cpp +++ b/src/libs/Pwm.cpp @@ -1,6 +1,7 @@ #include "Pwm.h" #include "nuts_bolts.h" +#include "utils.h" #define PID_PWM_MAX 256 diff --git a/src/libs/SlowTicker.cpp b/src/libs/SlowTicker.cpp index 8f895040..577d541c 100644 --- a/src/libs/SlowTicker.cpp +++ b/src/libs/SlowTicker.cpp @@ -18,6 +18,9 @@ using namespace std; #include +#include "SEGGER_SYSVIEW.h" +#include "SEGGER_RTT_Conf.h" + // This module uses a Timer to periodically call hooks // Modules register with a function ( callback ) and a frequency, and we then call that function at the given frequency. @@ -36,7 +39,7 @@ SlowTicker::SlowTicker(){ LPC_CCU1->CLKCCU[CLK_MX_TIMER2].CFG |= 1; LPC_RGU->RESET_CTRL1 = 1 << (RGU_TIMER2_RST & 31); //Trigger a peripheral reset for the timer while (!(LPC_RGU->RESET_ACTIVE_STATUS1 & (1 << (RGU_TIMER2_RST & 31)))){} - /* Configure Timer 0 */ + /* Configure Timer 2 */ LPC_TIMER2->CTCR = 0x0; // timer mode LPC_TIMER2->TCR = 0; // Disable interrupt LPC_TIMER2->PR = prescale - 1; @@ -135,8 +138,14 @@ void SlowTicker::on_idle(void*) } extern "C" void TIMER2_IRQHandler (void){ + // SEGGER_RTT_LOCK(); + SEGGER_SYSVIEW_RecordEnterISR(); + if((LPC_TIMER2->IR >> 0) & 1){ // If interrupt register set for MR0 LPC_TIMER2->IR |= 1 << 0; // Reset it } global_slow_ticker->tick(); + // NVIC_ClearPendingIRQ(TIMER2_IRQn); + SEGGER_SYSVIEW_RecordExitISR(); + // SEGGER_RTT_UNLOCK(); } diff --git a/src/libs/StepTicker.cpp b/src/libs/StepTicker.cpp index 56c0da17..12446b6c 100644 --- a/src/libs/StepTicker.cpp +++ b/src/libs/StepTicker.cpp @@ -19,9 +19,16 @@ #include #include +#include "SEGGER_SYSVIEW.h" +#include "SEGGER_RTT_Conf.h" + #ifdef STEPTICKER_DEBUG_PIN +// debug pins, only used if defined in src/makefile #include "gpio.h" -extern GPIO stepticker_debug_pin; +GPIO stepticker_debug_pin(STEPTICKER_DEBUG_PIN); +#define SET_STEPTICKER_DEBUG_PIN(n) {if(n) stepticker_debug_pin.set(); else stepticker_debug_pin.clear(); } +#else +#define SET_STEPTICKER_DEBUG_PIN(n) #endif @@ -53,7 +60,7 @@ StepTicker::StepTicker(){ LPC_CCU1->CLKCCU[CLK_MX_TIMER1].CFG |= 1; LPC_RGU->RESET_CTRL1 = 1 << (RGU_TIMER1_RST & 31); //Trigger a peripheral reset for the timer while (!(LPC_RGU->RESET_ACTIVE_STATUS1 & (1 << (RGU_TIMER1_RST & 31)))){} - /* Configure Timer 0 */ + /* Configure Timer 1 */ LPC_TIMER1->CTCR = 0x0; // timer mode LPC_TIMER1->TCR = 0; // Disable interrupt LPC_TIMER1->PR = prescale - 1; @@ -122,18 +129,36 @@ void StepTicker::unstep_tick() this->unstep.reset(); } +// Unstep timer interrupt handler extern "C" void TIMER1_IRQHandler (void) { +// SEGGER_RTT_LOCK(); +// SEGGER_SYSVIEW_RecordEnterISR(); LPC_TIMER1->IR |= 1 << 0; StepTicker::getInstance()->unstep_tick(); + +// NVIC_ClearPendingIRQ(TIMER1_IRQn); +// SEGGER_SYSVIEW_RecordExitISR(); +// SEGGER_RTT_UNLOCK(); } // The actual interrupt handler where we do all the work extern "C" void TIMER0_IRQHandler (void) { +// SEGGER_RTT_LOCK(); +// SEGGER_SYSVIEW_RecordEnterISR(); + // Reset interrupt register LPC_TIMER0->IR |= 1 << 0; StepTicker::getInstance()->step_tick(); + + // Reset interrupt register + LPC_TIMER0->IR |= 1 << 0; + StepTicker::getInstance()->step_tick(); +// NVIC_ClearPendingIRQ(TIMER0_IRQn); + +// SEGGER_SYSVIEW_RecordExitISR(); +// SEGGER_RTT_UNLOCK(); } extern "C" void PendSV_Handler(void) diff --git a/src/libs/checksumm.h b/src/libs/checksumm.h index 16aab8fa..3916b8d3 100644 --- a/src/libs/checksumm.h +++ b/src/libs/checksumm.h @@ -51,7 +51,9 @@ constexpr uint16_t operator "" _checksum(const char* s, size_t n) { #define CHECKSUM(X) std::integral_constant::value #else /* !CHECKSUM_USE_CPP */ - +#ifdef __CDT_PARSER__ +#define CHECKSUM(X) ((uint16_t)sizeof(X)) +#else /* Adam Green's old and crusty C approach. */ /* Recursively define SUM1, the basic checksum % 255 */ #define SUM1_1(X) ((X)[0] % 255) @@ -199,6 +201,7 @@ constexpr uint16_t operator "" _checksum(const char* s, size_t n) { sizeof(X) == 33 ? CHECKSUM_32(X) : \ 0xFFFF) #endif /* DEBUG */ +#endif /* __CDT_PARSER__ */ #endif /* CHECKSUM_USE_CPP */ #endif /* _CHECKSUM_MACRO_H_ */ diff --git a/src/libs/md5.h b/src/libs/md5.h new file mode 100644 index 00000000..7645620b --- /dev/null +++ b/src/libs/md5.h @@ -0,0 +1,92 @@ +/* MD5 + converted to C++ class by Frank Thilo (thilo@unix-ag.org) + for bzflag (http://www.bzflag.org) + + based on: + + md5.h and md5.c + reference implementation of RFC 1321 + + Copyright (C) 1991-2, RSA Data Security, Inc. Created 1991. All +rights reserved. + +License to copy and use this software is granted provided that it +is identified as the "RSA Data Security, Inc. MD5 Message-Digest +Algorithm" in all material mentioning or referencing this software +or this function. + +License is also granted to make and use derivative works provided +that such works are identified as "derived from the RSA Data +Security, Inc. MD5 Message-Digest Algorithm" in all material +mentioning or referencing the derived work. + +RSA Data Security, Inc. makes no representations concerning either +the merchantability of this software or the suitability of this +software for any particular purpose. It is provided "as is" +without express or implied warranty of any kind. + +These notices must be retained in any copies of any part of this +documentation and/or software. + +*/ + +#ifndef BZF_MD5_H +#define BZF_MD5_H + +#include + + +// a small class for calculating MD5 hashes of strings or byte arrays +// it is not meant to be fast or secure +// +// usage: 1) feed it blocks of uchars with update() +// 2) finalize() +// 3) get hexdigest() string +// or +// MD5(std::string).hexdigest() +// +// assumes that char is 8 bit and int is 32 bit +class MD5 +{ +public: + typedef unsigned int size_type; // must be 32bit + + MD5(); + MD5(const std::string &text); + void update(const unsigned char *buf, size_type length); + void update(const char *buf, size_type length); + MD5 &finalize(); + std::string hexdigest() const; + void bindigest(void *buf, int len) const; + +private: + void init(); + typedef unsigned char uint1; // 8bit + typedef unsigned int uint4; // 32bit + enum {blocksize = 64}; // VC6 won't eat a const static int here + + void transform(const uint1 block[blocksize]); + static void decode(uint4 output[], const uint1 input[], size_type len); + static void encode(uint1 output[], const uint4 input[], size_type len); + + bool finalized; + uint1 buffer[blocksize]; // bytes that didn't fit in last 64 byte chunk + uint4 count[2]; // 64bit counter for number of bits (lo, hi) + uint4 state[4]; // digest so far + uint1 digest[16]; // the result + + // low level logic operations + static inline uint4 F(uint4 x, uint4 y, uint4 z); + static inline uint4 G(uint4 x, uint4 y, uint4 z); + static inline uint4 H(uint4 x, uint4 y, uint4 z); + static inline uint4 I(uint4 x, uint4 y, uint4 z); + static inline uint4 rotate_left(uint4 x, int n); + static inline void FF(uint4 &a, uint4 b, uint4 c, uint4 d, uint4 x, uint4 s, uint4 ac); + static inline void GG(uint4 &a, uint4 b, uint4 c, uint4 d, uint4 x, uint4 s, uint4 ac); + static inline void HH(uint4 &a, uint4 b, uint4 c, uint4 d, uint4 x, uint4 s, uint4 ac); + static inline void II(uint4 &a, uint4 b, uint4 c, uint4 d, uint4 x, uint4 s, uint4 ac); +}; + +std::string md5(const std::string str); + +#endif diff --git a/src/libs/utils.cpp b/src/libs/utils.cpp index 0c197687..273a5991 100644 --- a/src/libs/utils.cpp +++ b/src/libs/utils.cpp @@ -152,6 +152,23 @@ bool file_exists( const string file_name ) return exists; } +/* +//TODO Add watchdog timer support +// Prepares and executes a watchdog reset for dfu or reboot +void system_reset( bool dfu ) +{ + if(dfu) { + LPC_WDT->WDCLKSEL = 0x1; // Set CLK src to PCLK + uint32_t clk = SystemCoreClock / 16; // WD has a fixed /4 prescaler, PCLK default is /4 + LPC_WDT->WDTC = 1 * (float)clk; // Reset in 1 second + LPC_WDT->WDMOD = 0x3; // Enabled and Reset + LPC_WDT->WDFEED = 0xAA; // Kick the dog! + LPC_WDT->WDFEED = 0x55; + } else { + NVIC_SystemReset(); + } +} +*/ // Convert a path indication ( absolute or relative ) into a path ( absolute ) string absolute_from_relative( string path ) { diff --git a/src/main.cpp b/src/main.cpp index 7b8e2734..bd8b0cbf 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -8,13 +8,28 @@ #include "libs/Kernel.h" #include "modules/tools/laser/Laser.h" +#include "modules/tools/spindle/Spindle.h" #include "modules/tools/extruder/ExtruderMaker.h" #include "modules/tools/temperaturecontrol/TemperatureControlPool.h" #include "modules/tools/endstops/Endstops.h" +#include "modules/tools/zprobe/ZProbe.h" +#include "modules/tools/scaracal/SCARAcal.h" +#include "RotaryDeltaCalibration.h" #include "modules/tools/switch/SwitchPool.h" +#include "modules/tools/temperatureswitch/TemperatureSwitch.h" #include "modules/tools/drillingcycles/Drillingcycles.h" +#include "FilamentDetector.h" +#include "MotorDriverControl.h" #include "modules/robot/Conveyor.h" +//#include "modules/utils/simpleshell/SimpleShell.h" +#include "modules/utils/configurator/Configurator.h" +#include "modules/utils/currentcontrol/CurrentControl.h" +//#include "modules/utils/player/Player.h" +#include "modules/utils/killbutton/KillButton.h" +#include "modules/utils/PlayLed/PlayLed.h" +//#include "modules/utils/panel/Panel.h" +//#include "libs/Network/uip/Network.h" #include "Config.h" #include "checksumm.h" #include "ConfigValue.h" @@ -38,13 +53,17 @@ */ #include "StreamOutputPool.h" #include "ToolManager.h" +#include "SEGGER_SYSVIEW.h" #include "version.h" -#include "system_LPC43xx.h" -#include "platform_memory.h" #include "mbed.h" +#define second_usb_serial_enable_checksum CHECKSUM("second_usb_serial_enable") +#define disable_msd_checksum CHECKSUM("msd_disable") +#define dfu_enable_checksum CHECKSUM("dfu_enable") +#define watchdog_timeout_checksum CHECKSUM("watchdog_timeout") + DigitalOut leds[4] = { #ifdef TARGET_BAMBINO210E DigitalOut(P6_11), @@ -78,6 +97,8 @@ USBMSD *msc= NULL; */ void init() { + SEGGER_SYSVIEW_Conf(); + // Kernel creates modules, and receives and dispatches events between them Kernel* kernel = new Kernel(); @@ -87,58 +108,173 @@ void init() { #ifdef CNC kernel->streams->printf(" CNC Build\r\n"); #endif -#ifdef DISABLEMSD - kernel->streams->printf(" NOMSD Build\r\n"); -#endif - // Create and add main modules +bool sdok= false; //TODO remove once SD Card code is working +// bool sdok= (sd.disk_initialize() == 0); +// if(!sdok) kernel->streams->printf("SDCard failed to initialize\r\n"); + +// #ifdef NONETWORK +// kernel->streams->printf("NETWORK is disabled\r\n"); +// #endif + +//#ifdef DISABLEMSD +// // attempt to be able to disable msd in config +// if(sdok && !kernel->config->value( disable_msd_checksum )->by_default(false)->as_bool()){ +// // HACK to zero the memory USBMSD uses as it and its objects seem to not initialize properly in the ctor +// size_t n= sizeof(USBMSD); +// void *v = AHB0.alloc(n); +// memset(v, 0, n); // clear the allocated memory +// msc= new(v) USBMSD(&u, &sd); // allocate object using zeroed memory +// }else{ +// msc= NULL; +// kernel->streams->printf("MSD is disabled\r\n"); +// } +//#endif + + // Create and add main modules +// kernel->add_module( new Player() ); + +// kernel->add_module( new CurrentControl() ); +// kernel->add_module( new KillButton() ); +// kernel->add_module( new PlayLed() ); + + // these modules can be completely disabled in the Makefile by adding to EXCLUDE_MODULES + #ifndef NO_TOOLS_ENDSTOPS kernel->add_module( new Endstops() ); - kernel->add_module( new Laser() ); + #endif - // Create all Switch modules + #ifndef NO_TOOLS_SWITCH SwitchPool *sp= new SwitchPool(); sp->load_tools(); delete sp; - - // Create all TemperatureControl modules. Note order is important here must be after extruder so Tn as a parameter will get executed first + #endif + #ifndef NO_TOOLS_EXTRUDER + // NOTE this must be done first before Temperature control so ToolManager can handle Tn before temperaturecontrol module does + ExtruderMaker *em= new ExtruderMaker(); + em->load_tools(); + delete em; + #endif + #ifndef NO_TOOLS_TEMPERATURECONTROL + // Note order is important here must be after extruder so Tn as a parameter will get executed first TemperatureControlPool *tp= new TemperatureControlPool(); tp->load_tools(); delete tp; + #endif + #ifndef NO_TOOLS_LASER + kernel->add_module( new Laser() ); + #endif + #ifndef NO_TOOLS_SPINDLE + kernel->add_module( new Spindle() ); + #endif + #ifndef NO_UTILS_PANEL +// kernel->add_module( new Panel() ); + #endif + #ifndef NO_TOOLS_ZPROBE + kernel->add_module( new ZProbe() ); + #endif + #ifndef NO_TOOLS_SCARACAL +// kernel->add_module( newSCARAcal() ); + #endif + #ifndef NO_TOOLS_ROTARYDELTACALIBRATION + kernel->add_module( new RotaryDeltaCalibration() ); + #endif + #ifndef NONETWORK +// kernel->add_module( new Network() ); + #endif + #ifndef NO_TOOLS_TEMPERATURESWITCH + // Must be loaded after TemperatureControl + kernel->add_module( new TemperatureSwitch() ); + #endif + #ifndef NO_TOOLS_DRILLINGCYCLES + kernel->add_module( new Drillingcycles() ); + #endif + #ifndef NO_TOOLS_FILAMENTDETECTOR + kernel->add_module( new FilamentDetector() ); + #endif + #ifndef NO_UTILS_MOTORDRIVERCONTROL + kernel->add_module( new MotorDriverControl(0) ); + #endif +// // Create and initialize USB stuff +// u.init(); +//#ifdef DISABLEMSD +// if(sdok && msc != NULL){ +// kernel->add_module( msc ); +// } +//#else +// kernel->add_module( &msc ); +//#endif - // TOADDBACK kernel->add_module( &u ); +// kernel->add_module( &usbserial ); +// if( kernel->config->value( second_usb_serial_enable_checksum )->by_default(false)->as_bool() ){ +// kernel->add_module( new USBSerial(&u) ); +// } +// if( kernel->config->value( dfu_enable_checksum )->by_default(false)->as_bool() ){ +// kernel->add_module( new DFU(&u)); +// } - // Clear the configuration cache as it is no longer needed +// // 10 second watchdog timeout (or config as seconds) +// float t= kernel->config->value( watchdog_timeout_checksum )->by_default(10.0F)->as_number(); +// if(t > 0.1F) { +// // NOTE setting WDT_RESET with the current bootloader would leave it in DFU mode which would be suboptimal +// kernel->add_module( new Watchdog(t*1000000, WDT_MRI)); // WDT_RESET)); +// kernel->streams->printf("Watchdog enabled for %f seconds\n", t); +// }else{ + kernel->streams->printf("WARNING Watchdog is disabled\n"); +// } + + +// kernel->add_module( &u ); + + // memory before cache is cleared + //SimpleShell::print_mem(kernel->streams); + + // clear up the config cache to save some memory kernel->config->config_cache_clear(); if(kernel->is_using_leds()) { // set some leds to indicate status... led0 init done, led1 mainloop running, led2 idle loop running, led3 sdcard ok - leds[0]= 0; // indicate we are done with init - // TOADDBACK leds[3]= sdok?1:0; // 4th led indicates sdcard is available (TODO maye should indicate config was found) + leds[0]= 1; // indicate we are done with init + leds[3]= sdok?1:0; // 4th led indicates sdcard is available (TODO maye should indicate config was found) + } + + if(sdok) { + // load config override file if present + // NOTE only Mxxx commands that set values should be put in this file. The file is generated by M500 + FILE *fp= fopen(kernel->config_override_filename(), "r"); + if(fp != NULL) { + char buf[132]; + kernel->streams->printf("Loading config override file: %s...\n", kernel->config_override_filename()); + while(fgets(buf, sizeof buf, fp) != NULL) { + kernel->streams->printf(" %s", buf); + if(buf[0] == ';') continue; // skip the comments + struct SerialMessage message= {&(StreamOutput::NullStream), buf}; + kernel->call_event(ON_CONSOLE_LINE_RECEIVED, &message); + } + kernel->streams->printf("config override file executed\n"); + fclose(fp); + } } // start the timers and interrupts - kernel->conveyor->start(THEROBOT->get_number_registered_motors()); - kernel->step_ticker->start(); + THEKERNEL->conveyor->start(THEROBOT->get_number_registered_motors()); + THEKERNEL->step_ticker->start(); THEKERNEL->slow_ticker->start(); } -int main() { - +int main() +{ init(); - - uint16_t cnt = 0; - + int cnt = 0; // Main loop while(1){ if(THEKERNEL->is_using_leds()) { // flash led 2 to show we are alive - leds[1]= (cnt++ & 0x1000) ? 1 : 0; + leds[0]= (cnt++ & 0x1000) ? 1 : 0; } THEKERNEL->call_event(ON_MAIN_LOOP); THEKERNEL->call_event(ON_IDLE); } } - diff --git a/src/makefile b/src/makefile index 2908320a..f4dcd9fb 100644 --- a/src/makefile +++ b/src/makefile @@ -16,7 +16,17 @@ MRI_BREAK_ON_INIT ?= 1 #OPTIMIZATION := 0 OPTIMIZATION := 2 - +# Set to non zero value if you want checks to be enabled which reserve a +# specific amount of space for the stack. The heap's growth will be +# constrained to reserve this much space for the stack and the stack won't be +# able to grow larger than this amount. +#STACK_SIZE=3072 + +# Set to 1 to allow MRI debug monitor to take full control of UART0 and use it +# as a dedicated debug channel. If you are using the USB based serial port for +# the console then this should cause you no problems. Set MRI_BREAK_ON_INIT to +# 0 if you don't want to break into GDB at startup. +ENABLE_DEBUG_MONITOR?=0 ######################################################################################################################## PROJECT := Smoothie2 @@ -26,11 +36,14 @@ NO_FLOAT_SCANF := 1 NO_FLOAT_PRINTF := 0 LIBS_PREFIX := configdefault.o MRI_SEMIHOST_STDIO := 0 +#MBED_LIBS += USBHost +#MBED_LIBS += USBDevice # Use C++11 features for the checksums DEFINES += -DCHECKSUM_USE_CPP - +#Disable the network +DEFINES += -DNONETWORK # Need to use a custom LPC4337 script for boards like the Smoothie2 hardware which use that chip instead of the LPC4330. ifeq "$(BOARD)" "Smoothie2Proto1" @@ -71,6 +84,11 @@ DEFINES += -DTARGET_BAMBINO200E MRI_UART ?= MRI_UART_TX_P5_6 MRI_UART_RX_P1_14 MRI_UART_BAUD=230400 endif + +# this is the default UART baud rate used if it is not set in config +# it is also the baud rate used to report any errors found while parsing the config file +DEFINES += -DDEFAULT_SERIAL_BAUD_RATE=9600 + # generate a git version string ifneq "$(OS)" "Windows_NT" DEFINES += -D__GITVERSIONSTRING__=\"$(shell ./generate-version.sh)\" @@ -90,6 +108,8 @@ endif include $(GCC4MBED_DIR)/build/gcc4mbed.mk +# set to not compile in any network support +#export NONETWORK = 1 configdefault.o : config.default @echo Packing $< diff --git a/src/modules/communication/GcodeDispatch.cpp b/src/modules/communication/GcodeDispatch.cpp index 8fe4469a..6ec38514 100644 --- a/src/modules/communication/GcodeDispatch.cpp +++ b/src/modules/communication/GcodeDispatch.cpp @@ -151,9 +151,13 @@ void GcodeDispatch::on_console_line_received(void *line) if(THEKERNEL->is_halted()) { // we ignore all commands until M999, unless it is in the exceptions list (like M105 get temp) if(gcode->has_m && gcode->m == 999) { - THEKERNEL->call_event(ON_HALT, (void *)1); // clears on_halt - - // fall through and pass onto other modules + if(THEKERNEL->is_halted()) { + THEKERNEL->call_event(ON_HALT, (void *)1); // clears on_halt + new_message.stream->printf("WARNING: After HALT you should HOME as position is currently unknown\n"); + } + new_message.stream->printf("ok\n"); + delete gcode; + continue; }else if(!is_allowed_mcode(gcode->m)) { // ignore everything, return error string to host @@ -196,7 +200,7 @@ void GcodeDispatch::on_console_line_received(void *line) } } // makes it handle the parameters as a machine position - THEKERNEL->robot->next_command_is_MCS= true; + THEROBOT->next_command_is_MCS= true; } @@ -265,8 +269,9 @@ void GcodeDispatch::on_console_line_received(void *line) while(is_whitespace(str.front())){ str= str.substr(1); } // strip leading whitespace delete gcode; - // TODO : TOADDBACK When Simpleshell is added back in - /*if(str.empty()) { + // TOADDBACK When Simpleshell is added back in + /* + if(str.empty()) { SimpleShell::parse_command("help", "", new_message.stream); }else{ @@ -332,7 +337,6 @@ void GcodeDispatch::on_console_line_received(void *line) gcode->add_nl= true; break; // fall through to process by modules } - } } @@ -356,6 +360,10 @@ void GcodeDispatch::on_console_line_received(void *line) new_message.stream->printf("unknown\r\n"); } + // we cannot continue safely after an error so we enter HALT state + new_message.stream->printf("Entering Alarm/Halt state\n"); + THEKERNEL->call_event(ON_HALT, nullptr); + }else{ if(gcode->add_nl) diff --git a/src/modules/communication/SerialConsole.cpp b/src/modules/communication/SerialConsole.cpp index 07171d5d..a9e1c7d7 100644 --- a/src/modules/communication/SerialConsole.cpp +++ b/src/modules/communication/SerialConsole.cpp @@ -20,8 +20,8 @@ using std::string; // Serial reading module // Treats every received line as a command and passes it ( via event call ) to the command dispatcher. // The command dispatcher will then ask other modules if they can do something with it -SerialConsole::SerialConsole( PinName tx_pin, PinName rx_pin, int baud_rate ){ - this->serial = new mbed::Serial( tx_pin, rx_pin ); +SerialConsole::SerialConsole( PinName rx_pin, PinName tx_pin, int baud_rate ){ + this->serial = new mbed::Serial( rx_pin, tx_pin ); this->serial->baud(baud_rate); } diff --git a/src/modules/communication/SerialConsole.h b/src/modules/communication/SerialConsole.h index 831099b2..49489db5 100644 --- a/src/modules/communication/SerialConsole.h +++ b/src/modules/communication/SerialConsole.h @@ -22,7 +22,7 @@ using std::string; class SerialConsole : public Module, public StreamOutput { public: - SerialConsole( PinName tx_pin, PinName rx_pin, int baud_rate ); + SerialConsole( PinName rx_pin, PinName tx_pin, int baud_rate ); void on_module_loaded(); void on_serial_char_received(); diff --git a/src/modules/communication/utils/Gcode.cpp b/src/modules/communication/utils/Gcode.cpp old mode 100644 new mode 100755 diff --git a/src/modules/communication/utils/Gcode.h b/src/modules/communication/utils/Gcode.h old mode 100644 new mode 100755 diff --git a/src/modules/robot/ActuatorCoordinates.h b/src/modules/robot/ActuatorCoordinates.h old mode 100644 new mode 100755 index a725b80f..737f71aa --- a/src/modules/robot/ActuatorCoordinates.h +++ b/src/modules/robot/ActuatorCoordinates.h @@ -5,17 +5,20 @@ You should have received a copy of the GNU General Public License along with Smoothie. If not, see . */ -#ifndef ACTUATOR_COORDINATES_H -#define ACTUATOR_COORDINATES_H +#pragma once + #include #ifndef MAX_ROBOT_ACTUATORS -#define MAX_ROBOT_ACTUATORS 4 + #ifdef CNC + #define MAX_ROBOT_ACTUATORS 3 + #else + // includes 2 extruders + #define MAX_ROBOT_ACTUATORS 5 + #endif #endif //The subset in use is determined by the arm solution's get_actuator_count(). //Keep MAX_ROBOT_ACTUATORS as small as practical it impacts block size and therefore free memory. const size_t k_max_actuators = MAX_ROBOT_ACTUATORS; typedef struct std::array ActuatorCoordinates; - -#endif diff --git a/src/modules/robot/Block.cpp b/src/modules/robot/Block.cpp old mode 100644 new mode 100755 diff --git a/src/modules/robot/Block.h b/src/modules/robot/Block.h index da5c1996..3df796ce 100644 --- a/src/modules/robot/Block.h +++ b/src/modules/robot/Block.h @@ -5,8 +5,7 @@ You should have received a copy of the GNU General Public License along with Smoothie. If not, see . */ -#ifndef BLOCK_H -#define BLOCK_H +#pragma once #include #include @@ -78,6 +77,3 @@ class Block { uint16_t s_value:12; // for laser 1.11 Fixed point }; }; - - -#endif diff --git a/src/modules/robot/Conveyor.h b/src/modules/robot/Conveyor.h index 9ea8e40a..ba2b09a7 100644 --- a/src/modules/robot/Conveyor.h +++ b/src/modules/robot/Conveyor.h @@ -5,8 +5,7 @@ You should have received a copy of the GNU General Public License along with Smoothie. If not, see . */ -#ifndef CONVEYOR_H -#define CONVEYOR_H +#pragma once #include "libs/Module.h" #include "HeapRing.h" @@ -64,6 +63,3 @@ class Conveyor : public Module }; }; - - -#endif // CONVEYOR_H diff --git a/src/modules/robot/Planner.cpp b/src/modules/robot/Planner.cpp index 09ef0a83..1432e44a 100644 --- a/src/modules/robot/Planner.cpp +++ b/src/modules/robot/Planner.cpp @@ -53,8 +53,7 @@ void Planner::config_load() bool Planner::append_block( ActuatorCoordinates &actuator_pos, uint8_t n_motors, float rate_mm_s, float distance, float *unit_vec, float acceleration, float s_value, bool g123) { // Create ( recycle ) a new block - //Block* block = THECONVEYOR->queue.head_ref(); - Block* block = THEKERNEL->conveyor->queue.head_ref(); + Block* block = THECONVEYOR->queue.head_ref(); // Direction bits bool has_steps = false; diff --git a/src/modules/robot/Robot.h b/src/modules/robot/Robot.h old mode 100644 new mode 100755 diff --git a/src/modules/robot/arm_solutions/BaseSolution.h b/src/modules/robot/arm_solutions/BaseSolution.h old mode 100644 new mode 100755 diff --git a/src/modules/robot/arm_solutions/CartesianSolution.cpp b/src/modules/robot/arm_solutions/CartesianSolution.cpp old mode 100644 new mode 100755 diff --git a/src/modules/robot/arm_solutions/CartesianSolution.h b/src/modules/robot/arm_solutions/CartesianSolution.h old mode 100644 new mode 100755 diff --git a/src/modules/robot/arm_solutions/CoreXZSolution.cpp b/src/modules/robot/arm_solutions/CoreXZSolution.cpp old mode 100644 new mode 100755 diff --git a/src/modules/robot/arm_solutions/CoreXZSolution.h b/src/modules/robot/arm_solutions/CoreXZSolution.h old mode 100644 new mode 100755 diff --git a/src/modules/robot/arm_solutions/ExperimentalDeltaSolution.cpp b/src/modules/robot/arm_solutions/ExperimentalDeltaSolution.cpp old mode 100644 new mode 100755 diff --git a/src/modules/robot/arm_solutions/ExperimentalDeltaSolution.h b/src/modules/robot/arm_solutions/ExperimentalDeltaSolution.h old mode 100644 new mode 100755 diff --git a/src/modules/robot/arm_solutions/HBotSolution.cpp b/src/modules/robot/arm_solutions/HBotSolution.cpp old mode 100644 new mode 100755 diff --git a/src/modules/robot/arm_solutions/HBotSolution.h b/src/modules/robot/arm_solutions/HBotSolution.h old mode 100644 new mode 100755 diff --git a/src/modules/robot/arm_solutions/LinearDeltaSolution.cpp b/src/modules/robot/arm_solutions/LinearDeltaSolution.cpp old mode 100644 new mode 100755 diff --git a/src/modules/robot/arm_solutions/LinearDeltaSolution.h b/src/modules/robot/arm_solutions/LinearDeltaSolution.h old mode 100644 new mode 100755 diff --git a/src/modules/robot/arm_solutions/MorganSCARASolution.cpp b/src/modules/robot/arm_solutions/MorganSCARASolution.cpp old mode 100644 new mode 100755 diff --git a/src/modules/robot/arm_solutions/MorganSCARASolution.h b/src/modules/robot/arm_solutions/MorganSCARASolution.h old mode 100644 new mode 100755 diff --git a/src/modules/robot/arm_solutions/RotaryDeltaSolution.cpp b/src/modules/robot/arm_solutions/RotaryDeltaSolution.cpp old mode 100644 new mode 100755 index fe9aeed9..5dd38a6e --- a/src/modules/robot/arm_solutions/RotaryDeltaSolution.cpp +++ b/src/modules/robot/arm_solutions/RotaryDeltaSolution.cpp @@ -17,7 +17,9 @@ #define delta_z_offset_checksum CHECKSUM("delta_z_offset") #define delta_ee_offs_checksum CHECKSUM("delta_ee_offs") -#define tool_offset_checksum CHECKSUM("tool_offset") +#define tool_offset_checksum CHECKSUM("delta_tool_offset") + +#define delta_mirror_xy_checksum CHECKSUM("delta_mirror_xy") const static float pi = 3.14159265358979323846; // PI const static float two_pi = 2 * pi; @@ -51,6 +53,10 @@ RotaryDeltaSolution::RotaryDeltaSolution(Config *config) // Distance between end effector ball joint plane and tip of tool (PnP) tool_offset = config->value(tool_offset_checksum)->by_default(30.500F)->as_number(); + // mirror the XY axis + mirror_xy= config->value(delta_mirror_xy_checksum)->by_default(true)->as_bool(); + + debug_flag= false; init(); } @@ -130,7 +136,6 @@ int RotaryDeltaSolution::delta_calcForward(float theta1, float theta2, float the void RotaryDeltaSolution::init() { - //these are calculated here and not in the config() as these variables can be fine tuned by the user. z_calc_offset = -(delta_z_offset - tool_offset - delta_ee_offs); } @@ -142,10 +147,16 @@ void RotaryDeltaSolution::cartesian_to_actuator(const float cartesian_mm[], Actu float beta_theta = 0.0F; float gamma_theta = 0.0F; - //Code from Trossen Robotics tutorial, note we put the X axis at the back and not the front of the robot. + //Code from Trossen Robotics tutorial, has X in front Y to the right and Z to the left + // firepick is X at the back and negates X0 X0 + // selected by a config option + float x0 = cartesian_mm[X_AXIS]; + float y0 = cartesian_mm[Y_AXIS]; + if(mirror_xy) { + x0= -x0; + y0= -y0; + } - float x0 = -cartesian_mm[X_AXIS]; - float y0 = -cartesian_mm[Y_AXIS]; float z_with_offset = cartesian_mm[Z_AXIS] + z_calc_offset; //The delta calculation below places zero at the top. Subtract the Z offset to make zero at the bottom. int status = delta_calcAngleYZ(x0, y0, z_with_offset, alpha_theta); @@ -154,11 +165,14 @@ void RotaryDeltaSolution::cartesian_to_actuator(const float cartesian_mm[], Actu if (status == -1) { //something went wrong, //force to actuator FPD home position as we know this is a valid position - actuator_mm[ALPHA_STEPPER] = 0; - actuator_mm[BETA_STEPPER ] = 0; - actuator_mm[GAMMA_STEPPER] = 0; - - //DEBUG CODE, uncomment the following to help determine what may be happening if you are trying to adapt this to your own different roational delta. + //TODO REwork this as forcing home is bad if the mechanics are not calibrate; we could crash into something + //ideally we should return a bad return-code so the calling logic knows it has asked this arm solution + //to do something it cannot achieve - Douglas Pearless 2016-11-27 +// actuator_mm[ALPHA_STEPPER] = 0; +// actuator_mm[BETA_STEPPER ] = 0; +// actuator_mm[GAMMA_STEPPER] = 0; + + //DEBUG CODE, uncomment the following to help determine what may be happening if you are trying to adapt this to your own different rotary delta. if(debug_flag) { THEKERNEL->streams->printf("//ERROR: Delta calculation fail! Unable to move to:\n"); THEKERNEL->streams->printf("// x= %f\n", cartesian_mm[X_AXIS]); @@ -187,8 +201,18 @@ void RotaryDeltaSolution::cartesian_to_actuator(const float cartesian_mm[], Actu void RotaryDeltaSolution::actuator_to_cartesian(const ActuatorCoordinates &actuator_mm, float cartesian_mm[] ) { + float x, y, z; //Use forward kinematics - delta_calcForward(actuator_mm[ALPHA_STEPPER], actuator_mm[BETA_STEPPER ], actuator_mm[GAMMA_STEPPER], cartesian_mm[X_AXIS], cartesian_mm[Y_AXIS], cartesian_mm[Z_AXIS]); + delta_calcForward(actuator_mm[ALPHA_STEPPER], actuator_mm[BETA_STEPPER ], actuator_mm[GAMMA_STEPPER], x, y, z); + if(mirror_xy) { + cartesian_mm[X_AXIS]= -x; + cartesian_mm[Y_AXIS]= -y; + cartesian_mm[Z_AXIS]= z; + }else{ + cartesian_mm[X_AXIS]= x; + cartesian_mm[Y_AXIS]= y; + cartesian_mm[Z_AXIS]= z; + } } bool RotaryDeltaSolution::set_optional(const arm_options_t &options) diff --git a/src/modules/robot/arm_solutions/RotaryDeltaSolution.h b/src/modules/robot/arm_solutions/RotaryDeltaSolution.h old mode 100644 new mode 100755 index dd475f7a..1f226c17 --- a/src/modules/robot/arm_solutions/RotaryDeltaSolution.h +++ b/src/modules/robot/arm_solutions/RotaryDeltaSolution.h @@ -1,5 +1,5 @@ -#ifndef RotaryDeltaSolution_H -#define RotaryDeltaSolution_H +#pragma once + #include "libs/Module.h" #include "BaseSolution.h" @@ -30,6 +30,8 @@ class RotaryDeltaSolution : public BaseSolution { float tool_offset; // Distance between end effector ball joint plane and tip of tool float z_calc_offset; - bool debug_flag{false}; + struct { + bool debug_flag:1; + bool mirror_xy:1; + }; }; -#endif // RotaryDeltaSolution_H diff --git a/src/modules/robot/arm_solutions/RotatableCartesianSolution.cpp b/src/modules/robot/arm_solutions/RotatableCartesianSolution.cpp old mode 100644 new mode 100755 diff --git a/src/modules/robot/arm_solutions/RotatableCartesianSolution.h b/src/modules/robot/arm_solutions/RotatableCartesianSolution.h old mode 100644 new mode 100755 diff --git a/src/modules/tools/drillingcycles/Drillingcycles.cpp b/src/modules/tools/drillingcycles/Drillingcycles.cpp old mode 100644 new mode 100755 diff --git a/src/modules/tools/drillingcycles/Drillingcycles.h b/src/modules/tools/drillingcycles/Drillingcycles.h old mode 100644 new mode 100755 diff --git a/src/modules/tools/endstops/Endstops.h b/src/modules/tools/endstops/Endstops.h index 2089b983..387781be 100644 --- a/src/modules/tools/endstops/Endstops.h +++ b/src/modules/tools/endstops/Endstops.h @@ -5,8 +5,7 @@ You should have received a copy of the GNU General Public License along with Smoothie. If not, see . */ -#ifndef ENDSTOPS_MODULE_H -#define ENDSTOPS_MODULE_H +#pragma once #include "libs/Module.h" #include "libs/Pin.h" @@ -67,6 +66,3 @@ class Endstops : public Module{ bool move_to_origin_after_home:1; }; }; - - -#endif diff --git a/src/modules/tools/filamentdetector/FilamentDetector.cpp b/src/modules/tools/filamentdetector/FilamentDetector.cpp new file mode 100644 index 00000000..217ccf26 --- /dev/null +++ b/src/modules/tools/filamentdetector/FilamentDetector.cpp @@ -0,0 +1,247 @@ + +/* + Handles a filament detector that has an optical encoder wheel, that generates pulses as the filament + moves through it. + It also supports a "bulge" detector that triggers if the filament has a bulge in it +*/ + +#include "FilamentDetector.h" +#include "Kernel.h" +#include "Config.h" +#include "checksumm.h" +#include "ConfigValue.h" +#include "SlowTicker.h" +#include "PublicData.h" +#include "StreamOutputPool.h" +#include "StreamOutput.h" +#include "SerialMessage.h" +#include "FilamentDetector.h" +#include "utils.h" +#include "Gcode.h" +#include "ExtruderPublicAccess.h" + +#include "InterruptIn.h" // mbed +#include "us_ticker_api.h" // mbed + +#define extruder_checksum CHECKSUM("extruder") + +#define filament_detector_checksum CHECKSUM("filament_detector") +#define enable_checksum CHECKSUM("enable") +#define encoder_pin_checksum CHECKSUM("encoder_pin") +#define bulge_pin_checksum CHECKSUM("bulge_pin") +#define seconds_per_check_checksum CHECKSUM("seconds_per_check") +#define pulses_per_mm_checksum CHECKSUM("pulses_per_mm") + +FilamentDetector::FilamentDetector() +{ + suspended= false; + filament_out_alarm= false; + bulge_detected= false; + active= true; + e_last_moved= NAN; +} + +FilamentDetector::~FilamentDetector() +{ + if(encoder_pin != nullptr) delete encoder_pin; +} + +void FilamentDetector::on_module_loaded() +{ + // if the module is disabled -> do nothing + if(!THEKERNEL->config->value( filament_detector_checksum, enable_checksum )->by_default(false)->as_bool()) { + // as this module is not needed free up the resource + delete this; + return; + } + + // encoder pin has to be interrupt enabled pin like 0.26, 0.27, 0.28 + Pin dummy_pin; + dummy_pin.from_string( THEKERNEL->config->value(filament_detector_checksum, encoder_pin_checksum)->by_default("nc" )->as_string()); + this->encoder_pin= dummy_pin.interrupt_pin(); + + // optional bulge detector + bulge_pin.from_string( THEKERNEL->config->value(filament_detector_checksum, bulge_pin_checksum)->by_default("nc" )->as_string())->as_input(); + if(bulge_pin.connected()) { + // input pin polling + THEKERNEL->slow_ticker->attach( 100, this, &FilamentDetector::button_tick); + } + + //Valid configurations contain an encoder pin, a bulge pin or both. + //free the module if not a valid configuration + if(this->encoder_pin == nullptr && !bulge_pin.connected()) { + delete this; + return; + } + + //only monitor the encoder if we are using the encoder. + if (this->encoder_pin != nullptr) { + // set interrupt on rising edge + this->encoder_pin->rise(this, &FilamentDetector::on_pin_rise); + //NVIC_SetPriority(EINT3_IRQn, 16); // set to low priority +//TODO We need to fix the interrupts, something like the following NVIC_SetPriority(PORTA_IRQn, 0x1); + } + + + // how many seconds between checks, must be long enough for several pulses to be detected, but not too long + seconds_per_check= THEKERNEL->config->value(filament_detector_checksum, seconds_per_check_checksum)->by_default(2)->as_number(); + + // the number of pulses per mm of filament moving through the detector, can be fractional + pulses_per_mm= THEKERNEL->config->value(filament_detector_checksum, pulses_per_mm_checksum)->by_default(1)->as_number(); + + // register event-handlers + if (this->encoder_pin != nullptr) { + //This event is only valid if we are using the encodeer. + register_for_event(ON_SECOND_TICK); + } + + register_for_event(ON_MAIN_LOOP); + register_for_event(ON_CONSOLE_LINE_RECEIVED); + this->register_for_event(ON_GCODE_RECEIVED); +} + + +void FilamentDetector::send_command(std::string msg, StreamOutput *stream) +{ + struct SerialMessage message; + message.message = msg; + message.stream = stream; + THEKERNEL->call_event(ON_CONSOLE_LINE_RECEIVED, &message ); +} + +// needed to detect when we resume +void FilamentDetector::on_console_line_received( void *argument ) +{ + if(!suspended) return; + + SerialMessage new_message = *static_cast(argument); + string possible_command = new_message.message; + string cmd = shift_parameter(possible_command); + if(cmd == "resume" || cmd == "M601") { + this->pulses= 0; + e_last_moved= NAN; + suspended= false; + } +} + +float FilamentDetector::get_emove() +{ + pad_extruder_t rd; + if(PublicData::get_value( extruder_checksum, (void *)&rd )) return rd.current_position; + return NAN; +} + +void FilamentDetector::on_gcode_received(void *argument) +{ + Gcode *gcode = static_cast(argument); + if (gcode->has_m) { + if (gcode->m == 404) { // set filament detector parameters S seconds per check, P pulses per mm + if(gcode->has_letter('S')){ + seconds_per_check= gcode->get_value('S'); + seconds_passed= 0; + } + if(gcode->has_letter('P')){ + pulses_per_mm= gcode->get_value('P'); + } + gcode->stream->printf("// pulses per mm: %f, seconds per check: %d\n", pulses_per_mm, seconds_per_check); + + } else if (gcode->m == 405) { // disable filament detector + active= false; + e_last_moved= get_emove(); + + }else if (gcode->m == 406) { // enable filament detector + this->pulses= 0; + e_last_moved= get_emove(); + active= true; + + }else if (gcode->m == 407) { // display filament detector pulses and status + float e_moved= get_emove(); + if(!isnan(e_moved)) { + float delta= e_moved - e_last_moved; + gcode->stream->printf("Extruder moved: %f mm\n", delta); + } + + gcode->stream->printf("Encoder pulses: %u\n", pulses.load()); + if(this->suspended) gcode->stream->printf("Filament detector triggered\n"); + gcode->stream->printf("Filament detector is %s\n", active?"enabled":"disabled"); + } + } +} + +void FilamentDetector::on_main_loop(void *argument) +{ + if (active && this->filament_out_alarm) { + this->filament_out_alarm = false; + if(bulge_detected){ + THEKERNEL->streams->printf("// Filament Detector has detected a bulge in the filament\n"); + bulge_detected= false; + }else{ + THEKERNEL->streams->printf("// Filament Detector has detected a filament jam\n"); + } + + if(!suspended) { + this->suspended= true; + // fire suspend command + this->send_command( "M600", &(StreamOutput::NullStream) ); + } + } +} + +void FilamentDetector::on_second_tick(void *argument) +{ + if(++seconds_passed >= seconds_per_check) { + seconds_passed= 0; + check_encoder(); + } +} + +// encoder pin interrupt +void FilamentDetector::on_pin_rise() +{ + this->pulses++; +} + +void FilamentDetector::check_encoder() +{ + if(suspended) return; // already suspended + if(!active) return; // not enabled + + uint32_t pulse_cnt= this->pulses.exchange(0); // atomic load and reset + + // get number of E steps taken and make sure we have seen enough pulses to cover that + float e_moved= get_emove(); + if(isnan(e_last_moved)) { + e_last_moved= e_moved; + return; + } + + float delta= e_moved - e_last_moved; + e_last_moved= e_moved; + if(delta < 0) { + // we ignore retracts for the purposes of jam detection + return; + } + + // figure out how many pulses need to have happened to cover that e move + uint32_t needed_pulses= floorf(delta*pulses_per_mm); + // NOTE if needed_pulses is 0 then extruder did not move since last check, or not enough to register + if(needed_pulses == 0) return; + + if(pulse_cnt == 0) { + // we got no pulses and E moved since last time so fire off alarm + this->filament_out_alarm= true; + } +} + +uint32_t FilamentDetector::button_tick(uint32_t dummy) +{ + if(!bulge_pin.connected() || suspended || !active) return 0; + + if(bulge_pin.get()) { + // we got a trigger from the bulge detector + this->filament_out_alarm= true; + this->bulge_detected= true; + } + + return 0; +} diff --git a/src/modules/tools/filamentdetector/FilamentDetector.h b/src/modules/tools/filamentdetector/FilamentDetector.h new file mode 100644 index 00000000..c5bdaa83 --- /dev/null +++ b/src/modules/tools/filamentdetector/FilamentDetector.h @@ -0,0 +1,48 @@ +#pragma once + +#include "Module.h" +#include "Pin.h" + +#include +#include +#include + +namespace mbed { + class InterruptIn; +} + +class StreamOutput; + +class FilamentDetector: public Module +{ +public: + FilamentDetector(); + ~FilamentDetector(); + void on_module_loaded(); + void on_main_loop(void* argument); + void on_second_tick(void* argument); + void on_console_line_received( void *argument ); + void on_gcode_received(void *argument); + +private: + void on_pin_rise(); + void check_encoder(); + void send_command(std::string msg, StreamOutput *stream); + uint32_t button_tick(uint32_t dummy); + float get_emove(); + + mbed::InterruptIn *encoder_pin{0}; + Pin bulge_pin; + float e_last_moved{0}; + std::atomic_uint pulses{0}; + float pulses_per_mm{0}; + uint8_t seconds_per_check{1}; + uint8_t seconds_passed{0}; + + struct { + bool filament_out_alarm:1; + bool bulge_detected:1; + bool suspended:1; + bool active:1; + }; +}; diff --git a/src/modules/tools/rotarydeltacalibration/RotaryDeltaCalibration.cpp b/src/modules/tools/rotarydeltacalibration/RotaryDeltaCalibration.cpp new file mode 100644 index 00000000..738fcf62 --- /dev/null +++ b/src/modules/tools/rotarydeltacalibration/RotaryDeltaCalibration.cpp @@ -0,0 +1,123 @@ +#include "RotaryDeltaCalibration.h" +#include "EndstopsPublicAccess.h" +#include "Kernel.h" +#include "Robot.h" +#include "Config.h" +#include "checksumm.h" +#include "ConfigValue.h" +#include "StreamOutput.h" +#include "PublicDataRequest.h" +#include "PublicData.h" +#include "Gcode.h" +#include "StepperMotor.h" + +#define rotarydelta_checksum CHECKSUM("rotary_delta_calibration") +#define enable_checksum CHECKSUM("enable") + +void RotaryDeltaCalibration::on_module_loaded() +{ + // if the module is disabled -> do nothing + if(!THEKERNEL->config->value( rotarydelta_checksum, enable_checksum )->by_default(false)->as_bool()) { + // as this module is not needed free up the resource + delete this; + return; + } + + // register event-handlers + register_for_event(ON_GCODE_RECEIVED); +} + +float *RotaryDeltaCalibration::get_homing_offset() +{ + float *theta_offset; // points to theta offset in Endstop module + bool ok = PublicData::get_value( endstops_checksum, home_offset_checksum, &theta_offset ); + return ok ? theta_offset : nullptr; +} + +void RotaryDeltaCalibration::on_gcode_received(void *argument) +{ + Gcode *gcode = static_cast(argument); + + if( gcode->has_m) { + switch( gcode->m ) { + case 206: { + float *theta_offset= get_homing_offset(); // points to theta offset in Endstop module + if (theta_offset == nullptr) { + gcode->stream->printf("error:no endstop module found\n"); + return; + } + + // set theta offset, set directly in the Endstop module (bad practice really) + if (gcode->has_letter('A')) theta_offset[0] = gcode->get_value('A'); + if (gcode->has_letter('B')) theta_offset[1] = gcode->get_value('B'); + if (gcode->has_letter('C')) theta_offset[2] = gcode->get_value('C'); + + gcode->stream->printf("Theta offset set: A %8.5f B %8.5f C %8.5f\n", theta_offset[0], theta_offset[1], theta_offset[2]); + + break; + } + + case 306: { + // for a rotary delta M306 calibrates the homing angle + // by doing M306 A-56.17 it will calculate the M206 A value (the theta offset for actuator A) based on the difference + // between what it thinks is the current angle and what the current angle actually is specified by A (ditto for B and C) + + ActuatorCoordinates current_angle; + // get the current angle for each actuator, NOTE we only deal with ABC so if there are more than 3 actuators this will probably go wonky + for (size_t i = 0; i < 3; i++) { + current_angle[i]= THEROBOT->actuators[i]->get_current_position(); + } + + if (gcode->has_letter('L') && gcode->get_value('L') != 0) { + // specifying L1 it will take the last probe position (set by G30 or G38.x ) and set the home offset based on that + // this allows the use of G30 to find the angle tool + uint8_t ok; + std::tie(current_angle[0], current_angle[1], current_angle[2], ok) = THEROBOT->get_last_probe_position(); + if(ok == 0) { + gcode->stream->printf("error:Nothing set as probe failed or not run\n"); + return; + } + } + + float *theta_offset= get_homing_offset(); // points to theta offset in Endstop module + if (theta_offset == nullptr) { + gcode->stream->printf("error:no endstop module found\n"); + return; + } + + int cnt= 0; + + //figure out what home_offset needs to be to correct the homing_position + if (gcode->has_letter('A')) { + float a = gcode->get_value('A'); // what actual angle is + theta_offset[0] -= (current_angle[0] - a); + current_angle[0]= a; + cnt++; + } + if (gcode->has_letter('B')) { + float b = gcode->get_value('B'); + theta_offset[1] -= (current_angle[1] - b); + current_angle[1]= b; + cnt++; + } + if (gcode->has_letter('C')) { + float c = gcode->get_value('C'); + theta_offset[2] -= (current_angle[2] - c); + current_angle[2]= c; + cnt++; + } + + // reset the actuator positions (and machine position accordingly) + // But only if all three actuators have been specified at the same time + if(cnt == 3 || (gcode->has_letter('R') && gcode->get_value('R') != 0)) { + THEROBOT->reset_actuator_position(current_angle); + gcode->stream->printf("NOTE: actuator position reset\n"); + }else{ + gcode->stream->printf("NOTE: actuator position NOT reset\n"); + } + + gcode->stream->printf("Theta Offset: A %8.5f B %8.5f C %8.5f\n", theta_offset[0], theta_offset[1], theta_offset[2]); + } + } + } +} diff --git a/src/modules/tools/rotarydeltacalibration/RotaryDeltaCalibration.h b/src/modules/tools/rotarydeltacalibration/RotaryDeltaCalibration.h new file mode 100644 index 00000000..1b4af195 --- /dev/null +++ b/src/modules/tools/rotarydeltacalibration/RotaryDeltaCalibration.h @@ -0,0 +1,18 @@ +#pragma once + +#include "Module.h" + +class Gcode; + +class RotaryDeltaCalibration : public Module +{ +public: + RotaryDeltaCalibration(){}; + virtual ~RotaryDeltaCalibration(){}; + + void on_module_loaded(); + +private: + void on_gcode_received(void *argument); + float *get_homing_offset(); +}; diff --git a/src/modules/tools/scaracal/SCARAcal.cpp b/src/modules/tools/scaracal/SCARAcal.cpp new file mode 100644 index 00000000..8b536adf --- /dev/null +++ b/src/modules/tools/scaracal/SCARAcal.cpp @@ -0,0 +1,291 @@ +/* + This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl). + Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. + Smoothie is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. + You should have received a copy of the GNU General Public License along with Smoothie. If not, see . +*/ + +#include "SCARAcal.h" + +#include "Kernel.h" +#include "BaseSolution.h" +#include "Config.h" +#include "Robot.h" +#include "StepperMotor.h" +#include "StreamOutputPool.h" +#include "Gcode.h" +#include "Conveyor.h" +#include "checksumm.h" +#include "ConfigValue.h" +#include "SerialMessage.h" +#include "EndstopsPublicAccess.h" +#include "PublicData.h" +#include +#include + +#define scaracal_checksum CHECKSUM("scaracal") +#define enable_checksum CHECKSUM("enable") +#define slow_feedrate_checksum CHECKSUM("slow_feedrate") +#define z_move_checksum CHECKSUM("z_move") + +#define X_AXIS 0 +#define Y_AXIS 1 +#define Z_AXIS 2 + +#define STEPPER THEROBOT->actuators +#define STEPS_PER_MM(a) (STEPPER[a]->get_steps_per_mm()) + +void SCARAcal::on_module_loaded() +{ + // if the module is disabled -> do nothing + if(!THEKERNEL->config->value( scaracal_checksum, enable_checksum )->by_default(false)->as_bool()) { + // as this module is not needed free up the resource + delete this; + return; + } + + // load settings + this->on_config_reload(this); + // register event-handlers + register_for_event(ON_GCODE_RECEIVED); +} + +void SCARAcal::on_config_reload(void *argument) +{ + this->slow_rate = THEKERNEL->config->value( scaracal_checksum, slow_feedrate_checksum )->by_default(5)->as_number(); // feedrate in mm/sec + this->z_move = THEKERNEL->config->value( scaracal_checksum, z_move_checksum )->by_default(0)->as_number(); // Optional movement of Z relative to the home position. + // positive values increase distance between nozzle and bed. + // negative will decrease. Useful when level code active to prevent collision + +} + + +// issue home command +void SCARAcal::home() +{ + Gcode gc("G28", &(StreamOutput::NullStream)); + THEKERNEL->call_event(ON_GCODE_RECEIVED, &gc); +} + +bool SCARAcal::get_trim(float& x, float& y, float& z) +{ + void *returned_data; + bool ok = PublicData::get_value( endstops_checksum, trim_checksum, &returned_data ); + + if (ok) { + float *trim = static_cast(returned_data); + x= trim[0]; + y= trim[1]; + z= trim[2]; + return true; + } + return false; +} + +bool SCARAcal::set_trim(float x, float y, float z, StreamOutput *stream) +{ + float t[3]{x, y, z}; + bool ok= PublicData::set_value( endstops_checksum, trim_checksum, t); + + if (ok) { + stream->printf("set trim to X:%f Y:%f Z:%f\n", x, y, z); + } else { + stream->printf("unable to set trim, is endstops enabled?\n"); + } + + return ok; +} + +bool SCARAcal::get_home_offset(float& x, float& y, float& z) +{ + void *returned_data; + bool ok = PublicData::get_value( endstops_checksum, home_offset_checksum, &returned_data ); + + if (ok) { + float *home_offset = static_cast(returned_data); + x= home_offset[0]; + y= home_offset[1]; + z= home_offset[2]; + return true; + } + return false; +} + +bool SCARAcal::set_home_offset(float x, float y, float z, StreamOutput *stream) +{ + char cmd[64]; + + // Assemble Gcode to add onto the queue + snprintf(cmd, sizeof(cmd), "M206 X%1.3f Y%1.3f Z%1.3f", x, y, z); // Send saved Z homing offset + + Gcode gc(cmd, &(StreamOutput::NullStream)); + THEKERNEL->call_event(ON_GCODE_RECEIVED, &gc); + + stream->printf("Set home_offset to X:%f Y:%f Z:%f\n", x, y, z); + + return true;//ok; +} + +bool SCARAcal::translate_trim(StreamOutput *stream) +{ + float S_trim[3], + home_off[3]; + ActuatorCoordinates actuator; + + this->get_home_offset(home_off[0], home_off[1], home_off[2]); // get home offsets + this->get_trim(S_trim[0], S_trim[1], S_trim[2]); // get current trim + + THEROBOT->arm_solution->cartesian_to_actuator( home_off, actuator ); // convert current home offset to actuator angles + + // Subtract trim values from actuators to determine the real home offset actuator position for X and Y. + + actuator[0] -= S_trim[0]; + actuator[1] -= S_trim[1]; + + // Clear X and Y trims internally + S_trim[0] = 0.0F; + S_trim[1] = 0.0F; + + // convert back to get the real cartesian home offsets + + THEROBOT->arm_solution->actuator_to_cartesian( actuator, home_off ); + + this->set_home_offset(home_off[0], home_off[1], home_off[2],stream); // get home offsets + // Set the correct home offsets; + + this->set_trim(S_trim[0], S_trim[1], S_trim[2], stream); // Now Clear relevant trims + + return true; +} + +void SCARAcal::SCARA_ang_move(float theta, float psi, float z, float feedrate) +{ + char cmd[64]; + float cartesian[3]; + ActuatorCoordinates actuator; + + // Assign the actuator angles from input + actuator[0] = theta; + actuator[1] = psi; + actuator[2] = z; + + // Calculate the physical position relating to the arm angles + THEROBOT->arm_solution->actuator_to_cartesian( actuator, cartesian ); + + // Assemble Gcode to add onto the queue + snprintf(cmd, sizeof(cmd), "G0 X%1.3f Y%1.3f Z%1.3f F%1.1f", cartesian[0], cartesian[1], cartesian[2], feedrate * 60); // use specified feedrate (mm/sec) + + //THEKERNEL->streams->printf("DEBUG: move: %s\n", cmd); + + Gcode gc(cmd, &(StreamOutput::NullStream)); + THEROBOT->on_gcode_received(&gc); // send to robot directly +} + +//A GCode has been received +//See if the current Gcode line has some orders for us +void SCARAcal::on_gcode_received(void *argument) +{ + Gcode *gcode = static_cast(argument); + + if( gcode->has_m) { + switch( gcode->m ) { + + case 114: { // Extra stuff for Morgan calibration + char buf[32]; + float cartesian[3]; + ActuatorCoordinates actuators; + + THEROBOT->get_axis_position(cartesian); // get actual position from robot + THEROBOT->arm_solution->cartesian_to_actuator( cartesian, actuators ); // translate to get actuator position + + int n = snprintf(buf, sizeof(buf), " A: Th:%1.3f Ps:%1.3f", + actuators[0], + actuators[1]); // display actuator angles Theta and Psi. + gcode->txt_after_ok.append(buf, n); + } + break; + + case 360: { + float target[2] = {0.0F, 120.0F}, + cartesian[3], + S_trim[3]; + + this->get_trim(S_trim[0], S_trim[1], S_trim[2]); // get current trim to conserve other calbration values + + if(gcode->has_letter('P')) { + // Program the current position as target + ActuatorCoordinates actuators; + float S_delta[2], + S_trim[3]; + + THEROBOT->get_axis_position(cartesian); // get actual position from robot + THEROBOT->arm_solution->cartesian_to_actuator( cartesian, actuators ); // translate to get actuator position + + S_delta[0] = actuators[0] - target[0]; + + set_trim(S_delta[0], S_trim[1], S_trim[2], gcode->stream); + } else { + set_trim(0, S_trim[1], S_trim[2], gcode->stream); // reset trim for calibration move + this->home(); // home + THEROBOT->get_axis_position(cartesian); // get actual position from robot + SCARA_ang_move(target[0], target[1], cartesian[2] + this->z_move, slow_rate * 3.0F); // move to target + } + } + break; + + case 361: { + float target[2] = {90.0F, 130.0F}, + cartesian[3]; + + if(gcode->has_letter('P')) { + // Program the current position as target + ActuatorCoordinates actuators; + + THEROBOT->get_axis_position(cartesian); // get actual position from robot + THEROBOT->arm_solution->cartesian_to_actuator( cartesian, actuators ); // translate to get actuator position + + STEPPER[0]->change_steps_per_mm(actuators[0] / target[0] * STEPPER[0]->get_steps_per_mm()); // Find angle difference + STEPPER[1]->change_steps_per_mm(STEPPER[0]->get_steps_per_mm()); // and change steps_per_mm to ensure correct steps per *angle* + } else { + this->home(); // home - This time leave trims as adjusted. + THEROBOT->get_axis_position(cartesian); // get actual position from robot + SCARA_ang_move(target[0], target[1], cartesian[2] + this->z_move, slow_rate * 3.0F); // move to target + } + + } + break; + + case 364: { + float target[2] = {45.0F, 135.0F}, + cartesian[3], + S_trim[3]; + + this->get_trim(S_trim[0], S_trim[1], S_trim[2]); // get current trim to conserve other calbration values + + if(gcode->has_letter('P')) { + // Program the current position as target + ActuatorCoordinates actuators; + float S_delta[2]; + + THEROBOT->get_axis_position(cartesian); // get actual position from robot + THEROBOT->arm_solution->cartesian_to_actuator( cartesian, actuators ); // translate it to get actual actuator angles + + S_delta[1] = ( actuators[1] - actuators[0]) - ( target[1] - target[0] ); // Find difference in angle - not actuator difference, and + set_trim(S_trim[0], S_delta[1], S_trim[2], gcode->stream); // set trim to reflect the difference + } else { + set_trim(S_trim[0], 0, S_trim[2], gcode->stream); // reset trim for calibration move + this->home(); // home + THEROBOT->get_axis_position(cartesian); // get actual position from robot + SCARA_ang_move(target[0], target[1], cartesian[2] + this->z_move, slow_rate * 3.0F); // move to target + } + } + break; + + case 366: // Translate trims to the actual endstop offsets for SCARA + this->translate_trim(gcode->stream); + break; + + } + } +} + diff --git a/src/modules/tools/scaracal/SCARAcal.h b/src/modules/tools/scaracal/SCARAcal.h new file mode 100644 index 00000000..50d4fff7 --- /dev/null +++ b/src/modules/tools/scaracal/SCARAcal.h @@ -0,0 +1,48 @@ +/* + This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl). + Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. + Smoothie is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. + You should have received a copy of the GNU General Public License along with Smoothie. If not, see . +*/ + +#ifndef SCARACAL_H_ +#define SCARACAL_H_ + +#include "Module.h" +#include "Pin.h" + +class StepperMotor; +class Gcode; +class StreamOutput; + +class SCARAcal: public Module +{ + +public: + void on_module_loaded(); + void on_config_reload(void *argument); + + void on_gcode_received(void *argument); + + +private: + void home(); + bool set_trim(float x, float y, float z, StreamOutput *stream); + bool get_trim(float& x, float& y, float& z); + + bool set_home_offset(float x, float y, float z, StreamOutput *stream); + bool get_home_offset(float& x, float& y, float& z); + + bool translate_trim(StreamOutput *stream); + + void SCARA_ang_move(float theta, float psi, float z, float feedrate); + + float slow_rate; + float z_move; + + struct { + bool is_scara:1; + }; +}; + +#endif /* SCARACAL_H_ */ diff --git a/src/modules/tools/spindle/Spindle.cpp b/src/modules/tools/spindle/Spindle.cpp new file mode 100644 index 00000000..e483b84a --- /dev/null +++ b/src/modules/tools/spindle/Spindle.cpp @@ -0,0 +1,210 @@ +/* + This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl). + Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. + Smoothie is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. + You should have received a copy of the GNU General Public License along with Smoothie. If not, see . +*/ + +#include "libs/Module.h" +#include "libs/Kernel.h" +#include "Spindle.h" +#include "Config.h" +#include "checksumm.h" +#include "ConfigValue.h" +#include "Gcode.h" +#include "StreamOutputPool.h" +#include "SlowTicker.h" +#include "Conveyor.h" +#include "nuts_bolts.h" + +#include "libs/Pin.h" +#include "mbed.h" +#include "PwmOut.h" +#include "port_api.h" +#include "us_ticker_api.h" + +#define spindle_enable_checksum CHECKSUM("spindle_enable") +#define spindle_pwm_pin_checksum CHECKSUM("spindle_pwm_pin") +#define spindle_pwm_period_checksum CHECKSUM("spindle_pwm_period") +#define spindle_feedback_pin_checksum CHECKSUM("spindle_feedback_pin") +#define spindle_pulses_per_rev_checksum CHECKSUM("spindle_pulses_per_rev") +#define spindle_default_rpm_checksum CHECKSUM("spindle_default_rpm") +#define spindle_control_P_checksum CHECKSUM("spindle_control_P") +#define spindle_control_I_checksum CHECKSUM("spindle_control_I") +#define spindle_control_D_checksum CHECKSUM("spindle_control_D") +#define spindle_control_smoothing_checksum CHECKSUM("spindle_control_smoothing") + +#define UPDATE_FREQ 1000 + +Spindle::Spindle() +{ +} + +void Spindle::on_module_loaded() +{ + last_time = 0; + last_edge = 0; + current_rpm = 0; + current_I_value = 0; + current_pwm_value = 0; + time_since_update = 0; + spindle_on = true; + + if (!THEKERNEL->config->value(spindle_enable_checksum)->by_default(false)->as_bool()) { + delete this; // Spindle control module is disabled + return; + } + + pulses_per_rev = THEKERNEL->config->value(spindle_pulses_per_rev_checksum)->by_default(1.0f)->as_number(); + target_rpm = THEKERNEL->config->value(spindle_default_rpm_checksum)->by_default(5000.0f)->as_number(); + control_P_term = THEKERNEL->config->value(spindle_control_P_checksum)->by_default(0.0001f)->as_number(); + control_I_term = THEKERNEL->config->value(spindle_control_I_checksum)->by_default(0.0001f)->as_number(); + control_D_term = THEKERNEL->config->value(spindle_control_D_checksum)->by_default(0.0001f)->as_number(); + + // Smoothing value is low pass filter time constant in seconds. + float smoothing_time = THEKERNEL->config->value(spindle_control_smoothing_checksum)->by_default(0.1f)->as_number(); + if (smoothing_time * UPDATE_FREQ < 1.0f) + smoothing_decay = 1.0f; + else + smoothing_decay = 1.0f / (UPDATE_FREQ * smoothing_time); + + // Get the pin for hardware pwm + { + Pin *smoothie_pin = new Pin(); + smoothie_pin->from_string(THEKERNEL->config->value(spindle_pwm_pin_checksum)->by_default("nc")->as_string()); + spindle_pin = smoothie_pin->as_output()->hardware_pwm(); + output_inverted = smoothie_pin->is_inverting(); + delete smoothie_pin; + } + + if (spindle_pin == NULL) { + THEKERNEL->streams->printf("Error: Spindle PWM pin must be P2.0-2.5 or other PWM pin\n"); + delete this; + return; + } + + int period = THEKERNEL->config->value(spindle_pwm_period_checksum)->by_default(1000)->as_int(); + spindle_pin->period_us(period); + spindle_pin->write(output_inverted ? 1 : 0); + + // Get the pin for interrupt + { + Pin *smoothie_pin = new Pin(); + smoothie_pin->from_string(THEKERNEL->config->value(spindle_feedback_pin_checksum)->by_default("nc")->as_string()); + smoothie_pin->as_input(); + if (smoothie_pin->port_number == 0 || smoothie_pin->port_number == 2) { + PinName pinname = port_pin((PortName)smoothie_pin->port_number, smoothie_pin->pin_number); + feedback_pin = new InterruptIn(pinname); + //feedback_pin.rise(&on_pin_rise); // TODO need to assign interrupt routine + //NVIC_SetPriority(EINT3_IRQn, 16); //TODO Need to set to low priority + } else { + THEKERNEL->streams->printf("Error: Spindle feedback pin has to be on P0 or P2.\n"); + delete this; + return; + } + delete smoothie_pin; + } + + THEKERNEL->slow_ticker->attach(UPDATE_FREQ, this, &Spindle::on_update_speed); + register_for_event(ON_GCODE_RECEIVED); +} + +void Spindle::on_pin_rise() +{ + uint32_t timestamp = us_ticker_read(); + last_time = timestamp - last_edge; + last_edge = timestamp; + irq_count++; +} + +uint32_t Spindle::on_update_speed(uint32_t dummy) +{ + // If we don't get any interrupts for 1 second, set current RPM to 0 + uint32_t new_irq = irq_count; + if (last_irq != new_irq) + time_since_update = 0; + else + time_since_update++; + last_irq = new_irq; + + if (time_since_update > UPDATE_FREQ) + last_time = 0; + + // Calculate current RPM + uint32_t t = last_time; + if (t == 0) { + current_rpm = 0; + } else { + float new_rpm = 1000000 * 60.0f / (t * pulses_per_rev); + current_rpm = smoothing_decay * new_rpm + (1.0f - smoothing_decay) * current_rpm; + } + + if (spindle_on) { + float error = target_rpm - current_rpm; + + current_I_value += control_I_term * error * 1.0f / UPDATE_FREQ; + current_I_value = confine(current_I_value, -1.0f, 1.0f); + + float new_pwm = 0.5f; + new_pwm += control_P_term * error; + new_pwm += current_I_value; + new_pwm += control_D_term * UPDATE_FREQ * (error - prev_error); + new_pwm = confine(new_pwm, 0.0f, 1.0f); + prev_error = error; + + current_pwm_value = new_pwm; + } else { + current_I_value = 0; + current_pwm_value = 0; + } + + if (output_inverted) + spindle_pin->write(1.0f - current_pwm_value); + else + spindle_pin->write(current_pwm_value); + + return 0; +} + + +void Spindle::on_gcode_received(void* argument) +{ + Gcode *gcode = static_cast(argument); + + if (gcode->has_m) { + if (gcode->m == 957) { + // M957: report spindle speed + THEKERNEL->streams->printf("Current RPM: %5.0f Target RPM: %5.0f PWM value: %5.3f\n", + current_rpm, target_rpm, current_pwm_value); + } else if (gcode->m == 958) { + // M958: set spindle PID parameters + if (gcode->has_letter('P')) + control_P_term = gcode->get_value('P'); + if (gcode->has_letter('I')) + control_I_term = gcode->get_value('I'); + if (gcode->has_letter('D')) + control_D_term = gcode->get_value('D'); + THEKERNEL->streams->printf("P: %0.6f I: %0.6f D: %0.6f\n", + control_P_term, control_I_term, control_D_term); + } else if (gcode->m == 3) { + THEKERNEL->conveyor->wait_for_idle(); + + // M3: Spindle on + spindle_on = true; + + if (gcode->has_letter('S')) { + target_rpm = gcode->get_value('S'); + } + + } else if (gcode->m == 5) { + THEKERNEL->conveyor->wait_for_idle(); + spindle_on = false; + } + + }else if(!gcode->has_m && !gcode->has_g && gcode->has_letter('S')) { + // special case S on its own + THEKERNEL->conveyor->wait_for_idle(); + target_rpm = gcode->get_value('S'); + } +} + diff --git a/src/modules/tools/spindle/Spindle.h b/src/modules/tools/spindle/Spindle.h new file mode 100644 index 00000000..d28b8726 --- /dev/null +++ b/src/modules/tools/spindle/Spindle.h @@ -0,0 +1,60 @@ +/* + This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl). + Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. + Smoothie is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. + You should have received a copy of the GNU General Public License along with Smoothie. If not, see . +*/ + +#ifndef SPINDLE_MODULE_H +#define SPINDLE_MODULE_H + +#include "libs/Module.h" +#include + +namespace mbed { + class PwmOut; + class InterruptIn; +} + +// This module implements closed loop PID control for spindle RPM. +class Spindle: public Module { + public: + Spindle(); + virtual ~Spindle() {}; + void on_module_loaded(); + + + private: + void on_pin_rise(); + void on_gcode_received(void *argument); + uint32_t on_update_speed(uint32_t dummy); + + mbed::PwmOut *spindle_pin; // PWM output for spindle speed control + mbed::InterruptIn *feedback_pin; // Interrupt pin for measuring speed + bool output_inverted; + + // Current values, updated at runtime + bool spindle_on; + float current_rpm; + float target_rpm; + float current_I_value; + float prev_error; + float current_pwm_value; + int time_since_update; + uint32_t last_irq; + + // Values from config + float pulses_per_rev; + float control_P_term; + float control_I_term; + float control_D_term; + float smoothing_decay; + + // These fields are updated by the interrupt + uint32_t last_edge; // Timestamp of last edge + volatile uint32_t last_time; // Time delay between last two edges + volatile uint32_t irq_count; +}; + +#endif + diff --git a/src/modules/tools/temperaturecontrol/AD8495.cpp b/src/modules/tools/temperaturecontrol/AD8495.cpp new file mode 100644 index 00000000..8b9a7057 --- /dev/null +++ b/src/modules/tools/temperaturecontrol/AD8495.cpp @@ -0,0 +1,90 @@ +/* + This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl). + Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. + Smoothie is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. + You should have received a copy of the GNU General Public License along with Smoothie. If not, see . +*/ + +#include "AD8495.h" +#include "libs/Kernel.h" +#include "libs/Pin.h" +#include "Config.h" +#include "checksumm.h" +#include "Adc.h" +#include "ConfigValue.h" +#include "libs/Median.h" +#include "utils.h" +#include "StreamOutputPool.h" + +#include + +//TODO ADDBACK #include "MRI_Hooks.h" + +#define UNDEFINED -1 + +#define AD8495_pin_checksum CHECKSUM("ad8495_pin") +#define AD8495_offset_checksum CHECKSUM("ad8495_offset") + +AD8495::AD8495() +{ + min_temp= 999; + max_temp= 0; +} + +AD8495::~AD8495() +{ +} + +// Get configuration from the config file +void AD8495::UpdateConfig(uint16_t module_checksum, uint16_t name_checksum) +{ + // Thermistor pin for ADC readings + this->AD8495_pin.from_string(THEKERNEL->config->value(module_checksum, name_checksum, AD8495_pin_checksum)->required()->as_string()); + this->AD8495_offset = THEKERNEL->config->value(module_checksum, name_checksum, AD8495_offset_checksum)->by_default(0)->as_number(); // Stated offset. For Adafruit board it is 250C. If pin 2(REF) of amplifier is connected to 0V then there is 0C offset. + + THEKERNEL->adc->enable_pin(&AD8495_pin); +} + + +float AD8495::get_temperature() +{ + float t= adc_value_to_temperature(new_AD8495_reading()); + // keep track of min/max for M305 + if(t > max_temp) max_temp= t; + if(t < min_temp) min_temp= t; + return t; +} + +void AD8495::get_raw() +{ + + int adc_value= new_AD8495_reading(); + const uint32_t max_adc_value= THEKERNEL->adc->get_max_value(); + float t=((float)adc_value)/(((float)max_adc_value)/3.3*0.005); + + t = t - this->AD8495_offset; + + THEKERNEL->streams->printf("adc= %d, max_adc= %lu, temp= %f, offset = %f\n", adc_value,max_adc_value,t, this->AD8495_offset); + + // reset the min/max + min_temp= max_temp= t; +} + +float AD8495::adc_value_to_temperature(uint32_t adc_value) +{ + const uint32_t max_adc_value= THEKERNEL->adc->get_max_value(); + if ((adc_value >= max_adc_value)) + return infinityf(); + + float t=((float)adc_value)/(((float)max_adc_value)/3.3*0.005); + + t=t-this->AD8495_offset; + + return t; +} + +int AD8495::new_AD8495_reading() +{ + // filtering now done in ADC + return THEKERNEL->adc->read(&AD8495_pin); +} diff --git a/src/modules/tools/temperaturecontrol/AD8495.h b/src/modules/tools/temperaturecontrol/AD8495.h new file mode 100644 index 00000000..cea83c76 --- /dev/null +++ b/src/modules/tools/temperaturecontrol/AD8495.h @@ -0,0 +1,42 @@ +/* + this file is part of smoothie (http://smoothieware.org/). the motion control part is heavily based on grbl (https://github.com/simen/grbl). + smoothie is free software: you can redistribute it and/or modify it under the terms of the gnu general public license as published by the free software foundation, either version 3 of the license, or (at your option) any later version. + smoothie is distributed in the hope that it will be useful, but without any warranty; without even the implied warranty of merchantability or fitness for a particular purpose. see the gnu general public license for more details. + you should have received a copy of the gnu general public license along with smoothie. if not, see . +*/ + +#ifndef AD8495_H +#define AD8495_H + +#include "TempSensor.h" +#include "RingBuffer.h" +#include "Pin.h" + +#include + +#define QUEUE_LEN 32 + +class StreamOutput; + +class AD8495 : public TempSensor +{ + public: + AD8495(); + ~AD8495(); + + // TempSensor interface. + void UpdateConfig(uint16_t module_checksum, uint16_t name_checksum); + float get_temperature(); + void get_raw(); + + private: + int new_AD8495_reading(); + float adc_value_to_temperature(uint32_t adc_value); + + Pin AD8495_pin; + float AD8495_offset; + + float min_temp, max_temp; +}; + +#endif diff --git a/src/modules/tools/temperaturecontrol/TemperatureControl.cpp b/src/modules/tools/temperaturecontrol/TemperatureControl.cpp index b4440c34..3f679950 100644 --- a/src/modules/tools/temperaturecontrol/TemperatureControl.cpp +++ b/src/modules/tools/temperaturecontrol/TemperatureControl.cpp @@ -31,6 +31,7 @@ // Temp sensor implementations: #include "Thermistor.h" #include "max31855.h" +#include "AD8495.h" // TOADDBACK #include "MRI_Hooks.h" @@ -63,6 +64,9 @@ #define preset1_checksum CHECKSUM("preset1") #define preset2_checksum CHECKSUM("preset2") +#define runaway_range_checksum CHECKSUM("runaway_range") +#define runaway_heating_timeout_checksum CHECKSUM("runaway_heating_timeout") + TemperatureControl::TemperatureControl(uint16_t name, int index) { name_checksum= name; @@ -114,7 +118,7 @@ void TemperatureControl::on_main_loop(void *argument) { if (this->temp_violated) { this->temp_violated = false; - THEKERNEL->streams->printf("Error: MINTEMP or MAXTEMP triggered on %s. Check your temperature sensors!\n", designator.c_str()); + THEKERNEL->streams->printf("ERROR: MINTEMP or MAXTEMP triggered on %s. Check your temperature sensors!\n", designator.c_str()); THEKERNEL->streams->printf("HALT asserted - reset or M999 required\n"); THEKERNEL->call_event(ON_HALT, nullptr); } @@ -128,10 +132,14 @@ void TemperatureControl::load_config() this->set_m_code = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, set_m_code_checksum)->by_default(104)->as_number(); this->set_and_wait_m_code = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, set_and_wait_m_code_checksum)->by_default(109)->as_number(); this->get_m_code = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, get_m_code_checksum)->by_default(105)->as_number(); - this->readings_per_second = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, readings_per_second_checksum)->by_default(160)->as_number(); + this->readings_per_second = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, readings_per_second_checksum)->by_default(20)->as_number(); this->designator = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, designator_checksum)->by_default(string("T"))->as_string(); + // Runaway parameters + this->runaway_range = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, runaway_range_checksum)->by_default(0)->as_number(); + this->runaway_heating_timeout = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, runaway_heating_timeout_checksum)->by_default(0)->as_number(); + // Max and min temperatures we are not allowed to get over (Safety) this->max_temp = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, max_temp_checksum)->by_default(300)->as_number(); this->min_temp = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, min_temp_checksum)->by_default(0)->as_number(); @@ -156,8 +164,8 @@ void TemperatureControl::load_config() sensor = new Thermistor(); } else if(sensor_type.compare("max31855") == 0) { sensor = new Max31855(); - //TOADDBACK } else if(sensor_type.compare("ad8495") == 0) { - //TOADDBACK sensor = new AD8495(); + } else if(sensor_type.compare("ad8495") == 0) { + sensor = new AD8495(); } else { sensor = new TempSensor(); // A dummy implementation } @@ -509,8 +517,46 @@ void TemperatureControl::pid_process(float temperature) void TemperatureControl::on_second_tick(void *argument) { + + // If waiting for a temperature to be reach, display it to keep host programs up to date on the progress if (waiting) THEKERNEL->streams->printf("%s:%3.1f /%3.1f @%d\n", designator.c_str(), get_temperature(), ((target_temperature <= 0) ? 0.0 : target_temperature), o); + + // Check whether or not there is a temperature runaway issue, if so stop everything and report it + if(THEKERNEL->is_halted()) return; + + if( this->target_temperature <= 0 ){ // If we are not trying to heat, state is NOT_HEATING + this->runaway_state = NOT_HEATING; + }else{ + switch( this->runaway_state ){ + case NOT_HEATING: // If we were previously not trying to heat, but we are now, change to state WAITING_FOR_TEMP_TO_BE_REACHED + if( this->target_temperature > 0 ){ + this->runaway_state = WAITING_FOR_TEMP_TO_BE_REACHED; + this->runaway_heating_timer = 0; + } + break; + case WAITING_FOR_TEMP_TO_BE_REACHED: // In we are in state 1 ( waiting for temperature to be reached ), and the temperature has been reached, change to state TARGET_TEMPERATURE_REACHED + if( this->get_temperature() >= this->target_temperature ){ + this->runaway_state = TARGET_TEMPERATURE_REACHED; + } + this->runaway_heating_timer++; + if( this->runaway_heating_timer > this->runaway_heating_timeout && this->runaway_heating_timeout != 0 ){ + this->runaway_heating_timer = 0; + THEKERNEL->streams->printf("ERROR: Temperature took too long to be reached on %s, HALT asserted, TURN POWER OFF IMMEDIATELY - reset or M999 required\n", designator.c_str()); + THEKERNEL->call_event(ON_HALT, nullptr); + } + break; + case TARGET_TEMPERATURE_REACHED: { // If we are in state TARGET_TEMPERATURE_REACHED, check for thermal runaway + float delta= this->get_temperature() - this->target_temperature; + // If the temperature is outside the acceptable range + if(this->runaway_range != 0 && fabsf(delta) > this->runaway_range){ + THEKERNEL->streams->printf("ERROR: Temperature runaway on %s (delta temp %f), HALT asserted, TURN POWER OFF IMMEDIATELY - reset or M999 required\n", designator.c_str(), delta); + THEKERNEL->call_event(ON_HALT, nullptr); + } + } + break; + } + } } void TemperatureControl::setPIDp(float p) diff --git a/src/modules/tools/temperaturecontrol/TemperatureControl.h b/src/modules/tools/temperaturecontrol/TemperatureControl.h index aa5ef4a2..2c956449 100644 --- a/src/modules/tools/temperaturecontrol/TemperatureControl.h +++ b/src/modules/tools/temperaturecontrol/TemperatureControl.h @@ -30,6 +30,8 @@ class TemperatureControl : public Module { void set_desired_temperature(float desired_temperature); float get_temperature(); + + enum RUNAWAY_TYPE {NOT_HEATING, WAITING_FOR_TEMP_TO_BE_REACHED, TARGET_TEMPERATURE_REACHED}; friend class PID_Autotuner; @@ -80,7 +82,15 @@ class TemperatureControl : public Module { float d_factor; float PIDdt; + // Temperature runaway values + RUNAWAY_TYPE runaway_state; + + struct { + uint8_t runaway_heating_timer:8; + // Temperature runaway config options + uint8_t runaway_range:8; + uint8_t runaway_heating_timeout:8; bool use_bangbang:1; bool waiting:1; bool temp_violated:1; diff --git a/src/modules/tools/temperaturecontrol/TemperatureControlPool.cpp b/src/modules/tools/temperaturecontrol/TemperatureControlPool.cpp index 65b8ed64..debd8bbf 100644 --- a/src/modules/tools/temperaturecontrol/TemperatureControlPool.cpp +++ b/src/modules/tools/temperaturecontrol/TemperatureControlPool.cpp @@ -22,7 +22,6 @@ using namespace std; void TemperatureControlPool::load_tools() { - vector modules; THEKERNEL->config->get_module_list( &modules, temperature_control_checksum ); int cnt = 0; @@ -30,7 +29,6 @@ void TemperatureControlPool::load_tools() // If module is enabled if( THEKERNEL->config->value(temperature_control_checksum, cs, enable_checksum )->as_bool() ) { TemperatureControl *controller = new TemperatureControl(cs, cnt++); - controllers.push_back( cs ); THEKERNEL->add_module(controller); } } diff --git a/src/modules/tools/temperaturecontrol/TemperatureControlPool.h b/src/modules/tools/temperaturecontrol/TemperatureControlPool.h index 9544ee13..c0d87678 100644 --- a/src/modules/tools/temperaturecontrol/TemperatureControlPool.h +++ b/src/modules/tools/temperaturecontrol/TemperatureControlPool.h @@ -13,10 +13,6 @@ class TemperatureControlPool { public: void load_tools(); - const std::vector& get_controllers() const { return controllers; }; - - private: - std::vector controllers; }; diff --git a/src/modules/tools/temperaturecontrol/max31855.cpp b/src/modules/tools/temperaturecontrol/max31855.cpp index bd4fe315..8688df7a 100644 --- a/src/modules/tools/temperaturecontrol/max31855.cpp +++ b/src/modules/tools/temperaturecontrol/max31855.cpp @@ -11,7 +11,7 @@ #include "Config.h" #include "checksumm.h" #include "ConfigValue.h" - +#include "StreamOutputPool.h" #include "max31855.h" // TOADDBACK #include "MRI_Hooks.h" @@ -32,31 +32,31 @@ Max31855::~Max31855() // Get configuration from the config file void Max31855::UpdateConfig(uint16_t module_checksum, uint16_t name_checksum) { - /* TOADDBACK // Chip select this->spi_cs_pin.from_string(THEKERNEL->config->value(module_checksum, name_checksum, chip_select_checksum)->by_default("0.16")->as_string()); this->spi_cs_pin.set(true); this->spi_cs_pin.as_output(); - + // select which SPI channel to use int spi_channel = THEKERNEL->config->value(module_checksum, name_checksum, spi_channel_checksum)->by_default(0)->as_number(); PinName miso; PinName mosi; PinName sclk; + if(spi_channel == 0) { - // Channel 0 - mosi=P0_18; miso=P0_17; sclk=P0_15; + mosi = SPI0_MOSI; miso = SPI0_MISO; sclk = SPI0_SCK; + } else if(spi_channel == 1) { + mosi = SPI1_MOSI; miso = SPI1_MISO; sclk = SPI1_SCK; } else { - // Channel 1 - mosi=P0_9; miso=P0_8; sclk=P0_7; - } + THEKERNEL->streams->printf("Temperaturecontrol ERROR: Unknown SPI Channel: %d\n", spi_channel); + return; + } delete spi; spi = new mbed::SPI(mosi, miso, sclk); // Spi settings: 1MHz (default), 16 bits, mode 0 (default) - spi->format(16); - */ + spi->format(16); //TODO the robot.cpp uses spi at 8 bits for the tmc2130 stepper motor in StepperMotor.cpp so we need to perhaps change this to 8 bits } float Max31855::get_temperature() @@ -94,7 +94,7 @@ float Max31855::read_temp() // uint16_t data2 = spi->write(0); this->spi_cs_pin.set(true); - + float temperature; //Process temp @@ -115,5 +115,5 @@ float Max31855::read_temp() temperature = ((data & 0x1FFF) + 1) / -4.f; } } - return temperature; + return temperature; } diff --git a/src/modules/tools/temperaturecontrol/max31855.h b/src/modules/tools/temperaturecontrol/max31855.h index 9f85f3c1..b0ee9985 100644 --- a/src/modules/tools/temperaturecontrol/max31855.h +++ b/src/modules/tools/temperaturecontrol/max31855.h @@ -23,7 +23,7 @@ class Max31855 : public TempSensor float get_temperature(); private: - float read_temp(); + float read_temp(); Pin spi_cs_pin; mbed::SPI *spi; RingBuffer readings; diff --git a/src/modules/tools/temperaturecontrol/predefined_thermistors.h b/src/modules/tools/temperaturecontrol/predefined_thermistors.h index c8fc71c5..410706dd 100644 --- a/src/modules/tools/temperaturecontrol/predefined_thermistors.h +++ b/src/modules/tools/temperaturecontrol/predefined_thermistors.h @@ -5,6 +5,8 @@ typedef struct { float beta, r0, t0; } const thermistor_beta_table_t; +// NOTE the order of these tables must NOT be changed as the index can be used in M305 to set the prefined thermistor to use + static const thermistor_beta_table_t predefined_thermistors_beta[] { // name, r1, r2, beta, r0, t0 {"EPCOS100K", 0, 4700, 4066.0F, 100000.0F, 25.0F}, // B57540G0104F000 @@ -22,7 +24,7 @@ typedef struct { float c1, c2, c3; } const thermistor_table_t; -// Use one of the following scripts to calcuate the coefficients: +// Use one of the following scripts to calculate the coefficients: // - http://www.thinksrs.com/downloads/programs/Therm%20Calc/NTCCalibrator/NTCcalculator.htm // - https://github.com/MarlinFirmware/Marlin/blob/Development/Marlin/scripts/createTemperatureLookupMarlin.py static const thermistor_table_t predefined_thermistors[] { diff --git a/src/modules/tools/temperatureswitch/TemperatureSwitch.cpp b/src/modules/tools/temperatureswitch/TemperatureSwitch.cpp new file mode 100755 index 00000000..510da512 --- /dev/null +++ b/src/modules/tools/temperatureswitch/TemperatureSwitch.cpp @@ -0,0 +1,241 @@ +/* + This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl). + Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. + Smoothie is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. + You should have received a copy of the GNU General Public License along with Smoothie. If not, see . +*/ + +/* +TemperatureSwitch is an optional module that will automatically turn on or off a switch +based on a setpoint temperature. It is commonly used to turn on/off a cooling fan or water pump +to cool the hot end's cold zone. Specifically, it turns one of the small MOSFETs on or off. + +Author: Michael Hackney, mhackney@eclecticangler.com +*/ + +#include "TemperatureSwitch.h" +#include "libs/Module.h" +#include "libs/Kernel.h" +#include "modules/tools/temperaturecontrol/TemperatureControlPublicAccess.h" +#include "SwitchPublicAccess.h" + +#include "utils.h" +#include "Gcode.h" +#include "Config.h" +#include "ConfigValue.h" +#include "checksumm.h" +#include "PublicData.h" +#include "StreamOutputPool.h" +#include "TemperatureControlPool.h" +#include "mri.h" + +#define temperatureswitch_checksum CHECKSUM("temperatureswitch") +#define enable_checksum CHECKSUM("enable") +#define temperatureswitch_hotend_checksum CHECKSUM("hotend") +#define temperatureswitch_threshold_temp_checksum CHECKSUM("threshold_temp") +#define temperatureswitch_type_checksum CHECKSUM("type") +#define temperatureswitch_switch_checksum CHECKSUM("switch") +#define temperatureswitch_heatup_poll_checksum CHECKSUM("heatup_poll") +#define temperatureswitch_cooldown_poll_checksum CHECKSUM("cooldown_poll") +#define temperatureswitch_trigger_checksum CHECKSUM("trigger") +#define temperatureswitch_inverted_checksum CHECKSUM("inverted") +#define temperatureswitch_arm_command_checksum CHECKSUM("arm_mcode") +#define designator_checksum CHECKSUM("designator") + +TemperatureSwitch::TemperatureSwitch() +{ +} + +TemperatureSwitch::~TemperatureSwitch() +{ + THEKERNEL->unregister_for_event(ON_SECOND_TICK, this); + THEKERNEL->unregister_for_event(ON_GCODE_RECEIVED, this); +} + +// Load module +void TemperatureSwitch::on_module_loaded() +{ + vector modulist; + // allow for multiple temperature switches + THEKERNEL->config->get_module_list(&modulist, temperatureswitch_checksum); + for (auto m : modulist) { + load_config(m); + } + + // no longer need this instance as it is just used to load the other instances + delete this; +} + +TemperatureSwitch* TemperatureSwitch::load_config(uint16_t modcs) +{ + // see if enabled + if (!THEKERNEL->config->value(temperatureswitch_checksum, modcs, enable_checksum)->by_default(false)->as_bool()) { + return nullptr; + } + + // create a temperature control and load settings + char designator= 0; + string s= THEKERNEL->config->value(temperatureswitch_checksum, modcs, designator_checksum)->by_default("")->as_string(); + if(s.empty()){ + // for backward compatibility temperatureswitch.hotend will need designator 'T' by default @DEPRECATED + if(modcs == temperatureswitch_hotend_checksum) designator= 'T'; + + }else{ + designator= s[0]; + } + + if(designator == 0) return nullptr; // no designator then not valid + + // load settings from config file + string switchname = THEKERNEL->config->value(temperatureswitch_checksum, modcs, temperatureswitch_switch_checksum)->by_default("")->as_string(); + if(switchname.empty()) { + // handle old configs where this was called type @DEPRECATED + switchname = THEKERNEL->config->value(temperatureswitch_checksum, modcs, temperatureswitch_type_checksum)->by_default("")->as_string(); + if(switchname.empty()) { + // no switch specified so invalid entry + THEKERNEL->streams->printf("WARNING TEMPERATURESWITCH: no switch specified\n"); + return nullptr; + } + } + + // create a new temperature switch module + TemperatureSwitch *ts= new TemperatureSwitch(); + + // save designator + ts->designator= designator; + + // if we should turn the switch on or off when trigger is hit + ts->inverted = THEKERNEL->config->value(temperatureswitch_checksum, modcs, temperatureswitch_inverted_checksum)->by_default(false)->as_bool(); + + // if we should trigger when above and below, or when rising through, or when falling through the specified temp + string trig = THEKERNEL->config->value(temperatureswitch_checksum, modcs, temperatureswitch_trigger_checksum)->by_default("level")->as_string(); + if(trig == "level") ts->trigger= LEVEL; + else if(trig == "rising") ts->trigger= RISING; + else if(trig == "falling") ts->trigger= FALLING; + else ts->trigger= LEVEL; + + // the mcode used to arm the switch + ts->arm_mcode = THEKERNEL->config->value(temperatureswitch_checksum, modcs, temperatureswitch_arm_command_checksum)->by_default(0)->as_number(); + + ts->temperatureswitch_switch_cs= get_checksum(switchname); // checksum of the switch to use + + ts->temperatureswitch_threshold_temp = THEKERNEL->config->value(temperatureswitch_checksum, modcs, temperatureswitch_threshold_temp_checksum)->by_default(50.0f)->as_number(); + + // these are to tune the heatup and cooldown polling frequencies + ts->temperatureswitch_heatup_poll = THEKERNEL->config->value(temperatureswitch_checksum, modcs, temperatureswitch_heatup_poll_checksum)->by_default(15)->as_number(); + ts->temperatureswitch_cooldown_poll = THEKERNEL->config->value(temperatureswitch_checksum, modcs, temperatureswitch_cooldown_poll_checksum)->by_default(60)->as_number(); + ts->current_delay = ts->temperatureswitch_heatup_poll; + + // set initial state + ts->current_state= NONE; + ts->second_counter = ts->current_delay; // do test immediately on first second_tick + // if not defined then always armed, otherwise start out disarmed + ts->armed= (ts->arm_mcode == 0); + + // Register for events + ts->register_for_event(ON_SECOND_TICK); + + if(ts->arm_mcode != 0) { + ts->register_for_event(ON_GCODE_RECEIVED); + } + return ts; +} + +void TemperatureSwitch::on_gcode_received(void *argument) +{ + Gcode *gcode = static_cast(argument); + if(gcode->has_m && gcode->m == this->arm_mcode) { + this->armed= (gcode->has_letter('S') && gcode->get_value('S') != 0); + gcode->stream->printf("temperature switch %s\n", this->armed ? "armed" : "disarmed"); + } +} + +// Called once a second but we only need to service on the cooldown and heatup poll intervals +void TemperatureSwitch::on_second_tick(void *argument) +{ + second_counter++; + if (second_counter < current_delay) return; + + second_counter = 0; + float current_temp = this->get_highest_temperature(); + + if (current_temp >= this->temperatureswitch_threshold_temp) { + set_state(HIGH_TEMP); + + } else { + set_state(LOW_TEMP); + } +} + +void TemperatureSwitch::set_state(STATE state) +{ + if(state == this->current_state) return; // state did not change + + // state has changed + switch(this->trigger) { + case LEVEL: + // switch on or off depending on HIGH or LOW + set_switch(state == HIGH_TEMP); + break; + + case RISING: + // switch on if rising edge + if(this->current_state == LOW_TEMP && state == HIGH_TEMP) set_switch(true); + break; + + case FALLING: + // switch off if falling edge + if(this->current_state == HIGH_TEMP && state == LOW_TEMP) set_switch(false); + break; + } + + this->current_delay = state == HIGH_TEMP ? this->temperatureswitch_cooldown_poll : this->temperatureswitch_heatup_poll; + this->current_state= state; +} + +// Get the highest temperature from the set of temperature controllers +float TemperatureSwitch::get_highest_temperature() +{ + float high_temp = 0.0; + + std::vector controllers; + bool ok = PublicData::get_value(temperature_control_checksum, poll_controls_checksum, &controllers); + if (ok) { + for (auto &c : controllers) { + // check if this controller's temp is the highest and save it if so + if (c.designator[0] == this->designator && c.current_temperature > high_temp) { + high_temp = c.current_temperature; + } + } + } + + return high_temp; +} + +// Turn the switch on (true) or off (false) +void TemperatureSwitch::set_switch(bool switch_state) +{ + if(!this->armed) return; // do not actually switch anything if not armed + + if(this->arm_mcode != 0 && this->trigger != LEVEL) { + // if edge triggered we only trigger once per arming, if level triggered we switch as long as we are armed + this->armed= false; + } + + if(this->inverted) switch_state= !switch_state; // turn switch on or off inverted + + // get current switch state + struct pad_switch pad; + bool ok = PublicData::get_value(switch_checksum, this->temperatureswitch_switch_cs, 0, &pad); + if (!ok) { + THEKERNEL->streams->printf("// Failed to get switch state.\r\n"); + return; + } + + if(pad.state == switch_state) return; // switch is already in the requested state + + ok = PublicData::set_value(switch_checksum, this->temperatureswitch_switch_cs, state_checksum, &switch_state); + if (!ok) { + THEKERNEL->streams->printf("// Failed changing switch state.\r\n"); + } +} diff --git a/src/modules/tools/temperatureswitch/TemperatureSwitch.h b/src/modules/tools/temperatureswitch/TemperatureSwitch.h new file mode 100755 index 00000000..1c45c90d --- /dev/null +++ b/src/modules/tools/temperatureswitch/TemperatureSwitch.h @@ -0,0 +1,82 @@ +/* + This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl). + Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. + Smoothie is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. + You should have received a copy of the GNU General Public License along with Smoothie. If not, see . +*/ + +/* +TemperatureSwitch is an optional module that will automatically turn on or off a switch +based on a setpoint temperature. It is commonly used to turn on/off a cooling fan or water pump +to cool the hot end's cold zone. Specifically, it turns one of the small MOSFETs on or off. + +Author: Michael Hackney, mhackney@eclecticangler.com +*/ + +#ifndef TEMPERATURESWITCH_MODULE_H +#define TEMPERATURESWITCH_MODULE_H + +using namespace std; + +#include "libs/Module.h" +#include +#include + +class TemperatureSwitch : public Module +{ + public: + TemperatureSwitch(); + ~TemperatureSwitch(); + void on_module_loaded(); + void on_second_tick(void *argument); + void on_gcode_received(void *argument); + TemperatureSwitch* load_config(uint16_t modcs); + + bool is_armed() const { return armed; } + + private: + enum TRIGGER_TYPE {LEVEL, RISING, FALLING}; + enum STATE {NONE, HIGH_TEMP, LOW_TEMP}; + + // get the highest temperature from the set of configured temperature controllers + float get_highest_temperature(); + + // turn the switch on or off + void set_switch(bool cooler_state); + + // temperature has changed state + void set_state(STATE state); + + // temperatureswitch.hotend.threshold_temp + float temperatureswitch_threshold_temp; + + // temperatureswitch.hotend.switch + uint16_t temperatureswitch_switch_cs; + + // check temps on heatup every X seconds + // this can be set in config: temperatureswitch.hotend.heatup_poll + uint16_t temperatureswitch_heatup_poll; + + // check temps on cooldown every X seconds + // this can be set in config: temperatureswitch.hotend.cooldown_poll + uint16_t temperatureswitch_cooldown_poll; + + // our internal second counter + uint16_t second_counter; + + // we are delaying for this many seconds + uint16_t current_delay; + + // the mcode that will arm the switch, 0 means always armed + uint16_t arm_mcode; + + struct { + char designator:8; + bool inverted:1; + bool armed:1; + TRIGGER_TYPE trigger:2; + STATE current_state:2; + }; +}; + +#endif diff --git a/src/modules/tools/toolmanager/Tool.h b/src/modules/tools/toolmanager/Tool.h index 7f5ae4c8..59e6fe41 100644 --- a/src/modules/tools/toolmanager/Tool.h +++ b/src/modules/tools/toolmanager/Tool.h @@ -5,8 +5,7 @@ you should have received a copy of the gnu general public license along with smoothie. if not, see . */ -#ifndef TOOL_H -#define TOOL_H +#pragma once #include "Module.h" @@ -18,16 +17,13 @@ class Tool : public Module Tool(){}; virtual ~Tool() {}; - virtual void enable(){ enabled= true; } - virtual void disable(){ enabled= false; } + virtual void select()= 0; + virtual void deselect()= 0; virtual const float *get_offset() const { return offset; } virtual uint16_t get_name() const { return identifier; } protected: - bool enabled; float offset[3]; uint16_t identifier; }; - -#endif diff --git a/src/modules/tools/toolmanager/ToolManager.cpp b/src/modules/tools/toolmanager/ToolManager.cpp index 6f09506a..16680101 100644 --- a/src/modules/tools/toolmanager/ToolManager.cpp +++ b/src/modules/tools/toolmanager/ToolManager.cpp @@ -56,14 +56,14 @@ void ToolManager::on_gcode_received(void *argument) if(new_tool != this->active_tool) { // We must wait for an empty queue before we can disable the current extruder THEKERNEL->conveyor->wait_for_idle(); - this->tools[active_tool]->disable(); + this->tools[active_tool]->deselect(); this->active_tool = new_tool; this->current_tool_name = this->tools[active_tool]->get_name(); - this->tools[active_tool]->enable(); + this->tools[active_tool]->select(); //send new_tool_offsets to robot const float *new_tool_offset = tools[new_tool]->get_offset(); - THEKERNEL->robot->setToolOffset(new_tool_offset); + THEROBOT->setToolOffset(new_tool_offset); } } } @@ -115,13 +115,13 @@ void ToolManager::on_set_public_data(void* argument) void ToolManager::add_tool(Tool* tool_to_add) { if(this->tools.size() == 0) { - tool_to_add->enable(); + tool_to_add->select(); this->current_tool_name = tool_to_add->get_name(); //send new_tool_offsets to robot const float *new_tool_offset = tool_to_add->get_offset(); - THEKERNEL->robot->setToolOffset(new_tool_offset); + THEROBOT->setToolOffset(new_tool_offset); } else { - tool_to_add->disable(); + tool_to_add->deselect(); } this->tools.push_back( tool_to_add ); } diff --git a/src/modules/tools/zprobe/DeltaCalibrationStrategy.cpp b/src/modules/tools/zprobe/DeltaCalibrationStrategy.cpp new file mode 100644 index 00000000..43806281 --- /dev/null +++ b/src/modules/tools/zprobe/DeltaCalibrationStrategy.cpp @@ -0,0 +1,424 @@ +#include "DeltaCalibrationStrategy.h" +#include "Kernel.h" +#include "Config.h" +#include "Robot.h" +#include "StreamOutputPool.h" +#include "Gcode.h" +#include "checksumm.h" +#include "ConfigValue.h" +#include "PublicDataRequest.h" +#include "EndstopsPublicAccess.h" +#include "PublicData.h" +#include "Conveyor.h" +#include "ZProbe.h" +#include "BaseSolution.h" +#include "StepperMotor.h" + +#include +#include + +#define radius_checksum CHECKSUM("radius") +#define initial_height_checksum CHECKSUM("initial_height") + +// deprecated +#define probe_radius_checksum CHECKSUM("probe_radius") + +bool DeltaCalibrationStrategy::handleConfig() +{ + // default is probably wrong + float r= THEKERNEL->config->value(leveling_strategy_checksum, delta_calibration_strategy_checksum, radius_checksum)->by_default(-1)->as_number(); + if(r == -1) { + // deprecated config syntax] + r = THEKERNEL->config->value(zprobe_checksum, probe_radius_checksum)->by_default(100.0F)->as_number(); + } + this->probe_radius= r; + + // the initial height above the bed we stop the intial move down after home to find the bed + // this should be a height that is enough that the probe will not hit the bed and is an offset from max_z (can be set to 0 if max_z takes into account the probe offset) + this->initial_height= THEKERNEL->config->value(leveling_strategy_checksum, delta_calibration_strategy_checksum, initial_height_checksum)->by_default(10)->as_number(); + return true; +} + +bool DeltaCalibrationStrategy::handleGcode(Gcode *gcode) +{ + if( gcode->has_g) { + // G code processing + if( gcode->g == 32 ) { // auto calibration for delta, Z bed mapping for cartesian + // first wait for an empty queue i.e. no moves left + THEKERNEL->conveyor->wait_for_idle(); + + // turn off any compensation transform as it will be invalidated anyway by this + THEROBOT->compensationTransform= nullptr; + + if(!gcode->has_letter('R')) { + if(!calibrate_delta_endstops(gcode)) { + gcode->stream->printf("Calibration failed to complete, check the initial probe height and/or initial_height settings\n"); + return true; + } + } + if(!gcode->has_letter('E')) { + if(!calibrate_delta_radius(gcode)) { + gcode->stream->printf("Calibration failed to complete, check the initial probe height and/or initial_height settings\n"); + return true; + } + } + gcode->stream->printf("Calibration complete, save settings with M500\n"); + return true; + + }else if (gcode->g == 29) { + // probe the 7 points + if(!probe_delta_points(gcode)) { + gcode->stream->printf("Calibration failed to complete, check the initial probe height and/or initial_height settings\n"); + } + return true; + } + + } else if(gcode->has_m) { + // handle mcodes + } + + return false; +} + +// calculate the X and Y positions for the three towers given the radius from the center +static std::tuple getCoordinates(float radius) +{ + float px = 0.866F * radius; // ~sin(60) + float py = 0.5F * radius; // cos(60) + float t1x = -px, t1y = -py; // X Tower + float t2x = px, t2y = -py; // Y Tower + float t3x = 0.0F, t3y = radius; // Z Tower + return std::make_tuple(t1x, t1y, t2x, t2y, t3x, t3y); +} + + +// Probes the 7 points on a delta can be used for off board calibration +bool DeltaCalibrationStrategy::probe_delta_points(Gcode *gcode) +{ + float bedht= findBed(); + if(isnan(bedht)) return false; + + gcode->stream->printf("initial Bed ht is %f mm\n", bedht); + + // check probe ht + float mm; + if(!zprobe->doProbeAt(mm, 0, 0)) return false; + float dz = zprobe->getProbeHeight() - mm; + gcode->stream->printf("center probe: %1.4f\n", dz); + + // get probe points + float t1x, t1y, t2x, t2y, t3x, t3y; + std::tie(t1x, t1y, t2x, t2y, t3x, t3y) = getCoordinates(this->probe_radius); + + // gather probe points + float pp[][2] {{t1x, t1y}, {t2x, t2y}, {t3x, t3y}, {0, 0}, {-t1x, -t1y}, {-t2x, -t2y}, {-t3x, -t3y}}; + + float max_delta= 0; + float last_z= NAN; + float start_z; + std::tie(std::ignore, std::ignore, start_z)= THEROBOT->get_axis_position(); + + for(auto& i : pp) { + float mm; + if(!zprobe->doProbeAt(mm, i[0], i[1])) return false; + float z = mm; + if(gcode->subcode == 0) { + gcode->stream->printf("X:%1.4f Y:%1.4f Z:%1.4f A:%1.4f B:%1.4f C:%1.4f\n", + i[0], i[1], z, + THEROBOT->actuators[0]->get_current_position()+z, + THEROBOT->actuators[1]->get_current_position()+z, + THEROBOT->actuators[2]->get_current_position()+z); + + }else if(gcode->subcode == 1) { + // format that can be pasted here http://escher3d.com/pages/wizards/wizarddelta.php + gcode->stream->printf("X%1.4f Y%1.4f Z%1.4f\n", i[0], i[1], start_z - z); + } + + if(isnan(last_z)) { + last_z= z; + }else{ + max_delta= std::max(max_delta, fabsf(z-last_z)); + } + } + + gcode->stream->printf("max delta: %f\n", max_delta); + + return true; +} + +float DeltaCalibrationStrategy::findBed() +{ + // home + zprobe->home(); + + // move to an initial position fast so as to not take all day, we move down max_z - initial_height, which is set in config, default 10mm + float deltaz= zprobe->getMaxZ() - initial_height; + zprobe->coordinated_move(NAN, NAN, -deltaz, zprobe->getFastFeedrate(), true); + + // find bed, run at slow rate so as to not hit bed hard + float mm; + if(!zprobe->run_probe(mm, false)) return NAN; + zprobe->return_probe(mm); + + // leave the probe zprobe->getProbeHeight() above bed + float dz= zprobe->getProbeHeight() - mm; + zprobe->coordinated_move(NAN, NAN, dz, zprobe->getFastFeedrate(), true); // relative move + + return mm + deltaz - zprobe->getProbeHeight(); // distance to move from home to 5mm above bed +} + +/* Run a calibration routine for a delta + 1. Home + 2. probe for z bed + 3. probe initial tower positions + 4. set initial trims such that trims will be minimal negative values + 5. home, probe three towers again + 6. calculate trim offset and apply to all trims + 7. repeat 5, 6 until it converges on a solution +*/ + +bool DeltaCalibrationStrategy::calibrate_delta_endstops(Gcode *gcode) +{ + float target = 0.03F; + if(gcode->has_letter('I')) target = gcode->get_value('I'); // override default target + if(gcode->has_letter('J')) this->probe_radius = gcode->get_value('J'); // override default probe radius + + bool keep = false; + if(gcode->has_letter('K')) keep = true; // keep current settings + + gcode->stream->printf("Calibrating Endstops: target %fmm, radius %fmm\n", target, this->probe_radius); + + // get probe points + float t1x, t1y, t2x, t2y, t3x, t3y; + std::tie(t1x, t1y, t2x, t2y, t3x, t3y) = getCoordinates(this->probe_radius); + + float trimx = 0.0F, trimy = 0.0F, trimz = 0.0F; + if(!keep) { + // zero trim values + if(!set_trim(0, 0, 0, gcode->stream)) return false; + + } else { + // get current trim, and continue from that + if (get_trim(trimx, trimy, trimz)) { + gcode->stream->printf("Current Trim X: %f, Y: %f, Z: %f\r\n", trimx, trimy, trimz); + + } else { + gcode->stream->printf("Could not get current trim, are endstops enabled?\n"); + return false; + } + } + + // find the bed, as we potentially have a temporary z probe we don't know how low under the nozzle it is + // so we need to find the initial place that the probe triggers when it hits the bed + float bedht= findBed(); + if(isnan(bedht)) return false; + gcode->stream->printf("initial Bed ht is %f mm\n", bedht); + + // check probe ht + float mm; + if(!zprobe->doProbeAt(mm, 0, 0)) return false; + float dz = zprobe->getProbeHeight() - mm; + gcode->stream->printf("center probe: %1.4f\n", dz); + if(fabsf(dz) > target) { + gcode->stream->printf("Probe was not repeatable to %f mm, (%f)\n", target, dz); + return false; + } + + // get initial probes + // probe the base of the X tower + if(!zprobe->doProbeAt(mm, t1x, t1y)) return false; + float t1z = mm; + gcode->stream->printf("T1-0 Z:%1.4f\n", t1z); + + // probe the base of the Y tower + if(!zprobe->doProbeAt(mm, t2x, t2y)) return false; + float t2z = mm; + gcode->stream->printf("T2-0 Z:%1.4f\n", t2z); + + // probe the base of the Z tower + if(!zprobe->doProbeAt(mm, t3x, t3y)) return false; + float t3z = mm; + gcode->stream->printf("T3-0 Z:%1.4f\n", t3z); + + float trimscale = 1.2522F; // empirically determined + + auto mmx = std::minmax({t1z, t2z, t3z}); + if((mmx.second - mmx.first) <= target) { + gcode->stream->printf("trim already set within required parameters: delta %f\n", mmx.second - mmx.first); + return true; + } + + // set trims to worst case so we always have a negative trim + trimx += (mmx.first - t1z) * trimscale; + trimy += (mmx.first - t2z) * trimscale; + trimz += (mmx.first - t3z) * trimscale; + + for (int i = 1; i <= 10; ++i) { + // set trim + if(!set_trim(trimx, trimy, trimz, gcode->stream)) return false; + + // home and move probe to start position just above the bed + zprobe->home(); + zprobe->coordinated_move(NAN, NAN, -bedht, zprobe->getFastFeedrate(), true); // do a relative move from home to the point above the bed + + // probe the base of the X tower + if(!zprobe->doProbeAt(mm, t1x, t1y)) return false; + t1z = mm; + gcode->stream->printf("T1-%d Z:%1.4f\n", i, t1z); + + // probe the base of the Y tower + if(!zprobe->doProbeAt(mm, t2x, t2y)) return false; + t2z = mm; + gcode->stream->printf("T2-%d Z:%1.4f\n", i, t2z); + + // probe the base of the Z tower + if(!zprobe->doProbeAt(mm, t3x, t3y)) return false; + t3z = mm; + gcode->stream->printf("T3-%d Z:%1.4f\n", i, t3z); + + mmx = std::minmax({t1z, t2z, t3z}); + if((mmx.second - mmx.first) <= target) { + gcode->stream->printf("trim set to within required parameters: delta %f\n", mmx.second - mmx.first); + break; + } + + // set new trim values based on min difference + trimx += (mmx.first - t1z) * trimscale; + trimy += (mmx.first - t2z) * trimscale; + trimz += (mmx.first - t3z) * trimscale; + + // flush the output + THEKERNEL->call_event(ON_IDLE); + } + + if((mmx.second - mmx.first) > target) { + gcode->stream->printf("WARNING: trim did not resolve to within required parameters: delta %f\n", mmx.second - mmx.first); + } + + return true; +} + +/* + probe edges to get outer positions, then probe center + modify the delta radius until center and X converge +*/ + +bool DeltaCalibrationStrategy::calibrate_delta_radius(Gcode *gcode) +{ + float target = 0.03F; + if(gcode->has_letter('I')) target = gcode->get_value('I'); // override default target + if(gcode->has_letter('J')) this->probe_radius = gcode->get_value('J'); // override default probe radius + + gcode->stream->printf("Calibrating delta radius: target %f, radius %f\n", target, this->probe_radius); + + // get probe points + float t1x, t1y, t2x, t2y, t3x, t3y; + std::tie(t1x, t1y, t2x, t2y, t3x, t3y) = getCoordinates(this->probe_radius); + + // find the bed, as we potentially have a temporary z probe we don't know how low under the nozzle it is + // so we need to find thr initial place that the probe triggers when it hits the bed + float bedht= findBed(); + if(isnan(bedht)) return false; + gcode->stream->printf("initial Bed ht is %f mm\n", bedht); + + // check probe ht + float mm; + if(!zprobe->doProbeAt(mm, 0, 0)) return false; + float dz = zprobe->getProbeHeight() - mm; + gcode->stream->printf("center probe: %1.4f\n", dz); + if(fabsf(dz) > target) { + gcode->stream->printf("Probe was not repeatable to %f mm, (%f)\n", target, dz); + return false; + } + + // probe center to get reference point at this Z height + float dc; + if(!zprobe->doProbeAt(dc, 0, 0)) return false; + gcode->stream->printf("CT Z:%1.3f\n", dc); + float cmm = dc; + + // get current delta radius + float delta_radius = 0.0F; + BaseSolution::arm_options_t options; + if(THEROBOT->arm_solution->get_optional(options)) { + delta_radius = options['R']; + } + if(delta_radius == 0.0F) { + gcode->stream->printf("This appears to not be a delta arm solution\n"); + return false; + } + options.clear(); + + bool good= false; + float drinc = 2.5F; // approx + for (int i = 1; i <= 10; ++i) { + // probe t1, t2, t3 and get average, but use coordinated moves, probing center won't change + float dx, dy, dz; + if(!zprobe->doProbeAt(dx, t1x, t1y)) return false; + gcode->stream->printf("T1-%d Z:%1.3f\n", i, dx); + if(!zprobe->doProbeAt(dy, t2x, t2y)) return false; + gcode->stream->printf("T2-%d Z:%1.3f\n", i, dy); + if(!zprobe->doProbeAt(dz, t3x, t3y)) return false; + gcode->stream->printf("T3-%d Z:%1.3f\n", i, dz); + + // now look at the difference and reduce it by adjusting delta radius + float m = (dx + dy + dz) / 3.0F; + float d = cmm - m; + gcode->stream->printf("C-%d Z-ave:%1.4f delta: %1.3f\n", i, m, d); + + if(fabsf(d) <= target){ + good= true; + break; // resolution of success + } + + // increase delta radius to adjust for low center + // decrease delta radius to adjust for high center + delta_radius += (d * drinc); + + // set the new delta radius + options['R'] = delta_radius; + THEROBOT->arm_solution->set_optional(options); + gcode->stream->printf("Setting delta radius to: %1.4f\n", delta_radius); + + zprobe->home(); + zprobe->coordinated_move(NAN, NAN, -bedht, zprobe->getFastFeedrate(), true); // needs to be a relative coordinated move + + // flush the output + THEKERNEL->call_event(ON_IDLE); + } + + if(!good) { + gcode->stream->printf("WARNING: delta radius did not resolve to within required parameters: %f\n", target); + } + + return true; +} + +bool DeltaCalibrationStrategy::set_trim(float x, float y, float z, StreamOutput *stream) +{ + float t[3] {x, y, z}; + bool ok = PublicData::set_value( endstops_checksum, trim_checksum, t); + + if (ok) { + stream->printf("set trim to X:%f Y:%f Z:%f\n", x, y, z); + } else { + stream->printf("unable to set trim, is endstops enabled?\n"); + } + + return ok; +} + +bool DeltaCalibrationStrategy::get_trim(float &x, float &y, float &z) +{ + void *returned_data; + bool ok = PublicData::get_value( endstops_checksum, trim_checksum, &returned_data ); + + if (ok) { + float *trim = static_cast(returned_data); + x = trim[0]; + y = trim[1]; + z = trim[2]; + return true; + } + return false; +} diff --git a/src/modules/tools/zprobe/DeltaCalibrationStrategy.h b/src/modules/tools/zprobe/DeltaCalibrationStrategy.h new file mode 100644 index 00000000..34c86004 --- /dev/null +++ b/src/modules/tools/zprobe/DeltaCalibrationStrategy.h @@ -0,0 +1,30 @@ +#ifndef _DELTALEVELINGSTRATEGY +#define _DELTALEVELINGSTRATEGY + +#include "LevelingStrategy.h" + +#define delta_calibration_strategy_checksum CHECKSUM("delta-calibration") + +class StreamOutput; + +class DeltaCalibrationStrategy : public LevelingStrategy +{ +public: + DeltaCalibrationStrategy(ZProbe *zprobe) : LevelingStrategy(zprobe){}; + ~DeltaCalibrationStrategy(){}; + bool handleGcode(Gcode* gcode); + bool handleConfig(); + +private: + bool set_trim(float x, float y, float z, StreamOutput *stream); + bool get_trim(float& x, float& y, float& z); + bool calibrate_delta_endstops(Gcode *gcode); + bool calibrate_delta_radius(Gcode *gcode); + bool probe_delta_points(Gcode *gcode); + float findBed(); + + float probe_radius; + float initial_height; +}; + +#endif diff --git a/src/modules/tools/zprobe/DeltaGridStrategy.cpp b/src/modules/tools/zprobe/DeltaGridStrategy.cpp new file mode 100644 index 00000000..93875cd0 --- /dev/null +++ b/src/modules/tools/zprobe/DeltaGridStrategy.cpp @@ -0,0 +1,595 @@ +/* + This code is derived from (and mostly copied from) Johann Rocholls code at https://github.com/jcrocholl/Marlin/blob/deltabot/Marlin/Marlin_main.cpp + license is the same as his code. + + Summary + ------- + Probes grid_size points in X and Y (total probes grid_size * grid_size) and stores the relative offsets from the 0,0 Z height + When enabled every move will calculate the Z offset based on interpolating the height offset within the grids nearest 4 points. + + Configuration + ------------- + The strategy must be enabled in the config as well as zprobe. + + leveling-strategy.delta-grid.enable true + + The radius of the bed must be specified with... + + leveling-strategy.delta-grid.radius 50 + + this needs to be at least as big as the maximum printing radius as moves outside of this will not be compensated for correctly + + The size of the grid can be set with... + + leveling-strategy.delta-grid.size 7 + + this is the X and Y size of the grid, it must be an odd number, the default is 7 which is 49 probe points + + Optionally probe offsets from the nozzle or tool head can be defined with... + + leveling-strategy.delta-grid.probe_offsets 0,0,0 # probe offsetrs x,y,z + + they may also be set with M565 X0 Y0 Z0 + + If the saved grid is to be loaded on boot then this must be set in the config... + + leveling-strategy.delta-grid.save true + + Then when M500 is issued it will save M375 which will cause the grid to be loaded on boot. The default is to not autoload the grid on boot + + Optionally an initial_height can be set that tell the intial probe where to stop the fast decent before it probes, this should be around 5-10mm above the bed + leveling-strategy.delta-grid.initial_height 10 + + + Usage + ----- + G29 test probes in a spiral pattern within the radius producing a map of offsets, this can be imported into a graphing program to visualize the bed heights + optional parameters {{In}} sets the number of points to the value n, {{Jn}} sets the radius for this probe. + + G31 probes the grid and turns the compensation on, this will remain in effect until reset or M561/M370 + optional parameters {{Jn}} sets the radius for this probe, which gets saved with M375 + + M370 clears the grid and turns off compensation + M374 Save grid to /sd/delta.grid + M374.1 delete /sd/delta.grid + M375 Load the grid from /sd/delta.grid and enable compensation + M375.1 display the current grid + M561 clears the grid and turns off compensation + M565 defines the probe offsets from the nozzle or tool head + + + M500 saves the probe points + M503 displays the current settings +*/ + +#include "DeltaGridStrategy.h" + +#include "Kernel.h" +#include "Config.h" +#include "Robot.h" +#include "StreamOutputPool.h" +#include "Gcode.h" +#include "checksumm.h" +#include "ConfigValue.h" +#include "PublicDataRequest.h" +#include "PublicData.h" +#include "Conveyor.h" +#include "ZProbe.h" +#include "nuts_bolts.h" +#include "utils.h" + +#include +#include +#include +#include + +#define grid_radius_checksum CHECKSUM("radius") +#define grid_size_checksum CHECKSUM("size") +#define tolerance_checksum CHECKSUM("tolerance") +#define save_checksum CHECKSUM("save") +#define probe_offsets_checksum CHECKSUM("probe_offsets") +#define initial_height_checksum CHECKSUM("initial_height") +#define x_max_checksum CHECKSUM("x_max") +#define y_max_checksum CHECKSUM("y_max") +#define do_home_checksum CHECKSUM("do_home") +#define is_square_checksum CHECKSUM("is_square") + +#define GRIDFILE "/sd/delta.grid" + +DeltaGridStrategy::DeltaGridStrategy(ZProbe *zprobe) : LevelingStrategy(zprobe) +{ + grid= nullptr; +} + +DeltaGridStrategy::~DeltaGridStrategy() +{ + if(grid != nullptr) free(grid); +} + +bool DeltaGridStrategy::handleConfig() +{ + grid_radius = THEKERNEL->config->value(leveling_strategy_checksum, delta_grid_leveling_strategy_checksum, grid_radius_checksum)->by_default(50.0F)->as_number(); + grid_size = THEKERNEL->config->value(leveling_strategy_checksum, delta_grid_leveling_strategy_checksum, grid_size_checksum)->by_default(7)->as_number(); + tolerance = THEKERNEL->config->value(leveling_strategy_checksum, delta_grid_leveling_strategy_checksum, tolerance_checksum)->by_default(0.03F)->as_number(); + save = THEKERNEL->config->value(leveling_strategy_checksum, delta_grid_leveling_strategy_checksum, save_checksum)->by_default(false)->as_bool(); + do_home = THEKERNEL->config->value(leveling_strategy_checksum, delta_grid_leveling_strategy_checksum, do_home_checksum)->by_default(true)->as_bool(); + is_square = THEKERNEL->config->value(leveling_strategy_checksum, delta_grid_leveling_strategy_checksum, is_square_checksum)->by_default(false)->as_bool(); + + if (is_square) + { + x_max = THEKERNEL->config->value(leveling_strategy_checksum, delta_grid_leveling_strategy_checksum, x_max_checksum)->by_default(0.0F)->as_number(); + y_max = THEKERNEL->config->value(leveling_strategy_checksum, delta_grid_leveling_strategy_checksum, y_max_checksum)->by_default(0.0F)->as_number(); + + // intelligently set defaults. + if (x_max >= 1.0F) grid_radius = x_max; + if (x_max < 1.0F) x_max = grid_radius; + if (y_max >= 1.0F) grid_radius = y_max; + if (y_max < 1.0F) y_max = grid_radius; + if (x_max >= 1.0F && y_max >= 1.0F) grid_radius = std::max(x_max, y_max); + } + else + { + x_max = grid_radius; + y_max = grid_radius; + } + + // the initial height above the bed we stop the intial move down after home to find the bed + // this should be a height that is enough that the probe will not hit the bed and is an offset from max_z (can be set to 0 if max_z takes into account the probe offset) + this->initial_height = THEKERNEL->config->value(leveling_strategy_checksum, delta_grid_leveling_strategy_checksum, initial_height_checksum)->by_default(10)->as_number(); + + // Probe offsets xxx,yyy,zzz + { + std::string po = THEKERNEL->config->value(leveling_strategy_checksum, delta_grid_leveling_strategy_checksum, probe_offsets_checksum)->by_default("0,0,0")->as_string(); + std::vector v = parse_number_list(po.c_str()); + if(v.size() >= 3) { + this->probe_offsets = std::make_tuple(v[0], v[1], v[2]); + } + } + + grid=(float *)malloc(grid_size * grid_size * sizeof(float)); + + reset_bed_level(); + + return true; +} + +void DeltaGridStrategy::save_grid(StreamOutput *stream) +{ + if(isnan(grid[0])) { + stream->printf("error:No grid to save\n"); + return; + } + + FILE *fp = fopen(GRIDFILE, "w"); + if(fp == NULL) { + stream->printf("error:Failed to open grid file %s\n", GRIDFILE); + return; + } + + if(fwrite(&grid_size, sizeof(uint8_t), 1, fp) != 1) { + stream->printf("error:Failed to write grid size\n"); + fclose(fp); + return; + } + + if(fwrite(&grid_radius, sizeof(float), 1, fp) != 1) { + stream->printf("error:Failed to write grid radius\n"); + fclose(fp); + return; + } + + for (int y = 0; y < grid_size; y++) { + for (int x = 0; x < grid_size; x++) { + if(fwrite(&grid[x + (grid_size*y)], sizeof(float), 1, fp) != 1) { + stream->printf("error:Failed to write grid\n"); + fclose(fp); + return; + } + } + } + stream->printf("grid saved to %s\n", GRIDFILE); + fclose(fp); +} + +bool DeltaGridStrategy::load_grid(StreamOutput *stream) +{ + FILE *fp = fopen(GRIDFILE, "r"); + if(fp == NULL) { + stream->printf("error:Failed to open grid %s\n", GRIDFILE); + return false; + } + + uint8_t size; + float radius; + + if(fread(&size, sizeof(uint8_t), 1, fp) != 1) { + stream->printf("error:Failed to read grid size\n"); + fclose(fp); + return false; + } + + if(size != grid_size) { + stream->printf("error:grid size is different read %d - config %d\n", size, grid_size); + fclose(fp); + return false; + } + + if(fread(&radius, sizeof(float), 1, fp) != 1) { + stream->printf("error:Failed to read grid radius\n"); + fclose(fp); + return false; + } + + if(radius != grid_radius) { + stream->printf("warning:grid radius is different read %f - config %f, overriding config\n", radius, grid_radius); + grid_radius= radius; + } + + for (int y = 0; y < grid_size; y++) { + for (int x = 0; x < grid_size; x++) { + if(fread(&grid[x + (grid_size*y)], sizeof(float), 1, fp) != 1) { + stream->printf("error:Failed to read grid\n"); + fclose(fp); + return false; + } + } + } + stream->printf("grid loaded, radius: %f, size: %d\n", grid_radius, grid_size); + fclose(fp); + return true; +} + +bool DeltaGridStrategy::probe_grid(int n, float radius, StreamOutput *stream) +{ + if(n < 5) { + stream->printf("Need at least a 5x5 grid to probe\n"); + return true; + } + + float initial_z = findBed(); + if(isnan(initial_z)) return false; + + float d= ((radius*2) / (n - 1)); + + for (int c = 0; c < n; ++c) { + float y = -radius + d*c; + for (int r = 0; r < n; ++r) { + float x = -radius + d*r; + // Avoid probing the corners (outside the round or hexagon print surface) on a delta printer. + float distance_from_center = sqrtf(x*x + y*y); + float z= 0.0F; + if ((!is_square && (distance_from_center <= radius)) || + (is_square && (x < -x_max || x > x_max || y < -y_max || y > y_max))) { + float mm; + if(!zprobe->doProbeAt(mm, x, y)) return false; + z = zprobe->getProbeHeight() - mm; + } + stream->printf("%8.4f ", z); + } + stream->printf("\n"); + } + return true; +} + +// taken from Oskars PR #713 +bool DeltaGridStrategy::probe_spiral(int n, float radius, StreamOutput *stream) +{ + float a = radius / (2 * sqrtf(n * M_PI)); + float step_length = radius * radius / (2 * a * n); + + float initial_z = findBed(); + if(isnan(initial_z)) return false; + + auto theta = [a](float length) {return sqrtf(2*length/a); }; + + float maxz= NAN, minz= NAN; + for (int i = 0; i < n; i++) { + float angle = theta(i * step_length); + float r = angle * a; + // polar to cartesian + float x = r * cosf(angle); + float y = r * sinf(angle); + + float mm; + if (!zprobe->doProbeAt(mm, x, y)) return false; + float z = zprobe->getProbeHeight() - mm; + stream->printf("PROBE: X%1.4f, Y%1.4f, Z%1.4f\n", x, y, z); + if(isnan(maxz) || z > maxz) maxz= z; + if(isnan(minz) || z < minz) minz= z; + } + + stream->printf("max: %1.4f, min: %1.4f, delta: %1.4f\n", maxz, minz, maxz-minz); + return true; +} + +bool DeltaGridStrategy::handleGcode(Gcode *gcode) +{ + if(gcode->has_g) { + if (gcode->g == 29) { // do a probe to test flatness + // first wait for an empty queue i.e. no moves left + THEKERNEL->conveyor->wait_for_idle(); + + int n= gcode->has_letter('I') ? gcode->get_value('I') : 0; + float radius = grid_radius; + if(gcode->has_letter('J')) radius = gcode->get_value('J'); // override default probe radius + if(gcode->subcode == 1){ + if(n==0) n= 50; + probe_spiral(n, radius, gcode->stream); + }else{ + if(n==0) n= 7; + probe_grid(n, radius, gcode->stream); + } + + return true; + + } else if( gcode->g == 31 ) { // do a grid probe + // first wait for an empty queue i.e. no moves left + THEKERNEL->conveyor->wait_for_idle(); + + if(!doProbe(gcode)) { + gcode->stream->printf("Probe failed to complete, check the initial probe height and/or initial_height settings\n"); + } else { + gcode->stream->printf("Probe completed\n"); + } + return true; + } + + } else if(gcode->has_m) { + if(gcode->m == 370 || gcode->m == 561) { // M370: Clear bed, M561: Set Identity Transform + // delete the compensationTransform in robot + setAdjustFunction(false); + reset_bed_level(); + gcode->stream->printf("grid cleared and disabled\n"); + return true; + + } else if(gcode->m == 374) { // M374: Save grid, M374.1: delete saved grid + if(gcode->subcode == 1) { + remove(GRIDFILE); + gcode->stream->printf("%s deleted\n", GRIDFILE); + } else { + save_grid(gcode->stream); + } + + return true; + + } else if(gcode->m == 375) { // M375: load grid, M375.1 display grid + if(gcode->subcode == 1) { + print_bed_level(gcode->stream); + } else { + if(load_grid(gcode->stream)) setAdjustFunction(true); + } + return true; + + } else if(gcode->m == 565) { // M565: Set Z probe offsets + float x = 0, y = 0, z = 0; + if(gcode->has_letter('X')) x = gcode->get_value('X'); + if(gcode->has_letter('Y')) y = gcode->get_value('Y'); + if(gcode->has_letter('Z')) z = gcode->get_value('Z'); + probe_offsets = std::make_tuple(x, y, z); + return true; + + } else if(gcode->m == 500 || gcode->m == 503) { // M500 save, M503 display + float x, y, z; + std::tie(x, y, z) = probe_offsets; + gcode->stream->printf(";Probe offsets:\nM565 X%1.5f Y%1.5f Z%1.5f\n", x, y, z); + if(save) { + if(!isnan(grid[0])) gcode->stream->printf(";Load saved grid\nM375\n"); + else if(gcode->m == 503) gcode->stream->printf(";WARNING No grid to save\n"); + } + return true; + } + } + + return false; +} + +// These are convenience defines to keep the code as close to the original as possible it also saves memory and flash +// set the rectangle in which to probe +#define LEFT_PROBE_BED_POSITION (-grid_radius) +#define RIGHT_PROBE_BED_POSITION (grid_radius) +#define BACK_PROBE_BED_POSITION (grid_radius) +#define FRONT_PROBE_BED_POSITION (-grid_radius) + +// probe at the points of a lattice grid +#define AUTO_BED_LEVELING_GRID_X ((RIGHT_PROBE_BED_POSITION - LEFT_PROBE_BED_POSITION) / (grid_size - 1)) +#define AUTO_BED_LEVELING_GRID_Y ((BACK_PROBE_BED_POSITION - FRONT_PROBE_BED_POSITION) / (grid_size - 1)) + +#define X_PROBE_OFFSET_FROM_EXTRUDER std::get<0>(probe_offsets) +#define Y_PROBE_OFFSET_FROM_EXTRUDER std::get<1>(probe_offsets) +#define Z_PROBE_OFFSET_FROM_EXTRUDER std::get<2>(probe_offsets) + +void DeltaGridStrategy::setAdjustFunction(bool on) +{ + if(on) { + // set the compensationTransform in robot + THEROBOT->compensationTransform = [this](float target[3]) { doCompensation(target); }; + } else { + // clear it + THEROBOT->compensationTransform = nullptr; + } +} + +float DeltaGridStrategy::findBed() +{ + if (do_home) zprobe->home(); + // move to an initial position fast so as to not take all day, we move down max_z - initial_height, which is set in config, default 10mm + float deltaz = initial_height; + zprobe->coordinated_move(NAN, NAN, deltaz, zprobe->getFastFeedrate()); + zprobe->coordinated_move(0, 0, NAN, zprobe->getFastFeedrate()); // move to 0,0 + + // find bed at 0,0 run at slow rate so as to not hit bed hard + float mm; + if(!zprobe->run_probe(mm, false)) return NAN; + + // leave the probe zprobe->getProbeHeight() above bed + zprobe->return_probe(mm); + + float dz= zprobe->getProbeHeight() - mm; + zprobe->coordinated_move(NAN, NAN, dz, zprobe->getFastFeedrate(), true); // relative move + + return mm + deltaz - zprobe->getProbeHeight(); // distance to move from home to 5mm above bed +} + +bool DeltaGridStrategy::doProbe(Gcode *gc) +{ + gc->stream->printf("Delta Grid Probe...\n"); + setAdjustFunction(false); + reset_bed_level(); + + if(gc->has_letter('J')) grid_radius = gc->get_value('J'); // override default probe radius, will get saved + + float radius = grid_radius; + // find bed, and leave probe probe height above bed + float initial_z = findBed(); + if(isnan(initial_z)) { + gc->stream->printf("Finding bed failed, check the maxz and initial height settings\n"); + return false; + } + + gc->stream->printf("Probe start ht is %f mm, probe radius is %f mm, grid size is %dx%d\n", initial_z, radius, grid_size, grid_size); + + // do first probe for 0,0 + float mm; + if(!zprobe->doProbeAt(mm, -X_PROBE_OFFSET_FROM_EXTRUDER, -Y_PROBE_OFFSET_FROM_EXTRUDER)) return false; + float z_reference = zprobe->getProbeHeight() - mm; // this should be zero + gc->stream->printf("probe at 0,0 is %f mm\n", z_reference); + + // probe all the points in the grid within the given radius + for (int yCount = 0; yCount < grid_size; yCount++) { + float yProbe = FRONT_PROBE_BED_POSITION + AUTO_BED_LEVELING_GRID_Y * yCount; + int xStart, xStop, xInc; + if (yCount % 2) { + xStart = 0; + xStop = grid_size; + xInc = 1; + } else { + xStart = grid_size - 1; + xStop = -1; + xInc = -1; + } + + for (int xCount = xStart; xCount != xStop; xCount += xInc) { + float xProbe = LEFT_PROBE_BED_POSITION + AUTO_BED_LEVELING_GRID_X * xCount; + + // avoid probing outside of x min/max on a cartesian + if (is_square) + { + if (xProbe < -x_max || xProbe > x_max || yProbe < -y_max || yProbe > y_max) continue; + } + else + { + // Avoid probing the corners (outside the round or hexagon print surface) on a delta printer. + float distance_from_center = sqrtf(xProbe * xProbe + yProbe * yProbe); + if (distance_from_center > radius) continue; + } + + if(!zprobe->doProbeAt(mm, xProbe - X_PROBE_OFFSET_FROM_EXTRUDER, yProbe - Y_PROBE_OFFSET_FROM_EXTRUDER)) return false; + float measured_z = zprobe->getProbeHeight() - mm - z_reference; // this is the delta z from bed at 0,0 + gc->stream->printf("DEBUG: X%1.4f, Y%1.4f, Z%1.4f\n", xProbe, yProbe, measured_z); + grid[xCount + (grid_size * yCount)] = measured_z; + } + } + + extrapolate_unprobed_bed_level(); + print_bed_level(gc->stream); + + setAdjustFunction(true); + + return true; +} + +void DeltaGridStrategy::extrapolate_one_point(int x, int y, int xdir, int ydir) +{ + if (!isnan(grid[x + (grid_size*y)])) { + return; // Don't overwrite good values. + } + float a = 2 * grid[(x + xdir) + (y*grid_size)] - grid[(x + xdir * 2) + (y*grid_size)]; // Left to right. + float b = 2 * grid[x + ((y + ydir) * grid_size)] - grid[x + ((y + ydir * 2) * grid_size)]; // Front to back. + float c = 2 * grid[(x + xdir) + ((y + ydir) * grid_size)] - grid[(x + xdir * 2) + ((y + ydir * 2) * grid_size)]; // Diagonal. + float median = c; // Median is robust (ignores outliers). + if (a < b) { + if (b < c) median = b; + if (c < a) median = a; + } else { // b <= a + if (c < b) median = b; + if (a < c) median = a; + } + grid[x + (grid_size*y)] = median; +} + +// Fill in the unprobed points (corners of circular print surface) +// using linear extrapolation, away from the center. +void DeltaGridStrategy::extrapolate_unprobed_bed_level() +{ + int half = (grid_size - 1) / 2; + for (int y = 0; y <= half; y++) { + for (int x = 0; x <= half; x++) { + if (x + y < 3) continue; + extrapolate_one_point(half - x, half - y, x > 1 ? +1 : 0, y > 1 ? +1 : 0); + extrapolate_one_point(half + x, half - y, x > 1 ? -1 : 0, y > 1 ? +1 : 0); + extrapolate_one_point(half - x, half + y, x > 1 ? +1 : 0, y > 1 ? -1 : 0); + extrapolate_one_point(half + x, half + y, x > 1 ? -1 : 0, y > 1 ? -1 : 0); + } + } +} + +void DeltaGridStrategy::doCompensation(float target[3]) +{ + // Adjust print surface height by linear interpolation over the bed_level array. + int half = (grid_size - 1) / 2; + float grid_x = std::max(0.001F - half, std::min(half - 0.001F, target[X_AXIS] / AUTO_BED_LEVELING_GRID_X)); + float grid_y = std::max(0.001F - half, std::min(half - 0.001F, target[Y_AXIS] / AUTO_BED_LEVELING_GRID_Y)); + int floor_x = floorf(grid_x); + int floor_y = floorf(grid_y); + float ratio_x = grid_x - floor_x; + float ratio_y = grid_y - floor_y; + float z1 = grid[(floor_x + half) + ((floor_y + half) * grid_size)]; + float z2 = grid[(floor_x + half) + ((floor_y + half + 1) * grid_size)]; + float z3 = grid[(floor_x + half + 1) + ((floor_y + half) * grid_size)]; + float z4 = grid[(floor_x + half + 1) + ((floor_y + half + 1) * grid_size)]; + float left = (1 - ratio_y) * z1 + ratio_y * z2; + float right = (1 - ratio_y) * z3 + ratio_y * z4; + float offset = (1 - ratio_x) * left + ratio_x * right; + + target[Z_AXIS] += offset; + + + /* + THEKERNEL->streams->printf("//DEBUG: TARGET: %f, %f, %f\n", target[0], target[1], target[2]); + THEKERNEL->streams->printf("//DEBUG: grid_x= %f\n", grid_x); + THEKERNEL->streams->printf("//DEBUG: grid_y= %f\n", grid_y); + THEKERNEL->streams->printf("//DEBUG: floor_x= %d\n", floor_x); + THEKERNEL->streams->printf("//DEBUG: floor_y= %d\n", floor_y); + THEKERNEL->streams->printf("//DEBUG: ratio_x= %f\n", ratio_x); + THEKERNEL->streams->printf("//DEBUG: ratio_y= %f\n", ratio_y); + THEKERNEL->streams->printf("//DEBUG: z1= %f\n", z1); + THEKERNEL->streams->printf("//DEBUG: z2= %f\n", z2); + THEKERNEL->streams->printf("//DEBUG: z3= %f\n", z3); + THEKERNEL->streams->printf("//DEBUG: z4= %f\n", z4); + THEKERNEL->streams->printf("//DEBUG: left= %f\n", left); + THEKERNEL->streams->printf("//DEBUG: right= %f\n", right); + THEKERNEL->streams->printf("//DEBUG: offset= %f\n", offset); + */ +} + + +// Print calibration results for plotting or manual frame adjustment. +void DeltaGridStrategy::print_bed_level(StreamOutput *stream) +{ + for (int y = 0; y < grid_size; y++) { + for (int x = 0; x < grid_size; x++) { + stream->printf("%7.4f ", grid[x + (grid_size*y)]); + } + stream->printf("\n"); + } +} + +// Reset calibration results to zero. +void DeltaGridStrategy::reset_bed_level() +{ + for (int y = 0; y < grid_size; y++) { + for (int x = 0; x < grid_size; x++) { + grid[x + (grid_size*y)] = NAN; + } + } +} diff --git a/src/modules/tools/zprobe/DeltaGridStrategy.h b/src/modules/tools/zprobe/DeltaGridStrategy.h new file mode 100644 index 00000000..bfa9d579 --- /dev/null +++ b/src/modules/tools/zprobe/DeltaGridStrategy.h @@ -0,0 +1,50 @@ +#pragma once + +#include "LevelingStrategy.h" + +#include +#include + +#define delta_grid_leveling_strategy_checksum CHECKSUM("delta-grid") + +class StreamOutput; +class Gcode; + +class DeltaGridStrategy : public LevelingStrategy +{ +public: + DeltaGridStrategy(ZProbe *zprobe); + ~DeltaGridStrategy(); + bool handleGcode(Gcode* gcode); + bool handleConfig(); + +private: + + void extrapolate_one_point(int x, int y, int xdir, int ydir); + void extrapolate_unprobed_bed_level(); + bool doProbe(Gcode *gc); + float findBed(); + void setAdjustFunction(bool on); + void print_bed_level(StreamOutput *stream); + void doCompensation(float target[3]); + void reset_bed_level(); + void save_grid(StreamOutput *stream); + bool load_grid(StreamOutput *stream); + bool probe_spiral(int n, float radius, StreamOutput *stream); + bool probe_grid(int n, float radius, StreamOutput *stream); + + float initial_height; + float tolerance; + + float *grid; + float grid_radius; + std::tuple probe_offsets; + uint8_t grid_size; + float x_max,y_max; + + struct { + bool save:1; + bool do_home:1; + bool is_square:1; + }; +}; diff --git a/src/modules/tools/zprobe/LevelingStrategy.h b/src/modules/tools/zprobe/LevelingStrategy.h new file mode 100644 index 00000000..6460611f --- /dev/null +++ b/src/modules/tools/zprobe/LevelingStrategy.h @@ -0,0 +1,24 @@ +/* + * A strategy called from ZProbe to handle a strategy for leveling + * examples are delta calibration, three point bed leveling, z height map + */ + +#ifndef _LEVELINGSTRATEGY +#define _LEVELINGSTRATEGY + +class ZProbe; +class Gcode; + +class LevelingStrategy +{ +public: + LevelingStrategy(ZProbe* zprobe) : zprobe(zprobe){}; + virtual ~LevelingStrategy(){}; + virtual bool handleGcode(Gcode* gcode)= 0; + virtual bool handleConfig()= 0; + +protected: + ZProbe *zprobe; + +}; +#endif diff --git a/src/modules/tools/zprobe/Plane3D.cpp b/src/modules/tools/zprobe/Plane3D.cpp new file mode 100644 index 00000000..38ad0138 --- /dev/null +++ b/src/modules/tools/zprobe/Plane3D.cpp @@ -0,0 +1,47 @@ +#include "Plane3D.h" + +Plane3D::Plane3D(const Vector3 &v1, const Vector3 &v2, const Vector3 &v3) +{ + // get the normal of the plane + Vector3 ab = v1.sub(v2); + Vector3 ac = v1.sub(v3); + + Vector3 cp = ab.cross(ac); + normal = cp.unit(); + + // ax+by+cz+d=0 + // solve for d + d = -normal.dot(v1); +} + +typedef union { float f; uint32_t u; } conv_t; +// ctor used to restore a saved plane +Plane3D::Plane3D(uint32_t a, uint32_t b, uint32_t c, uint32_t d) +{ + conv_t ca, cb, cc, cd; + ca.u= a; cb.u= b; cc.u= c; cd.u= d; + this->normal = Vector3(ca.f, cb.f, cc.f); + this->d= cd.f; +} + +void Plane3D::encode(uint32_t& a, uint32_t& b, uint32_t& c, uint32_t& d) +{ + conv_t ca, cb, cc, cd; + ca.f= this->normal[0]; + cb.f= this->normal[1]; + cc.f= this->normal[2]; + cd.f= this->d; + a= ca.u; b= cb.u; c= cc.u; d= cd.u; +} + +// solve for z given x and y +// z= (-ax - by - d)/c +float Plane3D::getz(float x, float y) +{ + return ((-normal[0] * x) - (normal[1] * y) - d) / normal[2]; +} + +Vector3 Plane3D::getNormal() const +{ + return normal; +} diff --git a/src/modules/tools/zprobe/Plane3D.h b/src/modules/tools/zprobe/Plane3D.h new file mode 100644 index 00000000..2dbc0099 --- /dev/null +++ b/src/modules/tools/zprobe/Plane3D.h @@ -0,0 +1,23 @@ +#ifndef __PLANE3D_H +#define __PLANE3D_H + +#include "Vector3.h" + +#include + +// define a plane given three points +class Plane3D +{ +private: + Vector3 normal; + float d; + +public: + Plane3D(const Vector3 &v1, const Vector3 &v2, const Vector3 &v3); + Plane3D(uint32_t a, uint32_t b, uint32_t c, uint32_t d); + float getz(float x, float y); + Vector3 getNormal() const; + void encode(uint32_t& a, uint32_t& b, uint32_t& c, uint32_t& d); +}; + +#endif diff --git a/src/modules/tools/zprobe/ThreePointStrategy.cpp b/src/modules/tools/zprobe/ThreePointStrategy.cpp new file mode 100644 index 00000000..9b9d2366 --- /dev/null +++ b/src/modules/tools/zprobe/ThreePointStrategy.cpp @@ -0,0 +1,399 @@ +/* + Author: Jim Morris (wolfmanjm@gmail.com) + License: GPL3 or better see + + Summary + ------- + Probes three user specified points on the bed and determines the plane of the bed relative to the probe. + as the head moves in X and Y it will adjust Z to keep the head tram with the bed. + + Configuration + ------------- + The strategy must be enabled in the cofnig as well as zprobe. + + leveling-strategy.three-point-leveling.enable true + + Three probe points must be defined, these are best if they are the three points of an equilateral triangle, as far apart as possible. + They can be defined in the config file as:- + + leveling-strategy.three-point-leveling.point1 100.0,0.0 # the first probe point (x,y) + leveling-strategy.three-point-leveling.point2 200.0,200.0 # the second probe point (x,y) + leveling-strategy.three-point-leveling.point3 0.0,200.0 # the third probe point (x,y) + + or they may be defined (and saved with M500) using M557 P0 X30 Y40.5 where P is 0,1,2 + + probe offsets from the nozzle or tool head can be defined with + + leveling-strategy.three-point-leveling.probe_offsets 0,0,0 # probe offsetrs x,y,z + + they may also be set with M565 X0 Y0 Z0 + + To force homing in X and Y before G32 does the probe the following can be set in config, this is the default + + leveling-strategy.three-point-leveling.home_first true # disable by setting to false + + The probe tolerance can be set using the config line + + leveling-strategy.three-point-leveling.tolerance 0.03 # the probe tolerance in mm, default is 0.03mm + + + Usage + ----- + G29 probes the three probe points and reports the Z at each point, if a plane is active it will be used to level the probe. + G32 probes the three probe points and defines the bed plane, this will remain in effect until reset or M561 + G31 reports the status + + M557 defines the probe points + M561 clears the plane and the bed leveling is disabled until G32 is run again + M565 defines the probe offsets from the nozzle or tool head + + M500 saves the probe points and the probe offsets + M503 displays the current settings +*/ + +#include "ThreePointStrategy.h" +#include "Kernel.h" +#include "Config.h" +#include "Robot.h" +#include "StreamOutputPool.h" +#include "Gcode.h" +#include "checksumm.h" +#include "ConfigValue.h" +#include "PublicDataRequest.h" +#include "PublicData.h" +#include "Conveyor.h" +#include "ZProbe.h" +#include "Plane3D.h" +#include "nuts_bolts.h" + +#include +#include +#include +#include + +#define probe_point_1_checksum CHECKSUM("point1") +#define probe_point_2_checksum CHECKSUM("point2") +#define probe_point_3_checksum CHECKSUM("point3") +#define probe_offsets_checksum CHECKSUM("probe_offsets") +#define home_checksum CHECKSUM("home_first") +#define tolerance_checksum CHECKSUM("tolerance") +#define save_plane_checksum CHECKSUM("save_plane") + +ThreePointStrategy::ThreePointStrategy(ZProbe *zprobe) : LevelingStrategy(zprobe) +{ + for (int i = 0; i < 3; ++i) { + probe_points[i] = std::make_tuple(NAN, NAN); + } + plane = nullptr; +} + +ThreePointStrategy::~ThreePointStrategy() +{ + delete plane; +} + +bool ThreePointStrategy::handleConfig() +{ + // format is xxx,yyy for the probe points + std::string p1 = THEKERNEL->config->value(leveling_strategy_checksum, three_point_leveling_strategy_checksum, probe_point_1_checksum)->by_default("")->as_string(); + std::string p2 = THEKERNEL->config->value(leveling_strategy_checksum, three_point_leveling_strategy_checksum, probe_point_2_checksum)->by_default("")->as_string(); + std::string p3 = THEKERNEL->config->value(leveling_strategy_checksum, three_point_leveling_strategy_checksum, probe_point_3_checksum)->by_default("")->as_string(); + if(!p1.empty()) probe_points[0] = parseXY(p1.c_str()); + if(!p2.empty()) probe_points[1] = parseXY(p2.c_str()); + if(!p3.empty()) probe_points[2] = parseXY(p3.c_str()); + + // Probe offsets xxx,yyy,zzz + std::string po = THEKERNEL->config->value(leveling_strategy_checksum, three_point_leveling_strategy_checksum, probe_offsets_checksum)->by_default("0,0,0")->as_string(); + this->probe_offsets= parseXYZ(po.c_str()); + + this->home= THEKERNEL->config->value(leveling_strategy_checksum, three_point_leveling_strategy_checksum, home_checksum)->by_default(true)->as_bool(); + this->tolerance= THEKERNEL->config->value(leveling_strategy_checksum, three_point_leveling_strategy_checksum, tolerance_checksum)->by_default(0.03F)->as_number(); + this->save= THEKERNEL->config->value(leveling_strategy_checksum, three_point_leveling_strategy_checksum, save_plane_checksum)->by_default(false)->as_bool(); + return true; +} + +bool ThreePointStrategy::handleGcode(Gcode *gcode) +{ + if(gcode->has_g) { + // G code processing + if(gcode->g == 29) { // test probe points for level + if(!test_probe_points(gcode)) { + gcode->stream->printf("Probe failed to complete, probe not triggered or other error\n"); + } + return true; + + } else if( gcode->g == 31 ) { // report status + if(this->plane == nullptr) { + gcode->stream->printf("Bed leveling plane is not set\n"); + }else{ + gcode->stream->printf("Bed leveling plane normal= %f, %f, %f\n", plane->getNormal()[0], plane->getNormal()[1], plane->getNormal()[2]); + } + gcode->stream->printf("Probe is %s\n", zprobe->getProbeStatus() ? "Triggered" : "Not triggered"); + return true; + + } else if( gcode->g == 32 ) { // three point probe + // first wait for an empty queue i.e. no moves left + THEKERNEL->conveyor->wait_for_idle(); + + // clear any existing plane and compensation + delete this->plane; + this->plane= nullptr; + setAdjustFunction(false); + + if(!doProbing(gcode->stream)) { + gcode->stream->printf("Probe failed to complete, probe not triggered or other error\n"); + } else { + gcode->stream->printf("Probe completed, bed plane defined\n"); + } + return true; + } + + } else if(gcode->has_m) { + if(gcode->m == 557) { // M557 - set probe points eg M557 P0 X30 Y40.5 where P is 0,1,2 + int idx = 0; + float x = NAN, y = NAN; + if(gcode->has_letter('P')) idx = gcode->get_value('P'); + if(gcode->has_letter('X')) x = gcode->get_value('X'); + if(gcode->has_letter('Y')) y = gcode->get_value('Y'); + if(idx >= 0 && idx <= 2) { + probe_points[idx] = std::make_tuple(x, y); + }else{ + gcode->stream->printf("only 3 probe points allowed P0-P2\n"); + } + return true; + + } else if(gcode->m == 561) { // M561: Set Identity Transform with no parameters, set the saved plane if A B C D are given + delete this->plane; + if(gcode->get_num_args() == 0) { + this->plane= nullptr; + // delete the compensationTransform in robot + setAdjustFunction(false); + gcode->stream->printf("saved plane cleared\n"); + }else{ + // smoothie specific way to restore a saved plane + uint32_t a,b,c,d; + a=b=c=d= 0; + if(gcode->has_letter('A')) a = gcode->get_uint('A'); + if(gcode->has_letter('B')) b = gcode->get_uint('B'); + if(gcode->has_letter('C')) c = gcode->get_uint('C'); + if(gcode->has_letter('D')) d = gcode->get_uint('D'); + this->plane= new Plane3D(a, b, c, d); + setAdjustFunction(true); + } + return true; + + } else if(gcode->m == 565) { // M565: Set Z probe offsets + float x= 0, y= 0, z= 0; + if(gcode->has_letter('X')) x = gcode->get_value('X'); + if(gcode->has_letter('Y')) y = gcode->get_value('Y'); + if(gcode->has_letter('Z')) z = gcode->get_value('Z'); + probe_offsets = std::make_tuple(x, y, z); + return true; + + } else if(gcode->m == 500 || gcode->m == 503) { // M500 save, M503 display + float x, y, z; + gcode->stream->printf(";Probe points:\n"); + for (int i = 0; i < 3; ++i) { + std::tie(x, y) = probe_points[i]; + gcode->stream->printf("M557 P%d X%1.5f Y%1.5f\n", i, x, y); + } + gcode->stream->printf(";Probe offsets:\n"); + std::tie(x, y, z) = probe_offsets; + gcode->stream->printf("M565 X%1.5f Y%1.5f Z%1.5f\n", x, y, z); + + // encode plane and save if set and M500 and enabled + if(this->save && this->plane != nullptr) { + if(gcode->m == 500) { + uint32_t a, b, c, d; + this->plane->encode(a, b, c, d); + gcode->stream->printf(";Saved bed plane:\nM561 A%lu B%lu C%lu D%lu \n", a, b, c, d); + }else{ + gcode->stream->printf(";The bed plane will be saved on M500\n"); + } + } + return true; + + } + #if 0 + else if(gcode->m == 9999) { + // DEBUG run a test M9999 A B C X Y set Z to A B C and test for point at X Y + Vector3 v[3]; + float x, y, z, a= 0, b= 0, c= 0; + if(gcode->has_letter('A')) a = gcode->get_value('A'); + if(gcode->has_letter('B')) b = gcode->get_value('B'); + if(gcode->has_letter('C')) c = gcode->get_value('C'); + std::tie(x, y) = probe_points[0]; v[0].set(x, y, a); + std::tie(x, y) = probe_points[1]; v[1].set(x, y, b); + std::tie(x, y) = probe_points[2]; v[2].set(x, y, c); + delete this->plane; + this->plane = new Plane3D(v[0], v[1], v[2]); + gcode->stream->printf("plane normal= %f, %f, %f\n", plane->getNormal()[0], plane->getNormal()[1], plane->getNormal()[2]); + x= 0; y=0; + if(gcode->has_letter('X')) x = gcode->get_value('X'); + if(gcode->has_letter('Y')) y = gcode->get_value('Y'); + z= getZOffset(x, y); + gcode->stream->printf("z= %f\n", z); + // tell robot to adjust z on each move + setAdjustFunction(true); + return true; + } + #endif + } + + return false; +} + +void ThreePointStrategy::homeXY() +{ + Gcode gc("G28 X0 Y0", &(StreamOutput::NullStream)); + THEKERNEL->call_event(ON_GCODE_RECEIVED, &gc); +} + +bool ThreePointStrategy::doProbing(StreamOutput *stream) +{ + float x, y; + // check the probe points have been defined + for (int i = 0; i < 3; ++i) { + std::tie(x, y) = probe_points[i]; + if(isnan(x) || isnan(y)) { + stream->printf("Probe point P%d has not been defined, use M557 P%d Xnnn Ynnn to define it\n", i, i); + return false; + } + } + + // optionally home XY axis first, but allow for manual homing + if(this->home) + homeXY(); + + // move to the first probe point + std::tie(x, y) = probe_points[0]; + // offset by the probe XY offset + x -= std::get(this->probe_offsets); + y -= std::get(this->probe_offsets); + zprobe->coordinated_move(x, y, NAN, zprobe->getFastFeedrate()); + + // for now we use probe to find bed and not the Z min endstop + // the first probe point becomes Z == 0 effectively so if we home Z or manually set z after this, it needs to be at the first probe point + + // TODO this needs to be configurable to use min z or probe + + // find bed via probe + float mm; + if(!zprobe->run_probe(mm)) return false; + + // TODO if using probe then we probably need to set Z to 0 at first probe point, but take into account probe offset from head + THEROBOT->reset_axis_position(std::get(this->probe_offsets), Z_AXIS); + + // move up to specified probe start position + zprobe->coordinated_move(NAN, NAN, zprobe->getProbeHeight(), zprobe->getSlowFeedrate()); // move to probe start position + + // probe the three points + Vector3 v[3]; + for (int i = 0; i < 3; ++i) { + std::tie(x, y) = probe_points[i]; + // offset moves by the probe XY offset + float z = zprobe->probeDistance(x-std::get(this->probe_offsets), y-std::get(this->probe_offsets)); + if(isnan(z)) return false; // probe failed + z= zprobe->getProbeHeight() - z; // relative distance between the probe points, lower is negative z + stream->printf("DEBUG: P%d:%1.4f\n", i, z); + v[i] = Vector3(x, y, z); + } + + // if first point is not within tolerance report it, it should ideally be 0 + if(fabsf(v[0][2]) > this->tolerance) { + stream->printf("WARNING: probe is not within tolerance: %f > %f\n", fabsf(v[0][2]), this->tolerance); + } + + // define the plane + delete this->plane; + // check tolerance level here default 0.03mm + auto mmx = std::minmax({v[0][2], v[1][2], v[2][2]}); + if((mmx.second - mmx.first) <= this->tolerance) { + this->plane= nullptr; // plane is flat no need to do anything + stream->printf("DEBUG: flat plane\n"); + // clear the compensationTransform in robot + setAdjustFunction(false); + + }else{ + this->plane = new Plane3D(v[0], v[1], v[2]); + stream->printf("DEBUG: plane normal= %f, %f, %f\n", plane->getNormal()[0], plane->getNormal()[1], plane->getNormal()[2]); + setAdjustFunction(true); + } + + return true; +} + +// Probes the 3 points and reports heights +bool ThreePointStrategy::test_probe_points(Gcode *gcode) +{ + // check the probe points have been defined + float max_delta= 0; + float last_z= NAN; + for (int i = 0; i < 3; ++i) { + float x, y; + std::tie(x, y) = probe_points[i]; + if(isnan(x) || isnan(y)) { + gcode->stream->printf("Probe point P%d has not been defined, use M557 P%d Xnnn Ynnn to define it\n", i, i); + return false; + } + + float z = zprobe->probeDistance(x-std::get(this->probe_offsets), y-std::get(this->probe_offsets)); + if(isnan(z)) return false; // probe failed + gcode->stream->printf("X:%1.4f Y:%1.4f Z:%1.4f\n", x, y, z); + + if(isnan(last_z)) { + last_z= z; + }else{ + max_delta= std::max(max_delta, fabsf(z-last_z)); + } + } + + gcode->stream->printf("max delta: %f\n", max_delta); + + return true; +} + +void ThreePointStrategy::setAdjustFunction(bool on) +{ + if(on) { + // set the compensationTransform in robot + THEROBOT->compensationTransform= [this](float target[3]) { target[2] += this->plane->getz(target[0], target[1]); }; + }else{ + // clear it + THEROBOT->compensationTransform= nullptr; + } +} + +// find the Z offset for the point on the plane at x, y +float ThreePointStrategy::getZOffset(float x, float y) +{ + if(this->plane == nullptr) return NAN; + return this->plane->getz(x, y); +} + +// parse a "X,Y" string return x,y +std::tuple ThreePointStrategy::parseXY(const char *str) +{ + float x = NAN, y = NAN; + char *p; + x = strtof(str, &p); + if(p + 1 < str + strlen(str)) { + y = strtof(p + 1, nullptr); + } + return std::make_tuple(x, y); +} + +// parse a "X,Y,Z" string return x,y,z tuple +std::tuple ThreePointStrategy::parseXYZ(const char *str) +{ + float x = 0, y = 0, z= 0; + char *p; + x = strtof(str, &p); + if(p + 1 < str + strlen(str)) { + y = strtof(p + 1, &p); + if(p + 1 < str + strlen(str)) { + z = strtof(p + 1, nullptr); + } + } + return std::make_tuple(x, y, z); +} diff --git a/src/modules/tools/zprobe/ThreePointStrategy.h b/src/modules/tools/zprobe/ThreePointStrategy.h new file mode 100644 index 00000000..652f4c20 --- /dev/null +++ b/src/modules/tools/zprobe/ThreePointStrategy.h @@ -0,0 +1,41 @@ +#ifndef _THREEPOINTSTRATEGY +#define _THREEPOINTSTRATEGY + +#include "LevelingStrategy.h" + +#include +#include + +#define three_point_leveling_strategy_checksum CHECKSUM("three-point-leveling") + +class StreamOutput; +class Plane3D; + +class ThreePointStrategy : public LevelingStrategy +{ +public: + ThreePointStrategy(ZProbe *zprobe); + ~ThreePointStrategy(); + bool handleGcode(Gcode* gcode); + bool handleConfig(); + float getZOffset(float x, float y); + +private: + void homeXY(); + bool doProbing(StreamOutput *stream); + std::tuple parseXY(const char *str); + std::tuple parseXYZ(const char *str); + void setAdjustFunction(bool); + bool test_probe_points(Gcode *gcode); + + std::tuple probe_offsets; + std::tuple probe_points[3]; + Plane3D *plane; + struct { + bool home:1; + bool save:1; + }; + float tolerance; +}; + +#endif diff --git a/src/modules/tools/zprobe/ZGridStrategy.cpp b/src/modules/tools/zprobe/ZGridStrategy.cpp new file mode 100644 index 00000000..97d4d6ac --- /dev/null +++ b/src/modules/tools/zprobe/ZGridStrategy.cpp @@ -0,0 +1,711 @@ +/* + Author: Quentin Harley (quentin.harley@gmail.com) + License: GPL3 or better see + + Summary + ------- + Probes user defined amount of calculated points on the bed and creates compensation grid data of the bed surface. + Bilinear + As the head moves in X and Y it will adjust Z to keep the head level with the bed. + + Configuration + ------------- + The strategy must be enabled in the config as well as zprobe. + + leveling-strategy.ZGrid-leveling.enable true + + The bed size limits must be defined, in order for the module to calculate the calibration points + + leveling-strategy.ZGrid-leveling.bed_x 200 + leveling-strategy.ZGrid-leveling.bed_y 200 + + Machine height, used to determine probe attachment point (bed_z / 2) + + leveling-strategy.ZGrid-leveling.bed_z 20 + + Probe attachement point, if defined, overrides the calculated point + leveling-strategy.ZGrid-leveling.probe_x 0 + leveling-strategy.ZGrid-leveling.probe_y 0 + leveling-strategy.ZGrid-leveling.probe_z 30 + + + Configure for Machines with bed 0:0 at center of platform + leveling-strategy.ZGrid-leveling.bed_zero false + + configure for Machines with circular beds + leveling-strategy.ZGrid-leveling.bed_circular false + + + The number of divisions for X and Y should be defined + + leveling-strategy.ZGrid-leveling.rows 7 # X divisions (Default 5) + leveling-strategy.ZGrid-leveling.cols 9 # Y divisions (Default 5) + + + The probe offset should be defined, default to zero offset + + leveling-strategy.ZGrid-leveling.probe_offsets 0,0,16.3 + + The machine can be told to wait for probe attachment and confirmation + + leveling-strategy.ZGrid-leveling.wait_for_probe true + + The machine can be told to home in one of the following modes: + + leveling-strategy.ZGrid-leveling.home_before_probe homexyz; # nohome homexy homexyz (default) + + + Slow feedrate can be defined for probe positioning speed. Note this is not Probing slow rate - it can be set to a fast speed if required. + + leveling-strategy.ZGrid-leveling.slow_feedrate 100 # ZGrid probe positioning feedrate + + + + Usage + ----- + G32 : probes the probe points and defines the bed ZGrid, this will remain in effect until reset or M370 + G31 : reports the status - Display probe data points + + M370 : clears the ZGrid and the bed levelling is disabled until G32 is run again + M370 X7 Y9 : allocates a new grid size of 7x9 and clears as above + + M371 : moves the head to the next calibration position without saving for manual calibration + M372 : move the head to the next calibration position after saving the current probe point to memory - manual calbration + M373 : completes calibration and enables the Z compensation grid + + M374 : Save the grid to "Zgrid" on SD card + M374 S### : Save custom grid to "Zgrid.###" on SD card + + M375 : Loads grid file "Zgrid" from SD + M375 S### : Load custom grid file "Zgrid.###" + + M565 X### Y### Z### : Set new probe offsets + + M500 : saves the probe offsets + M503 : displays the current settings +*/ + +#include "ZGridStrategy.h" +#include "Kernel.h" +#include "Config.h" +#include "Robot.h" +#include "StreamOutputPool.h" +#include "Gcode.h" +#include "checksumm.h" +#include "ConfigValue.h" +#include "PublicDataRequest.h" +#include "PublicData.h" +#include "EndstopsPublicAccess.h" +#include "Conveyor.h" +#include "ZProbe.h" +#include "libs/FileStream.h" +#include "nuts_bolts.h" +//#include "platform_memory.h" +//#include "MemoryPool.h" +#include "libs/utils.h" + +#include +#include +#include +#include +#include + +#define bed_x_checksum CHECKSUM("bed_x") +#define bed_y_checksum CHECKSUM("bed_y") +#define bed_z_checksum CHECKSUM("bed_z") + +#define probe_x_checksum CHECKSUM("probe_x") +#define probe_y_checksum CHECKSUM("probe_y") +#define probe_z_checksum CHECKSUM("probe_z") + +#define slow_feedrate_checksum CHECKSUM("slow_feedrate") +#define probe_offsets_checksum CHECKSUM("probe_offsets") +#define wait_for_probe_checksum CHECKSUM("wait_for_probe") +#define home_before_probe_checksum CHECKSUM("home_before_probe") +#define center_zero_checksum CHECKSUM("center_zero") +#define circular_bed_checksum CHECKSUM("circular_bed") +#define cal_offset_x_checksum CHECKSUM("cal_offset_x") +#define cal_offset_y_checksum CHECKSUM("cal_offset_y") + +#define NOHOME 0 +#define HOMEXY 1 +#define HOMEXYZ 2 + +#define cols_checksum CHECKSUM("cols") +#define rows_checksum CHECKSUM("rows") + +#define probe_points (this->numRows * this->numCols) + + + +ZGridStrategy::ZGridStrategy(ZProbe *zprobe) : LevelingStrategy(zprobe) +{ + this->cal[X_AXIS] = 0.0f; + this->cal[Y_AXIS] = 0.0f; + this->cal[Z_AXIS] = 30.0f; + + this->in_cal = false; + this->pData = nullptr; +} + +ZGridStrategy::~ZGridStrategy() +{ + // Free program memory for the pData grid + if(this->pData != nullptr) free(this->pData); +} + +bool ZGridStrategy::handleConfig() +{ + this->bed_x = THEKERNEL->config->value(leveling_strategy_checksum, ZGrid_leveling_checksum, bed_x_checksum)->by_default(200.0F)->as_number(); + this->bed_y = THEKERNEL->config->value(leveling_strategy_checksum, ZGrid_leveling_checksum, bed_y_checksum)->by_default(200.0F)->as_number(); + this->bed_z = THEKERNEL->config->value(leveling_strategy_checksum, ZGrid_leveling_checksum, bed_z_checksum)->by_default(20.0F)->as_number(); + + this->probe_x = THEKERNEL->config->value(leveling_strategy_checksum, ZGrid_leveling_checksum, probe_x_checksum)->by_default(this->center_zero ? this->bed_x / 2.0F : 0.0F)->as_number(); + this->probe_y = THEKERNEL->config->value(leveling_strategy_checksum, ZGrid_leveling_checksum, probe_y_checksum)->by_default(this->center_zero ? this->bed_y / 2.0F : 0.0F)->as_number(); + this->probe_z = THEKERNEL->config->value(leveling_strategy_checksum, ZGrid_leveling_checksum, probe_z_checksum)->by_default(this->bed_z / 2.0F)->as_number(); // Do this to keep default settings the same + + this->slow_rate = THEKERNEL->config->value(leveling_strategy_checksum, ZGrid_leveling_checksum, slow_feedrate_checksum)->by_default(20.0F)->as_number(); + + this->numRows = THEKERNEL->config->value(leveling_strategy_checksum, ZGrid_leveling_checksum, rows_checksum)->by_default(5)->as_number(); + this->numCols = THEKERNEL->config->value(leveling_strategy_checksum, ZGrid_leveling_checksum, cols_checksum)->by_default(5)->as_number(); + + this->wait_for_probe = THEKERNEL->config->value(leveling_strategy_checksum, ZGrid_leveling_checksum, wait_for_probe_checksum)->by_default(true)->as_bool(); // Morgan default = true + + std::string home_mode = THEKERNEL->config->value(leveling_strategy_checksum, ZGrid_leveling_checksum, home_before_probe_checksum)->by_default("homexyz")->as_string(); + if (home_mode.compare("nohome") == 0) { + this->home_before_probe = NOHOME; + } + else if (home_mode.compare("homexy") == 0) { + this->home_before_probe = HOMEXY; + } + else { // Default + this->home_before_probe = HOMEXYZ; + } + + + this->center_zero = THEKERNEL->config->value(leveling_strategy_checksum, ZGrid_leveling_checksum, center_zero_checksum)->by_default(false)->as_bool(); + this->circular_bed = THEKERNEL->config->value(leveling_strategy_checksum, ZGrid_leveling_checksum, circular_bed_checksum)->by_default(false)->as_bool(); + + // configures calbration positioning offset. Defaults to 0 for standard cartesian space machines, and to negative half of the current bed size in X and Y + this->cal_offset_x = THEKERNEL->config->value(leveling_strategy_checksum, ZGrid_leveling_checksum, cal_offset_x_checksum)->by_default( this->center_zero ? this->bed_x / -2.0F : 0.0F )->as_number(); + this->cal_offset_y = THEKERNEL->config->value(leveling_strategy_checksum, ZGrid_leveling_checksum, cal_offset_y_checksum)->by_default( this->center_zero ? this->bed_y / -2.0F : 0.0F )->as_number(); + + + // Probe offsets xxx,yyy,zzz + std::string po = THEKERNEL->config->value(leveling_strategy_checksum, ZGrid_leveling_checksum, probe_offsets_checksum)->by_default("0,0,0")->as_string(); + this->probe_offsets= parseXYZ(po.c_str()); + + this->calcConfig(); // Run calculations for Grid size and allocate initial grid memory + + for (int i=0; i<(probe_points); i++){ + this->pData[i] = 0.0F; // Clear the grid + } + + return true; +} + +void ZGridStrategy::calcConfig() +{ + this->bed_div_x = this->bed_x / float(this->numRows-1); // Find divisors to calculate the calbration points + this->bed_div_y = this->bed_y / float(this->numCols-1); + + // Ensure free program memory for the pData grid + if(this->pData != nullptr) free(this->pData); + + // Allocate program memory for the pData grid + this->pData = (float *)malloc(probe_points * sizeof(float)); +} + +bool ZGridStrategy::handleGcode(Gcode *gcode) +{ + string args = get_arguments(gcode->get_command()); + + // G code processing + if(gcode->has_g) { + if( gcode->g == 31 ) { // report status + + // Bed ZGrid data as gcode: + gcode->stream->printf(";Bed Level settings:\r\n"); + + for (int x=0; xnumRows; x++){ + gcode->stream->printf("X%i",x); + for (int y=0; ynumCols; y++){ + gcode->stream->printf(" %c%1.2f", 'A'+y, this->pData[(x*this->numCols)+y]); + } + gcode->stream->printf("\r\n"); + } + return true; + + } else if( gcode->g == 32 ) { //run probe + // first wait for an empty queue i.e. no moves left + THEKERNEL->conveyor->wait_for_idle(); + + this->setAdjustFunction(false); // Disable leveling code + if(!doProbing(gcode->stream)) { + gcode->stream->printf("Probe failed to complete, probe not triggered or other error\n"); + } else { + this->setAdjustFunction(true); // Enable leveling code + gcode->stream->printf("Probe completed, bed grid defined\n"); + } + return true; + } + + } else if(gcode->has_m) { + switch( gcode->m ) { + + // manual bed ZGrid calbration: M370 - M375 + // M370: Clear current ZGrid for calibration, and move to first position + case 370: { + this->setAdjustFunction(false); // Disable leveling code + this->cal[Z_AXIS] = std::get(this->probe_offsets) + zprobe->getProbeHeight(); + + + if(gcode->has_letter('X')) // Rows (X) + this->numRows = gcode->get_value('X'); + if(gcode->has_letter('Y')) // Cols (Y) + this->numCols = gcode->get_value('Y'); + + this->calcConfig(); // Run calculations for Grid size and allocate grid memory + + this->homexyz(); + for (int i=0; ipData[i] = 0.0F; // Clear the ZGrid + } + + this->cal[X_AXIS] = 0.0f; // Clear calibration position + this->cal[Y_AXIS] = 0.0f; + this->in_cal = true; // In calbration mode + + } + return true; + // M371: Move to next manual calibration position + case 371: { + if (in_cal){ + this->move(this->cal, slow_rate); + this->next_cal(); + } + + } + return true; + // M372: save current position in ZGrid, and move to next calibration position + case 372: { + if (in_cal){ + float cartesian[3]; + int pindex = 0; + + THEROBOT->get_axis_position(cartesian); // get actual position from robot + + pindex = int(cartesian[X_AXIS]/this->bed_div_x + 0.25)*this->numCols + int(cartesian[Y_AXIS]/this->bed_div_y + 0.25); + + this->move(this->cal, slow_rate); // move to the next position + this->next_cal(); // to not cause damage to machine due to Z-offset + + this->pData[pindex] = cartesian[Z_AXIS]; // save the offset + + } + } + return true; + // M373: finalize calibration + case 373: { + // normalize the grid + this->normalize_grid_2home(); + + this->in_cal = false; + this->setAdjustFunction(true); // Enable leveling code + + } + return true; + + // M374: Save grid + case 374:{ + char gridname[5]; + + if(gcode->has_letter('S')) // Custom grid number + snprintf(gridname, sizeof(gridname), "S%03.0f", gcode->get_value('S')); + else + gridname[0] = '\0'; + + if(this->saveGrid(gridname)) { + gcode->stream->printf("Grid saved: Filename: /sd/Zgrid.%s\n",gridname); + } + else { + gcode->stream->printf("Error: Grid not saved: Filename: /sd/Zgrid.%s\n",gridname); + } + } + return true; + + case 375:{ // Load grid values + char gridname[5]; + + if(gcode->has_letter('S')) // Custom grid number + snprintf(gridname, sizeof(gridname), "S%03.0f", gcode->get_value('S')); + else + gridname[0] = '\0'; + + if(this->loadGrid(gridname)) { + this->setAdjustFunction(true); // Enable leveling code + gcode->stream->printf("Grid loaded: /sd/Zgrid.%s\n",gridname); + } + else { + gcode->stream->printf("Error: Grid not loaded: /sd/Zgrid.%s\n",gridname); + } + } + return true; + +/* case 376: { // Check grid value calculations: For debug only. + float target[3]; + + for(char letter = 'X'; letter <= 'Z'; letter++) { + if( gcode->has_letter(letter) ) { + target[letter - 'X'] = gcode->get_value(letter); + } + } + gcode->stream->printf(" Z0 %1.3f\n",getZOffset(target[0], target[1])); + + } + return true; +*/ + case 565: { // M565: Set Z probe offsets + float x= 0, y= 0, z= 0; + if(gcode->has_letter('X')) x = gcode->get_value('X'); + if(gcode->has_letter('Y')) y = gcode->get_value('Y'); + if(gcode->has_letter('Z')) z = gcode->get_value('Z'); + probe_offsets = std::make_tuple(x, y, z); + } + return true; + + case 500: // M500 saves probe_offsets config override file + gcode->stream->printf(";Load default grid\nM375\n"); + + + case 503: { // M503 just prints the settings + + float x,y,z; + gcode->stream->printf(";Probe offsets:\n"); + std::tie(x, y, z) = probe_offsets; + gcode->stream->printf("M565 X%1.5f Y%1.5f Z%1.5f\n", x, y, z); + break; + } + + return true; + } + } + + return false; +} + + +bool ZGridStrategy::saveGrid(std::string args) +{ + args = "/sd/Zgrid." + args; + StreamOutput *ZMap_file = new FileStream(args.c_str()); + + ZMap_file->printf("P%i %i %i %1.3f\n", probe_points, this->numRows, this->numCols, getZhomeoffset()); // Store probe points to prevent loading undefined grid files + + for (int pos = 0; pos < probe_points; pos++){ + ZMap_file->printf("%1.3f\n", this->pData[pos]); + } + delete ZMap_file; + + return true; + +} + +bool ZGridStrategy::loadGrid(std::string args) +{ + char flag[20]; + + int fpoints, GridX = 5, GridY = 5; // for 25point file + float val, GridZ; + + args = "/sd/Zgrid." + args; + FILE *fd = fopen(args.c_str(), "r"); + if(fd != NULL) { + fscanf(fd, "%s\n", flag); + + if (flag[0] == 'P'){ + + sscanf(flag, "P%i\n", &fpoints); // read number of points, and Grid X and Y + fscanf(fd, "%i %i %f\n", &GridX, &GridY, &GridZ); // read number of points, and Grid X and Y and ZHoming offset + fscanf(fd, "%f\n", &val); // read first value from file + + } else { // original 25point file -- Backwards compatibility + fpoints = 25; + sscanf(flag, "%f\n", &val); // read first value from string + } + + if (GridX != this->numRows || GridY != this->numCols){ + this->numRows = GridX; // Change Rows and Columns to match the saved data + this->numCols = GridY; + this->calcConfig(); // Reallocate memory for the grid according to the grid loaded + } + + this->pData[0] = val; // Place the first read value in grid + + for (int pos = 1; pos < probe_points; pos++){ + fscanf(fd, "%f\n", &val); + this->pData[pos] = val; + } + + fclose(fd); + + this->setZoffset(GridZ); + + return true; + + } else { + return false; + } + +} + +float ZGridStrategy::getZhomeoffset() +{ + void* rd; + + bool ok = PublicData::get_value( endstops_checksum, home_offset_checksum, &rd ); + + if (ok) { + return ((float*)rd)[2]; + } + + return 0; +} + +void ZGridStrategy::setZoffset(float zval) +{ + char cmd[64]; + + // Assemble Gcode to add onto the queue + snprintf(cmd, sizeof(cmd), "M206 Z%1.3f", zval); // Send saved Z homing offset + + Gcode gc(cmd, &(StreamOutput::NullStream)); + THEKERNEL->call_event(ON_GCODE_RECEIVED, &gc); + +} + +bool ZGridStrategy::doProbing(StreamOutput *stream) // probed calibration +{ + // home first using selected mode: NOHOME, HOMEXY, HOMEXYZ + this->homexyz(); + + // deactivate correction during moves + this->setAdjustFunction(false); + + for (int i=0; ipData[i] = 0.0F; // Clear the ZGrid + } + + if (this->wait_for_probe){ + + this->cal[X_AXIS] = this->probe_x; //bed_x/2.0f; + this->cal[Y_AXIS] = this->probe_y; //bed_y/2.0f; + this->cal[Z_AXIS] = this->probe_z; //bed_z/2.0f; // Position head for probe attachment + this->move(this->cal, slow_rate); // Move to probe attachment point + + stream->printf("*** Ensure probe is attached and press probe when done ***\n"); + + while(!zprobe->getProbeStatus()){ // Wait for button press + THEKERNEL->call_event(ON_IDLE); + }; + } + + this->in_cal = true; // In calbration mode + + this->cal[X_AXIS] = 0.0f; // Clear calibration position + this->cal[Y_AXIS] = 0.0f; + this->cal[Z_AXIS] = std::get(this->probe_offsets) + zprobe->getProbeHeight(); + + this->move(this->cal, slow_rate); // Move to probe start point + + for (int probes = 0; probes < probe_points; probes++){ + int pindex = 0; + + // z = z home offset - probed distance + float z = getZhomeoffset() -zprobe->probeDistance((this->cal[X_AXIS] + this->cal_offset_x)-std::get(this->probe_offsets), + (this->cal[Y_AXIS] + this->cal_offset_y)-std::get(this->probe_offsets)); + + pindex = int(this->cal[X_AXIS]/this->bed_div_x + 0.25)*this->numCols + int(this->cal[Y_AXIS]/this->bed_div_y + 0.25); + + this->next_cal(); // Calculate next calibration position + + this->pData[pindex] = z ; // save the offset + } + + stream->printf("\nCalibration done.\n"); + if (this->wait_for_probe) { // Only do this it the config calls for probe removal position + this->cal[X_AXIS] = this->bed_x/2.0f; + this->cal[Y_AXIS] = this->bed_y/2.0f; + this->cal[Z_AXIS] = this->bed_z/2.0f; // Position head for probe removal + this->move(this->cal, slow_rate); + + stream->printf("Please remove probe\n"); + + } + + // activate correction + //this->normalize_grid(); + this->normalize_grid_2home(); + this->setAdjustFunction(true); + + this->in_cal = false; + + return true; +} + + +void ZGridStrategy::normalize_grid_2home() +{ + void* rd; + float home_Z_comp; + + bool ok = PublicData::get_value( endstops_checksum, home_offset_checksum, &rd ); + + if (ok) { + home_Z_comp = this->getZOffset(((float*)rd)[0],((float*)rd)[1]); // find the Z compensation at home position + } + else { + home_Z_comp = 0; + } + + // subtracts the home compensation offset to create a table of deltas, normalized to home compensation zero + for (int i = 0; i < probe_points; i++) + this->pData[i] -= home_Z_comp; + + // Doing this removes the need to change homing offset in Z because the reference remains unchanged. + + // add the offset to the current Z homing offset to preserve full probed offset. + // this->setZoffset(this->getZhomeoffset() + home_Z_comp); + +} + +void ZGridStrategy::homexyz() +{ + + switch(this->home_before_probe) { + case NOHOME : return; + + case HOMEXY : { + Gcode gc("G28 X0 Y0", &(StreamOutput::NullStream)); + THEKERNEL->call_event(ON_GCODE_RECEIVED, &gc); + break; + } + + case HOMEXYZ : { + Gcode gc("G28", &(StreamOutput::NullStream)); + THEKERNEL->call_event(ON_GCODE_RECEIVED, &gc); + break; + } + } +} + +void ZGridStrategy::move(float *position, float feed) +{ + // translate the position for non standard cartesian spaces (cal_offset) + zprobe->coordinated_move(position[0] + this->cal_offset_x, position[1] + this->cal_offset_y, position[2], feed); // use specified feedrate (mm/sec) + + //THEKERNEL->streams->printf("DEBUG: move: %s cent: %i\n", cmd, this->center_zero); +} + + +void ZGridStrategy::next_cal(void){ + if ((((int) roundf(this->cal[X_AXIS] / this->bed_div_x)) & 1) != 0){ // Odd row + this->cal[Y_AXIS] -= this->bed_div_y; + if (this->cal[Y_AXIS] < (0.0F - (bed_div_y / 2.0f))){ + + //THEKERNEL->streams->printf("DEBUG: Y (%f) < cond (%f)\n",this->cal[Y_AXIS], 0.0F); + + this->cal[X_AXIS] += bed_div_x; + if (this->cal[X_AXIS] > (this->bed_x + (this->bed_div_x / 2.0f))){ + this->cal[X_AXIS] = 0.0F; + this->cal[Y_AXIS] = 0.0F; + } + else + this->cal[Y_AXIS] = 0.0F; + } + } + else { // Even row (0 is an even row - starting point) + this->cal[Y_AXIS] += bed_div_y; + if (this->cal[Y_AXIS] > (this->bed_y + (bed_div_y / 2.0f))){ + + //THEKERNEL->streams->printf("DEBUG: Y (%f) > cond (%f)\n",this->cal[Y_AXIS], this->bed_y); + + this->cal[X_AXIS] += bed_div_x; + if (this->cal[X_AXIS] > (this->bed_x + (this->bed_div_x / 2.0f))){ + this->cal[X_AXIS] = 0.0F; + this->cal[Y_AXIS] = 0.0F; + } + else + this->cal[Y_AXIS] = this->bed_y; + } + } +} + + +void ZGridStrategy::setAdjustFunction(bool on) +{ + if(on) { + // set the compensationTransform in robot + THEROBOT->compensationTransform= [this](float target[3]) { target[2] += this->getZOffset(target[0], target[1]); }; + }else{ + // clear it + THEROBOT->compensationTransform= nullptr; + } +} + + +// find the Z offset for the point on the plane at x, y +float ZGridStrategy::getZOffset(float X, float Y) +{ + int xIndex2, yIndex2; + + // Subtract calibration offsets as applicable + X -= this->cal_offset_x; + Y -= this->cal_offset_y; + + float xdiff = X / this->bed_div_x; + float ydiff = Y / this->bed_div_y; + + float dCartX1, dCartX2; + + // Get floor of xdiff. Note that (int) of a negative number is its + // ceiling, not its floor. + + int xIndex = (int)(floorf(xdiff)); // Get the current sector (X) + int yIndex = (int)(floorf(ydiff)); // Get the current sector (Y) + + // Index bounds limited to be inside the table + if (xIndex < 0) xIndex = 0; + else if (xIndex > (this->numRows - 2)) xIndex = this->numRows - 2; + + if (yIndex < 0) yIndex = 0; + else if (yIndex > (this->numCols - 2)) yIndex = this->numCols - 2; + + xIndex2 = xIndex+1; + yIndex2 = yIndex+1; + + xdiff -= xIndex; // Find floating point + ydiff -= yIndex; // Find floating point + + dCartX1 = (1-xdiff) * this->pData[(xIndex*this->numCols)+yIndex] + (xdiff) * this->pData[(xIndex2)*this->numCols+yIndex]; + dCartX2 = (1-xdiff) * this->pData[(xIndex*this->numCols)+yIndex2] + (xdiff) * this->pData[(xIndex2)*this->numCols+yIndex2]; + + return ydiff * dCartX2 + (1-ydiff) * dCartX1; // Calculated Z-delta + +} + +// parse a "X,Y,Z" string return x,y,z tuple +std::tuple ZGridStrategy::parseXYZ(const char *str) +{ + float x = 0, y = 0, z= 0; + char *p; + x = strtof(str, &p); + if(p + 1 < str + strlen(str)) { + y = strtof(p + 1, &p); + if(p + 1 < str + strlen(str)) { + z = strtof(p + 1, nullptr); + } + } + return std::make_tuple(x, y, z); +} + diff --git a/src/modules/tools/zprobe/ZGridStrategy.h b/src/modules/tools/zprobe/ZGridStrategy.h new file mode 100644 index 00000000..13be866e --- /dev/null +++ b/src/modules/tools/zprobe/ZGridStrategy.h @@ -0,0 +1,66 @@ +#ifndef _ZGridSTRATEGY +#define _ZGridSTRATEGY + +#include "LevelingStrategy.h" + +#include +#include +#include + +#define ZGrid_leveling_checksum CHECKSUM("ZGrid-leveling") + +class StreamOutput; + +class ZGridStrategy : public LevelingStrategy +{ +public: + ZGridStrategy(ZProbe *zprobe); + ~ZGridStrategy(); + bool handleGcode(Gcode* gcode); + bool handleConfig(); + float getZOffset(float x, float y); + +private: + void homexyz(); + + void move(float *position, float feed); + void next_cal(void); + float getZhomeoffset(); + void setZoffset(float zval); + + void setAdjustFunction(bool); + bool doProbing(StreamOutput *stream); + void normalize_grid_2home(); + + bool loadGrid(std::string args); + bool saveGrid(std::string args); + void calcConfig(); + + std::tuple probe_offsets; + std::tuple parseXYZ(const char *str); + + uint16_t numRows; + uint16_t numCols; + float *pData; + float slow_rate; + float bed_x; + float bed_y; + float bed_z; + float probe_x; + float probe_y; + float probe_z; + float cal_offset_x; + float cal_offset_y; + float bed_div_x; + float bed_div_y; + float cal[3]; // calibration positions for manual leveling + struct { + char home_before_probe:4; + bool in_cal:1; + bool center_zero:1; + bool circular_bed:1; + bool wait_for_probe:1; + }; +}; + +#endif diff --git a/src/modules/tools/zprobe/ZProbe.cpp b/src/modules/tools/zprobe/ZProbe.cpp new file mode 100644 index 00000000..9c408a16 --- /dev/null +++ b/src/modules/tools/zprobe/ZProbe.cpp @@ -0,0 +1,531 @@ +/* + This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl). + Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. + Smoothie is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. + You should have received a copy of the GNU General Public License along with Smoothie. If not, see . +*/ + +#include "ZProbe.h" + +#include "Kernel.h" +#include "BaseSolution.h" +#include "Config.h" +#include "Robot.h" +#include "StepperMotor.h" +#include "StreamOutputPool.h" +#include "Gcode.h" +#include "Conveyor.h" +#include "checksumm.h" +#include "ConfigValue.h" +#include "SlowTicker.h" +#include "Planner.h" +#include "SerialMessage.h" +#include "PublicDataRequest.h" +#include "EndstopsPublicAccess.h" +#include "PublicData.h" +#include "LevelingStrategy.h" +#include "StepTicker.h" +#include "utils.h" + +// strategies we know about +#include "DeltaCalibrationStrategy.h" +#include "ThreePointStrategy.h" +//#include "ZGridStrategy.h" +#include "DeltaGridStrategy.h" + +#define enable_checksum CHECKSUM("enable") +#define probe_pin_checksum CHECKSUM("probe_pin") +#define debounce_ms_checksum CHECKSUM("debounce_ms") +#define slow_feedrate_checksum CHECKSUM("slow_feedrate") +#define fast_feedrate_checksum CHECKSUM("fast_feedrate") +#define return_feedrate_checksum CHECKSUM("return_feedrate") +#define probe_height_checksum CHECKSUM("probe_height") +#define gamma_max_checksum CHECKSUM("gamma_max") +#define reverse_z_direction_checksum CHECKSUM("reverse_z") + +// from endstop section +#define delta_homing_checksum CHECKSUM("delta_homing") +#define rdelta_homing_checksum CHECKSUM("rdelta_homing") + +#define X_AXIS 0 +#define Y_AXIS 1 +#define Z_AXIS 2 + +#define STEPPER THEROBOT->actuators +#define STEPS_PER_MM(a) (STEPPER[a]->get_steps_per_mm()) +#define Z_STEPS_PER_MM STEPS_PER_MM(Z_AXIS) + +#define abs(a) ((a<0) ? -a : a) + +void ZProbe::on_module_loaded() +{ + // if the module is disabled -> do nothing + if(!THEKERNEL->config->value( zprobe_checksum, enable_checksum )->by_default(false)->as_bool()) { + // as this module is not needed free up the resource + delete this; + return; + } + + // load settings + this->config_load(); + // register event-handlers + register_for_event(ON_GCODE_RECEIVED); + + // we read the probe in this timer, currently only for G38 probes. + probing= false; + THEKERNEL->slow_ticker->attach(1000, this, &ZProbe::read_probe); +} + +void ZProbe::config_load() +{ + this->pin.from_string( THEKERNEL->config->value(zprobe_checksum, probe_pin_checksum)->by_default("nc" )->as_string())->as_input(); + this->debounce_ms = THEKERNEL->config->value(zprobe_checksum, debounce_ms_checksum)->by_default(0 )->as_number(); + + // get strategies to load + vector modules; + THEKERNEL->config->get_module_list( &modules, leveling_strategy_checksum); + for( auto cs : modules ){ + if( THEKERNEL->config->value(leveling_strategy_checksum, cs, enable_checksum )->as_bool() ){ + bool found= false; + // check with each known strategy and load it if it matches + switch(cs) { + case delta_calibration_strategy_checksum: + this->strategies.push_back(new DeltaCalibrationStrategy(this)); + found= true; + break; + + case three_point_leveling_strategy_checksum: + // NOTE this strategy is mutually exclusive with the delta calibration strategy + this->strategies.push_back(new ThreePointStrategy(this)); + found= true; + break; + + // case ZGrid_leveling_checksum: + // this->strategies.push_back(new ZGridStrategy(this)); + // found= true; + // break; + + case delta_grid_leveling_strategy_checksum: + this->strategies.push_back(new DeltaGridStrategy(this)); + found= true; + break; + } + if(found) this->strategies.back()->handleConfig(); + } + } + + // need to know if we need to use delta kinematics for homing + this->is_delta = THEKERNEL->config->value(delta_homing_checksum)->by_default(false)->as_bool(); + this->is_rdelta = THEKERNEL->config->value(rdelta_homing_checksum)->by_default(false)->as_bool(); + + // default for backwards compatibility add DeltaCalibrationStrategy if a delta + // will be deprecated + if(this->strategies.empty()) { + if(this->is_delta) { + this->strategies.push_back(new DeltaCalibrationStrategy(this)); + this->strategies.back()->handleConfig(); + } + } + + this->probe_height = THEKERNEL->config->value(zprobe_checksum, probe_height_checksum)->by_default(5.0F)->as_number(); + this->slow_feedrate = THEKERNEL->config->value(zprobe_checksum, slow_feedrate_checksum)->by_default(5)->as_number(); // feedrate in mm/sec + this->fast_feedrate = THEKERNEL->config->value(zprobe_checksum, fast_feedrate_checksum)->by_default(100)->as_number(); // feedrate in mm/sec + this->return_feedrate = THEKERNEL->config->value(zprobe_checksum, return_feedrate_checksum)->by_default(0)->as_number(); // feedrate in mm/sec + this->reverse_z = THEKERNEL->config->value(zprobe_checksum, reverse_z_direction_checksum)->by_default(false)->as_bool(); // Z probe moves in reverse direction + this->max_z = THEKERNEL->config->value(gamma_max_checksum)->by_default(500)->as_number(); // maximum zprobe distance +} + +uint32_t ZProbe::read_probe(uint32_t dummy) +{ + if(!probing || probe_detected) return 0; + + if(STEPPER[Z_AXIS]->is_moving()) { + // if it is moving then we check the probe, and debounce it + if(this->pin.get()) { + if(debounce < debounce_ms) { + debounce++; + } else { + // we signal the motors to stop, which will preempt any moves on that axis + // we do all motors as it may be a delta + for(auto &a : THEROBOT->actuators) a->stop_moving(); + probe_detected= true; + debounce= 0; + } + + } else { + // The endstop was not hit yet + debounce= 0; + } + } + + return 0; +} + +// single probe in Z with custom feedrate +// returns boolean value indicating if probe was triggered +bool ZProbe::run_probe(float& mm, float feedrate, float max_dist, bool reverse) +{ + float maxz= max_dist < 0 ? this->max_z*2 : max_dist; + + probing= true; + probe_detected= false; + debounce= 0; + + // save current actuator position so we can report how far we moved + ActuatorCoordinates start_pos{ + THEROBOT->actuators[X_AXIS]->get_current_position(), + THEROBOT->actuators[Y_AXIS]->get_current_position(), + THEROBOT->actuators[Z_AXIS]->get_current_position() + }; + + // move Z down + THEROBOT->disable_segmentation= true; // we must disable segmentation as this won't work with it enabled + bool dir= (!reverse_z != reverse); // xor + float delta[3]= {0,0,0}; + delta[Z_AXIS]= dir ? -maxz : maxz; + THEROBOT->delta_move(delta, feedrate, 3); + + // wait until finished + THECONVEYOR->wait_for_idle(); + THEROBOT->disable_segmentation= false; + + // now see how far we moved, get delta in z we moved + // NOTE this works for deltas as well as all three actuators move the same amount in Z + mm= start_pos[2] - THEROBOT->actuators[2]->get_current_position(); + + // set the last probe position to the actuator units moved during this home + THEROBOT->set_last_probe_position( + std::make_tuple( + start_pos[0] - THEROBOT->actuators[0]->get_current_position(), + start_pos[1] - THEROBOT->actuators[1]->get_current_position(), + mm, + probe_detected?1:0)); + + probing= false; + + if(probe_detected) { + // if the probe stopped the move we need to correct the last_milestone as it did not reach where it thought + THEROBOT->reset_position_from_current_actuator_position(); + } + + return probe_detected; +} + +bool ZProbe::return_probe(float mm, bool reverse) +{ + // move probe back to where it was + float fr; + if(this->return_feedrate != 0) { // use return_feedrate if set + fr = this->return_feedrate; + } else { + fr = this->slow_feedrate*2; // nominally twice slow feedrate + if(fr > this->fast_feedrate) fr = this->fast_feedrate; // unless that is greater than fast feedrate + } + + bool dir= ((mm < 0) != reverse_z); // xor + if(reverse) dir= !dir; + + float delta[3]= {0,0,0}; + delta[Z_AXIS]= dir ? -mm : mm; + THEROBOT->delta_move(delta, fr, 3); + + // wait until finished + THECONVEYOR->wait_for_idle(); + + return true; +} + +bool ZProbe::doProbeAt(float &mm, float x, float y) +{ + float s; + // move to xy + coordinated_move(x, y, NAN, getFastFeedrate()); + if(!run_probe(s)) return false; + + // return to original Z + return_probe(s); + mm = s; + + return true; +} + +float ZProbe::probeDistance(float x, float y) +{ + float s; + if(!doProbeAt(s, x, y)) return NAN; + return s; +} + +void ZProbe::on_gcode_received(void *argument) +{ + Gcode *gcode = static_cast(argument); + + if( gcode->has_g && gcode->g >= 29 && gcode->g <= 32) { + + // make sure the probe is defined and not already triggered before moving motors + if(!this->pin.connected()) { + gcode->stream->printf("ZProbe not connected.\n"); + return; + } + if(this->pin.get()) { + gcode->stream->printf("ZProbe triggered before move, aborting command.\n"); + return; + } + + if( gcode->g == 30 ) { // simple Z probe + // first wait for an empty queue i.e. no moves left + THEKERNEL->conveyor->wait_for_idle(); + + // turn off any compensation transform + auto savect= THEROBOT->compensationTransform; + THEROBOT->compensationTransform= nullptr; + + bool probe_result; + bool reverse= (gcode->has_letter('R') && gcode->get_value('R') != 0); // specify to probe in reverse direction + float rate= gcode->has_letter('F') ? gcode->get_value('F') / 60 : this->slow_feedrate; + float mm; + probe_result = run_probe(mm, rate, -1, reverse); + + if(probe_result) { + // the result is in actuator coordinates and raw steps + gcode->stream->printf("Z:%1.4f\n", mm); + + // set the last probe position to the current actuator units + THEROBOT->set_last_probe_position(std::make_tuple( + THEROBOT->actuators[X_AXIS]->get_current_position(), + THEROBOT->actuators[Y_AXIS]->get_current_position(), + THEROBOT->actuators[Z_AXIS]->get_current_position(), + 1)); + + // move back to where it started, unless a Z is specified (and not a rotary delta) + if(gcode->has_letter('Z') && !is_rdelta) { + // set Z to the specified value, and leave probe where it is + THEROBOT->reset_axis_position(gcode->get_value('Z'), Z_AXIS); + + } else { + // return to pre probe position + return_probe(mm, reverse); + } + + } else { + gcode->stream->printf("ZProbe not triggered\n"); + THEROBOT->set_last_probe_position(std::make_tuple( + THEROBOT->actuators[X_AXIS]->get_current_position(), + THEROBOT->actuators[Y_AXIS]->get_current_position(), + THEROBOT->actuators[Z_AXIS]->get_current_position(), + 0)); + } + + // restore compensationTransform + THEROBOT->compensationTransform= savect; + + } else { + if(!gcode->has_letter('P')) { + // find the first strategy to handle the gcode + for(auto s : strategies){ + if(s->handleGcode(gcode)) { + return; + } + } + gcode->stream->printf("No strategy found to handle G%d\n", gcode->g); + + }else{ + // P paramater selects which strategy to send the code to + // they are loaded in the order they are defined in config, 0 being the first, 1 being the second and so on. + uint16_t i= gcode->get_value('P'); + if(i < strategies.size()) { + if(!strategies[i]->handleGcode(gcode)){ + gcode->stream->printf("strategy #%d did not handle G%d\n", i, gcode->g); + } + return; + + }else{ + gcode->stream->printf("strategy #%d is not loaded\n", i); + } + } + } + + } else if(gcode->has_g && gcode->g == 38 ) { // G38.2 Straight Probe with error, G38.3 straight probe without error + // linuxcnc/grbl style probe http://www.linuxcnc.org/docs/2.5/html/gcode/gcode.html#sec:G38-probe + if(gcode->subcode != 2 && gcode->subcode != 3) { + gcode->stream->printf("error:Only G38.2 and G38.3 are supported\n"); + return; + } + + // make sure the probe is defined and not already triggered before moving motors + if(!this->pin.connected()) { + gcode->stream->printf("error:ZProbe not connected.\n"); + return; + } + + if(this->pin.get()) { + gcode->stream->printf("error:ZProbe triggered before move, aborting command.\n"); + return; + } + + // first wait for an empty queue i.e. no moves left + THEKERNEL->conveyor->wait_for_idle(); + + // turn off any compensation transform + auto savect= THEROBOT->compensationTransform; + THEROBOT->compensationTransform= nullptr; + + if(gcode->has_letter('X')) { + // probe in the X axis + probe_XYZ(gcode, X_AXIS); + + }else if(gcode->has_letter('Y')) { + // probe in the Y axis + probe_XYZ(gcode, Y_AXIS); + + }else if(gcode->has_letter('Z')) { + // probe in the Z axis + probe_XYZ(gcode, Z_AXIS); + + }else{ + gcode->stream->printf("error:at least one of X Y or Z must be specified\n"); + } + + // restore compensationTransform + THEROBOT->compensationTransform= savect; + + return; + + } else if(gcode->has_m) { + // M code processing here + int c; + switch (gcode->m) { + case 119: + c = this->pin.get(); + gcode->stream->printf(" Probe: %d", c); + gcode->add_nl = true; + break; + + case 670: + if (gcode->has_letter('S')) this->slow_feedrate = gcode->get_value('S'); + if (gcode->has_letter('K')) this->fast_feedrate = gcode->get_value('K'); + if (gcode->has_letter('R')) this->return_feedrate = gcode->get_value('R'); + if (gcode->has_letter('Z')) this->max_z = gcode->get_value('Z'); + if (gcode->has_letter('H')) this->probe_height = gcode->get_value('H'); + if (gcode->has_letter('I')) { // NOTE this is temporary and toggles the invertion status of the pin + invert_override= (gcode->get_value('I') != 0); + pin.set_inverting(pin.is_inverting() != invert_override); // XOR so inverted pin is not inverted and vice versa + } + break; + + case 500: // save settings + case 503: // print settings + gcode->stream->printf(";Probe feedrates Slow/fast(K)/Return (mm/sec) max_z (mm) height (mm):\nM670 S%1.2f K%1.2f R%1.2f Z%1.2f H%1.2f\n", + this->slow_feedrate, this->fast_feedrate, this->return_feedrate, this->max_z, this->probe_height); + + // fall through is intended so leveling strategies can handle m-codes too + + default: + for(auto s : strategies){ + if(s->handleGcode(gcode)) { + return; + } + } + } + } +} + +// special way to probe in the X or Y or Z direction using planned moves, should work with any kinematics +void ZProbe::probe_XYZ(Gcode *gcode, int axis) +{ + // enable the probe checking in the timer + probing= true; + probe_detected= false; + THEROBOT->disable_segmentation= true; // we must disable segmentation as this won't work with it enabled (beware on deltas probing in X or Y) + + // get probe feedrate in mm/min and convert to mm/sec if specified + float rate = (gcode->has_letter('F')) ? gcode->get_value('F')/60 : this->slow_feedrate; + + // do a regular move which will stop as soon as the probe is triggered, or the distance is reached + switch(axis) { + case X_AXIS: coordinated_move(gcode->get_value('X'), 0, 0, rate, true); break; + case Y_AXIS: coordinated_move(0, gcode->get_value('Y'), 0, rate, true); break; + case Z_AXIS: coordinated_move(0, 0, gcode->get_value('Z'), rate, true); break; + } + + // coordinated_move returns when the move is finished + + // disable probe checking + probing= false; + THEROBOT->disable_segmentation= false; + + float pos[3]; + { + // get the current position + ActuatorCoordinates current_position{ + THEROBOT->actuators[X_AXIS]->get_current_position(), + THEROBOT->actuators[Y_AXIS]->get_current_position(), + THEROBOT->actuators[Z_AXIS]->get_current_position() + }; + + // get machine position from the actuator position using FK + THEROBOT->arm_solution->actuator_to_cartesian(current_position, pos); + } + + uint8_t probeok= this->probe_detected ? 1 : 0; + + // print results using the GRBL format + gcode->stream->printf("[PRB:%1.3f,%1.3f,%1.3f:%d]\n", pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], probeok); + THEROBOT->set_last_probe_position(std::make_tuple(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], probeok)); + + if(!probeok && gcode->subcode == 2) { + // issue error if probe was not triggered and subcode == 2 + gcode->stream->printf("ALARM:Probe fail\n"); + THEKERNEL->call_event(ON_HALT, nullptr); + + }else if(probeok){ + // if the probe stopped the move we need to correct the last_milestone as it did not reach where it thought + THEROBOT->reset_position_from_current_actuator_position(); + } +} + +// issue a coordinated move directly to robot, and return when done +// Only move the coordinates that are passed in as not nan +// NOTE must use G53 to force move in machine coordinates and ignore any WCS offsets +void ZProbe::coordinated_move(float x, float y, float z, float feedrate, bool relative) +{ + char buf[32]; + char cmd[64]; + + if(relative) strcpy(cmd, "G91 G0 "); + else strcpy(cmd, "G53 G0 "); // G53 forces movement in machine coordinate system + + if(!isnan(x)) { + int n = snprintf(buf, sizeof(buf), " X%1.3f", x); + strncat(cmd, buf, n); + } + if(!isnan(y)) { + int n = snprintf(buf, sizeof(buf), " Y%1.3f", y); + strncat(cmd, buf, n); + } + if(!isnan(z)) { + int n = snprintf(buf, sizeof(buf), " Z%1.3f", z); + strncat(cmd, buf, n); + } + + // use specified feedrate (mm/sec) + int n = snprintf(buf, sizeof(buf), " F%1.1f", feedrate * 60); // feed rate is converted to mm/min + strncat(cmd, buf, n); + if(relative) strcat(cmd, " G90"); + + //THEKERNEL->streams->printf("DEBUG: move: %s\n", cmd); + + // send as a command line as may have multiple G codes in it + struct SerialMessage message; + message.message = cmd; + message.stream = &(StreamOutput::NullStream); + THEKERNEL->call_event(ON_CONSOLE_LINE_RECEIVED, &message ); + THEKERNEL->conveyor->wait_for_idle(); +} + +// issue home command +void ZProbe::home() +{ + Gcode gc("G28", &(StreamOutput::NullStream)); + THEKERNEL->call_event(ON_GCODE_RECEIVED, &gc); +} diff --git a/src/modules/tools/zprobe/ZProbe.h b/src/modules/tools/zprobe/ZProbe.h new file mode 100644 index 00000000..7f4324a1 --- /dev/null +++ b/src/modules/tools/zprobe/ZProbe.h @@ -0,0 +1,75 @@ +/* + This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl). + Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. + Smoothie is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. + You should have received a copy of the GNU General Public License along with Smoothie. If not, see . +*/ + +#ifndef ZPROBE_H_ +#define ZPROBE_H_ + +#include "Module.h" +#include "Pin.h" + +#include + +// defined here as they are used in multiple files +#define zprobe_checksum CHECKSUM("zprobe") +#define leveling_strategy_checksum CHECKSUM("leveling-strategy") + +class StepperMotor; +class Gcode; +class StreamOutput; +class LevelingStrategy; + +class ZProbe: public Module +{ + +public: + ZProbe() : invert_override(false) {}; + virtual ~ZProbe() {}; + + void on_module_loaded(); + void on_gcode_received(void *argument); + + bool run_probe(float& mm, float feedrate, float max_dist= -1, bool reverse= false); + bool run_probe(float& mm, bool fast= false) { return run_probe(mm, fast ? this->fast_feedrate : this->slow_feedrate); } + bool return_probe(float mm, bool reverse= false); + bool doProbeAt(float &mm, float x, float y); + float probeDistance(float x, float y); + + void coordinated_move(float x, float y, float z, float feedrate, bool relative=false); + void home(); + + bool getProbeStatus() { return this->pin.get(); } + float getSlowFeedrate() const { return slow_feedrate; } + float getFastFeedrate() const { return fast_feedrate; } + float getProbeHeight() const { return probe_height; } + float getMaxZ() const { return max_z; } + +private: + void config_load(); + void probe_XYZ(Gcode *gc, int axis); + uint32_t read_probe(uint32_t dummy); + + float slow_feedrate; + float fast_feedrate; + float return_feedrate; + float probe_height; + float max_z; + + Pin pin; + std::vector strategies; + uint16_t debounce_ms, debounce; + + volatile struct { + bool is_delta:1; + bool is_rdelta:1; + bool probing:1; + bool reverse_z:1; + bool invert_override:1; + volatile bool probe_detected:1; + }; +}; + +#endif /* ZPROBE_H_ */ diff --git a/src/modules/utils/PlayLed/PlayLed.cpp b/src/modules/utils/PlayLed/PlayLed.cpp new file mode 100644 index 00000000..8e593303 --- /dev/null +++ b/src/modules/utils/PlayLed/PlayLed.cpp @@ -0,0 +1,60 @@ +#include "PlayLed.h" + +/* + * LED indicator: + * off = not paused, nothing to do + * fast flash = halted + * on = a block is being executed + */ + +#include "modules/robot/Conveyor.h" +#include "SlowTicker.h" +#include "Config.h" +#include "checksumm.h" +#include "ConfigValue.h" +#include "Gcode.h" + +#define pause_led_pin_checksum CHECKSUM("pause_led_pin") +#define play_led_pin_checksum CHECKSUM("play_led_pin") +#define play_led_disable_checksum CHECKSUM("play_led_disable") + +PlayLed::PlayLed() { + cnt= 0; +} + +void PlayLed::on_module_loaded() +{ + if(THEKERNEL->config->value( play_led_disable_checksum )->by_default(false)->as_bool()) { + delete this; + return; + } + + on_config_reload(this); + + THEKERNEL->slow_ticker->attach(12, this, &PlayLed::led_tick); +} + +void PlayLed::on_config_reload(void *argument) +{ + string ledpin = "4.28!"; + + ledpin = THEKERNEL->config->value( pause_led_pin_checksum )->by_default(ledpin)->as_string(); // check for pause_led_pin first + ledpin = THEKERNEL->config->value( play_led_pin_checksum )->by_default(ledpin)->as_string(); // override with play_led_pin if it's found + + led.from_string(ledpin)->as_output()->set(false); +} + +uint32_t PlayLed::led_tick(uint32_t) +{ + if(THEKERNEL->is_halted()) { + led.set(!led.get()); + return 0; + } + + if(++cnt >= 6) { // 6 ticks ~ 500ms + cnt= 0; + led.set(!THECONVEYOR->is_idle()); + } + + return 0; +} diff --git a/src/modules/utils/PlayLed/PlayLed.h b/src/modules/utils/PlayLed/PlayLed.h new file mode 100644 index 00000000..d139d8c9 --- /dev/null +++ b/src/modules/utils/PlayLed/PlayLed.h @@ -0,0 +1,24 @@ +#ifndef _PLAYLED_H +#define _PLAYLED_H + +#include "libs/Kernel.h" +#include "libs/Pin.h" + + +class PlayLed : public Module +{ +public: + PlayLed(); + + void on_module_loaded(void); + void on_config_reload(void *); + +private: + uint32_t led_tick(uint32_t); + Pin led; + struct { + uint8_t cnt:4; + }; +}; + +#endif /* _PLAYLED_H */ diff --git a/src/modules/utils/configurator/Configurator.cpp b/src/modules/utils/configurator/Configurator.cpp new file mode 100644 index 00000000..f6601fb1 --- /dev/null +++ b/src/modules/utils/configurator/Configurator.cpp @@ -0,0 +1,132 @@ +/* + This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl). + Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. + Smoothie is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. + You should have received a copy of the GNU General Public License along with Smoothie. If not, see . +*/ + + +#include "libs/Kernel.h" +#include "Configurator.h" +#include "libs/nuts_bolts.h" +#include "libs/utils.h" +#include "libs/SerialMessage.h" +#include "libs/StreamOutput.h" +#include "checksumm.h" +#include "Config.h" +#include "Gcode.h" +#include "ConfigSource.h" +#include "FileConfigSource.h" +#include "ConfigValue.h" +#include "ConfigCache.h" + +#define CONF_NONE 0 +#define CONF_ROM 1 +#define CONF_SD 2 +#define CONF_EEPROM 3 + + +// Output a ConfigValue from the specified ConfigSource to the stream +void Configurator::config_get_command( string parameters, StreamOutput *stream ) +{ + string source = shift_parameter(parameters); + string setting = shift_parameter(parameters); + if (setting == "") { // output settings from the config-cache + setting = source; + source = ""; + uint16_t setting_checksums[3]; + get_checksums(setting_checksums, setting ); + THEKERNEL->config->config_cache_load(); // need to load config cache first as it is unloaded after booting + ConfigValue *cv = THEKERNEL->config->value(setting_checksums); + if(cv != NULL && cv->found) { + string value = cv->as_string(); + stream->printf( "cached: %s is set to %s\r\n", setting.c_str(), value.c_str() ); + } else { + stream->printf( "cached: %s is not in config\r\n", setting.c_str()); + } + THEKERNEL->config->config_cache_clear(); + + } else { // output setting from specified source by parsing the config file + uint16_t source_checksum = get_checksum( source ); + uint16_t setting_checksums[3]; + get_checksums(setting_checksums, setting ); + for(unsigned int i = 0; i < THEKERNEL->config->config_sources.size(); i++) { + if( THEKERNEL->config->config_sources[i]->is_named(source_checksum) ) { + string value = THEKERNEL->config->config_sources[i]->read(setting_checksums); + if(value.empty()) { + stream->printf( "%s: %s is not in config\r\n", source.c_str(), setting.c_str() ); + } else { + stream->printf( "%s: %s is set to %s\r\n", source.c_str(), setting.c_str(), value.c_str() ); + } + break; + } + } + } +} + +// Write the specified setting to the specified ConfigSource +void Configurator::config_set_command( string parameters, StreamOutput *stream ) +{ + string source = shift_parameter(parameters); + string setting = shift_parameter(parameters); + string value = shift_parameter(parameters); + if(source.empty() || setting.empty() || value.empty()) { + stream->printf( "Usage: config-set source setting value # where source is sd, setting is the key and value is the new value\r\n" ); + return; + } + + uint16_t source_checksum = get_checksum(source); + for(unsigned int i = 0; i < THEKERNEL->config->config_sources.size(); i++) { + if( THEKERNEL->config->config_sources[i]->is_named(source_checksum) ) { + if(THEKERNEL->config->config_sources[i]->write(setting, value)) { + stream->printf( "%s: %s has been set to %s\r\n", source.c_str(), setting.c_str(), value.c_str() ); + } else { + stream->printf( "%s: %s not enough space to overwrite existing key/value\r\n", source.c_str(), setting.c_str() ); + } + return; + } + } + stream->printf( "%s source does not exist\r\n", source.c_str()); + + /* Live setting not really supported anymore as the cache is never left loaded + if (value == "") { + if(!THEKERNEL->config->config_cache_loaded) { + stream->printf( "live: setting not allowed as config cache is not loaded\r\n" ); + return; + } + value = setting; + setting = source; + source = ""; + THEKERNEL->config->set_string(setting, value); + stream->printf( "live: %s has been set to %s\r\n", setting.c_str(), value.c_str() ); + */ +} + +// Reload config values from the specified ConfigSource, NOTE used for debugging by dumping the config-cache +void Configurator::config_load_command( string parameters, StreamOutput *stream ) +{ + string source = shift_parameter(parameters); + if(source == "load") { + THEKERNEL->config->config_cache_load(); + stream->printf( "config cache loaded\r\n"); + + } else if(source == "unload") { + THEKERNEL->config->config_cache_clear(); + stream->printf( "config cache unloaded\r\n" ); + + } else if(source == "dump") { + THEKERNEL->config->config_cache_load(); + THEKERNEL->config->config_cache->dump(stream); + THEKERNEL->config->config_cache_clear(); + + } else if(source == "checksum") { + string key = shift_parameter(parameters); + uint16_t cs[3]; + get_checksums(cs, key); + stream->printf( "checksum of %s = %02X %02X %02X\n", key.c_str(), cs[0], cs[1], cs[2]); + + } else { + stream->printf( "unsupported option: must be one of load|unload|dump|checksum\n" ); + } +} + diff --git a/src/modules/utils/configurator/Configurator.h b/src/modules/utils/configurator/Configurator.h new file mode 100644 index 00000000..3752fd2c --- /dev/null +++ b/src/modules/utils/configurator/Configurator.h @@ -0,0 +1,28 @@ +/* + This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl). + Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. + Smoothie is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. + You should have received a copy of the GNU General Public License along with Smoothie. If not, see . +*/ + + +#ifndef configurator_h +#define configurator_h + +#include +using std::string; + +class StreamOutput; + +class Configurator +{ +public: + Configurator() {} + + void config_get_command( string parameters, StreamOutput *stream ); + void config_set_command( string parameters, StreamOutput *stream ); + void config_load_command(string parameters, StreamOutput *stream ); +}; + + +#endif // configurator_h diff --git a/src/modules/utils/currentcontrol/CurrentControl.cpp b/src/modules/utils/currentcontrol/CurrentControl.cpp new file mode 100644 index 00000000..2e30f3b0 --- /dev/null +++ b/src/modules/utils/currentcontrol/CurrentControl.cpp @@ -0,0 +1,110 @@ +#include "CurrentControl.h" +#include "libs/Kernel.h" +#include "libs/nuts_bolts.h" +#include "libs/utils.h" +#include "ConfigValue.h" +#include "libs/StreamOutput.h" + +#include "Gcode.h" +#include "Config.h" +#include "checksumm.h" +#include "DigipotBase.h" + +// add new digipot chips here +#include "mcp4451.h" +#include "ad5206.h" + +#include +using namespace std; + +#define alpha_current_checksum CHECKSUM("alpha_current") +#define beta_current_checksum CHECKSUM("beta_current") +#define gamma_current_checksum CHECKSUM("gamma_current") +#define delta_current_checksum CHECKSUM("delta_current") +#define epsilon_current_checksum CHECKSUM("epsilon_current") +#define zeta_current_checksum CHECKSUM("zeta_current") +#define eta_current_checksum CHECKSUM("eta_current") +#define theta_current_checksum CHECKSUM("theta_current") +#define currentcontrol_module_enable_checksum CHECKSUM("currentcontrol_module_enable") +#define digipotchip_checksum CHECKSUM("digipotchip") +#define digipot_max_current CHECKSUM("digipot_max_current") +#define digipot_factor CHECKSUM("digipot_factor") + +#define mcp4451_checksum CHECKSUM("mcp4451") +#define ad5206_checksum CHECKSUM("ad5206") + +CurrentControl::CurrentControl() +{ + digipot = NULL; +} + +void CurrentControl::on_module_loaded() +{ + if( !THEKERNEL->config->value( currentcontrol_module_enable_checksum )->by_default(false)->as_bool() ) { + // as this module is not needed free up the resource + delete this; + return; + } + + // allocate digipot, if already allocated delete it first + delete digipot; + + // see which chip to use + int chip_checksum = get_checksum(THEKERNEL->config->value(digipotchip_checksum)->by_default("mcp4451")->as_string()); + if(chip_checksum == mcp4451_checksum) { + digipot = new MCP4451(); + } else if(chip_checksum == ad5206_checksum) { + digipot = new AD5206(); + } else { // need a default so use smoothie + digipot = new MCP4451(); + } + + digipot->set_max_current( THEKERNEL->config->value(digipot_max_current )->by_default(2.0f)->as_number()); + digipot->set_factor( THEKERNEL->config->value(digipot_factor )->by_default(113.33f)->as_number()); + + // Get configuration + this->digipot->set_current(0, THEKERNEL->config->value(alpha_current_checksum )->by_default(0.8f)->as_number()); + this->digipot->set_current(1, THEKERNEL->config->value(beta_current_checksum )->by_default(0.8f)->as_number()); + this->digipot->set_current(2, THEKERNEL->config->value(gamma_current_checksum )->by_default(0.8f)->as_number()); + this->digipot->set_current(3, THEKERNEL->config->value(delta_current_checksum )->by_default(0.8f)->as_number()); + this->digipot->set_current(4, THEKERNEL->config->value(epsilon_current_checksum)->by_default(-1)->as_number()); + this->digipot->set_current(5, THEKERNEL->config->value(zeta_current_checksum )->by_default(-1)->as_number()); + this->digipot->set_current(6, THEKERNEL->config->value(eta_current_checksum )->by_default(-1)->as_number()); + this->digipot->set_current(7, THEKERNEL->config->value(theta_current_checksum )->by_default(-1)->as_number()); + + + this->register_for_event(ON_GCODE_RECEIVED); +} + + +void CurrentControl::on_gcode_received(void *argument) +{ + Gcode *gcode = static_cast(argument); + char alpha[8] = { 'X', 'Y', 'Z', 'E', 'A', 'B', 'C', 'D' }; + if (gcode->has_m) { + if (gcode->m == 907) { + for (int i = 0; i < 8; i++) { + if (gcode->has_letter(alpha[i])) { + float c = gcode->get_value(alpha[i]); + this->digipot->set_current(i, c); + } + } + + } else if(gcode->m == 500 || gcode->m == 503) { + float currents[8]; + bool has_setting= false; + for (int i = 0; i < 8; i++) { + currents[i]= this->digipot->get_current(i); + if(currents[i] >= 0) has_setting= true; + } + if(!has_setting) return; // don't oupuit anything if none are set using this current control + + gcode->stream->printf(";Digipot Motor currents:\nM907 "); + for (int i = 0; i < 8; i++) { + if(currents[i] >= 0) + gcode->stream->printf("%c%1.5f ", alpha[i], currents[i]); + } + gcode->stream->printf("\n"); + } + } +} diff --git a/src/modules/utils/currentcontrol/CurrentControl.h b/src/modules/utils/currentcontrol/CurrentControl.h new file mode 100644 index 00000000..a7269c7a --- /dev/null +++ b/src/modules/utils/currentcontrol/CurrentControl.h @@ -0,0 +1,25 @@ +#ifndef CURRENTCONTROL_H +#define CURRENTCONTROL_H + +#include "Module.h" + +class DigipotBase; + +class CurrentControl : public Module { + public: + CurrentControl(); + virtual ~CurrentControl() {}; + + void on_module_loaded(); + void on_gcode_received(void *); + + private: + DigipotBase* digipot; + +}; + + + + + +#endif diff --git a/src/modules/utils/currentcontrol/DigipotBase.h b/src/modules/utils/currentcontrol/DigipotBase.h new file mode 100644 index 00000000..2d6fabf4 --- /dev/null +++ b/src/modules/utils/currentcontrol/DigipotBase.h @@ -0,0 +1,20 @@ +#ifndef DIGIPOTBASE_H +#define DIGIPOTBASE_H + +class DigipotBase { + public: + DigipotBase(){} + virtual ~DigipotBase(){} + + virtual void set_current( int channel, float current )= 0; + virtual float get_current(int channel)= 0; + void set_max_current(float c) { max_current= c; } + void set_factor(float f) { factor= f; } + + protected: + float factor; + float max_current; +}; + + +#endif diff --git a/src/modules/utils/currentcontrol/ad5206.h b/src/modules/utils/currentcontrol/ad5206.h new file mode 100644 index 00000000..b6a986a5 --- /dev/null +++ b/src/modules/utils/currentcontrol/ad5206.h @@ -0,0 +1,63 @@ +#ifndef AD5206_H +#define AD5206_H + +#include "libs/Kernel.h" +#include "libs/utils.h" +#include +#include "mbed.h" +#include +#include + +class AD5206 : public DigipotBase { + public: + AD5206(){ + //TODO convert to use SPI channels + //SPI0_SCK + //this->spi= new mbed::SPI(P0_9,P0_8,P0_7); //should be able to set those pins in config + this->spi= new mbed::SPI(SPI0_MOSI,SPI0_MISO,SPI0_SCK); //should be able to set those pins in config + cs.from_string("4.29")->as_output(); //this also should be configurable + cs.set(1); + for (int i = 0; i < 6; i++) currents[i] = -1; + } + + void set_current( int channel, float current ) + { + if(channel<6){ + if(current < 0) { + currents[channel]= -1; + return; + } + current = min( max( current, 0.0F ), 2.0F ); + char adresses[6] = { 0x05, 0x03, 0x01, 0x00, 0x02, 0x04 }; + currents[channel] = current; + cs.set(0); + spi->write((int)adresses[channel]); + spi->write((int)current_to_wiper(current)); + cs.set(1); + } + } + + + //taken from 4pi firmware + unsigned char current_to_wiper( float current ){ + unsigned int count = int((current*1000)*100/743); //6.8k resistor and 10k pot + + return (unsigned char)count; + } + + float get_current(int channel) + { + if(channel < 6) + return currents[channel]; + return -1; + } + + private: + + Pin cs; + mbed::SPI* spi; + float currents[6]; +}; + + +#endif diff --git a/src/modules/utils/currentcontrol/mcp4451.h b/src/modules/utils/currentcontrol/mcp4451.h new file mode 100644 index 00000000..510b9f94 --- /dev/null +++ b/src/modules/utils/currentcontrol/mcp4451.h @@ -0,0 +1,71 @@ +#ifndef MCP4451_H +#define MCP4451_H + +#include "libs/Kernel.h" +#include "I2C.h" +#include "libs/utils.h" +#include "DigipotBase.h" +#include +#include + +class MCP4451 : public DigipotBase { + public: + MCP4451(){ + // I2C com + this->i2c = new mbed::I2C(I2C0_SDA, I2C0_SCL); + this->i2c->frequency(20000); + for (int i = 0; i < 8; i++) currents[i] = -1; + } + + ~MCP4451(){ + delete this->i2c; + } + + void set_current( int channel, float current ) + { + if(current < 0) { + currents[channel]= -1; + return; + } + current = min( (float) max( current, 0.0f ), this->max_current ); + currents[channel] = current; + char addr = 0x58; + while(channel > 3){ + addr += 0x02; + channel -= 4; + } + + // Initial setup + this->i2c_send( addr, 0x40, 0xff ); + this->i2c_send( addr, 0xA0, 0xff ); + + // Set actual wiper value + char addresses[4] = { 0x00, 0x10, 0x60, 0x70 }; + this->i2c_send( addr, addresses[channel], this->current_to_wiper(current) ); + } + + float get_current(int channel) + { + return currents[channel]; + } + + private: + + void i2c_send( char first, char second, char third ){ + this->i2c->start(); + this->i2c->write(first); + this->i2c->write(second); + this->i2c->write(third); + this->i2c->stop(); + } + + char current_to_wiper( float current ){ + return char(ceilf(float((this->factor*current)))); + } + + mbed::I2C* i2c; + float currents[8]; +}; + + +#endif diff --git a/src/modules/utils/killbutton/KillButton.cpp b/src/modules/utils/killbutton/KillButton.cpp new file mode 100644 index 00000000..cdceb1e9 --- /dev/null +++ b/src/modules/utils/killbutton/KillButton.cpp @@ -0,0 +1,109 @@ +#include "libs/Kernel.h" +#include "KillButton.h" +#include "libs/nuts_bolts.h" +#include "libs/utils.h" +#include "Config.h" +#include "SlowTicker.h" +#include "libs/SerialMessage.h" +#include "libs/StreamOutput.h" +#include "checksumm.h" +#include "ConfigValue.h" +#include "StreamOutputPool.h" + +using namespace std; + +#define pause_button_enable_checksum CHECKSUM("pause_button_enable") +#define kill_button_enable_checksum CHECKSUM("kill_button_enable") +#define unkill_checksum CHECKSUM("unkill_enable") +#define pause_button_pin_checksum CHECKSUM("pause_button_pin") +#define kill_button_pin_checksum CHECKSUM("kill_button_pin") + +KillButton::KillButton() +{ + this->state= IDLE; +} + +void KillButton::on_module_loaded() +{ + bool pause_enable = THEKERNEL->config->value( pause_button_enable_checksum )->by_default(false)->as_bool(); // @deprecated + bool kill_enable = pause_enable || THEKERNEL->config->value( kill_button_enable_checksum )->by_default(false)->as_bool(); + if(!kill_enable) { + delete this; + return; + } + this->unkill_enable = THEKERNEL->config->value( unkill_checksum )->by_default(true)->as_bool(); + + Pin pause_button; + pause_button.from_string( THEKERNEL->config->value( pause_button_pin_checksum )->by_default("2.12")->as_string())->as_input(); // @DEPRECATED + this->kill_button.from_string( THEKERNEL->config->value( kill_button_pin_checksum )->by_default("nc")->as_string())->as_input(); + + if(!this->kill_button.connected() && pause_button.connected()) { + // use pause button for kill button if kill button not specifically defined + this->kill_button = pause_button; + } + + if(!this->kill_button.connected()) { + delete this; + return; + } + + this->register_for_event(ON_IDLE); + THEKERNEL->slow_ticker->attach( 5, this, &KillButton::button_tick ); +} + +void KillButton::on_idle(void *argument) +{ + if(state == KILL_BUTTON_DOWN) { + if(!THEKERNEL->is_halted()) { + THEKERNEL->call_event(ON_HALT, nullptr); + THEKERNEL->streams->printf("Kill button pressed - reset or M999 to continue\r\n"); + } + + }else if(state == UNKILL_FIRE) { + if(THEKERNEL->is_halted()) { + THEKERNEL->call_event(ON_HALT, (void *)1); // clears on_halt + THEKERNEL->streams->printf("UnKill button pressed Halt cleared\r\n"); + } + } +} + +// Check the state of the button and act accordingly using the following FSM +// Note this is ISR so don't do anything nasty in here +uint32_t KillButton::button_tick(uint32_t dummy) +{ + bool killed= THEKERNEL->is_halted(); + + switch(state) { + case IDLE: + if(!this->kill_button.get()) state= KILL_BUTTON_DOWN; + else if(unkill_enable && killed) state= KILLED_BUTTON_UP; // allow kill button to unkill if kill was created fromsome other source + break; + case KILL_BUTTON_DOWN: + if(killed) state= KILLED_BUTTON_DOWN; + break; + case KILLED_BUTTON_DOWN: + if(this->kill_button.get()) state= KILLED_BUTTON_UP; + break; + case KILLED_BUTTON_UP: + if(!killed) state= IDLE; + else if(unkill_enable && !this->kill_button.get()) state= UNKILL_BUTTON_DOWN; + break; + case UNKILL_BUTTON_DOWN: + unkill_timer= 0; + state= UNKILL_TIMING_BUTTON_DOWN; + break; + case UNKILL_TIMING_BUTTON_DOWN: + if(++unkill_timer > 5*2) state= UNKILL_FIRE; + else if(this->kill_button.get()) unkill_timer= 0; + if(!killed) state= IDLE; + break; + case UNKILL_FIRE: + if(!killed) state= UNKILLED_BUTTON_DOWN; + break; + case UNKILLED_BUTTON_DOWN: + if(this->kill_button.get()) state= IDLE; + break; + } + + return 0; +} diff --git a/src/modules/utils/killbutton/KillButton.h b/src/modules/utils/killbutton/KillButton.h new file mode 100644 index 00000000..2bc2dfe9 --- /dev/null +++ b/src/modules/utils/killbutton/KillButton.h @@ -0,0 +1,31 @@ +#pragma once + +#include "libs/Pin.h" + +class KillButton : public Module { + public: + KillButton(); + + void on_module_loaded(); + void on_idle(void *argument); + uint32_t button_tick(uint32_t dummy); + + private: + Pin kill_button; + enum STATE { + IDLE, + KILL_BUTTON_DOWN, + KILLED_BUTTON_DOWN, + KILLED_BUTTON_UP, + UNKILL_BUTTON_DOWN, + UNKILL_TIMING_BUTTON_DOWN, + UNKILL_FIRE, + UNKILLED_BUTTON_DOWN + }; + + struct { + uint8_t unkill_timer:6; + volatile STATE state:4; + bool unkill_enable:1; + }; +}; diff --git a/src/modules/utils/motordrivercontrol/MotorDriverControl.cpp b/src/modules/utils/motordrivercontrol/MotorDriverControl.cpp new file mode 100644 index 00000000..4dde094a --- /dev/null +++ b/src/modules/utils/motordrivercontrol/MotorDriverControl.cpp @@ -0,0 +1,538 @@ +#include "MotorDriverControl.h" +#include "libs/Kernel.h" +#include "libs/nuts_bolts.h" +#include "libs/utils.h" +#include "ConfigValue.h" +#include "libs/StreamOutput.h" +#include "libs/StreamOutputPool.h" +#include "Robot.h" +#include "StepperMotor.h" +#include "PublicDataRequest.h" +#include "Pin.h" + +#include "Gcode.h" +#include "Config.h" +#include "checksumm.h" + +#include "mbed.h" // for SPI + +#include "drivers/TMC26X/TMC26X.h" +#include "drivers/DRV8711/drv8711.h" +#include "drivers/TMC21X/TMC21X.h" + +#include + +#define motor_driver_control_checksum CHECKSUM("motor_driver_control") +#define enable_checksum CHECKSUM("enable") +#define chip_checksum CHECKSUM("chip") +#define designator_checksum CHECKSUM("designator") +#define alarm_checksum CHECKSUM("alarm") +#define halt_on_alarm_checksum CHECKSUM("halt_on_alarm") + +#define current_checksum CHECKSUM("current") +#define hold_current_checksum CHECKSUM("hold_current") //TMC2130 has a hold current ration of 1/32 to 32/32 +#define max_current_checksum CHECKSUM("max_current") + +#define microsteps_checksum CHECKSUM("microsteps") +#define decay_mode_checksum CHECKSUM("decay_mode") + +#define raw_register_checksum CHECKSUM("reg") + +#define spi_channel_checksum CHECKSUM("spi_channel") +#define spi_cs_pin_checksum CHECKSUM("spi_cs_pin") +#define spi_frequency_checksum CHECKSUM("spi_frequency") + +MotorDriverControl::MotorDriverControl(uint8_t id) : id(id) +{ + enable_event= false; + current_override= false; + microstep_override= false; + chip = TMC2130; + if (chip==TMC2130) { + current = 31; // 31/31 ration driven + } else { + current = 1000; // 1000 mA + } + switch(chip) { + case DRV8711: max_current= DRV8711_max_current; break; + case TMC2660: max_current= TMC2660_max_current; break; + case TMC2130: max_current= TMC2130_max_current; break; //TMC2130 only allows a ratio of 0..31 which represents 1/32 .. 32/32 + } + current = max_current; + hold_current = max_current; + microsteps = 16; + designator = '0'; + spi = nullptr; +} + +MotorDriverControl::~MotorDriverControl() +{ +} + +// this will load all motor driver controls defined in config, called from main +void MotorDriverControl::on_module_loaded() +{ + vector modules; + THEKERNEL->config->get_module_list( &modules, motor_driver_control_checksum ); + uint8_t cnt = 1; + for( auto cs : modules ) { + // If module is enabled create an instance and initialize it + if( THEKERNEL->config->value(motor_driver_control_checksum, cs, enable_checksum )->as_bool() ) { + MotorDriverControl *controller = new MotorDriverControl(cnt++); + if(!controller->config_module(cs)) delete controller; + } + } + + // we don't need this instance anymore + delete this; +} + +bool MotorDriverControl::config_module(uint16_t cs) +{ + std::string str= THEKERNEL->config->value( motor_driver_control_checksum, cs, designator_checksum)->by_default("")->as_string(); + if(str.empty()) { + THEKERNEL->streams->printf("MotorDriverControl ERROR: designator not defined\n"); + return false; // designator required + } + designator= str[0]; + + spi_cs_pin.from_string(THEKERNEL->config->value( motor_driver_control_checksum, cs, spi_cs_pin_checksum)->by_default("nc")->as_string())->as_output(); + if(!spi_cs_pin.connected()) { + THEKERNEL->streams->printf("MotorDriverControl %c ERROR: chip select not defined\n", designator); + return false; // if not defined then we can't use this instance + } + spi_cs_pin.set(1); + + + str= THEKERNEL->config->value( motor_driver_control_checksum, cs, chip_checksum)->by_default("")->as_string(); + if(str.empty()) { + THEKERNEL->streams->printf("MotorDriverControl %c ERROR: chip type not defined\n", designator); + return false; // chip type required + } + + using std::placeholders::_1; + using std::placeholders::_2; + using std::placeholders::_3; + + if(str == "DRV8711") { + chip= DRV8711; + drv8711= new DRV8711DRV(std::bind( &MotorDriverControl::sendSPI, this, _1, _2, _3), designator); + + }else if(str == "TMC2130") { + chip= TMC2130; + tmc21x= new TMC21X(std::bind( &MotorDriverControl::sendSPI, this, _1, _2, _3), designator); + }else if(str == "TMC2660") { + chip= TMC2660; + tmc26x= new TMC26X(std::bind( &MotorDriverControl::sendSPI, this, _1, _2, _3), designator); + + }else{ + THEKERNEL->streams->printf("MotorDriverControl %c ERROR: Unknown chip type: %s\n", designator, str.c_str()); + return false; + } + + // select which SPI channel to use + int spi_channel = THEKERNEL->config->value(motor_driver_control_checksum, cs, spi_channel_checksum)->by_default(1)->as_number(); + int spi_frequency = THEKERNEL->config->value(motor_driver_control_checksum, cs, spi_frequency_checksum)->by_default(1000000)->as_number(); + + // select SPI channel to use + PinName mosi, miso, sclk; + if(spi_channel == 0) { + mosi = SPI0_MOSI; miso = SPI0_MISO; sclk = SPI0_SCK; + } else if(spi_channel == 1) { + mosi = SPI1_MOSI; miso = SPI1_MISO; sclk = SPI1_SCK; + } else { + THEKERNEL->streams->printf("MotorDriverControl %c ERROR: Unknown SPI Channel: %d\n", designator, spi_channel); + return false; + } + + this->spi = new mbed::SPI(mosi, miso, sclk); + this->spi->frequency(spi_frequency); + this->spi->format(8, 3); // 8bit, mode3 + + // set default max currents for each chip, can be overridden in config + switch(chip) { + case DRV8711: max_current= DRV8711_max_current; break; + case TMC2660: max_current= TMC2660_max_current; break; + case TMC2130: max_current= TMC2130_max_current; break; //TMC2130 only allows a ratio of 0..31 which represents 1/32 .. 32/32 + } + + max_current= THEKERNEL->config->value(motor_driver_control_checksum, cs, max_current_checksum )->by_default((int)max_current)->as_number(); + //current_factor= THEKERNEL->config->value(motor_driver_control_checksum, cs, current_factor_checksum )->by_default(1.0F)->as_number(); + + current= THEKERNEL->config->value(motor_driver_control_checksum, cs, current_checksum )->by_default((int)max_current)->as_number(); // set to maxCurrent as TMC2130 uses a ratio method and not mA + hold_current= THEKERNEL->config->value(motor_driver_control_checksum, cs, hold_current_checksum )->by_default((int)max_current)->as_number(); // set to maxCurrent as TMC2130 uses a ratio method and not mA + microsteps= THEKERNEL->config->value(motor_driver_control_checksum, cs, microsteps_checksum )->by_default(16)->as_number(); // 1/n + //decay_mode= THEKERNEL->config->value(motor_driver_control_checksum, cs, decay_mode_checksum )->by_default(1)->as_number(); + + // setup the chip via SPI + initialize_chip(cs); + + // if raw registers are defined set them 1,2,3 etc in hex + str= THEKERNEL->config->value( motor_driver_control_checksum, cs, raw_register_checksum)->by_default("")->as_string(); + if(!str.empty()) { + rawreg= true; + std::vector regs= parse_number_list(str.c_str(), 16); + if(!regs.empty()) { + uint32_t reg= 0; + for(auto i : regs) { + // this just sets the local storage, it does not write to the chip + switch(chip) { + case DRV8711: drv8711->set_raw_register(&StreamOutput::NullStream, ++reg, i); break; + case TMC2660: tmc26x->setRawRegister(&StreamOutput::NullStream, ++reg, i); break; + case TMC2130: tmc21x->setRawRegister(&StreamOutput::NullStream, ++reg, i); break; + } + } + + // write the stored registers + switch(chip) { + case DRV8711: drv8711->set_raw_register(&StreamOutput::NullStream, 255, 0); break; + case TMC2660: tmc26x->setRawRegister(&StreamOutput::NullStream, 255, 0); break; + case TMC2130: tmc21x->setRawRegister(&StreamOutput::NullStream, 255, 0); break; + } + } + + }else{ + rawreg= false; + } + + this->register_for_event(ON_GCODE_RECEIVED); + this->register_for_event(ON_HALT); + this->register_for_event(ON_ENABLE); + this->register_for_event(ON_IDLE); + + if( THEKERNEL->config->value(motor_driver_control_checksum, cs, alarm_checksum )->by_default(false)->as_bool() ) { + halt_on_alarm= THEKERNEL->config->value(motor_driver_control_checksum, cs, halt_on_alarm_checksum )->by_default(false)->as_bool(); + // enable alarm monitoring for the chip + this->register_for_event(ON_SECOND_TICK); + } +//TODO fix below to actually print out the human readable MBED pin name, e.e P3_0 rather than its address + THEKERNEL->streams->printf("MotorDriverControl INFO: configured motor %c (%d): as %s, cs: %04X\n", designator, id, chip==TMC2660?"TMC2660":chip==DRV8711?"DRV8711":chip==TMC2130?"TMC2130":"UNKNOWN", spi_cs_pin.getPinName()); + + return true; +} + +// event to handle enable on/off, as it could be called in an ISR we schedule to turn the steppers on or off in ON_IDLE +// This may cause the initial step to be missed if on-idle is delayed too much but we can't do SPI in an interrupt +void MotorDriverControl::on_enable(void *argument) +{ + enable_event= true; + enable_flg= (argument != nullptr); +} + +void MotorDriverControl::on_idle(void *argument) +{ + if(enable_event) { + enable_event= false; + enable(enable_flg); + } +} + +void MotorDriverControl::on_halt(void *argument) +{ + if(argument == nullptr) { + enable(false); + } +} + +// runs in on_idle, does SPI transaction +void MotorDriverControl::on_second_tick(void *argument) +{ + // we don't want to keep checking once we have been halted by an error + if(THEKERNEL->is_halted()) return; + + bool alarm=false;; + switch(chip) { + case DRV8711: + alarm= drv8711->check_alarm(); + break; + + case TMC2660: + alarm= tmc26x->checkAlarm(); + break; + + case TMC2130: + alarm= tmc21x->checkAlarm(); + break; + } + + if(halt_on_alarm && alarm) { + THEKERNEL->call_event(ON_HALT, nullptr); + THEKERNEL->streams->printf("Motor Driver alarm - reset or M999 required to continue\r\n"); + } +} + +void MotorDriverControl::on_gcode_received(void *argument) +{ + Gcode *gcode = static_cast(argument); + + if (gcode->has_m) { + if(gcode->m == 906) { + if (gcode->has_letter(designator)) { + // set motor currents in mA (Note not using M907 as digipots use that) + current= gcode->get_value(designator); + current= std::min(current, max_current); + set_current(current); + current_override= true; + } + + } else if(gcode->m == 909) { // M909 Annn set microstepping, M909.1 also change steps/mm + if (gcode->has_letter(designator)) { + uint32_t current_microsteps= microsteps; + microsteps= gcode->get_value(designator); + microsteps= set_microstep(microsteps); // driver may change the steps it sets to + if(gcode->subcode == 1 && current_microsteps != microsteps) { + // also reset the steps/mm + int a= designator-'A'; + if(a >= 0 && a <=2) { + float s= THEKERNEL->robot->actuators[a]->get_steps_per_mm()*((float)microsteps/current_microsteps); + THEKERNEL->robot->actuators[a]->change_steps_per_mm(s); + gcode->stream->printf("steps/mm for %c changed to: %f\n", designator, s); + THEKERNEL->robot->check_max_actuator_speeds(); + } + } + microstep_override= true; + } + + // } else if(gcode->m == 910) { // set decay mode + // if (gcode->has_letter(designator)) { + // decay_mode= gcode->get_value(designator); + // set_decay_mode(decay_mode); + // } + + } else if(gcode->m == 911) { + // set or get raw registers + // M911 will dump all the registers and status of all the motors + // M911.1 Pn (or A0) will dump the registers and status of the selected motor. X0 will request format in processing machine readable format + // M911.2 Pn (or B0) Rxxx Vyyy sets Register xxx to value yyy for motor nnn, xxx == 255 writes the registers, xxx == 0 shows what registers are mapped to what + // M911.3 Pn (or C0) will set the options based on the parameters passed as below... + // TMC2660:- + // M911.3 Onnn Qnnn setStallGuardThreshold O=stall_guard_threshold, Q=stall_guard_filter_enabled + // M911.3 Hnnn Innn Jnnn Knnn Lnnn setCoolStepConfiguration H=lower_SG_threshold, I=SG_hysteresis, J=current_decrement_step_size, K=current_increment_step_size, L=lower_current_limit + // M911.3 S0 Unnn Vnnn Wnnn Xnnn Ynnn setConstantOffTimeChopper U=constant_off_time, V=blank_time, W=fast_decay_time_setting, X=sine_wave_offset, Y=use_current_comparator + // M911.3 S1 Unnn Vnnn Wnnn Xnnn Ynnn setSpreadCycleChopper U=constant_off_time, V=blank_time, W=hysteresis_start, X=hysteresis_end, Y=hysteresis_decrement + // M911.3 S2 Zn setRandomOffTime Z=on|off Z1 is on Z0 is off + // M911.3 S3 Zn setDoubleEdge Z=on|off Z1 is on Z0 is off + // M911.3 S4 Zn setStepInterpolation Z=on|off Z1 is on Z0 is off + // M911.3 S5 Zn setCoolStepEnabled Z=on|off Z1 is on Z0 is off + // TMC2130:- + // M911.3 Onnn Qnnn setStallGuardThreshold O=stall_guard_threshold, Q=stall_guard_filter_enabled + // M911.3 Hnnn Innn Jnnn Knnn Lnnn setCoolStepConfiguration H=lower_SG_threshold, I=SG_hysteresis, J=current_decrement_step_size, K=current_increment_step_size, L=lower_current_limit + // M911.3 S0 Unnn Vnnn Wnnn Xnnn Ynnn setConstantOffTimeChopper U=constant_off_time, V=blank_time, W=fast_decay_time_setting, X=sine_wave_offset, Y=use_current_comparator + // M911.3 S1 Unnn Vnnn Wnnn Xnnn Ynnn setSpreadCycleChopper U=constant_off_time, V=blank_time, W=hysteresis_start, X=hysteresis_end, Y=hysteresis_decrement + // M911.3 S2 Zn setRandomOffTime Z=on|off Z1 is on Z0 is off + // M911.3 S3 Zn setDoubleEdge Z=on|off Z1 is on Z0 is off + // M911.3 S4 Zn setStepInterpolation Z=on|off Z1 is on Z0 is off + // M911.3 S5 Zn setCoolStepEnabled Z=on|off Z1 is on Z0 is off + + if(gcode->subcode == 0 && gcode->get_num_args() == 0) { + // M911 no args dump status for all drivers, M911.1 P0|A0 dump for specific driver + gcode->stream->printf("Motor %d (%c)...\n", id, designator); + dump_status(gcode->stream, true); + + }else if(gcode->get_value('P') == id || gcode->has_letter(designator)) { + if(gcode->subcode == 1) { + dump_status(gcode->stream, !gcode->has_letter('X')); + + }else if(gcode->subcode == 2 && gcode->has_letter('R') && gcode->has_letter('V')) { + set_raw_register(gcode->stream, gcode->get_value('R'), gcode->get_value('V')); + + }else if(gcode->subcode == 3 ) { + set_options(gcode); + } + } + + } else if(gcode->m == 500 || gcode->m == 503) { + if(current_override) { + gcode->stream->printf(";Motor %c id %d current mA:\n", designator, id); + gcode->stream->printf("M906 %c%lu\n", designator, current); + } + if(microstep_override) { + gcode->stream->printf(";Motor %c id %d microsteps:\n", designator, id); + gcode->stream->printf("M909 %c%lu\n", designator, microsteps); + } + //gcode->stream->printf("M910 %c%d\n", designator, decay_mode); + } + } +} + +void MotorDriverControl::initialize_chip(uint16_t cs) +{ + // send initialization sequence to chips + if(chip == DRV8711) { + drv8711->init(cs); + set_current(current); + set_microstep(microsteps); + + }else if(chip == TMC2660){ + tmc26x->init(cs); + set_current(current); + set_microstep(microsteps); + //set_decay_mode(decay_mode); + + }else if(chip == TMC2130){ + tmc21x->init(cs); + set_current(current); + set_microstep(microsteps); + //set_decay_mode(decay_mode); + } + +} + +// set current in milliamps +void MotorDriverControl::set_current(uint32_t c) +{ + switch(chip) { + case DRV8711: + drv8711->set_current(c); + break; + + case TMC2660: + tmc26x->setCurrent(c); + break; + + case TMC2130: + tmc21x->setCurrent(c); + break; + } +} + +// set microsteps where n is the number of microsteps eg 64 for 1/64 +uint32_t MotorDriverControl::set_microstep( uint32_t n ) +{ + uint32_t m= n; + switch(chip) { + case DRV8711: + m= drv8711->set_microsteps(n); + break; + + case TMC2660: + tmc26x->setMicrosteps(n); + m= tmc26x->getMicrosteps(); + break; + + case TMC2130: + tmc21x->setMicrosteps(n); + m= tmc21x->getMicrosteps(); + break; + } + return m; +} + +// TODO how to handle this? SO many options +void MotorDriverControl::set_decay_mode( uint8_t dm ) +{ + switch(chip) { + case DRV8711: break; + case TMC2660: break; + case TMC2130: break; + } +} + +void MotorDriverControl::enable(bool on) +{ + switch(chip) { + case DRV8711: + drv8711->set_enable(on); + break; + + case TMC2660: + tmc26x->setEnabled(on); + break; + + case TMC2130: + tmc21x->setEnabled(on); + break; + } +} + +void MotorDriverControl::dump_status(StreamOutput *stream, bool b) +{ + switch(chip) { + case DRV8711: + drv8711->dump_status(stream); + break; + + case TMC2660: + tmc26x->dumpStatus(stream, b); + break; + + case TMC2130: + tmc21x->dumpStatus(stream, b); + break; + } +} + +void MotorDriverControl::set_raw_register(StreamOutput *stream, uint32_t reg, uint32_t val) +{ + bool ok= false; + switch(chip) { + case DRV8711: ok= drv8711->set_raw_register(stream, reg, val); break; + case TMC2660: ok= tmc26x->setRawRegister(stream, reg, val); break; + case TMC2130: ok= tmc21x->setRawRegister(stream, reg, val); break; + } + if(ok) { + stream->printf("register operation succeeded\n"); + }else{ + stream->printf("register operation failed\n"); + } +} + +void MotorDriverControl::set_options(Gcode *gcode) +{ + switch(chip) { + case DRV8711: break; + + case TMC2660: { + TMC26X::options_t options= gcode->get_args_int(); + if(options.size() > 0) { + if(tmc26x->set_options(options)) { + gcode->stream->printf("options set\n"); + }else{ + gcode->stream->printf("failed to set any options\n"); + } + } + // options.clear(); + // if(tmc26x->get_optional(options)) { + // // foreach optional value + // for(auto &i : options) { + // // print all current values of supported options + // gcode->stream->printf("%c: %d ", i.first, i.second); + // gcode->add_nl = true; + // } + // } + } + break; + case TMC2130: { + TMC21X::options_t options= gcode->get_args_int(); + if(options.size() > 0) { + if(tmc21x->set_options(options)) { + gcode->stream->printf("options set\n"); + }else{ + gcode->stream->printf("failed to set any options\n"); + } + } + // options.clear(); + // if(tmc21x->get_optional(options)) { + // // foreach optional value + // for(auto &i : options) { + // // print all current values of supported options + // gcode->stream->printf("%c: %d ", i.first, i.second); + // gcode->add_nl = true; + // } + // } + } + break; + } +} + +// Called by the drivers codes to send and receive SPI data to/from the chip +int MotorDriverControl::sendSPI(uint8_t *b, int cnt, uint8_t *r) +{ + spi_cs_pin.set(0); + for (int i = 0; i < cnt; ++i) { + r[i]= spi->write(b[i]); + } + spi_cs_pin.set(1); + return cnt; +} + diff --git a/src/modules/utils/motordrivercontrol/MotorDriverControl.h b/src/modules/utils/motordrivercontrol/MotorDriverControl.h new file mode 100644 index 00000000..1e2a2740 --- /dev/null +++ b/src/modules/utils/motordrivercontrol/MotorDriverControl.h @@ -0,0 +1,79 @@ +#pragma once + +#include "Module.h" +#include "Pin.h" + +#include + +namespace mbed { + class SPI; +} + +class DRV8711DRV; +class TMC26X; +class TMC21X; +class StreamOutput; +class Gcode; + +class MotorDriverControl : public Module { + public: + MotorDriverControl(uint8_t id); + virtual ~MotorDriverControl(); + + void on_module_loaded(); + void on_gcode_received(void *); + void on_halt(void *argument); + void on_enable(void *argument); + void on_idle(void *argument); + void on_second_tick(void *argument); + + private: + bool config_module(uint16_t cs); + void initialize_chip(uint16_t cs); + void set_current( uint32_t current ); + uint32_t set_microstep( uint32_t ms ); + void set_decay_mode( uint8_t dm ); + void dump_status(StreamOutput*, bool); + void set_raw_register(StreamOutput *stream, uint32_t reg, uint32_t val); + void set_options(Gcode *gcode); + + void enable(bool on); + int sendSPI(uint8_t *b, int cnt, uint8_t *r); + + Pin spi_cs_pin; + mbed::SPI *spi; + + enum CHIP_TYPE { + DRV8711, + TMC2660, + TMC2130 + }; + CHIP_TYPE chip; + + // one of these drivers + union { + DRV8711DRV *drv8711; + TMC26X *tmc26x; + TMC21X *tmc21x; + }; + + //float current_factor; + uint32_t max_current; //Normally in milliamps, but for TMC2130, 0..31 represents a ratio of 1/32 to 32/32 + uint32_t current; //Normally in milliamps, but for TMC2130, 0..31 represents a ratio of 1/32 to 32/32 + uint32_t hold_current; //For TMC2130, 0..31 represents a ratio of 1/32 to 32/32 + uint32_t microsteps; + + char designator; + + struct{ + uint8_t id:4; + uint8_t decay_mode:4; + bool rawreg:1; + bool enable_event:1; + bool enable_flg:1; + bool current_override:1; + bool microstep_override:1; + bool halt_on_alarm:1; + }; + +}; diff --git a/src/modules/utils/motordrivercontrol/drivers/DRV8711/drv8711.cpp b/src/modules/utils/motordrivercontrol/drivers/DRV8711/drv8711.cpp new file mode 100644 index 00000000..0c048f26 --- /dev/null +++ b/src/modules/utils/motordrivercontrol/drivers/DRV8711/drv8711.cpp @@ -0,0 +1,459 @@ +#include "drv8711.h" + +#include "Kernel.h" +#include "StreamOutput.h" +#include "StreamOutputPool.h" +#include "ConfigValue.h" +#include "Config.h" +#include "checksumm.h" + +#define motor_driver_control_checksum CHECKSUM("motor_driver_control") +#define gain_checksum CHECKSUM("gain") +#define sense_resistor_checksum CHECKSUM("sense_resistor") + + +#define REGWRITE 0x00 +#define REGREAD 0x80 + +DRV8711DRV::DRV8711DRV(std::function spi, char d) : spi(spi), designator(d) +{ + error_reported.reset(); +} + +void DRV8711DRV::init (uint16_t cs) +{ + // read chip specific config entries + this->gain= THEKERNEL->config->value(motor_driver_control_checksum, cs, gain_checksum)->by_default(20)->as_number(); + this->resistor= THEKERNEL->config->value(motor_driver_control_checksum, cs, sense_resistor_checksum)->by_default(0.05F)->as_number(); // in ohms + + // initialize the in memory mirror of the registers + + // CTRL Register + G_CTRL_REG.raw = 0x0000; + G_CTRL_REG.Address = 0x00; + G_CTRL_REG.DTIME = 0x03; //850ns + G_CTRL_REG.EXSTALL = 0x00; //Internal Stall Detect + G_CTRL_REG.ISGAIN = 0x02; //Gain of 20 + G_CTRL_REG.MODE = 0x04; // also set by set_microstep() + G_CTRL_REG.RSTEP = 0x00; //No Action + G_CTRL_REG.RDIR = 0x00; //Direction set by DIR Pin + G_CTRL_REG.ENBL = 0x00; //enable motor, start disabled + + /// TORQUE Register + G_TORQUE_REG.raw = 0x0000; + G_TORQUE_REG.Address = 0x01; + G_TORQUE_REG.SIMPLTH = 0x01; //100uS Back EMF Sample Threshold + G_TORQUE_REG.TORQUE = 0x01; // low default set by set_current() + + // OFF Register + G_OFF_REG.raw = 0x0000; + G_OFF_REG.Address = 0x02; + G_OFF_REG.PWMMODE = 0x00; //Internal Indexer + G_OFF_REG.TOFF = 0x32; //Default + + // BLANK Register + G_BLANK_REG.raw = 0x0000; + G_BLANK_REG.Address = 0x03; + G_BLANK_REG.ABT = 0x01; //enable adaptive blanking time + G_BLANK_REG.TBLANK = 0x00; //no idea what this should be but the + + // DECAY Register. + G_DECAY_REG.raw = 0x0000; + G_DECAY_REG.Address = 0x04; + G_DECAY_REG.DECMOD = 0x05; // auto mixed decay + G_DECAY_REG.TDECAY = 0x10; //default + + // STALL Register + G_STALL_REG.raw = 0x0000; + G_STALL_REG.Address = 0x05; + G_STALL_REG.VDIV = 0x02; //Back EMF is divided by 8 + G_STALL_REG.SDCNT = 0x01; //stalln asserted after 2 steps + G_STALL_REG.SDTHR = 0x02; //recommended + + // DRIVE Register + G_DRIVE_REG.raw = 0x0000; + G_DRIVE_REG.Address = 0x06; + G_DRIVE_REG.IDRIVEP = 0x00; //High Side 50mA peak (source) + G_DRIVE_REG.IDRIVEN = 0x00; //Low Side 100mA peak (sink) + G_DRIVE_REG.TDRIVEP = 0x00; //High Side gate drive 500nS + G_DRIVE_REG.TDRIVEN = 0x00; //Low Side Gate Drive 500nS + G_DRIVE_REG.OCPDEG = 0x00; //OCP Deglitch Time 2uS + G_DRIVE_REG.OCPTH = 0x00; //OCP Threshold 500mV + + // STATUS Register + G_STATUS_REG.raw = 0x0000; + G_STATUS_REG.Address = 0x07; + G_STATUS_REG.STDLAT = 0x00; + G_STATUS_REG.STD = 0x00; + G_STATUS_REG.UVLO = 0x00; + G_STATUS_REG.BPDF = 0x00; + G_STATUS_REG.APDF = 0x00; + G_STATUS_REG.BOCP = 0x00; + G_STATUS_REG.AOCP = 0x00; + G_STATUS_REG.OTS = 0x00; + + WriteAllRegisters(); +} + +void DRV8711DRV::set_current(uint32_t currentma) +{ + // derive torque and gain from current + float c = currentma / 1000.0F; // current in amps + // I = (2.75 * Torque) / (256 * GAIN * Rsense) use (5,10,20,40) for gain, 0.05 is RSense Torque is 0-255 + // torque= (256*gain*resistor*I)/2.75 + float a = 256.0F * gain * resistor; + float t = (c * a) / 2.75F; + while(t > 255) { + // reduce gain + gain = gain / 2; + if(gain < 5) { + gain = 5; + t = 255; + break; + } + a = 256.0F * gain * resistor; + t = (c * a) / 2.75F; + } + while(t < 1.0F) { + // increase gain + gain = gain * 2; + if(gain > 40) { + gain = 40; + t = 1; + break; + } + a = 256.0F * gain * resistor; + t = (c * a) / 2.75F; + } + + G_TORQUE_REG.TORQUE = t; + + switch (gain) { + case 5: + G_CTRL_REG.ISGAIN = 0x0; //Gain of 5 + break; + case 10: + G_CTRL_REG.ISGAIN = 0x01; //Gain of 10 + break; + case 20: + G_CTRL_REG.ISGAIN = 0x02; //Gain of 20 + break; + case 40: + G_CTRL_REG.ISGAIN = 0x03; //Gain of 40 + break; + } + + //THEKERNEL->streams->printf("for requested current of %lumA, torque= %u, gain= %u, actual current= %fA\n", currentma, G_TORQUE_REG.TORQUE, gain, (2.75F * t) / (256.0F * gain * resistor)); + // for current of 1.500000A, torque= 139, gain= 20 + + // set GAIN + uint8_t dataHi = REGWRITE | ((G_CTRL_REG.raw >> 8) & 0x7F); + uint8_t dataLo = (G_CTRL_REG.raw & 0x00FF); + ReadWriteRegister(dataHi, dataLo); + + // set TORQUE + dataHi = REGWRITE | ((G_TORQUE_REG.raw >> 8) & 0x7F); + dataLo = (G_TORQUE_REG.raw & 0x00FF); + ReadWriteRegister(dataHi, dataLo); +} + +int DRV8711DRV::set_microsteps(int number_of_steps) +{ + int microsteps; + if (number_of_steps >= 256) { + G_CTRL_REG.MODE = 0x08; + microsteps = 256; + } else if (number_of_steps >= 128) { + G_CTRL_REG.MODE = 0x07; + microsteps = 128; + } else if (number_of_steps >= 64) { + G_CTRL_REG.MODE = 0x06; + microsteps = 64; + } else if (number_of_steps >= 32) { + G_CTRL_REG.MODE = 0x05; + microsteps = 32; + } else if (number_of_steps >= 16) { + G_CTRL_REG.MODE = 0x04; + microsteps = 16; + } else if (number_of_steps >= 8) { + G_CTRL_REG.MODE = 0x03; + microsteps = 8; + } else if (number_of_steps >= 4) { + G_CTRL_REG.MODE = 0x02; + microsteps = 4; + } else if (number_of_steps >= 2) { + G_CTRL_REG.MODE = 0x01; + microsteps = 2; + } else { + //1 and 0 lead to full step + G_CTRL_REG.MODE = 0x0; + microsteps = 1; + } + + uint8_t dataHi = REGWRITE | ((G_CTRL_REG.raw >> 8) & 0x7F); + uint8_t dataLo = (G_CTRL_REG.raw & 0x00FF); + ReadWriteRegister(dataHi, dataLo); + return microsteps; +} + +void DRV8711DRV::set_enable (bool enable) +{ + // Set Enable + G_CTRL_REG.ENBL = enable ? 0x01 : 0x00; + uint8_t dataHi = REGWRITE | ((G_CTRL_REG.raw >> 8) & 0x7F); + uint8_t dataLo = (G_CTRL_REG.raw & 0x00FF); + ReadWriteRegister(dataHi, dataLo); +} + +void DRV8711DRV::dump_status(StreamOutput *stream) +{ + CTRL_Register_t R_CTRL_REG; + TORQUE_Register_t R_TORQUE_REG; + OFF_Register_t R_OFF_REG; + BLANK_Register_t R_BLANK_REG; + DECAY_Register_t R_DECAY_REG; + STALL_Register_t R_STALL_REG; + DRIVE_Register_t R_DRIVE_REG; + STATUS_Register_t R_STATUS_REG; + + stream->printf("designator: %c, Register Dump:\n", designator); + + // Read CTRL Register + R_CTRL_REG.raw= ReadRegister(G_CTRL_REG.Address); + stream->printf("CTRL: %04X (%04X): ", R_CTRL_REG.raw & 0x0FFF, G_CTRL_REG.raw & 0x0FFF); + stream->printf("DTIME: %u, ISGAIN: %u, EXSTALL: %u, MODE: %u, RSTEP: %u, RDIR: %u, ENBL: %u - ", + R_CTRL_REG.DTIME, R_CTRL_REG.ISGAIN, R_CTRL_REG.EXSTALL, R_CTRL_REG.MODE, R_CTRL_REG.RSTEP, R_CTRL_REG.RDIR, R_CTRL_REG.ENBL); + stream->printf("(DTIME: %u, ISGAIN: %u, EXSTALL: %u, MODE: %u, RSTEP: %u, RDIR: %u, ENBL: %u)\n", + G_CTRL_REG.DTIME, G_CTRL_REG.ISGAIN, G_CTRL_REG.EXSTALL, G_CTRL_REG.MODE, G_CTRL_REG.RSTEP, G_CTRL_REG.RDIR, G_CTRL_REG.ENBL); + + // Read TORQUE Register + R_TORQUE_REG.raw= ReadRegister(G_TORQUE_REG.Address); + stream->printf("TORQUE: %04X (%04X):", R_TORQUE_REG.raw & 0x0FFF, G_TORQUE_REG.raw & 0x0FFF); + stream->printf("SIMPLTH: %u, TORQUE: %u - ", R_TORQUE_REG.SIMPLTH, R_TORQUE_REG.TORQUE); + stream->printf("(SIMPLTH: %u, TORQUE: %u)\n", G_TORQUE_REG.SIMPLTH, G_TORQUE_REG.TORQUE); + + // Read OFF Register + R_OFF_REG.raw= ReadRegister(G_OFF_REG.Address); + stream->printf("OFF: %04X (%04X) - ", R_OFF_REG.raw & 0x0FFF, G_OFF_REG.raw & 0x0FFF); + stream->printf("PWMMODE: %u, TOFF: %u - ", R_OFF_REG.PWMMODE, R_OFF_REG.TOFF); + stream->printf("(PWMMODE: %u, TOFF: %u)\n", G_OFF_REG.PWMMODE, G_OFF_REG.TOFF); + + // Read BLANK Register + R_BLANK_REG.raw= ReadRegister(G_BLANK_REG.Address); + stream->printf("BLANK: %04X (%04X) - ", R_BLANK_REG.raw & 0x0FFF, G_BLANK_REG.raw & 0x0FFF); + stream->printf("ABT: %u, TBLANK: %u - ", R_BLANK_REG.ABT, R_BLANK_REG.TBLANK); + stream->printf("(ABT: %u, TBLANK: %u)\n", G_BLANK_REG.ABT, G_BLANK_REG.TBLANK); + + // Read DECAY Register + R_DECAY_REG.raw= ReadRegister(G_DECAY_REG.Address); + stream->printf("DECAY: %04X (%04X) - ", R_DECAY_REG.raw & 0x0FFF, G_DECAY_REG.raw & 0x0FFF); + stream->printf("DECMOD: %u, TDECAY: %u - ", R_DECAY_REG.DECMOD, R_DECAY_REG.TDECAY); + stream->printf("(DECMOD: %u, TDECAY: %u)\n", G_DECAY_REG.DECMOD, G_DECAY_REG.TDECAY); + + // Read STALL Register + R_STALL_REG.raw= ReadRegister(G_STALL_REG.Address); + stream->printf("STALL: %04X (%04X) - ", R_STALL_REG.raw & 0x0FFF, G_STALL_REG.raw & 0x0FFF); + stream->printf("VDIV: %u, SDCNT: %u, SDTHR: %u - ", R_STALL_REG.VDIV, R_STALL_REG.SDCNT, R_STALL_REG.SDTHR); + stream->printf("(VDIV: %u, SDCNT: %u, SDTHR: %u)\n", G_STALL_REG.VDIV, G_STALL_REG.SDCNT, G_STALL_REG.SDTHR); + + // Read DRIVE Register + R_DRIVE_REG.raw= ReadRegister(G_DRIVE_REG.Address); + stream->printf("DRIVE: %04X (%04X) - ", R_DRIVE_REG.raw & 0x0FFF, G_DRIVE_REG.raw & 0x0FFF); + stream->printf("IDRIVEP: %u, IDRIVEN: %u, TDRIVEP: %u, TDRIVEN: %u, OCPDEG: %u, OCPTH: %u - ", + R_DRIVE_REG.IDRIVEP, R_DRIVE_REG.IDRIVEN, R_DRIVE_REG.TDRIVEP, R_DRIVE_REG.TDRIVEN, R_DRIVE_REG.OCPDEG, R_DRIVE_REG.OCPTH); + stream->printf("(IDRIVEP: %u, IDRIVEN: %u, TDRIVEP: %u, TDRIVEN: %u, OCPDEG: %u, OCPTH: %u)\n", + G_DRIVE_REG.IDRIVEP, G_DRIVE_REG.IDRIVEN, G_DRIVE_REG.TDRIVEP, G_DRIVE_REG.TDRIVEN, G_DRIVE_REG.OCPDEG, G_DRIVE_REG.OCPTH); + + // Read STATUS Register + R_STATUS_REG.raw= ReadRegister(G_STATUS_REG.Address); + stream->printf("STATUS: %02X - ", R_STATUS_REG.raw & 0x00FF); + stream->printf("STDLAT: %u, STD: %u, UVLO: %u, BPDF: %u, APDF: %u, BOCP: %u, AOCP: %u, OTS: %u\n", + R_STATUS_REG.STDLAT, R_STATUS_REG.STD, R_STATUS_REG.UVLO, R_STATUS_REG.BPDF, R_STATUS_REG.APDF, R_STATUS_REG.BOCP, R_STATUS_REG.AOCP, R_STATUS_REG.OTS); + + int gain = R_CTRL_REG.ISGAIN == 0 ? 5 : R_CTRL_REG.ISGAIN == 1 ? 10 : R_CTRL_REG.ISGAIN == 2 ? 20 : R_CTRL_REG.ISGAIN == 3 ? 40 : 0; + stream->printf(" Current: %f\n", (2.75F * R_TORQUE_REG.TORQUE) / (256.0F * gain * resistor)); + stream->printf(" Microsteps: 1/%d\n", R_CTRL_REG.MODE > 0 ? 2 << (R_CTRL_REG.MODE-1) : 1); + + stream->printf(" motor_driver_control.xxx.reg %03X,%03X,%03X,%03X,%03X,%03X,%03X\n", + G_CTRL_REG.raw & 0x0FFF, G_TORQUE_REG.raw & 0x0FFF, G_OFF_REG.raw & 0x0FFF, G_BLANK_REG.raw & 0x0FFF, G_DECAY_REG.raw & 0x0FFF, G_STALL_REG.raw & 0x0FFF, G_DRIVE_REG.raw & 0x0FFF); +} + +void DRV8711DRV::WriteAllRegisters() +{ + uint8_t dataHi = 0x00; + uint8_t dataLo = 0x00; + + // Write CTRL Register + dataHi = REGWRITE | (G_CTRL_REG.Address << 4) | (G_CTRL_REG.DTIME << 2) | (G_CTRL_REG.ISGAIN); + dataLo = (G_CTRL_REG.EXSTALL << 7) | (G_CTRL_REG.MODE << 3) | (G_CTRL_REG.RSTEP << 2) | (G_CTRL_REG.RDIR << 1) | (G_CTRL_REG.ENBL); + ReadWriteRegister(dataHi, dataLo); + + // Write TORQUE Register + dataHi = REGWRITE | (G_TORQUE_REG.Address << 4) | (G_TORQUE_REG.SIMPLTH); + dataLo = G_TORQUE_REG.TORQUE; + ReadWriteRegister(dataHi, dataLo); + + // Write OFF Register + dataHi = REGWRITE | (G_OFF_REG.Address << 4) | (G_OFF_REG.PWMMODE); + dataLo = G_OFF_REG.TOFF; + ReadWriteRegister(dataHi, dataLo); + + // Write BLANK Register + dataHi = REGWRITE | (G_BLANK_REG.Address << 4) | (G_BLANK_REG.ABT); + dataLo = G_BLANK_REG.TBLANK; + ReadWriteRegister(dataHi, dataLo); + + // Write DECAY Register + dataHi = REGWRITE | (G_DECAY_REG.Address << 4) | (G_DECAY_REG.DECMOD); + dataLo = G_DECAY_REG.TDECAY; + ReadWriteRegister(dataHi, dataLo); + + // Write STALL Register + dataHi = REGWRITE | (G_STALL_REG.Address << 4) | (G_STALL_REG.VDIV << 2) | (G_STALL_REG.SDCNT); + dataLo = G_STALL_REG.SDTHR; + ReadWriteRegister(dataHi, dataLo); + + // Write DRIVE Register + dataHi = REGWRITE | (G_DRIVE_REG.Address << 4) | (G_DRIVE_REG.IDRIVEP << 2) | (G_DRIVE_REG.IDRIVEN); + dataLo = (G_DRIVE_REG.TDRIVEP << 6) | (G_DRIVE_REG.TDRIVEN << 4) | (G_DRIVE_REG.OCPDEG << 2) | (G_DRIVE_REG.OCPTH); + ReadWriteRegister(dataHi, dataLo); + + // Write STATUS Register + dataHi = REGWRITE | (G_STATUS_REG.Address << 4); + dataLo = (G_STATUS_REG.STDLAT << 7) | (G_STATUS_REG.STD << 6) | (G_STATUS_REG.UVLO << 5) | (G_STATUS_REG.BPDF << 4) | (G_STATUS_REG.APDF << 3) | (G_STATUS_REG.BOCP << 2) | (G_STATUS_REG.AOCP << 1) | (G_STATUS_REG.OTS); + ReadWriteRegister(dataHi, dataLo); +} + +bool DRV8711DRV::check_alarm() +{ + bool error= false; + STATUS_Register_t R_STATUS_REG; + // Read STATUS Register + R_STATUS_REG.raw= ReadRegister(G_STATUS_REG.Address); + + if(R_STATUS_REG.OTS) { + if(!error_reported.test(0)) THEKERNEL->streams->printf("%c, ERROR: Overtemperature shutdown\n", designator); + error= true; + error_reported.set(0); + }else{ + error_reported.reset(0); + } + + + if(R_STATUS_REG.AOCP) { + if(!error_reported.test(1)) THEKERNEL->streams->printf("%c, ERROR: Channel A over current shutdown\n", designator); + error= true; + error_reported.set(1); + }else{ + error_reported.reset(1); + } + + + if(R_STATUS_REG.BOCP) { + if(!error_reported.test(2)) THEKERNEL->streams->printf("%c, ERROR: Channel B over current shutdown\n", designator); + error= true; + error_reported.set(2); + }else{ + error_reported.reset(2); + } + + if(R_STATUS_REG.APDF) { + if(!error_reported.test(3)) THEKERNEL->streams->printf("%c, ERROR: Channel A predriver fault\n", designator); + error= true; + error_reported.set(3); + }else{ + error_reported.reset(3); + } + + + if(R_STATUS_REG.BPDF) { + if(!error_reported.test(4)) THEKERNEL->streams->printf("%c, ERROR: Channel B predriver fault\n", designator); + error= true; + error_reported.set(4); + }else{ + error_reported.reset(4); + } + + + return error; +} + + +// sets a raw register to the value specified, for advanced settings +// register 255 writes them, 0 displays what registers are mapped to what +bool DRV8711DRV::set_raw_register(StreamOutput *stream, uint32_t reg, uint32_t val) +{ + switch(reg) { + case 255: + WriteAllRegisters(); + stream->printf("Registers written\n"); + break; + + case 1: G_CTRL_REG.raw &= 0xF000; G_CTRL_REG.raw |= (val & 0x0FFF); break; + case 2: G_TORQUE_REG.raw &= 0xF000; G_TORQUE_REG.raw |= (val & 0x0FFF); break; + case 3: G_OFF_REG.raw &= 0xF000; G_OFF_REG.raw |= (val & 0x0FFF); break; + case 4: G_BLANK_REG.raw &= 0xF000; G_BLANK_REG.raw |= (val & 0x0FFF); break; + case 5: G_DECAY_REG.raw &= 0xF000; G_DECAY_REG.raw |= (val & 0x0FFF); break; + case 6: G_STALL_REG.raw &= 0xF000; G_STALL_REG.raw |= (val & 0x0FFF); break; + case 7: G_DRIVE_REG.raw &= 0xF000; G_DRIVE_REG.raw |= (val & 0x0FFF); break; + + default: + stream->printf("1: CTRL Register\n"); + stream->printf("2: TORQUE Register\n"); + stream->printf("3: OFF Register\n"); + stream->printf("4: BLANK Register\n"); + stream->printf("5: DECAY Register\n"); + stream->printf("6: STALL Register\n"); + stream->printf("7: DRIVE Register\n"); + stream->printf("255: write registers to chip\n"); + return false; + } + return true; +} + +uint16_t DRV8711DRV::ReadRegister(uint8_t addr) +{ + return ReadWriteRegister(REGREAD | (addr << 4), 0); +} + +uint16_t DRV8711DRV::ReadWriteRegister(uint8_t dataHi, uint8_t dataLo) +{ + uint8_t buf[2] {dataHi, dataLo}; + uint8_t rbuf[2]; + + spi(buf, 2, rbuf); + //THEKERNEL->streams->printf("sent: %02X, %02X received:%02X, %02X\n", buf[0], buf[1], rbuf[0], rbuf[1]); + uint16_t readData = (rbuf[0] << 8) | rbuf[1]; + return readData; +} + +#if 0 +#define HAS(X) (options.find(X) != options.end()) +#define GET(X) (options.at(X)) +bool DRV8711DRV::set_options(const options_t& options) +{ + bool set = false; + if(HAS('O') || HAS('Q')) { + // void TMC26X::setStallGuardThreshold(int8_t stall_guard_threshold, int8_t stall_guard_filter_enabled) + int8_t o = HAS('O') ? GET('O') : getStallGuardThreshold(); + int8_t q = HAS('Q') ? GET('Q') : getStallGuardFilter(); + setStallGuardThreshold(o, q); + set = true; + } + + if(HAS('S')) { + uint32_t s = GET('S'); + if(s == 0 && HAS('U') && HAS('V') && HAS('W') && HAS('X') && HAS('Y')) { + //void TMC26X::setConstantOffTimeChopper(int8_t constant_off_time, int8_t blank_time, int8_t fast_decay_time_setting, int8_t sine_wave_offset, uint8_t use_current_comparator) + setConstantOffTimeChopper(GET('U'), GET('V'), GET('W'), GET('X'), GET('Y')); + set = true; + + } else if(s == 2 && HAS('Z')) { + setRandomOffTime(GET('Z')); + set = true; + } + } + + return set; +} +#endif diff --git a/src/modules/utils/motordrivercontrol/drivers/DRV8711/drv8711.h b/src/modules/utils/motordrivercontrol/drivers/DRV8711/drv8711.h new file mode 100644 index 00000000..0b893bfb --- /dev/null +++ b/src/modules/utils/motordrivercontrol/drivers/DRV8711/drv8711.h @@ -0,0 +1,151 @@ +#pragma once + +#include +#include + +class StreamOutput; + +#define DRV8711_max_current 4000 + +class DRV8711DRV +{ +public: + DRV8711DRV(std::function spi, char designator); + + void init(uint16_t cs) ; + + void set_enable(bool enable) ; + int set_microsteps(int number_of_steps); + void set_current(uint32_t currentma); + + void dump_status(StreamOutput *stream) ; + bool set_raw_register(StreamOutput *stream, uint32_t reg, uint32_t val); + bool check_alarm(); + +private: + + uint16_t ReadWriteRegister(uint8_t dataHi, uint8_t dataLo); + uint16_t ReadRegister(uint8_t addr); + void ReadAllRegisters () ; + void WriteAllRegisters () ; + +// WARNING this may not be portable, and endianess affects the order, but it works for LPC1769 +// CTRL Register + typedef union { + struct { + uint8_t ENBL: 1; // bit 0 + uint8_t RDIR: 1; // bit 1 + uint8_t RSTEP: 1; // bit 2 + uint8_t MODE: 4; // bits 6-3 + uint8_t EXSTALL: 1; // bit 7 + uint8_t ISGAIN: 2; // bits 9-8 + uint8_t DTIME: 2; // bits 11-10 + uint8_t Address: 3; // bits 14-12 + }; + uint16_t raw; + } CTRL_Register_t; + +// TORQUE Register + typedef union { + struct { + uint8_t TORQUE: 8; // bits 7-0 + uint8_t SIMPLTH: 3; // bits 10-8 + uint8_t Reserved: 1; // bit 11 + uint8_t Address: 3; // bits 14-12 + }; + uint16_t raw; + } TORQUE_Register_t; + +// OFF Register + typedef union { + struct { + uint8_t TOFF: 8; // bits 7-0 + uint8_t PWMMODE: 1; // bit 8 + uint8_t Reserved: 3; // bits 11-9 + uint8_t Address: 3; // bits 14-12 + }; + uint16_t raw; + } OFF_Register_t; + +// BLANK Register + typedef union { + struct { + uint8_t TBLANK:8; // bits 7-0 + uint8_t ABT:1; // bit 8 + uint8_t Reserved:3; // bits 11-9 + uint8_t Address:3; // bits 14-12 + }; + uint16_t raw; + } BLANK_Register_t; + +// DECAY Register + typedef union { + struct { + uint8_t TDECAY:8; // bits 7-0 + uint8_t DECMOD:3; // bits 10-8 + uint8_t Reserved:1; // bit 11 + uint8_t Address:3; // bits 14-12 + }; + uint16_t raw; + } DECAY_Register_t; + +// STALL Register + typedef union { + struct { + uint8_t SDTHR:8; // bits 7-0 + uint8_t SDCNT:2; // bits 9-8 + uint8_t VDIV:2; // bits 11-10 + uint8_t Address:3; // bits 14-12 + }; + uint16_t raw; + } STALL_Register_t; + +// DRIVE Register + typedef union { + struct { + uint8_t OCPTH:2; // bits 1-0 + uint8_t OCPDEG:2; // bits 3-2 + uint8_t TDRIVEN:2; // bits 5-4 + uint8_t TDRIVEP:2; // bits 7-6 + uint8_t IDRIVEN:2; // bits 9-8 + uint8_t IDRIVEP:2; // bits 11-10 + uint8_t Address:3; // bits 14-12 + }; + uint16_t raw; + } DRIVE_Register_t; + +// STATUS Register + typedef union { + struct { + uint8_t OTS:1; // bit 0 + uint8_t AOCP:1; // bit 1 + uint8_t BOCP:1; // bit 2 + uint8_t APDF:1; // bit 3 + uint8_t BPDF:1; // bit 4 + uint8_t UVLO:1; // bit 5 + uint8_t STD:1; // bit 6 + uint8_t STDLAT:1; // bit 7 + uint8_t Reserved:4; // bits 11-8 + uint8_t Address:3; // bits 14-12 + }; + uint16_t raw; + } STATUS_Register_t; + + CTRL_Register_t G_CTRL_REG; + TORQUE_Register_t G_TORQUE_REG; + OFF_Register_t G_OFF_REG; + BLANK_Register_t G_BLANK_REG; + DECAY_Register_t G_DECAY_REG; + STALL_Register_t G_STALL_REG; + DRIVE_Register_t G_DRIVE_REG; + STATUS_Register_t G_STATUS_REG; + + std::function spi; + float resistor{0.05}; + std::bitset<8> error_reported; + uint8_t gain{20}; + char designator; + + // float _amps; + // uint8_t _microstepreg; +}; diff --git a/src/modules/utils/motordrivercontrol/drivers/TMC21X/TMC21X.cpp b/src/modules/utils/motordrivercontrol/drivers/TMC21X/TMC21X.cpp new file mode 100644 index 00000000..7110a5f6 --- /dev/null +++ b/src/modules/utils/motordrivercontrol/drivers/TMC21X/TMC21X.cpp @@ -0,0 +1,1380 @@ +/* + Highly modified from.... + + TMC21X.cpp - - TMC21X Stepper library for Wiring/Arduino + + based on the stepper library by Tom Igoe, et. al. + + Copyright (c) 2011, Interactive Matter, Marcus Nowotny + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + 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. + + */ + +#include "TMC21X.h" +#include "mbed.h" +#include "StreamOutput.h" +#include "Kernel.h" +#include "libs/StreamOutputPool.h" +#include "Robot.h" +#include "StepperMotor.h" +#include "ConfigValue.h" +#include "Config.h" +#include "checksumm.h" + + +#define motor_driver_control_checksum CHECKSUM("motor_driver_control") +#define sense_resistor_checksum CHECKSUM("sense_resistor") + +//! return value for TMC21X.getOverTemperature() if there is a over temperature situation in the TMC chip +/*! + * This warning indicates that the TCM chip is too warm. + * It is still working but some parameters may be inferior. + * You should do something against it. + */ +#define TMC21X_OVERTEMPERATURE_PREWARING 1 +//! return value for TMC21X.getOverTemperature() if there is a over temperature shutdown in the TMC chip +/*! + * This warning indicates that the TCM chip is too warm to operate and has shut down to prevent damage. + * It will stop working until it cools down again. + * If you encounter this situation you must do something against it. Like reducing the current or improving the PCB layout + * and/or heat management. + */ +#define TMC21X_OVERTEMPERATURE_SHUTDOWN 2 + +//which values can be read out +/*! + * Selects to readout the microstep position from the motor. + *\sa readStatus() + */ +#define TMC21X_READOUT_POSITION 0 +/*! + * Selects to read out the StallGuard value of the motor. + *\sa readStatus() + */ +#define TMC21X_READOUT_STALLGUARD 1 +/*! + * Selects to read out the current current setting (acc. to CoolStep) and the upper bits of the StallGuard value from the motor. + *\sa readStatus(), setCurrent() + */ +#define TMC21X_READOUT_CURRENT 3 + +/*! + * Define to set the minimum current for CoolStep operation to 1/2 of the selected CS minium. + *\sa setCoolStepConfiguration() + */ +#define COOL_STEP_HALF_CS_LIMIT 0 +/*! + * Define to set the minimum current for CoolStep operation to 1/4 of the selected CS minimum. + *\sa setCoolStepConfiguration() + */ +#define COOL_STEP_QUARTDER_CS_LIMIT 1 + + +//some default values used in initialization +#define DEFAULT_MICROSTEPPING_VALUE 32 + +//TMC2130 read / write mask +#define TMC_WRITE_notREAD 0x80 //write flag + +//TMC2130 registers definitions // R = Read +// // W = Write +// // RC = Read +// // and Clear + // Number // + // of Bits// +#define REG_GCONF 0x00 // RW // 17 //Global configuration flags +#define REG_GSTAT 0x01 // RC // 3 //Global status flags +#define REG_IOIN 0x04 // R // 8+8 //The state of all input pins + //VELOCITY DEPENDENT DRIVER FEATURE CONTROL REGISTER SET (0X10…0X1F) +#define REG_IHOLD_IRUN 0x10 // W // 5+5+4 //Driver current control +#define REG_TPOWERDOWN 0x11 // W // 8 // sets the delay time after stand still (stst) of the motor to motor current power down. 8 bits + //Time range is about 0 to 4 seconds. 0…((2^8)-1) * 2^18 tCLK +#define REG_TSTEP 0x12 // R // 20 //Actual measured time between two 1/256 microsteps derived from the step input frequency. 20 bits + //In units of 1/fCLK. Measured value is (2^20)-1 in case of overflow or stand still. + //All TSTEP related thresholds use a hysteresis of 1/16 of the compare value to compensate for jitter in the clock or the step frequency. + //The flag small_hysteresis modifies the hysteresis to a smaller value of 1/32. + //(Txxx*15/16)-1 or (Txxx*31/32)-1 is used as a second compare value for each comparison value. + //This means, that the lower switching velocity equals the calculated setting, + //but the upper switching velocity is higher as defined by the hysteresis setting. + //In dcStep mode TSTEP will not show the mean velocity of the motor, but the velocities for each microstep, + //which may not be stable and thus does not represent the real motor velocity in case it runs slower than the target velocity. +#define REG_TPWMTHRS 0x13 // W // 20 //This is the upper velocity for stealthChop voltage PWM mode. + //TSTEP ≥ TPWMTHRS + // - stealthChop PWM mode is enabled, if configured + // - dcStep is disabled +#define REG_TCOOLTHRS 0x14 // W // 20 //This is the lower threshold velocity for switching on smart energy coolStep and stallGuard feature. + // (unsigned) + //Set this parameter to disable coolStep at low speeds, where it cannot work reliably. + //The stall detection and stallGuard output signal becomes enabled when exceeding this velocity. + //In non-dcStep mode, it becomes disabled again once the velocity falls below this threshold. + //TCOOLTHRS ≥ TSTEP ≥ THIGH: + // - coolStep is enabled, if configured + // - stealthChop voltage PWM mode is disabled + //TCOOLTHRS ≥ TSTEP + // - Stop on stall and stall output signal is enabled, if configured +#define REG_THIGH 0x15 // W // 20 //This velocity setting allows velocity dependent switching into a different chopper mode and fullstepping to maximize torque. + // (unsigned) + //The stall detection feature becomes switched off for 2-3 electrical periods whenever passing THIGH threshold to compensate for the effect of switching modes. + //TSTEP ≤ THIGH: + // - coolStep is disabled (motor runs with normal current scale) + // - stealthChop voltage PWM mode is disabled + // - If vhighchm is set, the chopper switches to chm=1 with TFD=0 (constant off time with slow decay, only). + // - chopSync2 is switched off (SYNC=0) + // - If vhighfs is set, the motor operates in fullstep mode and the stall detection becomes switched over to dcStep stall detection. + // + //NOTE: microstep velocity time reference t for velocities: TSTEP = fCLK / fSTEP +#define REG_XDIRECT 0x2D // RW // 32 // SPI Mode Register + //0: Normal operation + //1: Directly SPI driven motor current + // Direct mode operation: + // XDIRECT specifies Motor coil currents and polarity directly programmed via the serial interface. Use signed, two’s complement numbers. + // Coil A current (bits 8..0) (signed) + // Coil B current (bits 24..16) (signed) + // Range: +-248 for normal operation, up to +-255 with stealthChop + //In this mode, the current is scaled by IHOLD setting. + //Velocity based current regulation of voltage PWM is not available in this mode. + //The automatic voltage PWM current regulation will work only for low stepper motor velocities. + //dcStep is not available in this mode. coolStep and stallGuard only can be used, when additionally supplying a STEP signal. + //This will also enable automatic current scaling. +#define REG_VDCMIN 0x33 // W // 23 //dcStep Minimum Velocity Register + //The automatic commutation dcStep becomes enabled by the external signal DCEN. + //VDCMIN is used as the minimum step velocity when the motor is heavily loaded. + //Hint: Also set DCCTRL parameters in order to operate dcStep. + // + // MICROSTEPPING CONTROL REGISTER SET (0X60…0X6B) + //Each bit gives the difference between entry x and entry x+1 when combined with the + //corresponding MSLUTSEL W bits: + //0: W= %00: -1 + // %01: +0 + // %10: +1 + // %11: +2 + // 1: W= %00: +0 + // %01: +1 + // %10: +2 + // %11: +3 + // This is the differential coding for the first quarter of a wave. + // Start values for CUR_A and CUR_B are stored for MSCNT position 0 in + // START_SIN and START_SIN90. + // ofs31, ofs30, …, ofs01, ofs00 + // … + // ofs255, ofs254, …, ofs225, ofs224 +#define REG_MSLUT0 0x60 // W // 32 // Bits 0..31 - 1 x 32 bits of 0 or 1 reset default= sine wave table +#define REG_MSLUT1 0x61 // W // 32 // Bits 7 x 32 bits of 0 or 1 reset default= sine wave table +#define REG_MSLUT2 0x62 // W // 32 // 32 +#define REG_MSLUT3 0x63 // W // 32 // . +#define REG_MSLUT4 0x64 // W // 32 // . +#define REG_MSLUT5 0x65 // W // 32 // . +#define REG_MSLUT6 0x66 // W // 32 // +#define REG_MSLUT7 0x67 // W // 32 // 255 +#define REG_MSLUTSEL 0x68 // W // 32 //This register defines four segments within each quarter MSLUT wave. + //Four 2 bit entries determine the meaning of a 0 and a 1 bit in the corresponding segment of MSLUT. + //Range: 0 spi, char d) : spi(spi), designator(d) //TODO Convert +{ + //we are not started yet + started = false; + //by default cool step is not enabled + cool_step_enabled = false; + error_reported.reset(); +} + +/* + * configure the stepper driver + * just must be called. + */ +void TMC21X::init(uint16_t cs) //TODO Convert +{ + // read chip specific config entries + this->resistor= THEKERNEL->config->value(motor_driver_control_checksum, cs, sense_resistor_checksum)->by_default(50)->as_number(); // in milliohms + + // Initialize the TMC2130 + //TODO need to generalize this so any device using SPI will work + //TODO need to add semaphores or similar to prevent more than one SPI call happening at once + //set TMC2130 config + this->send2130(TMC_WRITE_notREAD|REG_GCONF, 0x00000001UL); //voltage on AIN is current reference + this->send2130(TMC_WRITE_notREAD|REG_IHOLD_IRUN, 0x00001010UL); //IHOLD=0x10, IRUN=0x10 + this->send2130(TMC_WRITE_notREAD|REG_CHOPCONF, 0x00008008UL); //native 256 microsteps, MRES=0, TBL=1=24, TOFF=8 + + + + /* + //setting the default register values + driver_control_register_value = DRIVER_CONTROL_REGISTER; + chopper_config_register = CHOPPER_CONFIG_REGISTER; + cool_step_register_value = COOL_STEP_REGISTER; + stall_guard2_current_register_value = STALL_GUARD2_LOAD_MEASURE_REGISTER; + driver_configuration_register_value = DRIVER_CONFIG_REGISTER | READ_STALL_GUARD_READING; + + //set the initial values + + + + send2130_old(driver_control_register_value); + send2130_old(chopper_config_register); + send2130_old(cool_step_register_value); + send2130_old(stall_guard2_current_register_value); + send2130_old(driver_configuration_register_value); +*/ + started = true; +/* +#if 1 + //set to a conservative start value + setConstantOffTimeChopper(7, 54, 13, 12, 1); +#else + // for 1.5amp kysan @ 12v + setSpreadCycleChopper(5, 54, 5, 0, 0); + // for 4amp Nema24 @ 12v + //setSpreadCycleChopper(5, 54, 4, 0, 0); +#endif +*/ + setEnabled(false); + + //set a nice microstepping value +// setMicrosteps(DEFAULT_MICROSTEPPING_VALUE); + + // set stallguard to a conservative value so it doesn't trigger immediately +// setStallGuardThreshold(10, 1); +} + +void TMC21X::setCurrent(unsigned int current) //TODO Convert +{ + uint8_t current_scaling = 0; + //calculate the current scaling from the max current setting (in mA) + double mASetting = (double)current; + double resistor_value = (double) this->resistor; + // remove vesense flag + this->driver_configuration_register_value &= ~(VSENSE); + //this is derrived from I=(cs+1)/32*(Vsense/Rsense) + //leading to cs = CS = 32*R*I/V (with V = 0,31V oder 0,165V and I = 1000*current) + //with Rsense=0,15 + //for vsense = 0,310V (VSENSE not set) + //or vsense = 0,165V (VSENSE set) + current_scaling = (uint8_t)((resistor_value * mASetting * 32.0F / (0.31F * 1000.0F * 1000.0F)) - 0.5F); //theoretically - 1.0 for better rounding it is 0.5 + + //check if the current scaling is too low + if (current_scaling < 16) { + //set the csense bit to get a use half the sense voltage (to support lower motor currents) + this->driver_configuration_register_value |= VSENSE; + //and recalculate the current setting + current_scaling = (uint8_t)((resistor_value * mASetting * 32.0F / (0.165F * 1000.0F * 1000.0F)) - 0.5F); //theoretically - 1.0 for better rounding it is 0.5 + } + + //do some sanity checks + if (current_scaling > 31) { + current_scaling = 31; + } + //delete the old value + stall_guard2_current_register_value &= ~(CURRENT_SCALING_PATTERN); + //set the new current scaling + stall_guard2_current_register_value |= current_scaling; + //if started we directly send it to the motor + if (started) { + send2130_old(driver_configuration_register_value); + send2130_old(stall_guard2_current_register_value); + } +} + +unsigned int TMC21X::getCurrent(void) //TODO Convert +{ + + //we calculate the current according to the datasheet to be on the safe side + //this is not the fastest but the most accurate and illustrative way + double result = (double)(stall_guard2_current_register_value & CURRENT_SCALING_PATTERN); + double resistor_value = (double)this->resistor; + double voltage = (driver_configuration_register_value & VSENSE) ? 0.165F : 0.31F; + result = (result + 1.0F) / 32.0F * voltage / resistor_value * 1000.0F * 1000.0F; + return (unsigned int)result; +} + +void TMC21X::setStallGuardThreshold(int8_t stall_guard_threshold, int8_t stall_guard_filter_enabled) //TODO Convert +{ + if (stall_guard_threshold < -64) { + stall_guard_threshold = -64; + //We just have 5 bits + } else if (stall_guard_threshold > 63) { + stall_guard_threshold = 63; + } + //add trim down to 7 bits + stall_guard_threshold &= 0x7f; + //delete old stall guard settings + stall_guard2_current_register_value &= ~(STALL_GUARD_CONFIG_PATTERN); + if (stall_guard_filter_enabled) { + stall_guard2_current_register_value |= STALL_GUARD_FILTER_ENABLED; + } + //Set the new stall guard threshold + stall_guard2_current_register_value |= (((unsigned long)stall_guard_threshold << 8) & STALL_GUARD_CONFIG_PATTERN); + //if started we directly send it to the motor + if (started) { + send2130_old(stall_guard2_current_register_value); + } +} + +int8_t TMC21X::getStallGuardThreshold(void) //TODO Convert +{ + unsigned long stall_guard_threshold = stall_guard2_current_register_value & STALL_GUARD_VALUE_PATTERN; + //shift it down to bit 0 + stall_guard_threshold >>= 8; + //convert the value to an int to correctly handle the negative numbers + int8_t result = stall_guard_threshold; + //check if it is negative and fill it up with leading 1 for proper negative number representation + if (result & (1 << 6)) { + result |= 0xC0; + } + return result; +} + +int8_t TMC21X::getStallGuardFilter(void) //TODO Convert +{ + if (stall_guard2_current_register_value & STALL_GUARD_FILTER_ENABLED) { + return -1; + } else { + return 0; + } +} +/* + * Set the number of microsteps per step. + * 0,2,4,8,16,32,64,128,256 is supported + * any value in between will be mapped to the next smaller value + * 0 and 1 set the motor in full step mode + */ +void TMC21X::setMicrosteps(int number_of_steps) //TODO Convert +{ + long setting_pattern; + //poor mans log + if (number_of_steps >= 256) { + setting_pattern = 0; + microsteps = 256; + } else if (number_of_steps >= 128) { + setting_pattern = 1; + microsteps = 128; + } else if (number_of_steps >= 64) { + setting_pattern = 2; + microsteps = 64; + } else if (number_of_steps >= 32) { + setting_pattern = 3; + microsteps = 32; + } else if (number_of_steps >= 16) { + setting_pattern = 4; + microsteps = 16; + } else if (number_of_steps >= 8) { + setting_pattern = 5; + microsteps = 8; + } else if (number_of_steps >= 4) { + setting_pattern = 6; + microsteps = 4; + } else if (number_of_steps >= 2) { + setting_pattern = 7; + microsteps = 2; + //1 and 0 lead to full step + } else if (number_of_steps <= 1) { + setting_pattern = 8; + microsteps = 1; + } + + //delete the old value + this->driver_control_register_value &= 0xFFFF0ul; + //set the new value + this->driver_control_register_value |= setting_pattern; + + //if started we directly send it to the motor + if (started) { + send2130_old(driver_control_register_value); + } +} + +/* + * returns the effective number of microsteps at the moment + */ +int TMC21X::getMicrosteps(void) //TODO Convert +{ + return microsteps; +} + +void TMC21X::setStepInterpolation(int8_t value) +{ + if (value) { + driver_control_register_value |= STEP_INTERPOLATION; + } else { + driver_control_register_value &= ~(STEP_INTERPOLATION); + } + //if started we directly send it to the motor + if (started) { + send2130_old(driver_control_register_value); + } +} + +void TMC21X::setDoubleEdge(int8_t value) //TODO Convert +{ + if (value) { + driver_control_register_value |= DOUBLE_EDGE_STEP; + } else { + driver_control_register_value &= ~(DOUBLE_EDGE_STEP); + } + //if started we directly send it to the motor + if (started) { + send2130_old(driver_control_register_value); + } +} + +/* + * constant_off_time: The off time setting controls the minimum chopper frequency. + * For most applications an off time within the range of 5μs to 20μs will fit. + * 2...15: off time setting + * + * blank_time: Selects the comparator blank time. This time needs to safely cover the switching event and the + * duration of the ringing on the sense resistor. For + * 0: min. setting 3: max. setting + * + * fast_decay_time_setting: Fast decay time setting. With CHM=1, these bits control the portion of fast decay for each chopper cycle. + * 0: slow decay only + * 1...15: duration of fast decay phase + * + * sine_wave_offset: Sine wave offset. With CHM=1, these bits control the sine wave offset. + * A positive offset corrects for zero crossing error. + * -3..-1: negative offset 0: no offset 1...12: positive offset + * + * use_current_comparator: Selects usage of the current comparator for termination of the fast decay cycle. + * If current comparator is enabled, it terminates the fast decay cycle in case the current + * reaches a higher negative value than the actual positive value. + * 1: enable comparator termination of fast decay cycle + * 0: end by time only + */ +void TMC21X::setConstantOffTimeChopper(int8_t constant_off_time, int8_t blank_time, int8_t fast_decay_time_setting, int8_t sine_wave_offset, uint8_t use_current_comparator) //TODO Convert +{ + //perform some sanity checks + if (constant_off_time < 2) { + constant_off_time = 2; + } else if (constant_off_time > 15) { + constant_off_time = 15; + } + //save the constant off time + this->constant_off_time = constant_off_time; + int8_t blank_value; + //calculate the value acc to the clock cycles + if (blank_time >= 54) { + blank_value = 3; + } else if (blank_time >= 36) { + blank_value = 2; + } else if (blank_time >= 24) { + blank_value = 1; + } else { + blank_value = 0; + } + this->blank_time = blank_time; + + if (fast_decay_time_setting < 0) { + fast_decay_time_setting = 0; + } else if (fast_decay_time_setting > 15) { + fast_decay_time_setting = 15; + } + if (sine_wave_offset < -3) { + sine_wave_offset = -3; + } else if (sine_wave_offset > 12) { + sine_wave_offset = 12; + } + //shift the sine_wave_offset + sine_wave_offset += 3; + + //calculate the register setting + //first of all delete all the values for this + chopper_config_register &= ~((1 << 12) | BLANK_TIMING_PATTERN | HYSTERESIS_DECREMENT_PATTERN | HYSTERESIS_LOW_VALUE_PATTERN | HYSTERESIS_START_VALUE_PATTERN | T_OFF_TIMING_PATERN); + //set the constant off pattern + chopper_config_register |= CHOPPER_MODE_T_OFF_FAST_DECAY; + //set the blank timing value + chopper_config_register |= ((unsigned long)blank_value) << BLANK_TIMING_SHIFT; + //setting the constant off time + chopper_config_register |= constant_off_time; + //set the fast decay time + //set msb + chopper_config_register |= (((unsigned long)(fast_decay_time_setting & 0x8)) << HYSTERESIS_DECREMENT_SHIFT); + //other bits + chopper_config_register |= (((unsigned long)(fast_decay_time_setting & 0x7)) << HYSTERESIS_START_VALUE_SHIFT); + //set the sine wave offset + chopper_config_register |= (unsigned long)sine_wave_offset << HYSTERESIS_LOW_SHIFT; + //using the current comparator? + if (!use_current_comparator) { + chopper_config_register |= (1 << 12); + } + //if started we directly send it to the motor + if (started) { + send2130_old(chopper_config_register); + } +} + +/* + * constant_off_time: The off time setting controls the minimum chopper frequency. + * For most applications an off time within the range of 5μs to 20μs will fit. + * 2...15: off time setting + * + * blank_time: Selects the comparator blank time. This time needs to safely cover the switching event and the + * duration of the ringing on the sense resistor. For + * 0: min. setting 3: max. setting + * + * hysteresis_start: Hysteresis start setting. Please remark, that this value is an offset to the hysteresis end value HEND. + * 1...8 + * + * hysteresis_end: Hysteresis end setting. Sets the hysteresis end value after a number of decrements. Decrement interval time is controlled by HDEC. + * The sum HSTRT+HEND must be <16. At a current setting CS of max. 30 (amplitude reduced to 240), the sum is not limited. + * -3..-1: negative HEND 0: zero HEND 1...12: positive HEND + * + * hysteresis_decrement: Hysteresis decrement setting. This setting determines the slope of the hysteresis during on time and during fast decay time. + * 0: fast decrement 3: very slow decrement + */ + +void TMC21X::setSpreadCycleChopper(int8_t constant_off_time, int8_t blank_time, int8_t hysteresis_start, int8_t hysteresis_end, int8_t hysteresis_decrement) //TODO Convert +{ + h_start = hysteresis_start; + h_end = hysteresis_end; + h_decrement = hysteresis_decrement; + this->blank_time = blank_time; + + //perform some sanity checks + if (constant_off_time < 2) { + constant_off_time = 2; + } else if (constant_off_time > 15) { + constant_off_time = 15; + } + //save the constant off time + this->constant_off_time = constant_off_time; + int8_t blank_value; + //calculate the value acc to the clock cycles + if (blank_time >= 54) { + blank_value = 3; + } else if (blank_time >= 36) { + blank_value = 2; + } else if (blank_time >= 24) { + blank_value = 1; + } else { + blank_value = 0; + } + + if (hysteresis_start < 1) { + hysteresis_start = 1; + } else if (hysteresis_start > 8) { + hysteresis_start = 8; + } + hysteresis_start--; + + if (hysteresis_end < -3) { + hysteresis_end = -3; + } else if (hysteresis_end > 12) { + hysteresis_end = 12; + } + //shift the hysteresis_end + hysteresis_end += 3; + + if (hysteresis_decrement < 0) { + hysteresis_decrement = 0; + } else if (hysteresis_decrement > 3) { + hysteresis_decrement = 3; + } + + //first of all delete all the values for this + chopper_config_register &= ~(CHOPPER_MODE_T_OFF_FAST_DECAY | BLANK_TIMING_PATTERN | HYSTERESIS_DECREMENT_PATTERN | HYSTERESIS_LOW_VALUE_PATTERN | HYSTERESIS_START_VALUE_PATTERN | T_OFF_TIMING_PATERN); + + //set the blank timing value + chopper_config_register |= ((unsigned long)blank_value) << BLANK_TIMING_SHIFT; + //setting the constant off time + chopper_config_register |= constant_off_time; + //set the hysteresis_start + chopper_config_register |= ((unsigned long)hysteresis_start) << HYSTERESIS_START_VALUE_SHIFT; + //set the hysteresis end + chopper_config_register |= ((unsigned long)hysteresis_end) << HYSTERESIS_LOW_SHIFT; + //set the hystereis decrement + chopper_config_register |= ((unsigned long)hysteresis_decrement) << HYSTERESIS_DECREMENT_SHIFT; + + //if started we directly send it to the motor + if (started) { + send2130_old(chopper_config_register); + } +} + +/* + * In a constant off time chopper scheme both coil choppers run freely, i.e. are not synchronized. + * The frequency of each chopper mainly depends on the coil current and the position dependant motor coil inductivity, thus it depends on the microstep position. + * With some motors a slightly audible beat can occur between the chopper frequencies, especially when they are near to each other. This typically occurs at a + * few microstep positions within each quarter wave. This effect normally is not audible when compared to mechanical noise generated by ball bearings, etc. + * Further factors which can cause a similar effect are a poor layout of sense resistor GND connection. + * Hint: A common factor, which can cause motor noise, is a bad PCB layout causing coupling of both sense resistor voltages + * (please refer to sense resistor layout hint in chapter 8.1). + * In order to minimize the effect of a beat between both chopper frequencies, an internal random generator is provided. + * It modulates the slow decay time setting when switched on by the RNDTF bit. The RNDTF feature further spreads the chopper spectrum, + * reducing electromagnetic emission on single frequencies. + */ +void TMC21X::setRandomOffTime(int8_t value) //TODO Convert +{ + if (value) { + chopper_config_register |= RANDOM_TOFF_TIME; + } else { + chopper_config_register &= ~(RANDOM_TOFF_TIME); + } + //if started we directly send it to the motor + if (started) { + send2130_old(chopper_config_register); + } +} + +void TMC21X::setCoolStepConfiguration(unsigned int lower_SG_threshold, unsigned int SG_hysteresis, uint8_t current_decrement_step_size, + uint8_t current_increment_step_size, uint8_t lower_current_limit) //TODO Convert +{ + //sanitize the input values + if (lower_SG_threshold > 480) { + lower_SG_threshold = 480; + } + //divide by 32 + lower_SG_threshold >>= 5; + if (SG_hysteresis > 480) { + SG_hysteresis = 480; + } + //divide by 32 + SG_hysteresis >>= 5; + if (current_decrement_step_size > 3) { + current_decrement_step_size = 3; + } + if (current_increment_step_size > 3) { + current_increment_step_size = 3; + } + if (lower_current_limit > 1) { + lower_current_limit = 1; + } + //store the lower level in order to enable/disable the cool step + this->cool_step_lower_threshold = lower_SG_threshold; + //if cool step is not enabled we delete the lower value to keep it disabled + if (!this->cool_step_enabled) { + lower_SG_threshold = 0; + } + //the good news is that we can start with a complete new cool step register value + //and simply set the values in the register + cool_step_register_value = ((unsigned long)lower_SG_threshold) | (((unsigned long)SG_hysteresis) << 8) | (((unsigned long)current_decrement_step_size) << 5) + | (((unsigned long)current_increment_step_size) << 13) | (((unsigned long)lower_current_limit) << 15) + //and of course we have to include the signature of the register + | COOL_STEP_REGISTER; + + if (started) { + send2130_old(cool_step_register_value); + } +} + +void TMC21X::setCoolStepEnabled(bool enabled) //TODO Convert +{ + //simply delete the lower limit to disable the cool step + cool_step_register_value &= ~SE_MIN_PATTERN; + //and set it to the proper value if cool step is to be enabled + if (enabled) { + cool_step_register_value |= this->cool_step_lower_threshold; + } + //and save the enabled status + this->cool_step_enabled = enabled; + //save the register value + if (started) { + send2130_old(cool_step_register_value); + } +} + +bool TMC21X::isCoolStepEnabled(void) //TODO Convert +{ + return this->cool_step_enabled; +} + +unsigned int TMC21X::getCoolStepLowerSgThreshold() //TODO Convert +{ + //we return our internally stored value - in order to provide the correct setting even if cool step is not enabled + return this->cool_step_lower_threshold << 5; +} + +unsigned int TMC21X::getCoolStepUpperSgThreshold() //TODO Convert +{ + return (uint8_t)((cool_step_register_value & SE_MAX_PATTERN) >> 8) << 5; +} + +uint8_t TMC21X::getCoolStepCurrentIncrementSize() //TODO Convert +{ + return (uint8_t)((cool_step_register_value & CURRENT_DOWN_STEP_SPEED_PATTERN) >> 13); +} + +uint8_t TMC21X::getCoolStepNumberOfSGReadings() //TODO Convert +{ + return (uint8_t)((cool_step_register_value & SE_CURRENT_STEP_WIDTH_PATTERN) >> 5); +} + +uint8_t TMC21X::getCoolStepLowerCurrentLimit() //TODO Convert +{ + return (uint8_t)((cool_step_register_value & MINIMUM_CURRENT_FOURTH) >> 15); +} + +void TMC21X::setEnabled(bool enabled) //TODO Convert +{ + //delete the t_off in the chopper config to get sure + chopper_config_register &= ~(T_OFF_PATTERN); + if (enabled) { + //and set the t_off time + chopper_config_register |= this->constant_off_time; + } + //if not enabled we don't have to do anything since we already delete t_off from the register + if (started) { + send2130_old(chopper_config_register); + } +} + +bool TMC21X::isEnabled() //TODO Convert +{ + if (chopper_config_register & T_OFF_PATTERN) { + return true; + } else { + return false; + } +} + +/* + * reads a value from the TMC21X status register. The value is not obtained directly but can then + * be read by the various status routines. + * + */ +void TMC21X::readStatus(int8_t read_value) //TODO Convert +{ + unsigned long old_driver_configuration_register_value = driver_configuration_register_value; + //reset the readout configuration + driver_configuration_register_value &= ~(READ_SELECTION_PATTERN); + //this now equals TMC21X_READOUT_POSITION - so we just have to check the other two options + if (read_value == TMC21X_READOUT_STALLGUARD) { + driver_configuration_register_value |= READ_STALL_GUARD_READING; + } else if (read_value == TMC21X_READOUT_CURRENT) { + driver_configuration_register_value |= READ_STALL_GUARD_AND_COOL_STEP; + } + //all other cases are ignored to prevent funny values + //check if the readout is configured for the value we are interested in + if (driver_configuration_register_value != old_driver_configuration_register_value) { + //because then we need to write the value twice - one time for configuring, second time to get the value, see below + send2130_old(driver_configuration_register_value); + } + //write the configuration to get the last status + send2130_old(driver_configuration_register_value); +} + +//reads the stall guard setting from last status +//returns -1 if stallguard information is not present +int TMC21X::getCurrentStallGuardReading(void) //TODO Convert +{ + //if we don't yet started there cannot be a stall guard value + if (!started) { + return -1; + } + //not time optimal, but solution optimal: + //first read out the stall guard value + readStatus(TMC21X_READOUT_STALLGUARD); + return getReadoutValue(); +} + +uint8_t TMC21X::getCurrentCSReading(void) //TODO Convert +{ + //if we don't yet started there cannot be a stall guard value + if (!started) { + return 0; + } + + //first read out the stall guard value + readStatus(TMC21X_READOUT_CURRENT); + return (getReadoutValue() & 0x1f); +} + +unsigned int TMC21X::getCoolstepCurrent(void) //TODO Convert +{ + float result = (float)getCurrentCSReading(); + float resistor_value = (float)this->resistor; + float voltage = (driver_configuration_register_value & VSENSE) ? 0.165F : 0.31F; + result = (result + 1.0F) / 32.0F * voltage / resistor_value * 1000.0F * 1000.0F; + return (unsigned int)roundf(result); +} + +/* + return true if the stallguard threshold has been reached +*/ +bool TMC21X::isStallGuardOverThreshold(void) //TODO Convert +{ + if (!this->started) { + return false; + } + return (driver_status_result & STATUS_STALL_GUARD_STATUS); +} + +/* + returns if there is any over temperature condition: + OVER_TEMPERATURE_PREWARING if pre warning level has been reached + OVER_TEMPERATURE_SHUTDOWN if the temperature is so hot that the driver is shut down + Any of those levels are not too good. +*/ +int8_t TMC21X::getOverTemperature(void) //TODO Convert +{ + if (!this->started) { + return 0; + } + if (driver_status_result & STATUS_OVER_TEMPERATURE_SHUTDOWN) { + return TMC21X_OVERTEMPERATURE_SHUTDOWN; + } + if (driver_status_result & STATUS_OVER_TEMPERATURE_WARNING) { + return TMC21X_OVERTEMPERATURE_PREWARING; + } + return 0; +} + +//is motor channel A shorted to ground +bool TMC21X::isShortToGroundA(void) //TODO Convert +{ + if (!this->started) { + return false; + } + return (driver_status_result & STATUS_SHORT_TO_GROUND_A); +} + +//is motor channel B shorted to ground +bool TMC21X::isShortToGroundB(void) //TODO Convert +{ + if (!this->started) { + return false; + } + return (driver_status_result & STATUS_SHORT_TO_GROUND_B); +} + +//is motor channel A connected +bool TMC21X::isOpenLoadA(void) //TODO Convert +{ + if (!this->started) { + return false; + } + return (driver_status_result & STATUS_OPEN_LOAD_A); +} + +//is motor channel B connected +bool TMC21X::isOpenLoadB(void) //TODO Convert +{ + if (!this->started) { + return false; + } + return (driver_status_result & STATUS_OPEN_LOAD_B); +} + +//is chopper inactive since 2^20 clock cycles - defaults to ~0,08s +bool TMC21X::isStandStill(void) //TODO Convert +{ + if (!this->started) { + return false; + } + return (driver_status_result & STATUS_STAND_STILL); +} + +//is chopper inactive since 2^20 clock cycles - defaults to ~0,08s +bool TMC21X::isStallGuardReached(void) //TODO Convert +{ + if (!this->started) { + return false; + } + return (driver_status_result & STATUS_STALL_GUARD_STATUS); +} + +//reads the stall guard setting from last status +//returns -1 if stallguard information is not present +int TMC21X::getReadoutValue(void) //TODO Convert +{ + return (int)(driver_status_result >> 10); +} + +bool TMC21X::isCurrentScalingHalfed() //TODO Convert +{ + if (this->driver_configuration_register_value & VSENSE) { + return true; + } else { + return false; + } +} + +void TMC21X::dumpStatus(StreamOutput *stream, bool readable) //TODO Convert +{ + if (readable) { + stream->printf("designator %c, Chip type TMC21X\n", designator); + + check_error_status_bits(stream); + + if (this->isStallGuardReached()) { + stream->printf("INFO: Stall Guard level reached!\n"); + } + + if (this->isStandStill()) { + stream->printf("INFO: Motor is standing still.\n"); + } + + int value = getReadoutValue(); + stream->printf("Microstep postion phase A: %d\n", value); + + value = getCurrentStallGuardReading(); + stream->printf("Stall Guard value: %d\n", value); + + stream->printf("Current setting: %dmA\n", getCurrent()); + stream->printf("Coolstep current: %dmA\n", getCoolstepCurrent()); + + stream->printf("Microsteps: 1/%d\n", microsteps); + + stream->printf("Register dump:\n"); + stream->printf(" driver control register: %08lX(%ld)\n", driver_control_register_value, driver_control_register_value); + stream->printf(" chopper config register: %08lX(%ld)\n", chopper_config_register, chopper_config_register); + stream->printf(" cool step register: %08lX(%ld)\n", cool_step_register_value, cool_step_register_value); + stream->printf(" stall guard2 current register: %08lX(%ld)\n", stall_guard2_current_register_value, stall_guard2_current_register_value); + stream->printf(" driver configuration register: %08lX(%ld)\n", driver_configuration_register_value, driver_configuration_register_value); + stream->printf(" motor_driver_control.xxx.reg %05lX,%05lX,%05lX,%05lX,%05lX\n", driver_control_register_value, chopper_config_register, cool_step_register_value, stall_guard2_current_register_value, driver_configuration_register_value); + + } else { + // TODO hardcoded for X need to select ABC as needed + bool moving = THEKERNEL->robot->actuators[0]->is_moving(); + // dump out in the format that the processing script needs + if (moving) { + stream->printf("#sg%d,p%lu,k%u,r,", getCurrentStallGuardReading(), THEKERNEL->robot->actuators[0]->get_current_step(), getCoolstepCurrent()); + } else { + readStatus(TMC21X_READOUT_POSITION); // get the status bits + stream->printf("#s,"); + } + stream->printf("d%d,", THEKERNEL->robot->actuators[0]->which_direction() ? 1 : -1); + stream->printf("c%u,m%d,", getCurrent(), getMicrosteps()); + // stream->printf('S'); + // stream->printf(tmc21XStepper.getSpeed(), DEC); + stream->printf("t%d,f%d,", getStallGuardThreshold(), getStallGuardFilter()); + + //print out the general cool step config + if (isCoolStepEnabled()) stream->printf("Ke+,"); + else stream->printf("Ke-,"); + + stream->printf("Kl%u,Ku%u,Kn%u,Ki%u,Km%u,", + getCoolStepLowerSgThreshold(), getCoolStepUpperSgThreshold(), getCoolStepNumberOfSGReadings(), getCoolStepCurrentIncrementSize(), getCoolStepLowerCurrentLimit()); + + //detect the winding status + if (isOpenLoadA()) { + stream->printf("ao,"); + } else if(isShortToGroundA()) { + stream->printf("ag,"); + } else { + stream->printf("a-,"); + } + //detect the winding status + if (isOpenLoadB()) { + stream->printf("bo,"); + } else if(isShortToGroundB()) { + stream->printf("bg,"); + } else { + stream->printf("b-,"); + } + + char temperature = getOverTemperature(); + if (temperature == 0) { + stream->printf("x-,"); + } else if (temperature == TMC21X_OVERTEMPERATURE_PREWARING) { + stream->printf("xw,"); + } else { + stream->printf("xe,"); + } + + if (isEnabled()) { + stream->printf("e1,"); + } else { + stream->printf("e0,"); + } + + //write out the current chopper config + stream->printf("Cm%d,", (chopper_config_register & CHOPPER_MODE_T_OFF_FAST_DECAY) != 0); + stream->printf("Co%d,Cb%d,", constant_off_time, blank_time); + if ((chopper_config_register & CHOPPER_MODE_T_OFF_FAST_DECAY) == 0) { + stream->printf("Cs%d,Ce%d,Cd%d,", h_start, h_end, h_decrement); + } + stream->printf("\n"); + } +} + +// check error bits and report, only report once +bool TMC21X::check_error_status_bits(StreamOutput *stream) //TODO Convert +{ + bool error= false; + readStatus(TMC21X_READOUT_POSITION); // get the status bits + + if (this->getOverTemperature()&TMC21X_OVERTEMPERATURE_PREWARING) { + if(!error_reported.test(0)) stream->printf("%c - WARNING: Overtemperature Prewarning!\n", designator); + error_reported.set(0); + }else{ + error_reported.reset(0); + } + + if (this->getOverTemperature()&TMC21X_OVERTEMPERATURE_SHUTDOWN) { + if(!error_reported.test(1)) stream->printf("%c - ERROR: Overtemperature Shutdown!\n", designator); + error=true; + error_reported.set(1); + }else{ + error_reported.reset(1); + } + + if (this->isShortToGroundA()) { + if(!error_reported.test(2)) stream->printf("%c - ERROR: SHORT to ground on channel A!\n", designator); + error=true; + error_reported.set(2); + }else{ + error_reported.reset(2); + } + + if (this->isShortToGroundB()) { + if(!error_reported.test(3)) stream->printf("%c - ERROR: SHORT to ground on channel B!\n", designator); + error=true; + error_reported.set(3); + }else{ + error_reported.reset(3); + } + + // these seem to be triggered when moving so ignore them for now + if (this->isOpenLoadA()) { + if(!error_reported.test(4)) stream->printf("%c - ERROR: Channel A seems to be unconnected!\n", designator); + error=true; + error_reported.set(4); + }else{ + error_reported.reset(4); + } + + if (this->isOpenLoadB()) { + if(!error_reported.test(5)) stream->printf("%c - ERROR: Channel B seems to be unconnected!\n", designator); + error=true; + error_reported.set(5); + }else{ + error_reported.reset(5); + } + + // if(error) { + // stream->printf("%08X\n", driver_status_result); + // } + return error; +} + +bool TMC21X::checkAlarm() //TODO Convert +{ + return check_error_status_bits(THEKERNEL->streams); +} + +// sets a raw register to the value specified, for advanced settings +// register 255 writes them, 0 displays what registers are mapped to what +// FIXME status registers not reading back correctly, check docs +bool TMC21X::setRawRegister(StreamOutput *stream, uint32_t reg, uint32_t val) //TODO Convert +{ + switch(reg) { + case 255: + send2130_old(driver_control_register_value); + send2130_old(chopper_config_register); + send2130_old(cool_step_register_value); + send2130_old(stall_guard2_current_register_value); + send2130_old(driver_configuration_register_value); + stream->printf("Registers written\n"); + break; + + + case 1: driver_control_register_value = val; stream->printf("driver control register set to %08lX\n", val); break; + case 2: chopper_config_register = val; stream->printf("chopper config register set to %08lX\n", val); break; + case 3: cool_step_register_value = val; stream->printf("cool step register set to %08lX\n", val); break; + case 4: stall_guard2_current_register_value = val; stream->printf("stall guard2 current register set to %08lX\n", val); break; + case 5: driver_configuration_register_value = val; stream->printf("driver configuration register set to %08lX\n", val); break; + + default: + stream->printf("1: driver control register\n"); + stream->printf("2: chopper config register\n"); + stream->printf("3: cool step register\n"); + stream->printf("4: stall guard2 current register\n"); + stream->printf("5: driver configuration register\n"); + stream->printf("255: update all registers\n"); + return false; + } + return true; +} + +/* + * send register settings to the stepper driver via SPI + * returns the current status + * sends 20bits, the last 20 bits of the 24bits is taken as the command + */ +void TMC21X::send2130_old(unsigned long datagram) //TODO Convert +{ + return; //TODO remove this routine + + uint8_t buf[] {(uint8_t)(datagram >> 16), (uint8_t)(datagram >> 8), (uint8_t)(datagram & 0xff)}; + uint8_t rbuf[3]; + + //write/read the values + spi(buf, 3, rbuf); + + // construct reply + unsigned long i_datagram = ((rbuf[0] << 16) | (rbuf[1] << 8) | (rbuf[2])) >> 4; + + //store the datagram as status result + driver_status_result = i_datagram; + + //THEKERNEL->streams->printf("sent: %02X, %02X, %02X received: %02X, %02X, %02X \n", buf[0], buf[1], buf[2], rbuf[0], rbuf[1], rbuf[2]); +} + +/* + * send register settings to the stepper driver via SPI + * returns the current status 40 bit datagram, first 8 bits is the status, the last 32 bits are the register contents + */ +void TMC21X::send2130(uint8_t reg, uint32_t datagram) //TODO Converted, needs testing +{ + uint8_t buf[5]; + + //Note: SPI write frist to last //TODO check that this is actually working as may have just swapped bytes and not the bit order + buf[0] = (uint8_t)(reg); + buf[1] = (uint8_t)(datagram >> 24); + buf[2] = (uint8_t)(datagram >> 16); + buf[3] = (uint8_t)(datagram >> 8); + buf[4] = (uint8_t)(datagram >> 0); + + uint8_t rbuf[5]; + + //write/read the values + spi(buf, 5, rbuf); + + // construct reply + uint32_t i_datagram = ((uint32_t)(rbuf[3] << 24) | (uint32_t)(rbuf[2] <<16) | (uint32_t)(rbuf[1] <<8) | (uint32_t)(rbuf[0] >>24) ); + + //store the datagram as status result + driver_status_result = i_datagram; + driver_status = rbuf[4]; //save the status register + + //THEKERNEL->streams->printf("sent: %02X, %02X, %02X, %02X, %02X received: %02X, %02X, %02X, %02X, %02X \n", buf[4], buf[4], buf[2], buf[1], buf[0], rbuf[4], rbuf[3], rbuf[2], rbuf[1], rbuf[0]); +} + + +#define HAS(X) (options.find(X) != options.end()) +#define GET(X) (options.at(X)) +bool TMC21X::set_options(const options_t& options) //TODO Convert +{ + bool set = false; + if(HAS('O') || HAS('Q')) { + // void TMC21X::setStallGuardThreshold(int8_t stall_guard_threshold, int8_t stall_guard_filter_enabled) + int8_t o = HAS('O') ? GET('O') : getStallGuardThreshold(); + int8_t q = HAS('Q') ? GET('Q') : getStallGuardFilter(); + setStallGuardThreshold(o, q); + set = true; + } + + if(HAS('H') && HAS('I') && HAS('J') && HAS('K') && HAS('L')) { + //void TMC21X::setCoolStepConfiguration(unsigned int lower_SG_threshold, unsigned int SG_hysteresis, uint8_t current_decrement_step_size, uint8_t current_increment_step_size, uint8_t lower_current_limit) + setCoolStepConfiguration(GET('H'), GET('I'), GET('J'), GET('K'), GET('L')); + set = true; + } + + if(HAS('S')) { + uint32_t s = GET('S'); + if(s == 0 && HAS('U') && HAS('V') && HAS('W') && HAS('X') && HAS('Y')) { + //void TMC21X::setConstantOffTimeChopper(int8_t constant_off_time, int8_t blank_time, int8_t fast_decay_time_setting, int8_t sine_wave_offset, uint8_t use_current_comparator) + setConstantOffTimeChopper(GET('U'), GET('V'), GET('W'), GET('X'), GET('Y')); + set = true; + + } else if(s == 1 && HAS('U') && HAS('V') && HAS('W') && HAS('X') && HAS('Y')) { + //void TMC21X::setSpreadCycleChopper(int8_t constant_off_time, int8_t blank_time, int8_t hysteresis_start, int8_t hysteresis_end, int8_t hysteresis_decrement); + setSpreadCycleChopper(GET('U'), GET('V'), GET('W'), GET('X'), GET('Y')); + set = true; + + } else if(s == 2 && HAS('Z')) { + setRandomOffTime(GET('Z')); + set = true; + + } else if(s == 3 && HAS('Z')) { + setDoubleEdge(GET('Z')); + set = true; + + } else if(s == 4 && HAS('Z')) { + setStepInterpolation(GET('Z')); + set = true; + + } else if(s == 5 && HAS('Z')) { + setCoolStepEnabled(GET('Z') == 1); + set = true; + } + } + + return set; +} diff --git a/src/modules/utils/motordrivercontrol/drivers/TMC21X/TMC21X.h b/src/modules/utils/motordrivercontrol/drivers/TMC21X/TMC21X.h new file mode 100644 index 00000000..e7ceb6e1 --- /dev/null +++ b/src/modules/utils/motordrivercontrol/drivers/TMC21X/TMC21X.h @@ -0,0 +1,455 @@ +/* + modified from... + TMC21X.cpp - - TMC21X Stepper library for Wiring/Arduino + + based on the stepper library by Tom Igoe, et. al. + + Copyright (c) 2011, Interactive Matter, Marcus Nowotny + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + 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. + + */ + + +// ensure this library description is only included once +#pragma once + +#include +#include +#include + +class StreamOutput; + +/*! + * \class TMC21X + * \brief Class representing a TMC21X stepper driver + */ +#define TMC2130_max_current 31 //TMC2130 only allows a ratio of 0..31 which represents 1/32 .. 32/32 + +class TMC21X +{ +public: + /*! + * \brief creates a new representation of a stepper motor connected to a TMC21X stepper driver + * + * This is the main constructor. If in doubt use this. You must provide all parameters as described below. + * + * \param spi send function + * + * By default the Constant Off Time chopper is used, see TCM21X.setConstantOffTimeChopper() for details. + * This should work on most motors (YMMV). You may want to configure and use the Spread Cycle Chopper, see setSpreadCycleChopper(). + * + * By default a microstepping of 1/32th is used to provide a smooth motor run, while still giving a good progression per step. + * You can select a different stepping with setMicrosteps() to aa different value. + * \sa start(), setMicrosteps() + */ + TMC21X(std::function spi, char designator); +// + /*! + * \brief configures the TMC21X stepper driver. Before you called this function the stepper driver is in nonfunctional mode. + * + * \param rms_current the maximum current to provide to the motor in mA (!). A value of 200 would send up to 200mA to the motor + * \param resistor the current sense resistor in milli Ohm, defaults to ,15 Ohm ( or 150 milli Ohm) as in the TMC260 Arduino Shield + + * This routine configures the TMC21X stepper driver for the given values via SPI. + * Most member functions are non functional if the driver has not been started. + * Therefore it is best to call this in your Arduino setup() function. + */ + void init(uint16_t cs); + + /*! + * \brief Set the number of microsteps in 2^i values (rounded) up to 256 + * + * This method set's the number of microsteps per step in 2^i interval. + * This means you can select 1, 2, 4, 16, 32, 64, 128 or 256 as valid microsteps. + * If you give any other value it will be rounded to the next smaller number (3 would give a microstepping of 2). + * You can always check the current microstepping with getMicrosteps(). + */ + void setMicrosteps(int number_of_steps); + + /*! + * \brief returns the effective current number of microsteps selected. + * + * This function always returns the effective number of microsteps. + * This can be a bit different than the micro steps set in setMicrosteps() since it is rounded to 2^i. + * + * \sa setMicrosteps() + */ + int getMicrosteps(void); + + void setStepInterpolation(int8_t value); + void setDoubleEdge(int8_t value); + + /*! + * \brief Sets and configure the classical Constant Off Timer Chopper + * \param constant_off_time The off time setting controls the minimum chopper frequency. For most applications an off time within the range of 5μs to 20μs will fit. Setting this parameter to zero completely disables all driver transistors and the motor can free-wheel. 0: chopper off, 1:15: off time setting (1 will work with minimum blank time of 24 clocks) + * \param blank_time Selects the comparator blank time. This time needs to safely cover the switching event and the duration of the ringing on the sense resistor. For most low current drivers, a setting of 1 or 2 is good. For high current applications with large MOSFETs, a setting of 2 or 3 will be required. 0 (min setting) … (3) amx setting + * \param fast_decay_time_setting Fast decay time setting. Controls the portion of fast decay for each chopper cycle. 0: slow decay only, 1…15: duration of fast decay phase + * \param sine_wave_offset Sine wave offset. Controls the sine wave offset. A positive offset corrects for zero crossing error. -3…-1: negative offset, 0: no offset,1…12: positive offset + * \param use_curreent_comparator Selects usage of the current comparator for termination of the fast decay cycle. If current comparator is enabled, it terminates the fast decay cycle in case the current reaches a higher negative value than the actual positive value. (0 disable, -1 enable). + * + * The classic constant off time chopper uses a fixed portion of fast decay following each on phase. + * While the duration of the on time is determined by the chopper comparator, the fast decay time needs + * to be set by the user in a way, that the current decay is enough for the driver to be able to follow + * the falling slope of the sine wave, and on the other hand it should not be too long, in order to minimize + * motor current ripple and power dissipation. This best can be tuned using an oscilloscope or + * trying out motor smoothness at different velocities. A good starting value is a fast decay time setting + * similar to the slow decay time setting. + * After tuning of the fast decay time, the offset should be determined, in order to have a smooth zero transition. + * This is necessary, because the fast decay phase leads to the absolute value of the motor current being lower + * than the target current (see figure 17). If the zero offset is too low, the motor stands still for a short + * moment during current zero crossing, if it is set too high, it makes a larger microstep. + * Typically, a positive offset setting is required for optimum operation. + * + * \sa setSpreadCycleChoper() for other alternatives. + * \sa setRandomOffTime() for spreading the noise over a wider spectrum + */ + void setConstantOffTimeChopper(int8_t constant_off_time, int8_t blank_time, int8_t fast_decay_time_setting, int8_t sine_wave_offset, uint8_t use_current_comparator); + + /*! + * \brief Sets and configures with spread cycle chopper. + * \param constant_off_time The off time setting controls the minimum chopper frequency. For most applications an off time within the range of 5μs to 20μs will fit. Setting this parameter to zero completely disables all driver transistors and the motor can free-wheel. 0: chopper off, 1:15: off time setting (1 will work with minimum blank time of 24 clocks) + * \param blank_time Selects the comparator blank time. This time needs to safely cover the switching event and the duration of the ringing on the sense resistor. For most low current drivers, a setting of 1 or 2 is good. For high current applications with large MOSFETs, a setting of 2 or 3 will be required. 0 (min setting) … (3) amx setting + * \param hysteresis_start Hysteresis start setting. Please remark, that this value is an offset to the hysteresis end value. 1 … 8 + * \param hysteresis_end Hysteresis end setting. Sets the hysteresis end value after a number of decrements. Decrement interval time is controlled by hysteresis_decrement. The sum hysteresis_start + hysteresis_end must be <16. At a current setting CS of max. 30 (amplitude reduced to 240), the sum is not limited. + * \param hysteresis_decrement Hysteresis decrement setting. This setting determines the slope of the hysteresis during on time and during fast decay time. 0 (fast decrement) … 3 (slow decrement). + * + * The spreadCycle chopper scheme (pat.fil.) is a precise and simple to use chopper principle, which automatically determines + * the optimum fast decay portion for the motor. Anyhow, a number of settings can be made in order to optimally fit the driver + * to the motor. + * Each chopper cycle is comprised of an on-phase, a slow decay phase, a fast decay phase and a second slow decay phase. + * The slow decay phases limit the maximum chopper frequency and are important for low motor and driver power dissipation. + * The hysteresis start setting limits the chopper frequency by forcing the driver to introduce a minimum amount of + * current ripple into the motor coils. The motor inductivity determines the ability to follow a changing motor current. + * The duration of the on- and fast decay phase needs to cover at least the blank time, because the current comparator is + * disabled during this time. + * + * \sa setRandomOffTime() for spreading the noise over a wider spectrum + */ + void setSpreadCycleChopper(int8_t constant_off_time, int8_t blank_time, int8_t hysteresis_start, int8_t hysteresis_end, int8_t hysteresis_decrement); + + /*! + * \brief Use random off time for noise reduction (0 for off, -1 for on). + * \param value 0 for off, -1 for on + * + * In a constant off time chopper scheme both coil choppers run freely, i.e. are not synchronized. + * The frequency of each chopper mainly depends on the coil current and the position dependant motor coil inductivity, + * thus it depends on the microstep position. With some motors a slightly audible beat can occur between the chopper + * frequencies, especially when they are near to each other. This typically occurs at a few microstep positions within + * each quarter wave. + * This effect normally is not audible when compared to mechanical noise generated by ball bearings, + * etc. Further factors which can cause a similar effect are a poor layout of sense resistor GND connection. + * In order to minimize the effect of a beat between both chopper frequencies, an internal random generator is provided. + * It modulates the slow decay time setting when switched on. The random off time feature further spreads the chopper spectrum, + * reducing electromagnetic emission on single frequencies. + */ + void setRandomOffTime(int8_t value); + + /*! + * \brief set the maximum motor current in mA (1000 is 1 Amp) + * Keep in mind this is the maximum peak Current. The RMS current will be 1/sqrt(2) smaller. The actual current can also be smaller + * by employing CoolStep. + * \param current the maximum motor current in mA + * \sa getCurrent(), getCurrentCurrent() + */ + void setCurrent(unsigned int current); + + /*! + * \brief readout the motor maximum current in mA (1000 is an Amp) + * This is the maximum current. to get the current current - which may be affected by CoolStep us getCurrentCurrent() + *\return the maximum motor current in milli amps + * \sa getCurrentCurrent() + */ + unsigned int getCurrent(void); + + /*! + * \brief set the StallGuard threshold in order to get sensible StallGuard readings. + * \param stall_guard_threshold -64 … 63 the StallGuard threshold + * \param stall_guard_filter_enabled 0 if the filter is disabled, -1 if it is enabled + * + * The StallGuard threshold is used to optimize the StallGuard reading to sensible values. It should be at 0 at + * the maximum allowable load on the motor (but not before). = is a good starting point (and the default) + * If you get Stall Gaurd readings of 0 without any load or with too little load increase the value. + * If you get readings of 1023 even with load decrease the setting. + * + * If you switch on the filter the StallGuard reading is only updated each 4th full step to reduce the noise in the + * reading. + * + * \sa getCurrentStallGuardReading() to read out the current value. + */ + void setStallGuardThreshold(int8_t stall_guard_threshold, int8_t stall_guard_filter_enabled); + + /*! + * \brief reads out the StallGuard threshold + * \return a number between -64 and 63. + */ + int8_t getStallGuardThreshold(void); + + /*! + * \brief returns the current setting of the StallGuard filter + * \return 0 if not set, -1 if set + */ + int8_t getStallGuardFilter(void); + + /*! + * \brief This method configures the CoolStep smart energy operation. You must have a proper StallGuard configuration for the motor situation (current, voltage, speed) in rder to use this feature. + * \param lower_SG_threshold Sets the lower threshold for stallGuard2TM reading. Below this value, the motor current becomes increased. Allowed values are 0...480 + * \param SG_hysteresis Sets the distance between the lower and the upper threshold for stallGuard2TM reading. Above the upper threshold (which is lower_SG_threshold+SG_hysteresis+1) the motor current becomes decreased. Allowed values are 0...480 + * \param current_decrement_step_size Sets the current decrement steps. If the StallGuard value is above the threshold the current gets decremented by this step size. 0...32 + * \param current_increment_step_size Sets the current increment step. The current becomes incremented for each measured stallGuard2TM value below the lower threshold. 0...8 + * \param lower_current_limit Sets the lower motor current limit for coolStepTM operation by scaling the CS value. Values can be COOL_STEP_HALF_CS_LIMIT, COOL_STEP_QUARTER_CS_LIMIT + * The CoolStep smart energy operation automatically adjust the current sent into the motor according to the current load, + * read out by the StallGuard in order to provide the optimum torque with the minimal current consumption. + * You configure the CoolStep current regulator by defining upper and lower bounds of StallGuard readouts. If the readout is above the + * limit the current gets increased, below the limit the current gets decreased. + * You can specify the upper an lower threshold of the StallGuard readout in order to adjust the current. You can also set the number of + * StallGuard readings necessary above or below the limit to get a more stable current adjustment. + * The current adjustment itself is configured by the number of steps the current gets in- or decreased and the absolute minimum current + * (1/2 or 1/4th of the configured current). + * \sa COOL_STEP_HALF_CS_LIMIT, COOL_STEP_QUARTER_CS_LIMIT + */ + void setCoolStepConfiguration(unsigned int lower_SG_threshold, unsigned int SG_hysteresis, uint8_t current_decrement_step_size, + uint8_t current_increment_step_size, uint8_t lower_current_limit); + + /*! + * \brief enables or disables the CoolStep smart energy operation feature. It must be configured before enabling it. + * \param enabled true if CoolStep should be enabled, false if not. + * \sa setCoolStepConfiguration() + */ + void setCoolStepEnabled(bool enabled); + + + /*! + * \brief check if the CoolStep feature is enabled + * \sa setCoolStepEnabled() + */ + bool isCoolStepEnabled(); + + /*! + * \brief returns the lower StallGuard threshold for the CoolStep operation + * \sa setCoolStepConfiguration() + */ + unsigned int getCoolStepLowerSgThreshold(); + + /*! + * \brief returns the upper StallGuard threshold for the CoolStep operation + * \sa setCoolStepConfiguration() + */ + unsigned int getCoolStepUpperSgThreshold(); + + /*! + * \brief returns the number of StallGuard readings befor CoolStep adjusts the motor current. + * \sa setCoolStepConfiguration() + */ + uint8_t getCoolStepNumberOfSGReadings(); + + /*! + * \brief returns the increment steps for the current for the CoolStep operation + * \sa setCoolStepConfiguration() + */ + uint8_t getCoolStepCurrentIncrementSize(); + + /*! + * \brief returns the absolute minimum current for the CoolStep operation + * \sa setCoolStepConfiguration() + * \sa COOL_STEP_HALF_CS_LIMIT, COOL_STEP_QUARTER_CS_LIMIT + */ + uint8_t getCoolStepLowerCurrentLimit(); + + /*! + * \brief Reads the current StallGuard value. + * \return The current StallGuard value, lesser values indicate higher load, 0 means stall detected. + * Keep in mind that this routine reads and writes a value via SPI - so this may take a bit time. + * \sa setStallGuardThreshold() for tuning the readout to sensible ranges. + */ + int getCurrentStallGuardReading(void); + + /*! + * \brief Reads the current current setting value as fraction of the maximum current + * Returns values between 0 and 31, representing 1/32 to 32/32 (=1) + * \sa setCoolStepConfiguration() + */ + uint8_t getCurrentCSReading(void); + + + /*! + *\brief a convenience method to determine if the current scaling uses 0.31V or 0.165V as reference. + *\return false if 0.13V is the reference voltage, true if 0.165V is used. + */ + bool isCurrentScalingHalfed(); + + /*! + * \brief Reads the current current setting value and recalculates the absolute current in mA (1A would be 1000). + * This method calculates the currently used current setting (either by setting or by CoolStep) and reconstructs + * the current in mA by using the VSENSE and resistor value. This method uses floating point math - so it + * may not be the fastest. + * \sa getCurrentCSReading(), getResistor(), isCurrentScalingHalfed(), getCurrent() + */ + unsigned int getCoolstepCurrent(void); + + /*! + * \brief checks if there is a StallGuard warning in the last status + * \return 0 if there was no warning, -1 if there was some warning. + * Keep in mind that this method does not enforce a readout but uses the value of the last status readout. + * You may want to use getMotorPosition() or getCurrentStallGuardReading() to enforce an updated status readout. + * + * \sa setStallGuardThreshold() for tuning the readout to sensible ranges. + */ + bool isStallGuardOverThreshold(void); + + /*! + * \brief Return over temperature status of the last status readout + * return 0 is everything is OK, TMC21X_OVERTEMPERATURE_PREWARING if status is reached, TMC21X_OVERTEMPERATURE_SHUTDOWN is the chip is shutdown, -1 if the status is unknown. + * Keep in mind that this method does not enforce a readout but uses the value of the last status readout. + * You may want to use getMotorPosition() or getCurrentStallGuardReading() to enforce an updated status readout. + */ + int8_t getOverTemperature(void); + + /*! + * \brief Is motor channel A shorted to ground detected in the last status readout. + * \return true is yes, false if not. + * Keep in mind that this method does not enforce a readout but uses the value of the last status readout. + * You may want to use getMotorPosition() or getCurrentStallGuardReading() to enforce an updated status readout. + */ + + bool isShortToGroundA(void); + + /*! + * \brief Is motor channel B shorted to ground detected in the last status readout. + * \return true is yes, false if not. + * Keep in mind that this method does not enforce a readout but uses the value of the last status readout. + * You may want to use getMotorPosition() or getCurrentStallGuardReading() to enforce an updated status readout. + */ + bool isShortToGroundB(void); + /*! + * \brief iIs motor channel A connected according to the last status readout. + * \return true is yes, false if not. + * Keep in mind that this method does not enforce a readout but uses the value of the last status readout. + * You may want to use getMotorPosition() or getCurrentStallGuardReading() to enforce an updated status readout. + */ + bool isOpenLoadA(void); + + /*! + * \brief iIs motor channel A connected according to the last status readout. + * \return true is yes, false if not. + * Keep in mind that this method does not enforce a readout but uses the value of the last status readout. + * You may want to use getMotorPosition() or getCurrentStallGuardReading() to enforce an updated status readout. + */ + bool isOpenLoadB(void); + + /*! + * \brief Is chopper inactive since 2^20 clock cycles - defaults to ~0,08s + * \return true is yes, false if not. + * Keep in mind that this method does not enforce a readout but uses the value of the last status readout. + * You may want to use getMotorPosition() or getCurrentStallGuardReading() to enforce an updated status readout. + */ + bool isStandStill(void); + + /*! + * \brief checks if there is a StallGuard warning in the last status + * \return 0 if there was no warning, -1 if there was some warning. + * Keep in mind that this method does not enforce a readout but uses the value of the last status readout. + * You may want to use getMotorPosition() or getCurrentStallGuardReading() to enforce an updated status readout. + * + * \sa isStallGuardOverThreshold() + * TODO why? + * + * \sa setStallGuardThreshold() for tuning the readout to sensible ranges. + */ + bool isStallGuardReached(void); + + /*! + *\brief enables or disables the motor driver bridges. If disabled the motor can run freely. If enabled not. + *\param enabled a bool value true if the motor should be enabled, false otherwise. + */ + void setEnabled(bool enabled); + + /*! + *\brief checks if the output bridges are enabled. If the bridges are not enabled the motor can run freely + *\return true if the bridges and by that the motor driver are enabled, false if not. + *\sa setEnabled() + */ + bool isEnabled(); + + /*! + * \brief Manually read out the status register + * This function sends a byte to the motor driver in order to get the current readout. The parameter read_value + * selects which value will get returned. If the read_vlaue changes in respect to the previous readout this method + * automatically send two bytes to the motor: one to set the readout and one to get the actual readout. So this method + * may take time to send and read one or two bits - depending on the previous readout. + * \param read_value selects which value to read out (0..3). You can use the defines TMC21X_READOUT_POSITION, TMC_2130_READOUT_STALLGUARD, or TMC_2130_READOUT_CURRENT + * \sa TMC21X_READOUT_POSITION, TMC_2130_READOUT_STALLGUARD, TMC_2130_READOUT_CURRENT + */ + void readStatus(int8_t read_value); + + /*! + * \brief Prints out all the information that can be found in the last status read out - it does not force a status readout. + * The result is printed via Serial + */ + void dumpStatus(StreamOutput *stream, bool readable= true); + bool setRawRegister(StreamOutput *stream, uint32_t reg, uint32_t val); + bool checkAlarm(); + + using options_t= std::map; + + bool set_options(const options_t& options); + +private: + //helper routine to get the top 10 bit of the readout + inline int getReadoutValue(); + bool check_error_status_bits(StreamOutput *stream); + + // SPI sender + inline void send2130_old(unsigned long datagram); + inline void send2130(uint8_t reg, uint32_t datagram); + std::function spi; + + unsigned int resistor{50}; // current sense resistor value in milliohm + + //driver control register copies to easily set & modify the registers + unsigned long driver_control_register_value; + unsigned long chopper_config_register; + unsigned long cool_step_register_value; + unsigned long stall_guard2_current_register_value; + unsigned long driver_configuration_register_value; + //the driver status result + uint32_t driver_status_result; + uint8_t driver_status; + + //status values + int microsteps; //the current number of micro steps + + std::bitset<8> error_reported; + + // only needed for the tuning app report + struct { + int8_t blank_time:8; + int8_t constant_off_time:5; //we need to remember this value in order to enable and disable the motor + int8_t h_start:4; + int8_t h_end:4; + int8_t h_decrement:3; + bool cool_step_enabled:1; //we need to remember this to configure the coolstep if it is enabled + bool started:1; //if the stepper has been started yet + }; + + uint8_t cool_step_lower_threshold; // we need to remember the threshold to enable and disable the CoolStep feature + char designator; + +}; + diff --git a/src/modules/utils/motordrivercontrol/drivers/TMC26X/TMC26X.cpp b/src/modules/utils/motordrivercontrol/drivers/TMC26X/TMC26X.cpp new file mode 100644 index 00000000..db26ef76 --- /dev/null +++ b/src/modules/utils/motordrivercontrol/drivers/TMC26X/TMC26X.cpp @@ -0,0 +1,1132 @@ +/* + Highly modifed from.... + + TMC26X.cpp - - TMC26X Stepper library for Wiring/Arduino + + based on the stepper library by Tom Igoe, et. al. + + Copyright (c) 2011, Interactive Matter, Marcus Nowotny + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + 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. + + */ + +#include "TMC26X.h" +#include "mbed.h" +#include "StreamOutput.h" +#include "Kernel.h" +#include "libs/StreamOutputPool.h" +#include "Robot.h" +#include "StepperMotor.h" +#include "ConfigValue.h" +#include "Config.h" +#include "checksumm.h" + +#define motor_driver_control_checksum CHECKSUM("motor_driver_control") +#define sense_resistor_checksum CHECKSUM("sense_resistor") + +//! return value for TMC26X.getOverTemperature() if there is a overtemperature situation in the TMC chip +/*! + * This warning indicates that the TCM chip is too warm. + * It is still working but some parameters may be inferior. + * You should do something against it. + */ +#define TMC26X_OVERTEMPERATURE_PREWARING 1 +//! return value for TMC26X.getOverTemperature() if there is a overtemperature shutdown in the TMC chip +/*! + * This warning indicates that the TCM chip is too warm to operate and has shut down to prevent damage. + * It will stop working until it cools down again. + * If you encouter this situation you must do something against it. Like reducing the current or improving the PCB layout + * and/or heat management. + */ +#define TMC26X_OVERTEMPERATURE_SHUTDOWN 2 + +//which values can be read out +/*! + * Selects to readout the microstep position from the motor. + *\sa readStatus() + */ +#define TMC26X_READOUT_POSITION 0 +/*! + * Selects to read out the StallGuard value of the motor. + *\sa readStatus() + */ +#define TMC26X_READOUT_STALLGUARD 1 +/*! + * Selects to read out the current current setting (acc. to CoolStep) and the upper bits of the StallGuard value from the motor. + *\sa readStatus(), setCurrent() + */ +#define TMC26X_READOUT_CURRENT 3 + +/*! + * Define to set the minimum current for CoolStep operation to 1/2 of the selected CS minium. + *\sa setCoolStepConfiguration() + */ +#define COOL_STEP_HALF_CS_LIMIT 0 +/*! + * Define to set the minimum current for CoolStep operation to 1/4 of the selected CS minium. + *\sa setCoolStepConfiguration() + */ +#define COOL_STEP_QUARTDER_CS_LIMIT 1 + + +//some default values used in initialization +#define DEFAULT_MICROSTEPPING_VALUE 32 + +//TMC26X register definitions +#define DRIVER_CONTROL_REGISTER 0x00000ul +#define CHOPPER_CONFIG_REGISTER 0x80000ul +#define COOL_STEP_REGISTER 0xA0000ul +#define STALL_GUARD2_LOAD_MEASURE_REGISTER 0xC0000ul +#define DRIVER_CONFIG_REGISTER 0xE0000ul + +#define REGISTER_BIT_PATTERN 0xFFFFFul + +//definitions for the driver control register DRVCTL +#define MICROSTEPPING_PATTERN 0x000Ful +#define STEP_INTERPOLATION 0x0200ul +#define DOUBLE_EDGE_STEP 0x0100ul + +//definitions for the driver config register DRVCONF +#define READ_MICROSTEP_POSITION 0x0000ul +#define READ_STALL_GUARD_READING 0x0010ul +#define READ_STALL_GUARD_AND_COOL_STEP 0x0020ul +#define READ_SELECTION_PATTERN 0x0030ul +#define VSENSE 0x0040ul + +//definitions for the chopper config register +#define CHOPPER_MODE_STANDARD 0x00000ul +#define CHOPPER_MODE_T_OFF_FAST_DECAY 0x04000ul +#define T_OFF_PATTERN 0x0000ful +#define RANDOM_TOFF_TIME 0x02000ul +#define BLANK_TIMING_PATTERN 0x18000ul +#define BLANK_TIMING_SHIFT 15 +#define HYSTERESIS_DECREMENT_PATTERN 0x01800ul +#define HYSTERESIS_DECREMENT_SHIFT 11 +#define HYSTERESIS_LOW_VALUE_PATTERN 0x00780ul +#define HYSTERESIS_LOW_SHIFT 7 +#define HYSTERESIS_START_VALUE_PATTERN 0x00078ul +#define HYSTERESIS_START_VALUE_SHIFT 4 +#define T_OFF_TIMING_PATERN 0x0000Ful + +//definitions for cool step register +#define MINIMUM_CURRENT_FOURTH 0x8000ul +#define CURRENT_DOWN_STEP_SPEED_PATTERN 0x6000ul +#define SE_MAX_PATTERN 0x0F00ul +#define SE_CURRENT_STEP_WIDTH_PATTERN 0x0060ul +#define SE_MIN_PATTERN 0x000Ful + +//definitions for stall guard2 current register +#define STALL_GUARD_FILTER_ENABLED 0x10000ul +#define STALL_GUARD_TRESHHOLD_VALUE_PATTERN 0x17F00ul +#define CURRENT_SCALING_PATTERN 0x0001Ful +#define STALL_GUARD_CONFIG_PATTERN 0x17F00ul +#define STALL_GUARD_VALUE_PATTERN 0x07F00ul + +//definitions for the input from the TCM2600 +#define STATUS_STALL_GUARD_STATUS 0x00001ul +#define STATUS_OVER_TEMPERATURE_SHUTDOWN 0x00002ul +#define STATUS_OVER_TEMPERATURE_WARNING 0x00004ul +#define STATUS_SHORT_TO_GROUND_A 0x00008ul +#define STATUS_SHORT_TO_GROUND_B 0x00010ul +#define STATUS_OPEN_LOAD_A 0x00020ul +#define STATUS_OPEN_LOAD_B 0x00040ul +#define STATUS_STAND_STILL 0x00080ul +#define READOUT_VALUE_PATTERN 0xFFC00ul + +//debuging output +//#define DEBUG + +/* + * Constructor + */ +TMC26X::TMC26X(std::function spi, char d) : spi(spi), designator(d) +{ + //we are not started yet + started = false; + //by default cool step is not enabled + cool_step_enabled = false; + error_reported.reset(); +} + +/* + * configure the stepper driver + * just must be called. + */ +void TMC26X::init(uint16_t cs) +{ + // read chip specific config entries + this->resistor= THEKERNEL->config->value(motor_driver_control_checksum, cs, sense_resistor_checksum)->by_default(50)->as_number(); // in milliohms + + //setting the default register values + driver_control_register_value = DRIVER_CONTROL_REGISTER; + chopper_config_register = CHOPPER_CONFIG_REGISTER; + cool_step_register_value = COOL_STEP_REGISTER; + stall_guard2_current_register_value = STALL_GUARD2_LOAD_MEASURE_REGISTER; + driver_configuration_register_value = DRIVER_CONFIG_REGISTER | READ_STALL_GUARD_READING; + + //set the initial values + send262(driver_control_register_value); + send262(chopper_config_register); + send262(cool_step_register_value); + send262(stall_guard2_current_register_value); + send262(driver_configuration_register_value); + + started = true; + +#if 1 + //set to a conservative start value + setConstantOffTimeChopper(7, 54, 13, 12, 1); +#else + // for 1.5amp kysan @ 12v + setSpreadCycleChopper(5, 54, 5, 0, 0); + // for 4amp Nema24 @ 12v + //setSpreadCycleChopper(5, 54, 4, 0, 0); +#endif + + setEnabled(false); + + //set a nice microstepping value + setMicrosteps(DEFAULT_MICROSTEPPING_VALUE); + + // set stallguard to a conservative value so it doesn't trigger immediately + setStallGuardThreshold(10, 1); +} + +void TMC26X::setCurrent(unsigned int current) +{ + uint8_t current_scaling = 0; + //calculate the current scaling from the max current setting (in mA) + double mASetting = (double)current; + double resistor_value = (double) this->resistor; + // remove vesense flag + this->driver_configuration_register_value &= ~(VSENSE); + //this is derrived from I=(cs+1)/32*(Vsense/Rsense) + //leading to cs = CS = 32*R*I/V (with V = 0,31V oder 0,165V and I = 1000*current) + //with Rsense=0,15 + //for vsense = 0,310V (VSENSE not set) + //or vsense = 0,165V (VSENSE set) + current_scaling = (uint8_t)((resistor_value * mASetting * 32.0F / (0.31F * 1000.0F * 1000.0F)) - 0.5F); //theoretically - 1.0 for better rounding it is 0.5 + + //check if the current scaling is too low + if (current_scaling < 16) { + //set the csense bit to get a use half the sense voltage (to support lower motor currents) + this->driver_configuration_register_value |= VSENSE; + //and recalculate the current setting + current_scaling = (uint8_t)((resistor_value * mASetting * 32.0F / (0.165F * 1000.0F * 1000.0F)) - 0.5F); //theoretically - 1.0 for better rounding it is 0.5 + } + + //do some sanity checks + if (current_scaling > 31) { + current_scaling = 31; + } + //delete the old value + stall_guard2_current_register_value &= ~(CURRENT_SCALING_PATTERN); + //set the new current scaling + stall_guard2_current_register_value |= current_scaling; + //if started we directly send it to the motor + if (started) { + send262(driver_configuration_register_value); + send262(stall_guard2_current_register_value); + } +} + +unsigned int TMC26X::getCurrent(void) +{ + //we calculate the current according to the datasheet to be on the safe side + //this is not the fastest but the most accurate and illustrative way + double result = (double)(stall_guard2_current_register_value & CURRENT_SCALING_PATTERN); + double resistor_value = (double)this->resistor; + double voltage = (driver_configuration_register_value & VSENSE) ? 0.165F : 0.31F; + result = (result + 1.0F) / 32.0F * voltage / resistor_value * 1000.0F * 1000.0F; + return (unsigned int)result; +} + +void TMC26X::setStallGuardThreshold(int8_t stall_guard_threshold, int8_t stall_guard_filter_enabled) +{ + if (stall_guard_threshold < -64) { + stall_guard_threshold = -64; + //We just have 5 bits + } else if (stall_guard_threshold > 63) { + stall_guard_threshold = 63; + } + //add trim down to 7 bits + stall_guard_threshold &= 0x7f; + //delete old stall guard settings + stall_guard2_current_register_value &= ~(STALL_GUARD_CONFIG_PATTERN); + if (stall_guard_filter_enabled) { + stall_guard2_current_register_value |= STALL_GUARD_FILTER_ENABLED; + } + //Set the new stall guard threshold + stall_guard2_current_register_value |= (((unsigned long)stall_guard_threshold << 8) & STALL_GUARD_CONFIG_PATTERN); + //if started we directly send it to the motor + if (started) { + send262(stall_guard2_current_register_value); + } +} + +int8_t TMC26X::getStallGuardThreshold(void) +{ + unsigned long stall_guard_threshold = stall_guard2_current_register_value & STALL_GUARD_VALUE_PATTERN; + //shift it down to bit 0 + stall_guard_threshold >>= 8; + //convert the value to an int to correctly handle the negative numbers + int8_t result = stall_guard_threshold; + //check if it is negative and fill it up with leading 1 for proper negative number representation + if (result & (1 << 6)) { + result |= 0xC0; + } + return result; +} + +int8_t TMC26X::getStallGuardFilter(void) +{ + if (stall_guard2_current_register_value & STALL_GUARD_FILTER_ENABLED) { + return -1; + } else { + return 0; + } +} +/* + * Set the number of microsteps per step. + * 0,2,4,8,16,32,64,128,256 is supported + * any value in between will be mapped to the next smaller value + * 0 and 1 set the motor in full step mode + */ +void TMC26X::setMicrosteps(int number_of_steps) +{ + long setting_pattern; + //poor mans log + if (number_of_steps >= 256) { + setting_pattern = 0; + microsteps = 256; + } else if (number_of_steps >= 128) { + setting_pattern = 1; + microsteps = 128; + } else if (number_of_steps >= 64) { + setting_pattern = 2; + microsteps = 64; + } else if (number_of_steps >= 32) { + setting_pattern = 3; + microsteps = 32; + } else if (number_of_steps >= 16) { + setting_pattern = 4; + microsteps = 16; + } else if (number_of_steps >= 8) { + setting_pattern = 5; + microsteps = 8; + } else if (number_of_steps >= 4) { + setting_pattern = 6; + microsteps = 4; + } else if (number_of_steps >= 2) { + setting_pattern = 7; + microsteps = 2; + //1 and 0 lead to full step + } else if (number_of_steps <= 1) { + setting_pattern = 8; + microsteps = 1; + } + + //delete the old value + this->driver_control_register_value &= 0xFFFF0ul; + //set the new value + this->driver_control_register_value |= setting_pattern; + + //if started we directly send it to the motor + if (started) { + send262(driver_control_register_value); + } +} + +/* + * returns the effective number of microsteps at the moment + */ +int TMC26X::getMicrosteps(void) +{ + return microsteps; +} + +void TMC26X::setStepInterpolation(int8_t value) +{ + if (value) { + driver_control_register_value |= STEP_INTERPOLATION; + } else { + driver_control_register_value &= ~(STEP_INTERPOLATION); + } + //if started we directly send it to the motor + if (started) { + send262(driver_control_register_value); + } +} + +void TMC26X::setDoubleEdge(int8_t value) +{ + if (value) { + driver_control_register_value |= DOUBLE_EDGE_STEP; + } else { + driver_control_register_value &= ~(DOUBLE_EDGE_STEP); + } + //if started we directly send it to the motor + if (started) { + send262(driver_control_register_value); + } +} + +/* + * constant_off_time: The off time setting controls the minimum chopper frequency. + * For most applications an off time within the range of 5μs to 20μs will fit. + * 2...15: off time setting + * + * blank_time: Selects the comparator blank time. This time needs to safely cover the switching event and the + * duration of the ringing on the sense resistor. For + * 0: min. setting 3: max. setting + * + * fast_decay_time_setting: Fast decay time setting. With CHM=1, these bits control the portion of fast decay for each chopper cycle. + * 0: slow decay only + * 1...15: duration of fast decay phase + * + * sine_wave_offset: Sine wave offset. With CHM=1, these bits control the sine wave offset. + * A positive offset corrects for zero crossing error. + * -3..-1: negative offset 0: no offset 1...12: positive offset + * + * use_current_comparator: Selects usage of the current comparator for termination of the fast decay cycle. + * If current comparator is enabled, it terminates the fast decay cycle in case the current + * reaches a higher negative value than the actual positive value. + * 1: enable comparator termination of fast decay cycle + * 0: end by time only + */ +void TMC26X::setConstantOffTimeChopper(int8_t constant_off_time, int8_t blank_time, int8_t fast_decay_time_setting, int8_t sine_wave_offset, uint8_t use_current_comparator) +{ + //perform some sanity checks + if (constant_off_time < 2) { + constant_off_time = 2; + } else if (constant_off_time > 15) { + constant_off_time = 15; + } + //save the constant off time + this->constant_off_time = constant_off_time; + int8_t blank_value; + //calculate the value acc to the clock cycles + if (blank_time >= 54) { + blank_value = 3; + } else if (blank_time >= 36) { + blank_value = 2; + } else if (blank_time >= 24) { + blank_value = 1; + } else { + blank_value = 0; + } + this->blank_time = blank_time; + + if (fast_decay_time_setting < 0) { + fast_decay_time_setting = 0; + } else if (fast_decay_time_setting > 15) { + fast_decay_time_setting = 15; + } + if (sine_wave_offset < -3) { + sine_wave_offset = -3; + } else if (sine_wave_offset > 12) { + sine_wave_offset = 12; + } + //shift the sine_wave_offset + sine_wave_offset += 3; + + //calculate the register setting + //first of all delete all the values for this + chopper_config_register &= ~((1 << 12) | BLANK_TIMING_PATTERN | HYSTERESIS_DECREMENT_PATTERN | HYSTERESIS_LOW_VALUE_PATTERN | HYSTERESIS_START_VALUE_PATTERN | T_OFF_TIMING_PATERN); + //set the constant off pattern + chopper_config_register |= CHOPPER_MODE_T_OFF_FAST_DECAY; + //set the blank timing value + chopper_config_register |= ((unsigned long)blank_value) << BLANK_TIMING_SHIFT; + //setting the constant off time + chopper_config_register |= constant_off_time; + //set the fast decay time + //set msb + chopper_config_register |= (((unsigned long)(fast_decay_time_setting & 0x8)) << HYSTERESIS_DECREMENT_SHIFT); + //other bits + chopper_config_register |= (((unsigned long)(fast_decay_time_setting & 0x7)) << HYSTERESIS_START_VALUE_SHIFT); + //set the sine wave offset + chopper_config_register |= (unsigned long)sine_wave_offset << HYSTERESIS_LOW_SHIFT; + //using the current comparator? + if (!use_current_comparator) { + chopper_config_register |= (1 << 12); + } + //if started we directly send it to the motor + if (started) { + send262(chopper_config_register); + } +} + +/* + * constant_off_time: The off time setting controls the minimum chopper frequency. + * For most applications an off time within the range of 5μs to 20μs will fit. + * 2...15: off time setting + * + * blank_time: Selects the comparator blank time. This time needs to safely cover the switching event and the + * duration of the ringing on the sense resistor. For + * 0: min. setting 3: max. setting + * + * hysteresis_start: Hysteresis start setting. Please remark, that this value is an offset to the hysteresis end value HEND. + * 1...8 + * + * hysteresis_end: Hysteresis end setting. Sets the hysteresis end value after a number of decrements. Decrement interval time is controlled by HDEC. + * The sum HSTRT+HEND must be <16. At a current setting CS of max. 30 (amplitude reduced to 240), the sum is not limited. + * -3..-1: negative HEND 0: zero HEND 1...12: positive HEND + * + * hysteresis_decrement: Hysteresis decrement setting. This setting determines the slope of the hysteresis during on time and during fast decay time. + * 0: fast decrement 3: very slow decrement + */ + +void TMC26X::setSpreadCycleChopper(int8_t constant_off_time, int8_t blank_time, int8_t hysteresis_start, int8_t hysteresis_end, int8_t hysteresis_decrement) +{ + h_start = hysteresis_start; + h_end = hysteresis_end; + h_decrement = hysteresis_decrement; + this->blank_time = blank_time; + + //perform some sanity checks + if (constant_off_time < 2) { + constant_off_time = 2; + } else if (constant_off_time > 15) { + constant_off_time = 15; + } + //save the constant off time + this->constant_off_time = constant_off_time; + int8_t blank_value; + //calculate the value acc to the clock cycles + if (blank_time >= 54) { + blank_value = 3; + } else if (blank_time >= 36) { + blank_value = 2; + } else if (blank_time >= 24) { + blank_value = 1; + } else { + blank_value = 0; + } + + if (hysteresis_start < 1) { + hysteresis_start = 1; + } else if (hysteresis_start > 8) { + hysteresis_start = 8; + } + hysteresis_start--; + + if (hysteresis_end < -3) { + hysteresis_end = -3; + } else if (hysteresis_end > 12) { + hysteresis_end = 12; + } + //shift the hysteresis_end + hysteresis_end += 3; + + if (hysteresis_decrement < 0) { + hysteresis_decrement = 0; + } else if (hysteresis_decrement > 3) { + hysteresis_decrement = 3; + } + + //first of all delete all the values for this + chopper_config_register &= ~(CHOPPER_MODE_T_OFF_FAST_DECAY | BLANK_TIMING_PATTERN | HYSTERESIS_DECREMENT_PATTERN | HYSTERESIS_LOW_VALUE_PATTERN | HYSTERESIS_START_VALUE_PATTERN | T_OFF_TIMING_PATERN); + + //set the blank timing value + chopper_config_register |= ((unsigned long)blank_value) << BLANK_TIMING_SHIFT; + //setting the constant off time + chopper_config_register |= constant_off_time; + //set the hysteresis_start + chopper_config_register |= ((unsigned long)hysteresis_start) << HYSTERESIS_START_VALUE_SHIFT; + //set the hysteresis end + chopper_config_register |= ((unsigned long)hysteresis_end) << HYSTERESIS_LOW_SHIFT; + //set the hystereis decrement + chopper_config_register |= ((unsigned long)hysteresis_decrement) << HYSTERESIS_DECREMENT_SHIFT; + + //if started we directly send it to the motor + if (started) { + send262(chopper_config_register); + } +} + +/* + * In a constant off time chopper scheme both coil choppers run freely, i.e. are not synchronized. + * The frequency of each chopper mainly depends on the coil current and the position dependant motor coil inductivity, thus it depends on the microstep position. + * With some motors a slightly audible beat can occur between the chopper frequencies, especially when they are near to each other. This typically occurs at a + * few microstep positions within each quarter wave. This effect normally is not audible when compared to mechanical noise generated by ball bearings, etc. + * Further factors which can cause a similar effect are a poor layout of sense resistor GND connection. + * Hint: A common factor, which can cause motor noise, is a bad PCB layout causing coupling of both sense resistor voltages + * (please refer to sense resistor layout hint in chapter 8.1). + * In order to minimize the effect of a beat between both chopper frequencies, an internal random generator is provided. + * It modulates the slow decay time setting when switched on by the RNDTF bit. The RNDTF feature further spreads the chopper spectrum, + * reducing electromagnetic emission on single frequencies. + */ +void TMC26X::setRandomOffTime(int8_t value) +{ + if (value) { + chopper_config_register |= RANDOM_TOFF_TIME; + } else { + chopper_config_register &= ~(RANDOM_TOFF_TIME); + } + //if started we directly send it to the motor + if (started) { + send262(chopper_config_register); + } +} + +void TMC26X::setCoolStepConfiguration(unsigned int lower_SG_threshold, unsigned int SG_hysteresis, uint8_t current_decrement_step_size, + uint8_t current_increment_step_size, uint8_t lower_current_limit) +{ + //sanitize the input values + if (lower_SG_threshold > 480) { + lower_SG_threshold = 480; + } + //divide by 32 + lower_SG_threshold >>= 5; + if (SG_hysteresis > 480) { + SG_hysteresis = 480; + } + //divide by 32 + SG_hysteresis >>= 5; + if (current_decrement_step_size > 3) { + current_decrement_step_size = 3; + } + if (current_increment_step_size > 3) { + current_increment_step_size = 3; + } + if (lower_current_limit > 1) { + lower_current_limit = 1; + } + //store the lower level in order to enable/disable the cool step + this->cool_step_lower_threshold = lower_SG_threshold; + //if cool step is not enabled we delete the lower value to keep it disabled + if (!this->cool_step_enabled) { + lower_SG_threshold = 0; + } + //the good news is that we can start with a complete new cool step register value + //and simply set the values in the register + cool_step_register_value = ((unsigned long)lower_SG_threshold) | (((unsigned long)SG_hysteresis) << 8) | (((unsigned long)current_decrement_step_size) << 5) + | (((unsigned long)current_increment_step_size) << 13) | (((unsigned long)lower_current_limit) << 15) + //and of course we have to include the signature of the register + | COOL_STEP_REGISTER; + + if (started) { + send262(cool_step_register_value); + } +} + +void TMC26X::setCoolStepEnabled(bool enabled) +{ + //simply delete the lower limit to disable the cool step + cool_step_register_value &= ~SE_MIN_PATTERN; + //and set it to the proper value if cool step is to be enabled + if (enabled) { + cool_step_register_value |= this->cool_step_lower_threshold; + } + //and save the enabled status + this->cool_step_enabled = enabled; + //save the register value + if (started) { + send262(cool_step_register_value); + } +} + +bool TMC26X::isCoolStepEnabled(void) +{ + return this->cool_step_enabled; +} + +unsigned int TMC26X::getCoolStepLowerSgThreshold() +{ + //we return our internally stored value - in order to provide the correct setting even if cool step is not enabled + return this->cool_step_lower_threshold << 5; +} + +unsigned int TMC26X::getCoolStepUpperSgThreshold() +{ + return (uint8_t)((cool_step_register_value & SE_MAX_PATTERN) >> 8) << 5; +} + +uint8_t TMC26X::getCoolStepCurrentIncrementSize() +{ + return (uint8_t)((cool_step_register_value & CURRENT_DOWN_STEP_SPEED_PATTERN) >> 13); +} + +uint8_t TMC26X::getCoolStepNumberOfSGReadings() +{ + return (uint8_t)((cool_step_register_value & SE_CURRENT_STEP_WIDTH_PATTERN) >> 5); +} + +uint8_t TMC26X::getCoolStepLowerCurrentLimit() +{ + return (uint8_t)((cool_step_register_value & MINIMUM_CURRENT_FOURTH) >> 15); +} + +void TMC26X::setEnabled(bool enabled) +{ + //delete the t_off in the chopper config to get sure + chopper_config_register &= ~(T_OFF_PATTERN); + if (enabled) { + //and set the t_off time + chopper_config_register |= this->constant_off_time; + } + //if not enabled we don't have to do anything since we already delete t_off from the register + if (started) { + send262(chopper_config_register); + } +} + +bool TMC26X::isEnabled() +{ + if (chopper_config_register & T_OFF_PATTERN) { + return true; + } else { + return false; + } +} + +/* + * reads a value from the TMC26X status register. The value is not obtained directly but can then + * be read by the various status routines. + * + */ +void TMC26X::readStatus(int8_t read_value) +{ + unsigned long old_driver_configuration_register_value = driver_configuration_register_value; + //reset the readout configuration + driver_configuration_register_value &= ~(READ_SELECTION_PATTERN); + //this now equals TMC26X_READOUT_POSITION - so we just have to check the other two options + if (read_value == TMC26X_READOUT_STALLGUARD) { + driver_configuration_register_value |= READ_STALL_GUARD_READING; + } else if (read_value == TMC26X_READOUT_CURRENT) { + driver_configuration_register_value |= READ_STALL_GUARD_AND_COOL_STEP; + } + //all other cases are ignored to prevent funny values + //check if the readout is configured for the value we are interested in + if (driver_configuration_register_value != old_driver_configuration_register_value) { + //because then we need to write the value twice - one time for configuring, second time to get the value, see below + send262(driver_configuration_register_value); + } + //write the configuration to get the last status + send262(driver_configuration_register_value); +} + +//reads the stall guard setting from last status +//returns -1 if stallguard information is not present +int TMC26X::getCurrentStallGuardReading(void) +{ + //if we don't yet started there cannot be a stall guard value + if (!started) { + return -1; + } + //not time optimal, but solution optiomal: + //first read out the stall guard value + readStatus(TMC26X_READOUT_STALLGUARD); + return getReadoutValue(); +} + +uint8_t TMC26X::getCurrentCSReading(void) +{ + //if we don't yet started there cannot be a stall guard value + if (!started) { + return 0; + } + + //first read out the stall guard value + readStatus(TMC26X_READOUT_CURRENT); + return (getReadoutValue() & 0x1f); +} + +unsigned int TMC26X::getCoolstepCurrent(void) +{ + float result = (float)getCurrentCSReading(); + float resistor_value = (float)this->resistor; + float voltage = (driver_configuration_register_value & VSENSE) ? 0.165F : 0.31F; + result = (result + 1.0F) / 32.0F * voltage / resistor_value * 1000.0F * 1000.0F; + return (unsigned int)roundf(result); +} + +/* + return true if the stallguard threshold has been reached +*/ +bool TMC26X::isStallGuardOverThreshold(void) +{ + if (!this->started) { + return false; + } + return (driver_status_result & STATUS_STALL_GUARD_STATUS); +} + +/* + returns if there is any over temperature condition: + OVER_TEMPERATURE_PREWARING if pre warning level has been reached + OVER_TEMPERATURE_SHUTDOWN if the temperature is so hot that the driver is shut down + Any of those levels are not too good. +*/ +int8_t TMC26X::getOverTemperature(void) +{ + if (!this->started) { + return 0; + } + if (driver_status_result & STATUS_OVER_TEMPERATURE_SHUTDOWN) { + return TMC26X_OVERTEMPERATURE_SHUTDOWN; + } + if (driver_status_result & STATUS_OVER_TEMPERATURE_WARNING) { + return TMC26X_OVERTEMPERATURE_PREWARING; + } + return 0; +} + +//is motor channel A shorted to ground +bool TMC26X::isShortToGroundA(void) +{ + if (!this->started) { + return false; + } + return (driver_status_result & STATUS_SHORT_TO_GROUND_A); +} + +//is motor channel B shorted to ground +bool TMC26X::isShortToGroundB(void) +{ + if (!this->started) { + return false; + } + return (driver_status_result & STATUS_SHORT_TO_GROUND_B); +} + +//is motor channel A connected +bool TMC26X::isOpenLoadA(void) +{ + if (!this->started) { + return false; + } + return (driver_status_result & STATUS_OPEN_LOAD_A); +} + +//is motor channel B connected +bool TMC26X::isOpenLoadB(void) +{ + if (!this->started) { + return false; + } + return (driver_status_result & STATUS_OPEN_LOAD_B); +} + +//is chopper inactive since 2^20 clock cycles - defaults to ~0,08s +bool TMC26X::isStandStill(void) +{ + if (!this->started) { + return false; + } + return (driver_status_result & STATUS_STAND_STILL); +} + +//is chopper inactive since 2^20 clock cycles - defaults to ~0,08s +bool TMC26X::isStallGuardReached(void) +{ + if (!this->started) { + return false; + } + return (driver_status_result & STATUS_STALL_GUARD_STATUS); +} + +//reads the stall guard setting from last status +//returns -1 if stallguard inforamtion is not present +int TMC26X::getReadoutValue(void) +{ + return (int)(driver_status_result >> 10); +} + +bool TMC26X::isCurrentScalingHalfed() +{ + if (this->driver_configuration_register_value & VSENSE) { + return true; + } else { + return false; + } +} + +void TMC26X::dumpStatus(StreamOutput *stream, bool readable) +{ + if (readable) { + stream->printf("designator %c, Chip type TMC26X\n", designator); + + check_error_status_bits(stream); + + if (this->isStallGuardReached()) { + stream->printf("INFO: Stall Guard level reached!\n"); + } + + if (this->isStandStill()) { + stream->printf("INFO: Motor is standing still.\n"); + } + + int value = getReadoutValue(); + stream->printf("Microstep postion phase A: %d\n", value); + + value = getCurrentStallGuardReading(); + stream->printf("Stall Guard value: %d\n", value); + + stream->printf("Current setting: %dmA\n", getCurrent()); + stream->printf("Coolstep current: %dmA\n", getCoolstepCurrent()); + + stream->printf("Microsteps: 1/%d\n", microsteps); + + stream->printf("Register dump:\n"); + stream->printf(" driver control register: %08lX(%ld)\n", driver_control_register_value, driver_control_register_value); + stream->printf(" chopper config register: %08lX(%ld)\n", chopper_config_register, chopper_config_register); + stream->printf(" cool step register: %08lX(%ld)\n", cool_step_register_value, cool_step_register_value); + stream->printf(" stall guard2 current register: %08lX(%ld)\n", stall_guard2_current_register_value, stall_guard2_current_register_value); + stream->printf(" driver configuration register: %08lX(%ld)\n", driver_configuration_register_value, driver_configuration_register_value); + stream->printf(" motor_driver_control.xxx.reg %05lX,%05lX,%05lX,%05lX,%05lX\n", driver_control_register_value, chopper_config_register, cool_step_register_value, stall_guard2_current_register_value, driver_configuration_register_value); + + } else { + // TODO hardcoded for X need to select ABC as needed + bool moving = THEROBOT->actuators[0]->is_moving(); + // dump out in the format that the processing script needs + if (moving) { + stream->printf("#sg%d,p%lu,k%u,r,", getCurrentStallGuardReading(), THEROBOT->actuators[0]->get_current_step(), getCoolstepCurrent()); + } else { + readStatus(TMC26X_READOUT_POSITION); // get the status bits + stream->printf("#s,"); + } + stream->printf("d%d,", THEROBOT->actuators[0]->which_direction() ? 1 : -1); + stream->printf("c%u,m%d,", getCurrent(), getMicrosteps()); + // stream->printf('S'); + // stream->printf(tmc26XStepper.getSpeed(), DEC); + stream->printf("t%d,f%d,", getStallGuardThreshold(), getStallGuardFilter()); + + //print out the general cool step config + if (isCoolStepEnabled()) stream->printf("Ke+,"); + else stream->printf("Ke-,"); + + stream->printf("Kl%u,Ku%u,Kn%u,Ki%u,Km%u,", + getCoolStepLowerSgThreshold(), getCoolStepUpperSgThreshold(), getCoolStepNumberOfSGReadings(), getCoolStepCurrentIncrementSize(), getCoolStepLowerCurrentLimit()); + + //detect the winding status + if (isOpenLoadA()) { + stream->printf("ao,"); + } else if(isShortToGroundA()) { + stream->printf("ag,"); + } else { + stream->printf("a-,"); + } + //detect the winding status + if (isOpenLoadB()) { + stream->printf("bo,"); + } else if(isShortToGroundB()) { + stream->printf("bg,"); + } else { + stream->printf("b-,"); + } + + char temperature = getOverTemperature(); + if (temperature == 0) { + stream->printf("x-,"); + } else if (temperature == TMC26X_OVERTEMPERATURE_PREWARING) { + stream->printf("xw,"); + } else { + stream->printf("xe,"); + } + + if (isEnabled()) { + stream->printf("e1,"); + } else { + stream->printf("e0,"); + } + + //write out the current chopper config + stream->printf("Cm%d,", (chopper_config_register & CHOPPER_MODE_T_OFF_FAST_DECAY) != 0); + stream->printf("Co%d,Cb%d,", constant_off_time, blank_time); + if ((chopper_config_register & CHOPPER_MODE_T_OFF_FAST_DECAY) == 0) { + stream->printf("Cs%d,Ce%d,Cd%d,", h_start, h_end, h_decrement); + } + stream->printf("\n"); + } +} + +// check error bits and report, only report once +bool TMC26X::check_error_status_bits(StreamOutput *stream) +{ + bool error= false; + readStatus(TMC26X_READOUT_POSITION); // get the status bits + + if (this->getOverTemperature()&TMC26X_OVERTEMPERATURE_PREWARING) { + if(!error_reported.test(0)) stream->printf("%c - WARNING: Overtemperature Prewarning!\n", designator); + error_reported.set(0); + }else{ + error_reported.reset(0); + } + + if (this->getOverTemperature()&TMC26X_OVERTEMPERATURE_SHUTDOWN) { + if(!error_reported.test(1)) stream->printf("%c - ERROR: Overtemperature Shutdown!\n", designator); + error=true; + error_reported.set(1); + }else{ + error_reported.reset(1); + } + + if (this->isShortToGroundA()) { + if(!error_reported.test(2)) stream->printf("%c - ERROR: SHORT to ground on channel A!\n", designator); + error=true; + error_reported.set(2); + }else{ + error_reported.reset(2); + } + + if (this->isShortToGroundB()) { + if(!error_reported.test(3)) stream->printf("%c - ERROR: SHORT to ground on channel B!\n", designator); + error=true; + error_reported.set(3); + }else{ + error_reported.reset(3); + } + + // these seem to be triggered when moving so ignore them for now + if (this->isOpenLoadA()) { + if(!error_reported.test(4)) stream->printf("%c - ERROR: Channel A seems to be unconnected!\n", designator); + error=true; + error_reported.set(4); + }else{ + error_reported.reset(4); + } + + if (this->isOpenLoadB()) { + if(!error_reported.test(5)) stream->printf("%c - ERROR: Channel B seems to be unconnected!\n", designator); + error=true; + error_reported.set(5); + }else{ + error_reported.reset(5); + } + + // if(error) { + // stream->printf("%08X\n", driver_status_result); + // } + return error; +} + +bool TMC26X::checkAlarm() +{ + return check_error_status_bits(THEKERNEL->streams); +} + +// sets a raw register to the value specified, for advanced settings +// register 255 writes them, 0 displays what registers are mapped to what +// FIXME status registers not reading back correctly, check docs +bool TMC26X::setRawRegister(StreamOutput *stream, uint32_t reg, uint32_t val) +{ + switch(reg) { + case 255: + send262(driver_control_register_value); + send262(chopper_config_register); + send262(cool_step_register_value); + send262(stall_guard2_current_register_value); + send262(driver_configuration_register_value); + stream->printf("Registers written\n"); + break; + + + case 1: driver_control_register_value = val; stream->printf("driver control register set to %08lX\n", val); break; + case 2: chopper_config_register = val; stream->printf("chopper config register set to %08lX\n", val); break; + case 3: cool_step_register_value = val; stream->printf("cool step register set to %08lX\n", val); break; + case 4: stall_guard2_current_register_value = val; stream->printf("stall guard2 current register set to %08lX\n", val); break; + case 5: driver_configuration_register_value = val; stream->printf("driver configuration register set to %08lX\n", val); break; + + default: + stream->printf("1: driver control register\n"); + stream->printf("2: chopper config register\n"); + stream->printf("3: cool step register\n"); + stream->printf("4: stall guard2 current register\n"); + stream->printf("5: driver configuration register\n"); + stream->printf("255: update all registers\n"); + return false; + } + return true; +} + +/* + * send register settings to the stepper driver via SPI + * returns the current status + * sends 20bits, the last 20 bits of the 24bits is taken as the command + */ +void TMC26X::send262(unsigned long datagram) +{ + uint8_t buf[] {(uint8_t)(datagram >> 16), (uint8_t)(datagram >> 8), (uint8_t)(datagram & 0xff)}; + uint8_t rbuf[3]; + + //write/read the values + spi(buf, 3, rbuf); + + // construct reply + unsigned long i_datagram = ((rbuf[0] << 16) | (rbuf[1] << 8) | (rbuf[2])) >> 4; + + //store the datagram as status result + driver_status_result = i_datagram; + + //THEKERNEL->streams->printf("sent: %02X, %02X, %02X received: %02X, %02X, %02X \n", buf[0], buf[1], buf[2], rbuf[0], rbuf[1], rbuf[2]); +} + +#define HAS(X) (options.find(X) != options.end()) +#define GET(X) (options.at(X)) +bool TMC26X::set_options(const options_t& options) +{ + bool set = false; + if(HAS('O') || HAS('Q')) { + // void TMC26X::setStallGuardThreshold(int8_t stall_guard_threshold, int8_t stall_guard_filter_enabled) + int8_t o = HAS('O') ? GET('O') : getStallGuardThreshold(); + int8_t q = HAS('Q') ? GET('Q') : getStallGuardFilter(); + setStallGuardThreshold(o, q); + set = true; + } + + if(HAS('H') && HAS('I') && HAS('J') && HAS('K') && HAS('L')) { + //void TMC26X::setCoolStepConfiguration(unsigned int lower_SG_threshold, unsigned int SG_hysteresis, uint8_t current_decrement_step_size, uint8_t current_increment_step_size, uint8_t lower_current_limit) + setCoolStepConfiguration(GET('H'), GET('I'), GET('J'), GET('K'), GET('L')); + set = true; + } + + if(HAS('S')) { + uint32_t s = GET('S'); + if(s == 0 && HAS('U') && HAS('V') && HAS('W') && HAS('X') && HAS('Y')) { + //void TMC26X::setConstantOffTimeChopper(int8_t constant_off_time, int8_t blank_time, int8_t fast_decay_time_setting, int8_t sine_wave_offset, uint8_t use_current_comparator) + setConstantOffTimeChopper(GET('U'), GET('V'), GET('W'), GET('X'), GET('Y')); + set = true; + + } else if(s == 1 && HAS('U') && HAS('V') && HAS('W') && HAS('X') && HAS('Y')) { + //void TMC26X::setSpreadCycleChopper(int8_t constant_off_time, int8_t blank_time, int8_t hysteresis_start, int8_t hysteresis_end, int8_t hysteresis_decrement); + setSpreadCycleChopper(GET('U'), GET('V'), GET('W'), GET('X'), GET('Y')); + set = true; + + } else if(s == 2 && HAS('Z')) { + setRandomOffTime(GET('Z')); + set = true; + + } else if(s == 3 && HAS('Z')) { + setDoubleEdge(GET('Z')); + set = true; + + } else if(s == 4 && HAS('Z')) { + setStepInterpolation(GET('Z')); + set = true; + + } else if(s == 5 && HAS('Z')) { + setCoolStepEnabled(GET('Z') == 1); + set = true; + } + } + + return set; +} diff --git a/src/modules/utils/motordrivercontrol/drivers/TMC26X/TMC26X.h b/src/modules/utils/motordrivercontrol/drivers/TMC26X/TMC26X.h new file mode 100644 index 00000000..661d5973 --- /dev/null +++ b/src/modules/utils/motordrivercontrol/drivers/TMC26X/TMC26X.h @@ -0,0 +1,453 @@ +/* + modified from... + TMC26X.cpp - - TMC26X Stepper library for Wiring/Arduino + + based on the stepper library by Tom Igoe, et. al. + + Copyright (c) 2011, Interactive Matter, Marcus Nowotny + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + 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. + + */ + + +// ensure this library description is only included once +#pragma once + +#include +#include +#include + +class StreamOutput; + +#define TMC2660_max_current 4000 + +/*! + * \class TMC26X + * \brief Class representing a TMC26X stepper driver + */ +class TMC26X +{ +public: + /*! + * \brief creates a new represenatation of a stepper motor connected to a TMC26X stepper driver + * + * This is the main constructor. If in doubt use this. You must provide all parameters as described below. + * + * \param spi send function + * + * By default the Constant Off Time chopper is used, see TCM2630Stepper.setConstantOffTimeChopper() for details. + * This should work on most motors (YMMV). You may want to configure and use the Spread Cycle Chopper, see setSpreadCycleChopper(). + * + * By default a microstepping of 1/32th is used to provide a smooth motor run, while still giving a good progression per step. + * You can select a different stepping with setMicrosteps() to aa different value. + * \sa start(), setMicrosteps() + */ + TMC26X(std::function spi, char designator); + + /*! + * \brief configures the TMC26X stepper driver. Before you called this function the stepper driver is in nonfunctional mode. + * + * \param rms_current the maximum current to privide to the motor in mA (!). A value of 200 would send up to 200mA to the motor + * \param resistor the current sense resistor in milli Ohm, defaults to ,15 Ohm ( or 150 milli Ohm) as in the TMC260 Arduino Shield + + * This routine configures the TMC26X stepper driver for the given values via SPI. + * Most member functions are non functional if the driver has not been started. + * Therefore it is best to call this in your Arduino setup() function. + */ + void init(uint16_t cs); + + /*! + * \brief Set the number of microsteps in 2^i values (rounded) up to 256 + * + * This method set's the number of microsteps per step in 2^i interval. + * This means you can select 1, 2, 4, 16, 32, 64, 128 or 256 as valid microsteps. + * If you give any other value it will be rounded to the next smaller number (3 would give a microstepping of 2). + * You can always check the current microstepping with getMicrosteps(). + */ + void setMicrosteps(int number_of_steps); + + /*! + * \brief returns the effective current number of microsteps selected. + * + * This function always returns the effective number of microsteps. + * This can be a bit different than the micro steps set in setMicrosteps() since it is rounded to 2^i. + * + * \sa setMicrosteps() + */ + int getMicrosteps(void); + + void setStepInterpolation(int8_t value); + void setDoubleEdge(int8_t value); + + /*! + * \brief Sets and configure the classical Constant Off Timer Chopper + * \param constant_off_time The off time setting controls the minimum chopper frequency. For most applications an off time within the range of 5μs to 20μs will fit. Setting this parameter to zero completely disables all driver transistors and the motor can free-wheel. 0: chopper off, 1:15: off time setting (1 will work with minimum blank time of 24 clocks) + * \param blank_time Selects the comparator blank time. This time needs to safely cover the switching event and the duration of the ringing on the sense resistor. For most low current drivers, a setting of 1 or 2 is good. For high current applications with large MOSFETs, a setting of 2 or 3 will be required. 0 (min setting) … (3) amx setting + * \param fast_decay_time_setting Fast decay time setting. Controls the portion of fast decay for each chopper cycle. 0: slow decay only, 1…15: duration of fast decay phase + * \param sine_wave_offset Sine wave offset. Controls the sine wave offset. A positive offset corrects for zero crossing error. -3…-1: negative offset, 0: no offset,1…12: positive offset + * \param use_curreent_comparator Selects usage of the current comparator for termination of the fast decay cycle. If current comparator is enabled, it terminates the fast decay cycle in case the current reaches a higher negative value than the actual positive value. (0 disable, -1 enable). + * + * The classic constant off time chopper uses a fixed portion of fast decay following each on phase. + * While the duration of the on time is determined by the chopper comparator, the fast decay time needs + * to be set by the user in a way, that the current decay is enough for the driver to be able to follow + * the falling slope of the sine wave, and on the other hand it should not be too long, in order to minimize + * motor current ripple and power dissipation. This best can be tuned using an oscilloscope or + * trying out motor smoothness at different velocities. A good starting value is a fast decay time setting + * similar to the slow decay time setting. + * After tuning of the fast decay time, the offset should be determined, in order to have a smooth zero transition. + * This is necessary, because the fast decay phase leads to the absolute value of the motor current being lower + * than the target current (see figure 17). If the zero offset is too low, the motor stands still for a short + * moment during current zero crossing, if it is set too high, it makes a larger microstep. + * Typically, a positive offset setting is required for optimum operation. + * + * \sa setSpreadCycleChoper() for other alternatives. + * \sa setRandomOffTime() for spreading the noise over a wider spectrum + */ + void setConstantOffTimeChopper(int8_t constant_off_time, int8_t blank_time, int8_t fast_decay_time_setting, int8_t sine_wave_offset, uint8_t use_current_comparator); + + /*! + * \brief Sets and configures with spread cycle chopper. + * \param constant_off_time The off time setting controls the minimum chopper frequency. For most applications an off time within the range of 5μs to 20μs will fit. Setting this parameter to zero completely disables all driver transistors and the motor can free-wheel. 0: chopper off, 1:15: off time setting (1 will work with minimum blank time of 24 clocks) + * \param blank_time Selects the comparator blank time. This time needs to safely cover the switching event and the duration of the ringing on the sense resistor. For most low current drivers, a setting of 1 or 2 is good. For high current applications with large MOSFETs, a setting of 2 or 3 will be required. 0 (min setting) … (3) amx setting + * \param hysteresis_start Hysteresis start setting. Please remark, that this value is an offset to the hysteresis end value. 1 … 8 + * \param hysteresis_end Hysteresis end setting. Sets the hysteresis end value after a number of decrements. Decrement interval time is controlled by hysteresis_decrement. The sum hysteresis_start + hysteresis_end must be <16. At a current setting CS of max. 30 (amplitude reduced to 240), the sum is not limited. + * \param hysteresis_decrement Hysteresis decrement setting. This setting determines the slope of the hysteresis during on time and during fast decay time. 0 (fast decrement) … 3 (slow decrement). + * + * The spreadCycle chopper scheme (pat.fil.) is a precise and simple to use chopper principle, which automatically determines + * the optimum fast decay portion for the motor. Anyhow, a number of settings can be made in order to optimally fit the driver + * to the motor. + * Each chopper cycle is comprised of an on-phase, a slow decay phase, a fast decay phase and a second slow decay phase. + * The slow decay phases limit the maximum chopper frequency and are important for low motor and driver power dissipation. + * The hysteresis start setting limits the chopper frequency by forcing the driver to introduce a minimum amount of + * current ripple into the motor coils. The motor inductivity determines the ability to follow a changing motor current. + * The duration of the on- and fast decay phase needs to cover at least the blank time, because the current comparator is + * disabled during this time. + * + * \sa setRandomOffTime() for spreading the noise over a wider spectrum + */ + void setSpreadCycleChopper(int8_t constant_off_time, int8_t blank_time, int8_t hysteresis_start, int8_t hysteresis_end, int8_t hysteresis_decrement); + + /*! + * \brief Use random off time for noise reduction (0 for off, -1 for on). + * \param value 0 for off, -1 for on + * + * In a constant off time chopper scheme both coil choppers run freely, i.e. are not synchronized. + * The frequency of each chopper mainly depends on the coil current and the position dependant motor coil inductivity, + * thus it depends on the microstep position. With some motors a slightly audible beat can occur between the chopper + * frequencies, especially when they are near to each other. This typically occurs at a few microstep positions within + * each quarter wave. + * This effect normally is not audible when compared to mechanical noise generated by ball bearings, + * etc. Further factors which can cause a similar effect are a poor layout of sense resistor GND connection. + * In order to minimize the effect of a beat between both chopper frequencies, an internal random generator is provided. + * It modulates the slow decay time setting when switched on. The random off time feature further spreads the chopper spectrum, + * reducing electromagnetic emission on single frequencies. + */ + void setRandomOffTime(int8_t value); + + /*! + * \brief set the maximum motor current in mA (1000 is 1 Amp) + * Keep in mind this is the maximum peak Current. The RMS current will be 1/sqrt(2) smaller. The actual current can also be smaller + * by employing CoolStep. + * \param current the maximum motor current in mA + * \sa getCurrent(), getCurrentCurrent() + */ + void setCurrent(unsigned int current); + + /*! + * \brief readout the motor maximum current in mA (1000 is an Amp) + * This is the maximum current. to get the current current - which may be affected by CoolStep us getCurrentCurrent() + *\return the maximum motor current in milli amps + * \sa getCurrentCurrent() + */ + unsigned int getCurrent(void); + + /*! + * \brief set the StallGuard threshold in order to get sensible StallGuard readings. + * \param stall_guard_threshold -64 … 63 the StallGuard threshold + * \param stall_guard_filter_enabled 0 if the filter is disabled, -1 if it is enabled + * + * The StallGuard threshold is used to optimize the StallGuard reading to sensible values. It should be at 0 at + * the maximum allowable load on the motor (but not before). = is a good starting point (and the default) + * If you get Stall Gaurd readings of 0 without any load or with too little load increase the value. + * If you get readings of 1023 even with load decrease the setting. + * + * If you switch on the filter the StallGuard reading is only updated each 4th full step to reduce the noise in the + * reading. + * + * \sa getCurrentStallGuardReading() to read out the current value. + */ + void setStallGuardThreshold(int8_t stall_guard_threshold, int8_t stall_guard_filter_enabled); + + /*! + * \brief reads out the StallGuard threshold + * \return a number between -64 and 63. + */ + int8_t getStallGuardThreshold(void); + + /*! + * \brief returns the current setting of the StallGuard filter + * \return 0 if not set, -1 if set + */ + int8_t getStallGuardFilter(void); + + /*! + * \brief This method configures the CoolStep smart energy operation. You must have a proper StallGuard configuration for the motor situation (current, voltage, speed) in rder to use this feature. + * \param lower_SG_threshold Sets the lower threshold for stallGuard2TM reading. Below this value, the motor current becomes increased. Allowed values are 0...480 + * \param SG_hysteresis Sets the distance between the lower and the upper threshold for stallGuard2TM reading. Above the upper threshold (which is lower_SG_threshold+SG_hysteresis+1) the motor current becomes decreased. Allowed values are 0...480 + * \param current_decrement_step_size Sets the current decrement steps. If the StallGuard value is above the threshold the current gets decremented by this step size. 0...32 + * \param current_increment_step_size Sets the current increment step. The current becomes incremented for each measured stallGuard2TM value below the lower threshold. 0...8 + * \param lower_current_limit Sets the lower motor current limit for coolStepTM operation by scaling the CS value. Values can be COOL_STEP_HALF_CS_LIMIT, COOL_STEP_QUARTER_CS_LIMIT + * The CoolStep smart energy operation automatically adjust the current sent into the motor according to the current load, + * read out by the StallGuard in order to provide the optimum torque with the minimal current consumption. + * You configure the CoolStep current regulator by defining upper and lower bounds of StallGuard readouts. If the readout is above the + * limit the current gets increased, below the limit the current gets decreased. + * You can specify the upper an lower threshold of the StallGuard readout in order to adjust the current. You can also set the number of + * StallGuard readings neccessary above or below the limit to get a more stable current adjustement. + * The current adjustement itself is configured by the number of steps the current gests in- or decreased and the absolut minimum current + * (1/2 or 1/4th otf the configured current). + * \sa COOL_STEP_HALF_CS_LIMIT, COOL_STEP_QUARTER_CS_LIMIT + */ + void setCoolStepConfiguration(unsigned int lower_SG_threshold, unsigned int SG_hysteresis, uint8_t current_decrement_step_size, + uint8_t current_increment_step_size, uint8_t lower_current_limit); + + /*! + * \brief enables or disables the CoolStep smart energy operation feature. It must be configured before enabling it. + * \param enabled true if CoolStep should be enabled, false if not. + * \sa setCoolStepConfiguration() + */ + void setCoolStepEnabled(bool enabled); + + + /*! + * \brief check if the CoolStep feature is enabled + * \sa setCoolStepEnabled() + */ + bool isCoolStepEnabled(); + + /*! + * \brief returns the lower StallGuard threshold for the CoolStep operation + * \sa setCoolStepConfiguration() + */ + unsigned int getCoolStepLowerSgThreshold(); + + /*! + * \brief returns the upper StallGuard threshold for the CoolStep operation + * \sa setCoolStepConfiguration() + */ + unsigned int getCoolStepUpperSgThreshold(); + + /*! + * \brief returns the number of StallGuard readings befor CoolStep adjusts the motor current. + * \sa setCoolStepConfiguration() + */ + uint8_t getCoolStepNumberOfSGReadings(); + + /*! + * \brief returns the increment steps for the current for the CoolStep operation + * \sa setCoolStepConfiguration() + */ + uint8_t getCoolStepCurrentIncrementSize(); + + /*! + * \brief returns the absolut minium current for the CoolStep operation + * \sa setCoolStepConfiguration() + * \sa COOL_STEP_HALF_CS_LIMIT, COOL_STEP_QUARTER_CS_LIMIT + */ + uint8_t getCoolStepLowerCurrentLimit(); + + /*! + * \brief Reads the current StallGuard value. + * \return The current StallGuard value, lesser values indicate higher load, 0 means stall detected. + * Keep in mind that this routine reads and writes a value via SPI - so this may take a bit time. + * \sa setStallGuardThreshold() for tuning the readout to sensible ranges. + */ + int getCurrentStallGuardReading(void); + + /*! + * \brief Reads the current current setting value as fraction of the maximum current + * Returns values between 0 and 31, representing 1/32 to 32/32 (=1) + * \sa setCoolStepConfiguration() + */ + uint8_t getCurrentCSReading(void); + + + /*! + *\brief a convenience method to determine if the current scaling uses 0.31V or 0.165V as reference. + *\return false if 0.13V is the reference voltage, true if 0.165V is used. + */ + bool isCurrentScalingHalfed(); + + /*! + * \brief Reads the current current setting value and recalculates the absolute current in mA (1A would be 1000). + * This method calculates the currently used current setting (either by setting or by CoolStep) and reconstructs + * the current in mA by usinge the VSENSE and resistor value. This method uses floating point math - so it + * may not be the fastest. + * \sa getCurrentCSReading(), getResistor(), isCurrentScalingHalfed(), getCurrent() + */ + unsigned int getCoolstepCurrent(void); + + /*! + * \brief checks if there is a StallGuard warning in the last status + * \return 0 if there was no warning, -1 if there was some warning. + * Keep in mind that this method does not enforce a readout but uses the value of the last status readout. + * You may want to use getMotorPosition() or getCurrentStallGuardReading() to enforce an updated status readout. + * + * \sa setStallGuardThreshold() for tuning the readout to sensible ranges. + */ + bool isStallGuardOverThreshold(void); + + /*! + * \brief Return over temperature status of the last status readout + * return 0 is everything is OK, TMC26X_OVERTEMPERATURE_PREWARING if status is reached, TMC26X_OVERTEMPERATURE_SHUTDOWN is the chip is shutdown, -1 if the status is unknown. + * Keep in mind that this method does not enforce a readout but uses the value of the last status readout. + * You may want to use getMotorPosition() or getCurrentStallGuardReading() to enforce an updated status readout. + */ + int8_t getOverTemperature(void); + + /*! + * \brief Is motor channel A shorted to ground detected in the last status readout. + * \return true is yes, false if not. + * Keep in mind that this method does not enforce a readout but uses the value of the last status readout. + * You may want to use getMotorPosition() or getCurrentStallGuardReading() to enforce an updated status readout. + */ + + bool isShortToGroundA(void); + + /*! + * \brief Is motor channel B shorted to ground detected in the last status readout. + * \return true is yes, false if not. + * Keep in mind that this method does not enforce a readout but uses the value of the last status readout. + * You may want to use getMotorPosition() or getCurrentStallGuardReading() to enforce an updated status readout. + */ + bool isShortToGroundB(void); + /*! + * \brief iIs motor channel A connected according to the last statu readout. + * \return true is yes, false if not. + * Keep in mind that this method does not enforce a readout but uses the value of the last status readout. + * You may want to use getMotorPosition() or getCurrentStallGuardReading() to enforce an updated status readout. + */ + bool isOpenLoadA(void); + + /*! + * \brief iIs motor channel A connected according to the last statu readout. + * \return true is yes, false if not. + * Keep in mind that this method does not enforce a readout but uses the value of the last status readout. + * You may want to use getMotorPosition() or getCurrentStallGuardReading() to enforce an updated status readout. + */ + bool isOpenLoadB(void); + + /*! + * \brief Is chopper inactive since 2^20 clock cycles - defaults to ~0,08s + * \return true is yes, false if not. + * Keep in mind that this method does not enforce a readout but uses the value of the last status readout. + * You may want to use getMotorPosition() or getCurrentStallGuardReading() to enforce an updated status readout. + */ + bool isStandStill(void); + + /*! + * \brief checks if there is a StallGuard warning in the last status + * \return 0 if there was no warning, -1 if there was some warning. + * Keep in mind that this method does not enforce a readout but uses the value of the last status readout. + * You may want to use getMotorPosition() or getCurrentStallGuardReading() to enforce an updated status readout. + * + * \sa isStallGuardOverThreshold() + * TODO why? + * + * \sa setStallGuardThreshold() for tuning the readout to sensible ranges. + */ + bool isStallGuardReached(void); + + /*! + *\brief enables or disables the motor driver bridges. If disabled the motor can run freely. If enabled not. + *\param enabled a bool value true if the motor should be enabled, false otherwise. + */ + void setEnabled(bool enabled); + + /*! + *\brief checks if the output bridges are enabled. If the bridges are not enabled the motor can run freely + *\return true if the bridges and by that the motor driver are enabled, false if not. + *\sa setEnabled() + */ + bool isEnabled(); + + /*! + * \brief Manually read out the status register + * This function sends a byte to the motor driver in order to get the current readout. The parameter read_value + * seletcs which value will get returned. If the read_vlaue changes in respect to the previous readout this method + * automatically send two bytes to the motor: one to set the redout and one to get the actual readout. So this method + * may take time to send and read one or two bits - depending on the previous readout. + * \param read_value selects which value to read out (0..3). You can use the defines TMC26X_READOUT_POSITION, TMC_2630_READOUT_STALLGUARD, or TMC_2630_READOUT_CURRENT + * \sa TMC26X_READOUT_POSITION, TMC_2630_READOUT_STALLGUARD, TMC_2630_READOUT_CURRENT + */ + void readStatus(int8_t read_value); + + /*! + * \brief Prints out all the information that can be found in the last status read out - it does not force a status readout. + * The result is printed via Serial + */ + void dumpStatus(StreamOutput *stream, bool readable= true); + bool setRawRegister(StreamOutput *stream, uint32_t reg, uint32_t val); + bool checkAlarm(); + + using options_t= std::map; + + bool set_options(const options_t& options); + +private: + //helper routione to get the top 10 bit of the readout + inline int getReadoutValue(); + bool check_error_status_bits(StreamOutput *stream); + + // SPI sender + inline void send262(unsigned long datagram); + std::function spi; + + unsigned int resistor{50}; // current sense resitor value in milliohm + + //driver control register copies to easily set & modify the registers + unsigned long driver_control_register_value; + unsigned long chopper_config_register; + unsigned long cool_step_register_value; + unsigned long stall_guard2_current_register_value; + unsigned long driver_configuration_register_value; + //the driver status result + unsigned long driver_status_result; + + //status values + int microsteps; //the current number of micro steps + + std::bitset<8> error_reported; + + // only needed for the tuning app report + struct { + int8_t blank_time:8; + int8_t constant_off_time:5; //we need to remember this value in order to enable and disable the motor + int8_t h_start:4; + int8_t h_end:4; + int8_t h_decrement:3; + bool cool_step_enabled:1; //we need to remember this to configure the coolstep if it si enabled + bool started:1; //if the stepper has been started yet + }; + + uint8_t cool_step_lower_threshold; // we need to remember the threshold to enable and disable the CoolStep feature + char designator; + +}; +