From: Jim Morris Date: Mon, 14 Dec 2015 21:54:01 +0000 (-0800) Subject: Merge remote-tracking branch 'upstream/edge' into upstream-master X-Git-Url: http://git.hcoop.net/clinton/Smoothieware.git/commitdiff_plain/803e6349eb296ee48d8dca16982a705329a7b673?hp=9d27a610634f1dc5a27e67e41faf398a2bdf554a Merge remote-tracking branch 'upstream/edge' into upstream-master Conflicts: FirmwareBin/firmware-disablemsd.bin FirmwareBin/firmware.bin FirmwareBin/firmware.bin.md5sum --- diff --git a/.travis.yml b/.travis.yml new file mode 100644 index 00000000..f453eedb --- /dev/null +++ b/.travis.yml @@ -0,0 +1,12 @@ +before_script: + - sudo add-apt-repository -y ppa:terry.guo/gcc-arm-embedded + - sudo apt-get update -qq + - sudo apt-get install -y gcc-arm-none-eabi + +script: make + +# whitelist +branches: + only: + - edge + diff --git a/ConfigSamples/AzteegX5Mini.delta/config b/ConfigSamples/AzteegX5Mini.delta/config index 1a07b903..21273adf 100644 --- a/ConfigSamples/AzteegX5Mini.delta/config +++ b/ConfigSamples/AzteegX5Mini.delta/config @@ -24,8 +24,7 @@ junction_deviation 0.05 # Similar to the o # Stepper module configuration microseconds_per_step_pulse 1 # Duration of step pulses to stepper drivers, in microseconds -minimum_steps_per_minute 1200 # Never step slower than this -base_stepping_frequency 100000 # Base frequency for stepping, higher gives smoother movement +base_stepping_frequency 100000 # Base frequency for stepping # Stepper module pins ( ports, and pin numbers, appending "!" to the number will invert a pin ) alpha_step_pin 2.1 # Pin for alpha stepper step signal @@ -53,6 +52,9 @@ z_axis_max_speed 30000.0 # mm/min 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) #msd_disable false # disable the MSD (USB SDCARD) when set to true +#leds_disable true # disable using leds after config loaded +#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 # Extruder module configuration extruder_module_enable true # Whether to activate the extruder module at all. All configuration is ignored if false @@ -67,10 +69,16 @@ extruder_en_pin 0.4 # Pin for extruder delta_current 0.7 # Extruder stepper motor current # Laser module configuration -laser_module_enable false # Whether to activate the laser module at all. All configuration is ignored if false. -#laser_module_pin 2.7 # this pin will be PWMed to control the laser -#laser_module_max_power 0.8 # this is the maximum duty cycle that will be applied to the laser -#laser_module_tickle_power 0.0 # this duty cycle will be used for travel moves to keep the laser active without actually burning +laser_module_enable false # Whether to activate the laser module at all. All configuration is + # ignored if false. +#laser_module_pin 2.5 # this pin will be PWMed to control the laser. Only P2.0 - P2.5, P1.18, P1.20, P1.21, P1.23, P1.24, P1.26, P3.25, P3.26 + # can be used since laser requires hardware PWM +#laser_module_maximum_power 1.0 # this is the maximum duty cycle that will be applied to the laser +#laser_module_minimum_power 0.0 # This is a value just below the minimum duty cycle that keeps the laser + # active without actually burning. +#laser_module_default_power 0.8 # This is the default laser power that will be used for cuts if a power has not been specified. The value is a scale between + # the maximum and minimum power levels specified above +#laser_module_pwm_period 20 # this sets the pwm frequency as the period in microseconds # Hotend temperature control configuration temperature_control.hotend.enable true # Whether to activate this ( "hotend" ) module at all. All configuration is ignored if false. @@ -82,6 +90,8 @@ temperature_control.hotend.thermistor EPCOS100K # see http://smoot 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 0 by default #P39.98 I5.00 D79.91 # temperature_control.hotend.p_factor 39.98 # @@ -92,7 +102,6 @@ temperature_control.hotend.designator T # temperature_control.bed.enable false # temperature_control.bed.thermistor_pin 0.23 # temperature_control.bed.heater_pin 2.7 # -temperature_control.bed.beta 4036 # temperature_control.bed.thermistor EPCOS100K # http://smoothieware.org/temperaturecontrol#toc5 #temperature_control.bed.beta 4066 # or set the beta value @@ -173,8 +182,9 @@ zprobe.probe_height 5 # how much above be #leveling-strategy.delta-calibration.enable true # basic delta calibration #leveling-strategy.delta-calibration.radius 100 # the probe radius -# Pause button -pause_button_enable true # +# kill button (used to be called pause) maybe assigned to a different pin, set to the onboard pin by default +kill_button_enable true # 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) # Panel See http://smoothieware.org/panel panel.enable false # set to true to enable the panel code @@ -225,5 +235,3 @@ digipot_max_current 2.4 # max current digipot_factor 106.0 # factor for converting current to digipot value return_error_on_unhandled_gcode false # - - diff --git a/ConfigSamples/AzteegX5Mini/config b/ConfigSamples/AzteegX5Mini/config index bdaa84a9..8155b8f8 100755 --- a/ConfigSamples/AzteegX5Mini/config +++ b/ConfigSamples/AzteegX5Mini/config @@ -1,8 +1,8 @@ -#leveling-strategy.three-point-leveling.probe_offsets 0,0,0 # the probe offsets from nozzle, must be x,y,z, default is no offset# Robot module configurations : general handling of movement G-codes and slicing into moves +# 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.5 # Arcs are cut into segments ( lines ), this is the length for these segments. Smaller values mean more resolution, higher values mean faster computation -mm_per_line_segment 5 # Lines can be cut into segments ( not usefull with cartesian coordinates robots ). +#mm_per_line_segment 5 # Lines can be cut into segments ( not usefull with cartesian coordinates robots ). # Arm solution configuration : Cartesian robot. Translates mm positions into stepper positions alpha_steps_per_mm 80 # Steps per mm for alpha stepper @@ -19,8 +19,7 @@ junction_deviation 0.05 # Similar to the o # Stepper module configuration microseconds_per_step_pulse 1 # Duration of step pulses to stepper drivers, in microseconds -minimum_steps_per_minute 1200 # Never step slower than this -base_stepping_frequency 100000 # Base frequency for stepping, higher gives smoother movement +base_stepping_frequency 100000 # Base frequency for stepping # Stepper module pins ( ports, and pin numbers, appending "!" to the number will invert a pin ) alpha_step_pin 2.1 # Pin for alpha stepper step signal @@ -47,7 +46,11 @@ gamma_max_rate 300.0 # mm/min actuator # Serial communications configuration ( baud rate default 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) + #msd_disable false # disable the MSD (USB SDCARD) when set to true +#leds_disable true # disable using leds after config loaded +#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 # Extruder module configuration extruder_module_enable true # Whether to activate the extruder module at all. All configuration is ignored if false @@ -62,10 +65,16 @@ extruder_en_pin 0.4 # Pin for extruder delta_current 1.0 # Extruder stepper motor current # Laser module configuration -laser_module_enable false # Whether to activate the laser module at all. All configuration is ignored if false. -#laser_module_pin 2.7 # this pin will be PWMed to control the laser -#laser_module_max_power 0.8 # this is the maximum duty cycle that will be applied to the laser -#laser_module_tickle_power 0.0 # this duty cycle will be used for travel moves to keep the laser active without actually burning +laser_module_enable false # Whether to activate the laser module at all. All configuration is + # ignored if false. +#laser_module_pin 2.5 # this pin will be PWMed to control the laser. Only P2.0 - P2.5, P1.18, P1.20, P1.21, P1.23, P1.24, P1.26, P3.25, P3.26 + # can be used since laser requires hardware PWM +#laser_module_maximum_power 1.0 # this is the maximum duty cycle that will be applied to the laser +#laser_module_minimum_power 0.0 # This is a value just below the minimum duty cycle that keeps the laser + # active without actually burning. +#laser_module_default_power 0.8 # This is the default laser power that will be used for cuts if a power has not been specified. The value is a scale between + # the maximum and minimum power levels specified above +#laser_module_pwm_period 20 # this sets the pwm frequency as the period in microseconds # Hotend temperature control configuration temperature_control.hotend.enable true # Whether to activate this ( "hotend" ) module at all. All configuration is ignored if false. @@ -77,6 +86,8 @@ temperature_control.hotend.thermistor EPCOS100K # see http://smoot 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 0 by default temperature_control.hotend.p_factor 13.7 # temperature_control.hotend.i_factor 0.097 # @@ -112,6 +123,12 @@ switch.misc.output_pin 2.4 # #temperatureswitch.hotend.heatup_poll 15 # poll heatup at 15 sec intervals #temperatureswitch.hotend.cooldown_poll 60 # poll cooldown at 60 sec intervals +# filament out detector +#filament_detector.enable true # +#filament_detector.encoder_pin 0.26 # must be interrupt enabled pin (0.26, 0.27, 0.28) +#filament_detector.seconds_per_check 2 # may need to be longer +#filament_detector.pulses_per_mm 1 .0 # will need to be tuned +#filament_detector.bulge_pin 0.27 # optional bulge detector switch and/or manual suspend # Switch module for spindle control #switch.spindle.enable false # @@ -175,8 +192,9 @@ zprobe.probe_height 5 # how much above be #leveling-strategy.three-point-leveling.save_plane false # set to true to allow the bed plane to be saved with M500 default is false -# Pause button -pause_button_enable true # +# kill button (used to be called pause) maybe assigned to a different pin, set to the onboard pin by default +kill_button_enable true # 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) # Panel See http://smoothieware.org/panel panel.enable false # set to true to enable the panel code @@ -237,5 +255,3 @@ digipot_max_current 2.4 # max current digipot_factor 106.0 # factor for converting current to digipot value return_error_on_unhandled_gcode false # - - diff --git a/ConfigSamples/FirePick.delta/config b/ConfigSamples/FirePick.delta/config new file mode 100644 index 00000000..110e74e4 --- /dev/null +++ b/ConfigSamples/FirePick.delta/config @@ -0,0 +1,328 @@ +# 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.5 # Arcs are cut into segments ( lines ), this is the length for + # these segments. Smaller values mean more resolution, + # higher values mean faster computation +#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 : Rotatable Delta robot. Translates mm positions into stepper positions +arm_solution rotatable_delta # selects the delta arm solution + +delta_e 131.636 # End effector length +delta_f 190.526 # Base length +delta_re 270.000 # Carbon rod length +delta_rf 90.000 # Servo horn length + +delta_z_offset 268.0 # Distance from delta 8mm rod/pulley to table/bed + # NOTE: For OpenPnP, set the zero to be about 25mm above the bed. +delta_ee_offs 15.000 # Ball joint plane to bottom of end effector surface +tool_offset 30.500 # Distance between end effector ball joint plane and tip of tool (PnP) +z_home_angle -67.200 # This is the angle where the arms hit the endstop sensor +delta_printable_radius 150.0 # Print surface diameter/2 minus unreachable space + +xyz_full_steps_per_rotation 400.0 # stepper motor steps per 360 full rotation + # for 1.8 degree this is 200 for 0.9 degree this is 400 + +#the steps per mm are calculated as (xyz_full_steps_per_rotation*xyz_microsteps*(big_pulley_teeth/small_pulley_teeth))/360 +# for a 0.9 degree stepper motor (400 steps per rotation) and an a4988 driver set for 16 microsteps = (400 * 16 * (150/16))/360 = 166.67 +# for a 1.8 degree stepper motor (200 steps per rotation) and an a4988 driver set for 16 microsteps = (200 * 16 * (150/16))/360 = 81.10 +# for a 0.9 degree stepper motor (400 steps per rotation) and an drv8825 driver set for 32 microsteps = (400 * 32 * (150/16))/360 = 331.49 +# for a 1.8 degree stepper motor (200 steps per rotation) and an drv8825 driver set for 32 microsteps = (200 * 32 * (150/16))/360 = 165.29 +alpha_steps_per_mm 331.49 # Steps per mm for alpha stepper +beta_steps_per_mm 331.49 # Steps per mm for beta stepper +gamma_steps_per_mm 331.49 # 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. +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#L409 + # 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 +#minimum_planner_speed 0.0 # sets the minimum planner speed in mm/sec + +# Stepper module configuration +microseconds_per_step_pulse 1 # Duration of step pulses to stepper drivers, in microseconds +base_stepping_frequency 100000 # Base frequency for stepping, higher gives smoother movement + +# 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 2.0 # Pin for alpha stepper step signal +alpha_dir_pin 0.5 # Pin for alpha stepper direction +alpha_en_pin 0.4 # Pin for alpha enable pin +alpha_current 1.5 # X stepper motor current +alpha_max_rate 3000.0 # mm/min + +beta_step_pin 2.1 # Pin for beta stepper step signal +beta_dir_pin 0.11 # Pin for beta stepper direction +beta_en_pin 0.10 # Pin for beta enable +beta_current 1.5 # Y stepper motor current +beta_max_rate 3000.0 # mm/min + +gamma_step_pin 2.2 # Pin for gamma stepper step signal +gamma_dir_pin 0.20 # Pin for gamma stepper direction +gamma_en_pin 0.19 # Pin for gamma enable +gamma_current 1.5 # Z stepper motor current +gamma_max_rate 3000.0 # mm/min + +# Serial communications configuration ( baud rate default 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 +#msd_disable false # disable the MSD (USB SDCARD) when set to true +#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 + +# 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, as of 0.6, arbitrary ratio +extruder.hotend.max_speed 50 # mm/s + +extruder.hotend.step_pin 2.1 # Pin for extruder step signal should be 2.3 +extruder.hotend.dir_pin 0.22 # Pin for extruder dir signal +extruder.hotend.en_pin 0.21 # Pin for extruder enable signal + +# extruder offset +#extruder.hotend.x_offset 0 # x offset from origin in mm +#extruder.hotend.y_offset 0 # y offset from origin in mm +#extruder.hotend.z_offset 0 # z offset from origin in mm + +# firmware retract settings when using G10/G11, these are the defaults if not defined, must be defined for each extruder if not using the defaults +#extruder.hotend.retract_length 3 # retract length in mm +#extruder.hotend.retract_feedrate 45 # retract feedrate in mm/sec +#extruder.hotend.retract_recover_length 0 # additional length for recover +#extruder.hotend.retract_recover_feedrate 8 # recover feedrate in mm/sec (should be less than retract feedrate) +#extruder.hotend.retract_zlift_length 0 # zlift on retract in mm, 0 disables +#extruder.hotend.retract_zlift_feedrate 6000 # zlift feedrate in mm/min (Note mm/min NOT mm/sec) + +delta_current 1.5 # First extruder stepper motor current + +# Second extruder module configuration example +#extruder.hotend2.enable true # 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 2.8 # Pin for extruder step signal +#extruder.hotend2.dir_pin 2.13 # Pin for extruder dir signal +#extruder.hotend2.en_pin 4.29 # Pin for extruder enable signal + +#extruder.hotend2.x_offset 0 # x offset from origin in mm +#extruder.hotend2.y_offset 25.0 # y offset from origin in mm +#extruder.hotend2.z_offset 0 # z offset from origin in mm +#epsilon_current 1.5 # Second extruder stepper motor current + +# Laser module configuration +laser_module_enable false # Whether to activate the laser module at all. All configuration is + # ignored if false. +#laser_module_pin 2.5 # this pin will be PWMed to control the laser. Only P2.0 - P2.5, P1.18, P1.20, P1.21, P1.23, P1.24, P1.26, P3.25, P3.26 + # can be used since laser requires hardware PWM +#laser_module_maximum_power 1.0 # this is the maximum duty cycle that will be applied to the laser +#laser_module_minimum_power 0.0 # This is a value just below the minimum duty cycle that keeps the laser + # active without actually burning. +#laser_module_default_power 0.8 # This is the default laser power that will be used for cuts if a power has not been specified. The value is a scale between + # the maximum and minimum power levels specified above +#laser_module_pwm_period 20 # this sets the pwm frequency as the period in microseconds + +# Hotend temperature control 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 0.23 # Pin for the thermistor to read +temperature_control.hotend.heater_pin 2.7 # Pin that controls the heater +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.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. + +# Hotend2 temperature control configuration +#temperature_control.hotend2.enable true # Whether to activate this ( "hotend" ) module at all. + # All configuration is ignored if false. + +#temperature_control.hotend2.thermistor_pin 0.25 # Pin for the thermistor to read +#temperature_control.hotend2.heater_pin 1.23 # Pin that controls the heater +#temperature_control.hotend2.thermistor EPCOS100K # http://smoothieware.org/temperaturecontrol#toc5 +##temperature_control.hotend2.beta 4066 # or set the beta value + +#temperature_control.hotend2.set_m_code 884 # +#temperature_control.hotend2.set_and_wait_m_code 889 # +#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 true # +temperature_control.bed.thermistor_pin 0.24 # +temperature_control.bed.heater_pin 2.5 # +temperature_control.bed.thermistor Honeywell100K # see http://smoothieware.org/temperaturecontrol#toc5 +#temperature_control.bed.beta 4066 # 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 2.6 # +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 2.4 # +#switch.misc.output_type digital # just an on or off pin + +# automatically toggle a switch at a specified temperature. Different ones of these may be defined to monitor different temperatures and switch different swithxes +# useful to turn on a fan or water pump to cool the hotend +#temperatureswitch.hotend.enable true # +#temperatureswitch.hotend.designator T # first character of the temperature control designator to use as the temperature sensor to monitor +#temperatureswitch.hotend.switch misc # select which switch to use, matches the name of the defined switch +#temperatureswitch.hotend.threshold_temp 60.0 # temperature to turn on (if rising) or off the switch +#temperatureswitch.hotend.heatup_poll 15 # poll heatup at 15 sec intervals +#temperatureswitch.hotend.cooldown_poll 60 # poll cooldown at 60 sec intervals + +# Switch module for spindle control +#switch.spindle.enable false # + +# Endstops +endstops_enable true # the endstop module is enabled by default and can be disabled here +delta_homing true # forces all three axis to home a the same time regardless of + # what is specified in G28 +alpha_min_endstop 1.24^ # +alpha_max_endstop nc # add ! to invert pullup if switch is NO to ground +alpha_homing_direction home_to_min # Home up as the rotational delta has the min at the top! +alpha_min 0 # The ‘0’ home position is some 60 degrees down from the min endstop +beta_min_endstop 1.26^ # +beta_max_endstop nc # +beta_homing_direction home_to_min # +beta_min 0 # +gamma_min_endstop 1.28^ # +gamma_max_endstop nc # +gamma_homing_direction home_to_min # +gamma_min 0 # + +alpha_fast_homing_rate_mm_s 200 # homing feedrates in mm/second +beta_fast_homing_rate_mm_s 200 # +gamma_fast_homing_rate_mm_s 200 # 200 +alpha_slow_homing_rate_mm_s 50 # +beta_slow_homing_rate_mm_s 50 # +gamma_slow_homing_rate_mm_s 50 # 20 + +alpha_homing_retract_mm 5 # retract/bounce distance after homing in mm +beta_homing_retract_mm 5 # +gamma_homing_retract_mm 5 # + +alpha_trim -61 # software trim for alpha stepper endstop (in mm) +beta_trim -61 # software trim for beta stepper endstop (in mm) +gamma_trim -61 # software trim for gamma stepper endstop (in mm) + +#endstop_debounce_count 100 # uncomment if you get noise on your endstops + +# optional Z probe +zprobe.enable false # set to true to enable a zprobe +zprobe.probe_pin 1.28!^ # 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 5 # how much above bed to start probe +#gamma_min_endstop nc # normally 1.28. Change to nc to prevent conflict, + +# associated with zprobe the leveling strategy to use +#leveling-strategy.delta-calibration.enable true # basic delta calibration +#leveling-strategy.delta-calibration.radius 100 # the probe radius + +# Kill button +kill_button_enable true # + +# Panel +panel.enable false # set to true to enable the panel code +panel.lcd smoothiepanel # set type of panel +panel.encoder_a_pin 3.25!^ # encoder pin +panel.encoder_b_pin 3.26!^ # encoder pin + +# Example for reprap discount GLCD +# on glcd EXP1 is to left and EXP2 is to right, pin 1 is bottom left, pin 2 is top left etc. +# +5v is EXP1 pin 10, Gnd is EXP1 pin 9 +#panel.lcd reprap_discount_glcd # +#panel.spi_channel 0 # spi channel to use ; GLCD EXP1 Pins 3,5 (MOSI, SCLK) +#panel.spi_cs_pin 0.16 # spi chip select ; GLCD EXP1 Pin 4 +#panel.encoder_a_pin 3.25!^ # encoder pin ; GLCD EXP2 Pin 3 +#panel.encoder_b_pin 3.26!^ # encoder pin ; GLCD EXP2 Pin 5 +#panel.click_button_pin 1.30!^ # click button ; GLCD EXP1 Pin 2 +#panel.buzz_pin 1.31 # pin for buzzer ; GLCD EXP1 Pin 1 +#panel.button_pause_pin 2.11^ # kill/pause ; GLCD EXP2 Pin 8 either +#panel.back_button_pin 2.11!^ # back button ; GLCD EXP2 Pin 8 or + +# pins used with other panels +#panel.up_button_pin 0.1! # up button if used +#panel.down_button_pin 0.0! # down button if used +#panel.click_button_pin 0.18! # click button if used + +panel.menu_offset 0 # some panels will need 1 here + +panel.alpha_jog_feedrate 6000 # x jogging feedrate in mm/min +panel.beta_jog_feedrate 6000 # y jogging feedrate in mm/min +panel.gamma_jog_feedrate 6000 # z jogging feedrate in mm/min + +panel.hotend_temperature 185 # temp to set hotend when preheat is selected +panel.bed_temperature 60 # temp to set bed when preheat is selected + +# Example of a custom menu entry, which will show up in the Custom entry. +# NOTE _ gets converted to space in the menu and commands, | is used to separate multiple commands +custom_menu.power_on.enable true # +custom_menu.power_on.name Power_on # +custom_menu.power_on.command M80 # + +custom_menu.power_off.enable true # +custom_menu.power_off.name Power_off # +custom_menu.power_off.command M81 # + +# Only needed on a smoothieboard +currentcontrol_module_enable true # + +return_error_on_unhandled_gcode false # + +# network settings +network.enable false # enable the ethernet network services +network.webserver.enable true # enable the webserver +network.telnet.enable true # enable the telnet server +network.ip_address auto # use dhcp to get ip address +# uncomment the 3 below to manually setup ip address +#network.ip_address 192.168.3.222 # the IP address +#network.ip_mask 255.255.255.0 # the ip mask +#network.ip_gateway 192.168.3.1 # the gateway address +#network.mac_override xx.xx.xx.xx.xx.xx # override the mac address, only do this if you have a conflict diff --git a/ConfigSamples/README.TXT b/ConfigSamples/README.TXT index 5070cb17..b5b660f1 100644 --- a/ConfigSamples/README.TXT +++ b/ConfigSamples/README.TXT @@ -11,9 +11,14 @@ The full configs found in the ConfigSamples director include: * Smoothieboard.delta - This directory contains a sample config for a Rostock or Kossel style linear delta bot using the most recent Smoothieboard. +* FirePick.delta - This directory contains a sample config for a + FirePick Rotatable delta bot using a custom Smoothie compatible board, see http://delta.firepick.org + * AzteegX5Mini - This directory contains a sample config file for using the Azteeg X5 Mini. +* Snippets - these are snippets of config that can be included in the main config or copy pasted. + There are also example config fragments to be be found in the rest of the docs. You can find even more information about Smoothie configuration at this URL: diff --git a/ConfigSamples/Smoothieboard.delta/config b/ConfigSamples/Smoothieboard.delta/config index e36fef8e..6f7a4b9f 100644 --- a/ConfigSamples/Smoothieboard.delta/config +++ b/ConfigSamples/Smoothieboard.delta/config @@ -34,8 +34,7 @@ junction_deviation 0.05 # Similar to the o # Stepper module configuration microseconds_per_step_pulse 1 # Duration of step pulses to stepper drivers, in microseconds -minimum_steps_per_minute 1200 # Never step slower than this -base_stepping_frequency 100000 # Base frequency for stepping, higher gives smoother movement +base_stepping_frequency 100000 # Base frequency for stepping # Cartesian axis speed limits x_axis_max_speed 30000 # mm/min @@ -67,6 +66,8 @@ second_usb_serial_enable false # This enables a s # and a terminal connected) #leds_disable true # disable using leds after config loaded #msd_disable false # disable the MSD (USB SDCARD) when set to true +#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 # Extruder module configuration extruder.hotend.enable true # Whether to activate the extruder module at all. All configuration is ignored if false @@ -113,11 +114,13 @@ delta_current 1.5 # First extruder s # Laser module configuration laser_module_enable false # Whether to activate the laser module at all. All configuration is # ignored if false. -#laser_module_pin 2.5 # this pin will be PWMed to control the laser. Only P2.0 - P2.5 +#laser_module_pin 2.5 # this pin will be PWMed to control the laser. Only P2.0 - P2.5, P1.18, P1.20, P1.21, P1.23, P1.24, P1.26, P3.25, P3.26 # can be used since laser requires hardware PWM -#laser_module_max_power 0.8 # this is the maximum duty cycle that will be applied to the laser -#laser_module_tickle_power 0.0 # this duty cycle will be used for travel moves to keep the laser - # active without actually burning +#laser_module_maximum_power 1.0 # this is the maximum duty cycle that will be applied to the laser +#laser_module_minimum_power 0.0 # This is a value just below the minimum duty cycle that keeps the laser + # active without actually burning. +#laser_module_default_power 0.8 # This is the default laser power that will be used for cuts if a power has not been specified. The value is a scale between + # the maximum and minimum power levels specified above #laser_module_pwm_period 20 # this sets the pwm frequency as the period in microseconds # Hotend temperature control configuration @@ -131,6 +134,8 @@ temperature_control.hotend.thermistor EPCOS100K # see http://smoot 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 0 by default #temperature_control.hotend.p_factor 13.7 # permanently set the PID values after an auto pid #temperature_control.hotend.i_factor 0.097 # @@ -147,8 +152,8 @@ temperature_control.hotend.designator T # #temperature_control.hotend2.thermistor EPCOS100K # http://smoothieware.org/temperaturecontrol#toc5 ##temperature_control.hotend2.beta 4066 # or set the beta value -#temperature_control.hotend2.set_m_code 884 # -#temperature_control.hotend2.set_and_wait_m_code 889 # +#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 @@ -244,8 +249,9 @@ zprobe.probe_height 5 # how much above be #leveling-strategy.delta-calibration.enable true # basic delta calibration #leveling-strategy.delta-calibration.radius 100 # the probe radius -# Pause button -pause_button_enable true # +# kill button (used to be called pause) maybe assigned to a different pin, set to the onboard pin by default +kill_button_enable true # 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) # Panel panel.enable false # set to true to enable the panel code @@ -305,4 +311,3 @@ network.ip_address auto # use dhcp to get #network.ip_mask 255.255.255.0 # the ip mask #network.ip_gateway 192.168.3.1 # the gateway address #network.mac_override xx.xx.xx.xx.xx.xx # override the mac address, only do this if you have a conflict - diff --git a/ConfigSamples/Smoothieboard/config b/ConfigSamples/Smoothieboard/config index 7e26c5d2..70d82828 100644 --- a/ConfigSamples/Smoothieboard/config +++ b/ConfigSamples/Smoothieboard/config @@ -5,7 +5,7 @@ default_seek_rate 4000 # Default rate ( m mm_per_arc_segment 0.5 # Arcs are cut into segments ( lines ), this is the length for # these segments. Smaller values mean more resolution, # higher values mean faster computation -mm_per_line_segment 5 # Lines can be cut into segments ( not usefull with cartesian +#mm_per_line_segment 5 # Lines can be cut into segments ( not usefull with cartesian # coordinates robots ). # Arm solution configuration : Cartesian robot. Translates mm positions into stepper positions @@ -28,8 +28,7 @@ junction_deviation 0.05 # Similar to the o # Stepper module configuration microseconds_per_step_pulse 1 # Duration of step pulses to stepper drivers, in microseconds -minimum_steps_per_minute 1200 # Never step slower than this -base_stepping_frequency 100000 # Base frequency for stepping, higher gives smoother movement +base_stepping_frequency 100000 # Base frequency for stepping # Cartesian axis speed limits x_axis_max_speed 30000 # mm/min @@ -61,18 +60,20 @@ second_usb_serial_enable false # This enables a s # and a terminal connected) #leds_disable true # disable using leds after config loaded #play_led_disable true # disable the play led -pause_button_enable true # Pause button enable -#pause_button_pin 2.12 # pause button pin. default is P2.12 -#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) + +# kill button (used to be called pause) maybe assigned to a different pin, set to the onboard pin by default +kill_button_enable true # 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 # 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, as of 0.6, arbitrary ratio +extruder.hotend.acceleration 500 # Acceleration for the stepper motor mm/sec² extruder.hotend.max_speed 50 # mm/s extruder.hotend.step_pin 2.3 # Pin for extruder step signal @@ -116,9 +117,11 @@ laser_module_enable false # Whether to activ # ignored if false. #laser_module_pin 2.5 # this pin will be PWMed to control the laser. Only P2.0 - P2.5, P1.18, P1.20, P1.21, P1.23, P1.24, P1.26, P3.25, P3.26 # can be used since laser requires hardware PWM -#laser_module_max_power 0.8 # this is the maximum duty cycle that will be applied to the laser -#laser_module_tickle_power 0.0 # this duty cycle will be used for travel moves to keep the laser - # active without actually burning +#laser_module_maximum_power 1.0 # this is the maximum duty cycle that will be applied to the laser +#laser_module_minimum_power 0.0 # This is a value just below the minimum duty cycle that keeps the laser + # active without actually burning. +#laser_module_default_power 0.8 # This is the default laser power that will be used for cuts if a power has not been specified. The value is a scale between + # the maximum and minimum power levels specified above #laser_module_pwm_period 20 # this sets the pwm frequency as the period in microseconds # Hotend temperature control configuration @@ -131,6 +134,8 @@ temperature_control.hotend.thermistor EPCOS100K # see http://smoot 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 # @@ -146,8 +151,8 @@ temperature_control.hotend.designator T # #temperature_control.hotend2.heater_pin 1.23 # 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 884 # -#temperature_control.hotend2.set_and_wait_m_code 889 # +#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 @@ -186,7 +191,7 @@ switch.fan.output_type pwm # pwm output setta # automatically toggle a switch at a specified temperature. Different ones of these may be defined to monitor different temperatures and switch different swithxes # useful to turn on a fan or water pump to cool the hotend -#temperatureswitch.hotend.enable true # +#temperatureswitch.hotend.enable true # #temperatureswitch.hotend.designator T # first character of the temperature control designator to use as the temperature sensor to monitor #temperatureswitch.hotend.switch misc # select which switch to use, matches the name of the defined switch #temperatureswitch.hotend.threshold_temp 60.0 # temperature to turn on (if rising) or off the switch diff --git a/ConfigSamples/Snippets/ZprobeGrid.config b/ConfigSamples/Snippets/ZprobeGrid.config new file mode 100644 index 00000000..45e216b0 --- /dev/null +++ b/ConfigSamples/Snippets/ZprobeGrid.config @@ -0,0 +1,27 @@ +leveling-strategy.ZGrid-leveling.enable true # +leveling-strategy.ZGrid-leveling.bed_x 200 # Bed size +leveling-strategy.ZGrid-leveling.bed_y 200 # Bed Size + +# Machine height, used to determine probe attachment point (bed_z / 2) +leveling-strategy.ZGrid-leveling.bed_z 20 + +leveling-strategy.ZGrid-leveling.rows 7 # X divisions (Default 5) +leveling-strategy.ZGrid-leveling.cols 9 # Y divisions (Default 5) + +leveling-strategy.ZGrid-leveling.probe_offsets 5,5,16.3 # + +#leveling-strategy.ZGrid-leveling.slow_feedrate 100 # optional + +#Configure for Machines with bed 0,0 at center of platform +leveling-strategy.ZGrid-leveling.center_zero false + +# configure for Machines with circular beds +leveling-strategy.ZGrid-leveling.circular_bed false + +# 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) + +#gamma_max 200 # make sure this is set correctly diff --git a/ConfigSamples/Snippets/corexz.config b/ConfigSamples/Snippets/corexz.config new file mode 100644 index 00000000..3220b32e --- /dev/null +++ b/ConfigSamples/Snippets/corexz.config @@ -0,0 +1,7 @@ +# Sample arm solution configuration for a CoreXZ style robot +arm_solution corexz # selects the corexz solution +x_reduction 1.0 # standard CoreXZ has no reduction on x axis +z_reduction 3.0 # standard CoreXZ has 3X reduction on z axis +alpha_steps_per_mm 55 # xz motor 1 +beta_steps_per_mm 55 # xz motor 2 +gamma_steps_per_mm 110 # y axis motor diff --git a/ConfigSamples/Snippets/drill_cycles.config b/ConfigSamples/Snippets/drill_cycles.config new file mode 100644 index 00000000..da5af05b --- /dev/null +++ b/ConfigSamples/Snippets/drill_cycles.config @@ -0,0 +1,6 @@ +# Drills module +# Implement the Canned Drilling Cycles +# G80-83, G98, G99 in absolute mode only +# Incremental mode not implemented (L) +drillingcycles.enable false # enable module, default false +drillingcycles.dwell_units S # dwell units [S = seconds, P = millis], default: S diff --git a/ConfigSamples/Snippets/filament-change-menu.config b/ConfigSamples/Snippets/filament-change-menu.config new file mode 100644 index 00000000..8064eb39 --- /dev/null +++ b/ConfigSamples/Snippets/filament-change-menu.config @@ -0,0 +1,10 @@ +custom_menu.filament_change.enable true # +custom_menu.filament_change.name filament_change # +custom_menu.filament_change.command M600 # + +custom_menu.resume.enable true # +custom_menu.resume.name resume # +custom_menu.resume.command M601 # + +after_suspend_gcode G91_G0Z10_G90 # optionally raise nozzle 10mm after suspend +leave_heaters_on_suspend true # set to false if you want the heaters turned off on suspend diff --git a/ConfigSamples/Snippets/filament_out_switch.config b/ConfigSamples/Snippets/filament_out_switch.config new file mode 100644 index 00000000..bf2aae39 --- /dev/null +++ b/ConfigSamples/Snippets/filament_out_switch.config @@ -0,0 +1,11 @@ +switch.filamentout.enable true # +switch.filamentout.input_pin 1.30^ # pin where filemant out button is connected +switch.filamentout.output_on_command suspend # command + +switch.resume.enable true # +switch.resume.input_pin 1.31^ # pin where resum ebutton is connected +switch.resume.output_on_command resume # command + +after_suspend_gcode G91_G0E-5_G0Z10_G90_G0X-50Y-50 # gcode to run after suspend, retract then get head out of way +before_resume_gcode G91_G1E1_G90 # gcode to run after temp is reached but before resume - do a prime + diff --git a/ConfigSamples/Snippets/morgan_scara.config b/ConfigSamples/Snippets/morgan_scara.config new file mode 100644 index 00000000..565265b8 --- /dev/null +++ b/ConfigSamples/Snippets/morgan_scara.config @@ -0,0 +1,30 @@ +# Arm solution configuration : SCARA robot. Translates mm positions into stepper positions +arm_solution morgan # selects the morgan arm solution +alpha_steps_per_mm 537.03528 # Steps per mm for alpha stepper +beta_steps_per_mm 537.03528 # Steps per mm for beta stepper +gamma_steps_per_mm 420 # Steps per mm for gamma stepper + +arm1_length 150 # this is the length of an arm1 (inner) from hinge to hinge +arm2_length 150 # this is the length of an arm2 (outer) from hinge to hinge +morgan_offset_x 190.0 # tower offset from bed 0:0 default 100.0 +morgan_offset_y -65.0 # tower offset from bed 0:0 default -65.0 +morgan_undefined_min 0.95 # Defines undefined SCARA ratio: default 0.95 +morgan_undefined_max 0.90 # Defines undefined SCARA ratio: default 0.95 + +scara_homing true # always home XY together + +# optional SCARAcal (highly recommended for Morgan) +scaracal.enable true +scaracal.slow_feedrate 20 # slow enough not to slip, fast enough not to frustrate the user +scaracal.z_move -20 # Optional movement of Z relative to the home position. + # positive: more distance between bed and probe +# associated with zprobe the leveling strategy to use +leveling-strategy.ZGrid-leveling.enable true # enable map level +leveling-strategy.ZGrid-leveling.bed_x 380 +leveling-strategy.ZGrid-leveling.bed_y 220 +leveling-strategy.ZGrid-leveling.bed_z 200 +leveling-strategy.ZGrid-leveling.slow_feedrate 100 +leveling-strategy.ZGrid-leveling.probe_offsets 0,0,14.5 # the probe offsets from nozzle, must be x,y,z, default is no offset +leveling-strategy.ZGrid-leveling.wait_for_probe true # Makes system wait for probe confirmation +leveling-strategy.ZGrid-leveling.rows 7 # X divisions (Default 5) +leveling-strategy.ZGrid-leveling.cols 5 # Y divisions (Default 5) diff --git a/ConfigSamples/Snippets/psu_off_when_cooled_down.config b/ConfigSamples/Snippets/psu_off_when_cooled_down.config new file mode 100644 index 00000000..8a499839 --- /dev/null +++ b/ConfigSamples/Snippets/psu_off_when_cooled_down.config @@ -0,0 +1,20 @@ +switch.psu.enable true # turn atx on/off +switch.psu.input_on_command M80 # +switch.psu.input_off_command M81 # +switch.psu.output_pin 2.4 # small mosfet +switch.psu.output_type digital # on/off only + +# turn the PSU off when the hotend temp cools below 50, only do this after M1100 S1 has been executed + +temperatureswitch.psu_off.enable true # +temperatureswitch.psu_off.designator T # first character of the temperature control designator to use as the temperature sensor to monitor +temperatureswitch.psu_off.switch psu # select which switch to use, matches the name of the defined switch +temperatureswitch.psu_off.threshold_temp 50.0 # temperature to trigger at when falling +temperatureswitch.psu_off.heatup_poll 30 # poll heatup every 30 seconds +temperatureswitch.psu_off.cooldown_poll 30 # poll cooldown every 30 seconds +temperatureswitch.psu_off.arm_mcode 1100 # M1100 S1 will arm it +temperatureswitch.psu_off.trigger falling # only trigger when the temp falls below after being above +temperatureswitch.psu_off.inverted false # turn the switch off when we trigger (by default switches on when rising and off when falling) + + + diff --git a/FirmwareBin/Readme.md b/FirmwareBin/Readme.md new file mode 100644 index 00000000..e8e9c64a --- /dev/null +++ b/FirmwareBin/Readme.md @@ -0,0 +1,7 @@ +**NOTE** + +Make sure you download the binary file and not the HTML page. + +Click the file you want, then click the View Raw button to get the raw binary file. + +Flashing the HTML will mean the leds will not flash :) diff --git a/FirmwareBin/firmware-disablemsd.bin b/FirmwareBin/firmware-disablemsd.bin dissimilarity index 66% index d0607b1d..5ce3eff2 100755 Binary files a/FirmwareBin/firmware-disablemsd.bin and b/FirmwareBin/firmware-disablemsd.bin differ diff --git a/FirmwareBin/firmware.bin b/FirmwareBin/firmware.bin dissimilarity index 66% index cb3d4f54..316ff818 100755 Binary files a/FirmwareBin/firmware.bin and b/FirmwareBin/firmware.bin differ diff --git a/FirmwareBin/firmware.bin.md5sum b/FirmwareBin/firmware.bin.md5sum index d4e68520..ffa2d477 100644 --- a/FirmwareBin/firmware.bin.md5sum +++ b/FirmwareBin/firmware.bin.md5sum @@ -1 +1 @@ -f0496aeccbb3097c70ab7d6f0bec75cd FirmwareBin/firmware.bin +4963f9a9bf4f4b228c8501f6710e350d FirmwareBin/firmware.bin diff --git a/README.creole b/README.creole index d7c3e426..c37bcca3 100644 --- a/README.creole +++ b/README.creole @@ -1,3 +1,5 @@ +Current build status: {{https://travis-ci.org/Smoothieware/Smoothieware.svg?branch=edge}} + ==Overview Smoothie is a free, opensource, high performance G-code interpreter and CNC controller written in Object-Oriented C++ for the LPC17xx micro-controller ( ARM Cortex M3 architecture ). It will run on a mBed, a LPCXpresso, a SmoothieBoard, R2C2 or any other LPC17xx-based board. The motion control part is a port of the awesome grbl. diff --git a/Rakefile b/Rakefile index e510a3ac..5213d6b0 100644 --- a/Rakefile +++ b/Rakefile @@ -2,6 +2,10 @@ require 'rake' require 'pathname' require 'fileutils' +verbose(ENV['verbose'] == '1') +DEBUG = ENV['debug'] == '1' +TESTING = ENV['testing'] == '1' + def pop_path(path) Pathname(path).each_filename.to_a[1..-1] end @@ -70,6 +74,13 @@ SIZE = "#{TOOLSBIN}size" # include a defaults file if present load 'rakefile.defaults' if File.exists?('rakefile.defaults') +if TESTING + BUILDTYPE= 'Testing' + +elsif DEBUG + BUILDTYPE= 'Debug' + ENABLE_DEBUG_MONITOR= '0' +end # Set build type BUILDTYPE= ENV['BUILDTYPE'] || 'Checked' unless defined? BUILDTYPE @@ -81,7 +92,9 @@ ENABLE_DEBUG_MONITOR = ENV['ENABLE_DEBUG_MONITOR'] || '0' unless defined? ENABLE DEFAULT_SERIAL_BAUD_RATE= ENV['BAUDRATE'] || '115200' unless defined? DEFAULT_SERIAL_BAUD_RATE # set to true to eliminate all the network code -NONETWORK= false unless defined? NONETWORK +unless defined? NONETWORK + NONETWORK= false || TESTING +end # list of modules to exclude, include directory it is in EXCLUDE_MODULES= %w(tools/touchprobe) unless defined? EXCLUDE_MODULES @@ -101,9 +114,21 @@ else nonetwork= false end -SRC = FileList['src/**/*.{c,cpp}'].exclude(/#{excludes.join('|')}/) +if TESTING + # add modules to be tested here + TESTMODULES= %w(tools/temperatureswitch) unless defined? EXCLUDE_MODULES + puts "Modules under test: #{TESTMODULES}" + excludes << %w(Kernel.cpp main.cpp) # we replace these with mock versions in testframework -puts "WARNING Excluding modules: #{EXCLUDE_MODULES.join(' ')}" unless exclude_defines.empty? + frameworkfiles= FileList['src/testframework/*.{c,cpp}', 'src/testframework/easyunit/*.{c,cpp}'] + extrafiles= FileList['src/modules/communication/SerialConsole.cpp', 'src/modules/communication/utils/Gcode.cpp', 'src/modules/robot/Conveyor.cpp', 'src/modules/robot/Block.cpp'] + testmodules= FileList['src/libs/**/*.{c,cpp}'].include(TESTMODULES.collect { |e| "src/modules/#{e}/**/*.{c,cpp}"}).include(TESTMODULES.collect { |e| "src/testframework/unittests/#{e}/*.{c,cpp}"}).exclude(/#{excludes.join('|')}/) + SRC = frameworkfiles + extrafiles + testmodules +else + excludes << %w(testframework) + SRC = FileList['src/**/*.{c,cpp}'].exclude(/#{excludes.join('|')}/) + puts "WARNING Excluding modules: #{EXCLUDE_MODULES.join(' ')}" unless exclude_defines.empty? +end OBJDIR = 'OBJ' OBJ = SRC.collect { |fn| File.join(OBJDIR, pop_path(File.dirname(fn)), File.basename(fn).ext('o')) } + @@ -147,6 +172,10 @@ when 'checked' OPTIMIZATION = 2 MRI_ENABLE = 1 MRI_SEMIHOST_STDIO = 1 unless defined? MRI_SEMIHOST_STDIO +when 'testing' + OPTIMIZATION = 0 + MRI_ENABLE = 1 + MRI_SEMIHOST_STDIO = 0 unless defined? MRI_SEMIHOST_STDIO end MRI_ENABLE = 1 unless defined? MRI_ENABLE # set to 0 to disable MRI @@ -169,8 +198,9 @@ DEFINES= defines.join(' ') # Compiler flags used to enable creation of header dependencies. DEPFLAGS = '-MMD ' -CFLAGS = DEPFLAGS + "-Wall -Wextra -Wno-unused-parameter -Wcast-align -Wpointer-arith -Wredundant-decls -Wcast-qual -Wcast-align -O#{OPTIMIZATION} -g3 -mcpu=cortex-m3 -mthumb -mthumb-interwork -ffunction-sections -fdata-sections -fno-exceptions -fno-delete-null-pointer-checks" -CPPFLAGS = CFLAGS + ' -fno-rtti -std=gnu++11' +CFLAGS = DEPFLAGS + "-Wall -Wextra -Wno-unused-parameter -Wcast-align -Wpointer-arith -Wredundant-decls -Wcast-qual -Wcast-align -O#{OPTIMIZATION} -g3 -mcpu=cortex-m3 -mthumb -mthumb-interwork -ffunction-sections -fdata-sections -fno-delete-null-pointer-checks" +CPPFLAGS = CFLAGS + ' -fno-rtti -std=gnu++11 -fno-exceptions' +CXXFLAGS = CFLAGS + ' -fno-rtti -std=gnu++11 -fexceptions' # used for a .cxx file that needs to be compiled with exceptions MRI_WRAPS = MRI_ENABLE == 1 ? ',--wrap=_read,--wrap=_write,--wrap=semihost_connected' : '' @@ -243,7 +273,7 @@ file MBED_LIB do end file "#{OBJDIR}/mbed_custom.o" => ['./build/mbed_custom.cpp'] do |t| - puts "Compiling #{t.source}" + puts "Compiling mbed_custom.cpp" sh "#{CCPP} #{CPPFLAGS} #{INCLUDE} #{DEFINES} -c -o #{t.name} #{t.prerequisites[0]}" end @@ -268,6 +298,11 @@ rule '.o' => lambda{ |objfile| obj2src(objfile, 'cpp') } do |t| sh "#{CCPP} #{CPPFLAGS} #{INCLUDE} #{DEFINES} #{VERSION} -c -o #{t.name} #{t.source}" end +rule '.o' => lambda{ |objfile| obj2src(objfile, 'cxx') } do |t| + puts "Compiling #{t.source}" + sh "#{CCPP} #{CXXFLAGS} #{INCLUDE} #{DEFINES} #{VERSION} -c -o #{t.name} #{t.source}" +end + rule '.o' => lambda{ |objfile| obj2src(objfile, 'c') } do |t| puts "Compiling #{t.source}" sh "#{CC} #{CFLAGS} #{INCLUDE} #{DEFINES} #{VERSION} -c -o #{t.name} #{t.source}" diff --git a/build/common.mk b/build/common.mk index 9d30f281..c42c94c5 100755 --- a/build/common.mk +++ b/build/common.mk @@ -79,12 +79,15 @@ OUTDIR=../$(DEVICE) CSRCS1 = $(wildcard $(SRC)/*.c $(SRC)/*/*.c $(SRC)/*/*/*.c $(SRC)/*/*/*/*.c $(SRC)/*/*/*/*/*.c $(SRC)/*/*/*/*/*/*.c) # Totally exclude network if NONETWORK is defined ifeq "$(NONETWORK)" "1" -CSRCS = $(filter-out $(SRC)/libs/Network/%,$(CSRCS1)) +CSRCS2 = $(filter-out $(SRC)/libs/Network/%,$(CSRCS1)) DEFINES += -DNONETWORK else -CSRCS = $(CSRCS1) +CSRCS2 = $(CSRCS1) endif +# do not compile the src/testframework as that can only be done with rake +CSRCS = $(filter-out $(SRC)/testframework/%,$(CSRCS2)) + ifeq "$(DISABLEMSD)" "1" DEFINES += -DDISABLEMSD endif @@ -104,9 +107,12 @@ endif # uppercase function uc = $(subst a,A,$(subst b,B,$(subst c,C,$(subst d,D,$(subst e,E,$(subst f,F,$(subst g,G,$(subst h,H,$(subst i,I,$(subst j,J,$(subst k,K,$(subst l,L,$(subst m,M,$(subst n,N,$(subst o,O,$(subst p,P,$(subst q,Q,$(subst r,R,$(subst s,S,$(subst t,T,$(subst u,U,$(subst v,V,$(subst w,W,$(subst x,X,$(subst y,Y,$(subst z,Z,$1)))))))))))))))))))))))))) EXL = $(patsubst %,$(SRC)/modules/%/%,$(EXCLUDED_MODULES)) -CPPSRCS = $(filter-out $(EXL),$(CPPSRCS2)) +CPPSRCS3 = $(filter-out $(EXL),$(CPPSRCS2)) DEFINES += $(call uc, $(subst /,_,$(patsubst %,-DNO_%,$(EXCLUDED_MODULES)))) +# do not compile the src/testframework as that can only be done with rake +CPPSRCS = $(filter-out $(SRC)/testframework/%,$(CPPSRCS3)) + # List of the objects files to be compiled/assembled OBJECTS = $(patsubst %.c,$(OUTDIR)/%.o,$(CSRCS)) $(patsubst %.s,$(OUTDIR)/%.o,$(patsubst %.S,$(OUTDIR)/%.o,$(ASRCS))) $(patsubst %.cpp,$(OUTDIR)/%.o,$(CPPSRCS)) @@ -142,7 +148,8 @@ endif # Libraries to be linked into final binary MBED_LIBS = $(MBED_DIR)/$(DEVICE)/GCC_ARM/libmbed.a -SYS_LIBS = -lstdc++_s -lsupc++_s -lm -lgcc -lc_s -lgcc -lc_s -lnosys +#SYS_LIBS = -lstdc++_s -lsupc++_s -lm -lgcc -lc_s -lgcc -lc_s -lnosys +SYS_LIBS = -specs=nano.specs -lstdc++ -lsupc++ -lm -lgcc -lc -lnosys LIBS = $(LIBS_PREFIX) ifeq "$(MRI_ENABLE)" "1" diff --git a/mac_install b/mac_install index e74b6a75..064b0ddc 100755 --- a/mac_install +++ b/mac_install @@ -16,13 +16,13 @@ # Logs the command to be run and then executes the command while logging the results. RunAndLog () { - echo `date` Executing $@>>$LOGFILE - $@ 1>>$LOGFILE 2>$ERRORFILE + echo `date` Executing "$@">>"$LOGFILE" + "$@" 1>>"$LOGFILE" 2>"$ERRORFILE" if [ "$?" != "0" ] ; then - cat $ERRORFILE >>$LOGFILE - echo `date` Failure forced early exit>>$LOGFILE - cat $LOGFILE - rm -f $ERRORFILE + cat "$ERRORFILE" >>"$LOGFILE" + echo `date` Failure forced early exit>>"$LOGFILE" + cat "$LOGFILE" + rm -f "$ERRORFILE" popd >/dev/null read -n 1 -sp "Press any key to continue..." dummy ; echo exit 1 @@ -35,63 +35,64 @@ ROOTDIR=$0 ROOTDIR=${ROOTDIR%/*} pushd $ROOTDIR ROOTDIR=$PWD -LOGFILE=$ROOTDIR/mac_install.log -ERRORFILE=$ROOTDIR/mac_install.err +LOGFILE="$ROOTDIR"/mac_install.log +ERRORFILE="$ROOTDIR"/mac_install.err GCC4ARM_VERSION=gcc-arm-none-eabi-4_8-2014q1 GCC4ARM_FILENAME=gcc-arm-none-eabi-4_8-2014q1-20140314-mac.tar.bz2 GCC4ARM_URL=https://launchpad.net/gcc-arm-embedded/4.8/4.8-2014-q1-update/+download/$GCC4ARM_FILENAME -GCC4ARM_TAR=$ROOTDIR/$GCC4ARM_FILENAME +GCC4ARM_TAR="$ROOTDIR"/$GCC4ARM_FILENAME GCC4ARM_MD5=5d34d95a53ba545f1585b9136cbb6805 -GCC4ARM_EXTRACT=$ROOTDIR/$GCC4ARM_VERSION -GCC4ARM_DIR=$ROOTDIR/gcc-arm-none-eabi +GCC4ARM_EXTRACT="$ROOTDIR"/$GCC4ARM_VERSION +GCC4ARM_DIR="$ROOTDIR"/gcc-arm-none-eabi GCC4ARM_BINDIR=$GCC4ARM_DIR/bin -MACBIN_DIR=$ROOTDIR/build/osx64 -BUILDSHELL_CMD=$ROOTDIR/BuildShell +MACBIN_DIR="$ROOTDIR"/build/osx64 +BUILDSHELL_CMD="$ROOTDIR"/BuildShell -echo Logging install results to $LOGFILE -echo `date` Starting $0 $*>$LOGFILE +echo Logging install results to "$LOGFILE" +echo `date` Starting $0 $*>"$LOGFILE" echo Downloading GNU Tools for ARM Embedded Processors... -echo `date` Executing curl -L0 $GCC4ARM_URL>>$LOGFILE +echo `date` Executing curl -L0 $GCC4ARM_URL>>"$LOGFILE" curl -L0 $GCC4ARM_URL >$GCC4ARM_FILENAME echo Validating md5 signature of GNU Tools for ARM Embedded Processors... -echo `date` Validating md5 signature of GNU Tools for ARM Embedded Processors>>$LOGFILE +echo `date` Validating md5 signature of GNU Tools for ARM Embedded Processors>>"$LOGFILE" archive_match=`md5 -q $GCC4ARM_FILENAME | grep -c $GCC4ARM_MD5` if [ "$archive_match" != "1" ] ; then - echo $GCC4ARM_FILENAME failed MD5 signature check.>>$LOGFILE - echo `date` Failure forced early exit>>$LOGFILE - cat $LOGFILE - rm -f $ERRORFILE + echo $GCC4ARM_FILENAME failed MD5 signature check.>>"$LOGFILE" + echo `date` Failure forced early exit>>"$LOGFILE" + cat "$LOGFILE" + rm -f "$ERRORFILE" popd >/dev/null read -n 1 -sp "Press any key to continue..." dummy ; echo exit 1 fi echo Extracting GNU Tools for ARM Embedded Processors... -rm -r $GCC4ARM_DIR >/dev/null 2>/dev/null -RunAndLog tar xf $GCC4ARM_TAR -RunAndLog mv $GCC4ARM_EXTRACT $GCC4ARM_DIR +rm -r "$GCC4ARM_DIR" >/dev/null 2>/dev/null +echo $GCC4ARM_TAR +RunAndLog tar xf "$GCC4ARM_TAR" +RunAndLog mv "$GCC4ARM_EXTRACT" "$GCC4ARM_DIR" echo Installing patched 64-bit Intel Mac OS X GDB binaries... -RunAndLog cp $MACBIN_DIR/arm-none-eabi-gdb* $GCC4ARM_BINDIR/ +RunAndLog cp "$MACBIN_DIR"/arm-none-eabi-gdb* "$GCC4ARM_BINDIR/" echo Creating helper scripts... -echo "#! /usr/bin/env bash">$BUILDSHELL_CMD -echo "# Modify next line and set destination drive to match mbed device">>$BUILDSHELL_CMD -echo "export LPC_DEPLOY='cp PROJECT.bin /Volumes/MBED/ ; sync'">>$BUILDSHELL_CMD -echo>>$BUILDSHELL_CMD -echo "SCRIPT_PATH=\$0">>$BUILDSHELL_CMD -echo "SCRIPT_PATH=\${SCRIPT_PATH%/*}">>$BUILDSHELL_CMD -echo "cd \$SCRIPT_PATH">>$BUILDSHELL_CMD -echo "SCRIPT_PATH=\$PWD">>$BUILDSHELL_CMD -echo "export PATH=\$SCRIPT_PATH/gcc-arm-none-eabi/bin:\$SCRIPT_PATH/build/osx64:\$PATH">>$BUILDSHELL_CMD -echo "exec bash">>$BUILDSHELL_CMD -chmod +x $BUILDSHELL_CMD +echo "#! /usr/bin/env bash">"$BUILDSHELL_CMD" +echo "# Modify next line and set destination drive to match mbed device">>"$BUILDSHELL_CMD" +echo "export LPC_DEPLOY='cp PROJECT.bin /Volumes/MBED/ ; sync'">>"$BUILDSHELL_CMD" +echo>>"$BUILDSHELL_CMD" +echo "SCRIPT_PATH=\$0">>"$BUILDSHELL_CMD" +echo "SCRIPT_PATH=\${SCRIPT_PATH%/*}">>"$BUILDSHELL_CMD" +echo "cd \$SCRIPT_PATH">>"$BUILDSHELL_CMD" +echo "SCRIPT_PATH=\$PWD">>"$BUILDSHELL_CMD" +echo "export PATH=\$SCRIPT_PATH/gcc-arm-none-eabi/bin:\$SCRIPT_PATH/build/osx64:\$PATH">>"$BUILDSHELL_CMD" +echo "exec bash">>"$BUILDSHELL_CMD" +chmod +x "$BUILDSHELL_CMD" echo Cleaning up intermediate files... -RunAndLog rm $GCC4ARM_TAR +RunAndLog rm "$GCC4ARM_TAR" echo echo To build Smoothie, you will first need to run the following script @@ -108,8 +109,8 @@ echo $GCC4ARM_BINDIR # Restore current directory and exit script on success. -echo `date` Finished successfully>>$LOGFILE +echo `date` Finished successfully>>"$LOGFILE" echo Finished successfully -rm -f $ERRORFILE +rm -f "$ERRORFILE" popd >/dev/null read -n 1 -sp "Press any key to continue..." dummy ; echo diff --git a/mbed/src/cpp/stdio.cpp b/mbed/src/cpp/stdio.cpp index cd0a97ee..1e222683 100644 --- a/mbed/src/cpp/stdio.cpp +++ b/mbed/src/cpp/stdio.cpp @@ -305,11 +305,11 @@ extern "C" int remove(const char *path) { } extern "C" int rename(const char *oldname, const char *newname) { - FilePath fp(oldname); - FileSystemLike *fs = fp.fileSystem(); - if (fs == NULL) return -1; + FilePath a(oldname); + FilePath b(newname); + if (!a.fileSystem() || a.fileSystem() != b.fileSystem()) return -1; - return fs->rename(fp.fileName(), newname); + return a.fileSystem()->rename(a.fileName(), b.fileName()); } extern "C" char *tmpnam(char *s) { diff --git a/rakefile.defaults.example b/rakefile.defaults.example new file mode 100644 index 00000000..3026938c --- /dev/null +++ b/rakefile.defaults.example @@ -0,0 +1,8 @@ +# if regular build the modules to exclude from the build +NONETWORK = true +EXCLUDE_MODULES = %w(tools/touchprobe tools/laser tools/temperaturecontrol tools/extruder) + +# if test framework build the modules to include in the test... +# here we enable unit tests for tools/temperatureswitch module and any unit tests in /src/testframework/unittests/tools/temperatureswitch/ +# and any unittests in the src/testframework/unittests/libs folder +TESTMODULES= %w(tools/temperatureswitch libs) diff --git a/src/libs/ADC/adc.cpp b/src/libs/ADC/adc.cpp index a6fc7012..945b5d1c 100644 --- a/src/libs/ADC/adc.cpp +++ b/src/libs/ADC/adc.cpp @@ -4,15 +4,16 @@ */ #include "mbed.h" #include "adc.h" - - + +using namespace mbed; + ADC *ADC::instance; - + ADC::ADC(int sample_rate, int cclk_div) { - + int i, adc_clk_freq, pclk, clock_div, max_div=1; - + //Work out CCLK adc_clk_freq=CLKS_PER_SAMPLE*sample_rate; int m = (LPC_SC->PLL0CFG & 0xFFFF) + 1; @@ -20,7 +21,7 @@ ADC::ADC(int sample_rate, int cclk_div) int cclkdiv = LPC_SC->CCLKCFG + 1; int Fcco = (2 * m * XTAL_FREQ) / n; int cclk = Fcco / cclkdiv; - + //Power up the ADC LPC_SC->PCONP |= (1 << 12); //Set clock at cclk / 1. @@ -39,7 +40,7 @@ ADC::ADC(int sample_rate, int cclk_div) LPC_SC->PCLKSEL0 |= 0x3 << 24; break; default: - printf("Warning: ADC CCLK clock divider must be 1, 2, 4 or 8. %u supplied.\n", + printf("ADC Warning: ADC CCLK clock divider must be 1, 2, 4 or 8. %u supplied.\n", cclk_div); printf("Defaulting to 1.\n"); LPC_SC->PCLKSEL0 |= 0x1 << 24; @@ -47,29 +48,28 @@ ADC::ADC(int sample_rate, int cclk_div) } pclk = cclk / cclk_div; clock_div=pclk / adc_clk_freq; - + if (clock_div > 0xFF) { - //printf("Warning: Clock division is %u which is above 255 limit. Re-Setting at limit.\n", - // clock_div); + printf("ADC Warning: Clock division is %u which is above 255 limit. Re-Setting at limit.\n", clock_div); clock_div=0xFF; } if (clock_div == 0) { - printf("Warning: Clock division is 0. Re-Setting to 1.\n"); + printf("ADC Warning: Clock division is 0. Re-Setting to 1.\n"); clock_div=1; } - + _adc_clk_freq=pclk / clock_div; if (_adc_clk_freq > MAX_ADC_CLOCK) { - printf("Warning: Actual ADC sample rate of %u which is above %u limit\n", + printf("ADC Warning: Actual ADC sample rate of %u which is above %u limit\n", _adc_clk_freq / CLKS_PER_SAMPLE, MAX_ADC_CLOCK / CLKS_PER_SAMPLE); while ((pclk / max_div) > MAX_ADC_CLOCK) max_div++; - printf("Maximum recommended sample rate is %u\n", (pclk / max_div) / CLKS_PER_SAMPLE); + printf("ADC Warning: Maximum recommended sample rate is %u\n", (pclk / max_div) / CLKS_PER_SAMPLE); } - + LPC_ADC->ADCR = ((clock_div - 1 ) << 8 ) | //Clkdiv ( 1 << 21 ); //A/D operational - + //Default no channels enabled LPC_ADC->ADCR &= ~0xFF; //Default NULL global custom isr @@ -79,28 +79,28 @@ ADC::ADC(int sample_rate, int cclk_div) _adc_data[i] = 0; _adc_isr[i] = NULL; } - - + + //* Attach IRQ instance = this; NVIC_SetVector(ADC_IRQn, (uint32_t)&_adcisr); - + //Disable global interrupt LPC_ADC->ADINTEN &= ~0x100; - + }; - + void ADC::_adcisr(void) { instance->adcisr(); } - - + + void ADC::adcisr(void) { uint32_t stat; int chan; - + // Read status stat = LPC_ADC->ADSTAT; //Scan channels for over-run or done and update array @@ -112,7 +112,7 @@ void ADC::adcisr(void) if (stat & 0x2020) _adc_data[5] = LPC_ADC->ADDR5; if (stat & 0x4040) _adc_data[6] = LPC_ADC->ADDR6; if (stat & 0x8080) _adc_data[7] = LPC_ADC->ADDR7; - + // Channel that triggered interrupt chan = (LPC_ADC->ADGDR >> 24) & 0x07; //User defined interrupt handlers @@ -122,7 +122,7 @@ void ADC::adcisr(void) _adc_g_isr(chan, _adc_data[chan]); return; } - + int ADC::_pin_to_channel(PinName pin) { int chan; switch (pin) { @@ -148,25 +148,25 @@ int ADC::_pin_to_channel(PinName pin) { } return(chan); } - + PinName ADC::channel_to_pin(int chan) { const PinName pin[8]={p15, p16, p17, p18, p19, p20, p15, p15}; - + if ((chan < 0) || (chan > 5)) fprintf(stderr, "ADC channel %u is outside range available to MBED pins.\n", chan); return(pin[chan & 0x07]); } - - + + int ADC::channel_to_pin_number(int chan) { const int pin[8]={15, 16, 17, 18, 19, 20, 0, 0}; - + if ((chan < 0) || (chan > 5)) fprintf(stderr, "ADC channel %u is outside range available to MBED pins.\n", chan); return(pin[chan & 0x07]); } - - + + uint32_t ADC::_data_of_pin(PinName pin) { //If in burst mode and at least one interrupt enabled then //take all values from _adc_data @@ -191,7 +191,7 @@ uint32_t ADC::_data_of_pin(PinName pin) { } } } - + //Enable or disable an ADC pin void ADC::setup(PinName pin, int state) { int chan; @@ -275,27 +275,27 @@ void ADC::setup(PinName pin, int state) { //Return channel enabled/disabled state int ADC::setup(PinName pin) { int chan; - + chan = _pin_to_channel(pin); return((LPC_ADC->ADCR & (1 << chan)) >> chan); } - + //Select channel already setup void ADC::select(PinName pin) { int chan; - + //Only one channel can be selected at a time if not in burst mode if (!burst()) LPC_ADC->ADCR &= ~0xFF; //Select channel chan = _pin_to_channel(pin); LPC_ADC->ADCR |= (1 << chan); } - + //Enable or disable burst mode void ADC::burst(int state) { if ((state & 1) == 1) { if (startmode(0) != 0) - fprintf(stderr, "Warning. startmode is %u. Must be 0 for burst mode.\n", startmode(0)); + fprintf(stderr, "ADC Warning. startmode is %u. Must be 0 for burst mode.\n", startmode(0)); LPC_ADC->ADCR |= (1 << 16); } else @@ -305,18 +305,18 @@ void ADC::burst(int state) { int ADC::burst(void) { return((LPC_ADC->ADCR & (1 << 16)) >> 16); } - + //Set startmode and edge void ADC::startmode(int mode, int edge) { int lpc_adc_temp; - + //Reset start mode and edge bit, lpc_adc_temp = LPC_ADC->ADCR & ~(0x0F << 24); //Write with new values lpc_adc_temp |= ((mode & 7) << 24) | ((edge & 1) << 27); LPC_ADC->ADCR = lpc_adc_temp; } - + //Return startmode state according to mode_edge=0: mode and mode_edge=1: edge int ADC::startmode(int mode_edge){ switch (mode_edge) { @@ -327,17 +327,17 @@ int ADC::startmode(int mode_edge){ return((LPC_ADC->ADCR >> 27) & 0x01); } } - + //Start ADC conversion void ADC::start(void) { startmode(1,0); } - - + + //Set interrupt enable/disable for pin to state void ADC::interrupt_state(PinName pin, int state) { int chan; - + chan = _pin_to_channel(pin); if (state == 1) { LPC_ADC->ADINTEN &= ~0x100; @@ -351,67 +351,67 @@ void ADC::interrupt_state(PinName pin, int state) { NVIC_DisableIRQ(ADC_IRQn); } } - + //Return enable/disable state of interrupt for pin int ADC::interrupt_state(PinName pin) { int chan; - + chan = _pin_to_channel(pin); return((LPC_ADC->ADINTEN >> chan) & 0x01); } - - + + //Attach custom interrupt handler replacing default void ADC::attach(void(*fptr)(void)) { //* Attach IRQ NVIC_SetVector(ADC_IRQn, (uint32_t)fptr); } - + //Restore default interrupt handler void ADC::detach(void) { //* Attach IRQ instance = this; NVIC_SetVector(ADC_IRQn, (uint32_t)&_adcisr); } - - + + //Append interrupt handler for pin to function isr void ADC::append(PinName pin, void(*fptr)(uint32_t value)) { int chan; - + chan = _pin_to_channel(pin); _adc_isr[chan] = fptr; } - + //Append interrupt handler for pin to function isr void ADC::unappend(PinName pin) { int chan; - + chan = _pin_to_channel(pin); _adc_isr[chan] = NULL; } - + //Unappend global interrupt handler to function isr void ADC::append(void(*fptr)(int chan, uint32_t value)) { _adc_g_isr = fptr; } - + //Detach global interrupt handler to function isr void ADC::unappend() { _adc_g_isr = NULL; } - + //Set ADC offset -void offset(int offset) { +void ADC::offset(int offset) { LPC_ADC->ADTRM &= ~(0x07 << 4); LPC_ADC->ADTRM |= (offset & 0x07) << 4; } - + //Return current ADC offset -int offset(void) { +int ADC::offset(void) { return((LPC_ADC->ADTRM >> 4) & 0x07); } - + //Return value of ADC on pin int ADC::read(PinName pin) { //Reset DONE and OVERRUN flags of interrupt handled ADC data @@ -419,21 +419,21 @@ int ADC::read(PinName pin) { //Return value return((_data_of_pin(pin) >> 4) & 0xFFF); } - + //Return DONE flag of ADC on pin int ADC::done(PinName pin) { return((_data_of_pin(pin) >> 31) & 0x01); } - + //Return OVERRUN flag of ADC on pin int ADC::overrun(PinName pin) { return((_data_of_pin(pin) >> 30) & 0x01); } - + int ADC::actual_adc_clock(void) { return(_adc_clk_freq); } - + int ADC::actual_sample_rate(void) { return(_adc_clk_freq / CLKS_PER_SAMPLE); } diff --git a/src/libs/ADC/adc.h b/src/libs/ADC/adc.h index 9ff7248e..fbc7255b 100644 --- a/src/libs/ADC/adc.h +++ b/src/libs/ADC/adc.h @@ -2,39 +2,40 @@ * Copyright (c) 2010, sblandford * released under MIT license http://mbed.org/licence/mit */ - + #ifndef MBED_ADC_H #define MBED_ADC_H - + #include "PinNames.h" // mbed.h lib #define XTAL_FREQ 12000000 #define MAX_ADC_CLOCK 13000000 #define CLKS_PER_SAMPLE 64 - + +namespace mbed { class ADC { public: - + //Initialize ADC with ADC maximum sample rate of //sample_rate and system clock divider of cclk_div //Maximum recommened sample rate is 184000 ADC(int sample_rate, int cclk_div); - + //Enable/disable ADC on pin according to state //and also select/de-select for next conversion void setup(PinName pin, int state); - + //Return enabled/disabled state of ADC on pin int setup(PinName pin); - + //Enable/disable burst mode according to state void burst(int state); - + //Select channel already setup void select(PinName pin); - + //Return burst mode enabled/disabled int burst(void); - + /*Set start condition and edge according to mode: 0 - No start (this value should be used when clearing PDN to 0). 1 - Start conversion now. @@ -54,78 +55,79 @@ public: 1 - Falling edge */ void startmode(int mode, int edge); - + //Return startmode state according to mode_edge=0: mode and mode_edge=1: edge int startmode(int mode_edge); - + //Start ADC conversion void start(void); - + //Set interrupt enable/disable for pin to state void interrupt_state(PinName pin, int state); - + //Return enable/disable state of interrupt for pin int interrupt_state(PinName pin); - + //Attach custom interrupt handler replacing default void attach(void(*fptr)(void)); - + //Restore default interrupt handler void detach(void); - + //Append custom interrupt handler for pin void append(PinName pin, void(*fptr)(uint32_t value)); - + //Unappend custom interrupt handler for pin void unappend(PinName pin); - + //Append custom global interrupt handler void append(void(*fptr)(int chan, uint32_t value)); - + //Unappend custom global interrupt handler void unappend(void); - + //Set ADC offset to a value 0-7 void offset(int offset); - + //Return current ADC offset int offset(void); - + //Return value of ADC on pin int read(PinName pin); - + //Return DONE flag of ADC on pin int done(PinName pin); - + //Return OVERRUN flag of ADC on pin int overrun(PinName pin); - + //Return actual ADC clock int actual_adc_clock(void); - + //Return actual maximum sample rate int actual_sample_rate(void); - + //Return pin ID of ADC channel PinName channel_to_pin(int chan); - + //Return pin number of ADC channel int channel_to_pin_number(int chan); - - -private: + int _pin_to_channel(PinName pin); + +private: uint32_t _data_of_pin(PinName pin); - + int _adc_clk_freq; void adcisr(void); static void _adcisr(void); static ADC *instance; - + uint32_t _adc_data[8]; void(*_adc_isr[8])(uint32_t value); void(*_adc_g_isr)(int chan, uint32_t value); void(*_adc_m_isr)(void); }; - +} + #endif diff --git a/src/libs/Adc.cpp b/src/libs/Adc.cpp index 568449ee..c85d38c7 100644 --- a/src/libs/Adc.cpp +++ b/src/libs/Adc.cpp @@ -11,45 +11,132 @@ #include "libs/Pin.h" #include "libs/ADC/adc.h" #include "libs/Pin.h" +#include "libs/Median.h" +#include +#include + +#include "mbed.h" -using namespace std; -#include // This is an interface to the mbed.org ADC library you can find in libs/ADC/adc.h // TODO : Having the same name is confusing, should change that -Adc::Adc(){ - this->adc = new ADC(1000, 1); +Adc *Adc::instance; + +static void sample_isr(int chan, uint32_t value) +{ + Adc::instance->new_sample(chan, value); } +Adc::Adc() +{ + instance = this; + // ADC sample rate need to be fast enough to be able to read the enabled channels within the thermistor poll time + // even though ther maybe 32 samples we only need one new one within the polling time + const uint32_t sample_rate= 1000; // 1KHz sample rate + this->adc = new mbed::ADC(sample_rate, 8); + this->adc->append(sample_isr); +} + +/* +LPC176x ADC channels and pins + +Adc Channel Port Pin Pin Functions Associated PINSEL Register +AD0 P0.23 0-GPIO, 1-AD0[0], 2-I2SRX_CLK, 3-CAP3[0] 14,15 bits of PINSEL1 +AD1 P0.24 0-GPIO, 1-AD0[1], 2-I2SRX_WS, 3-CAP3[1] 16,17 bits of PINSEL1 +AD2 P0.25 0-GPIO, 1-AD0[2], 2-I2SRX_SDA, 3-TXD3 18,19 bits of PINSEL1 +AD3 P0.26 0-GPIO, 1-AD0[3], 2-AOUT, 3-RXD3 20,21 bits of PINSEL1 +AD4 P1.30 0-GPIO, 1-VBUS, 2- , 3-AD0[4] 28,29 bits of PINSEL3 +AD5 P1.31 0-GPIO, 1-SCK1, 2- , 3-AD0[5] 30,31 bits of PINSEL3 +AD6 P0.3 0-GPIO, 1-RXD0, 2-AD0[6], 3- 6,7 bits of PINSEL0 +AD7 P0.2 0-GPIO, 1-TXD0, 2-AD0[7], 3- 4,5 bits of PINSEL0 +*/ + // Enables ADC on a given pin -void Adc::enable_pin(Pin* pin){ +void Adc::enable_pin(Pin *pin) +{ PinName pin_name = this->_pin_to_pinname(pin); + int channel = adc->_pin_to_channel(pin_name); + memset(sample_buffers[channel], 0, sizeof(sample_buffers[0])); + this->adc->burst(1); - this->adc->setup(pin_name,1); - this->adc->interrupt_state(pin_name,1); + this->adc->setup(pin_name, 1); + this->adc->interrupt_state(pin_name, 1); } -// Read the last value ( burst mode ) on a given pin -unsigned int Adc::read(Pin* pin){ - return this->adc->read(this->_pin_to_pinname(pin)); +// Keeps the last 8 values for each channel +// This is called in an ISR, so sample_buffers needs to be accessed atomically +void Adc::new_sample(int chan, uint32_t value) +{ + // Shuffle down and add new value to the end + if(chan < num_channels) { + memmove(&sample_buffers[chan][0], &sample_buffers[chan][1], sizeof(sample_buffers[0]) - sizeof(sample_buffers[0][0])); + sample_buffers[chan][num_samples - 1] = (value >> 4) & 0xFFF; // the 12 bit ADC reading + } +} + +//#define USE_MEDIAN_FILTER +// Read the filtered value ( burst mode ) on a given pin +unsigned int Adc::read(Pin *pin) +{ + PinName p = this->_pin_to_pinname(pin); + int channel = adc->_pin_to_channel(p); + + uint16_t median_buffer[num_samples]; + // needs atomic access TODO maybe be able to use std::atomic here or some lockless mutex + __disable_irq(); + memcpy(median_buffer, sample_buffers[channel], sizeof(median_buffer)); + __enable_irq(); + +#ifdef USE_MEDIAN_FILTER + // returns the median value of the last 8 samples + return median_buffer[quick_median(median_buffer, num_samples)]; + +#elif defined(OVERSAMPLE) + // Oversample to get 2 extra bits of resolution + // weed out top and bottom worst values then oversample the rest + // put into a 4 element moving average and return the average of the last 4 oversampled readings + static uint16_t ave_buf[num_channels][4] = { {0} }; + std::sort(median_buffer, median_buffer + num_samples); + uint32_t sum = 0; + for (int i = num_samples / 4; i < (num_samples - (num_samples / 4)); ++i) { + sum += median_buffer[i]; + } + // this slows down the rate of change a little bit + ave_buf[channel][3]= ave_buf[channel][2]; + ave_buf[channel][2]= ave_buf[channel][1]; + ave_buf[channel][1]= ave_buf[channel][0]; + ave_buf[channel][0]= sum >> OVERSAMPLE; + return roundf((ave_buf[channel][0]+ave_buf[channel][1]+ave_buf[channel][2]+ave_buf[channel][3])/4.0F); + +#else + // sort the 8 readings and return the average of the middle 4 + std::sort(median_buffer, median_buffer + num_samples); + int sum = 0; + for (int i = num_samples / 4; i < (num_samples - (num_samples / 4)); ++i) { + sum += median_buffer[i]; + } + return sum / (num_samples / 2); + +#endif } // Convert a smoothie Pin into a mBed Pin -PinName Adc::_pin_to_pinname(Pin* pin){ - if( pin->port == LPC_GPIO0 && pin->pin == 23 ){ +PinName Adc::_pin_to_pinname(Pin *pin) +{ + if( pin->port == LPC_GPIO0 && pin->pin == 23 ) { return p15; - }else if( pin->port == LPC_GPIO0 && pin->pin == 24 ){ + } else if( pin->port == LPC_GPIO0 && pin->pin == 24 ) { return p16; - }else if( pin->port == LPC_GPIO0 && pin->pin == 25 ){ + } else if( pin->port == LPC_GPIO0 && pin->pin == 25 ) { return p17; - }else if( pin->port == LPC_GPIO0 && pin->pin == 26 ){ + } else if( pin->port == LPC_GPIO0 && pin->pin == 26 ) { return p18; - }else if( pin->port == LPC_GPIO1 && pin->pin == 30 ){ + } else if( pin->port == LPC_GPIO1 && pin->pin == 30 ) { return p19; - }else if( pin->port == LPC_GPIO1 && pin->pin == 31 ){ + } else if( pin->port == LPC_GPIO1 && pin->pin == 31 ) { return p20; - }else{ + } else { //TODO: Error return NC; } diff --git a/src/libs/Adc.h b/src/libs/Adc.h index 41cba86e..163d0a85 100644 --- a/src/libs/Adc.h +++ b/src/libs/Adc.h @@ -10,22 +10,48 @@ #ifndef ADC_H #define ADC_H -#include "libs/Module.h" #include "PinNames.h" // mbed.h lib -#include "libs/ADC/adc.h" + +#include class Pin; +namespace mbed { + class ADC; +} + +// define how many bits of extra resolution required +// 2 bits means the 12bit ADC is 14 bits of resolution +#define OVERSAMPLE 2 + +class Adc +{ +public: + Adc(); + void enable_pin(Pin *pin); + unsigned int read(Pin *pin); + + static Adc *instance; + void new_sample(int chan, uint32_t value); + // return the maximum ADC value, base is 12bits 4095. +#ifdef OVERSAMPLE + int get_max_value() const { return 4095 << OVERSAMPLE;} +#else + int get_max_value() const { return 4095;} +#endif -class Adc : public Module{ - public: - Adc(); - void enable_pin(Pin* pin); - unsigned int read(Pin* pin); - PinName _pin_to_pinname(Pin* pin); +private: + PinName _pin_to_pinname(Pin *pin); + mbed::ADC *adc; - ADC* adc; + static const int num_channels= 6; +#ifdef OVERSAMPLE + // we need 4^n sample to oversample and we get double that to filter out spikes + static const int num_samples= powf(4, OVERSAMPLE)*2; +#else + static const int num_samples= 8; +#endif + // buffers storing the last num_samples readings for each channel + uint16_t sample_buffers[num_channels][num_samples]; }; - - #endif diff --git a/src/libs/AppendFileStream.cpp b/src/libs/AppendFileStream.cpp index 6e138b7b..48afcbc6 100644 --- a/src/libs/AppendFileStream.cpp +++ b/src/libs/AppendFileStream.cpp @@ -1,5 +1,5 @@ #include "AppendFileStream.h" -#include "stdio.h" +#include int AppendFileStream::puts(const char *str) { diff --git a/src/libs/Config.cpp b/src/libs/Config.cpp index b4e11b7b..af14f8bc 100644 --- a/src/libs/Config.cpp +++ b/src/libs/Config.cpp @@ -48,6 +48,20 @@ Config::Config() this->config_sources.push_back( fcs ); } +Config::Config(ConfigSource *cs) +{ + this->config_cache = NULL; + this->config_sources.push_back( cs ); +} + +Config::~Config() +{ + config_cache_clear(); + for(auto i : this->config_sources) { + delete i; + } +} + void Config::on_module_loaded() {} void Config::on_console_line_received( void *argument ) {} diff --git a/src/libs/Config.h b/src/libs/Config.h index ac199739..d7bc7cc6 100644 --- a/src/libs/Config.h +++ b/src/libs/Config.h @@ -20,6 +20,8 @@ class ConfigCache; class Config : public Module { public: Config(); + Config(ConfigSource*); + ~Config(); void on_module_loaded(); void on_console_line_received( void* argument ); diff --git a/src/libs/ConfigSource.h b/src/libs/ConfigSource.h index b5c86844..2ac2d026 100644 --- a/src/libs/ConfigSource.h +++ b/src/libs/ConfigSource.h @@ -18,6 +18,7 @@ class ConfigCache; class ConfigSource { public: ConfigSource(){} + virtual ~ConfigSource(){} // Read each value, and append it as a ConfigValue to the config_cache we were passed virtual void transfer_values_to_cache( ConfigCache* ) = 0; diff --git a/src/libs/ConfigSources/FirmConfigSource.cpp b/src/libs/ConfigSources/FirmConfigSource.cpp index aa9c13ae..d5c362ff 100644 --- a/src/libs/ConfigSources/FirmConfigSource.cpp +++ b/src/libs/ConfigSources/FirmConfigSource.cpp @@ -20,20 +20,27 @@ using namespace std; extern char _binary_config_default_start; extern char _binary_config_default_end; - FirmConfigSource::FirmConfigSource(const char* name){ this->name_checksum = get_checksum(name); + this->start= &_binary_config_default_start; + this->end= &_binary_config_default_end; +} + +FirmConfigSource::FirmConfigSource(const char* name, const char *start, const char *end){ + this->name_checksum = get_checksum(name); + this->start= start; + this->end= end; } // Transfer all values found in the file to the passed cache void FirmConfigSource::transfer_values_to_cache( ConfigCache* cache ){ - char* p = &_binary_config_default_start; + const char* p = this->start; // For each line - while( p < &_binary_config_default_end ){ + while( p < this->end ){ // find eol - char *eol= p; - while(eol < &_binary_config_default_end) { + const char *eol= p; + while(eol < this->end) { if(*eol++ == '\n') break; } string line(p, eol-p); @@ -59,12 +66,12 @@ string FirmConfigSource::read( uint16_t check_sums[3] ){ string value = ""; - char* p = &_binary_config_default_start; + const char* p = this->start; // For each line - while( p < &_binary_config_default_end ){ + while( p < this->end ){ // find eol - char *eol= p; - while(eol < &_binary_config_default_end) { + const char *eol= p; + while(eol < this->end) { if(*eol++ == '\n') break; } string line(p, eol-p); diff --git a/src/libs/ConfigSources/FirmConfigSource.h b/src/libs/ConfigSources/FirmConfigSource.h index f662e37a..ea7e810d 100644 --- a/src/libs/ConfigSources/FirmConfigSource.h +++ b/src/libs/ConfigSources/FirmConfigSource.h @@ -16,14 +16,19 @@ class ConfigCache; using namespace std; #include -class FirmConfigSource : public ConfigSource { - public: - FirmConfigSource(const char* name); - void transfer_values_to_cache( ConfigCache* cache ); - bool is_named( uint16_t check_sum ); - bool write( string setting, string value ); - string read( uint16_t check_sums[3] ); - +class FirmConfigSource : public ConfigSource +{ +public: + FirmConfigSource(const char *name); + FirmConfigSource(const char* name, const char *start, const char *end); + + void transfer_values_to_cache( ConfigCache *cache ); + bool is_named( uint16_t check_sum ); + bool write( string setting, string value ); + string read( uint16_t check_sums[3] ); + +private: + const char *start, *end; }; diff --git a/src/libs/FileStream.h b/src/libs/FileStream.h index 10078ec8..e285cf04 100644 --- a/src/libs/FileStream.h +++ b/src/libs/FileStream.h @@ -2,7 +2,8 @@ #define _FILESTREAM_H_ #include "StreamOutput.h" -#include "stdlib.h" +#include +#include class FileStream : public StreamOutput { public: diff --git a/src/libs/Kernel.cpp b/src/libs/Kernel.cpp index f3a73836..036740ff 100644 --- a/src/libs/Kernel.cpp +++ b/src/libs/Kernel.cpp @@ -24,21 +24,26 @@ #include "modules/robot/Robot.h" #include "modules/robot/Stepper.h" #include "modules/robot/Conveyor.h" -#include "modules/robot/Pauser.h" +#include "StepperMotor.h" #include #include +#include #define baud_rate_setting_checksum CHECKSUM("baud_rate") #define uart0_checksum CHECKSUM("uart0") #define base_stepping_frequency_checksum CHECKSUM("base_stepping_frequency") #define microseconds_per_step_pulse_checksum CHECKSUM("microseconds_per_step_pulse") +#define acceleration_ticks_per_second_checksum CHECKSUM("acceleration_ticks_per_second") +#define disable_leds_checksum CHECKSUM("leds_disable") Kernel* Kernel::instance; // The kernel is the central point in Smoothie : it stores modules, and handles event calls Kernel::Kernel(){ + halted= false; + instance= this; // setup the Singleton instance of the kernel // serial first at fixed baud rate (DEFAULT_SERIAL_BAUD_RATE) so config can report errors to serial @@ -46,7 +51,7 @@ Kernel::Kernel(){ this->serial = new SerialConsole(USBTX, USBRX, DEFAULT_SERIAL_BAUD_RATE); // Config next, but does not load cache yet - this->config = new Config(); + this->config = new Config(); // Pre-load the config cache, do after setting up serial so we can report errors to serial this->config->config_cache_load(); @@ -55,7 +60,7 @@ Kernel::Kernel(){ delete this->serial; this->serial= NULL; - this->streams = new StreamOutputPool(); + this->streams = new StreamOutputPool(); this->current_path = "/"; @@ -84,58 +89,90 @@ Kernel::Kernel(){ 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(); + this->add_module( this->config ); this->add_module( this->serial ); // HAL stuff - add_module( this->slow_ticker = new SlowTicker()); - this->step_ticker = new StepTicker(); - this->adc = new Adc(); + add_module( this->slow_ticker = new SlowTicker()); + + this->step_ticker = new StepTicker(); + this->adc = new Adc(); // TODO : These should go into platform-specific files // LPC17xx-specific NVIC_SetPriorityGrouping(0); NVIC_SetPriority(TIMER0_IRQn, 2); NVIC_SetPriority(TIMER1_IRQn, 1); - NVIC_SetPriority(TIMER2_IRQn, 3); + NVIC_SetPriority(TIMER2_IRQn, 4); + NVIC_SetPriority(PendSV_IRQn, 3); + NVIC_SetPriority(RIT_IRQn, 3); // we make acceleration tick the same prio as pendsv so it can't be pre-empted by end of block // Set other priorities lower than the timers - NVIC_SetPriority(ADC_IRQn, 4); - NVIC_SetPriority(USB_IRQn, 4); + NVIC_SetPriority(ADC_IRQn, 5); + NVIC_SetPriority(USB_IRQn, 5); // If MRI is enabled if( MRI_ENABLE ){ - if( NVIC_GetPriority(UART0_IRQn) > 0 ){ NVIC_SetPriority(UART0_IRQn, 4); } - if( NVIC_GetPriority(UART1_IRQn) > 0 ){ NVIC_SetPriority(UART1_IRQn, 4); } - if( NVIC_GetPriority(UART2_IRQn) > 0 ){ NVIC_SetPriority(UART2_IRQn, 4); } - if( NVIC_GetPriority(UART3_IRQn) > 0 ){ NVIC_SetPriority(UART3_IRQn, 4); } + 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, 4); - NVIC_SetPriority(UART1_IRQn, 4); - NVIC_SetPriority(UART2_IRQn, 4); - NVIC_SetPriority(UART3_IRQn, 4); + 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->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(); + 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(); + this->acceleration_ticks_per_second = THEKERNEL->config->value(acceleration_ticks_per_second_checksum)->by_default(1000)->as_number(); // Configure the step ticker ( TODO : shouldnt this go into stepticker's code ? ) - this->step_ticker->set_reset_delay( microseconds_per_step_pulse / 1000000L ); + this->step_ticker->set_reset_delay( microseconds_per_step_pulse ); this->step_ticker->set_frequency( this->base_stepping_frequency ); + this->step_ticker->set_acceleration_ticks_per_second(acceleration_ticks_per_second); // must be set after set_frequency // Core modules this->add_module( new GcodeDispatch() ); this->add_module( this->robot = new Robot() ); this->add_module( this->stepper = new Stepper() ); this->add_module( this->conveyor = new Conveyor() ); - this->add_module( this->pauser = new Pauser() ); this->planner = new Planner(); } -// Add a module to Kernel. We don't actually hold a list of modules, we just tell it where Kernel is +// return a GRBL-like query string for serial ? +std::string Kernel::get_query_string() +{ + std::string str; + str.append("<"); + if(halted) { + str.append("Alarm,"); + }else if(this->conveyor->is_queue_empty()) { + str.append("Idle,"); + }else{ + str.append("Run,"); + } + + char buf[64]; + size_t n= snprintf(buf, sizeof(buf), "%f,%f,%f,", this->robot->actuators[X_AXIS]->get_current_position(), this->robot->actuators[Y_AXIS]->get_current_position(), this->robot->actuators[Z_AXIS]->get_current_position()); + str.append("MPos:").append(buf, n); + + float pos[3]; + this->robot->get_axis_position(pos); + n= snprintf(buf, sizeof(buf), "%f,%f,%f", pos[0], pos[1], pos[2]); + str.append("WPos:").append(buf, n); + str.append(">\r\n"); + return str; +} + +// Add a module to Kernel. We don't actually hold a list of modules we just call its on_module_loaded void Kernel::add_module(Module* module){ module->on_module_loaded(); } @@ -145,16 +182,32 @@ void Kernel::register_for_event(_EVENT_ENUM id_event, Module *mod){ this->hooks[id_event].push_back(mod); } -// Call a specific event without arguments -void Kernel::call_event(_EVENT_ENUM id_event){ +// Call a specific event with an argument +void Kernel::call_event(_EVENT_ENUM id_event, void * argument){ + if(id_event == ON_HALT) { + this->halted= (argument == nullptr); + } for (auto m : hooks[id_event]) { - (m->*kernel_callback_functions[id_event])(this); + (m->*kernel_callback_functions[id_event])(argument); } } -// Call a specific event with an argument -void Kernel::call_event(_EVENT_ENUM id_event, void * argument){ +// These are used by tests to test for various things. basically mocks +bool Kernel::kernel_has_event(_EVENT_ENUM id_event, Module *mod) +{ for (auto m : hooks[id_event]) { - (m->*kernel_callback_functions[id_event])(argument); + if(m == mod) return true; } + return false; } + +void Kernel::unregister_for_event(_EVENT_ENUM id_event, Module *mod) +{ + for (auto i = hooks[id_event].begin(); i != hooks[id_event].end(); ++i) { + if(*i == mod) { + hooks[id_event].erase(i); + return; + } + } +} + diff --git a/src/libs/Kernel.h b/src/libs/Kernel.h index 790b3841..30599cf4 100644 --- a/src/libs/Kernel.h +++ b/src/libs/Kernel.h @@ -20,7 +20,6 @@ class Config; class Module; class Conveyor; class SlowTicker; -class Pauser; class SerialConsole; class StreamOutputPool; class GcodeDispatch; @@ -39,8 +38,14 @@ class Kernel { void add_module(Module* module); void register_for_event(_EVENT_ENUM id_event, Module *module); - void call_event(_EVENT_ENUM id_event); - void call_event(_EVENT_ENUM id_event, void * argument); + void call_event(_EVENT_ENUM id_event, void * argument= nullptr); + + bool kernel_has_event(_EVENT_ENUM id_event, Module *mod); + void unregister_for_event(_EVENT_ENUM id_event, Module *module); + + bool is_using_leds() const { return use_leds; } + bool is_halted() const { return halted; } + std::string get_query_string(); // These modules are available to all other modules SerialConsole* serial; @@ -51,19 +56,22 @@ class Kernel { Planner* planner; Config* config; Conveyor* conveyor; - Pauser* pauser; int debug; SlowTicker* slow_ticker; StepTicker* step_ticker; Adc* adc; - bool use_leds; std::string current_path; - int base_stepping_frequency; + 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 std::array, NUMBER_OF_DEFINED_EVENTS> hooks; + struct { + bool use_leds:1; + bool halted:1; + }; }; diff --git a/src/libs/Module.cpp b/src/libs/Module.cpp index 50577a6a..1ab52123 100644 --- a/src/libs/Module.cpp +++ b/src/libs/Module.cpp @@ -21,13 +21,12 @@ const ModuleCallback kernel_callback_functions[NUMBER_OF_DEFINED_EVENTS] = { &Module::on_speed_change, &Module::on_block_begin, &Module::on_block_end, - &Module::on_play, - &Module::on_pause, &Module::on_idle, &Module::on_second_tick, &Module::on_get_public_data, &Module::on_set_public_data, - &Module::on_halt + &Module::on_halt, + &Module::on_enable }; diff --git a/src/libs/Module.h b/src/libs/Module.h index 84da421a..1c38a5e7 100644 --- a/src/libs/Module.h +++ b/src/libs/Module.h @@ -19,13 +19,12 @@ enum _EVENT_ENUM { ON_SPEED_CHANGE, ON_BLOCK_BEGIN, ON_BLOCK_END, - ON_PLAY, - ON_PAUSE, ON_IDLE, ON_SECOND_TICK, ON_GET_PUBLIC_DATA, ON_SET_PUBLIC_DATA, ON_HALT, + ON_ENABLE, NUMBER_OF_DEFINED_EVENTS }; @@ -53,13 +52,12 @@ public: virtual void on_speed_change(void *) {}; virtual void on_block_begin(void *) {}; virtual void on_block_end(void *) {}; - virtual void on_play(void *) {}; - virtual void on_pause(void *) {}; virtual void on_idle(void *) {}; virtual void on_second_tick(void *) {}; virtual void on_get_public_data(void *) {}; virtual void on_set_public_data(void *) {}; virtual void on_halt(void *) {}; + virtual void on_enable(void *) {}; }; diff --git a/src/libs/Network/uip/Network.cpp b/src/libs/Network/uip/Network.cpp index 1fe36e15..ad6e40e8 100644 --- a/src/libs/Network/uip/Network.cpp +++ b/src/libs/Network/uip/Network.cpp @@ -23,7 +23,7 @@ #include "webserver.h" #include "dhcpc.h" #include "sftpd.h" - +#include "plan9.h" #include @@ -32,6 +32,7 @@ #define network_enable_checksum CHECKSUM("enable") #define network_webserver_checksum CHECKSUM("webserver") #define network_telnet_checksum CHECKSUM("telnet") +#define network_plan9_checksum CHECKSUM("plan9") #define network_mac_override_checksum CHECKSUM("mac_override") #define network_ip_address_checksum CHECKSUM("ip_address") #define network_hostname_checksum CHECKSUM("hostname") @@ -43,7 +44,7 @@ extern "C" void uip_log(char *m) printf("uIP log message: %s\n", m); } -static bool webserver_enabled, telnet_enabled, use_dhcp; +static bool webserver_enabled, telnet_enabled, plan9_enabled, use_dhcp; static Network *theNetwork; static Sftpd *sftpd; static CommandQueue *command_q= CommandQueue::getInstance(); @@ -129,6 +130,7 @@ void Network::on_module_loaded() webserver_enabled = THEKERNEL->config->value( network_checksum, network_webserver_checksum, network_enable_checksum )->by_default(false)->as_bool(); telnet_enabled = THEKERNEL->config->value( network_checksum, network_telnet_checksum, network_enable_checksum )->by_default(false)->as_bool(); + plan9_enabled = THEKERNEL->config->value( network_checksum, network_plan9_checksum, network_enable_checksum )->by_default(false)->as_bool(); string mac = THEKERNEL->config->value( network_checksum, network_mac_override_checksum )->by_default("")->as_string(); if (mac.size() == 17 ) { // parse mac address @@ -304,6 +306,12 @@ static void setup_servers() printf("Telnetd initialized\n"); } + if (plan9_enabled) { + // Initialize the plan9 server + Plan9::init(); + printf("Plan9 initialized\n"); + } + // sftpd service, which is lazily created on reciept of first packet uip_listen(HTONS(115)); } @@ -395,6 +403,10 @@ extern "C" void app_select_appcall(void) if (telnet_enabled) Telnetd::appcall(); break; + case HTONS(564): + if (plan9_enabled) Plan9::appcall(); + break; + case HTONS(115): if(sftpd == NULL) { sftpd= new Sftpd(); diff --git a/src/libs/Network/uip/plan9/plan9.cpp b/src/libs/Network/uip/plan9/plan9.cpp new file mode 100644 index 00000000..12e849fc --- /dev/null +++ b/src/libs/Network/uip/plan9/plan9.cpp @@ -0,0 +1,760 @@ +/* + * 9P network filesystem protocol implementation + * + * by Daniel Mendler + */ + +#include "plan9.h" +#include "DirHandle.h" +#include "FATFileSystem.h" +#include "StreamOutputPool.h" +#include "Kernel.h" +#include "utils.h" +#include "uip.h" + +//#define DEBUG_PRINTF(...) printf("9p " __VA_ARGS__) +#define DEBUG_PRINTF(...) + +#define ERROR(...) do { error(bufout, msize, __LINE__, ##__VA_ARGS__); return 0; } while (0) +#define CHECK(cond, ...) do { if (!(cond)) ERROR(__VA_ARGS__); } while (0) +#define IOUNIT (msize - sizeof (Message::Twrite)) +#define PACKEDSTRUCT struct __attribute__ ((packed)) +#define RESPONSE(t) response->size = sizeof (response->t); response->type = request->type+1; response->tag = request->tag + +namespace { + +// See error mapping http://lxr.free-electrons.com/source/net/9p/error.c +const char ENOENT[] = "No such file or directory", + EIO[] = "Input/output error", + FID_UNKNOWN[] = "fid unknown or out of range", + FID_IN_USE[] = "fid already in use", + EBADMSG[] = "Bad message", + EEXIST[] = "File exists", + EFAULT[] = "Bad address", + ENOSYS[] = "Function not implemented", + ENOTEMPTY[] = "Directory not empty", + ENFILE[] = "Too many open files"; + +enum { + // 9P message types + Tversion = 100, + Tauth = 102, + Tattach = 104, + Rerror = 107, + Tflush = 108, + Twalk = 110, + Topen = 112, + Tcreate = 114, + Tread = 116, + Twrite = 118, + Tclunk = 120, + Tremove = 122, + Tstat = 124, + Twstat = 126, + + // Qid type + QTDIR = 0x80, // directories + QTAPPEND = 0x40, // append only files + QTEXCL = 0x20, // exclusive use files + QTMOUNT = 0x10, // mounted channel + QTAUTH = 0x08, // authentication file + QTTMP = 0x04, // non-backed-up file + QTLINK = 0x02, // symbolic link + QTFILE = 0x00, // plain file + + // mode + OREAD = 0, // open for read + OWRITE = 1, // write + ORDWR = 2, // read and write + OEXEC = 3, // execute, == read but check execute permission + OTRUNC = 0x10, // or'ed in (except for exec), truncate file first + ORCLOSE = 0x40, // or'ed in, remove on close + + // permission bits + DMDIR = 0x80000000, // directories + DMAPPEND = 0x40000000, // append only files + DMEXCL = 0x20000000, // exclusive use files + DMMOUNT = 0x10000000, // mounted channel + DMAUTH = 0x08000000, // authentication file + DMTMP = 0x04000000, // non-backed-up file + + MAXWELEM = 16, + MAXENTRIES = 32, + MAXFIDS = 32, + MAXREQUESTS = 4, +}; + +// TODO: Maybe this should be moved to utils? +class File { + FILE* fp; +public: + File(const std::string& path, const char* mode) + : fp(fopen(path.c_str(), mode)) {} + + ~File() + { + if (fp) + fclose(fp); + } + + operator FILE*() { return fp; } +}; + +// TODO: Maybe this should be moved to utils? +class Dir { + DIR* dir; +public: + Dir(const std::string& path) + : dir(opendir(path.c_str())) {} + + ~Dir() + { + if (dir) + closedir(dir); + } + + operator DIR*() { return dir; } +}; + +PACKEDSTRUCT Header { + uint32_t size; + uint8_t type; + uint16_t tag; +}; + +// TODO: move to utils? +uint64_t fletcher64(const std::string& s) +{ + uint32_t lo = 0, hi = 0; + for (const char* p = s.c_str(); p < s.c_str() + s.size();) { + uint32_t v = 0; + for (int i = 0; i < 4 && p < s.c_str() + s.size(); ++i) + v = (v << 8) | *p++; + lo += v; + hi += lo; + } + return (uint64_t(hi) << 32) | lo; +} + +PACKEDSTRUCT Qid { + uint8_t type; + uint32_t vers; // we don't use the version field + uint64_t path; + + Qid() {} + Qid(uint8_t t, const std::string& p) + : type(t), vers(0), path(fletcher64(p)) {} + Qid(Plan9::Entry e) + : type(e->second.type), vers(0), path(fletcher64(e->first)) {} +}; + +PACKEDSTRUCT Stat { + char buf[0]; + uint16_t size; + uint16_t type; + uint32_t dev; + Qid qid; + uint32_t mode; + uint32_t atime; + uint32_t mtime; + uint64_t length; +}; + +} // anonymous namespace + +// Important: 9P assumes little endian byte ordering! +union __attribute__ ((packed)) Plan9::Message { + char buf[0]; + + PACKEDSTRUCT { + uint32_t size; + uint8_t type; + uint16_t tag; + uint32_t fid; + }; + + // size[4] Tversion tag[2] msize[4] version[s] + // size[4] Rversion tag[2] msize[4] version[s] + PACKEDSTRUCT { + Header _header; + uint32_t msize; + } Tversion, Rversion; + + // // size[4] Tauth tag[2] afid[4] uname[s] aname[s] + // PACKEDSTRUCT { + // Header _header; + // uint32_t afid; + // } Tauth; + + // // size[4] Rauth tag[2] aqid[13] + // PACKEDSTRUCT { + // Header _header; + // Qid aqid; + // } Rauth; + + // size[4] Rerror tag[2] ename[s] + // size[4] Tclunk tag[2] fid[4] + // size[4] Tremove tag[2] fid[4] + // size[4] Tstat tag[2] fid[4] + // size[4] Tattach tag[2] fid[4] afid[4] uname[s] aname[s] + + // size[4] Rclunk tag[2] + // size[4] Rflush tag[2] + // size[4] Rremove tag[2] + // size[4] Rwstat tag[2] + Header Rclunk, Rflush, Rremove, Rwstat; + + // size[4] Rattach tag[2] qid[13] + PACKEDSTRUCT { + Header _header; + Qid qid; + } Rattach; + + // size[4] Tflush tag[2] oldtag[2] + PACKEDSTRUCT { + Header _header; + uint16_t oldtag; + } Tflush; + + // size[4] Twalk tag[2] fid[4] newfid[4] nwname[2] nwname*(wname[s]) + PACKEDSTRUCT { + Header _header; + uint32_t fid; + uint32_t newfid; + uint16_t nwname; + char wname[0]; + } Twalk; + + // size[4] Rwalk tag[2] nwqid[2] nwqid*(wqid[13]) + PACKEDSTRUCT { + Header _header; + uint16_t nwqid; + Qid wqid[0]; + } Rwalk; + + // size[4] Topen tag[2] fid[4] mode[1] + PACKEDSTRUCT { + Header _header; + uint32_t fid; + uint8_t mode; + } Topen; + + // size[4] Ropen tag[2] qid[13] iounit[4] + // size[4] Rcreate tag[2] qid[13] iounit[4] + PACKEDSTRUCT { + Header _header; + Qid qid; + uint32_t iounit; + } Ropen, Rcreate; + + // size[4] Tcreate tag[2] fid[4] name[s] perm[4] mode[1] + PACKEDSTRUCT { + Header _header; + uint32_t fid; + uint16_t name_size; + char name[0]; + } Tcreate; + + // size[4] Tread tag[2] fid[4] offset[8] count[4] + PACKEDSTRUCT { + Header _header; + uint32_t fid; + uint64_t offset; + uint32_t count; + } Tread; + + // size[4] Rread tag[2] count[4] data[count] + // size[4] Rwrite tag[2] count[4] + PACKEDSTRUCT { + Header _header; + uint32_t count; + } Rread, Rwrite; + + // size[4] Twrite tag[2] fid[4] offset[8] count[4] data[count] + PACKEDSTRUCT { + Header _header; + uint32_t fid; + uint64_t offset; + uint32_t count; + } Twrite; + + // size[4] Rstat tag[2] stat[n] + PACKEDSTRUCT { + Header _header; + uint16_t stat_size; + Stat stat; + } Rstat; + + // size[4] Twstat tag[2] fid[4] stat[n] + PACKEDSTRUCT { + Header _header; + uint32_t fid; + uint16_t stat_size; + Stat stat; + } Twstat; +}; + +namespace { + +char* putstr(char* p, char* end, const char* s) +{ + auto n = strlen(s); + if (!p || p + 2 + n > end) + return nullptr; + *p++ = n & 0xFF; + *p++ = (n >> 8) & 0xFF; + memcpy(p, s, n); + return p + n; +} + +inline long flen(const std::string& path) +{ + File fp(path, "r"); + if (!fp) + return 0; + fseek(fp, 0, SEEK_END); + return max(0l, ftell(fp)); +} + +size_t putstat(Stat* stat, char* end, uint8_t type, const std::string& path) +{ + char* p = stat->buf + sizeof (Stat); + if (p > end) + return 0; + + stat->type = 0; + stat->dev = 0; + stat->qid = Qid(type, path); + stat->mode = type == QTDIR ? (DMDIR | 0755) : 0644; + stat->atime = stat->mtime = 1423420000; + stat->length = (stat->mode & DMDIR) ? 0 : flen(path); + + p = putstr(p, end, path == "/" ? "/" : path.substr(path.rfind('/') + 1).c_str()); + p = putstr(p, end, "smoothie"); + p = putstr(p, end, "smoothie"); + p = putstr(p, end, "smoothie"); + if (!p) return 0; + + stat->size = p - stat->buf - 2; + return p - stat->buf; +} + +inline void error(char* bufout, uint32_t msize, int line) {} +void error(char* bufout, uint32_t msize, int line, const char* text) +{ + DEBUG_PRINTF("error %s, line %d\n", text, __LINE__); + Plan9::Message* response = reinterpret_cast(bufout); + response->type = Rerror; + response->size = sizeof (Header); + response->size = putstr(response->buf + sizeof (Header), response->buf + msize, text) - response->buf; +} + +// TODO: Move to utils +inline std::string absolute_path(const std::string& path) +{ + // TODO: remove /../ and /./ from paths + return path; +} + +// TODO: Move to utils +std::string join_path(const std::string& a, const std::string& b) +{ + return a.back() != '/' ? absolute_path(a + "/" + b) : + absolute_path(a + b); +} + +} // anonymous namespace + +Plan9::Plan9() +: msize(INITIAL_MSIZE), queue_bytes(0) +{ + PSOCK_INIT(&sin, bufin + 4, sizeof(bufin) - 4); + PSOCK_INIT(&sout, bufout + 4, sizeof(bufout) - 4); +} + +Plan9::~Plan9() +{ + PSOCK_CLOSE(&sin); + PSOCK_CLOSE(&sout); +} + +Plan9::Entry Plan9::add_entry(uint32_t fid, uint8_t type, const std::string& path) +{ + CHECK(fids.find(fid) == fids.end(), FID_IN_USE); + CHECK(fids.size() < MAXFIDS, ENFILE); + std::string abspath = absolute_path(path); + auto i = entries.find(abspath); + if (i == entries.end()) { + CHECK(entries.size() < MAXENTRIES, ENFILE); + entries[abspath] = EntryData(type); + i = entries.find(abspath); + } + Entry entry = &(*i); + CHECK(add_fid(fid, entry)); + return entry; +} + +Plan9::Entry Plan9::get_entry(uint32_t fid) +{ + auto i = fids.find(fid); + CHECK(i != fids.end()); + return i->second; +} + +bool Plan9::add_fid(uint32_t fid, Entry entry) +{ + CHECK(fids.find(fid) == fids.end(), FID_IN_USE); + CHECK(fids.size() < MAXFIDS, ENFILE); + fids[fid] = entry; + ++entry->second.refcount; + return true; +} + +void Plan9::remove_fid(uint32_t fid) +{ + auto i = fids.find(fid); + if (i != fids.end()) { + fids.erase(i); + --i->second->second.refcount; + if (i->second->second.refcount == 0) + entries.erase(i->second->first); + } +} + +void Plan9::init() +{ + uip_listen(HTONS(564)); +} + +void Plan9::appcall() +{ + Plan9* instance = static_cast(uip_conn->appstate); + + if (uip_connected() && !instance) { + instance = new Plan9; + DEBUG_PRINTF("new instance: %p\n", instance); + uip_conn->appstate = instance; + } + + if (uip_closed() || uip_aborted() || uip_timedout()) { + DEBUG_PRINTF("closed: %p\n", instance); + if(instance) { + delete instance; + uip_conn->appstate = nullptr; + } + return; + } + + if (!instance) { + DEBUG_PRINTF("null instance\n"); + uip_abort(); + return; + } + + instance->receive(); + instance->send(); +} + +int Plan9::receive() +{ + Message *request = reinterpret_cast(bufin); + + PSOCK_BEGIN(&sin); + (void)PT_YIELD_FLAG; // avoid warning unused variable + + for (;;) { + DEBUG_PRINTF("receive thread fids=%d entries=%d queue_size=%d queue_bytes=%lu\n", fids.size(), entries.size(), queue.size(), queue_bytes); + + PSOCK_READBUF_LEN(&sin, 4); + memcpy(request, request->buf + 4, 4); // copy size to buffer beginning + + DEBUG_PRINTF("receive size=%lu\n", request->size); + if (request->size > msize) { + DEBUG_PRINTF("Bad message received %lu\n", request->size); + PSOCK_CLOSE_EXIT(&sin); + } else { + PSOCK_READBUF_LEN(&sin, request->size - 4); + DEBUG_PRINTF("receive size=%lu type=%u tag=%d\n", request->size, request->type, request->tag); + } + + PSOCK_WAIT_UNTIL(&sin, queue_bytes < MAXREQUESTS * INITIAL_MSIZE); + + Message* copy = reinterpret_cast(new char[request->size]); + memcpy(copy, request, request->size); + queue.push(copy); + queue_bytes += copy->size; + + // store message + DEBUG_PRINTF("store size=%lu type=%u tag=%d queue_size=%d queue_bytes=%lu\n", request->size, request->type, request->tag, queue.size(), queue_bytes); + } + + PSOCK_END(&sin); +} + +int Plan9::send() +{ + Message* response = reinterpret_cast(bufout); + + PSOCK_BEGIN(&sout); + (void)PT_YIELD_FLAG; // avoid warning unused variable + + for (;;) { + DEBUG_PRINTF("send thread fids=%d entries=%d queue_size=%d queue_bytes=%lu\n", fids.size(), entries.size(), queue.size(), queue_bytes); + + PSOCK_WAIT_UNTIL(&sout, !queue.empty()); + + { + Message* request = queue.front(); + queue.pop(); + queue_bytes -= request->size; + process(request, response); + char* buf = request->buf; + delete[] buf; + } + + + DEBUG_PRINTF("send size=%lu type=%u tag=%d\n", response->size, response->type, response->tag); + PSOCK_SEND(&sout, response->buf, response->size); + } + + PSOCK_END(&sout); +} + + +bool Plan9::process(Message* request, Message* response) +{ + Entry entry; + + switch (request->type) { + case Tversion: + DEBUG_PRINTF("Tversion\n"); + RESPONSE(Rversion); + msize = response->Rversion.msize = min(INITIAL_MSIZE, request->Tversion.msize); + response->size = putstr(response->buf + response->size, response->buf + msize, "9P2000") - response->buf; + break; + + case Tattach: + DEBUG_PRINTF("Tattach\n"); + CHECK(entry = add_entry(request->fid, QTDIR, "/")); + RESPONSE(Rattach); + response->Rattach.qid = entry; + break; + + case Tflush: + DEBUG_PRINTF("Tflush\n"); + CHECK(request->size == sizeof (request->Tflush), EBADMSG); + RESPONSE(Rflush); + // do nothing + break; + + case Twalk: + DEBUG_PRINTF("Twalk fid=%lu newfid=%lu nwname=%u\n", request->Twalk.fid, request->Twalk.newfid, request->Twalk.nwname); + CHECK(entry = get_entry(request->fid)); + CHECK(request->Twalk.nwname <= MAXWELEM, EBADMSG); + + RESPONSE(Rwalk); + response->Rwalk.nwqid = 0; + + if (request->Twalk.nwname == 0) { + CHECK(add_fid(request->Twalk.newfid, entry)); + } else { + std::string path = entry->first; + const char* wname = request->Twalk.wname; + size_t last_path_size = 0; + Qid* wqid = response->Rwalk.wqid; + + for (uint16_t i = 0; i < request->Twalk.nwname; ++i) { + CHECK(wname + 2 <= request->buf + request->size, EBADMSG); + uint16_t len = *wname++; + len |= *wname++ << 8; + CHECK(wname + len <= request->buf + request->size, EBADMSG); + path = join_path(path, std::string(wname, len)); + wname += len; + + DEBUG_PRINTF("Twalk path=%s\n", path.c_str()); + + if (Dir(path)) { + *wqid++ = Qid(QTDIR, path); + ++response->Rwalk.nwqid; + last_path_size = path.size(); + } else { + if (File(path, "r")) { + *wqid++ = Qid(QTFILE, path); + ++response->Rwalk.nwqid; + last_path_size = path.size(); + } + i = request->Twalk.nwname; + } + } + + CHECK(response->Rwalk.nwqid > 0, ENOENT); + response->size += sizeof (Qid) * response->Rwalk.nwqid; + CHECK(add_entry(request->Twalk.newfid, wqid[-1].type, path.substr(0, last_path_size))); + } + break; + + case Tstat: + CHECK(request->size == sizeof (Header) + 4, EBADMSG); + CHECK(entry = get_entry(request->fid)); + + DEBUG_PRINTF("Tstat fid=%lu %s\n", request->fid, entry->first.c_str()); + + RESPONSE(Rstat); + CHECK((response->Rstat.stat_size = putstat(&response->Rstat.stat, response->buf + msize, entry->second.type, entry->first)) > 0, EFAULT); + response->size = sizeof (Header) + 2 + response->Rstat.stat_size; + break; + + case Tclunk: + DEBUG_PRINTF("Tclunk fid=%lu\n", request->fid); + CHECK(request->size == sizeof (Header) + 4, EBADMSG); + remove_fid(request->fid); + RESPONSE(Rclunk); + break; + + case Topen: + CHECK(request->size == sizeof (request->Topen), EBADMSG); + CHECK(entry = get_entry(request->fid)); + DEBUG_PRINTF("Topen fid=%lu %s\n", request->fid, entry->first.c_str()); + + if (entry->second.type != QTDIR && (request->Topen.mode & OTRUNC)) + CHECK(File(entry->first, "w"), EIO); + + RESPONSE(Ropen); + response->Ropen.qid = entry; + response->Ropen.iounit = IOUNIT; + break; + + case Tread: + DEBUG_PRINTF("Tread fid=%lu\n", request->fid); + CHECK(request->size == sizeof (request->Tread) && request->Tread.count <= IOUNIT, EBADMSG); + CHECK(entry = get_entry(request->fid)); + RESPONSE(Rread); + + if (entry->second.type == QTDIR) { + Dir dir(entry->first); + CHECK(dir, EIO); + + char* data = response->buf + sizeof (response->Rread); + struct dirent* d; + while ((d = readdir(dir)) && request->Tread.count > 0) { + auto path = join_path(entry->first, d->d_name); + DEBUG_PRINTF("Tread path %s\n", path.c_str()); + + char stat_buf[sizeof (Stat) + 128]; + size_t stat_size = putstat(reinterpret_cast(stat_buf), stat_buf + sizeof (stat_buf), d->d_isdir ? QTDIR : QTFILE, path); + CHECK(stat_size > 0, EFAULT); + + if (request->Tread.offset >= stat_size) { + request->Tread.offset -= stat_size; + } else { + CHECK(request->Tread.offset == 0, EBADMSG); + if (stat_size > request->Tread.count) { + request->Tread.count = 0; + } else { + memcpy(data, stat_buf, stat_size); + data += stat_size; + response->Rread.count += stat_size; + response->size += stat_size; + request->Tread.count -= stat_size; + } + } + } + } else { + File fp(entry->first, "r"); + CHECK(fp, EIO); + CHECK(!fseek(fp, request->Tread.offset, SEEK_SET), EIO); + response->Rread.count = fread(response->buf + response->size, 1, request->Tread.count, fp); + CHECK(response->Rread.count == request->Tread.count || !ferror(fp), EIO); + response->size += response->Rread.count; + } + break; + + case Tcreate: + { + CHECK(request->size == sizeof (request->Tcreate) + request->Tcreate.name_size + 4 + 1 && + request->Tcreate.name + request->Tcreate.name_size + 4 <= request->buf + request->size, EBADMSG); + CHECK(entry = get_entry(request->fid)); + + auto path = join_path(entry->first, std::string(request->Tcreate.name, request->Tcreate.name_size)); + uint32_t perm; + memcpy(&perm, request->Tcreate.name + request->Tcreate.name_size, 4); + + DEBUG_PRINTF("Tcreate fid=%lu path=%s\n", request->fid, path.c_str()); + CHECK(!(perm & ~(DMDIR | 0777)), ENOSYS); + + if (perm & DMDIR) + CHECK(!mkdir(path.c_str(), 0755), EEXIST); + else + CHECK(File(path, "w"), EIO); + remove_fid(request->fid); + CHECK(entry = add_entry(request->fid, (perm & DMDIR) ? QTDIR : QTFILE, path)); + RESPONSE(Rcreate); + response->Rcreate.qid = entry; + response->Rcreate.iounit = IOUNIT; + } + break; + + case Twrite: + { + DEBUG_PRINTF("Twrite fid=%lu\n", request->fid); + CHECK(request->size == sizeof (request->Twrite) + request->Twrite.count && + request->Twrite.count <= IOUNIT, EBADMSG); + CHECK(entry = get_entry(request->fid)); + + File fp(entry->first, "r+"); + CHECK(fp, EIO); + CHECK(!fseek(fp, request->Twrite.offset, SEEK_SET), EIO); + + RESPONSE(Rwrite); + response->Rwrite.count = fwrite(request->buf + sizeof (request->Twrite), 1, request->Twrite.count, fp); + CHECK(response->Rwrite.count == request->Twrite.count || !ferror(fp), EIO); + } + break; + + case Tremove: + { + DEBUG_PRINTF("Tremove fid=%lu\n", request->fid); + CHECK(request->size == sizeof (Header) + 4, EBADMSG); + CHECK(entry = get_entry(request->fid)); + auto e = *entry; + remove_fid(request->fid); + CHECK(!remove(e.first.c_str()), e.second.type == QTDIR ? ENOTEMPTY : EIO); + RESPONSE(Rremove); + } + break; + + case Twstat: + { + DEBUG_PRINTF("Twstat fid=%lu\n", request->fid); + CHECK(entry = get_entry(request->fid)); + char* name = request->buf + sizeof (request->Twstat); + uint16_t len = *name++; + len |= *name++ << 8; + CHECK(name + len <= request->buf + request->size, EBADMSG); + RESPONSE(Rwstat); + if (len > 0 && entry->first != "/") { + std::string newpath = join_path(entry->first.substr(0, entry->first.rfind('/')), std::string(name, len)); + if (newpath != entry->first) { + CHECK(!rename(entry->first.c_str(), newpath.c_str()), EIO); + uint8_t type = entry->second.type; + remove_fid(request->fid); + CHECK(add_entry(request->fid, type, newpath)); + } + } + } + break; + + // not implemented + // case Tauth: + // response(msg); + // msg->Rauth.aqid.type = 0; + // msg->Rauth.aqid.vers = 0; + // msg->Rauth.aqid.path = 1; + // break; + + default: + DEBUG_PRINTF("Unknown message %u\n", request->type); + ERROR(ENOSYS); + } + return true; +} diff --git a/src/libs/Network/uip/plan9/plan9.h b/src/libs/Network/uip/plan9/plan9.h new file mode 100644 index 00000000..86edf3b5 --- /dev/null +++ b/src/libs/Network/uip/plan9/plan9.h @@ -0,0 +1,73 @@ +#ifndef __PLAN9_H__ +#define __PLAN9_H__ + +/* + * 9P network filesystem protocol + * + * by Daniel Mendler + * + * Resources: + * + * - Documentation: http://9p.cat-v.org/ + * - List of implementations: http://9p.cat-v.org/implementations + * - Specification: http://ericvh.github.io/9p-rfc/ + * - Linux documentation: https://www.kernel.org/doc/Documentation/filesystems/9p.txt + * + * How to use it: + * + * 1. Add "network.plan9.enable true" to the config + * 2. Mount under Linux with "mount -t 9p $ip /mnt/smoothie + */ + +#include +#include +#include +#include + +extern "C" { +#include "psock.h" +} + +class Plan9 +{ +public: + Plan9(); + ~Plan9(); + + static void init(); + static void appcall(); + + struct EntryData { + uint8_t type; + int refcount; + + EntryData() {} + EntryData(uint8_t t) + : type(t), refcount(0) {} + }; + + typedef std::map EntryMap; + typedef EntryMap::value_type* Entry; + typedef std::map FidMap; + union Message; + +private: + int receive(); + int send(); + bool process(Message*, Message*); + + Entry add_entry(uint32_t, uint8_t, const std::string&); + Entry get_entry(uint32_t); + bool add_fid(uint32_t, Entry); + void remove_fid(uint32_t); + + static const uint32_t INITIAL_MSIZE = 300; + EntryMap entries; + FidMap fids; + psock sin, sout; + char bufin[INITIAL_MSIZE], bufout[INITIAL_MSIZE]; + std::queue queue; + uint32_t msize, queue_bytes; +}; + +#endif diff --git a/src/libs/Network/uip/telnetd/shell.cpp b/src/libs/Network/uip/telnetd/shell.cpp index 5702047b..42f5bd2d 100644 --- a/src/libs/Network/uip/telnetd/shell.cpp +++ b/src/libs/Network/uip/telnetd/shell.cpp @@ -223,7 +223,9 @@ int Shell::command_result(const char *str, void *p) /*---------------------------------------------------------------------------*/ void Shell::start() -{ +{ // add it to the kernels output stream + DEBUG_PRINTF("Shell: Adding stream to kernel streams\n"); + THEKERNEL->streams->append_stream(pstream); telnet->output("Smoothie command shell\r\n> "); } @@ -252,11 +254,6 @@ void Shell::close() void Shell::setConsole() { - // add it to the kernels output stream if we are a console - // TODO do we do this for all connections? so pronterface will get file done when playing from M24? - // then we need to turn it off for the streaming app - DEBUG_PRINTF("Shell: Adding stream to kernel streams\n"); - THEKERNEL->streams->append_stream(pstream); isConsole= true; } @@ -271,10 +268,9 @@ Shell::Shell(Telnetd *telnet) Shell::~Shell() { - if(isConsole) { - DEBUG_PRINTF("Shell: Removing stream from kernel streams\n"); - THEKERNEL->streams->remove_stream(pstream); - } + DEBUG_PRINTF("Shell: Removing stream from kernel streams\n"); + THEKERNEL->streams->remove_stream(pstream); + // we cannot delete this stream until it is no longer in any command queue entries // so mark it as closed, and allow it to delete itself when it is no longer being used static_cast(pstream)->mark_closed(); // mark the stream as closed so we do not get any callbacks diff --git a/src/libs/Network/uip/telnetd/telnetd.cpp b/src/libs/Network/uip/telnetd/telnetd.cpp index e0bbf9a7..3963bf6d 100644 --- a/src/libs/Network/uip/telnetd/telnetd.cpp +++ b/src/libs/Network/uip/telnetd/telnetd.cpp @@ -285,7 +285,6 @@ void Telnetd::newdata(void) }else if (c == TELNET_GA) { // enable prompt if telnet client running prompt= true; - shell->setConsole(); // tell shell we are a console, as this is sent be telnet clients }else{ /* Reply with a WONT */ //sendopt(TELNET_WONT, c); diff --git a/src/libs/Pin.cpp b/src/libs/Pin.cpp index cdd1f448..a147111d 100644 --- a/src/libs/Pin.cpp +++ b/src/libs/Pin.cpp @@ -3,20 +3,31 @@ // mbed libraries for hardware pwm #include "PwmOut.h" +#include "InterruptIn.h" #include "PinNames.h" +#include "port_api.h" Pin::Pin(){ this->inverting= false; + this->valid= false; + this->pin= 32; + this->port= nullptr; } // Make a new pin object from a string Pin* Pin::from_string(std::string value){ + if(value == "nc") { + this->valid= false; + return this; // optimize the nc case + } + LPC_GPIO_TypeDef* gpios[5] ={LPC_GPIO0,LPC_GPIO1,LPC_GPIO2,LPC_GPIO3,LPC_GPIO4}; // cs is the current position in the string const char* cs = value.c_str(); // cn is the position of the next char after the number we just read char* cn = NULL; + valid= true; // grab first integer as port. pointer to first non-digit goes in cn this->port_number = strtol(cs, &cn, 10); @@ -75,16 +86,17 @@ Pin* Pin::from_string(std::string value){ } // from_string failed. TODO: some sort of error + valid= false; port_number = 0; port = gpios[0]; - pin = 255; + pin = 32; inverting = false; return this; } // Configure this pin as OD Pin* Pin::as_open_drain(){ - if (this->pin >= 32) return this; + if (!this->valid) return this; if( this->port_number == 0 ){ LPC_PINCON->PINMODE_OD0 |= (1<pin); } if( this->port_number == 1 ){ LPC_PINCON->PINMODE_OD1 |= (1<pin); } if( this->port_number == 2 ){ LPC_PINCON->PINMODE_OD2 |= (1<pin); } @@ -97,7 +109,7 @@ Pin* Pin::as_open_drain(){ // Configure this pin as a repeater Pin* Pin::as_repeater(){ - if (this->pin >= 32) return this; + if (!this->valid) return this; // Set the two bits for this pin as 01 if( this->port_number == 0 && this->pin < 16 ){ LPC_PINCON->PINMODE0 |= (1<<( this->pin*2)); LPC_PINCON->PINMODE0 &= ~(2<<( this->pin *2)); } if( this->port_number == 0 && this->pin >= 16 ){ LPC_PINCON->PINMODE1 |= (1<<( this->pin*2)); LPC_PINCON->PINMODE1 &= ~(2<<((this->pin-16)*2)); } @@ -111,7 +123,7 @@ Pin* Pin::as_repeater(){ // Configure this pin as no pullup or pulldown Pin* Pin::pull_none(){ - if (this->pin >= 32) return this; + if (!this->valid) return this; // Set the two bits for this pin as 10 if( this->port_number == 0 && this->pin < 16 ){ LPC_PINCON->PINMODE0 |= (2<<( this->pin*2)); LPC_PINCON->PINMODE0 &= ~(1<<( this->pin *2)); } if( this->port_number == 0 && this->pin >= 16 ){ LPC_PINCON->PINMODE1 |= (2<<( this->pin*2)); LPC_PINCON->PINMODE1 &= ~(1<<((this->pin-16)*2)); } @@ -125,7 +137,7 @@ Pin* Pin::pull_none(){ // Configure this pin as a pullup Pin* Pin::pull_up(){ - if (this->pin >= 32) return this; + if (!this->valid) return this; // Set the two bits for this pin as 00 if( this->port_number == 0 && this->pin < 16 ){ LPC_PINCON->PINMODE0 &= ~(3<<( this->pin *2)); } if( this->port_number == 0 && this->pin >= 16 ){ LPC_PINCON->PINMODE1 &= ~(3<<((this->pin-16)*2)); } @@ -139,7 +151,7 @@ Pin* Pin::pull_up(){ // Configure this pin as a pulldown Pin* Pin::pull_down(){ - if (this->pin >= 32) return this; + if (!this->valid) return this; // Set the two bits for this pin as 11 if( this->port_number == 0 && this->pin < 16 ){ LPC_PINCON->PINMODE0 |= (3<<( this->pin *2)); } if( this->port_number == 0 && this->pin >= 16 ){ LPC_PINCON->PINMODE1 |= (3<<((this->pin-16)*2)); } @@ -177,5 +189,22 @@ mbed::PwmOut* Pin::hardware_pwm() if (pin == 25) { return new mbed::PwmOut(P3_25); } if (pin == 26) { return new mbed::PwmOut(P3_26); } } - return NULL; + return nullptr; +} + +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); + return new mbed::InterruptIn(pinname); + + }else{ + this->valid= false; + return nullptr; + } } diff --git a/src/libs/Pin.h b/src/libs/Pin.h index 38e7db58..48a6c642 100644 --- a/src/libs/Pin.h +++ b/src/libs/Pin.h @@ -10,6 +10,7 @@ namespace mbed { class PwmOut; + class InterruptIn; } class Pin { @@ -19,7 +20,7 @@ class Pin { Pin* from_string(std::string value); inline bool connected(){ - return this->pin < 32; + return this->valid; } inline bool equals(const Pin& other) const { @@ -27,13 +28,13 @@ class Pin { } inline Pin* as_output(){ - if (this->pin < 32) + if (this->valid) this->port->FIODIR |= 1<pin; return this; } inline Pin* as_input(){ - if (this->pin < 32) + if (this->valid) this->port->FIODIR &= ~(1<pin); return this; } @@ -49,14 +50,13 @@ class Pin { Pin* pull_none(void); inline bool get(){ - - if (this->pin >= 32) return false; + if (!this->valid) return false; return this->inverting ^ (( this->port->FIOPIN >> this->pin ) & 1); } inline void set(bool value) { - if (this->pin >= 32) return; + if (!this->valid) return; if ( this->inverting ^ value ) this->port->FIOSET = 1 << this->pin; else @@ -64,11 +64,18 @@ class Pin { } mbed::PwmOut *hardware_pwm(); - + + mbed::InterruptIn *interrupt_pin(); + + // these should be private, and use getters LPC_GPIO_TypeDef* port; - bool inverting; - char port_number; + unsigned char pin; + char port_number; + struct { + bool inverting:1; + bool valid:1; + }; }; diff --git a/src/libs/PublicData.cpp b/src/libs/PublicData.cpp index ec927e01..28401dc7 100644 --- a/src/libs/PublicData.cpp +++ b/src/libs/PublicData.cpp @@ -2,10 +2,16 @@ #include "PublicData.h" #include "PublicDataRequest.h" -bool PublicData::get_value(uint16_t csa, uint16_t csb, uint16_t csc, void **data) { +bool PublicData::get_value(uint16_t csa, uint16_t csb, uint16_t csc, void *data) { PublicDataRequest pdr(csa, csb, csc); + // the caller may have created the storage for the returned data so we clear the flag, + // if it gets set by the callee setting the data ptr that means the data is a pointer to a pointer and is set to a pointer to the returned data + pdr.set_data_ptr(data, false); THEKERNEL->call_event(ON_GET_PUBLIC_DATA, &pdr ); - *data= pdr.get_data_ptr(); + if(pdr.is_taken() && pdr.has_returned_data()) { + // the callee set the returned data pointer + *(void**)data= pdr.get_data_ptr(); + } return pdr.is_taken(); } diff --git a/src/libs/PublicData.h b/src/libs/PublicData.h index fe52e106..a888e9e9 100644 --- a/src/libs/PublicData.h +++ b/src/libs/PublicData.h @@ -10,10 +10,14 @@ class PublicData { public: - static bool get_value(uint16_t csa, void **data) { return get_value(csa, 0, 0, data); } - static bool get_value(uint16_t csa, uint16_t csb, void **data) { return get_value(csa, csb, 0, data); } - static bool get_value(uint16_t cs[3], void **data) { return get_value(cs[0], cs[1], cs[2], data); }; - static bool get_value(uint16_t csa, uint16_t csb, uint16_t csc, void **data); + // there are two ways to get data from a module + // 1. pass in a pointer to a data storage area that the caller creates, the callee module will put the returned data in that pointer + // 2. pass in a pointer to a pointer, the callee will set that pointer to some storage the callee has control over, with the requested data + // the version used is dependent on the target (callee) module + static bool get_value(uint16_t csa, void *data) { return get_value(csa, 0, 0, data); } + static bool get_value(uint16_t csa, uint16_t csb, void *data) { return get_value(csa, csb, 0, data); } + static bool get_value(uint16_t cs[3], void *data) { return get_value(cs[0], cs[1], cs[2], data); }; + static bool get_value(uint16_t csa, uint16_t csb, uint16_t csc, void *data); static bool set_value(uint16_t csa, void *data) { return set_value(csa, 0, 0, data); } static bool set_value(uint16_t csa, uint16_t csb, void *data) { return set_value(csa, csb, 0, data); } diff --git a/src/libs/PublicDataRequest.h b/src/libs/PublicDataRequest.h index 128e26a6..aabb449d 100644 --- a/src/libs/PublicDataRequest.h +++ b/src/libs/PublicDataRequest.h @@ -10,11 +10,11 @@ class PublicDataRequest { public: - PublicDataRequest(uint16_t addrcs1){ target[0]= addrcs1; target[1]= 0; target[2]= 0; data_taken= false; data= NULL; } - PublicDataRequest(uint16_t addrcs1, uint16_t addrcs2){ target[0]= addrcs1; target[1]= addrcs2; target[2]= 0; data_taken= false; data= NULL; } - PublicDataRequest(uint16_t addrcs1, uint16_t addrcs2, uint16_t addrcs3){ target[0]= addrcs1; target[1]= addrcs2; target[2]= addrcs3; data_taken= false; data= NULL; } + PublicDataRequest(uint16_t addrcs1){ target[0]= addrcs1; target[1]= 0; target[2]= 0; data_taken= false; data= NULL; returned_data= true; } + PublicDataRequest(uint16_t addrcs1, uint16_t addrcs2){ target[0]= addrcs1; target[1]= addrcs2; target[2]= 0; data_taken= false; data= NULL; returned_data= true; } + PublicDataRequest(uint16_t addrcs1, uint16_t addrcs2, uint16_t addrcs3){ target[0]= addrcs1; target[1]= addrcs2; target[2]= addrcs3; data_taken= false; data= NULL; returned_data= true; } - virtual ~PublicDataRequest() { data= NULL; } + virtual ~PublicDataRequest() { data= nullptr; } bool starts_with(uint16_t addr) const { return addr == this->target[0]; } bool second_element_is(uint16_t addr) const { return addr == this->target[1]; } @@ -22,14 +22,17 @@ class PublicDataRequest { bool is_taken() const { return this->data_taken; } void set_taken() { this->data_taken= true; } - - void set_data_ptr(void *d) { this->data= d; } + bool has_returned_data() const { return this->returned_data; } + void set_data_ptr(void *d, bool flag= true) { this->data= d; returned_data= flag; } void* get_data_ptr(void) const { return this->data; } private: uint16_t target[3]; void* data; - bool data_taken; + struct { + bool data_taken:1; + bool returned_data:1; + }; }; #endif diff --git a/src/libs/Pwm.h b/src/libs/Pwm.h index 649fc589..559194bb 100644 --- a/src/libs/Pwm.h +++ b/src/libs/Pwm.h @@ -17,6 +17,7 @@ public: int max_pwm(void); void pwm(int); + int get_pwm() const { return _pwm; } void set(bool); private: diff --git a/src/libs/SlowTicker.cpp b/src/libs/SlowTicker.cpp index d54e50cf..a2d516dd 100644 --- a/src/libs/SlowTicker.cpp +++ b/src/libs/SlowTicker.cpp @@ -11,9 +11,9 @@ using namespace std; #include "libs/Module.h" #include "libs/Kernel.h" #include "SlowTicker.h" +#include "StepTicker.h" #include "libs/Hook.h" #include "modules/robot/Conveyor.h" -#include "Pauser.h" #include "Gcode.h" #include @@ -35,9 +35,6 @@ SlowTicker::SlowTicker(){ flag_1s_flag = 0; flag_1s_count = SystemCoreClock>>2; - g4_ticks = 0; - g4_pause = false; - // Configure the actual timer after setup to avoid race conditions LPC_SC->PCONP |= (1 << 22); // Power Ticker ON LPC_TIM2->MR0 = 10000; // Initial dummy value for Match Register @@ -48,8 +45,6 @@ SlowTicker::SlowTicker(){ void SlowTicker::on_module_loaded(){ register_for_event(ON_IDLE); - register_for_event(ON_GCODE_RECEIVED); - register_for_event(ON_GCODE_EXECUTE); } // Set the base frequency we use for all sub-frequencies @@ -65,8 +60,7 @@ void SlowTicker::set_frequency( int frequency ){ void SlowTicker::tick(){ // Call all hooks that need to be called ( bresenham ) - for (uint32_t i=0; ihooks.size(); i++){ - Hook* hook = this->hooks.at(i); + for (Hook* hook : this->hooks){ hook->countdown -= this->interval; if (hook->countdown < 0) { @@ -86,16 +80,6 @@ void SlowTicker::tick(){ flag_1s_flag++; } - // if we're counting down a pause - if (g4_ticks > 0) - { - // deduct tick time from timeout - if (g4_ticks > interval) - g4_ticks -= interval; - else - g4_ticks = 0; - } - // Enter MRI mode if the ISP button is pressed // TODO: This should have it's own module if (ispbtn.get() == 0) @@ -127,7 +111,7 @@ extern GPIO leds[]; void SlowTicker::on_idle(void*) { static uint16_t ledcnt= 0; - if(THEKERNEL->use_leds) { + if(THEKERNEL->is_using_leds()) { // flash led 3 to show we are alive leds[2]= (ledcnt++ & 0x1000) ? 1 : 0; } @@ -136,52 +120,6 @@ void SlowTicker::on_idle(void*) if (flag_1s()) // fire the on_second_tick event THEKERNEL->call_event(ON_SECOND_TICK); - - // if G4 has finished, release our pause - if (g4_pause && (g4_ticks == 0)) - { - g4_pause = false; - THEKERNEL->pauser->release(); - } -} - -// When a G4-type gcode is received, add it to the queue so we can execute it in time -void SlowTicker::on_gcode_received(void* argument){ - Gcode* gcode = static_cast(argument); - // Add the gcode to the queue ourselves if we need it - if( gcode->has_g && gcode->g == 4 ){ - THEKERNEL->conveyor->append_gcode(gcode); - // ensure that no subsequent gcodes get executed along with our G4 - THEKERNEL->conveyor->queue_head_block(); - } -} - -// When a G4-type gcode is executed, start the pause -void SlowTicker::on_gcode_execute(void* argument){ - Gcode* gcode = static_cast(argument); - - if (gcode->has_g){ - if (gcode->g == 4){ - gcode->mark_as_taken(); - bool updated = false; - if (gcode->has_letter('P')) { - updated = true; - g4_ticks += gcode->get_int('P') * ((SystemCoreClock >> 2) / 1000UL); - } - if (gcode->has_letter('S')) { - updated = true; - g4_ticks += gcode->get_int('S') * (SystemCoreClock >> 2); - } - if (updated){ - // G4 Smm Pnn should pause for mm seconds + nn milliseconds - // at 120MHz core clock, the longest possible delay is (2^32 / (120MHz / 4)) = 143 seconds - if (!g4_pause){ - g4_pause = true; - THEKERNEL->pauser->take(); - } - } - } - } } extern "C" void TIMER2_IRQHandler (void){ diff --git a/src/libs/SlowTicker.h b/src/libs/SlowTicker.h index 133dd787..091f931b 100644 --- a/src/libs/SlowTicker.h +++ b/src/libs/SlowTicker.h @@ -27,8 +27,6 @@ class SlowTicker : public Module{ void on_module_loaded(void); void on_idle(void*); - void on_gcode_received(void*); - void on_gcode_execute(void*); void set_frequency( int frequency ); void tick(); @@ -36,7 +34,7 @@ class SlowTicker : public Module{ // TODO replace this with std::function() template Hook* attach( uint32_t frequency, T *optr, uint32_t ( T::*fptr )( uint32_t ) ){ Hook* hook = new Hook(); - hook->interval = int(floor((SystemCoreClock/4)/frequency)); + hook->interval = floorf((SystemCoreClock/4)/frequency); hook->attach(optr, fptr); hook->countdown = hook->interval; @@ -58,9 +56,6 @@ class SlowTicker : public Module{ uint32_t max_frequency; uint32_t interval; - uint32_t g4_ticks; - bool g4_pause; - Pin ispbtn; protected: int flag_1s_count; diff --git a/src/libs/StepTicker.cpp b/src/libs/StepTicker.cpp dissimilarity index 67% index c6275f8e..6fc64455 100644 --- a/src/libs/StepTicker.cpp +++ b/src/libs/StepTicker.cpp @@ -1,276 +1,256 @@ -/* - 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 "StepTicker.h" - -using namespace std; -#include - -#include "libs/nuts_bolts.h" -#include "libs/Module.h" -#include "libs/Kernel.h" -#include "StepperMotor.h" - -#include "system_LPC17xx.h" // mbed.h lib -#include -#include - -extern bool _isr_context; - -// StepTicker handles the base frequency ticking for the Stepper Motors / Actuators -// It has a list of those, and calls their tick() functions at regular intervals -// They then do Bresenham stuff themselves - -StepTicker* StepTicker::global_step_ticker; - -StepTicker::StepTicker(){ - StepTicker::global_step_ticker = this; - - // Configure the timer - LPC_TIM0->MR0 = 10000000; // Initial dummy value for Match Register - LPC_TIM0->MCR = 3; // Match on MR0, reset on MR0, match on MR1 - LPC_TIM0->TCR = 0; // Disable interrupt - - LPC_SC->PCONP |= (1 << 2); // Power Ticker ON - LPC_TIM1->MR0 = 1000000; - LPC_TIM1->MCR = 1; - LPC_TIM1->TCR = 1; // Enable interrupt - - // Default start values - this->moves_finished = false; - this->reset_step_pins = false; - this->debug = 0; - this->has_axes = 0; - this->set_frequency(0.001); - this->set_reset_delay(100); - this->last_duration = 0; - for (int i = 0; i < 12; i++){ - this->active_motors[i] = NULL; - } - this->active_motor_bm = 0; - - NVIC_EnableIRQ(TIMER0_IRQn); // Enable interrupt handler - NVIC_EnableIRQ(TIMER1_IRQn); // Enable interrupt handler -} - -// Set the base stepping frequency -void StepTicker::set_frequency( float frequency ){ - this->frequency = frequency; - this->period = int(floor((SystemCoreClock/4)/frequency)); // SystemCoreClock/4 = Timer increments in a second - LPC_TIM0->MR0 = this->period; - if( LPC_TIM0->TC > LPC_TIM0->MR0 ){ - LPC_TIM0->TCR = 3; // Reset - LPC_TIM0->TCR = 1; // Reset - } -} - -// Set the reset delay -void StepTicker::set_reset_delay( float seconds ){ - this->delay = int(floor(float(SystemCoreClock/4)*( seconds ))); // SystemCoreClock/4 = Timer increments in a second - LPC_TIM1->MR0 = this->delay; -} - -// Add a stepper motor object to our list of steppers we must take care of -StepperMotor* StepTicker::add_stepper_motor(StepperMotor* stepper_motor){ - this->stepper_motors.push_back(stepper_motor); - stepper_motor->step_ticker = this; - this->has_axes = true; - return stepper_motor; -} - -// Call tick() on each active motor -inline void StepTicker::tick(){ - _isr_context = true; - int i; - uint32_t bm = 1; - // We iterate over each active motor - for (i = 0; i < 12; i++, bm <<= 1){ - if (this->active_motor_bm & bm){ - this->active_motors[i]->tick(); - } - } - _isr_context = false; -} - -// Call signal_mode_finished() on each active motor that asked to be signaled. We do this instead of inside of tick() so that -// all tick()s are called before we do the move finishing -void StepTicker::signal_moves_finished(){ - _isr_context = true; - - uint16_t bitmask = 1; - for ( uint8_t motor = 0; motor < 12; motor++, bitmask <<= 1){ - if (this->active_motor_bm & bitmask){ - if(this->active_motors[motor]->is_move_finished){ - this->active_motors[motor]->signal_move_finished(); - if(this->active_motors[motor]->moving == false){ - if (motor > 0){ - motor--; - bitmask >>= 1; - } - } - } - } - } - this->moves_finished = false; - - _isr_context = false; -} - -// Reset step pins on all active motors -inline void StepTicker::reset_tick(){ - _isr_context = true; - - int i; - uint32_t bm; - for (i = 0, bm = 1; i < 12; i++, bm <<= 1) - { - if (this->active_motor_bm & bm) - this->active_motors[i]->unstep(); - } - - _isr_context = false; -} - -extern "C" void TIMER1_IRQHandler (void){ - LPC_TIM1->IR |= 1 << 0; - StepTicker::global_step_ticker->reset_tick(); -} - -// The actual interrupt handler where we do all the work -extern "C" void TIMER0_IRQHandler (void){ - StepTicker::global_step_ticker->TIMER0_IRQHandler(); -} - -void StepTicker::TIMER0_IRQHandler (void){ - // Reset interrupt register - LPC_TIM0->IR |= 1 << 0; - - // Step pins - uint16_t bitmask = 1; - for (uint8_t motor = 0; motor < 12; motor++, bitmask <<= 1){ - if (this->active_motor_bm & bitmask){ - this->active_motors[motor]->tick(); - } - } - - // We may have set a pin on in this tick, now we start the timer to set it off - if( this->reset_step_pins ){ - LPC_TIM1->TCR = 3; - LPC_TIM1->TCR = 1; - this->reset_step_pins = false; - }else{ - // Nothing happened, nothing after this really matters - // TODO : This could be a problem when we use Actuators instead of StepperMotors, because this flag is specific to step generation - LPC_TIM0->MR0 = this->period; - return; - } - - // If a move finished in this tick, we have to tell the actuator to act accordingly - if( this->moves_finished ){ - - // Do not get out of here before everything is nice and tidy - LPC_TIM0->MR0 = 20000000; - - this->signal_moves_finished(); - - // If we went over the duration an interrupt is supposed to last, we have a problem - // That can happen tipically when we change blocks, where more than usual computation is done - // This can be OK, if we take notice of it, which we do now - if( LPC_TIM0->TC > this->period ){ // TODO: remove the size condition - - uint32_t start_tc = LPC_TIM0->TC; - - // How many ticks we want to skip ( this does not include the current tick, but we add the time we spent doing this computation last time ) - uint32_t ticks_to_skip = ( ( LPC_TIM0->TC + this->last_duration ) / this->period ); - - // Next step is now to reduce this to how many steps we can *actually* skip - uint32_t ticks_we_actually_can_skip = ticks_to_skip; - - int i; - uint32_t bm; - for (i = 0, bm = 1; i < 12; i++, bm <<= 1) - { - if (this->active_motor_bm & bm) - ticks_we_actually_can_skip = - min(ticks_we_actually_can_skip, - (uint32_t)((uint64_t)( (uint64_t)this->active_motors[i]->fx_ticks_per_step - (uint64_t)this->active_motors[i]->fx_counter ) >> 32) - ); - } - - // Adding to MR0 for this time is not enough, we must also increment the counters ourself artificially - for (i = 0, bm = 1; i < 12; i++, bm <<= 1) - { - if (this->active_motor_bm & bm) - this->active_motors[i]->fx_counter += (uint64_t)((uint64_t)(ticks_we_actually_can_skip)<<32); - } - - // When must we have our next MR0 ? ( +1 is here to account that we are actually doing a legit MR0 match here too, not only overtime ) - LPC_TIM0->MR0 = ( ticks_to_skip + 1 ) * this->period; - - // This is so that we know how long this computation takes, and we can take it into account next time - int difference = (int)(LPC_TIM0->TC) - (int)(start_tc); - if( difference > 0 ){ this->last_duration = (uint32_t)difference; } - - }else{ - LPC_TIM0->MR0 = this->period; - } - - while( LPC_TIM0->TC > LPC_TIM0->MR0 ){ - LPC_TIM0->MR0 += this->period; - } - - } - -} - - -// We make a list of steppers that want to be called so that we don't call them for nothing -void StepTicker::add_motor_to_active_list(StepperMotor* motor) -{ - uint32_t bm; - int i; - for (i = 0, bm = 1; i < 12; i++, bm <<= 1) - { - if (this->active_motors[i] == motor) - { - this->active_motor_bm |= bm; - if( this->active_motor_bm != 0 ){ - LPC_TIM0->TCR = 1; // Enable interrupt - } - return; - } - if (this->active_motors[i] == NULL) - { - this->active_motors[i] = motor; - this->active_motor_bm |= bm; - if( this->active_motor_bm != 0 ){ - LPC_TIM0->TCR = 1; // Enable interrupt - } - return; - } - } - return; -} - -// Remove a stepper from the list of active motors -void StepTicker::remove_motor_from_active_list(StepperMotor* motor) -{ - uint32_t bm; int i; - for (i = 0, bm = 1; i < 12; i++, bm <<= 1) - { - if (this->active_motors[i] == motor) - { - this->active_motor_bm &= ~bm; - // If we have no motor to work on, disable the whole interrupt - if( this->active_motor_bm == 0 ){ - LPC_TIM0->TCR = 0; // Disable interrupt - } - return; - } - } -} +/* + 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 "StepTicker.h" + +#include "libs/nuts_bolts.h" +#include "libs/Module.h" +#include "libs/Kernel.h" +#include "StepperMotor.h" +#include "StreamOutputPool.h" +#include "system_LPC17xx.h" // mbed.h lib +#include +#include + +#ifdef STEPTICKER_DEBUG_PIN +#include "gpio.h" +extern GPIO stepticker_debug_pin; +#endif + + +// StepTicker handles the base frequency ticking for the Stepper Motors / Actuators +// It has a list of those, and calls their tick() functions at regular intervals +// They then do Bresenham stuff themselves + +StepTicker* StepTicker::global_step_ticker; + +StepTicker::StepTicker(){ + StepTicker::global_step_ticker = this; + + // Configure the timer + LPC_TIM0->MR0 = 10000000; // Initial dummy value for Match Register + LPC_TIM0->MCR = 3; // Match on MR0, reset on MR0, match on MR1 + LPC_TIM0->TCR = 0; // Disable interrupt + + LPC_SC->PCONP |= (1 << 2); // Power Ticker ON + LPC_TIM1->MR0 = 1000000; + LPC_TIM1->MCR = 1; + LPC_TIM1->TCR = 0; // Disable interrupt + + // Setup RIT timer + LPC_SC->PCONP |= (1L<<16); // RIT Power + LPC_SC->PCLKSEL1 &= ~(3L << 26); // Clear PCLK_RIT bits; + LPC_SC->PCLKSEL1 |= (1L << 26); // Set PCLK_RIT bits to 0x01; + LPC_RIT->RICOMPVAL = (uint32_t)(((SystemCoreClock / 1000000L) * 1000)-1); // 1ms period + LPC_RIT->RICOUNTER = 0; + // Set counter clear/reset after interrupt + LPC_RIT->RICTRL |= (2L); //RITENCLR + LPC_RIT->RICTRL &= ~(8L); // disable + //NVIC_SetVector(RIT_IRQn, (uint32_t)&_ritisr); + + // Default start values + this->a_move_finished = false; + this->do_move_finished = 0; + this->unstep.reset(); + this->set_frequency(100000); + this->set_reset_delay(100); + this->set_acceleration_ticks_per_second(1000); + this->num_motors= 0; + this->active_motor.reset(); + this->tick_cnt= 0; +} + +StepTicker::~StepTicker() { +} + +//called when everythinf is setup and interrupts can start +void StepTicker::start() { + NVIC_EnableIRQ(TIMER0_IRQn); // Enable interrupt handler + NVIC_EnableIRQ(TIMER1_IRQn); // Enable interrupt handler + NVIC_EnableIRQ(RIT_IRQn); +} + +// Set the base stepping frequency +void StepTicker::set_frequency( float frequency ){ + this->frequency = frequency; + this->period = floorf((SystemCoreClock/4.0F)/frequency); // SystemCoreClock/4 = Timer increments in a second + LPC_TIM0->MR0 = this->period; + if( LPC_TIM0->TC > LPC_TIM0->MR0 ){ + LPC_TIM0->TCR = 3; // Reset + LPC_TIM0->TCR = 1; // Reset + } +} + +// Set the reset delay +void StepTicker::set_reset_delay( float microseconds ){ + uint32_t delay = floorf((SystemCoreClock/4.0F)*(microseconds/1000000.0F)); // SystemCoreClock/4 = Timer increments in a second + LPC_TIM1->MR0 = delay; +} + +// this is the number of acceleration ticks per second +void StepTicker::set_acceleration_ticks_per_second(uint32_t acceleration_ticks_per_second) { + uint32_t us= roundf(1000000.0F/acceleration_ticks_per_second); // period in microseconds + LPC_RIT->RICOMPVAL = (uint32_t)(((SystemCoreClock / 1000000L) * us)-1); // us + LPC_RIT->RICOUNTER = 0; + LPC_RIT->RICTRL |= (8L); // Enable rit +} + +// Synchronize the acceleration timer, and optionally schedule it to fire now +void StepTicker::synchronize_acceleration(bool fire_now) { + LPC_RIT->RICOUNTER = 0; + if(fire_now){ + NVIC_SetPendingIRQ(RIT_IRQn); + }else{ + if(NVIC_GetPendingIRQ(RIT_IRQn)) { + // clear pending interrupt so it does not interrupt immediately + LPC_RIT->RICTRL |= 1L; // also clear the interrupt in case it fired + NVIC_ClearPendingIRQ(RIT_IRQn); + } + } +} + + +// Call signal_move_finished() on each active motor that asked to be signaled. We do this instead of inside of tick() so that +// all tick()s are called before we do the move finishing +void StepTicker::signal_a_move_finished(){ + for (int motor = 0; motor < num_motors; motor++){ + if (this->active_motor[motor] && this->motor[motor]->is_move_finished){ + this->motor[motor]->signal_move_finished(); + // Theoretically this does nothing and the reason for it is currently unknown and/or forgotten + // if(this->motor[motor]->moving == false){ + // if (motor > 0){ + // motor--; + // bitmask >>= 1; + // } + // } + } + } +} + +// Reset step pins on any motor that was stepped +inline void StepTicker::unstep_tick(){ + for (int i = 0; i < num_motors; i++) { + if(this->unstep[i]){ + this->motor[i]->unstep(); + } + } + this->unstep.reset(); +} + +extern "C" void TIMER1_IRQHandler (void){ + LPC_TIM1->IR |= 1 << 0; + StepTicker::global_step_ticker->unstep_tick(); +} + +// The actual interrupt handler where we do all the work +extern "C" void TIMER0_IRQHandler (void){ + StepTicker::global_step_ticker->TIMER0_IRQHandler(); +} + +extern "C" void RIT_IRQHandler (void){ + LPC_RIT->RICTRL |= 1L; + StepTicker::global_step_ticker->acceleration_tick(); +} + +extern "C" void PendSV_Handler(void) { + StepTicker::global_step_ticker->PendSV_IRQHandler(); +} + +// slightly lower priority than TIMER0, the whole end of block/start of block is done here allowing the timer to continue ticking +void StepTicker::PendSV_IRQHandler (void) { + + if(this->do_move_finished.load() > 0) { + this->do_move_finished--; + #ifdef STEPTICKER_DEBUG_PIN + stepticker_debug_pin= 1; + #endif + + this->signal_a_move_finished(); + + #ifdef STEPTICKER_DEBUG_PIN + stepticker_debug_pin= 0; + #endif + } +} + +// run in RIT lower priority than PendSV +void StepTicker::acceleration_tick() { + // call registered acceleration handlers + for (size_t i = 0; i < acceleration_tick_handlers.size(); ++i) { + acceleration_tick_handlers[i](); + } +} + +void StepTicker::TIMER0_IRQHandler (void){ + // Reset interrupt register + LPC_TIM0->IR |= 1 << 0; + tick_cnt++; // count number of ticks + + // Step pins NOTE takes 1.2us when nothing to step, 1.8-2us for one motor stepped and 2.6us when two motors stepped, 3.167us when three motors stepped + for (uint32_t motor = 0; motor < num_motors; motor++){ + // send tick to all active motors + if(this->active_motor[motor] && this->motor[motor]->tick()){ + // we stepped so schedule an unstep + this->unstep[motor]= 1; + } + } + + // We may have set a pin on in this tick, now we reset the timer to set it off + // Note there could be a race here if we run another tick before the unsteps have happened, + // right now it takes about 3-4us but if the unstep were near 10uS or greater it would be an issue + // also it takes at least 2us to get here so even when set to 1us pulse width it will still be about 3us + if( this->unstep.any()){ + LPC_TIM1->TCR = 3; + LPC_TIM1->TCR = 1; + } + // just let it run it will fire every 143 seconds + // else{ + // LPC_TIM1->TCR = 0; // disable interrupt, no point in it running if nothing to do + // } + + if(this->a_move_finished) { + this->a_move_finished= false; + this->do_move_finished++; // Note this is an atomic variable because it is updated in two interrupts of different priorities so can be pre-empted + } + + // If a move finished in this tick, we have to tell the actuator to act accordingly + if(this->do_move_finished.load() > 0){ + // we delegate the slow stuff to the pendsv handler which will run as soon as this interrupt exits + //NVIC_SetPendingIRQ(PendSV_IRQn); this doesn't work + SCB->ICSR = 0x10000000; // SCB_ICSR_PENDSVSET_Msk; + } +} + +// returns index of the stepper motor in the array and bitset +int StepTicker::register_motor(StepperMotor* motor) +{ + this->motor.push_back(motor); + this->num_motors= this->motor.size(); + return this->num_motors-1; +} + +// activate the specified motor, must have been registered +void StepTicker::add_motor_to_active_list(StepperMotor* motor) +{ + bool enabled= active_motor.any(); // see if interrupt was previously enabled + active_motor[motor->index]= 1; + if(!enabled) { + LPC_TIM0->TCR = 1; // Enable interrupt + } +} + +// Remove a stepper from the list of active motors +void StepTicker::remove_motor_from_active_list(StepperMotor* motor) +{ + active_motor[motor->index]= 0; + // If we have no motor to work on, disable the whole interrupt + if(this->active_motor.none()){ + LPC_TIM0->TCR = 0; // Disable interrupt + tick_cnt= 0; + } +} diff --git a/src/libs/StepTicker.h b/src/libs/StepTicker.h index 1e14ba5a..3923cadc 100644 --- a/src/libs/StepTicker.h +++ b/src/libs/StepTicker.h @@ -10,43 +10,55 @@ #ifndef STEPTICKER_H #define STEPTICKER_H -using namespace std; -#include #include +#include +#include +#include +#include class StepperMotor; class StepTicker{ public: - friend class StepperMotor; static StepTicker* global_step_ticker; StepTicker(); + ~StepTicker(); void set_frequency( float frequency ); - void tick(); - void signal_moves_finished(); - StepperMotor* add_stepper_motor(StepperMotor* stepper_motor); + void signal_a_move_finished(); void set_reset_delay( float seconds ); - void reset_tick(); + int register_motor(StepperMotor* motor); void add_motor_to_active_list(StepperMotor* motor); void remove_motor_from_active_list(StepperMotor* motor); + void set_acceleration_ticks_per_second(uint32_t acceleration_ticks_per_second); + float get_frequency() const { return frequency; } + void unstep_tick(); + uint32_t get_tick_cnt() const { return tick_cnt; } + uint32_t ticks_since(uint32_t last) const { return (tick_cnt>=last) ? tick_cnt-last : (UINT32_MAX-last) + tick_cnt + 1; } + void TIMER0_IRQHandler (void); + void PendSV_IRQHandler (void); + void register_acceleration_tick_handler(std::function cb){ + acceleration_tick_handlers.push_back(cb); + } + void acceleration_tick(); + void synchronize_acceleration(bool fire_now); + + void start(); + + friend class StepperMotor; private: float frequency; - vector stepper_motors; - uint32_t delay; uint32_t period; - uint32_t debug; - uint32_t last_duration; - bool has_axes; - - bool moves_finished; - bool reset_step_pins; - - StepperMotor* active_motors[12]; - uint32_t active_motor_bm; - + volatile uint32_t tick_cnt; + std::vector> acceleration_tick_handlers; + std::vector motor; + std::bitset<32> active_motor; // limit to 32 motors + std::bitset<32> unstep; // limit to 32 motors + std::atomic_uchar do_move_finished; + uint8_t num_motors; + volatile bool a_move_finished; }; diff --git a/src/libs/StepperMotor.cpp b/src/libs/StepperMotor.cpp index b49a4936..19861160 100644 --- a/src/libs/StepperMotor.cpp +++ b/src/libs/StepperMotor.cpp @@ -12,8 +12,10 @@ #include +// in steps/sec the default minimum speed (was 20steps/sec hardcoded) +float StepperMotor::default_minimum_actuator_rate= 20.0F; + // A StepperMotor represents an actual stepper motor. It is used to generate steps that move the actual motor at a given speed -// TODO : Abstract this into Actuator StepperMotor::StepperMotor() { @@ -27,35 +29,44 @@ StepperMotor::StepperMotor(Pin &step, Pin &dir, Pin &en) : step_pin(step), dir_p set_high_on_debug(en.port_number, en.pin); } +StepperMotor::~StepperMotor() +{ +} + void StepperMotor::init() { + // register this motor with the step ticker, and get its index in that array and bit position + this->index= THEKERNEL->step_ticker->register_motor(this); this->moving = false; - this->paused = false; this->fx_counter = 0; + this->fx_ticks_per_step = 0xFFFFF000UL; // some big number so we don't start stepping before it is set this->stepped = 0; - this->fx_ticks_per_step = 0; this->steps_to_move = 0; - this->remove_from_active_list_next_reset = false; this->is_move_finished = false; - this->signal_step = false; - this->step_signal_hook = new Hook(); + this->last_step_tick_valid= false; + this->last_step_tick= 0; steps_per_mm = 1.0F; max_rate = 50.0F; + minimum_step_rate = default_minimum_actuator_rate; - current_position_steps= 0; last_milestone_steps = 0; last_milestone_mm = 0.0F; + current_position_steps= 0; + signal_step= 0; } // This is called ( see the .h file, we had to put a part of things there for obscure inline reasons ) when a step has to be generated -// we also here check if the move is finished etc ... +// we also here check if the move is finished etc .. +// This is in highest priority interrupt so cannot be pre-empted void StepperMotor::step() { + // ignore if we are still processing the end of a block + if(this->is_move_finished) return; + // output to pins 37t this->step_pin.set( 1 ); - this->step_ticker->reset_step_pins = true; // move counter back 11t this->fx_counter -= this->fx_ticks_per_step; @@ -63,20 +74,22 @@ void StepperMotor::step() // we have moved a step 9t this->stepped++; - // Do we need to signal this step - if( this->stepped == this->signal_step_number && this->signal_step ) { - this->step_signal_hook->call(); - } - // keep track of actuators actual position in steps this->current_position_steps += (this->direction ? -1 : 1); + // we may need to callback on a specific step, usually used to synchronize deceleration timer + if(this->signal_step != 0 && this->stepped == this->signal_step) { + THEKERNEL->step_ticker->synchronize_acceleration(true); + this->signal_step= 0; + } + // Is this move finished ? if( this->stepped == this->steps_to_move ) { // Mark it as finished, then StepTicker will call signal_mode_finished() // This is so we don't call that before all the steps have been generated for this tick() this->is_move_finished = true; - this->step_ticker->moves_finished = true; + THEKERNEL->step_ticker->a_move_finished= true; + this->last_step_tick= THEKERNEL->step_ticker->get_tick_cnt(); // remember when last step was } } @@ -87,38 +100,35 @@ void StepperMotor::signal_move_finished() // work is done ! 8t this->moving = false; this->steps_to_move = 0; + this->minimum_step_rate = default_minimum_actuator_rate; - // signal it to whatever cares 41t 411t + // signal it to whatever cares + // in this call a new block may start, new moves set and new speeds this->end_hook->call(); // We only need to do this if we were not instructed to move - if( this->moving == false ) { + if( !this->moving ) { this->update_exit_tick(); } this->is_move_finished = false; } -// This is just a way not to check for ( !this->moving || this->paused || this->fx_ticks_per_step == 0 ) at every tick() -inline void StepperMotor::update_exit_tick() +// This is just a way not to check for ( !this->moving || this->fx_ticks_per_step == 0 ) at every tick() +void StepperMotor::update_exit_tick() { - if( !this->moving || this->paused || this->steps_to_move == 0 ) { - // We must exit tick() after setting the pins, no bresenham is done - //this->remove_from_active_list_next_reset = true; - this->step_ticker->remove_motor_from_active_list(this); + if( !this->moving || this->steps_to_move == 0 ) { + // No more ticks will be recieved and no more events from StepTicker + THEKERNEL->step_ticker->remove_motor_from_active_list(this); } else { - // We must do the bresenham in tick() - // We have to do this or there could be a bug where the removal still happens when it doesn't need to - this->step_ticker->add_motor_to_active_list(this); + // we will now get ticks and StepTIcker will send us events + THEKERNEL->step_ticker->add_motor_to_active_list(this); } } - - // Instruct the StepperMotor to move a certain number of steps -void StepperMotor::move( bool direction, unsigned int steps ) +StepperMotor* StepperMotor::move( bool direction, unsigned int steps, float initial_speed) { - // We do not set the direction directly, we will set the pin just before the step pin on the next tick this->dir_pin.set(direction); this->direction = direction; @@ -126,56 +136,57 @@ void StepperMotor::move( bool direction, unsigned int steps ) this->steps_to_move = steps; // Zero our tool counters - this->fx_counter = 0; // Bresenheim counter this->stepped = 0; - - // Do not signal steps until we get instructed to - this->signal_step = false; + this->fx_ticks_per_step = 0xFFFFF000UL; // some big number so we don't start stepping before it is set again + if(this->last_step_tick_valid) { + // we set this based on when the last step was, thus compensating for missed ticks + uint32_t ts= THEKERNEL->step_ticker->ticks_since(this->last_step_tick); + // if an axis stops too soon then we can get a huge number of ticks here which causes problems, so if the number of ticks is too great we ignore them + // example of when this happens is when one axis is going very slow an the min 20steps/sec kicks in, the axis will reach its target much sooner leaving a long gap + // until the end of the block. + // TODO we may need to set this based on the current step rate, trouble is we don't know what that is yet, we could use the last fx_ticks_per_step as a guide + if(ts > 5) ts= 5; // limit to 50us catch up around 1-2 steps + else if(ts > 15) ts= 0; // no way to know what the delay was + this->fx_counter= ts*fx_increment; + }else{ + this->fx_counter = 0; // set to zero as there was no step last block + } // Starting now we are moving if( steps > 0 ) { + if(initial_speed >= 0.0F) set_speed(initial_speed); this->moving = true; } else { this->moving = false; } this->update_exit_tick(); - + return this; } -// Set the speed at which this steper moves -void StepperMotor::set_speed( float speed ) +// Set the speed at which this stepper moves in steps/sec, should be called set_step_rate() +// we need to make sure that we have a minimum speed here and that it fits the 32bit fixed point fx counters +// Note nothing will really ever go as slow as the minimum speed here, it is just forced to avoid bad errors +// fx_ticks_per_step is what actually sets the step rate, it is fixed point 18.14 +StepperMotor* StepperMotor::set_speed( float speed ) { + if(speed < minimum_step_rate) { + speed= minimum_step_rate; + } - // FIXME NOTE this can cause axis to run faster than expected thus making the line incorrect, or on a delta make the effector move wrong - // seems we can do... minimum_speed = ceil(step_ticker->frequency/65536.0F), which would be 2 not 20 at 100Khz - if (speed < 20.0F) - speed = 20.0F; + // if(speed <= 0.0F) { // we can't actually do 0 but we can get close, need to avoid divide by zero later on + // this->fx_ticks_per_step= 0xFFFFFFFFUL; // 0.381 steps/sec + // this->steps_per_second = THEKERNEL->step_ticker->get_frequency() / (this->fx_ticks_per_step >> fx_shift); + // return; + // } // How many steps we must output per second this->steps_per_second = speed; - // How many ticks ( base steps ) between each actual step at this speed, in fixed point 64 <--- REALLY? I don't think it is at the moment looks like 32bit fixed point - float ticks_per_step = (float)( (float)this->step_ticker->frequency / speed ); - //float double_fx_ticks_per_step = (float)(1<<8) * ( (float)(1<<8) * ticks_per_step ); // 8x8 because we had to do 16x16 because 32 did not work - float double_fx_ticks_per_step = 65536.0F * ticks_per_step; // isn't this better on a 32bit machine? - this->fx_ticks_per_step = (uint32_t)( floor(double_fx_ticks_per_step) ); + // set the new speed, NOTE this can be pre-empted by stepticker so the following write needs to be atomic + this->fx_ticks_per_step= floor(fx_increment * THEKERNEL->step_ticker->get_frequency() / speed); + return this; } -// Pause this stepper motor -void StepperMotor::pause() -{ - this->paused = true; - this->update_exit_tick(); -} - -// Unpause this stepper motor -void StepperMotor::unpause() -{ - this->paused = false; - this->update_exit_tick(); -} - - void StepperMotor::change_steps_per_mm(float new_steps) { steps_per_mm = new_steps; diff --git a/src/libs/StepperMotor.h b/src/libs/StepperMotor.h index a06b7446..893dbcce 100644 --- a/src/libs/StepperMotor.h +++ b/src/libs/StepperMotor.h @@ -10,6 +10,8 @@ #include "libs/Hook.h" #include "Pin.h" +#include +#include class StepTicker; class Hook; @@ -18,29 +20,32 @@ class StepperMotor { public: StepperMotor(); StepperMotor(Pin& step, Pin& dir, Pin& en); - + ~StepperMotor(); void step(); inline void unstep() { step_pin.set(0); }; inline void enable(bool state) { en_pin.set(!state); }; - bool is_moving() { return moving; } + bool is_moving() const { return moving; } + bool which_direction() const { return direction; } void move_finished(); - void move( bool direction, unsigned int steps ); + StepperMotor* move( bool direction, unsigned int steps, float initial_speed= -1.0F); void signal_move_finished(); - void set_speed( float speed ); + StepperMotor* set_speed( float speed ); + void set_moved_last_block(bool flg) { last_step_tick_valid= flg; } void update_exit_tick(); - void pause(); - void unpause(); float get_steps_per_second() const { return steps_per_second; } - void set_steps_per_second(float ss) { steps_per_second= ss; } float get_steps_per_mm() const { return steps_per_mm; } void change_steps_per_mm(float); void change_last_milestone(float); float get_last_milestone(void) const { return last_milestone_mm; } float get_current_position(void) const { return (float)current_position_steps/steps_per_mm; } + float get_max_rate(void) const { return max_rate; } + void set_max_rate(float mr) { max_rate= mr; } + float get_min_rate(void) const { return minimum_step_rate; } + void set_min_rate(float mr) { minimum_step_rate= mr; } int steps_to_target(float); uint32_t get_steps_to_move() const { return steps_to_move; } @@ -52,12 +57,6 @@ class StepperMotor { this->end_hook = hook; } - template void attach_signal_step(uint32_t step, T *optr, uint32_t ( T::*fptr )( uint32_t ) ){ - this->step_signal_hook->attach(optr, fptr); - this->signal_step_number = step; - this->signal_step = true; - } - friend class StepTicker; friend class Stepper; friend class Planner; @@ -65,19 +64,19 @@ class StepperMotor { private: void init(); - Hook* end_hook; - Hook* step_signal_hook; - uint32_t signal_step_number; + int index; + Hook* end_hook; - StepTicker* step_ticker; Pin step_pin; Pin dir_pin; Pin en_pin; float steps_per_second; float steps_per_mm; - float max_rate; + float max_rate; // this is not really rate it is in mm/sec, misnamed used in Robot and Extruder + float minimum_step_rate; // this is the minimum step_rate in steps/sec for this motor for this block + static float default_minimum_actuator_rate; volatile int32_t current_position_steps; int32_t last_milestone_steps; @@ -85,26 +84,33 @@ class StepperMotor { uint32_t steps_to_move; uint32_t stepped; + uint32_t last_step_tick; + uint32_t signal_step; + + // set to 32 bit fixed point, 18:14 bits fractional + static const uint32_t fx_shift= 14; + static const uint32_t fx_increment= ((uint32_t)1<= fx_ticks_per_step) + // if we are to step now + if (fx_counter >= fx_ticks_per_step){ step(); + return true; + } + return false; }; }; diff --git a/src/libs/Vector3.cpp b/src/libs/Vector3.cpp index 31298939..71a7daee 100644 --- a/src/libs/Vector3.cpp +++ b/src/libs/Vector3.cpp @@ -3,49 +3,11 @@ #include #include -float Vector3::nan = NAN; - -Vector3::Vector3() -{ - elem[0] = elem[1] = elem[2] = 0.0F; -} - -Vector3::Vector3(float a, float b, float c) -{ - elem[0] = a; - elem[1] = b; - elem[2] = c; -} - -Vector3::Vector3(const Vector3 &to_copy) -{ - elem[0] = to_copy.elem[0]; - elem[1] = to_copy.elem[1]; - elem[2] = to_copy.elem[2]; -} - -Vector3& Vector3::operator= (const Vector3 &to_copy) -{ - if( this != &to_copy ) { - elem[0] = to_copy.elem[0]; - elem[1] = to_copy.elem[1]; - elem[2] = to_copy.elem[2]; - } - return *this; -} - float Vector3::operator[](int i) const { if (i >= 0 && i <= 2) return elem[i]; - return nan; -} - -void Vector3::set(float a, float b, float c) -{ - elem[0] = a; - elem[1] = b; - elem[2] = c; + return NAN; } Vector3 Vector3::cross(const Vector3 &vec) const @@ -111,17 +73,6 @@ Vector3 Vector3::mul(float scalar) const return out; } -Vector3 Vector3::mul(const Vector3& v) const -{ - Vector3 out; - - out.elem[0] = elem[0] * v[0]; - out.elem[1] = elem[1] * v[1]; - out.elem[2] = elem[2] * v[2]; - - return out; -} - Vector3 Vector3::unit() const { Vector3 out; diff --git a/src/libs/Vector3.h b/src/libs/Vector3.h index 3ea58529..eb933180 100644 --- a/src/libs/Vector3.h +++ b/src/libs/Vector3.h @@ -4,13 +4,11 @@ class Vector3 { public: - Vector3(); - Vector3(float, float, float); - Vector3(const Vector3& to_copy); - Vector3& operator= (const Vector3& to_copy); + Vector3() = default; + Vector3(float a, float b, float c) : elem{a,b,c} {} + Vector3(const Vector3& to_copy) = default; float operator[](int) const; - void set(float a, float b, float c); Vector3 cross(const Vector3&) const; float dot(const Vector3&) const; @@ -22,13 +20,13 @@ public: Vector3 sub(const Vector3&) const; Vector3 mul(float) const; - Vector3 mul(const Vector3& v) const; Vector3 unit(void) const; + float * data() { return elem; } + float const* data() const { return elem; } private: - float elem[3]; - static float nan; + float elem[3]{}; }; // typedef float Vector3[3]; diff --git a/src/libs/Watchdog.cpp b/src/libs/Watchdog.cpp index 1bee2648..0f725b93 100644 --- a/src/libs/Watchdog.cpp +++ b/src/libs/Watchdog.cpp @@ -1,9 +1,13 @@ #include "Watchdog.h" +#include "Kernel.h" #include #include +#include "gpio.h" +extern GPIO leds[]; + // TODO : comment this // Basically, when stuff stop answering, reset, or enter MRI mode, or something @@ -12,6 +16,11 @@ Watchdog::Watchdog(uint32_t timeout, WDT_ACTION action) WDT_Init(WDT_CLKSRC_IRC, (action == WDT_MRI)?WDT_MODE_INT_ONLY:WDT_MODE_RESET); WDT_Start(timeout); WDT_Feed(); + if(action == WDT_MRI) { + // enable the interrupt + NVIC_EnableIRQ(WDT_IRQn); + NVIC_SetPriority(WDT_IRQn, 1); + } } void Watchdog::feed() @@ -31,9 +40,19 @@ void Watchdog::on_idle(void*) } +// when watchdog triggers, set a led pattern and enter MRI which turns everything off into a safe state +// TODO handle when MRI is disabled extern "C" void WDT_IRQHandler(void) { - WDT_ClrTimeOutFlag(); + if(THEKERNEL->is_using_leds()) { + // set led pattern to show we are in watchdog timeout + leds[0]= 0; + leds[1]= 1; + leds[2]= 0; + leds[3]= 1; + } + + WDT_ClrTimeOutFlag(); // bootloader uses this flag to enter DFU mode WDT_Feed(); __debugbreak(); } diff --git a/src/libs/md5.cpp b/src/libs/md5.cpp index 3fc27fd0..9ed1d970 100644 --- a/src/libs/md5.cpp +++ b/src/libs/md5.cpp @@ -35,6 +35,7 @@ documentation and/or software. /* system implementation headers */ #include +#include // Constants for MD5Transform routine. #define S11 7 @@ -338,18 +339,18 @@ MD5 &MD5::finalize() ////////////////////////////// // return hex representation of digest as string -// std::string MD5::hexdigest() const -// { -// if (!finalized) -// return ""; +std::string MD5::hexdigest() const +{ + if (!finalized) + return ""; -// char buf[33]; -// for (int i = 0; i < 16; i++) -// sprintf(buf + i * 2, "%02x", digest[i]); -// buf[32] = 0; + char buf[33]; + for (int i = 0; i < 16; i++) + sprintf(buf + i * 2, "%02x", digest[i]); + buf[32] = 0; -// return std::string(buf); -// } + return std::string(buf); +} // return the slected number of bytes from the digest void MD5::bindigest(void *buf, int len) const @@ -359,9 +360,9 @@ void MD5::bindigest(void *buf, int len) const ////////////////////////////// -std::string md5(const std::string str) -{ - MD5 md5 = MD5(str); +// std::string md5(const std::string str) +// { +// MD5 md5 = MD5(str); - return md5.hexdigest(); -} +// return md5.hexdigest(); +// } diff --git a/src/libs/utils.cpp b/src/libs/utils.cpp index de174c64..45665d68 100644 --- a/src/libs/utils.cpp +++ b/src/libs/utils.cpp @@ -14,9 +14,9 @@ #include #include #include -using std::string; +#include -volatile bool _isr_context = false; +using std::string; uint16_t get_checksum(const string &to_check) { @@ -168,6 +168,7 @@ void system_reset( bool dfu ) } // Convert a path indication ( absolute or relative ) into a path ( absolute ) +// TODO: Combine with plan9 absolute_path, current_path as argument? string absolute_from_relative( string path ) { string cwd = THEKERNEL->current_path; @@ -198,3 +199,56 @@ string absolute_from_relative( string path ) return cwd + '/' + path; } + +// FIXME this does not handle empty strings correctly +//split a string on a delimiter, return a vector of the split tokens +vector split(const char *str, char c) +{ + vector result; + + do { + const char *begin = str; + + while(*str != c && *str) + str++; + + result.push_back(string(begin, str)); + } while (0 != *str++); + + return result; +} + +// FIXME this does not handle empty strings correctly +// parse a number list "1.1,2.2,3.3" and return the numbers in a vector of floats +vector parse_number_list(const char *str) +{ + vector l= split(str, ','); + vector r; + for(auto& s : l){ + float x = strtof(s.c_str(), nullptr); + r.push_back(x); + } + return r; +} + +vector parse_number_list(const char *str, uint8_t radix) +{ + vector l= split(str, ','); + vector r; + for(auto& s : l){ + uint32_t x = strtol(s.c_str(), nullptr, radix); + r.push_back(x); + } + return r; +} + +int append_parameters(char *buf, std::vector> params, size_t bufsize) +{ + size_t n= 0; + for(auto &i : params) { + if(n >= bufsize) break; + buf[n++]= i.first; + n += snprintf(&buf[n], bufsize-n, "%1.4f ", i.second); + } + return n; +} diff --git a/src/libs/utils.h b/src/libs/utils.h index a9ad3ff3..5465da52 100644 --- a/src/libs/utils.h +++ b/src/libs/utils.h @@ -1,13 +1,12 @@ -#ifndef utils_h -#define utils_h +#ifndef UTILS_H +#define UTILS_H #include -using namespace std; #include #include -using std::string; -extern volatile bool _isr_context; +using std::string; +using std::vector; string lc(const string& str); @@ -17,6 +16,10 @@ bool is_numeric( int ); bool is_alphanum( int ); bool is_whitespace( int ); +vector split(const char *str, char c = ','); +vector parse_number_list(const char *str); +vector parse_number_list(const char *str, uint8_t radix); + string remove_non_number( string str ); uint16_t get_checksum(const string& to_check); @@ -34,5 +37,6 @@ void system_reset( bool dfu= false ); string absolute_from_relative( string path ); +int append_parameters(char *buf, std::vector> params, size_t bufsize); #endif diff --git a/src/main.cpp b/src/main.cpp index 56cce1f3..9a2f2a38 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -16,19 +16,23 @@ #include "modules/tools/scaracal/SCARAcal.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/pausebutton/PauseButton.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" +#include "StepTicker.h" // #include "libs/ChaNFSSD/SDFileSystem.h" #include "libs/nuts_bolts.h" @@ -55,10 +59,9 @@ #define second_usb_serial_enable_checksum CHECKSUM("second_usb_serial_enable") #define disable_msd_checksum CHECKSUM("msd_disable") -#define disable_leds_checksum CHECKSUM("leds_disable") #define dfu_enable_checksum CHECKSUM("dfu_enable") +#define watchdog_timeout_checksum CHECKSUM("watchdog_timeout") -// Watchdog wd(5000000, WDT_MRI); // USB Stuff SDCard sd __attribute__ ((section ("AHBSRAM0"))) (P0_9, P0_8, P0_7, P0_6); // this selects SPI1 as the sdcard as it is on Smoothieboard @@ -83,6 +86,11 @@ GPIO leds[5] = { GPIO(P4_28) }; +// debug pins, only used if defined in src/makefile +#ifdef STEPTICKER_DEBUG_PIN +GPIO stepticker_debug_pin(STEPTICKER_DEBUG_PIN); +#endif + void init() { // Default pins to low status @@ -91,15 +99,17 @@ void init() { leds[i]= 0; } +#ifdef STEPTICKER_DEBUG_PIN + stepticker_debug_pin.output(); + stepticker_debug_pin= 0; +#endif + Kernel* kernel = new Kernel(); kernel->streams->printf("Smoothie Running @%ldMHz\r\n", SystemCoreClock / 1000000); Version version; kernel->streams->printf(" Build version %s, Build date %s\r\n", version.get_build(), version.get_build_date()); - //some boards don't have leds.. TOO BAD! - kernel->use_leds= !kernel->config->value( disable_leds_checksum )->by_default(false)->as_bool(); - bool sdok= (sd.disk_initialize() == 0); if(!sdok) kernel->streams->printf("SDCard is disabled\r\n"); @@ -122,7 +132,7 @@ void init() { kernel->add_module( new SimpleShell() ); kernel->add_module( new Configurator() ); kernel->add_module( new CurrentControl() ); - kernel->add_module( new PauseButton() ); + kernel->add_module( new KillButton() ); kernel->add_module( new PlayLed() ); kernel->add_module( new Endstops() ); kernel->add_module( new Player() ); @@ -168,10 +178,18 @@ void init() { kernel->add_module( new Network() ); #endif #ifndef NO_TOOLS_TEMPERATURESWITCH - // Must be loaded after TemperatureControlPool + // 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(); @@ -191,12 +209,24 @@ void init() { if( kernel->config->value( dfu_enable_checksum )->by_default(false)->as_bool() ){ kernel->add_module( new(AHB0) DFU(&u)); } + + // 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 ); // clear up the config cache to save some memory kernel->config->config_cache_clear(); - if(kernel->use_leds) { + if(kernel->is_using_leds()) { // set some leds to indicate status... led0 init doe, led1 mainloop running, led2 idle loop running, led3 sdcard ok leds[0]= 1; // indicate we are done with init leds[3]= sdok?1:0; // 4th led inidicates sdcard is available (TODO maye should indicate config was found) @@ -219,6 +249,8 @@ void init() { fclose(fp); } } + + THEKERNEL->step_ticker->start(); } int main() @@ -228,7 +260,7 @@ int main() uint16_t cnt= 0; // Main loop while(1){ - if(THEKERNEL->use_leds) { + if(THEKERNEL->is_using_leds()) { // flash led 2 to show we are alive leds[1]= (cnt++ & 0x1000) ? 1 : 0; } diff --git a/src/makefile b/src/makefile index 54524d54..047b097c 100644 --- a/src/makefile +++ b/src/makefile @@ -53,11 +53,23 @@ endif # use c++11 features for the checksums and set default baud rate for serial uart DEFINES += -DCHECKSUM_USE_CPP -DDEFAULT_SERIAL_BAUD_RATE=$(DEFAULT_SERIAL_BAUD_RATE) +ifneq "$(STEPTICKER_DEBUG_PIN)" "" +# Set a Pin here that toggles on end of move +DEFINES += -DSTEPTICKER_DEBUG_PIN=$(STEPTICKER_DEBUG_PIN) +endif + # add any modules that you do not want included in the build -export EXCLUDED_MODULES = tools/touchprobe +EXCLUDE_MODULES = tools/touchprobe # e.g for a CNC machine #export EXCLUDED_MODULES = tools/touchprobe tools/laser tools/temperaturecontrol tools/extruder +ifneq "$(INCLUDE_MODULE)" "" +export EXCLUDED_MODULES = $(filter-out $(INCLUDE_MODULE),$(EXCLUDE_MODULES)) +else +export EXCLUDED_MODULES = $(EXCLUDE_MODULES) +endif + + # set to not compile in any network support #export NONETWORK = 1 diff --git a/src/modules/communication/GcodeDispatch.cpp b/src/modules/communication/GcodeDispatch.cpp index e58582ae..2e5a8a50 100644 --- a/src/modules/communication/GcodeDispatch.cpp +++ b/src/modules/communication/GcodeDispatch.cpp @@ -5,27 +5,32 @@ You should have received a copy of the GNU General Public License along with Smoothie. If not, see . */ -#include -using std::string; -#include "libs/Module.h" +#include "GcodeDispatch.h" + #include "libs/Kernel.h" #include "utils/Gcode.h" -#include "Pauser.h" #include "libs/nuts_bolts.h" -#include "GcodeDispatch.h" #include "modules/robot/Conveyor.h" #include "libs/SerialMessage.h" #include "libs/StreamOutput.h" #include "libs/StreamOutputPool.h" #include "libs/FileStream.h" +#include "libs/AppendFileStream.h" #include "Config.h" #include "checksumm.h" #include "ConfigValue.h" +#include "PublicDataRequest.h" +#include "PublicData.h" +#include "SimpleShell.h" +#include "utils.h" +#include "LPC17xx.h" #define return_error_on_unhandled_gcode_checksum CHECKSUM("return_error_on_unhandled_gcode") +#define panel_display_message_checksum CHECKSUM("display_message") +#define panel_checksum CHECKSUM("panel") // goes in Flash, list of Mxxx codes that are allowed when in Halted state -static const int allowed_mcodes[]= {105,114}; // get temp, get pos +static const int allowed_mcodes[]= {105,114,119,80,81,911,503,106,107}; // get temp, get pos, get endstops etc static bool is_allowed_mcode(int m) { for (size_t i = 0; i < sizeof(allowed_mcodes)/sizeof(int); ++i) { if(allowed_mcodes[i] == m) return true; @@ -35,7 +40,6 @@ static bool is_allowed_mcode(int m) { GcodeDispatch::GcodeDispatch() { - halted= false; uploading = false; currentline = -1; last_g= 255; @@ -44,15 +48,7 @@ GcodeDispatch::GcodeDispatch() // Called when the module has just been loaded void GcodeDispatch::on_module_loaded() { - return_error_on_unhandled_gcode = THEKERNEL->config->value( return_error_on_unhandled_gcode_checksum )->by_default(false)->as_bool(); this->register_for_event(ON_CONSOLE_LINE_RECEIVED); - this->register_for_event(ON_HALT); -} - -void GcodeDispatch::on_halt(void *arg) -{ - // set halt stream and ignore everything until M999 - this->halted= (arg == nullptr); } // When a command is received, if it is a Gcode, dispatch it as an object via an event @@ -119,7 +115,8 @@ try_again: } while(possible_command.size() > 0) { - size_t nextcmd = possible_command.find_first_of("GM", possible_command.find_first_of("GM") + 1); + // assumes G or M are always the first on the line + size_t nextcmd = possible_command.find_first_of("GM", 2); string single_command; if(nextcmd == string::npos) { single_command = possible_command; @@ -134,11 +131,11 @@ try_again: //Prepare gcode for dispatch Gcode *gcode = new Gcode(single_command, new_message.stream); - if(halted) { + 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 - halted= false; + // fall through and pass onto other modules }else if(!is_allowed_mcode(gcode->m)) { @@ -163,9 +160,9 @@ try_again: upload_fd = fopen(this->upload_filename.c_str(), "w"); if(upload_fd != NULL) { this->uploading = true; - new_message.stream->printf("Writing to file: %s\r\n", this->upload_filename.c_str()); + new_message.stream->printf("Writing to file: %s\r\nok\r\n", this->upload_filename.c_str()); } else { - new_message.stream->printf("open failed, File: %s.\r\n", this->upload_filename.c_str()); + new_message.stream->printf("open failed, File: %s.\r\nok\r\n", this->upload_filename.c_str()); } //printf("Start Uploading file: %s, %p\n", upload_filename.c_str(), upload_fd); continue; @@ -178,20 +175,62 @@ try_again: delete gcode; return; + case 117: // M117 is a special non compliant Gcode as it allows arbitrary text on the line following the command + { // concatenate the command again and send to panel if enabled + string str= single_command.substr(4) + possible_command; + PublicData::set_value( panel_checksum, panel_display_message_checksum, &str ); + delete gcode; + new_message.stream->printf("ok\r\n"); + return; + } + + case 1000: // M1000 is a special comanad that will pass thru the raw lowercased command to the simpleshell (for hosts that do not allow such things) + { + // reconstruct entire command line again + string str= single_command.substr(5) + possible_command; + while(is_whitespace(str.front())){ str= str.substr(1); } // strip leading whitespace + + delete gcode; + + if(str.empty()) { + SimpleShell::parse_command("help", "", new_message.stream); + + }else{ + string args= lc(str); + string cmd = shift_parameter(args); + // find command and execute it + if(!SimpleShell::parse_command(cmd.c_str(), args, new_message.stream)) { + new_message.stream->printf("Command not found: %s\n", cmd.c_str()); + } + } + + new_message.stream->printf("ok\r\n"); + return; + } + case 500: // M500 save volatile settings to config-override + THEKERNEL->conveyor->wait_for_empty_queue(); //just to be safe as it can take a while to run + //remove(THEKERNEL->config_override_filename()); // seems to cause a hang every now and then + __disable_irq(); + { + FileStream fs(THEKERNEL->config_override_filename()); + fs.printf("; DO NOT EDIT THIS FILE\n"); + // this also will truncate the existing file instead of deleting it + } // replace stream with one that writes to config-override file - gcode->stream = new FileStream(THEKERNEL->config_override_filename()); + gcode->stream = new AppendFileStream(THEKERNEL->config_override_filename()); // dispatch the M500 here so we can free up the stream when done THEKERNEL->call_event(ON_GCODE_RECEIVED, gcode ); delete gcode->stream; delete gcode; + __enable_irq(); new_message.stream->printf("Settings Stored to %s\r\nok\r\n", THEKERNEL->config_override_filename()); continue; case 502: // M502 deletes config-override so everything defaults to what is in config remove(THEKERNEL->config_override_filename()); - new_message.stream->printf("config override file deleted %s, reboot needed\r\nok\r\n", THEKERNEL->config_override_filename()); delete gcode; + new_message.stream->printf("config override file deleted %s, reboot needed\r\nok\r\n", THEKERNEL->config_override_filename()); continue; case 503: { // M503 display live settings and indicates if there is an override file @@ -215,9 +254,7 @@ try_again: if(gcode->add_nl) new_message.stream->printf("\r\n"); - if( return_error_on_unhandled_gcode == true && gcode->accepted_by_module == false) - new_message.stream->printf("ok (command unclaimed)\r\n"); - else if(!gcode->txt_after_ok.empty()) { + if(!gcode->txt_after_ok.empty()) { new_message.stream->printf("ok %s\r\n", gcode->txt_after_ok.c_str()); gcode->txt_after_ok.clear(); } else @@ -233,7 +270,7 @@ try_again: upload_fd = NULL; uploading = false; upload_filename.clear(); - new_message.stream->printf("Done saving file.\r\n"); + new_message.stream->printf("Done saving file.\r\nok\r\n"); continue; } diff --git a/src/modules/communication/GcodeDispatch.h b/src/modules/communication/GcodeDispatch.h index e487b846..0fcdbd4e 100644 --- a/src/modules/communication/GcodeDispatch.h +++ b/src/modules/communication/GcodeDispatch.h @@ -8,9 +8,11 @@ #ifndef GCODE_DISPATCH_H #define GCODE_DISPATCH_H +#include "libs/Module.h" + +#include #include using std::string; -#include "libs/Module.h" class GcodeDispatch : public Module { @@ -19,7 +21,6 @@ public: virtual void on_module_loaded(); virtual void on_console_line_received(void *line); - void on_halt(void *arg); private: int currentline; @@ -28,8 +29,6 @@ private: uint8_t last_g; struct { bool uploading: 1; - bool halted: 1; - bool return_error_on_unhandled_gcode:1; }; }; diff --git a/src/modules/communication/SerialConsole.cpp b/src/modules/communication/SerialConsole.cpp index 301937df..88f08940 100644 --- a/src/modules/communication/SerialConsole.cpp +++ b/src/modules/communication/SerialConsole.cpp @@ -29,9 +29,12 @@ SerialConsole::SerialConsole( PinName rx_pin, PinName tx_pin, int baud_rate ){ void SerialConsole::on_module_loaded() { // We want to be called every time a new char is received this->serial->attach(this, &SerialConsole::on_serial_char_received, mbed::Serial::RxIrq); + query_flag= false; + halt_flag= false; // We only call the command dispatcher in the main loop, nowhere else this->register_for_event(ON_MAIN_LOOP); + this->register_for_event(ON_IDLE); // Add to the pack of streams kernel can call to, for example for broadcasting THEKERNEL->streams->append_stream(this); @@ -41,12 +44,32 @@ void SerialConsole::on_module_loaded() { void SerialConsole::on_serial_char_received(){ while(this->serial->readable()){ char received = this->serial->getc(); + if(received == '?') { + query_flag= true; + continue; + } + if(received == 'X'-'A') { // ^X + halt_flag= true; + continue; + } // convert CR to NL (for host OSs that don't send NL) if( received == '\r' ){ received = '\n'; } this->buffer.push_back(received); } } +void SerialConsole::on_idle(void * argument) +{ + if(query_flag) { + query_flag= false; + puts(THEKERNEL->get_query_string().c_str()); + } + if(halt_flag) { + halt_flag= false; + THEKERNEL->call_event(ON_HALT, nullptr); + } +} + // Actual event calling must happen in the main loop because if it happens in the interrupt we will loose data void SerialConsole::on_main_loop(void * argument){ if( this->has_char('\n') ){ diff --git a/src/modules/communication/SerialConsole.h b/src/modules/communication/SerialConsole.h index 6dfbcb4c..49489db5 100644 --- a/src/modules/communication/SerialConsole.h +++ b/src/modules/communication/SerialConsole.h @@ -27,6 +27,7 @@ class SerialConsole : public Module, public StreamOutput { void on_module_loaded(); void on_serial_char_received(); void on_main_loop(void * argument); + void on_idle(void * argument); bool has_char(char letter); int _putc(int c); @@ -37,6 +38,10 @@ class SerialConsole : public Module, public StreamOutput { //vector received_lines; // Received lines are stored here until they are requested RingBuffer buffer; // Receive buffer mbed::Serial* serial; + struct { + bool query_flag:1; + bool halt_flag:1; + }; }; #endif diff --git a/src/modules/communication/utils/Gcode.cpp b/src/modules/communication/utils/Gcode.cpp index 328ad93a..4cb88a9d 100644 --- a/src/modules/communication/utils/Gcode.cpp +++ b/src/modules/communication/utils/Gcode.cpp @@ -19,11 +19,12 @@ Gcode::Gcode(const string &command, StreamOutput *stream, bool strip) this->command= strdup(command.c_str()); this->m= 0; this->g= 0; + this->subcode= 0; this->add_nl= false; this->stream= stream; this->millimeters_of_travel = 0.0F; - this->accepted_by_module = false; prepare_cached_values(strip); + this->stripped= strip; } Gcode::~Gcode() @@ -42,9 +43,9 @@ Gcode::Gcode(const Gcode &to_copy) this->has_g = to_copy.has_g; this->m = to_copy.m; this->g = to_copy.g; + this->subcode = to_copy.subcode; this->add_nl = to_copy.add_nl; this->stream = to_copy.stream; - this->accepted_by_module = false; this->txt_after_ok.assign( to_copy.txt_after_ok ); } @@ -57,11 +58,11 @@ Gcode &Gcode::operator= (const Gcode &to_copy) this->has_g = to_copy.has_g; this->m = to_copy.m; this->g = to_copy.g; + this->subcode = to_copy.subcode; this->add_nl = to_copy.add_nl; this->stream = to_copy.stream; this->txt_after_ok.assign( to_copy.txt_after_ok ); } - this->accepted_by_module = false; return *this; } @@ -132,14 +133,41 @@ uint32_t Gcode::get_uint( char letter, char **ptr ) const int Gcode::get_num_args() const { int count = 0; - for(size_t i = 1; i < strlen(command); i++) { + for(size_t i = stripped?0:1; i < strlen(command); i++) { if( this->command[i] >= 'A' && this->command[i] <= 'Z' ) { + if(this->command[i] == 'T') continue; count++; } } return count; } +std::map Gcode::get_args() const +{ + std::map m; + for(size_t i = stripped?0:1; i < strlen(command); i++) { + char c= this->command[i]; + if( c >= 'A' && c <= 'Z' ) { + if(c == 'T') continue; + m[c]= get_value(c); + } + } + return m; +} + +std::map Gcode::get_args_int() const +{ + std::map m; + for(size_t i = stripped?0:1; i < strlen(command); i++) { + char c= this->command[i]; + if( c >= 'A' && c <= 'Z' ) { + if(c == 'T') continue; + m[c]= get_int(c); + } + } + return m; +} + // Cache some of this command's properties, so we don't have to parse the string every time we want to look at them void Gcode::prepare_cached_values(bool strip) { @@ -147,16 +175,29 @@ void Gcode::prepare_cached_values(bool strip) if( this->has_letter('G') ) { this->has_g = true; this->g = this->get_int('G', &p); + } else { this->has_g = false; } + if( this->has_letter('M') ) { this->has_m = true; this->m = this->get_int('M', &p); + } else { this->has_m = false; } + if(has_g || has_m) { + // look for subcode and extract it + if(p != nullptr && *p == '.') { + this->subcode = strtoul(p+1, &p, 10); + + }else{ + this->subcode= 0; + } + } + if(!strip) return; // remove the Gxxx or Mxxx from string @@ -167,11 +208,6 @@ void Gcode::prepare_cached_values(bool strip) } } -void Gcode::mark_as_taken() -{ - this->accepted_by_module = true; -} - // strip off X Y Z I J K parameters if G0/1/2/3 void Gcode::strip_parameters() { diff --git a/src/modules/communication/utils/Gcode.h b/src/modules/communication/utils/Gcode.h index ed5b93a0..02dc442d 100644 --- a/src/modules/communication/utils/Gcode.h +++ b/src/modules/communication/utils/Gcode.h @@ -9,6 +9,8 @@ #ifndef GCODE_H #define GCODE_H #include +#include + using std::string; class StreamOutput; @@ -27,7 +29,8 @@ class Gcode { int get_int ( char letter, char **ptr= nullptr ) const; uint32_t get_uint ( char letter, char **ptr= nullptr ) const; int get_num_args() const; - void mark_as_taken(); + std::map get_args() const; + std::map get_args_int() const; void strip_parameters(); // FIXME these should be private @@ -39,7 +42,8 @@ class Gcode { bool add_nl:1; bool has_m:1; bool has_g:1; - bool accepted_by_module:1; + bool stripped:1; + uint8_t subcode:3; }; StreamOutput* stream; diff --git a/src/modules/robot/Block.cpp b/src/modules/robot/Block.cpp index 9090b765..e0d798f2 100644 --- a/src/modules/robot/Block.cpp +++ b/src/modules/robot/Block.cpp @@ -48,6 +48,7 @@ void Block::clear() entry_speed = 0.0F; exit_speed = 0.0F; rate_delta = 0.0F; + acceleration = 100.0F; // we don't want to get devide by zeroes if this is not set initial_rate = -1; final_rate = -1; accelerate_until = 0; @@ -102,13 +103,13 @@ void Block::calculate_trapezoid( float entryspeed, float exitspeed ) return; // The planner passes us factors, we need to transform them in rates - this->initial_rate = ceil(this->nominal_rate * entryspeed / this->nominal_speed); // (step/s) - this->final_rate = ceil(this->nominal_rate * exitspeed / this->nominal_speed); // (step/s) + this->initial_rate = ceilf(this->nominal_rate * entryspeed / this->nominal_speed); // (step/s) + this->final_rate = ceilf(this->nominal_rate * exitspeed / this->nominal_speed); // (step/s) // How many steps to accelerate and decelerate - float acceleration_per_second = this->rate_delta * THEKERNEL->stepper->get_acceleration_ticks_per_second(); // ( step/s^2) - int accelerate_steps = ceil( this->estimate_acceleration_distance( this->initial_rate, this->nominal_rate, acceleration_per_second ) ); - int decelerate_steps = floor( this->estimate_acceleration_distance( this->nominal_rate, this->final_rate, -acceleration_per_second ) ); + float acceleration_per_second = this->rate_delta * THEKERNEL->acceleration_ticks_per_second; // ( step/s^2) + int accelerate_steps = ceilf( this->estimate_acceleration_distance( this->initial_rate, this->nominal_rate, acceleration_per_second ) ); + int decelerate_steps = floorf( this->estimate_acceleration_distance( this->nominal_rate, this->final_rate, -acceleration_per_second ) ); // Calculate the size of Plateau of Nominal Rate ( during which we don't accelerate nor decelerate, but just cruise ) int plateau_steps = this->steps_event_count - accelerate_steps - decelerate_steps; @@ -117,7 +118,7 @@ void Block::calculate_trapezoid( float entryspeed, float exitspeed ) // have to use intersection_distance() to calculate when to abort acceleration and start braking // in order to reach the final_rate exactly at the end of this block. if (plateau_steps < 0) { - accelerate_steps = ceil(this->intersection_distance(this->initial_rate, this->final_rate, acceleration_per_second, this->steps_event_count)); + accelerate_steps = ceilf(this->intersection_distance(this->initial_rate, this->final_rate, acceleration_per_second, this->steps_event_count)); accelerate_steps = max( accelerate_steps, 0 ); // Check limits due to numerical round-off accelerate_steps = min( accelerate_steps, int(this->steps_event_count) ); plateau_steps = 0; @@ -174,7 +175,7 @@ float Block::reverse_pass(float exit_speed) // for max allowable speed if block is decelerating and nominal length is false. if ((!this->nominal_length_flag) && (this->max_entry_speed > exit_speed)) { - float max_entry_speed = max_allowable_speed(-THEKERNEL->planner->get_acceleration(), exit_speed, this->millimeters); + float max_entry_speed = max_allowable_speed(-this->acceleration, exit_speed, this->millimeters); this->entry_speed = min(max_entry_speed, this->max_entry_speed); @@ -231,7 +232,7 @@ float Block::max_exit_speed() return nominal_speed; // otherwise, we have to work out max exit speed based on entry and acceleration - float max = max_allowable_speed(-THEKERNEL->planner->get_acceleration(), this->entry_speed, this->millimeters); + float max = max_allowable_speed(-this->acceleration, this->entry_speed, this->millimeters); return min(max, nominal_speed); } @@ -257,6 +258,7 @@ void Block::begin() for(unsigned int index = 0; index < gcodes.size(); index++) THEKERNEL->call_event(ON_GCODE_EXECUTE, &(gcodes[index])); + THEKERNEL->call_event(ON_BLOCK_BEGIN, this); if (times_taken < 0) @@ -280,11 +282,9 @@ void Block::take() // Mark the block as no longer taken by one module, go to next block if this free's it void Block::release() { - if (--this->times_taken <= 0) - { + if (--this->times_taken <= 0) { times_taken = 0; - if (is_ready) - { + if (is_ready) { is_ready = false; THEKERNEL->call_event(ON_BLOCK_END, this); diff --git a/src/modules/robot/Block.h b/src/modules/robot/Block.h index f960c90d..f9c034f2 100644 --- a/src/modules/robot/Block.h +++ b/src/modules/robot/Block.h @@ -19,7 +19,6 @@ class Block { void calculate_trapezoid( float entry_speed, float exit_speed ); float estimate_acceleration_distance( float initial_rate, float target_rate, float acceleration ); float intersection_distance(float initial_rate, float final_rate, float acceleration, float distance); - float get_duration_left(unsigned int already_taken_steps); float max_allowable_speed( float acceleration, float target_velocity, float distance); float reverse_pass(float exit_speed); @@ -50,22 +49,22 @@ class Block { float entry_speed; float exit_speed; float rate_delta; // Nomber of steps to add to the speed for each acceleration tick + float acceleration; // the acceleratoin for this block unsigned int initial_rate; // Initial speed in steps per second unsigned int final_rate; // Final speed in steps per second unsigned int accelerate_until; // Stop accelerating after this number of steps unsigned int decelerate_after; // Start decelerating after this number of steps - std::bitset<3> direction_bits; // Direction for each axis in bit form, relative to the direction port's mask + float max_entry_speed; + + short times_taken; // A block can be "taken" by any number of modules, and the next block is not moved to until all the modules have "released" it. This value serves as a tracker. + + std::bitset<3> direction_bits; // Direction for each axis in bit form, relative to the direction port's mask struct { bool recalculate_flag:1; // Planner flag to recalculate trapezoids on entry junction bool nominal_length_flag:1; // Planner flag for nominal speed always reached bool is_ready:1; }; - - float max_entry_speed; - - short times_taken; // A block can be "taken" by any number of modules, and the next block is not moved to until all the modules have "released" it. This value serves as a tracker. - }; diff --git a/src/modules/robot/Conveyor.cpp b/src/modules/robot/Conveyor.cpp index 25922b1e..f8b68736 100644 --- a/src/modules/robot/Conveyor.cpp +++ b/src/modules/robot/Conveyor.cpp @@ -136,7 +136,6 @@ void Conveyor::on_config_reload(void* argument) void Conveyor::append_gcode(Gcode* gcode) { - gcode->mark_as_taken(); queue.head_ref()->append_gcode(gcode); } diff --git a/src/modules/robot/Pauser.cpp b/src/modules/robot/Pauser.cpp deleted file mode 100644 index e9c91167..00000000 --- a/src/modules/robot/Pauser.cpp +++ /dev/null @@ -1,66 +0,0 @@ -#include "Pauser.h" - -#include "libs/Kernel.h" -#include "Block.h" -#include "libs/nuts_bolts.h" -#include "libs/utils.h" - -#include -using namespace std; - -// The Pauser module is the core of the pausing subsystem in smoothie. -// Basically we want several modules to be able to pause smoothie at the same -// time ( think both the user with a button, and the temperature control -// because a temperature is not reached ). To do that, modules call the take() -// methode, a pause event is called, and the pause does not end before all -// modules have called the release() method. -// Please note : Modules should keep track of their pause status themselves... -// or Not for instance it may be better for the play/pause button to be able -// to unpause when something else causes a pause -Pauser::Pauser(){ - paused_block = NULL; -} - -void Pauser::on_module_loaded(){ - this->counter = 0; - register_for_event(ON_BLOCK_BEGIN); -} - -void Pauser::on_block_begin(void* argument) -{ - Block* block = static_cast(argument); - - if (counter) - { - block->take(); - paused_block = block; - } -} - -// Pause smoothie if nobody else is currently doing so -void Pauser::take(){ - this->counter++; - //THEKERNEL->streams->printf("take: %u \r\n", this->counter ); - if( this->counter == 1 ){ - THEKERNEL->call_event(ON_PAUSE, &this->counter); - } -} - -// Unpause smoothie unless something else is pausing it too -void Pauser::release(){ - if( --this->counter <= 0 ){ - this->counter= 0; - THEKERNEL->call_event(ON_PLAY, &this->counter); - if (paused_block) - { - Block* tmp = paused_block; - paused_block = NULL; - tmp->release(); - } - } -} - -// Return wether smoothie is paused -bool Pauser::paused(){ - return (counter != 0); -} diff --git a/src/modules/robot/Pauser.h b/src/modules/robot/Pauser.h deleted file mode 100644 index 57085f14..00000000 --- a/src/modules/robot/Pauser.h +++ /dev/null @@ -1,24 +0,0 @@ -#ifndef PAUSER_H -#define PAUSER_H - -#include "Module.h" - -class Block; - -class Pauser : public Module { - public: - Pauser(); - void on_module_loaded(); - void on_block_begin(void*); - - void take(); - void release(); - - bool paused(); - - private: - Block* paused_block; - unsigned short counter; -}; - -#endif diff --git a/src/modules/robot/Planner.cpp b/src/modules/robot/Planner.cpp index 372e4c9b..e7792c56 100644 --- a/src/modules/robot/Planner.cpp +++ b/src/modules/robot/Planner.cpp @@ -28,7 +28,6 @@ using namespace std; #define acceleration_checksum CHECKSUM("acceleration") #define z_acceleration_checksum CHECKSUM("z_acceleration") -#define max_jerk_checksum CHECKSUM("max_jerk") #define junction_deviation_checksum CHECKSUM("junction_deviation") #define z_junction_deviation_checksum CHECKSUM("z_junction_deviation") #define minimum_planner_speed_checksum CHECKSUM("minimum_planner_speed") @@ -86,6 +85,8 @@ void Planner::append_block( float actuator_pos[], float rate_mm_s, float distanc if(this->z_junction_deviation >= 0.0F) junction_deviation= this->z_junction_deviation; } + block->acceleration= acceleration; // save in block + // Max number of steps, for all axes block->steps_event_count = max( block->steps[ALPHA_STEPPER], max( block->steps[BETA_STEPPER], block->steps[GAMMA_STEPPER] ) ); @@ -95,7 +96,7 @@ void Planner::append_block( float actuator_pos[], float rate_mm_s, float distanc // NOTE: Minimum stepper speed is limited by MINIMUM_STEPS_PER_MINUTE in stepper.c if( distance > 0.0F ){ block->nominal_speed = rate_mm_s; // (mm/s) Always > 0 - block->nominal_rate = ceil(block->steps_event_count * rate_mm_s / distance); // (step/s) Always > 0 + block->nominal_rate = ceilf(block->steps_event_count * rate_mm_s / distance); // (step/s) Always > 0 }else{ block->nominal_speed = 0.0F; block->nominal_rate = 0; @@ -108,7 +109,7 @@ void Planner::append_block( float actuator_pos[], float rate_mm_s, float distanc // To generate trapezoids with contant acceleration between blocks the rate_delta must be computed // specifically for each line to compensate for this phenomenon: // Convert universal acceleration for direction-dependent stepper rate change parameter - block->rate_delta = (block->steps_event_count * acceleration) / (distance * THEKERNEL->stepper->get_acceleration_ticks_per_second()); // (step/min/acceleration_tick) + block->rate_delta = (block->steps_event_count * acceleration) / (distance * THEKERNEL->acceleration_ticks_per_second); // (step/min/acceleration_tick) // Compute maximum allowable entry speed at junction by centripetal acceleration approximation. // Let a circle be tangent to both previous and current path line segments, where the junction @@ -119,6 +120,9 @@ void Planner::append_block( float actuator_pos[], float rate_mm_s, float distanc // path width or max_jerk in the previous grbl version. This approach does not actually deviate // from path, but used as a robust way to compute cornering speeds, as it takes into account the // nonlinearities of both the junction angle and junction velocity. + + // NOTE however it does not take into account independent axis, in most cartesian X and Y and Z are totally independent + // and this allows one to stop with little to no decleration in many cases. This is particualrly bad on leadscrew based systems that will skip steps. float vmax_junction = minimum_planner_speed; // Set default max junction speed if (!THEKERNEL->conveyor->is_queue_empty()) @@ -147,7 +151,7 @@ void Planner::append_block( float actuator_pos[], float rate_mm_s, float distanc block->max_entry_speed = vmax_junction; // Initialize block entry speed. Compute based on deceleration to user-defined minimum_planner_speed. - float v_allowable = max_allowable_speed(-acceleration, minimum_planner_speed, block->millimeters); //TODO: Get from config + float v_allowable = max_allowable_speed(-acceleration, minimum_planner_speed, block->millimeters); block->entry_speed = min(vmax_junction, v_allowable); // Initialize planner efficiency flags diff --git a/src/modules/robot/Robot.cpp b/src/modules/robot/Robot.cpp index a841693d..aaf986e9 100644 --- a/src/modules/robot/Robot.cpp +++ b/src/modules/robot/Robot.cpp @@ -8,6 +8,8 @@ #include "libs/Module.h" #include "libs/Kernel.h" +#include "mbed.h" // for us_ticker_read() + #include #include using std::string; @@ -20,12 +22,14 @@ using std::string; #include "StepperMotor.h" #include "Gcode.h" #include "PublicDataRequest.h" -#include "RobotPublicAccess.h" +#include "PublicData.h" #include "arm_solutions/BaseSolution.h" #include "arm_solutions/CartesianSolution.h" #include "arm_solutions/RotatableCartesianSolution.h" #include "arm_solutions/LinearDeltaSolution.h" +#include "arm_solutions/RotatableDeltaSolution.h" #include "arm_solutions/HBotSolution.h" +#include "arm_solutions/CoreXZSolution.h" #include "arm_solutions/MorganSCARASolution.h" #include "StepTicker.h" #include "checksumm.h" @@ -33,6 +37,7 @@ using std::string; #include "ConfigValue.h" #include "libs/StreamOutput.h" #include "StreamOutputPool.h" +#include "ExtruderPublicAccess.h" #define default_seek_rate_checksum CHECKSUM("default_seek_rate") #define default_feed_rate_checksum CHECKSUM("default_feed_rate") @@ -50,9 +55,11 @@ using std::string; #define rotatable_cartesian_checksum CHECKSUM("rotatable_cartesian") #define rostock_checksum CHECKSUM("rostock") #define linear_delta_checksum CHECKSUM("linear_delta") +#define rotatable_delta_checksum CHECKSUM("rotatable_delta") #define delta_checksum CHECKSUM("delta") #define hbot_checksum CHECKSUM("hbot") #define corexy_checksum CHECKSUM("corexy") +#define corexz_checksum CHECKSUM("corexz") #define kossel_checksum CHECKSUM("kossel") #define morgan_checksum CHECKSUM("morgan") @@ -90,7 +97,6 @@ using std::string; #define beta_checksum CHECKSUM("beta") #define gamma_checksum CHECKSUM("gamma") - #define NEXT_ACTION_DEFAULT 0 #define NEXT_ACTION_DWELL 1 #define NEXT_ACTION_GO_HOME 2 @@ -112,9 +118,10 @@ using std::string; #define SPINDLE_DIRECTION_CW 0 #define SPINDLE_DIRECTION_CCW 1 +#define ARC_ANGULAR_TRAVEL_EPSILON 5E-7 // Float (radians) + // The Robot converts GCodes into actual movements, and then adds them to the Planner, which passes them to the Conveyor so they can be added to the queue // It takes care of cutting arcs into segments, same thing for line that are too long -#define max(a,b) (((a) > (b)) ? (a) : (b)) Robot::Robot() { @@ -128,16 +135,12 @@ Robot::Robot() seconds_per_minute = 60.0F; this->clearToolOffset(); this->compensationTransform= nullptr; - this->halted= false; } //Called when the module has just been loaded void Robot::on_module_loaded() { this->register_for_event(ON_GCODE_RECEIVED); - this->register_for_event(ON_GET_PUBLIC_DATA); - this->register_for_event(ON_SET_PUBLIC_DATA); - this->register_for_event(ON_HALT); // Configuration this->on_config_reload(this); @@ -156,12 +159,19 @@ void Robot::on_config_reload(void *argument) if(solution_checksum == hbot_checksum || solution_checksum == corexy_checksum) { this->arm_solution = new HBotSolution(THEKERNEL->config); + } else if(solution_checksum == corexz_checksum) { + this->arm_solution = new CoreXZSolution(THEKERNEL->config); + } else if(solution_checksum == rostock_checksum || solution_checksum == kossel_checksum || solution_checksum == delta_checksum || solution_checksum == linear_delta_checksum) { this->arm_solution = new LinearDeltaSolution(THEKERNEL->config); } else if(solution_checksum == rotatable_cartesian_checksum) { this->arm_solution = new RotatableCartesianSolution(THEKERNEL->config); + } else if(solution_checksum == rotatable_delta_checksum) { + this->arm_solution = new RotatableDeltaSolution(THEKERNEL->config); + + } else if(solution_checksum == morgan_checksum) { this->arm_solution = new MorganSCARASolution(THEKERNEL->config); @@ -212,17 +222,17 @@ void Robot::on_config_reload(void *argument) // TODO: delete or detect old steppermotors // Make our 3 StepperMotors - this->alpha_stepper_motor = THEKERNEL->step_ticker->add_stepper_motor( new StepperMotor(alpha_step_pin, alpha_dir_pin, alpha_en_pin) ); - this->beta_stepper_motor = THEKERNEL->step_ticker->add_stepper_motor( new StepperMotor(beta_step_pin, beta_dir_pin, beta_en_pin ) ); - this->gamma_stepper_motor = THEKERNEL->step_ticker->add_stepper_motor( new StepperMotor(gamma_step_pin, gamma_dir_pin, gamma_en_pin) ); + this->alpha_stepper_motor = new StepperMotor(alpha_step_pin, alpha_dir_pin, alpha_en_pin); + this->beta_stepper_motor = new StepperMotor(beta_step_pin, beta_dir_pin, beta_en_pin ); + this->gamma_stepper_motor = new StepperMotor(gamma_step_pin, gamma_dir_pin, gamma_en_pin); alpha_stepper_motor->change_steps_per_mm(steps_per_mm[0]); beta_stepper_motor->change_steps_per_mm(steps_per_mm[1]); gamma_stepper_motor->change_steps_per_mm(steps_per_mm[2]); - alpha_stepper_motor->max_rate = THEKERNEL->config->value(alpha_max_rate_checksum)->by_default(30000.0F)->as_number() / 60.0F; - beta_stepper_motor->max_rate = THEKERNEL->config->value(beta_max_rate_checksum )->by_default(30000.0F)->as_number() / 60.0F; - gamma_stepper_motor->max_rate = THEKERNEL->config->value(gamma_max_rate_checksum)->by_default(30000.0F)->as_number() / 60.0F; + alpha_stepper_motor->set_max_rate(THEKERNEL->config->value(alpha_max_rate_checksum)->by_default(30000.0F)->as_number() / 60.0F); + beta_stepper_motor->set_max_rate(THEKERNEL->config->value(beta_max_rate_checksum )->by_default(30000.0F)->as_number() / 60.0F); + gamma_stepper_motor->set_max_rate(THEKERNEL->config->value(gamma_max_rate_checksum)->by_default(30000.0F)->as_number() / 60.0F); check_max_actuator_speeds(); // check the configs are sane actuators.clear(); @@ -241,86 +251,49 @@ void Robot::on_config_reload(void *argument) //this->clearToolOffset(); } +void Robot::push_state() +{ + bool am= this->absolute_mode; + bool im= this->inch_mode; + saved_state_t s(this->feed_rate, this->seek_rate, am, im); + state_stack.push(s); +} + +void Robot::pop_state() +{ + if(!state_stack.empty()) { + auto s= state_stack.top(); + state_stack.pop(); + this->feed_rate= std::get<0>(s); + this->seek_rate= std::get<1>(s); + this->absolute_mode= std::get<2>(s); + this->inch_mode= std::get<3>(s); + } +} + // this does a sanity check that actuator speeds do not exceed steps rate capability // we will override the actuator max_rate if the combination of max_rate and steps/sec exceeds base_stepping_frequency void Robot::check_max_actuator_speeds() { - float step_freq= alpha_stepper_motor->max_rate * alpha_stepper_motor->get_steps_per_mm(); + float step_freq= alpha_stepper_motor->get_max_rate() * alpha_stepper_motor->get_steps_per_mm(); if(step_freq > THEKERNEL->base_stepping_frequency) { - alpha_stepper_motor->max_rate= floorf(THEKERNEL->base_stepping_frequency / alpha_stepper_motor->get_steps_per_mm()); + alpha_stepper_motor->set_max_rate(floorf(THEKERNEL->base_stepping_frequency / alpha_stepper_motor->get_steps_per_mm())); THEKERNEL->streams->printf("WARNING: alpha_max_rate exceeds base_stepping_frequency * alpha_steps_per_mm: %f, setting to %f\n", step_freq, alpha_stepper_motor->max_rate); } - step_freq= beta_stepper_motor->max_rate * beta_stepper_motor->get_steps_per_mm(); + step_freq= beta_stepper_motor->get_max_rate() * beta_stepper_motor->get_steps_per_mm(); if(step_freq > THEKERNEL->base_stepping_frequency) { - beta_stepper_motor->max_rate= floorf(THEKERNEL->base_stepping_frequency / beta_stepper_motor->get_steps_per_mm()); + beta_stepper_motor->set_max_rate(floorf(THEKERNEL->base_stepping_frequency / beta_stepper_motor->get_steps_per_mm())); THEKERNEL->streams->printf("WARNING: beta_max_rate exceeds base_stepping_frequency * beta_steps_per_mm: %f, setting to %f\n", step_freq, beta_stepper_motor->max_rate); } - step_freq= gamma_stepper_motor->max_rate * gamma_stepper_motor->get_steps_per_mm(); + step_freq= gamma_stepper_motor->get_max_rate() * gamma_stepper_motor->get_steps_per_mm(); if(step_freq > THEKERNEL->base_stepping_frequency) { - gamma_stepper_motor->max_rate= floorf(THEKERNEL->base_stepping_frequency / gamma_stepper_motor->get_steps_per_mm()); + gamma_stepper_motor->set_max_rate(floorf(THEKERNEL->base_stepping_frequency / gamma_stepper_motor->get_steps_per_mm())); THEKERNEL->streams->printf("WARNING: gamma_max_rate exceeds base_stepping_frequency * gamma_steps_per_mm: %f, setting to %f\n", step_freq, gamma_stepper_motor->max_rate); } } -void Robot::on_halt(void *arg) -{ - halted= (arg == nullptr); -} - -void Robot::on_get_public_data(void *argument) -{ - PublicDataRequest *pdr = static_cast(argument); - - if(!pdr->starts_with(robot_checksum)) return; - - if(pdr->second_element_is(speed_override_percent_checksum)) { - static float return_data; - return_data = 100.0F * 60.0F / seconds_per_minute; - pdr->set_data_ptr(&return_data); - pdr->set_taken(); - - } else if(pdr->second_element_is(current_position_checksum)) { - static float return_data[3]; - return_data[0] = from_millimeters(this->last_milestone[0]); - return_data[1] = from_millimeters(this->last_milestone[1]); - return_data[2] = from_millimeters(this->last_milestone[2]); - - pdr->set_data_ptr(&return_data); - pdr->set_taken(); - } -} - -void Robot::on_set_public_data(void *argument) -{ - PublicDataRequest *pdr = static_cast(argument); - - if(!pdr->starts_with(robot_checksum)) return; - - if(pdr->second_element_is(speed_override_percent_checksum)) { - // NOTE do not use this while printing! - float t = *static_cast(pdr->get_data_ptr()); - // enforce minimum 10% speed - if (t < 10.0F) t = 10.0F; - - this->seconds_per_minute = t / 0.6F; // t * 60 / 100 - pdr->set_taken(); - } else if(pdr->second_element_is(current_position_checksum)) { - float *t = static_cast(pdr->get_data_ptr()); - for (int i = 0; i < 3; i++) { - this->last_milestone[i] = this->to_millimeters(t[i]); - } - - float actuator_pos[3]; - arm_solution->cartesian_to_actuator(last_milestone, actuator_pos); - for (int i = 0; i < 3; i++) - actuators[i]->change_last_milestone(actuator_pos[i]); - - pdr->set_taken(); - } -} - //A GCode has been received //See if the current Gcode line has some orders for us void Robot::on_gcode_received(void *argument) @@ -332,17 +305,36 @@ void Robot::on_gcode_received(void *argument) //G-letter Gcodes are mostly what the Robot module is interrested in, other modules also catch the gcode event and do stuff accordingly if( gcode->has_g) { switch( gcode->g ) { - case 0: this->motion_mode = MOTION_MODE_SEEK; gcode->mark_as_taken(); break; - case 1: this->motion_mode = MOTION_MODE_LINEAR; gcode->mark_as_taken(); break; - case 2: this->motion_mode = MOTION_MODE_CW_ARC; gcode->mark_as_taken(); break; - case 3: this->motion_mode = MOTION_MODE_CCW_ARC; gcode->mark_as_taken(); break; - case 17: this->select_plane(X_AXIS, Y_AXIS, Z_AXIS); gcode->mark_as_taken(); break; - case 18: this->select_plane(X_AXIS, Z_AXIS, Y_AXIS); gcode->mark_as_taken(); break; - case 19: this->select_plane(Y_AXIS, Z_AXIS, X_AXIS); gcode->mark_as_taken(); break; - case 20: this->inch_mode = true; gcode->mark_as_taken(); break; - case 21: this->inch_mode = false; gcode->mark_as_taken(); break; - case 90: this->absolute_mode = true; gcode->mark_as_taken(); break; - case 91: this->absolute_mode = false; gcode->mark_as_taken(); break; + case 0: this->motion_mode = MOTION_MODE_SEEK; break; + case 1: this->motion_mode = MOTION_MODE_LINEAR; break; + case 2: this->motion_mode = MOTION_MODE_CW_ARC; break; + case 3: this->motion_mode = MOTION_MODE_CCW_ARC; break; + case 4: { + uint32_t delay_ms= 0; + if (gcode->has_letter('P')) { + delay_ms= gcode->get_int('P'); + } + if (gcode->has_letter('S')) { + delay_ms += gcode->get_int('S') * 1000; + } + if (delay_ms > 0){ + // drain queue + THEKERNEL->conveyor->wait_for_empty_queue(); + // wait for specified time + uint32_t start= us_ticker_read(); // mbed call + while ((us_ticker_read() - start) < delay_ms*1000) { + THEKERNEL->call_event(ON_IDLE, this); + } + } + } + break; + case 17: this->select_plane(X_AXIS, Y_AXIS, Z_AXIS); break; + case 18: this->select_plane(X_AXIS, Z_AXIS, Y_AXIS); break; + case 19: this->select_plane(Y_AXIS, Z_AXIS, X_AXIS); break; + case 20: this->inch_mode = true; break; + case 21: this->inch_mode = false; break; + case 90: this->absolute_mode = true; break; + case 91: this->absolute_mode = false; break; case 92: { if(gcode->get_num_args() == 0) { for (int i = X_AXIS; i <= Z_AXIS; ++i) { @@ -356,8 +348,6 @@ void Robot::on_gcode_received(void *argument) } } } - - gcode->mark_as_taken(); return; } } @@ -375,9 +365,9 @@ void Robot::on_gcode_received(void *argument) gcode->stream->printf("X:%g Y:%g Z:%g F:%g ", actuators[0]->steps_per_mm, actuators[1]->steps_per_mm, actuators[2]->steps_per_mm, seconds_per_minute); gcode->add_nl = true; - gcode->mark_as_taken(); check_max_actuator_speeds(); return; + case 114: { char buf[64]; int n = snprintf(buf, sizeof(buf), "C: X:%1.3f Y:%1.3f Z:%1.3f A:%1.3f B:%1.3f C:%1.3f ", @@ -388,10 +378,17 @@ void Robot::on_gcode_received(void *argument) actuators[Y_AXIS]->get_current_position(), actuators[Z_AXIS]->get_current_position() ); gcode->txt_after_ok.append(buf, n); - gcode->mark_as_taken(); } return; + case 120: // push state + push_state(); + break; + + case 121: // pop state + pop_state(); + break; + case 203: // M203 Set maximum feedrates in mm/sec if (gcode->has_letter('X')) this->max_speeds[X_AXIS] = gcode->get_value('X'); @@ -400,27 +397,25 @@ void Robot::on_gcode_received(void *argument) if (gcode->has_letter('Z')) this->max_speeds[Z_AXIS] = gcode->get_value('Z'); if (gcode->has_letter('A')) - alpha_stepper_motor->max_rate = gcode->get_value('A'); + alpha_stepper_motor->set_max_rate(gcode->get_value('A')); if (gcode->has_letter('B')) - beta_stepper_motor->max_rate = gcode->get_value('B'); + beta_stepper_motor->set_max_rate(gcode->get_value('B')); if (gcode->has_letter('C')) - gamma_stepper_motor->max_rate = gcode->get_value('C'); + gamma_stepper_motor->set_max_rate(gcode->get_value('C')); check_max_actuator_speeds(); - gcode->stream->printf("X:%g Y:%g Z:%g A:%g B:%g C:%g ", - this->max_speeds[X_AXIS], this->max_speeds[Y_AXIS], this->max_speeds[Z_AXIS], - alpha_stepper_motor->max_rate, beta_stepper_motor->max_rate, gamma_stepper_motor->max_rate); - gcode->add_nl = true; - gcode->mark_as_taken(); + if(gcode->get_num_args() == 0) { + gcode->stream->printf("X:%g Y:%g Z:%g A:%g B:%g C:%g ", + this->max_speeds[X_AXIS], this->max_speeds[Y_AXIS], this->max_speeds[Z_AXIS], + alpha_stepper_motor->get_max_rate(), beta_stepper_motor->get_max_rate(), gamma_stepper_motor->get_max_rate()); + gcode->add_nl = true; + } + break; case 204: // M204 Snnn - set acceleration to nnn, Znnn sets z acceleration - gcode->mark_as_taken(); - if (gcode->has_letter('S')) { - // TODO for safety so it applies only to following gcodes, maybe a better way to do this? - THEKERNEL->conveyor->wait_for_empty_queue(); float acc = gcode->get_value('S'); // mm/s^2 // enforce minimum if (acc < 1.0F) @@ -428,8 +423,6 @@ void Robot::on_gcode_received(void *argument) THEKERNEL->planner->acceleration = acc; } if (gcode->has_letter('Z')) { - // TODO for safety so it applies only to following gcodes, maybe a better way to do this? - THEKERNEL->conveyor->wait_for_empty_queue(); float acc = gcode->get_value('Z'); // mm/s^2 // enforce positive if (acc < 0.0F) @@ -438,8 +431,7 @@ void Robot::on_gcode_received(void *argument) } break; - case 205: // M205 Xnnn - set junction deviation, Z - set Z junction deviation, Snnn - Set minimum planner speed - gcode->mark_as_taken(); + case 205: // M205 Xnnn - set junction deviation, Z - set Z junction deviation, Snnn - Set minimum planner speed, Ynnn - set minimum step rate if (gcode->has_letter('X')) { float jd = gcode->get_value('X'); // enforce minimum @@ -461,10 +453,12 @@ void Robot::on_gcode_received(void *argument) mps = 0.0F; THEKERNEL->planner->minimum_planner_speed = mps; } + if (gcode->has_letter('Y')) { + alpha_stepper_motor->default_minimum_actuator_rate = gcode->get_value('Y'); + } break; case 220: // M220 - speed override percentage - gcode->mark_as_taken(); if (gcode->has_letter('S')) { float factor = gcode->get_value('S'); // enforce minimum 10% speed @@ -475,11 +469,12 @@ void Robot::on_gcode_received(void *argument) factor = 1000.0F; seconds_per_minute = 6000.0F / factor; + }else{ + gcode->stream->printf("Speed factor at %6.2f %%\n", 6000.0F / seconds_per_minute); } break; case 400: // wait until all moves are done up to this point - gcode->mark_as_taken(); THEKERNEL->conveyor->wait_for_empty_queue(); break; @@ -487,10 +482,10 @@ void Robot::on_gcode_received(void *argument) case 503: { // M503 just prints the settings gcode->stream->printf(";Steps per unit:\nM92 X%1.5f Y%1.5f Z%1.5f\n", actuators[0]->steps_per_mm, actuators[1]->steps_per_mm, actuators[2]->steps_per_mm); gcode->stream->printf(";Acceleration mm/sec^2:\nM204 S%1.5f Z%1.5f\n", THEKERNEL->planner->acceleration, THEKERNEL->planner->z_acceleration); - gcode->stream->printf(";X- Junction Deviation, Z- Z junction deviation, S - Minimum Planner speed:\nM205 X%1.5f Z%1.5f S%1.5f\n", THEKERNEL->planner->junction_deviation, THEKERNEL->planner->z_junction_deviation, THEKERNEL->planner->minimum_planner_speed); + gcode->stream->printf(";X- Junction Deviation, Z- Z junction deviation, S - Minimum Planner speed mm/sec:\nM205 X%1.5f Z%1.5f S%1.5f\n", THEKERNEL->planner->junction_deviation, THEKERNEL->planner->z_junction_deviation, THEKERNEL->planner->minimum_planner_speed); gcode->stream->printf(";Max feedrates in mm/sec, XYZ cartesian, ABC actuator:\nM203 X%1.5f Y%1.5f Z%1.5f A%1.5f B%1.5f C%1.5f\n", this->max_speeds[X_AXIS], this->max_speeds[Y_AXIS], this->max_speeds[Z_AXIS], - alpha_stepper_motor->max_rate, beta_stepper_motor->max_rate, gamma_stepper_motor->max_rate); + alpha_stepper_motor->get_max_rate(), beta_stepper_motor->get_max_rate(), gamma_stepper_motor->get_max_rate()); // get or save any arm solution specific optional values BaseSolution::arm_options_t options; @@ -501,33 +496,39 @@ void Robot::on_gcode_received(void *argument) } gcode->stream->printf("\n"); } - gcode->mark_as_taken(); + break; } case 665: { // M665 set optional arm solution variables based on arm solution. - gcode->mark_as_taken(); - // the parameter args could be any letter except S so ask solution what options it supports - BaseSolution::arm_options_t options; + // the parameter args could be any letter each arm solution only accepts certain ones + BaseSolution::arm_options_t options= gcode->get_args(); + options.erase('S'); // don't include the S + options.erase('U'); // don't include the U + if(options.size() > 0) { + // set the specified options + arm_solution->set_optional(options); + } + options.clear(); if(arm_solution->get_optional(options)) { + // foreach optional value for(auto &i : options) { - // foreach optional value - char c = i.first; - if(gcode->has_letter(c)) { // set new value - i.second = gcode->get_value(c); - } // print all current values of supported options gcode->stream->printf("%c: %8.4f ", i.first, i.second); gcode->add_nl = true; } - // set the new options - arm_solution->set_optional(options); } - // set delta segments per second, not saved by M500 - if(gcode->has_letter('S')) { + if(gcode->has_letter('S')) { // set delta segments per second, not saved by M500 this->delta_segments_per_second = gcode->get_value('S'); + gcode->stream->printf("Delta segments set to %8.4f segs/sec\n", this->delta_segments_per_second); + + }else if(gcode->has_letter('U')) { // or set mm_per_line_segment, not saved by M500 + this->mm_per_line_segment = gcode->get_value('U'); + this->delta_segments_per_second = 0; + gcode->stream->printf("mm per line segment set to %8.4f\n", this->mm_per_line_segment); } + break; } } @@ -617,10 +618,15 @@ void Robot::reset_position_from_current_actuator_position() float actuator_pos[]= {actuators[X_AXIS]->get_current_position(), actuators[Y_AXIS]->get_current_position(), actuators[Z_AXIS]->get_current_position()}; arm_solution->actuator_to_cartesian(actuator_pos, this->last_milestone); memcpy(this->transformed_last_milestone, this->last_milestone, sizeof(this->transformed_last_milestone)); + + // now reset actuator correctly, NOTE this may lose a little precision + arm_solution->cartesian_to_actuator(this->last_milestone, actuator_pos); + for (int i = 0; i < 3; i++) + actuators[i]->change_last_milestone(actuator_pos[i]); } // Convert target from millimeters to steps, and append this to the planner -void Robot::append_milestone( float target[], float rate_mm_s ) +void Robot::append_milestone(Gcode *gcode, float target[], float rate_mm_s) { float deltas[3]; float unit_vec[3]; @@ -664,12 +670,14 @@ void Robot::append_milestone( float target[], float rate_mm_s ) // find actuator position given cartesian position, use actual adjusted target arm_solution->cartesian_to_actuator( transformed_target, actuator_pos ); + float isecs= rate_mm_s / millimeters_of_travel; // check per-actuator speed limits for (int actuator = 0; actuator <= 2; actuator++) { - float actuator_rate = fabs(actuator_pos[actuator] - actuators[actuator]->last_milestone_mm) * rate_mm_s / millimeters_of_travel; - - if (actuator_rate > actuators[actuator]->max_rate) - rate_mm_s *= (actuators[actuator]->max_rate / actuator_rate); + float actuator_rate = fabsf(actuator_pos[actuator] - actuators[actuator]->last_milestone_mm) * isecs; + if (actuator_rate > actuators[actuator]->get_max_rate()){ + rate_mm_s *= (actuators[actuator]->get_max_rate() / actuator_rate); + isecs= rate_mm_s / millimeters_of_travel; + } } // Append the block to the planner @@ -683,23 +691,38 @@ void Robot::append_milestone( float target[], float rate_mm_s ) // Append a move to the queue ( cutting it into segments if needed ) void Robot::append_line(Gcode *gcode, float target[], float rate_mm_s ) { - // Find out the distance for this gcode - gcode->millimeters_of_travel = powf( target[X_AXIS] - this->last_milestone[X_AXIS], 2 ) + powf( target[Y_AXIS] - this->last_milestone[Y_AXIS], 2 ) + powf( target[Z_AXIS] - this->last_milestone[Z_AXIS], 2 ); + // NOTE we need to do sqrt here as this setting of millimeters_of_travel is used by extruder and other modules even if there is no XYZ move + gcode->millimeters_of_travel = sqrtf(powf( target[X_AXIS] - this->last_milestone[X_AXIS], 2 ) + powf( target[Y_AXIS] - this->last_milestone[Y_AXIS], 2 ) + powf( target[Z_AXIS] - this->last_milestone[Z_AXIS], 2 )); - // We ignore non-moves ( for example, extruder moves are not XYZ moves ) - if( gcode->millimeters_of_travel < 1e-8F ) { + // We ignore non- XYZ moves ( for example, extruder moves are not XYZ moves ) + if( gcode->millimeters_of_travel < 0.00001F ) { return; } - gcode->millimeters_of_travel = sqrtf(gcode->millimeters_of_travel); - // Mark the gcode as having a known distance this->distance_in_gcode_is_known( gcode ); + // if we have volumetric limits enabled we calculate the volume for this move and limit the rate if it exceeds the stated limit + // Note we need to be using volumetric extrusion for this to work as Ennn is in mm³ not mm + // We also check we are not exceeding the E max_speed for the current extruder + // We ask Extruder to do all the work, but as Extruder won't even see this gcode until after it has been planned + // we need to ask it now passing in the relevant data. + // NOTE we need to do this before we segment the line (for deltas) + if(gcode->has_letter('E')) { + float data[2]; + data[0]= gcode->get_value('E'); // E target (maybe absolute or relative) + data[1]= rate_mm_s / gcode->millimeters_of_travel; // inverted seconds for the move + if(PublicData::set_value(extruder_checksum, target_checksum, data)) { + rate_mm_s *= data[1]; + //THEKERNEL->streams->printf("Extruder has changed the rate by %f to %f\n", data[1], rate_mm_s); + } + } + // We cut the line into smaller segments. This is not usefull in a cartesian robot, but necessary for robots with rotational axes. // In cartesian robot, a high "mm_per_line_segment" setting will prevent waste. - // In delta robots either mm_per_line_segment can be used OR delta_segments_per_second The latter is more efficient and avoids splitting fast long lines into very small segments, like initial z move to 0, it is what Johanns Marlin delta port does + // In delta robots either mm_per_line_segment can be used OR delta_segments_per_second + // The latter is more efficient and avoids splitting fast long lines into very small segments, like initial z move to 0, it is what Johanns Marlin delta port does uint16_t segments; if(this->delta_segments_per_second > 1.0F) { @@ -708,14 +731,14 @@ void Robot::append_line(Gcode *gcode, float target[], float rate_mm_s ) // the faster the travel speed the fewer segments needed // NOTE rate is mm/sec and we take into account any speed override float seconds = gcode->millimeters_of_travel / rate_mm_s; - segments = max(1, ceil(this->delta_segments_per_second * seconds)); + segments = max(1.0F, ceilf(this->delta_segments_per_second * seconds)); // TODO if we are only moving in Z on a delta we don't really need to segment at all } else { if(this->mm_per_line_segment == 0.0F) { segments = 1; // don't split it up } else { - segments = ceil( gcode->millimeters_of_travel / this->mm_per_line_segment); + segments = ceilf( gcode->millimeters_of_travel / this->mm_per_line_segment); } } @@ -731,17 +754,17 @@ void Robot::append_line(Gcode *gcode, float target[], float rate_mm_s ) // segment 0 is already done - it's the end point of the previous move so we start at segment 1 // We always add another point after this loop so we stop at segments-1, ie i < segments for (int i = 1; i < segments; i++) { - if(halted) return; // don;t queue any more segments + if(THEKERNEL->is_halted()) return; // don't queue any more segments for(int axis = X_AXIS; axis <= Z_AXIS; axis++ ) segment_end[axis] = last_milestone[axis] + segment_delta[axis]; // Append the end of this segment to the queue - this->append_milestone(segment_end, rate_mm_s); + this->append_milestone(gcode, segment_end, rate_mm_s); } } // Append the end of this full move to the queue - this->append_milestone(target, rate_mm_s); + this->append_milestone(gcode, target, rate_mm_s); // if adding these blocks didn't start executing, do that now THEKERNEL->conveyor->ensure_running(); @@ -761,20 +784,20 @@ void Robot::append_arc(Gcode *gcode, float target[], float offset[], float radiu float rt_axis0 = target[this->plane_axis_0] - center_axis0; float rt_axis1 = target[this->plane_axis_1] - center_axis1; + // Patch from GRBL Firmware - Christoph Baumann 04072015 // CCW angle between position and target from circle center. Only one atan2() trig computation required. - float angular_travel = atan2(r_axis0 * rt_axis1 - r_axis1 * rt_axis0, r_axis0 * rt_axis0 + r_axis1 * rt_axis1); - if (angular_travel < 0) { - angular_travel += 2 * M_PI; - } - if (is_clockwise) { - angular_travel -= 2 * M_PI; + float angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1); + if (is_clockwise) { // Correct atan2 output per direction + if (angular_travel >= -ARC_ANGULAR_TRAVEL_EPSILON) { angular_travel -= 2*M_PI; } + } else { + if (angular_travel <= ARC_ANGULAR_TRAVEL_EPSILON) { angular_travel += 2*M_PI; } } // Find the distance for this gcode gcode->millimeters_of_travel = hypotf(angular_travel * radius, fabs(linear_travel)); // We don't care about non-XYZ moves ( for example the extruder produces some of those ) - if( gcode->millimeters_of_travel < 0.0001F ) { + if( gcode->millimeters_of_travel < 0.00001F ) { return; } @@ -782,7 +805,7 @@ void Robot::append_arc(Gcode *gcode, float target[], float offset[], float radiu this->distance_in_gcode_is_known( gcode ); // Figure out how many segments for this gcode - uint16_t segments = floor(gcode->millimeters_of_travel / this->mm_per_arc_segment); + uint16_t segments = floorf(gcode->millimeters_of_travel / this->mm_per_arc_segment); float theta_per_segment = angular_travel / segments; float linear_per_segment = linear_travel / segments; @@ -825,7 +848,7 @@ void Robot::append_arc(Gcode *gcode, float target[], float offset[], float radiu arc_target[this->plane_axis_2] = this->last_milestone[this->plane_axis_2]; for (i = 1; i < segments; i++) { // Increment (segments-1) - if(halted) return; // don't queue any more segments + if(THEKERNEL->is_halted()) return; // don't queue any more segments if (count < this->arc_correction ) { // Apply vector rotation matrix @@ -849,12 +872,12 @@ void Robot::append_arc(Gcode *gcode, float target[], float offset[], float radiu arc_target[this->plane_axis_2] += linear_per_segment; // Append this segment to the queue - this->append_milestone(arc_target, this->feed_rate / seconds_per_minute); + this->append_milestone(gcode, arc_target, this->feed_rate / seconds_per_minute); } // Ensure last segment arrives at target location. - this->append_milestone(target, this->feed_rate / seconds_per_minute); + this->append_milestone(gcode, target, this->feed_rate / seconds_per_minute); } // Do the math for an arc and add it to the queue diff --git a/src/modules/robot/Robot.h b/src/modules/robot/Robot.h index e6d8e609..b45af925 100644 --- a/src/modules/robot/Robot.h +++ b/src/modules/robot/Robot.h @@ -12,6 +12,7 @@ using std::string; #include #include +#include #include "libs/Module.h" @@ -25,9 +26,6 @@ class Robot : public Module { void on_module_loaded(); void on_config_reload(void* argument); void on_gcode_received(void* argument); - void on_get_public_data(void* argument); - void on_set_public_data(void* argument); - void on_halt(void *arg); void reset_axis_position(float position, int axis); void reset_axis_position(float x, float y, float z); @@ -37,10 +35,13 @@ class Robot : public Module { float from_millimeters(float value); float get_seconds_per_minute() const { return seconds_per_minute; } float get_z_maxfeedrate() const { return this->max_speeds[2]; } + void setToolOffset(const float offset[3]); + float get_feed_rate() const { return feed_rate; } + void push_state(); + void pop_state(); + void check_max_actuator_speeds(); BaseSolution* arm_solution; // Selected Arm solution ( millimeters to step calculation ) - bool absolute_mode; // true for absolute mode ( default ), false for relative mode - void setToolOffset(const float offset[3]); // gets accessed by Panel, Endstops, ZProbe std::vector actuators; @@ -48,21 +49,24 @@ class Robot : public Module { // set by a leveling strategy to transform the target of a move according to the current plan std::function compensationTransform; + struct { + bool inch_mode:1; // true for inch mode, false for millimeter mode ( default ) + bool absolute_mode:1; // true for absolute mode ( default ), false for relative mode + }; + private: void distance_in_gcode_is_known(Gcode* gcode); - void append_milestone( float target[], float rate_mm_s); + void append_milestone( Gcode *gcode, float target[], float rate_mm_s); void append_line( Gcode* gcode, float target[], float rate_mm_s); - //void append_arc(float theta_start, float angular_travel, float radius, float depth, float rate); void append_arc( Gcode* gcode, float target[], float offset[], float radius, bool is_clockwise ); - - void compute_arc(Gcode* gcode, float offset[], float target[]); float theta(float x, float y); void select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2); void clearToolOffset(); - void check_max_actuator_speeds(); + typedef std::tuple saved_state_t; // save current feedrate and absolute mode, inch mode + std::stack state_stack; // saves state from M120 float last_milestone[3]; // Last position, in millimeters float transformed_last_milestone[3]; // Last transformed position int8_t motion_mode; // Motion mode for the current received Gcode @@ -91,11 +95,6 @@ class Robot : public Module { StepperMotor* alpha_stepper_motor; StepperMotor* beta_stepper_motor; StepperMotor* gamma_stepper_motor; - - struct { - bool halted:1; - bool inch_mode:1; // true for inch mode, false for millimeter mode ( default ) - }; }; // Convert from inches to millimeters ( our internal storage unit ) if needed diff --git a/src/modules/robot/RobotPublicAccess.h b/src/modules/robot/RobotPublicAccess.h deleted file mode 100644 index 0255f5cf..00000000 --- a/src/modules/robot/RobotPublicAccess.h +++ /dev/null @@ -1,9 +0,0 @@ -#ifndef __ROBOTPUBLICACCESS_H_ -#define __ROBOTPUBLICACCESS_H_ - -// addresses used for public data access -#define robot_checksum CHECKSUM("robot") -#define speed_override_percent_checksum CHECKSUM("speed_override_percent") -#define current_position_checksum CHECKSUM("current_position") - -#endif \ No newline at end of file diff --git a/src/modules/robot/Stepper.cpp b/src/modules/robot/Stepper.cpp index 264bb411..40c807f9 100644 --- a/src/modules/robot/Stepper.cpp +++ b/src/modules/robot/Stepper.cpp @@ -19,6 +19,7 @@ #include "ConfigValue.h" #include "Gcode.h" #include "Block.h" +#include "StepTicker.h" #include using namespace std; @@ -28,17 +29,12 @@ using namespace std; #include -#define acceleration_ticks_per_second_checksum CHECKSUM("acceleration_ticks_per_second") -#define minimum_steps_per_minute_checksum CHECKSUM("minimum_steps_per_minute") - // The stepper reacts to blocks that have XYZ movement to transform them into actual stepper motor moves // TODO: This does accel, accel should be in StepperMotor Stepper::Stepper() { this->current_block = NULL; - this->paused = false; - this->trapezoid_generator_busy = false; this->force_speed_update = false; this->halted= false; } @@ -50,15 +46,13 @@ void Stepper::on_module_loaded() this->register_for_event(ON_BLOCK_END); this->register_for_event(ON_GCODE_EXECUTE); this->register_for_event(ON_GCODE_RECEIVED); - this->register_for_event(ON_PLAY); - this->register_for_event(ON_PAUSE); this->register_for_event(ON_HALT); // Get onfiguration this->on_config_reload(this); // Acceleration ticker - this->acceleration_tick_hook = THEKERNEL->slow_ticker->attach( this->acceleration_ticks_per_second, this, &Stepper::trapezoid_generator_tick ); + THEKERNEL->step_ticker->register_acceleration_tick_handler([this](){trapezoid_generator_tick(); }); // Attach to the end_of_move stepper event THEKERNEL->robot->alpha_stepper_motor->attach(this, &Stepper::stepper_motor_finished_move ); @@ -69,33 +63,10 @@ void Stepper::on_module_loaded() // Get configuration from the config file void Stepper::on_config_reload(void *argument) { - - this->acceleration_ticks_per_second = THEKERNEL->config->value(acceleration_ticks_per_second_checksum)->by_default(100 )->as_number(); - this->minimum_steps_per_second = THEKERNEL->config->value(minimum_steps_per_minute_checksum )->by_default(3000 )->as_number() / 60.0F; - // Steppers start off by default this->turn_enable_pins_off(); } -// When the play/pause button is set to pause, or a module calls the ON_PAUSE event -void Stepper::on_pause(void *argument) -{ - this->paused = true; - THEKERNEL->robot->alpha_stepper_motor->pause(); - THEKERNEL->robot->beta_stepper_motor->pause(); - THEKERNEL->robot->gamma_stepper_motor->pause(); -} - -// When the play/pause button is set to play, or a module calls the ON_PLAY event -void Stepper::on_play(void *argument) -{ - // TODO: Re-compute the whole queue for a cold-start - this->paused = false; - THEKERNEL->robot->alpha_stepper_motor->unpause(); - THEKERNEL->robot->beta_stepper_motor->unpause(); - THEKERNEL->robot->gamma_stepper_motor->unpause(); -} - void Stepper::on_halt(void *argument) { if(argument == nullptr) { @@ -112,7 +83,6 @@ void Stepper::on_gcode_received(void *argument) // Attach gcodes to the last block for on_gcode_execute if( gcode->has_m && (gcode->m == 84 || gcode->m == 17 || gcode->m == 18 )) { THEKERNEL->conveyor->append_gcode(gcode); - } } @@ -137,6 +107,7 @@ void Stepper::turn_enable_pins_on() for (StepperMotor *m : THEKERNEL->robot->actuators) m->enable(true); this->enable_pins_status = true; + THEKERNEL->call_event(ON_ENABLE, (void*)1); } // Disable steppers @@ -145,6 +116,7 @@ void Stepper::turn_enable_pins_off() for (StepperMotor *m : THEKERNEL->robot->actuators) m->enable(false); this->enable_pins_status = false; + THEKERNEL->call_event(ON_ENABLE, nullptr); } // A new block is popped from the queue @@ -152,15 +124,15 @@ void Stepper::on_block_begin(void *argument) { Block *block = static_cast(argument); - // The stepper does not care about 0-blocks - if( block->millimeters == 0.0F ) { - return; - } - - // Mark the new block as of interrest to us - if( block->steps[ALPHA_STEPPER] > 0 || block->steps[BETA_STEPPER] > 0 || block->steps[GAMMA_STEPPER] > 0 ) { + // Mark the new block as of interrest to us, handle blocks that have no axis moves properly (like Extrude blocks etc) + if(block->millimeters > 0.0F && (block->steps[ALPHA_STEPPER] > 0 || block->steps[BETA_STEPPER] > 0 || block->steps[GAMMA_STEPPER] > 0) ) { block->take(); + } else { + // none of the steppers move this block so make sure they know that + for(auto a : THEKERNEL->robot->actuators) { + a->set_moved_last_block(false); + } return; } @@ -170,14 +142,29 @@ void Stepper::on_block_begin(void *argument) } // Setup : instruct stepper motors to move + // Find the stepper with the more steps, it's the one the speed calculations will want to follow + this->main_stepper= nullptr; if( block->steps[ALPHA_STEPPER] > 0 ) { - THEKERNEL->robot->alpha_stepper_motor->move( block->direction_bits[ALPHA_STEPPER], block->steps[ALPHA_STEPPER] ); + THEKERNEL->robot->alpha_stepper_motor->move( block->direction_bits[ALPHA_STEPPER], block->steps[ALPHA_STEPPER])->set_moved_last_block(true); + this->main_stepper = THEKERNEL->robot->alpha_stepper_motor; + }else{ + THEKERNEL->robot->alpha_stepper_motor->set_moved_last_block(false); } + if( block->steps[BETA_STEPPER ] > 0 ) { - THEKERNEL->robot->beta_stepper_motor->move( block->direction_bits[BETA_STEPPER], block->steps[BETA_STEPPER ] ); + THEKERNEL->robot->beta_stepper_motor->move( block->direction_bits[BETA_STEPPER], block->steps[BETA_STEPPER ])->set_moved_last_block(true); + if(this->main_stepper == nullptr || THEKERNEL->robot->beta_stepper_motor->get_steps_to_move() > this->main_stepper->get_steps_to_move()) + this->main_stepper = THEKERNEL->robot->beta_stepper_motor; + }else{ + THEKERNEL->robot->beta_stepper_motor->set_moved_last_block(false); } + if( block->steps[GAMMA_STEPPER] > 0 ) { - THEKERNEL->robot->gamma_stepper_motor->move( block->direction_bits[GAMMA_STEPPER], block->steps[GAMMA_STEPPER] ); + THEKERNEL->robot->gamma_stepper_motor->move( block->direction_bits[GAMMA_STEPPER], block->steps[GAMMA_STEPPER])->set_moved_last_block(true); + if(this->main_stepper == nullptr || THEKERNEL->robot->gamma_stepper_motor->get_steps_to_move() > this->main_stepper->get_steps_to_move()) + this->main_stepper = THEKERNEL->robot->gamma_stepper_motor; + }else{ + THEKERNEL->robot->gamma_stepper_motor->set_moved_last_block(false); } this->current_block = block; @@ -185,21 +172,16 @@ void Stepper::on_block_begin(void *argument) // Setup acceleration for this block this->trapezoid_generator_reset(); - // Find the stepper with the more steps, it's the one the speed calculations will want to follow - this->main_stepper = THEKERNEL->robot->alpha_stepper_motor; - if( THEKERNEL->robot->beta_stepper_motor->steps_to_move > this->main_stepper->steps_to_move ) { - this->main_stepper = THEKERNEL->robot->beta_stepper_motor; - } - if( THEKERNEL->robot->gamma_stepper_motor->steps_to_move > this->main_stepper->steps_to_move ) { - this->main_stepper = THEKERNEL->robot->gamma_stepper_motor; - } - // Set the initial speed for this move - this->trapezoid_generator_tick(0); + this->trapezoid_generator_tick(); - // Synchronise the acceleration curve with the stepping - this->synchronize_acceleration(0); + // synchronize the acceleration timer with the start of the new block so it does not drift and randomly fire during the block + THEKERNEL->step_ticker->synchronize_acceleration(false); + // set a flag to synchronize the acceleration timer with the deceleration step, and fire it immediately we get to that step + if( block->decelerate_after > 0 && block->decelerate_after+1 < this->main_stepper->steps_to_move ) { + this->main_stepper->signal_step= block->decelerate_after+1; // we make it +1 as deceleration does not start until steps > decelerate_after + } } // Current block is discarded @@ -211,7 +193,6 @@ void Stepper::on_block_end(void *argument) // When a stepper motor has finished it's assigned movement uint32_t Stepper::stepper_motor_finished_move(uint32_t dummy) { - // We care only if none is still moving if( THEKERNEL->robot->alpha_stepper_motor->moving || THEKERNEL->robot->beta_stepper_motor->moving || THEKERNEL->robot->gamma_stepper_motor->moving ) { return 0; @@ -229,18 +210,20 @@ uint32_t Stepper::stepper_motor_finished_move(uint32_t dummy) // This is called ACCELERATION_TICKS_PER_SECOND times per second by the step_event // interrupt. It can be assumed that the trapezoid-generator-parameters and the // current_block stays untouched by outside handlers for the duration of this function call. -uint32_t Stepper::trapezoid_generator_tick( uint32_t dummy ) +// NOTE caled at the same priority as PendSV so it may make that longer but it is better that having htis pre empted by pendsv +void Stepper::trapezoid_generator_tick(void) { - // Do not do the accel math for nothing - if(this->current_block && !this->paused && this->main_stepper->moving ) { + if(this->current_block && this->main_stepper->moving ) { // Store this here because we use it a lot down there uint32_t current_steps_completed = this->main_stepper->stepped; + float last_rate= trapezoid_adjusted_rate; if( this->force_speed_update ) { // Do not accel, just set the value this->force_speed_update = false; + last_rate= -1; } else if(THEKERNEL->conveyor->is_flushing()) { // if we are flushing the queue, decelerate to 0 then finish this block @@ -248,16 +231,16 @@ uint32_t Stepper::trapezoid_generator_tick( uint32_t dummy ) trapezoid_adjusted_rate -= current_block->rate_delta; } else if (trapezoid_adjusted_rate == current_block->rate_delta * 0.5F) { - for (auto i : THEKERNEL->robot->actuators) - i->move(i->direction, 0); - if (current_block) - current_block->release(); - return 0; + for (auto i : THEKERNEL->robot->actuators) i->move(i->direction, 0); // stop motors + if (current_block) current_block->release(); + THEKERNEL->call_event(ON_SPEED_CHANGE, 0); // tell others we stopped + return; + } else { trapezoid_adjusted_rate = current_block->rate_delta * 0.5F; } - } else if(current_steps_completed <= this->current_block->accelerate_until + 1) { + } else if(current_steps_completed <= this->current_block->accelerate_until) { // If we are accelerating // Increase speed this->trapezoid_adjusted_rate += this->current_block->rate_delta; @@ -285,10 +268,11 @@ uint32_t Stepper::trapezoid_generator_tick( uint32_t dummy ) this->trapezoid_adjusted_rate = this->current_block->nominal_rate; } - this->set_step_events_per_second(this->trapezoid_adjusted_rate); + if(last_rate != trapezoid_adjusted_rate) { + // don't call this if speed did not change + this->set_step_events_per_second(this->trapezoid_adjusted_rate); + } } - - return 0; } // Initializes the trapezoid generator from the current block. Called whenever a new @@ -297,66 +281,26 @@ inline void Stepper::trapezoid_generator_reset() { this->trapezoid_adjusted_rate = this->current_block->initial_rate; this->force_speed_update = true; - this->trapezoid_tick_cycle_counter = 0; } // Update the speed for all steppers void Stepper::set_step_events_per_second( float steps_per_second ) { - // We do not step slower than this, FIXME shoul dbe calculated for the slowest axis not the fastest - //steps_per_second = max(steps_per_second, this->minimum_steps_per_second); - if( steps_per_second < this->minimum_steps_per_second ) { - steps_per_second = this->minimum_steps_per_second; - } + float isps= steps_per_second / this->current_block->steps_event_count; // Instruct the stepper motors if( THEKERNEL->robot->alpha_stepper_motor->moving ) { - THEKERNEL->robot->alpha_stepper_motor->set_speed( steps_per_second * ( (float)this->current_block->steps[ALPHA_STEPPER] / (float)this->current_block->steps_event_count ) ); + THEKERNEL->robot->alpha_stepper_motor->set_speed(isps * this->current_block->steps[ALPHA_STEPPER]); } if( THEKERNEL->robot->beta_stepper_motor->moving ) { - THEKERNEL->robot->beta_stepper_motor->set_speed( steps_per_second * ( (float)this->current_block->steps[BETA_STEPPER ] / (float)this->current_block->steps_event_count ) ); + THEKERNEL->robot->beta_stepper_motor->set_speed(isps * this->current_block->steps[BETA_STEPPER]); } if( THEKERNEL->robot->gamma_stepper_motor->moving ) { - THEKERNEL->robot->gamma_stepper_motor->set_speed( steps_per_second * ( (float)this->current_block->steps[GAMMA_STEPPER] / (float)this->current_block->steps_event_count ) ); + THEKERNEL->robot->gamma_stepper_motor->set_speed(isps * this->current_block->steps[GAMMA_STEPPER]); } // Other modules might want to know the speed changed THEKERNEL->call_event(ON_SPEED_CHANGE, this); - } -// This function has the role of making sure acceleration and deceleration curves have their -// rhythm synchronized. The accel/decel must start at the same moment as the speed update routine -// This is caller in "step just occured" or "block just began" ( step Timer ) context, so we need to be fast. -// All we do is reset the other timer so that it does what we want -uint32_t Stepper::synchronize_acceleration(uint32_t dummy) -{ - - // No move was done, this is called from on_block_begin - // This means we setup the accel timer in a way where it gets called right after - // we exit this step interrupt, and so that it is then in synch with - if( this->main_stepper->stepped == 0 ) { - // Whatever happens, we must call the accel interrupt asap - // Because it will set the initial rate - // We also want to synchronize in case we start accelerating or decelerating now - - // Accel interrupt must happen asap - NVIC_SetPendingIRQ(TIMER2_IRQn); - // Synchronize both counters - LPC_TIM2->TC = LPC_TIM0->TC; - - // If we start decelerating after this, we must ask the actuator to warn us - // so we can do what we do in the "else" bellow - if( this->current_block->decelerate_after > 0 && this->current_block->decelerate_after < this->main_stepper->steps_to_move ) { - this->main_stepper->attach_signal_step(this->current_block->decelerate_after, this, &Stepper::synchronize_acceleration); - } - } else { - // If we are called not at the first steps, this means we are beginning deceleration - NVIC_SetPendingIRQ(TIMER2_IRQn); - // Synchronize both counters - LPC_TIM2->TC = LPC_TIM0->TC; - } - - return 0; -} diff --git a/src/modules/robot/Stepper.h b/src/modules/robot/Stepper.h index ef78cac7..db375280 100644 --- a/src/modules/robot/Stepper.h +++ b/src/modules/robot/Stepper.h @@ -12,7 +12,6 @@ #include class Block; -class Hook; class StepperMotor; class Stepper : public Module @@ -25,50 +24,27 @@ public: void on_block_end(void *argument); void on_gcode_received(void *argument); void on_gcode_execute(void *argument); - void on_play(void *argument); - void on_pause(void *argument); void on_halt(void *argument); - uint32_t main_interrupt(uint32_t dummy); + void trapezoid_generator_reset(); void set_step_events_per_second(float); - uint32_t trapezoid_generator_tick(uint32_t dummy); + void trapezoid_generator_tick(void); uint32_t stepper_motor_finished_move(uint32_t dummy); int config_step_timer( int cycles ); void turn_enable_pins_on(); void turn_enable_pins_off(); - uint32_t synchronize_acceleration(uint32_t dummy); - int get_acceleration_ticks_per_second() const { return acceleration_ticks_per_second; } - unsigned int get_minimum_steps_per_second() const { return minimum_steps_per_second; } float get_trapezoid_adjusted_rate() const { return trapezoid_adjusted_rate; } const Block *get_current_block() const { return current_block; } private: Block *current_block; - int counters[3]; - int stepped[3]; - int offsets[3]; - float counter_alpha; - float counter_beta; - float counter_gamma; - unsigned int out_bits; float trapezoid_adjusted_rate; - int trapezoid_tick_cycle_counter; - int cycles_per_step_event; - int microseconds_per_step_pulse; - int acceleration_ticks_per_second; - unsigned int minimum_steps_per_second; - int base_stepping_frequency; - unsigned short step_bits[3]; - int counter_increment; - Hook *acceleration_tick_hook; StepperMotor *main_stepper; struct { bool enable_pins_status:1; bool force_speed_update:1; - bool paused:1; - bool trapezoid_generator_busy:1; bool halted:1; }; diff --git a/src/modules/robot/arm_solutions/BaseSolution.h b/src/modules/robot/arm_solutions/BaseSolution.h index eecdd13c..406070c1 100644 --- a/src/modules/robot/arm_solutions/BaseSolution.h +++ b/src/modules/robot/arm_solutions/BaseSolution.h @@ -10,11 +10,11 @@ class BaseSolution { BaseSolution(){}; BaseSolution(Config*){}; virtual ~BaseSolution() {}; - virtual void cartesian_to_actuator( float[], float[] ) = 0; - virtual void actuator_to_cartesian( float[], float[] ) = 0; + virtual void cartesian_to_actuator(const float[], float[] ) = 0; + virtual void actuator_to_cartesian(const float[], float[] ) = 0; typedef std::map arm_options_t; virtual bool set_optional(const arm_options_t& options) { return false; }; - virtual bool get_optional(arm_options_t& options) { return false; }; + virtual bool get_optional(arm_options_t& options, bool force_all= false) { return false; }; }; #endif diff --git a/src/modules/robot/arm_solutions/CartesianSolution.cpp b/src/modules/robot/arm_solutions/CartesianSolution.cpp index 5308700d..cef25d43 100644 --- a/src/modules/robot/arm_solutions/CartesianSolution.cpp +++ b/src/modules/robot/arm_solutions/CartesianSolution.cpp @@ -1,13 +1,13 @@ #include "CartesianSolution.h" #include -void CartesianSolution::cartesian_to_actuator( float cartesian_mm[], float actuator_mm[] ){ +void CartesianSolution::cartesian_to_actuator( const float cartesian_mm[], float actuator_mm[] ){ actuator_mm[ALPHA_STEPPER] = cartesian_mm[X_AXIS]; actuator_mm[BETA_STEPPER ] = cartesian_mm[Y_AXIS]; actuator_mm[GAMMA_STEPPER] = cartesian_mm[Z_AXIS]; } -void CartesianSolution::actuator_to_cartesian( float actuator_mm[], float cartesian_mm[] ){ +void CartesianSolution::actuator_to_cartesian( const float actuator_mm[], float cartesian_mm[] ){ cartesian_mm[ALPHA_STEPPER] = actuator_mm[X_AXIS]; cartesian_mm[BETA_STEPPER ] = actuator_mm[Y_AXIS]; cartesian_mm[GAMMA_STEPPER] = actuator_mm[Z_AXIS]; diff --git a/src/modules/robot/arm_solutions/CartesianSolution.h b/src/modules/robot/arm_solutions/CartesianSolution.h index c2d04cb7..2f82c7e7 100644 --- a/src/modules/robot/arm_solutions/CartesianSolution.h +++ b/src/modules/robot/arm_solutions/CartesianSolution.h @@ -11,8 +11,8 @@ class CartesianSolution : public BaseSolution { public: CartesianSolution(){}; CartesianSolution(Config*){}; - void cartesian_to_actuator( float millimeters[], float steps[] ); - void actuator_to_cartesian( float steps[], float millimeters[] ); + void cartesian_to_actuator( const float millimeters[], float steps[] ); + void actuator_to_cartesian( const float steps[], float millimeters[] ); }; diff --git a/src/modules/robot/arm_solutions/CoreXZSolution.cpp b/src/modules/robot/arm_solutions/CoreXZSolution.cpp new file mode 100644 index 00000000..c7660915 --- /dev/null +++ b/src/modules/robot/arm_solutions/CoreXZSolution.cpp @@ -0,0 +1,24 @@ +#include "CoreXZSolution.h" +#include "ConfigValue.h" +#include "checksumm.h" + +#define x_reduction_checksum CHECKSUM("x_reduction") +#define z_reduction_checksum CHECKSUM("z_reduction") + +CoreXZSolution::CoreXZSolution(Config* config) +{ + x_reduction = config->value(x_reduction_checksum)->by_default(1.0f)->as_number(); + z_reduction = config->value(z_reduction_checksum)->by_default(3.0f)->as_number(); +} + +void CoreXZSolution::cartesian_to_actuator(const float cartesian_mm[], float actuator_mm[] ){ + actuator_mm[ALPHA_STEPPER] = (this->x_reduction * cartesian_mm[X_AXIS]) + (this->z_reduction * cartesian_mm[Z_AXIS]); + actuator_mm[BETA_STEPPER ] = (this->x_reduction * cartesian_mm[X_AXIS]) - (this->z_reduction * cartesian_mm[Z_AXIS]); + actuator_mm[GAMMA_STEPPER] = cartesian_mm[Y_AXIS]; +} + +void CoreXZSolution::actuator_to_cartesian(const float actuator_mm[], float cartesian_mm[] ){ + cartesian_mm[X_AXIS] = (0.5F/this->x_reduction) * (actuator_mm[ALPHA_STEPPER] + actuator_mm[BETA_STEPPER]); + cartesian_mm[Z_AXIS] = (0.5F/this->z_reduction) * (actuator_mm[ALPHA_STEPPER] - actuator_mm[BETA_STEPPER]); + cartesian_mm[Y_AXIS] = actuator_mm[GAMMA_STEPPER]; +} diff --git a/src/modules/robot/arm_solutions/CoreXZSolution.h b/src/modules/robot/arm_solutions/CoreXZSolution.h new file mode 100644 index 00000000..212e0712 --- /dev/null +++ b/src/modules/robot/arm_solutions/CoreXZSolution.h @@ -0,0 +1,21 @@ +#ifndef COREXZSOLUTION_H +#define COREXZSOLUTION_H +#include "libs/Module.h" +#include "libs/Kernel.h" +#include "BaseSolution.h" +#include "libs/nuts_bolts.h" + +#include "libs/Config.h" + +class CoreXZSolution : public BaseSolution { + public: + CoreXZSolution(Config*); + void cartesian_to_actuator(const float[], float[] ); + void actuator_to_cartesian(const float[], float[] ); + + private: + float x_reduction; + float z_reduction; +}; + +#endif // COREXZSOLUTION_H diff --git a/src/modules/robot/arm_solutions/ExperimentalDeltaSolution.cpp b/src/modules/robot/arm_solutions/ExperimentalDeltaSolution.cpp index b7d72dc9..c995d1c1 100644 --- a/src/modules/robot/arm_solutions/ExperimentalDeltaSolution.cpp +++ b/src/modules/robot/arm_solutions/ExperimentalDeltaSolution.cpp @@ -37,7 +37,7 @@ ExperimentalDeltaSolution::ExperimentalDeltaSolution(Config* config) arm_length_squared = powf(arm_length, 2); } -void ExperimentalDeltaSolution::cartesian_to_actuator( float cartesian_mm[], float actuator_mm[] ){ +void ExperimentalDeltaSolution::cartesian_to_actuator(const float cartesian_mm[], float actuator_mm[] ){ float alpha_rotated[3], rotated[3]; if( sin_alpha == 0 && cos_alpha == 1){ @@ -56,7 +56,7 @@ void ExperimentalDeltaSolution::cartesian_to_actuator( float cartesian_mm[], flo actuator_mm[GAMMA_STEPPER] = solve_arm( rotated ); } -void ExperimentalDeltaSolution::actuator_to_cartesian( float actuator_mm[], float cartesian_mm[] ){ +void ExperimentalDeltaSolution::actuator_to_cartesian(const float actuator_mm[], float cartesian_mm[] ){ // unimplemented } @@ -64,7 +64,7 @@ float ExperimentalDeltaSolution::solve_arm( float cartesian_mm[]) { return sqrtf(arm_length_squared - powf(cartesian_mm[X_AXIS] - arm_radius, 2) - powf(cartesian_mm[Y_AXIS], 2)) + cartesian_mm[Z_AXIS]; } -void ExperimentalDeltaSolution::rotate(float in[], float out[], float sin, float cos ){ +void ExperimentalDeltaSolution::rotate(const float in[], float out[], float sin, float cos ){ out[X_AXIS] = cos * in[X_AXIS] - sin * in[Y_AXIS]; out[Y_AXIS] = sin * in[X_AXIS] + cos * in[Y_AXIS]; out[Z_AXIS] = in[Z_AXIS]; diff --git a/src/modules/robot/arm_solutions/ExperimentalDeltaSolution.h b/src/modules/robot/arm_solutions/ExperimentalDeltaSolution.h index 5479997e..595c1246 100644 --- a/src/modules/robot/arm_solutions/ExperimentalDeltaSolution.h +++ b/src/modules/robot/arm_solutions/ExperimentalDeltaSolution.h @@ -8,11 +8,11 @@ class Config; class ExperimentalDeltaSolution : public BaseSolution { public: ExperimentalDeltaSolution(Config*); - void cartesian_to_actuator( float[], float[] ); - void actuator_to_cartesian( float[], float[] ); + void cartesian_to_actuator(const float[], float[] ); + void actuator_to_cartesian(const float[], float[] ); float solve_arm( float millimeters[] ); - void rotate( float in[], float out[], float sin, float cos ); + void rotate(const float in[], float out[], float sin, float cos ); private: float arm_length; diff --git a/src/modules/robot/arm_solutions/HBotSolution.cpp b/src/modules/robot/arm_solutions/HBotSolution.cpp index b22cf4ec..106e6d1a 100644 --- a/src/modules/robot/arm_solutions/HBotSolution.cpp +++ b/src/modules/robot/arm_solutions/HBotSolution.cpp @@ -1,13 +1,13 @@ #include "HBotSolution.h" #include -void HBotSolution::cartesian_to_actuator( float cartesian_mm[], float actuator_mm[] ){ +void HBotSolution::cartesian_to_actuator(const float cartesian_mm[], float actuator_mm[] ){ actuator_mm[ALPHA_STEPPER] = cartesian_mm[X_AXIS] + cartesian_mm[Y_AXIS]; actuator_mm[BETA_STEPPER ] = cartesian_mm[X_AXIS] - cartesian_mm[Y_AXIS]; actuator_mm[GAMMA_STEPPER] = cartesian_mm[Z_AXIS]; } -void HBotSolution::actuator_to_cartesian( float actuator_mm[], float cartesian_mm[] ){ +void HBotSolution::actuator_to_cartesian(const float actuator_mm[], float cartesian_mm[] ){ cartesian_mm[X_AXIS] = 0.5F * (actuator_mm[ALPHA_STEPPER] + actuator_mm[BETA_STEPPER]); cartesian_mm[Y_AXIS] = 0.5F * (actuator_mm[ALPHA_STEPPER] - actuator_mm[BETA_STEPPER]); cartesian_mm[Z_AXIS] = actuator_mm[GAMMA_STEPPER]; diff --git a/src/modules/robot/arm_solutions/HBotSolution.h b/src/modules/robot/arm_solutions/HBotSolution.h index 24771bb5..c35eca27 100644 --- a/src/modules/robot/arm_solutions/HBotSolution.h +++ b/src/modules/robot/arm_solutions/HBotSolution.h @@ -11,8 +11,8 @@ class HBotSolution : public BaseSolution { public: HBotSolution(); HBotSolution(Config*){}; - void cartesian_to_actuator( float[], float[] ); - void actuator_to_cartesian( float[], float[] ); + void cartesian_to_actuator(const float[], float[] ); + void actuator_to_cartesian(const float[], float[] ); }; diff --git a/src/modules/robot/arm_solutions/LinearDeltaSolution.cpp b/src/modules/robot/arm_solutions/LinearDeltaSolution.cpp index 2ce55e6a..69617407 100644 --- a/src/modules/robot/arm_solutions/LinearDeltaSolution.cpp +++ b/src/modules/robot/arm_solutions/LinearDeltaSolution.cpp @@ -1,27 +1,41 @@ #include "LinearDeltaSolution.h" -#include #include "checksumm.h" #include "ConfigValue.h" #include "libs/Kernel.h" - #include "libs/nuts_bolts.h" - #include "libs/Config.h" + +#include #include "Vector3.h" + #define arm_length_checksum CHECKSUM("arm_length") #define arm_radius_checksum CHECKSUM("arm_radius") +#define tower1_offset_checksum CHECKSUM("delta_tower1_offset") +#define tower2_offset_checksum CHECKSUM("delta_tower2_offset") +#define tower3_offset_checksum CHECKSUM("delta_tower3_offset") +#define tower1_angle_checksum CHECKSUM("delta_tower1_angle") +#define tower2_angle_checksum CHECKSUM("delta_tower2_angle") +#define tower3_angle_checksum CHECKSUM("delta_tower3_angle") #define SQ(x) powf(x, 2) -#define ROUND(x, y) (roundf(x * 1e ## y) / 1e ## y) +#define ROUND(x, y) (roundf(x * (float)(1e ## y)) / (float)(1e ## y)) +#define PIOVER180 0.01745329251994329576923690768489F LinearDeltaSolution::LinearDeltaSolution(Config* config) { // arm_length is the length of the arm from hinge to hinge - arm_length = config->value(arm_length_checksum)->by_default(250.0f)->as_number(); + arm_length = config->value(arm_length_checksum)->by_default(250.0f)->as_number(); // arm_radius is the horizontal distance from hinge to hinge when the effector is centered - arm_radius = config->value(arm_radius_checksum)->by_default(124.0f)->as_number(); + arm_radius = config->value(arm_radius_checksum)->by_default(124.0f)->as_number(); + + tower1_angle = config->value(tower1_angle_checksum)->by_default(0.0f)->as_number(); + tower2_angle = config->value(tower2_angle_checksum)->by_default(0.0f)->as_number(); + tower3_angle = config->value(tower3_angle_checksum)->by_default(0.0f)->as_number(); + tower1_offset = config->value(tower1_offset_checksum)->by_default(0.0f)->as_number(); + tower2_offset = config->value(tower2_offset_checksum)->by_default(0.0f)->as_number(); + tower3_offset = config->value(tower3_offset_checksum)->by_default(0.0f)->as_number(); init(); } @@ -29,45 +43,41 @@ LinearDeltaSolution::LinearDeltaSolution(Config* config) void LinearDeltaSolution::init() { arm_length_squared = SQ(arm_length); - // Effective X/Y positions of the three vertical towers. - float DELTA_RADIUS = arm_radius; - - float SIN_60 = 0.8660254037844386F; - float COS_60 = 0.5F; - - DELTA_TOWER1_X = -SIN_60 * DELTA_RADIUS; // front left tower - DELTA_TOWER1_Y = -COS_60 * DELTA_RADIUS; + // Effective X/Y positions of the three vertical towers. + float delta_radius = arm_radius; - DELTA_TOWER2_X = SIN_60 * DELTA_RADIUS; // front right tower - DELTA_TOWER2_Y = -COS_60 * DELTA_RADIUS; - - DELTA_TOWER3_X = 0.0F; // back middle tower - DELTA_TOWER3_Y = DELTA_RADIUS; + delta_tower1_x = (delta_radius + tower1_offset) * cosf((210.0F + tower1_angle) * PIOVER180); // front left tower + delta_tower1_y = (delta_radius + tower1_offset) * sinf((210.0F + tower1_angle) * PIOVER180); + delta_tower2_x = (delta_radius + tower2_offset) * cosf((330.0F + tower2_angle) * PIOVER180); // front right tower + delta_tower2_y = (delta_radius + tower2_offset) * sinf((330.0F + tower2_angle) * PIOVER180); + delta_tower3_x = (delta_radius + tower3_offset) * cosf((90.0F + tower3_angle) * PIOVER180); // back middle tower + delta_tower3_y = (delta_radius + tower3_offset) * sinf((90.0F + tower3_angle) * PIOVER180); } -void LinearDeltaSolution::cartesian_to_actuator( float cartesian_mm[], float actuator_mm[] ) +void LinearDeltaSolution::cartesian_to_actuator(const float cartesian_mm[], float actuator_mm[] ) { + actuator_mm[ALPHA_STEPPER] = sqrtf(this->arm_length_squared - - SQ(DELTA_TOWER1_X - cartesian_mm[X_AXIS]) - - SQ(DELTA_TOWER1_Y - cartesian_mm[Y_AXIS]) - ) + cartesian_mm[Z_AXIS]; + - SQ(delta_tower1_x - cartesian_mm[X_AXIS]) + - SQ(delta_tower1_y - cartesian_mm[Y_AXIS]) + ) + cartesian_mm[Z_AXIS]; actuator_mm[BETA_STEPPER ] = sqrtf(this->arm_length_squared - - SQ(DELTA_TOWER2_X - cartesian_mm[X_AXIS]) - - SQ(DELTA_TOWER2_Y - cartesian_mm[Y_AXIS]) - ) + cartesian_mm[Z_AXIS]; + - SQ(delta_tower2_x - cartesian_mm[X_AXIS]) + - SQ(delta_tower2_y - cartesian_mm[Y_AXIS]) + ) + cartesian_mm[Z_AXIS]; actuator_mm[GAMMA_STEPPER] = sqrtf(this->arm_length_squared - - SQ(DELTA_TOWER3_X - cartesian_mm[X_AXIS]) - - SQ(DELTA_TOWER3_Y - cartesian_mm[Y_AXIS]) - ) + cartesian_mm[Z_AXIS]; + - SQ(delta_tower3_x - cartesian_mm[X_AXIS]) + - SQ(delta_tower3_y - cartesian_mm[Y_AXIS]) + ) + cartesian_mm[Z_AXIS]; } -void LinearDeltaSolution::actuator_to_cartesian( float actuator_mm[], float cartesian_mm[] ) +void LinearDeltaSolution::actuator_to_cartesian(const float actuator_mm[], float cartesian_mm[] ) { // from http://en.wikipedia.org/wiki/Circumscribed_circle#Barycentric_coordinates_from_cross-_and_dot-products // based on https://github.com/ambrop72/aprinter/blob/2de69a/aprinter/printer/DeltaTransform.h#L81 - Vector3 tower1( DELTA_TOWER1_X, DELTA_TOWER1_Y, actuator_mm[0] ); - Vector3 tower2( DELTA_TOWER2_X, DELTA_TOWER2_Y, actuator_mm[1] ); - Vector3 tower3( DELTA_TOWER3_X, DELTA_TOWER3_Y, actuator_mm[2] ); + Vector3 tower1( delta_tower1_x, delta_tower1_y, actuator_mm[0] ); + Vector3 tower2( delta_tower2_x, delta_tower2_y, actuator_mm[1] ); + Vector3 tower3( delta_tower3_x, delta_tower3_y, actuator_mm[2] ); Vector3 s12 = tower1.sub(tower2); Vector3 s23 = tower2.sub(tower3); @@ -86,8 +96,8 @@ void LinearDeltaSolution::actuator_to_cartesian( float actuator_mm[], float cart float b = q * magsq_s13 * s12.dot(s23) * -1.0F; // negate because we use s12 instead of s21 float c = q * magsq_s12 * s13.dot(s23); - Vector3 circumcenter( DELTA_TOWER1_X * a + DELTA_TOWER2_X * b + DELTA_TOWER3_X * c, - DELTA_TOWER1_Y * a + DELTA_TOWER2_Y * b + DELTA_TOWER3_Y * c, + Vector3 circumcenter( delta_tower1_x * a + delta_tower2_x * b + delta_tower3_x * c, + delta_tower1_y * a + delta_tower2_y * b + delta_tower3_y * c, actuator_mm[0] * a + actuator_mm[1] * b + actuator_mm[2] * c ); float r_sq = 0.5F * q * magsq_s12 * magsq_s23 * magsq_s13; @@ -102,23 +112,38 @@ void LinearDeltaSolution::actuator_to_cartesian( float actuator_mm[], float cart bool LinearDeltaSolution::set_optional(const arm_options_t& options) { - arm_options_t::const_iterator i; - - i= options.find('L'); - if(i != options.end()) { - arm_length= i->second; - - } - i= options.find('R'); - if(i != options.end()) { - arm_radius= i->second; + for(auto &i : options) { + switch(i.first) { + case 'L': arm_length= i.second; break; + case 'R': arm_radius= i.second; break; + case 'A': tower1_offset = i.second; break; + case 'B': tower2_offset = i.second; break; + case 'C': tower3_offset = i.second; break; + case 'D': tower1_angle = i.second; break; + case 'E': tower2_angle = i.second; break; + case 'F': tower3_angle = i.second; break; // WARNING this will be deprecated + case 'H': tower3_angle = i.second; break; + } } init(); return true; } -bool LinearDeltaSolution::get_optional(arm_options_t& options) { +bool LinearDeltaSolution::get_optional(arm_options_t& options, bool force_all) { options['L']= this->arm_length; options['R']= this->arm_radius; + + // don't report these if none of them are set + if(force_all || (this->tower1_offset != 0.0F || this->tower2_offset != 0.0F || this->tower3_offset != 0.0F || + this->tower1_angle != 0.0F || this->tower2_angle != 0.0F || this->tower3_angle != 0.0F) ) { + + options['A'] = this->tower1_offset; + options['B'] = this->tower2_offset; + options['C'] = this->tower3_offset; + options['D'] = this->tower1_angle; + options['E'] = this->tower2_angle; + options['H'] = this->tower3_angle; + } + return true; }; diff --git a/src/modules/robot/arm_solutions/LinearDeltaSolution.h b/src/modules/robot/arm_solutions/LinearDeltaSolution.h index e7f78682..05639dcb 100644 --- a/src/modules/robot/arm_solutions/LinearDeltaSolution.h +++ b/src/modules/robot/arm_solutions/LinearDeltaSolution.h @@ -8,11 +8,11 @@ class Config; class LinearDeltaSolution : public BaseSolution { public: LinearDeltaSolution(Config*); - void cartesian_to_actuator( float[], float[] ); - void actuator_to_cartesian( float[], float[] ); + void cartesian_to_actuator(const float[], float[] ); + void actuator_to_cartesian(const float[], float[] ); bool set_optional(const arm_options_t& options); - bool get_optional(arm_options_t& options); + bool get_optional(arm_options_t& options, bool force_all); private: void init(); @@ -21,11 +21,17 @@ class LinearDeltaSolution : public BaseSolution { float arm_radius; float arm_length_squared; - float DELTA_TOWER1_X; - float DELTA_TOWER1_Y; - float DELTA_TOWER2_X; - float DELTA_TOWER2_Y; - float DELTA_TOWER3_X; - float DELTA_TOWER3_Y; + float delta_tower1_x; + float delta_tower1_y; + float delta_tower2_x; + float delta_tower2_y; + float delta_tower3_x; + float delta_tower3_y; + float tower1_offset; + float tower2_offset; + float tower3_offset; + float tower1_angle; + float tower2_angle; + float tower3_angle; }; #endif // LINEARDELTASOLUTION_H diff --git a/src/modules/robot/arm_solutions/MorganSCARASolution.cpp b/src/modules/robot/arm_solutions/MorganSCARASolution.cpp index d96913b4..1c4ec1e8 100644 --- a/src/modules/robot/arm_solutions/MorganSCARASolution.cpp +++ b/src/modules/robot/arm_solutions/MorganSCARASolution.cpp @@ -3,7 +3,7 @@ #include "checksumm.h" #include "ConfigValue.h" #include "libs/Kernel.h" -//#include "StreamOutputPool.h" +#include "StreamOutputPool.h" //#include "Gcode.h" //#include "SerialMessage.h" //#include "Conveyor.h" @@ -14,13 +14,15 @@ #include "libs/Config.h" -#define arm1_length_checksum CHECKSUM("arm1_length") -#define arm2_length_checksum CHECKSUM("arm2_length") -#define morgan_offset_x_checksum CHECKSUM("morgan_offset_x") -#define morgan_offset_y_checksum CHECKSUM("morgan_offset_y") -#define axis_scaling_x_checksum CHECKSUM("axis_scaling_x") -#define axis_scaling_y_checksum CHECKSUM("axis_scaling_y") -#define morgan_homing_checksum CHECKSUM("morgan_homing") +#define arm1_length_checksum CHECKSUM("arm1_length") +#define arm2_length_checksum CHECKSUM("arm2_length") +#define morgan_offset_x_checksum CHECKSUM("morgan_offset_x") +#define morgan_offset_y_checksum CHECKSUM("morgan_offset_y") +#define morgan_scaling_x_checksum CHECKSUM("morgan_scaling_x") +#define morgan_scaling_y_checksum CHECKSUM("morgan_scaling_y") +#define morgan_homing_checksum CHECKSUM("morgan_homing") +#define morgan_undefined_min_checksum CHECKSUM("morgan_undefined_min") +#define morgan_undefined_max_checksum CHECKSUM("morgan_undefined_max") #define SQ(x) powf(x, 2) #define ROUND(x, y) (roundf(x * 1e ## y) / 1e ## y) @@ -34,7 +36,16 @@ MorganSCARASolution::MorganSCARASolution(Config* config) // morgan_offset_x is the x offset of bed zero position towards the SCARA tower center morgan_offset_x = config->value(morgan_offset_x_checksum)->by_default(100.0f)->as_number(); // morgan_offset_y is the y offset of bed zero position towards the SCARA tower center - morgan_offset_y = config->value(morgan_offset_y_checksum)->by_default(-65.0f)->as_number(); + morgan_offset_y = config->value(morgan_offset_y_checksum)->by_default(-60.0f)->as_number(); + // Axis scaling is used in final calibration + morgan_scaling_x = config->value(morgan_scaling_x_checksum)->by_default(1.0F)->as_number(); // 1 = 100% : No scaling + morgan_scaling_y = config->value(morgan_scaling_y_checksum)->by_default(1.0F)->as_number(); + // morgan_undefined is the ratio at which the SCARA position is undefined. + // required to prevent the arm moving through singularity points + // min: head close to tower + morgan_undefined_min = config->value(morgan_undefined_min_checksum)->by_default(0.95f)->as_number(); + // max: head on maximum reach + morgan_undefined_max = config->value(morgan_undefined_max_checksum)->by_default(0.95f)->as_number(); init(); } @@ -47,7 +58,7 @@ float MorganSCARASolution::to_degrees(float radians) { return radians*(180.0F/3.14159265359f); } -void MorganSCARASolution::cartesian_to_actuator( float cartesian_mm[], float actuator_mm[] ) +void MorganSCARASolution::cartesian_to_actuator(const float cartesian_mm[], float actuator_mm[] ) { float SCARA_pos[2], @@ -57,33 +68,34 @@ void MorganSCARASolution::cartesian_to_actuator( float cartesian_mm[], float act SCARA_K2, SCARA_theta, SCARA_psi; - - SCARA_pos[X_AXIS] = cartesian_mm[X_AXIS] - this->morgan_offset_x; //Translate cartesian to tower centric SCARA X Y - SCARA_pos[Y_AXIS] = cartesian_mm[Y_AXIS] - this->morgan_offset_y; // morgan_offset not to be confused with home offset. Makes the SCARA math work. - + + SCARA_pos[X_AXIS] = (cartesian_mm[X_AXIS] - this->morgan_offset_x) * this->morgan_scaling_x; //Translate cartesian to tower centric SCARA X Y AND apply scaling factor from this offset. + SCARA_pos[Y_AXIS] = (cartesian_mm[Y_AXIS] * this->morgan_scaling_y - this->morgan_offset_y); // morgan_offset not to be confused with home offset. This makes the SCARA math work. + // Y has to be scaled before subtracting offset to ensure position on bed. + if (this->arm1_length == this->arm2_length) SCARA_C2 = (SQ(SCARA_pos[X_AXIS])+SQ(SCARA_pos[Y_AXIS])-2.0f*SQ(this->arm1_length)) / (2.0f * SQ(this->arm1_length)); else SCARA_C2 = (SQ(SCARA_pos[X_AXIS])+SQ(SCARA_pos[Y_AXIS])-SQ(this->arm1_length)-SQ(this->arm2_length)) / (2.0f * SQ(this->arm1_length)); // SCARA position is undefined if abs(SCARA_C2) >=1 - // In reality abs(SCARA_C2) >0.95 is problematic. + // In reality abs(SCARA_C2) >0.95 can be problematic. + + if (SCARA_C2 > this->morgan_undefined_max) + SCARA_C2 = this->morgan_undefined_max; + else if (SCARA_C2 < -this->morgan_undefined_min) + SCARA_C2 = -this->morgan_undefined_min; - if (SCARA_C2 > 0.95f) - SCARA_C2 = 0.95f; - else if (SCARA_C2 < -0.95f) - SCARA_C2 = -0.95f; - SCARA_S2 = sqrtf(1.0f-SQ(SCARA_C2)); SCARA_K1 = this->arm1_length+this->arm2_length*SCARA_C2; SCARA_K2 = this->arm2_length*SCARA_S2; - + SCARA_theta = (atan2f(SCARA_pos[X_AXIS],SCARA_pos[Y_AXIS])-atan2f(SCARA_K1, SCARA_K2))*-1.0f; // Morgan Thomas turns Theta in oposite direction SCARA_psi = atan2f(SCARA_S2,SCARA_C2); - - + + actuator_mm[ALPHA_STEPPER] = to_degrees(SCARA_theta); // Multiply by 180/Pi - theta is support arm angle actuator_mm[BETA_STEPPER ] = to_degrees(SCARA_theta + SCARA_psi); // Morgan kinematics (dual arm) //actuator_mm[BETA_STEPPER ] = to_degrees(SCARA_psi); // real scara @@ -91,9 +103,9 @@ void MorganSCARASolution::cartesian_to_actuator( float cartesian_mm[], float act } -void MorganSCARASolution::actuator_to_cartesian( float actuator_mm[], float cartesian_mm[] ) { +void MorganSCARASolution::actuator_to_cartesian(const float actuator_mm[], float cartesian_mm[] ) { // Perform forward kinematics, and place results in cartesian_mm[] - + float y1, y2, actuator_rad[2]; @@ -102,14 +114,14 @@ void MorganSCARASolution::actuator_to_cartesian( float actuator_mm[], float cart y1 = sinf(actuator_rad[X_AXIS])*this->arm1_length; y2 = sinf(actuator_rad[Y_AXIS])*this->arm2_length + y1; - - cartesian_mm[X_AXIS] = cosf(actuator_rad[X_AXIS])*this->arm1_length + cosf(actuator_rad[Y_AXIS])*this->arm2_length + this->morgan_offset_x; - cartesian_mm[Y_AXIS] = y2 + this->morgan_offset_y; + + cartesian_mm[X_AXIS] = (((cosf(actuator_rad[X_AXIS])*this->arm1_length) + (cosf(actuator_rad[Y_AXIS])*this->arm2_length)) / this->morgan_scaling_x) + this->morgan_offset_x; + cartesian_mm[Y_AXIS] = (y2 + this->morgan_offset_y) / this->morgan_scaling_y; cartesian_mm[Z_AXIS] = actuator_mm[Z_AXIS]; - cartesian_mm[0] = ROUND(cartesian_mm[0], 4); - cartesian_mm[1] = ROUND(cartesian_mm[1], 4); - cartesian_mm[2] = ROUND(cartesian_mm[2], 4); + cartesian_mm[0] = ROUND(cartesian_mm[0], 7); + cartesian_mm[1] = ROUND(cartesian_mm[1], 7); + cartesian_mm[2] = ROUND(cartesian_mm[2], 7); } bool MorganSCARASolution::set_optional(const arm_options_t& options) { @@ -133,15 +145,41 @@ bool MorganSCARASolution::set_optional(const arm_options_t& options) { if(i != options.end()) { morgan_offset_y= i->second; } - + i= options.find('A'); // Scaling X_AXIS + if(i != options.end()) { + morgan_scaling_x= i->second; + } + i= options.find('B'); // Scaling Y_AXIS + if(i != options.end()) { + morgan_scaling_y= i->second; + } + //i= options.find('C'); // Scaling Z_AXIS + //if(i != options.end()) { + // morgan_scaling_z= i->second; + //} + i= options.find('D'); // Undefined min + if(i != options.end()) { + this->morgan_undefined_min = i->second; + } + i= options.find('E'); // undefined max + if(i != options.end()) { + this->morgan_undefined_max = i->second; + } + init(); return true; } -bool MorganSCARASolution::get_optional(arm_options_t& options) { +bool MorganSCARASolution::get_optional(arm_options_t& options, bool force_all) { options['T']= this->arm1_length; options['P']= this->arm2_length; options['X']= this->morgan_offset_x; options['Y']= this->morgan_offset_y; + options['A']= this->morgan_scaling_x; + options['B']= this->morgan_scaling_y; + // options['C']= this->morgan_scaling_z; + options['D']= this->morgan_undefined_min; + options['E']= this->morgan_undefined_max; + return true; }; diff --git a/src/modules/robot/arm_solutions/MorganSCARASolution.h b/src/modules/robot/arm_solutions/MorganSCARASolution.h index a8f53785..3632e313 100644 --- a/src/modules/robot/arm_solutions/MorganSCARASolution.h +++ b/src/modules/robot/arm_solutions/MorganSCARASolution.h @@ -8,11 +8,11 @@ class Config; class MorganSCARASolution : public BaseSolution { public: MorganSCARASolution(Config*); - void cartesian_to_actuator( float[], float[] ); - void actuator_to_cartesian( float[], float[] ); + void cartesian_to_actuator(const float[], float[] ); + void actuator_to_cartesian(const float[], float[] ); bool set_optional(const arm_options_t& options); - bool get_optional(arm_options_t& options); + bool get_optional(arm_options_t& options, bool force_all); private: void init(); @@ -22,6 +22,10 @@ class MorganSCARASolution : public BaseSolution { float arm2_length; float morgan_offset_x; float morgan_offset_y; + float morgan_scaling_x; + float morgan_scaling_y; + float morgan_undefined_min; + float morgan_undefined_max; float slow_rate; }; diff --git a/src/modules/robot/arm_solutions/RotatableCartesianSolution.cpp b/src/modules/robot/arm_solutions/RotatableCartesianSolution.cpp index e877a4ee..beb3adec 100644 --- a/src/modules/robot/arm_solutions/RotatableCartesianSolution.cpp +++ b/src/modules/robot/arm_solutions/RotatableCartesianSolution.cpp @@ -12,15 +12,15 @@ RotatableCartesianSolution::RotatableCartesianSolution(Config* config) { cos_alpha = cosf(alpha_angle); } -void RotatableCartesianSolution::cartesian_to_actuator( float cartesian_mm[], float actuator_mm[] ){ +void RotatableCartesianSolution::cartesian_to_actuator(const float cartesian_mm[], float actuator_mm[] ){ rotate( cartesian_mm, actuator_mm, sin_alpha, cos_alpha ); } -void RotatableCartesianSolution::actuator_to_cartesian( float actuator_mm[], float cartesian_mm[] ){ +void RotatableCartesianSolution::actuator_to_cartesian(const float actuator_mm[], float cartesian_mm[] ){ rotate( actuator_mm, cartesian_mm, - sin_alpha, cos_alpha ); } -void RotatableCartesianSolution::rotate(float in[], float out[], float sin, float cos ){ +void RotatableCartesianSolution::rotate(const float in[], float out[], float sin, float cos ){ out[ALPHA_STEPPER] = cos * in[X_AXIS] - sin * in[Y_AXIS]; out[BETA_STEPPER ] = sin * in[X_AXIS] + cos * in[Y_AXIS]; out[GAMMA_STEPPER] = in[Z_AXIS]; diff --git a/src/modules/robot/arm_solutions/RotatableCartesianSolution.h b/src/modules/robot/arm_solutions/RotatableCartesianSolution.h index bbd443a9..83d04ce5 100644 --- a/src/modules/robot/arm_solutions/RotatableCartesianSolution.h +++ b/src/modules/robot/arm_solutions/RotatableCartesianSolution.h @@ -12,10 +12,10 @@ class RotatableCartesianSolution : public BaseSolution { public: RotatableCartesianSolution(Config*); - void cartesian_to_actuator( float[], float[] ); - void actuator_to_cartesian( float[], float[] ); + void cartesian_to_actuator(const float[], float[] ); + void actuator_to_cartesian(const float[], float[] ); - void rotate( float in[], float out[], float sin, float cos ); + void rotate(const float in[], float out[], float sin, float cos ); float sin_alpha; float cos_alpha; diff --git a/src/modules/robot/arm_solutions/RotatableDeltaSolution.cpp b/src/modules/robot/arm_solutions/RotatableDeltaSolution.cpp new file mode 100644 index 00000000..0b1992d7 --- /dev/null +++ b/src/modules/robot/arm_solutions/RotatableDeltaSolution.cpp @@ -0,0 +1,226 @@ +#include "RotatableDeltaSolution.h" +#include "checksumm.h" +#include "ConfigValue.h" +#include "ConfigCache.h" +#include "libs/Kernel.h" +#include "libs/nuts_bolts.h" +#include "libs/Config.h" +#include "libs/utils.h" +#include "StreamOutputPool.h" +#include + +#define delta_e_checksum CHECKSUM("delta_e_checksum") +#define delta_f_checksum CHECKSUM("delta_f_checksum") +#define delta_re_checksum CHECKSUM("delta_re_checksum") +#define delta_rf_checksum CHECKSUM("delta_rf_checksum") +#define delta_z_offset_checksum CHECKSUM("delta_z_offset_checksum") + +#define delta_ee_offs_checksum CHECKSUM("delta_ee_offs_checksum") +#define tool_offset_checksum CHECKSUM("tool_offset_checksum") + +const static float pi = 3.14159265358979323846; // PI +const static float two_pi = 2 * pi; +const static float sin120 = 0.86602540378443864676372317075294; //sqrt3/2.0 +const static float cos120 = -0.5; +const static float tan60 = 1.7320508075688772935274463415059; //sqrt3; +const static float sin30 = 0.5; +const static float tan30 = 0.57735026918962576450914878050196; //1/sqrt3 + +RotatableDeltaSolution::RotatableDeltaSolution(Config *config) +{ + // End effector length + delta_e = config->value(delta_e_checksum)->by_default(131.636F)->as_number(); + + // Base length + delta_f = config->value(delta_f_checksum)->by_default(190.526F)->as_number(); + + // Carbon rod length + delta_re = config->value(delta_re_checksum)->by_default(270.000F)->as_number(); + + // Servo horn length + delta_rf = config->value(delta_rf_checksum)->by_default(90.000F)->as_number(); + + // Distance from delta 8mm rod/pulley to table/bed, + // NOTE: For OpenPnP, set the zero to be about 25mm above the bed.. + delta_z_offset = config->value(delta_z_offset_checksum)->by_default(290.700F)->as_number(); + + // Ball joint plane to bottom of end effector surface + delta_ee_offs = config->value(delta_ee_offs_checksum)->by_default(15.000F)->as_number(); + + // 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(); + + init(); +} + +// inverse kinematics +// helper functions, calculates angle theta1 (for YZ-pane) +int RotatableDeltaSolution::delta_calcAngleYZ(float x0, float y0, float z0, float &theta) +{ + float y1 = -0.5F * tan30 * delta_f; // f/2 * tan 30 + y0 -= 0.5F * tan30 * delta_e; // shift center to edge + // z = a + b*y + float a = (x0 * x0 + y0 * y0 + z0 * z0 + delta_rf * delta_rf - delta_re * delta_re - y1 * y1) / (2.0F * z0); + float b = (y1 - y0) / z0; + + float d = -(a + b * y1) * (a + b * y1) + delta_rf * (b * b * delta_rf + delta_rf); // discriminant + if (d < 0.0F) return -1; // non-existing point + + float yj = (y1 - a * b - sqrtf(d)) / (b * b + 1.0F); // choosing outer point + float zj = a + b * yj; + + theta = 180.0F * atanf(-zj / (y1 - yj)) / pi + ((yj > y1) ? 180.0F : 0.0F); + return 0; +} + +// forward kinematics: (theta1, theta2, theta3) -> (x0, y0, z0) +// returned status: 0=OK, -1=non-existing position +int RotatableDeltaSolution::delta_calcForward(float theta1, float theta2, float theta3, float &x0, float &y0, float &z0) +{ + float t = (delta_f - delta_e) * tan30 / 2.0F; + float degrees_to_radians = pi / 180.0F; + + theta1 *= degrees_to_radians; + theta2 *= degrees_to_radians; + theta3 *= degrees_to_radians; + + float y1 = -(t + delta_rf * cosf(theta1)); + float z1 = -delta_rf * sinf(theta1); + + float y2 = (t + delta_rf * cosf(theta2)) * sin30; + float x2 = y2 * tan60; + float z2 = -delta_rf * sinf(theta2); + + float y3 = (t + delta_rf * cosf(theta3)) * sin30; + float x3 = -y3 * tan60; + float z3 = -delta_rf * sinf(theta3); + + float dnm = (y2 - y1) * x3 - (y3 - y1) * x2; + + float w1 = y1 * y1 + z1 * z1; + float w2 = x2 * x2 + y2 * y2 + z2 * z2; + float w3 = x3 * x3 + y3 * y3 + z3 * z3; + + // x = (a1*z + b1)/dnm + float a1 = (z2 - z1) * (y3 - y1) - (z3 - z1) * (y2 - y1); + float b1 = -((w2 - w1) * (y3 - y1) - (w3 - w1) * (y2 - y1)) / 2.0F; + + // y = (a2*z + b2)/dnm; + float a2 = -(z2 - z1) * x3 + (z3 - z1) * x2; + float b2 = ((w2 - w1) * x3 - (w3 - w1) * x2) / 2.0F; + + // a*z^2 + b*z + c = 0 + float a = a1 * a1 + a2 * a2 + dnm * dnm; + float b = 2.0F * (a1 * b1 + a2 * (b2 - y1 * dnm) - z1 * dnm * dnm); + float c = (b2 - y1 * dnm) * (b2 - y1 * dnm) + b1 * b1 + dnm * dnm * (z1 * z1 - delta_re * delta_re); + + // discriminant + float d = b * b - (float)4.0F * a * c; + if (d < 0.0F) return -1; // non-existing point + + z0 = -(float)0.5F * (b + sqrtf(d)) / a; + x0 = (a1 * z0 + b1) / dnm; + y0 = (a2 * z0 + b2) / dnm; + + z0 += z_calc_offset; //nj + return 0; +} + + +void RotatableDeltaSolution::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) * -1.0F; +} + +void RotatableDeltaSolution::cartesian_to_actuator(const float cartesian_mm[], float actuator_mm[] ) +{ + //We need to translate the Cartesian coordinates in mm to the actuator position required in mm so the stepper motor functions + float alpha_theta = 0.0F; + 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. + + 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); + if (status == 0) status = delta_calcAngleYZ(x0 * cos120 + y0 * sin120, y0 * cos120 - x0 * sin120, z_with_offset, beta_theta); // rotate co-ordinates to +120 deg + if (status == 0) status = delta_calcAngleYZ(x0 * cos120 - y0 * sin120, y0 * cos120 + x0 * sin120, z_with_offset, gamma_theta); // rotate co-ordinates to -120 deg + + 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. + // THEKERNEL->streams->printf("ERROR: Delta calculation fail! Unable to move to:\n"); + // THEKERNEL->streams->printf(" x= %f\n",cartesian_mm[X_AXIS]); + // THEKERNEL->streams->printf(" y= %f\n",cartesian_mm[Y_AXIS]); + // THEKERNEL->streams->printf(" z= %f\n",cartesian_mm[Z_AXIS]); + // THEKERNEL->streams->printf(" CalcZ= %f\n",z_calc_offset); + // THEKERNEL->streams->printf(" Offz= %f\n",z_with_offset); + } else { + actuator_mm[ALPHA_STEPPER] = alpha_theta; + actuator_mm[BETA_STEPPER ] = beta_theta; + actuator_mm[GAMMA_STEPPER] = gamma_theta; + + // THEKERNEL->streams->printf("cartesian x= %f\n\r",cartesian_mm[X_AXIS]); + // THEKERNEL->streams->printf(" y= %f\n\r",cartesian_mm[Y_AXIS]); + // THEKERNEL->streams->printf(" z= %f\n\r",cartesian_mm[Z_AXIS]); + // THEKERNEL->streams->printf(" Offz= %f\n\r",z_with_offset); + // THEKERNEL->streams->printf(" delta x= %f\n\r",delta[X_AXIS]); + // THEKERNEL->streams->printf(" y= %f\n\r",delta[Y_AXIS]); + // THEKERNEL->streams->printf(" z= %f\n\r",delta[Z_AXIS]); + } + +} + +void RotatableDeltaSolution::actuator_to_cartesian(const float actuator_mm[], float cartesian_mm[] ) +{ + //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]); +} + +bool RotatableDeltaSolution::set_optional(const arm_options_t &options) +{ + + for(auto &i : options) { + switch(i.first) { + case 'A': delta_e = i.second; break; + case 'B': delta_f = i.second; break; + case 'C': delta_re = i.second; break; + case 'D': delta_rf = i.second; break; + case 'E': delta_z_offset = i.second; break; + case 'F': delta_ee_offs = i.second; break; + case 'H': tool_offset = i.second; break; + } + } + init(); + return true; +} + +bool RotatableDeltaSolution::get_optional(arm_options_t &options, bool force_all) +{ + + // don't report these if none of them are set + if(force_all || (this->delta_e != 0.0F || this->delta_f != 0.0F || this->delta_re != 0.0F || + this->delta_rf != 0.0F || this->delta_z_offset != 0.0F || this->delta_ee_offs != 0.0F || + this->tool_offset != 0.0F) ) { + + options['A'] = this->delta_e; + options['B'] = this->delta_f; + options['C'] = this->delta_re; + options['D'] = this->delta_rf; + options['E'] = this->delta_z_offset; + options['F'] = this->delta_ee_offs; + options['H'] = this->tool_offset; + } + + return true; +}; + diff --git a/src/modules/robot/arm_solutions/RotatableDeltaSolution.h b/src/modules/robot/arm_solutions/RotatableDeltaSolution.h new file mode 100644 index 00000000..3da04f74 --- /dev/null +++ b/src/modules/robot/arm_solutions/RotatableDeltaSolution.h @@ -0,0 +1,33 @@ +#ifndef RotatableDeltaSolution_H +#define RotatableDeltaSolution_H +#include "libs/Module.h" +#include "BaseSolution.h" + +class Config; + +class RotatableDeltaSolution : public BaseSolution { + public: + RotatableDeltaSolution(Config*); + void cartesian_to_actuator(const float[], float[] ); + void actuator_to_cartesian(const float[], float[] ); + + bool set_optional(const arm_options_t& options); + bool get_optional(arm_options_t& options, bool force_all); + + private: + void init(); + int delta_calcAngleYZ(float x0, float y0, float z0, float &theta); + int delta_calcForward(float theta1, float theta2, float theta3, float &x0, float &y0, float &z0); + + float delta_e; // End effector length + float delta_f; // Base length + float delta_re; // Carbon rod length + float delta_rf; // Servo horn length + float delta_z_offset ; // Distance from delta 8mm rod/pulley to table/bed + // NOTE: For OpenPnP, set the zero to be about 25mm above the bed + + float delta_ee_offs; // Ball joint plane to bottom of end effector surface + float tool_offset; // Distance between end effector ball joint plane and tip of tool + float z_calc_offset; +}; +#endif // RotatableDeltaSolution_H diff --git a/src/modules/tools/drillingcycles/Drillingcycles.cpp b/src/modules/tools/drillingcycles/Drillingcycles.cpp new file mode 100644 index 00000000..7cc35016 --- /dev/null +++ b/src/modules/tools/drillingcycles/Drillingcycles.cpp @@ -0,0 +1,246 @@ +/* + 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 "Kernel.h" +#include "Drillingcycles.h" +#include "checksumm.h" +#include "Config.h" +#include "ConfigValue.h" +#include "Gcode.h" +#include "Robot.h" +#include "Conveyor.h" +#include "SlowTicker.h" +#include "StepperMotor.h" +#include "StreamOutputPool.h" +#include /* fmod */ + +// axis index +#define X_AXIS 0 +#define Y_AXIS 1 +#define Z_AXIS 2 + +// retract modes +#define RETRACT_TO_Z 0 +#define RETRACT_TO_R 1 + +// dwell units +#define DWELL_UNITS_S 0 // seconds +#define DWELL_UNITS_P 1 // millis + +// config names +#define drillingcycles_checksum CHECKSUM("drillingcycles") +#define enable_checksum CHECKSUM("enable") +#define dwell_units_checksum CHECKSUM("dwell_units") + +Drillingcycles::Drillingcycles() {} + +void Drillingcycles::on_module_loaded() +{ + // if the module is disabled -> do nothing + if(! THEKERNEL->config->value(drillingcycles_checksum, enable_checksum)->by_default(false)->as_bool()) { + // as this module is not needed free up the resource + delete this; + return; + } + + // Settings + this->on_config_reload(this); + + // events + this->register_for_event(ON_GCODE_RECEIVED); + + // reset values + this->cycle_started = false; + this->retract_type = RETRACT_TO_Z; + + this->initial_z = 0; + this->r_plane = 0; + + this->reset_sticky(); +} + +void Drillingcycles::on_config_reload(void *argument) +{ + // take the dwell units configured by user, or select S (seconds) by default + string dwell_units = THEKERNEL->config->value(drillingcycles_checksum, dwell_units_checksum)->by_default("S")->as_string(); + this->dwell_units = (dwell_units == "P") ? DWELL_UNITS_P : DWELL_UNITS_S; +} + +/* +The canned cycles have been implemented as described on this page : +http://www.tormach.com/g81_g89_backgroung.html + +/!\ This code expects a clean gcode, no fail safe at this time. + +Implemented : G80-83, G98, G99 +Absolute mode : yes +Relative mode : no +Incremental (L) : no +*/ + +/* reset all sticky values, called before each cycle */ +void Drillingcycles::reset_sticky() +{ + this->sticky_z = 0; // Z depth + this->sticky_r = 0; // R plane + this->sticky_f = 0; // feedrate + this->sticky_q = 0; // peck drilling increment + this->sticky_p = 0; // dwell in seconds +} + +/* update all sticky values, called before each hole */ +void Drillingcycles::update_sticky(Gcode *gcode) +{ + if (gcode->has_letter('Z')) this->sticky_z = gcode->get_value('Z'); + if (gcode->has_letter('R')) this->sticky_r = gcode->get_value('R'); + if (gcode->has_letter('F')) this->sticky_f = gcode->get_value('F'); + if (gcode->has_letter('Q')) this->sticky_q = gcode->get_value('Q'); + if (gcode->has_letter('P')) this->sticky_p = gcode->get_int('P'); + + // set retract plane + if (this->retract_type == RETRACT_TO_Z) + this->r_plane = this->initial_z; + else + this->r_plane = this->sticky_r; +} + +/* send a formatted Gcode line */ +int Drillingcycles::send_gcode(const char* format, ...) +{ + // handle variable arguments + va_list args; + va_start(args, format); + // make the formatted string + char line[32]; // max length for an gcode line + int n = vsnprintf(line, sizeof(line), format, args); + va_end(args); + // debug, print the gcode sended + //THEKERNEL->streams->printf(">>> %s\r\n", line); + // make gcode object and send it (right way) + Gcode gc(line, &(StreamOutput::NullStream)); + THEKERNEL->call_event(ON_GCODE_RECEIVED, &gc); + // return the gcode srting length + return n; +} + +/* G83: peck drilling */ +void Drillingcycles::peck_hole() +{ + // start values + float depth = this->sticky_r - this->sticky_z; // travel depth + float cycles = depth / this->sticky_q; // cycles count + float rest = fmod(depth, this->sticky_q); // final pass + float z_pos = this->sticky_r; // current z position + + // for each cycle + for (int i = 1; i < cycles; i++) { + // decrement depth + z_pos -= this->sticky_q; + // feed down to depth at feedrate (F and Z) + this->send_gcode("G1 F%1.4f Z%1.4f", this->sticky_f, z_pos); + // rapids to retract position (R) + this->send_gcode("G0 Z%1.4f", this->sticky_r); + } + + // final depth not reached + if (rest > 0) { + // feed down to final depth at feedrate (F and Z) + this->send_gcode("G1 F%1.4f Z%1.4f", this->sticky_f, this->sticky_z); + } +} + +void Drillingcycles::make_hole(Gcode *gcode) +{ + // compile X and Y values + char x[16] = ""; + char y[16] = ""; + if (gcode->has_letter('X')) + snprintf(x, sizeof(x), " X%1.4f", gcode->get_value('X')); + if (gcode->has_letter('Y')) + snprintf(y, sizeof(y), " Y%1.4f", gcode->get_value('Y')); + + // rapids to X/Y + this->send_gcode("G0%s%s", x, y); + // rapids to retract position (R) + this->send_gcode("G0 Z%1.4f", this->sticky_r); + + // if peck drilling + if (this->sticky_q > 0) + this->peck_hole(); + else + // feed down to depth at feedrate (F and Z) + this->send_gcode("G1 F%1.4f Z%1.4f", this->sticky_f, this->sticky_z); + + // if dwell, wait for x seconds + if (this->sticky_p > 0) { + // dwell exprimed in seconds + if (this->dwell_units == DWELL_UNITS_S) + this->send_gcode("G4 S%u", this->sticky_p); + // dwell exprimed in milliseconds + else + this->send_gcode("G4 P%u", this->sticky_p); + } + + // rapids retract at R-Plane (Initial-Z or R) + this->send_gcode("G0 Z%1.4f", this->r_plane); +} + +void Drillingcycles::on_gcode_received(void* argument) +{ + // received gcode + Gcode *gcode = static_cast(argument); + + // no "G" in gcode, exit... + if (! gcode->has_g) + return; + + // "G" value + int code = gcode->g; + + // cycle start + if (code == 98 || code == 99) { + // wait for any moves left and current position is update + THEKERNEL->conveyor->wait_for_empty_queue(); + // get actual position from robot + float pos[3]; + THEKERNEL->robot->get_axis_position(pos); + // backup Z position as Initial-Z value + this->initial_z = pos[Z_AXIS]; + // set retract type + this->retract_type = (code == 98) ? RETRACT_TO_Z : RETRACT_TO_R; + // reset sticky values + this->reset_sticky(); + // mark cycle started and gcode taken + this->cycle_started = true; + } + // cycle end + else if (code == 80) { + // mark cycle endded and gcode taken + this->cycle_started = false; + + // if retract position is R-Plane + if (this->retract_type == RETRACT_TO_R) { + // rapids retract at Initial-Z to avoid futur collisions + this->send_gcode("G0 Z%1.4f", this->initial_z); + } + } + // in cycle + else if (this->cycle_started) { + // relative mode not supported for now... + if (THEKERNEL->robot->absolute_mode == false) { + gcode->stream->printf("Drillingcycles: relative mode not supported.\r\n"); + gcode->stream->printf("Drillingcycles: skip hole...\r\n"); + // exit + return; + } + // implemented cycles + if (code == 81 || code == 82 || code == 83) { + this->update_sticky(gcode); + this->make_hole(gcode); + } + } +} diff --git a/src/modules/tools/drillingcycles/Drillingcycles.h b/src/modules/tools/drillingcycles/Drillingcycles.h new file mode 100644 index 00000000..f0cd9fa5 --- /dev/null +++ b/src/modules/tools/drillingcycles/Drillingcycles.h @@ -0,0 +1,47 @@ +/* + 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 DRILLINGCYCLES_MODULE_H +#define DRILLINGCYCLES_MODULE_H + +#include "libs/Module.h" + +class Gcode; + +class Drillingcycles : public Module +{ + public: + Drillingcycles(); + virtual ~Drillingcycles() {}; + void on_module_loaded(); + + private: + void on_config_reload(void *argument); + void on_gcode_received(void *argument); + void reset_sticky(); + void update_sticky(Gcode *gcode); + int send_gcode(const char* format, ...); + void make_hole(Gcode *gcode); + void peck_hole(); + + bool cycle_started; // cycle status + int retract_type; // rretract type + + float initial_z; // Initial-Z + float r_plane; // R-Plane + + float sticky_z; // final depth + float sticky_r; // R-Plane + float sticky_f; // feedrate + + float sticky_q; // depth increment + int sticky_p; // dwell pause + + int dwell_units; // units for dwell +}; + +#endif diff --git a/src/modules/tools/endstops/Endstops.cpp b/src/modules/tools/endstops/Endstops.cpp index 8ed74ede..62baa651 100644 --- a/src/modules/tools/endstops/Endstops.cpp +++ b/src/modules/tools/endstops/Endstops.cpp @@ -26,7 +26,8 @@ #include "PublicDataRequest.h" #include "EndstopsPublicAccess.h" #include "StreamOutputPool.h" -#include "Pauser.h" +#include "StepTicker.h" +#include "BaseSolution.h" #include @@ -105,8 +106,6 @@ #define STEPPER THEKERNEL->robot->actuators #define STEPS_PER_MM(a) (STEPPER[a]->get_steps_per_mm()) -#define max(a,b) (((a) > (b)) ? (a) : (b)) -#define min(a,b) (((a) <= (b)) ? (a) : (b)) // Homing States enum{ @@ -137,7 +136,7 @@ void Endstops::on_module_loaded() register_for_event(ON_GET_PUBLIC_DATA); register_for_event(ON_SET_PUBLIC_DATA); - THEKERNEL->slow_ticker->attach( THEKERNEL->stepper->get_acceleration_ticks_per_second() , this, &Endstops::acceleration_tick ); + THEKERNEL->step_ticker->register_acceleration_tick_handler([this](){acceleration_tick(); }); // Settings this->on_config_reload(this); @@ -177,7 +176,6 @@ void Endstops::on_config_reload(void *argument) this->debounce_count = THEKERNEL->config->value(endstop_debounce_count_checksum )->by_default(100)->as_number(); - // get homing direction and convert to boolean where true is home to min, and false is home to max int home_dir = get_checksum(THEKERNEL->config->value(alpha_homing_direction_checksum)->by_default("home_to_min")->as_string()); this->home_direction[0] = home_dir != home_to_max_checksum; @@ -227,7 +225,35 @@ void Endstops::on_config_reload(void *argument) if(this->limit_enable[X_AXIS] || this->limit_enable[Y_AXIS] || this->limit_enable[Z_AXIS]){ register_for_event(ON_IDLE); + if(this->is_delta) { + // we must enable all the limits not just one + this->limit_enable[X_AXIS]= true; + this->limit_enable[Y_AXIS]= true; + this->limit_enable[Z_AXIS]= true; + } + } + + // NOTE this may also be true of scara. TBD + if(this->is_delta) { + // some things must be the same or they will die, so force it here to avoid config errors + this->fast_rates[1]= this->fast_rates[2]= this->fast_rates[0]; + this->slow_rates[1]= this->slow_rates[2]= this->slow_rates[0]; + this->retract_mm[1]= this->retract_mm[2]= this->retract_mm[0]; + this->home_direction[1]= this->home_direction[2]= this->home_direction[0]; + this->homing_position[0]= this->homing_position[1]= 0; + } +} + +bool Endstops::debounced_get(int pin) +{ + uint8_t debounce= 0; + while(this->pins[pin].get()) { + if ( ++debounce >= this->debounce_count ) { + // pin triggered + return true; + } } + return false; } static const char *endstop_names[]= {"min_x", "min_y", "min_z", "max_x", "max_y", "max_z"}; @@ -267,16 +293,13 @@ void Endstops::on_idle(void *argument) // check min and max endstops for (int i : minmax) { int n= c+i; - uint8_t debounce= 0; - while(this->pins[n].get()) { - if ( ++debounce >= debounce_count ) { - // endstop triggered - THEKERNEL->streams->printf("Limit switch %s was hit - reset or M999 required\n", endstop_names[n]); - this->status= LIMIT_TRIGGERED; - // disables heaters and motors, ignores incoming Gcode and flushes block queue - THEKERNEL->call_event(ON_HALT, nullptr); - return; - } + if(debounced_get(n)) { + // endstop triggered + THEKERNEL->streams->printf("Limit switch %s was hit - reset or M999 required\n", endstop_names[n]); + this->status= LIMIT_TRIGGERED; + // disables heaters and motors, ignores incoming Gcode and flushes block queue + THEKERNEL->call_event(ON_HALT, nullptr); + return; } } } @@ -287,24 +310,42 @@ void Endstops::on_idle(void *argument) // checks if triggered and only backs off if triggered void Endstops::back_off_home(char axes_to_move) { + std::vector> params; this->status = BACK_OFF_HOME; - for( int c = X_AXIS; c <= Z_AXIS; c++ ) { - if( ((axes_to_move >> c ) & 1) == 0) continue; // only for axes we asked to move - if(this->limit_enable[c]) { - if( !this->pins[c + (this->home_direction[c] ? 0 : 3)].get() ) continue; // if not triggered no need to move off - - // Move off of the endstop using a regular relative move - char buf[32]; - snprintf(buf, sizeof(buf), "G0 %c%1.4f F%1.4f", c+'X', this->retract_mm[c]*(this->home_direction[c]?1:-1), this->fast_rates[c]*60.0F); - Gcode gc(buf, &(StreamOutput::NullStream)); - bool oldmode= THEKERNEL->robot->absolute_mode; - THEKERNEL->robot->absolute_mode= false; // needs to be relative mode - THEKERNEL->robot->on_gcode_received(&gc); // send to robot directly - THEKERNEL->robot->absolute_mode= oldmode; // restore mode + + // these are handled differently + if((is_delta || is_scara) && this->limit_enable[X_AXIS]) { + // Move off of the endstop using a regular relative move in Z only + params.push_back({'Z', this->retract_mm[Z_AXIS]*(this->home_direction[Z_AXIS]?1:-1)}); + + }else{ + // cartesians, concatenate all the moves we need to do into one gcode + for( int c = X_AXIS; c <= Z_AXIS; c++ ) { + if( ((axes_to_move >> c ) & 1) == 0) continue; // only for axes we asked to move + + // if not triggered no need to move off + if(this->limit_enable[c] && debounced_get(c + (this->home_direction[c] ? 0 : 3)) ) { + params.push_back({c+'X', this->retract_mm[c]*(this->home_direction[c]?1:-1)}); + } } } - // Wait for above to finish - THEKERNEL->conveyor->wait_for_empty_queue(); + + if(!params.empty()) { + // Move off of the endstop using a regular relative move + params.insert(params.begin(), {'G', 0}); + // use X slow rate to move, Z should have a max speed set anyway + params.push_back({'F', this->slow_rates[X_AXIS]*60.0F}); + char gcode_buf[64]; + append_parameters(gcode_buf, params, sizeof(gcode_buf)); + Gcode gc(gcode_buf, &(StreamOutput::NullStream)); + bool oldmode= THEKERNEL->robot->absolute_mode; + THEKERNEL->robot->absolute_mode= false; // needs to be relative mode + THEKERNEL->robot->on_gcode_received(&gc); // send to robot directly + THEKERNEL->robot->absolute_mode= oldmode; // restore mode + // Wait for above to finish + THEKERNEL->conveyor->wait_for_empty_queue(); + } + this->status = NOT_HOMING; } @@ -318,7 +359,7 @@ void Endstops::move_to_origin(char axes_to_move) this->status = MOVE_TO_ORIGIN; // Move to center using a regular move, use slower of X and Y fast rate - float rate= min(this->fast_rates[0], this->fast_rates[1])*60.0F; + float rate= std::min(this->fast_rates[0], this->fast_rates[1])*60.0F; char buf[32]; snprintf(buf, sizeof(buf), "G0 X0 Y0 F%1.4f", rate); Gcode gc(buf, &(StreamOutput::NullStream)); @@ -329,13 +370,17 @@ void Endstops::move_to_origin(char axes_to_move) this->status = NOT_HOMING; } -void Endstops::wait_for_homed(char axes_to_move) +bool Endstops::wait_for_homed(char axes_to_move) { bool running = true; unsigned int debounce[3] = {0, 0, 0}; while (running) { running = false; THEKERNEL->call_event(ON_IDLE); + + // check if on_halt (eg kill) + if(THEKERNEL->is_halted()) return false; + for ( int c = X_AXIS; c <= Z_AXIS; c++ ) { if ( ( axes_to_move >> c ) & 1 ) { if ( this->pins[c + (this->home_direction[c] ? 0 : 3)].get() ) { @@ -354,23 +399,26 @@ void Endstops::wait_for_homed(char axes_to_move) } } } + return true; } void Endstops::do_homing_cartesian(char axes_to_move) { + // check if on_halt (eg kill) + if(THEKERNEL->is_halted()) return; + // this homing works for cartesian and delta printers // Start moving the axes to the origin this->status = MOVING_TO_ENDSTOP_FAST; for ( int c = X_AXIS; c <= Z_AXIS; c++ ) { if ( ( axes_to_move >> c) & 1 ) { this->feed_rate[c]= this->fast_rates[c]; - STEPPER[c]->set_speed(0); - STEPPER[c]->move(this->home_direction[c], 10000000); + STEPPER[c]->move(this->home_direction[c], 10000000, 0); } } // Wait for all axes to have homed - this->wait_for_homed(axes_to_move); + if(!this->wait_for_homed(axes_to_move)) return; // Move back a small distance this->status = MOVING_BACK; @@ -379,8 +427,7 @@ void Endstops::do_homing_cartesian(char axes_to_move) if ( ( axes_to_move >> c ) & 1 ) { inverted_dir = !this->home_direction[c]; this->feed_rate[c]= this->slow_rates[c]; - STEPPER[c]->set_speed(0); - STEPPER[c]->move(inverted_dir, this->retract_mm[c]*STEPS_PER_MM(c)); + STEPPER[c]->move(inverted_dir, this->retract_mm[c]*STEPS_PER_MM(c), 0); } } @@ -389,6 +436,7 @@ void Endstops::do_homing_cartesian(char axes_to_move) if ( ( axes_to_move >> c ) & 1 ) { while ( STEPPER[c]->is_moving() ) { THEKERNEL->call_event(ON_IDLE); + if(THEKERNEL->is_halted()) return; } } } @@ -398,50 +446,25 @@ void Endstops::do_homing_cartesian(char axes_to_move) for ( int c = X_AXIS; c <= Z_AXIS; c++ ) { if ( ( axes_to_move >> c ) & 1 ) { this->feed_rate[c]= this->slow_rates[c]; - STEPPER[c]->set_speed(0); - STEPPER[c]->move(this->home_direction[c], 10000000); + STEPPER[c]->move(this->home_direction[c], 10000000, 0); } } // Wait for all axes to have homed - this->wait_for_homed(axes_to_move); - - if (this->is_delta || this->is_scara) { - // move for soft trim - this->status = MOVING_BACK; - for ( int c = X_AXIS; c <= Z_AXIS; c++ ) { - if ( this->trim_mm[c] != 0.0F && ( axes_to_move >> c ) & 1 ) { - inverted_dir = this->home_direction[c]; - // move up or down depending on sign of trim, -ive is down away from home - if (this->trim_mm[c] < 0) inverted_dir = !inverted_dir; - this->feed_rate[c]= this->slow_rates[c]; - STEPPER[c]->set_speed(0); - STEPPER[c]->move(inverted_dir, abs(round(this->trim_mm[c]*STEPS_PER_MM(c)))); - } - } - - // Wait for moves to be done - for ( int c = X_AXIS; c <= Z_AXIS; c++ ) { - if ( ( axes_to_move >> c ) & 1 ) { - //THEKERNEL->streams->printf("axis %c \r\n", c ); - while ( STEPPER[c]->is_moving() ) { - THEKERNEL->call_event(ON_IDLE); - } - } - } - } - - // Homing is done - this->status = NOT_HOMING; + if(!this->wait_for_homed(axes_to_move)) return; } -void Endstops::wait_for_homed_corexy(int axis) +bool Endstops::wait_for_homed_corexy(int axis) { bool running = true; unsigned int debounce[3] = {0, 0, 0}; while (running) { running = false; THEKERNEL->call_event(ON_IDLE); + + // check if on_halt (eg kill) + if(THEKERNEL->is_halted()) return false; + if ( this->pins[axis + (this->home_direction[axis] ? 0 : 3)].get() ) { if ( debounce[axis] < debounce_count ) { debounce[axis] ++; @@ -457,46 +480,45 @@ void Endstops::wait_for_homed_corexy(int axis) debounce[axis] = 0; } } + return true; } void Endstops::corexy_home(int home_axis, bool dirx, bool diry, float fast_rate, float slow_rate, unsigned int retract_steps) { + // check if on_halt (eg kill) + if(THEKERNEL->is_halted()) return; + this->status = MOVING_TO_ENDSTOP_FAST; this->feed_rate[X_AXIS]= fast_rate; - STEPPER[X_AXIS]->set_speed(0); - STEPPER[X_AXIS]->move(dirx, 10000000); + STEPPER[X_AXIS]->move(dirx, 10000000, 0); this->feed_rate[Y_AXIS]= fast_rate; - STEPPER[Y_AXIS]->set_speed(0); - STEPPER[Y_AXIS]->move(diry, 10000000); + STEPPER[Y_AXIS]->move(diry, 10000000, 0); // wait for primary axis - this->wait_for_homed_corexy(home_axis); + if(!this->wait_for_homed_corexy(home_axis)) return; // Move back a small distance this->status = MOVING_BACK; this->feed_rate[X_AXIS]= slow_rate; - STEPPER[X_AXIS]->set_speed(0); - STEPPER[X_AXIS]->move(!dirx, retract_steps); + STEPPER[X_AXIS]->move(!dirx, retract_steps, 0); this->feed_rate[Y_AXIS]= slow_rate; - STEPPER[Y_AXIS]->set_speed(0); - STEPPER[Y_AXIS]->move(!diry, retract_steps); + STEPPER[Y_AXIS]->move(!diry, retract_steps, 0); // wait until done while ( STEPPER[X_AXIS]->is_moving() || STEPPER[Y_AXIS]->is_moving()) { THEKERNEL->call_event(ON_IDLE); + if(THEKERNEL->is_halted()) return; } // Start moving the axes to the origin slowly this->status = MOVING_TO_ENDSTOP_SLOW; this->feed_rate[X_AXIS]= slow_rate; - STEPPER[X_AXIS]->set_speed(0); - STEPPER[X_AXIS]->move(dirx, 10000000); + STEPPER[X_AXIS]->move(dirx, 10000000, 0); this->feed_rate[Y_AXIS]= slow_rate; - STEPPER[Y_AXIS]->set_speed(0); - STEPPER[Y_AXIS]->move(diry, 10000000); + STEPPER[Y_AXIS]->move(diry, 10000000, 0); // wait for primary axis - this->wait_for_homed_corexy(home_axis); + if(!this->wait_for_homed_corexy(home_axis)) return; } // this homing works for HBots/CoreXY @@ -529,13 +551,14 @@ void Endstops::do_homing_corexy(char axes_to_move) // then move both X and Y until one hits the endstop this->status = MOVING_TO_ENDSTOP_FAST; + // need to allow for more ground covered when moving diagonally this->feed_rate[motor]= this->fast_rates[motor]*1.4142; - STEPPER[motor]->set_speed(0); // need to allow for more ground covered when moving diagonally - STEPPER[motor]->move(dir, 10000000); + STEPPER[motor]->move(dir, 10000000, 0); // wait until either X or Y hits the endstop bool running= true; while (running) { THEKERNEL->call_event(ON_IDLE); + if(THEKERNEL->is_halted()) return; for(int m=X_AXIS;m<=Y_AXIS;m++) { if(this->pins[m + (this->home_direction[m] ? 0 : 3)].get()) { // turn off motor @@ -561,13 +584,15 @@ void Endstops::do_homing_corexy(char axes_to_move) if (axes_to_move & 0x04) { // move Z do_homing_cartesian(0x04); // just home normally for Z } - - // Homing is done - this->status = NOT_HOMING; } void Endstops::home(char axes_to_move) { + // not a block move so disable the last tick setting + for ( int c = X_AXIS; c <= Z_AXIS; c++ ) { + STEPPER[c]->set_moved_last_block(false); + } + if (is_corexy){ // corexy/HBot homing do_homing_corexy(axes_to_move); @@ -575,6 +600,12 @@ void Endstops::home(char axes_to_move) // cartesian/delta homing do_homing_cartesian(axes_to_move); } + + // make sure all steppers are off (especially if aborted) + for ( int c = X_AXIS; c <= Z_AXIS; c++ ) { + STEPPER[c]->move(0, 0); + } + this->status = NOT_HOMING; } // Start homing sequences by response to GCode commands @@ -583,7 +614,7 @@ void Endstops::on_gcode_received(void *argument) Gcode *gcode = static_cast(argument); if ( gcode->has_g) { if ( gcode->g == 28 ) { - gcode->mark_as_taken(); + // G28 is received, we have homing to do // First wait for the queue to be empty @@ -610,21 +641,56 @@ void Endstops::on_gcode_received(void *argument) // eg 0b00100001 would be Y X Z, 0b00100100 would be X Y Z for (uint8_t m = homing_order; m != 0; m >>= 2) { int a= (1 << (m & 0x03)); // axis to move - if((a & axes_to_move) != 0) + if((a & axes_to_move) != 0){ home(a); + } + // check if on_halt (eg kill) + if(THEKERNEL->is_halted()) break; } + }else { // they all home at the same time home(axes_to_move); } + // check if on_halt (eg kill) + if(THEKERNEL->is_halted()){ + THEKERNEL->streams->printf("Homing cycle aborted by kill\n"); + return; + } + if(home_all) { // for deltas this may be important rather than setting each individually - THEKERNEL->robot->reset_axis_position( + + // Here's where we would have been if the endstops were perfectly trimmed + float ideal_position[3] = { this->homing_position[X_AXIS] + this->home_offset[X_AXIS], this->homing_position[Y_AXIS] + this->home_offset[Y_AXIS], - this->homing_position[Z_AXIS] + this->home_offset[Z_AXIS]); - }else{ + this->homing_position[Z_AXIS] + this->home_offset[Z_AXIS] + }; + + bool has_endstop_trim = this->is_delta || this->is_scara; + if (has_endstop_trim) { + float ideal_actuator_position[3]; + THEKERNEL->robot->arm_solution->cartesian_to_actuator(ideal_position, ideal_actuator_position); + + // We are actually not at the ideal position, but a trim away + float real_actuator_position[3] = { + ideal_actuator_position[X_AXIS] - this->trim_mm[X_AXIS], + ideal_actuator_position[Y_AXIS] - this->trim_mm[Y_AXIS], + ideal_actuator_position[Z_AXIS] - this->trim_mm[Z_AXIS] + }; + + float real_position[3]; + THEKERNEL->robot->arm_solution->actuator_to_cartesian(real_actuator_position, real_position); + // Reset the actuator positions to correspond our real position + THEKERNEL->robot->reset_axis_position(real_position[0], real_position[1], real_position[2]); + } else { + // without endstop trim, real_position == ideal_position + // Reset the actuator positions to correspond our real position + THEKERNEL->robot->reset_axis_position(ideal_position[0], ideal_position[1], ideal_position[2]); + } + } else { // Zero the ax(i/e)s position, add in the home offset for ( int c = X_AXIS; c <= Z_AXIS; c++ ) { if ( (axes_to_move >> c) & 1 ) { @@ -635,11 +701,13 @@ void Endstops::on_gcode_received(void *argument) // on some systems where 0,0 is bed center it is noce to have home goto 0,0 after homing // default is off - if(this->move_to_origin_after_home) - move_to_origin(axes_to_move); + if(!is_delta && this->move_to_origin_after_home) move_to_origin(axes_to_move); // if limit switches are enabled we must back off endstop after setting home back_off_home(axes_to_move); + + // deltas are not left at 0,0 becuase of the trim settings, so move to 0,0 if requested + if(is_delta && this->move_to_origin_after_home) move_to_origin(axes_to_move); } } else if (gcode->has_m) { @@ -650,7 +718,7 @@ void Endstops::on_gcode_received(void *argument) gcode->stream->printf("%s:%d ", endstop_names[i], this->pins[i].get()); } gcode->add_nl= true; - gcode->mark_as_taken(); + } break; @@ -659,7 +727,7 @@ void Endstops::on_gcode_received(void *argument) if (gcode->has_letter('Y')) home_offset[1] = gcode->get_value('Y'); if (gcode->has_letter('Z')) home_offset[2] = gcode->get_value('Z'); gcode->stream->printf("X %5.3f Y %5.3f Z %5.3f\n", home_offset[0], home_offset[1], home_offset[2]); - gcode->mark_as_taken(); + break; case 306: // Similar to M206 and G92 but sets Homing offsets based on current position, Would be M207 but that is taken @@ -680,22 +748,21 @@ void Endstops::on_gcode_received(void *argument) } gcode->stream->printf("Homing Offset: X %5.3f Y %5.3f Z %5.3f\n", home_offset[0], home_offset[1], home_offset[2]); - gcode->mark_as_taken(); + } break; case 500: // save settings case 503: // print settings gcode->stream->printf(";Home offset (mm):\nM206 X%1.2f Y%1.2f Z%1.2f\n", home_offset[0], home_offset[1], home_offset[2]); - if (is_delta) { + if (this->is_delta || this->is_scara) { gcode->stream->printf(";Trim (mm):\nM666 X%1.3f Y%1.3f Z%1.3f\n", trim_mm[0], trim_mm[1], trim_mm[2]); gcode->stream->printf(";Max Z\nM665 Z%1.3f\n", this->homing_position[2]); } - gcode->mark_as_taken(); break; case 665: { // M665 - set max gamma/z height - gcode->mark_as_taken(); + float gamma_max = this->homing_position[2]; if (gcode->has_letter('Z')) { this->homing_position[2] = gamma_max = gcode->get_value('Z'); @@ -714,34 +781,38 @@ void Endstops::on_gcode_received(void *argument) // print the current trim values in mm gcode->stream->printf("X: %5.3f Y: %5.3f Z: %5.3f\n", trim_mm[0], trim_mm[1], trim_mm[2]); - gcode->mark_as_taken(); + } break; // NOTE this is to test accuracy of lead screws etc. - case 910: { // M910 - move specific number of raw steps - // Enable the motors - THEKERNEL->stepper->turn_enable_pins_on(); - - int x= 0, y=0 , z= 0, f= 200*16; - if (gcode->has_letter('F')) f = gcode->get_value('F'); - if (gcode->has_letter('X')) { - x = gcode->get_value('X'); - STEPPER[X_AXIS]->set_speed(f); - STEPPER[X_AXIS]->move(x<0, abs(x)); - } - if (gcode->has_letter('Y')) { - y = gcode->get_value('Y'); - STEPPER[Y_AXIS]->set_speed(f); - STEPPER[Y_AXIS]->move(y<0, abs(y)); - } - if (gcode->has_letter('Z')) { - z = gcode->get_value('Z'); - STEPPER[Z_AXIS]->set_speed(f); - STEPPER[Z_AXIS]->move(z<0, abs(z)); + case 1910: { // M1910 - move specific number of raw steps + if(gcode->subcode == 0) { + // Enable the motors + THEKERNEL->stepper->turn_enable_pins_on(); + + int x= 0, y=0 , z= 0, f= 200*16; + if (gcode->has_letter('F')) f = gcode->get_value('F'); + if (gcode->has_letter('X')) { + x = gcode->get_value('X'); + STEPPER[X_AXIS]->move(x<0, abs(x), f); + } + if (gcode->has_letter('Y')) { + y = gcode->get_value('Y'); + STEPPER[Y_AXIS]->move(y<0, abs(y), f); + } + if (gcode->has_letter('Z')) { + z = gcode->get_value('Z'); + STEPPER[Z_AXIS]->move(z<0, abs(z), f); + } + gcode->stream->printf("Moving X %d Y %d Z %d steps at F %d steps/sec\n", x, y, z, f); + + }else if(gcode->subcode == 1) { + // stop any that are moving + for (int i = 0; i < 3; ++i) { + if(STEPPER[i]->is_moving()) STEPPER[i]->move(0, 0); + } } - gcode->stream->printf("Moved X %d Y %d Z %d F %d steps\n", x, y, z, f); - gcode->mark_as_taken(); break; } } @@ -749,28 +820,28 @@ void Endstops::on_gcode_received(void *argument) } // Called periodically to change the speed to match acceleration -uint32_t Endstops::acceleration_tick(uint32_t dummy) +void Endstops::acceleration_tick(void) { - if(this->status >= NOT_HOMING) return(0); // nothing to do, only do this when moving for homing sequence + if(this->status >= NOT_HOMING) return; // nothing to do, only do this when moving for homing sequence // foreach stepper that is moving for ( int c = X_AXIS; c <= Z_AXIS; c++ ) { if( !STEPPER[c]->is_moving() ) continue; uint32_t current_rate = STEPPER[c]->get_steps_per_second(); - uint32_t target_rate = int(floor(this->feed_rate[c]*STEPS_PER_MM(c))); + uint32_t target_rate = floorf(this->feed_rate[c]*STEPS_PER_MM(c)); float acc= (c==Z_AXIS) ? THEKERNEL->planner->get_z_acceleration() : THEKERNEL->planner->get_acceleration(); if( current_rate < target_rate ){ - uint32_t rate_increase = int(floor((acc/THEKERNEL->stepper->get_acceleration_ticks_per_second())*STEPS_PER_MM(c))); + uint32_t rate_increase = floorf((acc/THEKERNEL->acceleration_ticks_per_second)*STEPS_PER_MM(c)); current_rate = min( target_rate, current_rate + rate_increase ); } if( current_rate > target_rate ){ current_rate = target_rate; } // steps per second - STEPPER[c]->set_speed(max(current_rate, THEKERNEL->stepper->get_minimum_steps_per_second())); + STEPPER[c]->set_speed(current_rate); } - return 0; + return; } void Endstops::on_get_public_data(void* argument){ diff --git a/src/modules/tools/endstops/Endstops.h b/src/modules/tools/endstops/Endstops.h index 3a025400..6ed78247 100644 --- a/src/modules/tools/endstops/Endstops.h +++ b/src/modules/tools/endstops/Endstops.h @@ -21,20 +21,21 @@ class Endstops : public Module{ void on_module_loaded(); void on_gcode_received(void* argument); void on_config_reload(void* argument); - uint32_t acceleration_tick(uint32_t dummy); + void acceleration_tick(void); private: void home(char axes_to_move); void do_homing_cartesian(char axes_to_move); void do_homing_corexy(char axes_to_move); - void wait_for_homed(char axes_to_move); - void wait_for_homed_corexy(int axis); + bool wait_for_homed(char axes_to_move); + bool wait_for_homed_corexy(int axis); void corexy_home(int home_axis, bool dirx, bool diry, float fast_rate, float slow_rate, unsigned int retract_steps); void back_off_home(char axes_to_move); void move_to_origin(char); void on_get_public_data(void* argument); void on_set_public_data(void* argument); void on_idle(void *argument); + bool debounced_get(int pin); float homing_position[3]; float home_offset[3]; diff --git a/src/modules/tools/extruder/Extruder.cpp b/src/modules/tools/extruder/Extruder.cpp index 3a8fa8e9..944c301e 100644 --- a/src/modules/tools/extruder/Extruder.cpp +++ b/src/modules/tools/extruder/Extruder.cpp @@ -24,6 +24,8 @@ #include "Gcode.h" #include "libs/StreamOutput.h" #include "PublicDataRequest.h" +#include "StreamOutputPool.h" +#include "ExtruderPublicAccess.h" #include @@ -36,9 +38,9 @@ #define extruder_dir_pin_checksum CHECKSUM("extruder_dir_pin") #define extruder_en_pin_checksum CHECKSUM("extruder_en_pin") #define extruder_max_speed_checksum CHECKSUM("extruder_max_speed") +#define extruder_default_feed_rate_checksum CHECKSUM("extruder_default_feed_rate") // NEW config names -#define extruder_checksum CHECKSUM("extruder") #define default_feed_rate_checksum CHECKSUM("default_feed_rate") #define steps_per_mm_checksum CHECKSUM("steps_per_mm") @@ -69,7 +71,6 @@ #define PI 3.14159265358979F -#define max(a,b) (((a) > (b)) ? (a) : (b)) /* The extruder module controls a filament extruder for 3D printing: http://en.wikipedia.org/wiki/Fused_deposition_modeling * It can work in two modes : either the head does not move, and the extruder moves the filament at a specified speed ( SOLO mode here ) @@ -79,59 +80,59 @@ Extruder::Extruder( uint16_t config_identifier, bool single ) { this->absolute_mode = true; + this->milestone_absolute_mode = true; this->enabled = false; - this->paused = false; this->single_config = single; this->identifier = config_identifier; this->retracted = false; this->volumetric_multiplier = 1.0F; this->extruder_multiplier = 1.0F; + this->stepper_motor = nullptr; + this->milestone_last_position = 0; + this->max_volumetric_rate = 0; memset(this->offset, 0, sizeof(this->offset)); } +Extruder::~Extruder() +{ + delete stepper_motor; +} + void Extruder::on_halt(void *arg) { if(arg == nullptr) { // turn off motor this->en_pin.set(1); - // disable if multi extruder - if(!this->single_config) - this->enabled= false; } } void Extruder::on_module_loaded() { - // Settings this->on_config_reload(this); + // Start values + this->target_position = 0; + this->current_position = 0; + this->unstepped_distance = 0; + this->current_block = NULL; + this->mode = OFF; + // We work on the same Block as Stepper, so we need to know when it gets a new one and drops one this->register_for_event(ON_BLOCK_BEGIN); this->register_for_event(ON_BLOCK_END); this->register_for_event(ON_GCODE_RECEIVED); this->register_for_event(ON_GCODE_EXECUTE); - this->register_for_event(ON_PLAY); - this->register_for_event(ON_PAUSE); this->register_for_event(ON_HALT); this->register_for_event(ON_SPEED_CHANGE); this->register_for_event(ON_GET_PUBLIC_DATA); - - // Start values - this->target_position = 0; - this->current_position = 0; - this->unstepped_distance = 0; - this->current_block = NULL; - this->mode = OFF; + this->register_for_event(ON_SET_PUBLIC_DATA); // Update speed every *acceleration_ticks_per_second* - // TODO: Make this an independent setting - THEKERNEL->slow_ticker->attach( THEKERNEL->stepper->get_acceleration_ticks_per_second() , this, &Extruder::acceleration_tick ); - - // Stepper motor object for the extruder - this->stepper_motor = THEKERNEL->step_ticker->add_stepper_motor( new StepperMotor(step_pin, dir_pin, en_pin) ); - this->stepper_motor->attach(this, &Extruder::stepper_motor_finished_move ); + THEKERNEL->step_ticker->register_acceleration_tick_handler([this]() { + acceleration_tick(); + }); } // Get config @@ -143,8 +144,7 @@ void Extruder::on_config_reload(void *argument) this->steps_per_millimeter = THEKERNEL->config->value(extruder_steps_per_mm_checksum )->by_default(1)->as_number(); this->filament_diameter = THEKERNEL->config->value(extruder_filament_diameter_checksum )->by_default(0)->as_number(); this->acceleration = THEKERNEL->config->value(extruder_acceleration_checksum )->by_default(1000)->as_number(); - this->max_speed = THEKERNEL->config->value(extruder_max_speed_checksum )->by_default(1000)->as_number(); - this->feed_rate = THEKERNEL->config->value(default_feed_rate_checksum )->by_default(1000)->as_number(); + this->feed_rate = THEKERNEL->config->value(extruder_default_feed_rate_checksum )->by_default(1000)->as_number(); this->step_pin.from_string( THEKERNEL->config->value(extruder_step_pin_checksum )->by_default("nc" )->as_string())->as_output(); this->dir_pin.from_string( THEKERNEL->config->value(extruder_dir_pin_checksum )->by_default("nc" )->as_string())->as_output(); @@ -162,8 +162,7 @@ void Extruder::on_config_reload(void *argument) this->steps_per_millimeter = THEKERNEL->config->value(extruder_checksum, this->identifier, steps_per_mm_checksum )->by_default(1)->as_number(); this->filament_diameter = THEKERNEL->config->value(extruder_checksum, this->identifier, filament_diameter_checksum )->by_default(0)->as_number(); this->acceleration = THEKERNEL->config->value(extruder_checksum, this->identifier, acceleration_checksum )->by_default(1000)->as_number(); - this->max_speed = THEKERNEL->config->value(extruder_checksum, this->identifier, max_speed_checksum )->by_default(1000)->as_number(); - this->feed_rate = THEKERNEL->config->value( default_feed_rate_checksum )->by_default(1000)->as_number(); + this->feed_rate = THEKERNEL->config->value(extruder_checksum, this->identifier, default_feed_rate_checksum )->by_default(1000)->as_number(); this->step_pin.from_string( THEKERNEL->config->value(extruder_checksum, this->identifier, step_pin_checksum )->by_default("nc" )->as_string())->as_output(); this->dir_pin.from_string( THEKERNEL->config->value(extruder_checksum, this->identifier, dir_pin_checksum )->by_default("nc" )->as_string())->as_output(); @@ -181,15 +180,25 @@ void Extruder::on_config_reload(void *argument) this->retract_recover_length = THEKERNEL->config->value(extruder_checksum, this->identifier, retract_recover_length_checksum)->by_default(0)->as_number(); this->retract_recover_feedrate = THEKERNEL->config->value(extruder_checksum, this->identifier, retract_recover_feedrate_checksum)->by_default(8)->as_number(); this->retract_zlift_length = THEKERNEL->config->value(extruder_checksum, this->identifier, retract_zlift_length_checksum)->by_default(0)->as_number(); - this->retract_zlift_feedrate = THEKERNEL->config->value(extruder_checksum, this->identifier, retract_zlift_feedrate_checksum)->by_default(100*60)->as_number(); // mm/min + this->retract_zlift_feedrate = THEKERNEL->config->value(extruder_checksum, this->identifier, retract_zlift_feedrate_checksum)->by_default(100 * 60)->as_number(); // mm/min - if(filament_diameter > 0.01) { + if(filament_diameter > 0.01F) { this->volumetric_multiplier = 1.0F / (powf(this->filament_diameter / 2, 2) * PI); } + + // Stepper motor object for the extruder + this->stepper_motor = new StepperMotor(step_pin, dir_pin, en_pin); + this->stepper_motor->attach(this, &Extruder::stepper_motor_finished_move ); + if( this->single_config ) { + this->stepper_motor->set_max_rate(THEKERNEL->config->value(extruder_max_speed_checksum)->by_default(1000)->as_number()); + } else { + this->stepper_motor->set_max_rate(THEKERNEL->config->value(extruder_checksum, this->identifier, max_speed_checksum)->by_default(1000)->as_number()); + } } -void Extruder::on_get_public_data(void* argument){ - PublicDataRequest* pdr = static_cast(argument); +void Extruder::on_get_public_data(void *argument) +{ + PublicDataRequest *pdr = static_cast(argument); if(!pdr->starts_with(extruder_checksum)) return; @@ -200,18 +209,85 @@ void Extruder::on_get_public_data(void* argument){ } } -// When the play/pause button is set to pause, or a module calls the ON_PAUSE event -void Extruder::on_pause(void *argument) +// check against maximum speeds and return the rate modifier +float Extruder::check_max_speeds(float target, float isecs) { - this->paused = true; - this->stepper_motor->pause(); + float rm = 1.0F; // default no rate modification + float delta; + // get change in E (may be mm or mm³) + if(milestone_absolute_mode) { + delta = fabsf(target - milestone_last_position); // delta move + milestone_last_position = target; + + } else { + delta = target; + milestone_last_position += target; + } + + if(this->max_volumetric_rate > 0 && this->filament_diameter > 0.01F) { + // volumetric enabled and check for volumetric rate + float v = delta * isecs; // the flow rate in mm³/sec + + // return the rate change needed to stay within the max rate + if(v > max_volumetric_rate) { + rm = max_volumetric_rate / v; + isecs *= rm; // this slows the rate down for the next test + } + //THEKERNEL->streams->printf("requested flow rate: %f mm³/sec, corrected flow rate: %f mm³/sec\n", v, v * rm); + } + + // check for max speed as well + float max_speed = this->stepper_motor->get_max_rate(); + if(max_speed > 0) { + if(this->filament_diameter > 0.01F) { + // volumetric so need to convert delta which is mm³ to mm + delta *= volumetric_multiplier; + } + + float sm = 1.0F; + float v = delta * isecs; // the speed in mm/sec + if(v > max_speed) { + sm *= (max_speed / v); + } + //THEKERNEL->streams->printf("requested speed: %f mm/sec, corrected speed: %f mm/sec\n", v, v * sm); + rm *= sm; + } + return rm; } -// When the play/pause button is set to play, or a module calls the ON_PLAY event -void Extruder::on_play(void *argument) +void Extruder::on_set_public_data(void *argument) { - this->paused = false; - this->stepper_motor->unpause(); + PublicDataRequest *pdr = static_cast(argument); + + if(!pdr->starts_with(extruder_checksum)) return; + + // handle extrude rates request from robot + if(pdr->second_element_is(target_checksum)) { + // disabled extruders do not reply NOTE only one enabled extruder supported + if(!this->enabled) return; + + float *d = static_cast(pdr->get_data_ptr()); + float target = d[0]; // the E passed in on Gcode is in mm³ (maybe absolute or relative) + float isecs = d[1]; // inverted secs + + // check against maximum speeds and return rate modifier + d[1] = check_max_speeds(target, isecs); + + pdr->set_taken(); + return; + } + + // save or restore state + if(pdr->second_element_is(save_state_checksum)) { + this->saved_current_position = this->current_position; + this->saved_absolute_mode = this->absolute_mode; + pdr->set_taken(); + } else if(pdr->second_element_is(restore_state_checksum)) { + // NOTE this only gets called when the queue is empty so the milestones will be the same + this->milestone_last_position= this->current_position = this->saved_current_position; + this->milestone_absolute_mode= this->absolute_mode = this->saved_absolute_mode; + pdr->set_taken(); + } } void Extruder::on_gcode_received(void *argument) @@ -224,7 +300,6 @@ void Extruder::on_gcode_received(void *argument) char buf[16]; int n = snprintf(buf, sizeof(buf), " E:%1.3f ", this->current_position); gcode->txt_after_ok.append(buf, n); - gcode->mark_as_taken(); } else if (gcode->m == 92 && ( (this->enabled && !gcode->has_letter('P')) || (gcode->has_letter('P') && gcode->get_value('P') == this->identifier) ) ) { float spm = this->steps_per_millimeter; @@ -235,87 +310,112 @@ void Extruder::on_gcode_received(void *argument) gcode->stream->printf("E:%g ", spm); gcode->add_nl = true; - gcode->mark_as_taken(); } else if (gcode->m == 200 && ( (this->enabled && !gcode->has_letter('P')) || (gcode->has_letter('P') && gcode->get_value('P') == this->identifier)) ) { if (gcode->has_letter('D')) { THEKERNEL->conveyor->wait_for_empty_queue(); // only apply after the queue has emptied this->filament_diameter = gcode->get_value('D'); - if(filament_diameter > 0.01) { + if(filament_diameter > 0.01F) { this->volumetric_multiplier = 1.0F / (powf(this->filament_diameter / 2, 2) * PI); - }else{ + } else { this->volumetric_multiplier = 1.0F; } + } else { + if(filament_diameter > 0.01F) { + gcode->stream->printf("Filament Diameter: %f\n", this->filament_diameter); + } else { + gcode->stream->printf("Volumetric extrusion is disabled\n"); + } + } + + } else if (gcode->m == 203 && ( (this->enabled && !gcode->has_letter('P')) || (gcode->has_letter('P') && gcode->get_value('P') == this->identifier)) ) { + // M203 Exxx Vyyy Set maximum feedrates xxx mm/sec and/or yyy mm³/sec + if(gcode->get_num_args() == 0) { + gcode->stream->printf("E:%g V:%g", this->stepper_motor->get_max_rate(), this->max_volumetric_rate); + gcode->add_nl = true; + + } else { + if(gcode->has_letter('E')) { + this->stepper_motor->set_max_rate(gcode->get_value('E')); + } + if(gcode->has_letter('V')) { + this->max_volumetric_rate = gcode->get_value('V'); + } } - gcode->mark_as_taken(); } else if (gcode->m == 204 && gcode->has_letter('E') && ( (this->enabled && !gcode->has_letter('P')) || (gcode->has_letter('P') && gcode->get_value('P') == this->identifier)) ) { // extruder acceleration M204 Ennn mm/sec^2 (Pnnn sets the specific extruder for M500) - this->acceleration= gcode->get_value('E'); - gcode->mark_as_taken(); + this->acceleration = gcode->get_value('E'); } else if (gcode->m == 207 && ( (this->enabled && !gcode->has_letter('P')) || (gcode->has_letter('P') && gcode->get_value('P') == this->identifier)) ) { // M207 - set retract length S[positive mm] F[feedrate mm/min] Z[additional zlift/hop] Q[zlift feedrate mm/min] if(gcode->has_letter('S')) retract_length = gcode->get_value('S'); - if(gcode->has_letter('F')) retract_feedrate = gcode->get_value('F')/60.0F; // specified in mm/min converted to mm/sec + if(gcode->has_letter('F')) retract_feedrate = gcode->get_value('F') / 60.0F; // specified in mm/min converted to mm/sec if(gcode->has_letter('Z')) retract_zlift_length = gcode->get_value('Z'); if(gcode->has_letter('Q')) retract_zlift_feedrate = gcode->get_value('Q'); - gcode->mark_as_taken(); } else if (gcode->m == 208 && ( (this->enabled && !gcode->has_letter('P')) || (gcode->has_letter('P') && gcode->get_value('P') == this->identifier)) ) { // M208 - set retract recover length S[positive mm surplus to the M207 S*] F[feedrate mm/min] if(gcode->has_letter('S')) retract_recover_length = gcode->get_value('S'); - if(gcode->has_letter('F')) retract_recover_feedrate = gcode->get_value('F')/60.0F; // specified in mm/min converted to mm/sec - gcode->mark_as_taken(); + if(gcode->has_letter('F')) retract_recover_feedrate = gcode->get_value('F') / 60.0F; // specified in mm/min converted to mm/sec } else if (gcode->m == 221 && this->enabled) { // M221 S100 change flow rate by percentage - if(gcode->has_letter('S')) this->extruder_multiplier= gcode->get_value('S')/100.0F; - gcode->mark_as_taken(); + if(gcode->has_letter('S')) { + this->extruder_multiplier = gcode->get_value('S') / 100.0F; + } else { + gcode->stream->printf("Flow rate at %6.2f %%\n", this->extruder_multiplier * 100.0F); + } } else if (gcode->m == 500 || gcode->m == 503) { // M500 saves some volatile settings to config override file, M503 just prints the settings if( this->single_config ) { gcode->stream->printf(";E Steps per mm:\nM92 E%1.4f\n", this->steps_per_millimeter); gcode->stream->printf(";E Filament diameter:\nM200 D%1.4f\n", this->filament_diameter); - gcode->stream->printf(";E retract length, feedrate, zlift length, feedrate:\nM207 S%1.4f F%1.4f Z%1.4f Q%1.4f\n", this->retract_length, this->retract_feedrate*60.0F, this->retract_zlift_length, this->retract_zlift_feedrate); - gcode->stream->printf(";E retract recover length, feedrate:\nM208 S%1.4f F%1.4f\n", this->retract_recover_length, this->retract_recover_feedrate*60.0F); - gcode->stream->printf(";E acceleration mm/sec^2:\nM204 E%1.4f\n", this->acceleration); + gcode->stream->printf(";E retract length, feedrate, zlift length, feedrate:\nM207 S%1.4f F%1.4f Z%1.4f Q%1.4f\n", this->retract_length, this->retract_feedrate * 60.0F, this->retract_zlift_length, this->retract_zlift_feedrate); + gcode->stream->printf(";E retract recover length, feedrate:\nM208 S%1.4f F%1.4f\n", this->retract_recover_length, this->retract_recover_feedrate * 60.0F); + gcode->stream->printf(";E acceleration mm/sec²:\nM204 E%1.4f\n", this->acceleration); + gcode->stream->printf(";E max feed rate mm/sec:\nM203 E%1.4f\n", this->stepper_motor->get_max_rate()); + if(this->max_volumetric_rate > 0) { + gcode->stream->printf(";E max volumetric rate mm³/sec:\nM203 V%1.4f\n", this->max_volumetric_rate); + } } else { gcode->stream->printf(";E Steps per mm:\nM92 E%1.4f P%d\n", this->steps_per_millimeter, this->identifier); gcode->stream->printf(";E Filament diameter:\nM200 D%1.4f P%d\n", this->filament_diameter, this->identifier); - gcode->stream->printf(";E retract length, feedrate:\nM207 S%1.4f F%1.4f Z%1.4f Q%1.4f P%d\n", this->retract_length, this->retract_feedrate*60.0F, this->retract_zlift_length, this->retract_zlift_feedrate, this->identifier); - gcode->stream->printf(";E retract recover length, feedrate:\nM208 S%1.4f F%1.4f P%d\n", this->retract_recover_length, this->retract_recover_feedrate*60.0F, this->identifier); - gcode->stream->printf(";E acceleration mm/sec^2:\nM204 E%1.4f P%d\n", this->acceleration, this->identifier); + gcode->stream->printf(";E retract length, feedrate:\nM207 S%1.4f F%1.4f Z%1.4f Q%1.4f P%d\n", this->retract_length, this->retract_feedrate * 60.0F, this->retract_zlift_length, this->retract_zlift_feedrate, this->identifier); + gcode->stream->printf(";E retract recover length, feedrate:\nM208 S%1.4f F%1.4f P%d\n", this->retract_recover_length, this->retract_recover_feedrate * 60.0F, this->identifier); + gcode->stream->printf(";E acceleration mm/sec²:\nM204 E%1.4f P%d\n", this->acceleration, this->identifier); + gcode->stream->printf(";E max feed rate mm/sec:\nM203 E%1.4f P%d\n", this->stepper_motor->get_max_rate(), this->identifier); + if(this->max_volumetric_rate > 0) { + gcode->stream->printf(";E max volumetric rate mm³/sec:\nM203 V%1.4f P%d\n", this->max_volumetric_rate, this->identifier); + } } - gcode->mark_as_taken(); + } else if( gcode->m == 17 || gcode->m == 18 || gcode->m == 82 || gcode->m == 83 || gcode->m == 84 ) { // Mcodes to pass along to on_gcode_execute THEKERNEL->conveyor->append_gcode(gcode); - gcode->mark_as_taken(); + } - }else if(gcode->has_g) { + } else if(gcode->has_g) { // G codes, NOTE some are ignored if not enabled if( (gcode->g == 92 && gcode->has_letter('E')) || (gcode->g == 90 || gcode->g == 91) ) { // Gcodes to pass along to on_gcode_execute THEKERNEL->conveyor->append_gcode(gcode); - gcode->mark_as_taken(); - }else if( this->enabled && gcode->g < 4 && gcode->has_letter('E') && !gcode->has_letter('X') && !gcode->has_letter('Y') && !gcode->has_letter('Z') ) { - // This is a solo move, we add an empty block to the queue to prevent subsequent gcodes being executed at the same time + } else if( this->enabled && gcode->g < 4 && gcode->has_letter('E') && fabsf(gcode->millimeters_of_travel) < 0.00001F ) { // With floating numbers, we can have 0 != 0, NOTE needs to be same as in Robot.cpp#745 + // NOTE was ... gcode->has_letter('E') && !gcode->has_letter('X') && !gcode->has_letter('Y') && !gcode->has_letter('Z') ) { + // This is a SOLO move, we add an empty block to the queue to prevent subsequent gcodes being executed at the same time THEKERNEL->conveyor->append_gcode(gcode); THEKERNEL->conveyor->queue_head_block(); - gcode->mark_as_taken(); - }else if( this->enabled && (gcode->g == 10 || gcode->g == 11) ) { // firmware retract command - gcode->mark_as_taken(); + } else if( this->enabled && (gcode->g == 10 || gcode->g == 11) ) { // firmware retract command // check we are in the correct state of retract or unretract if(gcode->g == 10 && !retracted) { - this->retracted= true; - this->cancel_zlift_restore= false; - } else if(gcode->g == 11 && retracted){ - this->retracted= false; + this->retracted = true; + this->cancel_zlift_restore = false; + } else if(gcode->g == 11 && retracted) { + this->retracted = false; } else return; // ignore duplicates @@ -325,13 +425,13 @@ void Extruder::on_gcode_received(void *argument) // reverse zlift happens before unretract // NOTE we do not do this if cancel_zlift_restore is set to true, which happens if there is an absolute Z move inbetween G10 and G11 char buf[32]; - int n= snprintf(buf, sizeof(buf), "G0 Z%1.4f F%1.4f", -retract_zlift_length, retract_zlift_feedrate); + int n = snprintf(buf, sizeof(buf), "G0 Z%1.4f F%1.4f", -retract_zlift_length, retract_zlift_feedrate); string cmd(buf, n); Gcode gc(cmd, &(StreamOutput::NullStream)); - bool oldmode= THEKERNEL->robot->absolute_mode; - THEKERNEL->robot->absolute_mode= false; // needs to be relative mode + bool oldmode = THEKERNEL->robot->absolute_mode; + THEKERNEL->robot->absolute_mode = false; // needs to be relative mode THEKERNEL->robot->on_gcode_received(&gc); // send to robot directly - THEKERNEL->robot->absolute_mode= oldmode; // restore mode + THEKERNEL->robot->absolute_mode = oldmode; // restore mode } // This is a solo move, we add an empty block to the queue to prevent subsequent gcodes being executed at the same time @@ -340,18 +440,42 @@ void Extruder::on_gcode_received(void *argument) if(retract_zlift_length > 0 && gcode->g == 10) { char buf[32]; - int n= snprintf(buf, sizeof(buf), "G0 Z%1.4f F%1.4f", retract_zlift_length, retract_zlift_feedrate); + int n = snprintf(buf, sizeof(buf), "G0 Z%1.4f F%1.4f", retract_zlift_length, retract_zlift_feedrate); string cmd(buf, n); Gcode gc(cmd, &(StreamOutput::NullStream)); - bool oldmode= THEKERNEL->robot->absolute_mode; - THEKERNEL->robot->absolute_mode= false; // needs to be relative mode + bool oldmode = THEKERNEL->robot->absolute_mode; + THEKERNEL->robot->absolute_mode = false; // needs to be relative mode THEKERNEL->robot->on_gcode_received(&gc); // send to robot directly - THEKERNEL->robot->absolute_mode= oldmode; // restore mode + THEKERNEL->robot->absolute_mode = oldmode; // restore mode } - }else if( this->enabled && this->retracted && (gcode->g == 0 || gcode->g == 1) && gcode->has_letter('Z')) { + } else if( this->enabled && this->retracted && (gcode->g == 0 || gcode->g == 1) && gcode->has_letter('Z')) { // NOTE we cancel the zlift restore for the following G11 as we have moved to an absolute Z which we need to stay at - this->cancel_zlift_restore= true; + this->cancel_zlift_restore = true; + } + } + + // handle some codes now for the volumetric rate limiting + // G90 G91 G92 M82 M83 + if(gcode->has_m) { + switch(gcode->m) { + case 82: this->milestone_absolute_mode = true; break; + case 83: this->milestone_absolute_mode = false; break; + } + + } else if(gcode->has_g) { + switch(gcode->g) { + case 90: this->milestone_absolute_mode = true; break; + case 91: this->milestone_absolute_mode = false; break; + case 92: + if(this->enabled) { + if(gcode->has_letter('E')) { + this->milestone_last_position = gcode->get_value('E'); + } else if(gcode->get_num_args() == 0) { + this->milestone_last_position = 0; + } + } + break; } } } @@ -367,21 +491,21 @@ void Extruder::on_gcode_execute(void *argument) // Absolute/relative mode, globably modal affect all extruders whether enabled or not if( gcode->has_m ) { switch(gcode->m) { - case 17: - this->en_pin.set(0); - break; - case 18: - this->en_pin.set(1); - break; - case 82: - this->absolute_mode = true; - break; - case 83: - this->absolute_mode = false; - break; - case 84: - this->en_pin.set(1); - break; + case 17: + this->en_pin.set(0); + break; + case 18: + this->en_pin.set(1); + break; + case 82: + this->absolute_mode = true; + break; + case 83: + this->absolute_mode = false; + break; + case 84: + this->en_pin.set(1); + break; } return; @@ -406,7 +530,7 @@ void Extruder::on_gcode_execute(void *argument) } else if (gcode->g == 10) { // FW retract command - feed_rate= retract_feedrate; // mm/sec + feed_rate = retract_feedrate; // mm/sec this->mode = SOLO; this->travel_distance = -retract_length; this->target_position += this->travel_distance; @@ -414,16 +538,16 @@ void Extruder::on_gcode_execute(void *argument) } else if (gcode->g == 11) { // un retract command - feed_rate= retract_recover_feedrate; // mm/sec + feed_rate = retract_recover_feedrate; // mm/sec this->mode = SOLO; this->travel_distance = (retract_length + retract_recover_length); this->target_position += this->travel_distance; this->en_pin.set(0); - } else if (gcode->g == 0 || gcode->g == 1) { + } else if (gcode->g <= 3) { // Extrusion length from 'G' Gcode if( gcode->has_letter('E' )) { - // Get relative extrusion distance depending on mode ( in absolute mode we must substract target_position ) + // Get relative extrusion distance depending on mode ( in absolute mode we must subtract target_position ) float extrusion_distance = gcode->get_value('E'); float relative_extrusion_distance = extrusion_distance; if (this->absolute_mode) { @@ -434,91 +558,78 @@ void Extruder::on_gcode_execute(void *argument) } // If the robot is moving, we follow it's movement, otherwise, we move alone - if( fabs(gcode->millimeters_of_travel) < 0.0001F ) { // With floating numbers, we can have 0 != 0 ... beeeh. For more info see : http://upload.wikimedia.org/wikipedia/commons/0/0a/Cain_Henri_Vidal_Tuileries.jpg + if( fabsf(gcode->millimeters_of_travel) < 0.00001F ) { // With floating numbers, we can have 0 != 0, NOTE needs to be same as in Robot.cpp#745 this->mode = SOLO; this->travel_distance = relative_extrusion_distance; } else { // We move proportionally to the robot's movement this->mode = FOLLOW; this->travel_ratio = (relative_extrusion_distance * this->volumetric_multiplier * this->extruder_multiplier) / gcode->millimeters_of_travel; // adjust for volumetric extrusion and extruder multiplier - // TODO: check resulting flowrate, limit robot speed if it exceeds max_speed } this->en_pin.set(0); } + // NOTE this is only used in SOLO mode, but any F on a G0/G1 will set the speed for future retracts that are not firmware retracts if (gcode->has_letter('F')) { feed_rate = gcode->get_value('F') / THEKERNEL->robot->get_seconds_per_minute(); - if (feed_rate > max_speed) - feed_rate = max_speed; + if (stepper_motor->get_max_rate() > 0 && feed_rate > stepper_motor->get_max_rate()) + feed_rate = stepper_motor->get_max_rate(); } } } - } // When a new block begins, either follow the robot, or step by ourselves ( or stay back and do nothing ) void Extruder::on_block_begin(void *argument) { if(!this->enabled) return; - Block *block = static_cast(argument); - - - if( this->mode == SOLO ) { - // In solo mode we take the block so we can move even if the stepper has nothing to do - - this->current_position += this->travel_distance ; - - int steps_to_step = abs(int(floor(this->steps_per_millimeter * (this->travel_distance + this->unstepped_distance) ))); - - if ( this->travel_distance > 0 ) { - this->unstepped_distance += this->travel_distance - (steps_to_step / this->steps_per_millimeter); //catch any overflow - } else { - this->unstepped_distance += this->travel_distance + (steps_to_step / this->steps_per_millimeter); //catch any overflow - } - - if( steps_to_step != 0 ) { - - // We take the block, we have to release it or everything gets stuck - block->take(); - this->current_block = block; - this->stepper_motor->set_steps_per_second(0); - this->stepper_motor->move( ( this->travel_distance > 0 ), steps_to_step); - - } else { - this->current_block = NULL; - } + if( this->mode == OFF ) { + this->current_block = NULL; + this->stepper_motor->set_moved_last_block(false); + return; + } - } else if( this->mode == FOLLOW ) { - // In non-solo mode, we just follow the stepper module + Block *block = static_cast(argument); + if( this->mode == FOLLOW ) { + // In FOLLOW mode, we just follow the stepper module this->travel_distance = block->millimeters * this->travel_ratio; + } - this->current_position += this->travel_distance; + // common for both FOLLOW and SOLO + this->current_position += this->travel_distance ; - int steps_to_step = abs(int(floor(this->steps_per_millimeter * (this->travel_distance + this->unstepped_distance) ))); + // round down, we take care of the fractional part next time + int steps_to_step = abs(floorf(this->steps_per_millimeter * (this->travel_distance + this->unstepped_distance) )); - if ( this->travel_distance > 0 ) { - this->unstepped_distance += this->travel_distance - (steps_to_step / this->steps_per_millimeter); //catch any overflow - } else { - this->unstepped_distance += this->travel_distance + (steps_to_step / this->steps_per_millimeter); //catch any overflow - } + // accumulate the fractional part + if ( this->travel_distance > 0 ) { + this->unstepped_distance += this->travel_distance - (steps_to_step / this->steps_per_millimeter); + } else { + this->unstepped_distance += this->travel_distance + (steps_to_step / this->steps_per_millimeter); + } - if( steps_to_step != 0 ) { - block->take(); - this->current_block = block; + if( steps_to_step != 0 ) { + // We take the block, we have to release it or everything gets stuck + block->take(); + this->current_block = block; + this->stepper_motor->move( (this->travel_distance > 0), steps_to_step); - this->stepper_motor->move( ( this->travel_distance > 0 ), steps_to_step ); - this->on_speed_change(0); // initialise speed in case we get called first + if(this->mode == FOLLOW) { + on_speed_change(this); // set initial speed + this->stepper_motor->set_moved_last_block(true); } else { - this->current_block = NULL; + // SOLO + uint32_t target_rate = floorf(this->feed_rate * this->steps_per_millimeter); + this->stepper_motor->set_speed(min( target_rate, rate_increase() )); // start at first acceleration step + this->stepper_motor->set_moved_last_block(false); } - } else if( this->mode == OFF ) { - // No movement means we must reset our speed + } else { + // no steps to take this time this->current_block = NULL; - //this->stepper_motor->set_speed(0); - + this->stepper_motor->set_moved_last_block(false); } } @@ -530,40 +641,46 @@ void Extruder::on_block_end(void *argument) this->current_block = NULL; } -// Called periodically to change the speed to match acceleration or to match the speed of the robot -uint32_t Extruder::acceleration_tick(uint32_t dummy) +uint32_t Extruder::rate_increase() const { - if(!this->enabled) return 0; + return floorf((this->acceleration / THEKERNEL->acceleration_ticks_per_second) * this->steps_per_millimeter); +} +// Called periodically to change the speed to match acceleration or to match the speed of the robot +// Only used in SOLO mode +void Extruder::acceleration_tick(void) +{ // Avoid trying to work when we really shouldn't ( between blocks or re-entry ) - if( this->current_block == NULL || this->paused || this->mode != SOLO ) { - return 0; + if(!this->enabled || this->mode != SOLO || this->current_block == NULL || !this->stepper_motor->is_moving() ) { + return; } uint32_t current_rate = this->stepper_motor->get_steps_per_second(); - uint32_t target_rate = int(floor(this->feed_rate * this->steps_per_millimeter)); + uint32_t target_rate = floorf(this->feed_rate * this->steps_per_millimeter); if( current_rate < target_rate ) { - uint32_t rate_increase = int(floor((this->acceleration / THEKERNEL->stepper->get_acceleration_ticks_per_second()) * this->steps_per_millimeter)); - current_rate = min( target_rate, current_rate + rate_increase ); + current_rate = min( target_rate, current_rate + rate_increase() ); + // steps per second + this->stepper_motor->set_speed(current_rate); } - if( current_rate > target_rate ) { - current_rate = target_rate; - } - - // steps per second - this->stepper_motor->set_speed(max(current_rate, THEKERNEL->stepper->get_minimum_steps_per_second())); - return 0; + return; } // Speed has been updated for the robot's stepper, we must update accordingly void Extruder::on_speed_change( void *argument ) { - if(!this->enabled) return; - // Avoid trying to work when we really shouldn't ( between blocks or re-entry ) - if( this->current_block == NULL || this->paused || this->mode != FOLLOW || this->stepper_motor->is_moving() != true ) { + if(!this->enabled || this->current_block == NULL || this->mode != FOLLOW || !this->stepper_motor->is_moving()) { + return; + } + + // if we are flushing the queue we need to stop the motor when it has decelerated to zero, we get this call with argumnet == 0 when this happens + // this is what steppermotor does + if(argument == 0) { + this->stepper_motor->move(0, 0); + this->current_block->release(); + this->current_block = NULL; return; } @@ -576,8 +693,7 @@ void Extruder::on_speed_change( void *argument ) * or even : ( stepper steps per second ) * ( extruder steps / current block's steps ) */ - this->stepper_motor->set_speed( max( ( THEKERNEL->stepper->get_trapezoid_adjusted_rate()) * ( (float)this->stepper_motor->get_steps_to_move() / (float)this->current_block->steps_event_count ), THEKERNEL->stepper->get_minimum_steps_per_second() ) ); - + this->stepper_motor->set_speed(THEKERNEL->stepper->get_trapezoid_adjusted_rate() * (float)this->stepper_motor->get_steps_to_move() / (float)this->current_block->steps_event_count); } // When the stepper has finished it's move diff --git a/src/modules/tools/extruder/Extruder.h b/src/modules/tools/extruder/Extruder.h index e3452366..bad76b21 100644 --- a/src/modules/tools/extruder/Extruder.h +++ b/src/modules/tools/extruder/Extruder.h @@ -20,7 +20,7 @@ class Block; class Extruder : public Tool { public: Extruder(uint16_t config_identifier, bool single= false); - virtual ~Extruder() {} + virtual ~Extruder(); void on_module_loaded(); void on_config_reload(void* argument); @@ -28,24 +28,23 @@ class Extruder : public Tool { void on_gcode_execute(void* argument); void on_block_begin(void* argument); void on_block_end(void* argument); - void on_play(void* argument); - void on_pause(void* argument); void on_halt(void* argument); void on_speed_change(void* argument); - uint32_t acceleration_tick(uint32_t dummy); + void acceleration_tick(void); uint32_t stepper_motor_finished_move(uint32_t dummy); Block* append_empty_block(); private: void on_get_public_data(void* argument); + void on_set_public_data(void* argument); + uint32_t rate_increase() const; + float check_max_speeds(float target, float isecs); StepperMotor* stepper_motor; Pin step_pin; // Step pin for the stepper driver Pin dir_pin; // Dir pin for the stepper driver Pin en_pin; - float target_position; // End point ( in mm ) for the current move - float current_position; // Current point ( in mm ) for the current move, incremented every time a move is executed float unstepped_distance; // overflow buffer for requested moves that are less than 1 step Block* current_block; // Current block we are stepping, same as Stepper's one @@ -56,29 +55,33 @@ class Extruder : public Tool { float extruder_multiplier; // flow rate 1.0 == 100% float acceleration; // extruder accleration SOLO setting float retract_length; // firmware retract length + float current_position; // Current point ( in mm ) for the current move, incremented every time a move is executed }; - float volumetric_multiplier; - float feed_rate; // - float max_speed; + float saved_current_position; + float volumetric_multiplier; + float feed_rate; // default rate mm/sec for SOLO moves only + float milestone_last_position; // used for calculating volumemetric rate, last position in mm³ + float max_volumetric_rate; // used for calculating volumetric rate in mm³/sec - float travel_ratio; - float travel_distance; + float travel_ratio; + float travel_distance; // for firmware retract - float retract_feedrate; - float retract_recover_feedrate; - float retract_recover_length; - float retract_zlift_length; - float retract_zlift_feedrate; + float retract_feedrate; + float retract_recover_feedrate; + float retract_recover_length; + float retract_zlift_length; + float retract_zlift_feedrate; struct { char mode:3; // extruder motion mode, OFF, SOLO, or FOLLOW bool absolute_mode:1; // absolute/relative coordinate mode switch - bool paused:1; + bool saved_absolute_mode:1; bool single_config:1; bool retracted:1; bool cancel_zlift_restore:1; // hack to stop a G11 zlift restore from overring an absolute Z setting + bool milestone_absolute_mode:1; }; diff --git a/src/modules/tools/extruder/ExtruderPublicAccess.h b/src/modules/tools/extruder/ExtruderPublicAccess.h new file mode 100644 index 00000000..d5c16cdd --- /dev/null +++ b/src/modules/tools/extruder/ExtruderPublicAccess.h @@ -0,0 +1,6 @@ +#pragma once + +#define extruder_checksum CHECKSUM("extruder") +#define save_state_checksum CHECKSUM("save_state") +#define restore_state_checksum CHECKSUM("restore_state") +#define target_checksum CHECKSUM("target") diff --git a/src/modules/tools/filamentdetector/FilamentDetector.cpp b/src/modules/tools/filamentdetector/FilamentDetector.cpp new file mode 100644 index 00000000..3bff1d43 --- /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 "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 encodeer. + 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 + } + + + // 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") { + this->pulses= 0; + e_last_moved= NAN; + suspended= false; + } +} + +float FilamentDetector::get_emove() +{ + float *rd; + if(PublicData::get_value( extruder_checksum, (void **)&rd )) { + return *(rd+5); // current position for extruder in mm + } + 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/laser/Laser.cpp b/src/modules/tools/laser/Laser.cpp index 43175957..3d51f21c 100644 --- a/src/modules/tools/laser/Laser.cpp +++ b/src/modules/tools/laser/Laser.cpp @@ -24,8 +24,11 @@ #define laser_module_enable_checksum CHECKSUM("laser_module_enable") #define laser_module_pin_checksum CHECKSUM("laser_module_pin") #define laser_module_pwm_period_checksum CHECKSUM("laser_module_pwm_period") -#define laser_module_max_power_checksum CHECKSUM("laser_module_max_power") +#define laser_module_maximum_power_checksum CHECKSUM("laser_module_maximum_power") +#define laser_module_minimum_power_checksum CHECKSUM("laser_module_minimum_power") +#define laser_module_default_power_checksum CHECKSUM("laser_module_default_power") #define laser_module_tickle_power_checksum CHECKSUM("laser_module_tickle_power") +#define laser_module_max_power_checksum CHECKSUM("laser_module_max_power") Laser::Laser(){ } @@ -45,7 +48,7 @@ void Laser::on_module_loaded() { if (laser_pin == NULL) { - THEKERNEL->streams->printf("Error: Laser cannot use P%d.%d (P2.0 - P2.5, P1.18, P1.20, P1.21, P1.23, P1.24, P1.26, P3.25, P2.26 only). Laser module disabled.\n", dummy_pin->port_number, dummy_pin->pin); + THEKERNEL->streams->printf("Error: Laser cannot use P%d.%d (P2.0 - P2.5, P1.18, P1.20, P1.21, P1.23, P1.24, P1.26, P3.25, P3.26 only). Laser module disabled.\n", dummy_pin->port_number, dummy_pin->pin); delete dummy_pin; delete this; return; @@ -58,15 +61,19 @@ void Laser::on_module_loaded() { this->laser_pin->period_us(THEKERNEL->config->value(laser_module_pwm_period_checksum)->by_default(20)->as_number()); this->laser_pin->write(this->laser_inverting ? 1 : 0); + this->laser_maximum_power = THEKERNEL->config->value(laser_module_maximum_power_checksum )->by_default(1.0f)->as_number() ; + + // These config variables are deprecated, they have been replaced with laser_module_default_power and laser_module_minimum_power + this->laser_minimum_power = THEKERNEL->config->value(laser_module_tickle_power_checksum)->by_default(0)->as_number() ; + this->laser_power = THEKERNEL->config->value(laser_module_max_power_checksum)->by_default(0.8f)->as_number() ; - this->laser_max_power = THEKERNEL->config->value(laser_module_max_power_checksum )->by_default(0.8f)->as_number() ; - this->laser_tickle_power = THEKERNEL->config->value(laser_module_tickle_power_checksum)->by_default(0 )->as_number() ; + // Load in our preferred config variables + this->laser_minimum_power = THEKERNEL->config->value(laser_module_minimum_power_checksum)->by_default(this->laser_minimum_power)->as_number() ; + this->laser_power = THEKERNEL->config->value(laser_module_default_power_checksum)->by_default(this->laser_power)->as_number() ; //register for events this->register_for_event(ON_GCODE_EXECUTE); this->register_for_event(ON_SPEED_CHANGE); - this->register_for_event(ON_PLAY); - this->register_for_event(ON_PAUSE); this->register_for_event(ON_BLOCK_BEGIN); this->register_for_event(ON_BLOCK_END); } @@ -81,16 +88,6 @@ void Laser::on_block_begin(void* argument){ this->set_proportional_power(); } -// When the play/pause button is set to pause, or a module calls the ON_PAUSE event -void Laser::on_pause(void* argument){ - this->laser_pin->write(this->laser_inverting ? 1 : 0); -} - -// When the play/pause button is set to play, or a module calls the ON_PLAY event -void Laser::on_play(void* argument){ - this->set_proportional_power(); -} - // Turn laser on/off depending on received GCodes void Laser::on_gcode_execute(void* argument){ Gcode* gcode = static_cast(argument); @@ -98,15 +95,14 @@ void Laser::on_gcode_execute(void* argument){ if( gcode->has_g){ int code = gcode->g; if( code == 0 ){ // G0 - this->laser_pin->write(this->laser_inverting ? 1 - this->laser_tickle_power : this->laser_tickle_power); + this->laser_pin->write(this->laser_inverting ? 1 - this->laser_minimum_power : this->laser_minimum_power); this->laser_on = false; }else if( code >= 1 && code <= 3 ){ // G1, G2, G3 this->laser_on = true; } } if ( gcode->has_letter('S' )){ - this->laser_max_power = gcode->get_value('S'); -// THEKERNEL->streams->printf("Adjusted laser power to %d/100\r\n",(int)(this->laser_max_power*100.0+0.5)); + this->laser_power = gcode->get_value('S'); } } @@ -121,7 +117,7 @@ void Laser::on_speed_change(void* argument){ void Laser::set_proportional_power(){ if( this->laser_on && THEKERNEL->stepper->get_current_block() ){ // adjust power to maximum power and actual velocity - float proportional_power = float(float(this->laser_max_power) * float(THEKERNEL->stepper->get_trapezoid_adjusted_rate()) / float(THEKERNEL->stepper->get_current_block()->nominal_rate)); + float proportional_power = (((this->laser_maximum_power-this->laser_minimum_power)*(this->laser_power * THEKERNEL->stepper->get_trapezoid_adjusted_rate() / THEKERNEL->stepper->get_current_block()->nominal_rate))+this->laser_minimum_power); this->laser_pin->write(this->laser_inverting ? 1 - proportional_power : proportional_power); } } diff --git a/src/modules/tools/laser/Laser.h b/src/modules/tools/laser/Laser.h index 60f66e9b..8f955e3e 100644 --- a/src/modules/tools/laser/Laser.h +++ b/src/modules/tools/laser/Laser.h @@ -21,8 +21,6 @@ class Laser : public Module{ void on_module_loaded(); void on_block_end(void* argument); void on_block_begin(void* argument); - void on_play(void* argument); - void on_pause(void* argument); void on_gcode_execute(void* argument); void on_speed_change(void* argument); @@ -33,8 +31,9 @@ class Laser : public Module{ bool laser_on:1; // Laser status bool laser_inverting:1; // stores whether the pwm period should be inverted }; - float laser_max_power; // maximum allowed laser power to be output on the pwm pin - float laser_tickle_power; // value used to tickle the laser on moves + float laser_maximum_power; // maximum allowed laser power to be output on the pwm pin + float laser_minimum_power; // value used to tickle the laser on moves. Also minimum value for auto-scaling + float laser_power; // current laser power }; #endif diff --git a/src/modules/tools/scaracal/SCARAcal.cpp b/src/modules/tools/scaracal/SCARAcal.cpp index 4ee2181a..be7964e2 100644 --- a/src/modules/tools/scaracal/SCARAcal.cpp +++ b/src/modules/tools/scaracal/SCARAcal.cpp @@ -21,10 +21,13 @@ #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 @@ -51,6 +54,9 @@ void SCARAcal::on_module_loaded() 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 } @@ -62,6 +68,21 @@ void SCARAcal::home() 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}; @@ -76,21 +97,68 @@ bool SCARAcal::set_trim(float x, float y, float z, StreamOutput *stream) return ok; } -bool SCARAcal::get_trim(float& x, float& y, float& z) +bool SCARAcal::get_home_offset(float& x, float& y, float& z) { void *returned_data; - bool ok = PublicData::get_value( endstops_checksum, trim_checksum, &returned_data ); + bool ok = PublicData::get_value( endstops_checksum, home_offset_checksum, &returned_data ); if (ok) { - float *trim = static_cast(returned_data); - x= trim[0]; - y= trim[1]; - z= trim[2]; + 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], + actuator[3];; + + 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 + + THEKERNEL->robot->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 + + THEKERNEL->robot->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]; @@ -135,21 +203,19 @@ void SCARAcal::on_gcode_received(void *argument) actuators[0], actuators[1]); // display actuator angles Theta and Psi. gcode->txt_after_ok.append(buf, n); - gcode->mark_as_taken(); - } - return; - + 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 - float cartesian[3], - actuators[3], + float actuators[3], S_delta[2], S_trim[3]; @@ -158,60 +224,69 @@ void SCARAcal::on_gcode_received(void *argument) S_delta[0] = actuators[0] - target[0]; - set_trim(S_delta[0], S_trim[1], 0, gcode->stream); + set_trim(S_delta[0], S_trim[1], S_trim[2], gcode->stream); } else { - set_trim(0, S_trim[1], 0, gcode->stream); // reset trim for calibration move + set_trim(0, S_trim[1], S_trim[2], gcode->stream); // reset trim for calibration move this->home(); // home - SCARA_ang_move(target[0], target[1], 100.0F, slow_rate * 3.0F); // move to target + THEKERNEL->robot->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 } - gcode->mark_as_taken(); } - return; + break; + case 361: { - float target[2] = {90.0F, 130.0F}; + float target[2] = {90.0F, 130.0F}, + cartesian[3]; + if(gcode->has_letter('P')) { // Program the current position as target - float cartesian[3], - actuators[3]; + float actuators[3]; THEKERNEL->robot->get_axis_position(cartesian); // get actual position from robot THEKERNEL->robot->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* + 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. - SCARA_ang_move(target[0], target[1], 100.0F, slow_rate * 3.0F); // move to target + THEKERNEL->robot->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 } - gcode->mark_as_taken(); + } - return; - case 364: { + 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 - float cartesian[3], - actuators[3], + float actuators[3], S_delta[2]; THEKERNEL->robot->get_axis_position(cartesian); // get actual position from robot THEKERNEL->robot->arm_solution->cartesian_to_actuator( cartesian, actuators ); // translate it to get actual actuator angles - S_delta[1] = actuators[1] - target[1]; // Find difference, and - set_trim(S_trim[0], S_delta[1], 0, gcode->stream); // set trim to reflect the difference + 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, 0, gcode->stream); // reset trim for calibration move - this->home(); // home - SCARA_ang_move(target[0], target[1], 100.0F, slow_rate * 3.0F); // move to target + set_trim(S_trim[0], 0, S_trim[2], gcode->stream); // reset trim for calibration move + this->home(); // home + THEKERNEL->robot->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 } - gcode->mark_as_taken(); } - return; + 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 index d679d909..50d4fff7 100644 --- a/src/modules/tools/scaracal/SCARAcal.h +++ b/src/modules/tools/scaracal/SCARAcal.h @@ -23,7 +23,6 @@ public: void on_config_reload(void *argument); void on_gcode_received(void *argument); -// void on_idle(void *argument); private: @@ -31,9 +30,15 @@ private: 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; diff --git a/src/modules/tools/spindle/Spindle.cpp b/src/modules/tools/spindle/Spindle.cpp index b7b7ef17..fcacb9c1 100644 --- a/src/modules/tools/spindle/Spindle.cpp +++ b/src/modules/tools/spindle/Spindle.cpp @@ -22,6 +22,7 @@ #include "InterruptIn.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") @@ -32,6 +33,7 @@ #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 @@ -48,7 +50,7 @@ void Spindle::on_module_loaded() 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 @@ -60,7 +62,14 @@ void Spindle::on_module_loaded() 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(); @@ -69,18 +78,18 @@ void Spindle::on_module_loaded() output_inverted = smoothie_pin->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(); @@ -91,6 +100,7 @@ void Spindle::on_module_loaded() PinName pinname = port_pin((PortName)smoothie_pin->port_number, smoothie_pin->pin); feedback_pin = new mbed::InterruptIn(pinname); feedback_pin->rise(this, &Spindle::on_pin_rise); + NVIC_SetPriority(EINT3_IRQn, 16); } else { @@ -100,9 +110,7 @@ void Spindle::on_module_loaded() } delete smoothie_pin; } - - SysTick_Config(SYSTICK_MAXCOUNT, false); - + THEKERNEL->slow_ticker->attach(UPDATE_FREQ, this, &Spindle::on_update_speed); register_for_event(ON_GCODE_RECEIVED); register_for_event(ON_GCODE_EXECUTE); @@ -110,8 +118,8 @@ void Spindle::on_module_loaded() void Spindle::on_pin_rise() { - uint32_t timestamp = SYSTICK_MAXCOUNT - SysTick->VAL; - last_time = (timestamp - last_edge) & SYSTICK_MAXCOUNT; + uint32_t timestamp = us_ticker_read(); + last_time = timestamp - last_edge; last_edge = timestamp; irq_count++; } @@ -125,31 +133,36 @@ uint32_t Spindle::on_update_speed(uint32_t dummy) 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 - current_rpm = SystemCoreClock * 60.0f / (t * pulses_per_rev); - + { + 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 @@ -157,12 +170,12 @@ uint32_t Spindle::on_update_speed(uint32_t dummy) 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; } @@ -170,7 +183,7 @@ uint32_t Spindle::on_update_speed(uint32_t dummy) void Spindle::on_gcode_received(void* argument) { Gcode *gcode = static_cast(argument); - + if (gcode->has_m) { if (gcode->m == 957) @@ -178,7 +191,6 @@ void Spindle::on_gcode_received(void* argument) // 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); - gcode->mark_as_taken(); } else if (gcode->m == 958) { @@ -196,7 +208,6 @@ void Spindle::on_gcode_received(void* argument) { // M3: Spindle on, M5: Spindle off THEKERNEL->conveyor->append_gcode(gcode); - gcode->mark_as_taken(); } } } @@ -204,14 +215,14 @@ void Spindle::on_gcode_received(void* argument) void Spindle::on_gcode_execute(void* argument) { Gcode *gcode = static_cast(argument); - + if (gcode->has_m) { if (gcode->m == 3) { // M3: Spindle on spindle_on = true; - + if (gcode->has_letter('S')) { target_rpm = gcode->get_value('S'); diff --git a/src/modules/tools/spindle/Spindle.h b/src/modules/tools/spindle/Spindle.h index a4034500..dd8951d4 100644 --- a/src/modules/tools/spindle/Spindle.h +++ b/src/modules/tools/spindle/Spindle.h @@ -49,6 +49,7 @@ class Spindle: public Module { 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 diff --git a/src/modules/tools/switch/Switch.cpp b/src/modules/tools/switch/Switch.cpp index c1e36b74..17455e46 100644 --- a/src/modules/tools/switch/Switch.cpp +++ b/src/modules/tools/switch/Switch.cpp @@ -19,7 +19,10 @@ #include "Gcode.h" #include "checksumm.h" #include "ConfigValue.h" -#include "libs/StreamOutput.h" +#include "StreamOutput.h" +#include "StreamOutputPool.h" + +#include "PwmOut.h" #include "MRI_Hooks.h" @@ -36,6 +39,9 @@ #define max_pwm_checksum CHECKSUM("max_pwm") #define output_on_command_checksum CHECKSUM("output_on_command") #define output_off_command_checksum CHECKSUM("output_off_command") +#define pwm_period_ms_checksum CHECKSUM("pwm_period_ms") +#define failsafe_checksum CHECKSUM("failsafe_set_to") +#define ignore_onhalt_checksum CHECKSUM("ignore_on_halt") Switch::Switch() {} @@ -45,53 +51,128 @@ Switch::Switch(uint16_t name) //this->dummy_stream = &(StreamOutput::NullStream); } +// set the pin to the fail safe value on halt +void Switch::on_halt(void *arg) +{ + if(arg == nullptr) { + if(this->ignore_on_halt) return; + + // set pin to failsafe value + switch(this->output_type) { + case DIGITAL: this->digital_pin->set(this->failsafe); break; + case SIGMADELTA: this->sigmadelta_pin->set(this->failsafe); break; + case HWPWM: this->pwm_pin->write(0); break; + case NONE: break; + } + this->switch_state= this->failsafe; + } +} + void Switch::on_module_loaded() { this->switch_changed = false; this->register_for_event(ON_GCODE_RECEIVED); - this->register_for_event(ON_GCODE_EXECUTE); this->register_for_event(ON_MAIN_LOOP); this->register_for_event(ON_GET_PUBLIC_DATA); this->register_for_event(ON_SET_PUBLIC_DATA); + this->register_for_event(ON_HALT); // Settings this->on_config_reload(this); } - // Get config void Switch::on_config_reload(void *argument) { this->input_pin.from_string( THEKERNEL->config->value(switch_checksum, this->name_checksum, input_pin_checksum )->by_default("nc")->as_string())->as_input(); - this->input_pin_behavior = THEKERNEL->config->value(switch_checksum, this->name_checksum, input_pin_behavior_checksum )->by_default(momentary_checksum)->as_number(); - std::string input_on_command = THEKERNEL->config->value(switch_checksum, this->name_checksum, input_on_command_checksum )->by_default("")->as_string(); - std::string input_off_command = THEKERNEL->config->value(switch_checksum, this->name_checksum, input_off_command_checksum )->by_default("")->as_string(); - this->output_pin.from_string(THEKERNEL->config->value(switch_checksum, this->name_checksum, output_pin_checksum )->by_default("nc")->as_string())->as_output(); - this->output_on_command = THEKERNEL->config->value(switch_checksum, this->name_checksum, output_on_command_checksum )->by_default("")->as_string(); - this->output_off_command = THEKERNEL->config->value(switch_checksum, this->name_checksum, output_off_command_checksum )->by_default("")->as_string(); - this->switch_state = THEKERNEL->config->value(switch_checksum, this->name_checksum, startup_state_checksum )->by_default(false)->as_bool(); - string type = THEKERNEL->config->value(switch_checksum, this->name_checksum, output_type_checksum )->by_default("pwm")->as_string(); - - if(type == "pwm") this->output_type= PWM; - else if(type == "digital") this->output_type= DIGITAL; - else this->output_type= PWM; // unkown type default to pwm - - if(this->output_pin.connected()) { - if(this->output_type == PWM) { - this->output_pin.max_pwm(THEKERNEL->config->value(switch_checksum, this->name_checksum, max_pwm_checksum )->by_default(255)->as_number()); - this->switch_value = THEKERNEL->config->value(switch_checksum, this->name_checksum, startup_value_checksum )->by_default(this->output_pin.max_pwm())->as_number(); - if(this->switch_state) { - this->output_pin.pwm(this->switch_value); // will be truncated to max_pwm - } else { - this->output_pin.set(false); + this->input_pin_behavior = THEKERNEL->config->value(switch_checksum, this->name_checksum, input_pin_behavior_checksum )->by_default(momentary_checksum)->as_number(); + std::string input_on_command = THEKERNEL->config->value(switch_checksum, this->name_checksum, input_on_command_checksum )->by_default("")->as_string(); + std::string input_off_command = THEKERNEL->config->value(switch_checksum, this->name_checksum, input_off_command_checksum )->by_default("")->as_string(); + this->output_on_command = THEKERNEL->config->value(switch_checksum, this->name_checksum, output_on_command_checksum )->by_default("")->as_string(); + this->output_off_command = THEKERNEL->config->value(switch_checksum, this->name_checksum, output_off_command_checksum )->by_default("")->as_string(); + this->switch_state = THEKERNEL->config->value(switch_checksum, this->name_checksum, startup_state_checksum )->by_default(false)->as_bool(); + string type = THEKERNEL->config->value(switch_checksum, this->name_checksum, output_type_checksum )->by_default("pwm")->as_string(); + this->failsafe= THEKERNEL->config->value(switch_checksum, this->name_checksum, failsafe_checksum )->by_default(0)->as_number(); + this->ignore_on_halt= THEKERNEL->config->value(switch_checksum, this->name_checksum, ignore_onhalt_checksum )->by_default(false)->as_bool(); + + if(type == "pwm"){ + this->output_type= SIGMADELTA; + this->sigmadelta_pin= new Pwm(); + this->sigmadelta_pin->from_string(THEKERNEL->config->value(switch_checksum, this->name_checksum, output_pin_checksum )->by_default("nc")->as_string())->as_output(); + if(this->sigmadelta_pin->connected()) { + if(failsafe == 1) { + set_high_on_debug(sigmadelta_pin->port_number, sigmadelta_pin->pin); + }else{ + set_low_on_debug(sigmadelta_pin->port_number, sigmadelta_pin->pin); } - } else { - this->output_pin.set(this->switch_state); + }else{ + this->output_type= NONE; + delete this->sigmadelta_pin; + this->sigmadelta_pin= nullptr; + } + + }else if(type == "digital"){ + this->output_type= DIGITAL; + this->digital_pin= new Pin(); + this->digital_pin->from_string(THEKERNEL->config->value(switch_checksum, this->name_checksum, output_pin_checksum )->by_default("nc")->as_string())->as_output(); + if(this->digital_pin->connected()) { + if(failsafe == 1) { + set_high_on_debug(digital_pin->port_number, digital_pin->pin); + }else{ + set_low_on_debug(digital_pin->port_number, digital_pin->pin); + } + }else{ + this->output_type= NONE; + delete this->digital_pin; + this->digital_pin= nullptr; } + + }else if(type == "hwpwm"){ + this->output_type= HWPWM; + Pin *pin= new Pin(); + pin->from_string(THEKERNEL->config->value(switch_checksum, this->name_checksum, output_pin_checksum )->by_default("nc")->as_string())->as_output(); + this->pwm_pin= pin->hardware_pwm(); + if(failsafe == 1) { + set_high_on_debug(pin->port_number, pin->pin); + }else{ + set_low_on_debug(pin->port_number, pin->pin); + } + delete pin; + if(this->pwm_pin == nullptr) { + THEKERNEL->streams->printf("Selected Switch output pin is not PWM capable - disabled"); + this->output_type= NONE; + } + + } else { + this->output_type= NONE; } - set_low_on_debug(output_pin.port_number, output_pin.pin); + if(this->output_type == SIGMADELTA) { + this->sigmadelta_pin->max_pwm(THEKERNEL->config->value(switch_checksum, this->name_checksum, max_pwm_checksum )->by_default(255)->as_number()); + this->switch_value = THEKERNEL->config->value(switch_checksum, this->name_checksum, startup_value_checksum )->by_default(this->sigmadelta_pin->max_pwm())->as_number(); + if(this->switch_state) { + this->sigmadelta_pin->pwm(this->switch_value); // will be truncated to max_pwm + } else { + this->sigmadelta_pin->set(false); + } + + } else if(this->output_type == HWPWM) { + // default is 50Hz + float p= THEKERNEL->config->value(switch_checksum, this->name_checksum, pwm_period_ms_checksum )->by_default(20)->as_number() * 1000.0F; // ms but fractions are allowed + this->pwm_pin->period_us(p); + + // default is 0% duty cycle + this->switch_value = THEKERNEL->config->value(switch_checksum, this->name_checksum, startup_value_checksum )->by_default(0)->as_number(); + if(this->switch_state) { + this->pwm_pin->write(this->switch_value/100.0F); + } else { + this->pwm_pin->write(0); + } + + } else if(this->output_type == DIGITAL){ + this->digital_pin->set(this->switch_state); + } // Set the on/off command codes, Use GCode to do the parsing input_on_command_letter = 0; @@ -125,9 +206,9 @@ void Switch::on_config_reload(void *argument) THEKERNEL->slow_ticker->attach( 100, this, &Switch::pinpoll_tick); } - if(this->output_type == PWM && this->output_pin.connected()) { - // PWM - THEKERNEL->slow_ticker->attach(1000, &this->output_pin, &Pwm::on_tick); + if(this->output_type == SIGMADELTA) { + // SIGMADELTA + THEKERNEL->slow_ticker->attach(1000, this->sigmadelta_pin, &Pwm::on_tick); } } @@ -147,41 +228,68 @@ void Switch::on_gcode_received(void *argument) { Gcode *gcode = static_cast(argument); // Add the gcode to the queue ourselves if we need it - if (match_input_on_gcode(gcode) || match_input_off_gcode(gcode)) { - THEKERNEL->conveyor->append_gcode(gcode); + if (!(match_input_on_gcode(gcode) || match_input_off_gcode(gcode))) { + return; } -} - -// Turn pin on and off -void Switch::on_gcode_execute(void *argument) -{ - Gcode *gcode = static_cast(argument); + // we need to sync this with the queue, so we need to wait for queue to empty, however due to certain slicers + // issuing redundant swicth on calls regularly we need to optimize by making sure the value is actually changing + // hence we need to do the wait for queue in each case rather than just once at the start if(match_input_on_gcode(gcode)) { - int v; - if (this->output_type == PWM) { - // PWM output pin turn on (or off if S0) + if (this->output_type == SIGMADELTA) { + // SIGMADELTA output pin turn on (or off if S0) if(gcode->has_letter('S')) { - v = round(gcode->get_value('S') * output_pin.max_pwm() / 255.0); // scale by max_pwm so input of 255 and max_pwm of 128 would set value to 128 - this->output_pin.pwm(v); - this->switch_state= (v > 0); + int v = round(gcode->get_value('S') * sigmadelta_pin->max_pwm() / 255.0); // scale by max_pwm so input of 255 and max_pwm of 128 would set value to 128 + if(v != this->sigmadelta_pin->get_pwm()){ // optimize... ignore if already set to the same pwm + // drain queue + THEKERNEL->conveyor->wait_for_empty_queue(); + this->sigmadelta_pin->pwm(v); + this->switch_state= (v > 0); + } } else { - this->output_pin.pwm(this->switch_value); + // drain queue + THEKERNEL->conveyor->wait_for_empty_queue(); + this->sigmadelta_pin->pwm(this->switch_value); this->switch_state= (this->switch_value > 0); } - } else { + + } else if (this->output_type == HWPWM) { + // drain queue + THEKERNEL->conveyor->wait_for_empty_queue(); + // PWM output pin set duty cycle 0 - 100 + if(gcode->has_letter('S')) { + float v = gcode->get_value('S'); + if(v > 100) v= 100; + else if(v < 0) v= 0; + this->pwm_pin->write(v/100.0F); + this->switch_state= (v != 0); + } else { + this->pwm_pin->write(this->switch_value); + this->switch_state= (this->switch_value != 0); + } + + } else if (this->output_type == DIGITAL) { + // drain queue + THEKERNEL->conveyor->wait_for_empty_queue(); // logic pin turn on - this->output_pin.set(true); + this->digital_pin->set(true); this->switch_state = true; } + } else if(match_input_off_gcode(gcode)) { + // drain queue + THEKERNEL->conveyor->wait_for_empty_queue(); this->switch_state = false; - if (this->output_type == PWM) { - // PWM output pin - this->output_pin.set(false); - } else { + if (this->output_type == SIGMADELTA) { + // SIGMADELTA output pin + this->sigmadelta_pin->set(false); + + } else if (this->output_type == HWPWM) { + this->pwm_pin->write(0); + + } else if (this->output_type == DIGITAL) { // logic pin turn off - this->output_pin.set(false); + this->digital_pin->set(false); } } } @@ -195,13 +303,11 @@ void Switch::on_get_public_data(void *argument) if(!pdr->second_element_is(this->name_checksum)) return; // likely fan, but could be anything // ok this is targeted at us, so send back the requested data - // this must be static as it will be accessed long after we have returned - static struct pad_switch pad; - pad.name = this->name_checksum; - pad.state = this->switch_state; - pad.value = this->switch_value; - - pdr->set_data_ptr(&pad); + // caller has provided the location to write the state to + struct pad_switch *pad= static_cast(pdr->get_data_ptr()); + pad->name = this->name_checksum; + pad->state = this->switch_state; + pad->value = this->switch_value; pdr->set_taken(); } @@ -233,20 +339,29 @@ void Switch::on_main_loop(void *argument) if(this->switch_changed) { if(this->switch_state) { if(!this->output_on_command.empty()) this->send_gcode( this->output_on_command, &(StreamOutput::NullStream) ); - if(this->output_pin.connected()) { - if(this->output_type == PWM) - this->output_pin.pwm(this->switch_value); // this requires the value has been set otherwise it switches on to whatever it last was - else - this->output_pin.set(true); + + if(this->output_type == SIGMADELTA) { + this->sigmadelta_pin->pwm(this->switch_value); // this requires the value has been set otherwise it switches on to whatever it last was + + } else if (this->output_type == HWPWM) { + this->pwm_pin->write(this->switch_value/100.0F); + + } else if (this->output_type == DIGITAL) { + this->digital_pin->set(true); } } else { + if(!this->output_off_command.empty()) this->send_gcode( this->output_off_command, &(StreamOutput::NullStream) ); - if(this->output_pin.connected()) { - if(this->output_type == PWM) - this->output_pin.set(false); - else - this->output_pin.set(false); + + if(this->output_type == SIGMADELTA) { + this->sigmadelta_pin->set(false); + + } else if (this->output_type == HWPWM) { + this->pwm_pin->write(0); + + } else if (this->output_type == DIGITAL) { + this->digital_pin->set(false); } } this->switch_changed = false; diff --git a/src/modules/tools/switch/Switch.h b/src/modules/tools/switch/Switch.h index bc8270d6..a19dcf8f 100644 --- a/src/modules/tools/switch/Switch.h +++ b/src/modules/tools/switch/Switch.h @@ -18,41 +18,55 @@ using std::string; class Gcode; class StreamOutput; +namespace mbed { + class PwmOut; +} + class Switch : public Module { public: Switch(); Switch(uint16_t name); void on_module_loaded(); + void on_main_loop(void *argument); void on_config_reload(void* argument); void on_gcode_received(void* argument); - void on_gcode_execute(void* argument); - void on_main_loop(void* argument); void on_get_public_data(void* argument); void on_set_public_data(void* argument); + void on_halt(void *arg); + uint32_t pinpoll_tick(uint32_t dummy); - enum OUTPUT_TYPE {PWM, DIGITAL}; + enum OUTPUT_TYPE {NONE, SIGMADELTA, DIGITAL, HWPWM}; + private: void flip(); void send_gcode(string msg, StreamOutput* stream); bool match_input_on_gcode(const Gcode* gcode) const; bool match_input_off_gcode(const Gcode* gcode) const; - uint16_t name_checksum; Pin input_pin; - uint16_t input_pin_behavior; - bool input_pin_state; - char input_on_command_letter; - char input_off_command_letter; - uint16_t input_on_command_code; - uint16_t input_off_command_code; - bool switch_state; float switch_value; - bool switch_changed; OUTPUT_TYPE output_type; - Pwm output_pin; + union { + Pin *digital_pin; + Pwm *sigmadelta_pin; + mbed::PwmOut *pwm_pin; + }; string output_on_command; string output_off_command; + uint16_t name_checksum; + uint16_t input_pin_behavior; + uint16_t input_on_command_code; + uint16_t input_off_command_code; + char input_on_command_letter; + char input_off_command_letter; + struct { + bool switch_changed:1; + bool input_pin_state:1; + bool switch_state:1; + bool ignore_on_halt:1; + uint8_t failsafe:1; + }; }; #endif // SWITCH_H diff --git a/src/modules/tools/temperaturecontrol/AD8495.cpp b/src/modules/tools/temperaturecontrol/AD8495.cpp new file mode 100644 index 00000000..bead7013 --- /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 + +#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/PID_Autotuner.cpp b/src/modules/tools/temperaturecontrol/PID_Autotuner.cpp index 353ded4a..37b0df64 100644 --- a/src/modules/tools/temperaturecontrol/PID_Autotuner.cpp +++ b/src/modules/tools/temperaturecontrol/PID_Autotuner.cpp @@ -3,7 +3,8 @@ #include "SlowTicker.h" #include "Gcode.h" #include "TemperatureControl.h" -#include "libs/StreamOutput.h" +#include "StreamOutput.h" +#include "StreamOutputPool.h" #include "TemperatureControlPublicAccess.h" #include "PublicDataRequest.h" #include "PublicData.h" @@ -16,11 +17,11 @@ PID_Autotuner::PID_Autotuner() { temp_control = NULL; - s = NULL; lastInputs = NULL; peaks = NULL; tick = false; tickCnt = 0; + nLookBack = 10 * 20; // 10 seconds of lookback (fixed 20ms tick period) } void PID_Autotuner::on_module_loaded() @@ -31,19 +32,16 @@ void PID_Autotuner::on_module_loaded() register_for_event(ON_GCODE_RECEIVED); } -void PID_Autotuner::begin(float target, StreamOutput *stream, int ncycles) +void PID_Autotuner::begin(float target, int ncycles) { noiseBand = 0.5; oStep = temp_control->heater_pin.max_pwm(); // use max pwm to cycle temp - nLookBack = 5 * 20; // 5 seconds of lookback lookBackCnt = 0; tickCnt = 0; if (lastInputs != NULL) delete[] lastInputs; lastInputs = new float[nLookBack + 1]; - s = stream; - temp_control->heater_pin.set(0); temp_control->target_temperature = 0.0; @@ -60,14 +58,8 @@ void PID_Autotuner::begin(float target, StreamOutput *stream, int ncycles) peakType = 0; peakCount = 0; justchanged = false; - - float refVal = temp_control->get_temperature(); - absMax = refVal; - absMin = refVal; - output = oStep; - temp_control->heater_pin.pwm(oStep); // turn on to start heating - - s->printf("%s: Starting PID Autotune, %d max cycles, M304 aborts\n", temp_control->designator.c_str(), ncycles); + firstPeak= false; + output= 0; } void PID_Autotuner::abort() @@ -79,10 +71,6 @@ void PID_Autotuner::abort() temp_control->heater_pin.set(0); temp_control = NULL; - if (s) - s->printf("PID Autotune Aborted\n"); - s = NULL; - if (peaks != NULL) delete[] peaks; peaks = NULL; @@ -97,11 +85,10 @@ void PID_Autotuner::on_gcode_received(void *argument) if(gcode->has_m) { if(gcode->m == 304) { - gcode->mark_as_taken(); abort(); + gcode->stream->printf("PID Autotune Aborted\n"); } else if (gcode->m == 303 && gcode->has_letter('E')) { - gcode->mark_as_taken(); int pool_index = gcode->get_value('E'); // get the temperature control instance with this pool index @@ -116,17 +103,35 @@ void PID_Autotuner::on_gcode_received(void *argument) return; } + // set target float target = 150.0; if (gcode->has_letter('S')) { target = gcode->get_value('S'); gcode->stream->printf("Target: %5.1f\n", target); } + + // set the cycles, really not needed for this new version int ncycles = 8; if (gcode->has_letter('C')) { ncycles = gcode->get_value('C'); + if(ncycles < 8) ncycles= 8; + } + + // optionally set the noise band, default is 0.5 + if (gcode->has_letter('B')) { + noiseBand = gcode->get_value('B'); } + + // optionally set the look back in seconds default is 10 seconds + if (gcode->has_letter('L')) { + nLookBack = gcode->get_value('L'); + } + gcode->stream->printf("Start PID tune for index E%d, designator: %s\n", pool_index, this->temp_control->designator.c_str()); - this->begin(target, gcode->stream, ncycles); + + this->begin(target, ncycles); + + gcode->stream->printf("%s: Starting PID Autotune, %d max cycles, M304 aborts\n", temp_control->designator.c_str(), ncycles); } } } @@ -136,7 +141,7 @@ uint32_t PID_Autotuner::on_tick(uint32_t dummy) if (temp_control != NULL) tick = true; - tickCnt += 1000 / 20; // millisecond tick count + tickCnt += (1000 / 20); // millisecond tick count return 0; } @@ -154,28 +159,41 @@ void PID_Autotuner::on_idle(void *) return; if(peakCount >= requested_cycles) { + // NOTE we output to kernel::streams becuase it is out-of-band data and original stream may be closed + THEKERNEL->streams->printf("// WARNING: Autopid did not resolve within %d cycles, these results are probably innacurate\n", requested_cycles); finishUp(); return; } float refVal = temp_control->get_temperature(); - if (refVal > absMax) absMax = refVal; - if (refVal < absMin) absMin = refVal; - // oscillate the output base on the input's relation to the setpoint if (refVal > target_temperature + noiseBand) { output = 0; //temp_control->heater_pin.pwm(output); temp_control->heater_pin.set(0); + if(!firstPeak) { + firstPeak= true; + absMax= refVal; + absMin= refVal; + } + } else if (refVal < target_temperature - noiseBand) { output = oStep; temp_control->heater_pin.pwm(output); } - bool isMax = true, isMin = true; + if ((tickCnt % 1000) == 0) { + THEKERNEL->streams->printf("// Autopid Status - %5.1f/%5.1f @%d %d/%d\n", refVal, target_temperature, output, peakCount, requested_cycles); + } - // id peaks + if(!firstPeak){ + // we wait until we hit the first peak befire we do anything else,we need to ignore the itial warmup temperatures + return; + } + + // find the peaks high and low + bool isMax = true, isMin = true; for (int i = nLookBack - 1; i >= 0; i--) { float val = lastInputs[i]; if (isMax) isMax = refVal > val; @@ -185,13 +203,15 @@ void PID_Autotuner::on_idle(void *) lastInputs[0] = refVal; + //we don't want to trust the maxes or mins until the inputs array has been filled if (lookBackCnt < nLookBack) { lookBackCnt++; // count number of times we have filled lastInputs - //we don't want to trust the maxes or mins until the inputs array has been filled return; } if (isMax) { + if(refVal > absMax) absMax= refVal; + if (peakType == 0) peakType = 1; if (peakType == -1) { peakType = 1; @@ -202,6 +222,7 @@ void PID_Autotuner::on_idle(void *) peaks[peakCount] = refVal; } else if (isMin) { + if(refVal < absMin) absMin= refVal; if (peakType == 0) peakType = -1; if (peakType == 1) { peakType = -1; @@ -212,28 +233,22 @@ void PID_Autotuner::on_idle(void *) if (peakCount < requested_cycles) peaks[peakCount] = refVal; } - // we need to ignore the first cycle warming up from room temp - - if (justchanged && peakCount > 2) { - if(peakCount == 3) { // reset min to new min - absMin = refVal; - } - //we've transitioned. check if we can autotune based on the last peaks + if (justchanged && peakCount >= 4) { + // we've transitioned. check if we can autotune based on the last peaks float avgSeparation = (std::abs(peaks[peakCount - 1] - peaks[peakCount - 2]) + std::abs(peaks[peakCount - 2] - peaks[peakCount - 3])) / 2; - s->printf("Cycle %d: max: %g, min: %g, avg separation: %g\n", peakCount, absMax, absMin, avgSeparation); - if (peakCount > 3 && avgSeparation < 0.05 * (absMax - absMin)) { + THEKERNEL->streams->printf("// Cycle %d: max: %g, min: %g, avg separation: %g\n", peakCount, absMax, absMin, avgSeparation); + if (peakCount > 3 && avgSeparation < (0.05 * (absMax - absMin))) { DEBUG_PRINTF("Stabilized\n"); finishUp(); return; } } - justchanged = false; - if ((tickCnt % 1000) == 0) { - s->printf("%s: %5.1f/%5.1f @%d %d/%d\n", temp_control->designator.c_str(), temp_control->get_temperature(), target_temperature, output, peakCount, requested_cycles); DEBUG_PRINTF("lookBackCnt= %d, peakCount= %d, absmax= %g, absmin= %g, peak1= %lu, peak2= %lu\n", lookBackCnt, peakCount, absMax, absMin, peak1, peak2); } + + justchanged = false; } @@ -242,26 +257,25 @@ void PID_Autotuner::finishUp() //we can generate tuning parameters! float Ku = 4 * (2 * oStep) / ((absMax - absMin) * 3.14159); float Pu = (float)(peak1 - peak2) / 1000; - s->printf("\tKu: %g, Pu: %g\n", Ku, Pu); + THEKERNEL->streams->printf("\tKu: %g, Pu: %g\n", Ku, Pu); float kp = 0.6 * Ku; float ki = 1.2 * Ku / Pu; float kd = Ku * Pu * 0.075; - s->printf("\tTrying:\n\tKp: %5.1f\n\tKi: %5.3f\n\tKd: %5.0f\n", kp, ki, kd); + THEKERNEL->streams->printf("\tTrying:\n\tKp: %5.1f\n\tKi: %5.3f\n\tKd: %5.0f\n", kp, ki, kd); temp_control->setPIDp(kp); temp_control->setPIDi(ki); temp_control->setPIDd(kd); - s->printf("PID Autotune Complete! The settings above have been loaded into memory, but not written to your config file.\n"); + THEKERNEL->streams->printf("PID Autotune Complete! The settings above have been loaded into memory, but not written to your config file.\n"); // and clean up temp_control->target_temperature = 0; temp_control->heater_pin.set(0); temp_control = NULL; - s = NULL; if (peaks != NULL) delete[] peaks; diff --git a/src/modules/tools/temperaturecontrol/PID_Autotuner.h b/src/modules/tools/temperaturecontrol/PID_Autotuner.h index 182c24da..f5b27c14 100644 --- a/src/modules/tools/temperaturecontrol/PID_Autotuner.h +++ b/src/modules/tools/temperaturecontrol/PID_Autotuner.h @@ -10,7 +10,6 @@ #include "Module.h" class TemperatureControl; -class StreamOutput; class PID_Autotuner : public Module { @@ -23,15 +22,12 @@ public: void on_gcode_received(void *); private: - void begin(float, StreamOutput *, int ); + void begin(float, int ); void abort(); void finishUp(); TemperatureControl *temp_control; float target_temperature; - StreamOutput *s; - - volatile bool tick; float *peaks; int requested_cycles; @@ -43,11 +39,15 @@ private: int peakType; float *lastInputs; int peakCount; - bool justchanged; float absMax, absMin; float oStep; int output; - unsigned long tickCnt; + volatile unsigned long tickCnt; + struct { + bool justchanged:1; + volatile bool tick:1; + bool firstPeak:1; + }; }; #endif /* _PID_AUTOTUNE_H */ diff --git a/src/modules/tools/temperaturecontrol/TempSensor.h b/src/modules/tools/temperaturecontrol/TempSensor.h index 767f908f..ddd709d1 100644 --- a/src/modules/tools/temperaturecontrol/TempSensor.h +++ b/src/modules/tools/temperaturecontrol/TempSensor.h @@ -5,20 +5,27 @@ you should have received a copy of the gnu general public license along with smoothie. if not, see . */ -#ifndef tempsensor_h -#define tempsensor_h +#ifndef TEMPSENSOR_H +#define TEMPSENSOR_H + +#include +#include class TempSensor { public: + virtual ~TempSensor() {} + // Load config parameters using provided "base" names. - virtual void UpdateConfig(uint16_t module_checksum, uint16_t name_checksum) {}; + virtual void UpdateConfig(uint16_t module_checksum, uint16_t name_checksum) {} // Return temperature in degrees Celsius. - virtual float get_temperature() { return -1.f; }; + virtual float get_temperature() { return -1.0F; } - // Make sure the interface provides a destructor. - virtual ~TempSensor() {} + typedef std::map sensor_options_t; + virtual bool set_optional(const sensor_options_t& options) { return false; } + virtual bool get_optional(sensor_options_t& options) { return false; } + virtual void get_raw() {} }; #endif diff --git a/src/modules/tools/temperaturecontrol/TemperatureControl.cpp b/src/modules/tools/temperaturecontrol/TemperatureControl.cpp index 6d77601c..2ba56bf7 100644 --- a/src/modules/tools/temperaturecontrol/TemperatureControl.cpp +++ b/src/modules/tools/temperaturecontrol/TemperatureControl.cpp @@ -23,13 +23,15 @@ #include "checksumm.h" #include "Gcode.h" #include "SlowTicker.h" -#include "Pauser.h" #include "ConfigValue.h" #include "PID_Autotuner.h" +#include "SerialMessage.h" +#include "utils.h" // Temp sensor implementations: #include "Thermistor.h" #include "max31855.h" +#include "AD8495.h" #include "MRI_Hooks.h" @@ -44,6 +46,7 @@ #define hysteresis_checksum CHECKSUM("hysteresis") #define heater_pin_checksum CHECKSUM("heater_pin") #define max_temp_checksum CHECKSUM("max_temp") +#define min_temp_checksum CHECKSUM("min_temp") #define get_m_code_checksum CHECKSUM("get_m_code") #define set_m_code_checksum CHECKSUM("set_m_code") @@ -56,6 +59,7 @@ #define d_factor_checksum CHECKSUM("d_factor") #define i_max_checksum CHECKSUM("i_max") +#define windup_checksum CHECKSUM("windup") #define preset1_checksum CHECKSUM("preset1") #define preset2_checksum CHECKSUM("preset2") @@ -65,7 +69,7 @@ TemperatureControl::TemperatureControl(uint16_t name, int index) name_checksum= name; pool_index= index; waiting= false; - min_temp_violated= false; + temp_violated= false; sensor= nullptr; readonly= false; } @@ -80,6 +84,7 @@ void TemperatureControl::on_module_loaded() // We start not desiring any temp this->target_temperature = UNDEFINED; + this->sensor_settings= false; // set to true if sensor settings have been overriden // Settings this->load_config(); @@ -89,7 +94,6 @@ void TemperatureControl::on_module_loaded() this->register_for_event(ON_GET_PUBLIC_DATA); if(!this->readonly) { - this->register_for_event(ON_GCODE_EXECUTE); this->register_for_event(ON_SECOND_TICK); this->register_for_event(ON_MAIN_LOOP); this->register_for_event(ON_SET_PUBLIC_DATA); @@ -109,9 +113,11 @@ void TemperatureControl::on_halt(void *arg) void TemperatureControl::on_main_loop(void *argument) { - if (this->min_temp_violated) { - THEKERNEL->streams->printf("Error: MINTEMP triggered. Check your temperature sensors!\n"); - this->min_temp_violated = false; + if (this->temp_violated) { + this->temp_violated = false; + THEKERNEL->streams->printf("Error: MINTEMP or MAXTEMP triggered. Check your temperature sensors!\n"); + THEKERNEL->streams->printf("HALT asserted - reset or M999 required\n"); + THEKERNEL->call_event(ON_HALT, nullptr); } } @@ -127,8 +133,9 @@ void TemperatureControl::load_config() this->designator = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, designator_checksum)->by_default(string("T"))->as_string(); - // Max temperature we are not allowed to get over - this->max_temp = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, max_temp_checksum)->by_default(1000)->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(); // Heater pin this->heater_pin.from_string( THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, heater_pin_checksum)->by_default("nc")->as_string()); @@ -150,6 +157,8 @@ void TemperatureControl::load_config() sensor = new Thermistor(); } else if(sensor_type.compare("max31855") == 0) { sensor = new Max31855(); + } else if(sensor_type.compare("ad8495") == 0) { + sensor = new AD8495(); } else { sensor = new TempSensor(); // A dummy implementation } @@ -166,6 +175,7 @@ void TemperatureControl::load_config() // used to enable bang bang control of heater this->use_bangbang = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, bang_bang_checksum)->by_default(false)->as_bool(); this->hysteresis = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, hysteresis_checksum)->by_default(2)->as_number(); + this->windup = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, windup_checksum)->by_default(false)->as_bool(); this->heater_pin.max_pwm( THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, max_pwm_checksum)->by_default(255)->as_number() ); this->heater_pin.set(0); set_low_on_debug(heater_pin.port_number, heater_pin.pin); @@ -200,17 +210,59 @@ void TemperatureControl::on_gcode_received(void *argument) if( gcode->m == this->get_m_code ) { char buf[32]; // should be big enough for any status - int n = snprintf(buf, sizeof(buf), "%s:%3.1f /%3.1f @%d ", this->designator.c_str(), this->get_temperature(), ((target_temperature == UNDEFINED) ? 0.0 : target_temperature), this->o); + int n = snprintf(buf, sizeof(buf), "%s:%3.1f /%3.1f @%d ", this->designator.c_str(), this->get_temperature(), ((target_temperature <= 0) ? 0.0 : target_temperature), this->o); gcode->txt_after_ok.append(buf, n); - gcode->mark_as_taken(); + return; + } + + if (gcode->m == 305) { // set or get sensor settings + if (gcode->has_letter('S') && (gcode->get_value('S') == this->pool_index)) { + TempSensor::sensor_options_t args= gcode->get_args(); + args.erase('S'); // don't include the S + if(args.size() > 0) { + // set the new options + if(sensor->set_optional(args)) { + this->sensor_settings= true; + }else{ + gcode->stream->printf("Unable to properly set sensor settings, make sure you specify all required values\n"); + } + }else{ + // don't override + this->sensor_settings= false; + } + + }else if(!gcode->has_letter('S')) { + gcode->stream->printf("%s(S%d): using %s\n", this->designator.c_str(), this->pool_index, this->readonly?"Readonly" : this->use_bangbang?"Bangbang":"PID"); + sensor->get_raw(); + TempSensor::sensor_options_t options; + if(sensor->get_optional(options)) { + for(auto &i : options) { + // foreach optional value + gcode->stream->printf("%s(S%d): %c %1.18f\n", this->designator.c_str(), this->pool_index, i.first, i.second); + } + } + } + return; } // readonly sensors don't handle the rest if(this->readonly) return; - if (gcode->m == 301) { - gcode->mark_as_taken(); + if (gcode->m == 143) { + if (gcode->has_letter('S') && (gcode->get_value('S') == this->pool_index)) { + if(gcode->has_letter('P')) { + max_temp= gcode->get_value('P'); + + } else { + gcode->stream->printf("Nothing set NOTE Usage is M143 S0 P300 where is the hotend index and

is the maximum temp to set\n"); + } + + }else if(gcode->get_num_args() == 0) { + gcode->stream->printf("Maximum temperature for %s(%d) is %f°C\n", this->designator.c_str(), this->pool_index, max_temp); + } + + } else if (gcode->m == 301) { if (gcode->has_letter('S') && (gcode->get_value('S') == this->pool_index)) { if (gcode->has_letter('P')) setPIDp( gcode->get_value('P') ); @@ -219,17 +271,33 @@ void TemperatureControl::on_gcode_received(void *argument) if (gcode->has_letter('D')) setPIDd( gcode->get_value('D') ); if (gcode->has_letter('X')) - this->i_max = gcode->get_value('X'); + this->i_max = gcode->get_value('X'); + if (gcode->has_letter('Y')) + this->heater_pin.max_pwm(gcode->get_value('Y')); + + }else if(!gcode->has_letter('S')) { + gcode->stream->printf("%s(S%d): Pf:%g If:%g Df:%g X(I_max):%g max pwm: %d O:%d\n", this->designator.c_str(), this->pool_index, this->p_factor, this->i_factor / this->PIDdt, this->d_factor * this->PIDdt, this->i_max, this->heater_pin.max_pwm(), o); } - //gcode->stream->printf("%s(S%d): Pf:%g If:%g Df:%g X(I_max):%g Pv:%g Iv:%g Dv:%g O:%d\n", this->designator.c_str(), this->pool_index, this->p_factor, this->i_factor/this->PIDdt, this->d_factor*this->PIDdt, this->i_max, this->p, this->i, this->d, o); - gcode->stream->printf("%s(S%d): Pf:%g If:%g Df:%g X(I_max):%g O:%d\n", this->designator.c_str(), this->pool_index, this->p_factor, this->i_factor / this->PIDdt, this->d_factor * this->PIDdt, this->i_max, o); } else if (gcode->m == 500 || gcode->m == 503) { // M500 saves some volatile settings to config override file, M503 just prints the settings - gcode->stream->printf(";PID settings:\nM301 S%d P%1.4f I%1.4f D%1.4f\n", this->pool_index, this->p_factor, this->i_factor / this->PIDdt, this->d_factor * this->PIDdt); - gcode->mark_as_taken(); + gcode->stream->printf(";PID settings:\nM301 S%d P%1.4f I%1.4f D%1.4f X%1.4f Y%d\n", this->pool_index, this->p_factor, this->i_factor / this->PIDdt, this->d_factor * this->PIDdt, this->i_max, this->heater_pin.max_pwm()); + + gcode->stream->printf(";Max temperature setting:\nM143 S%d P%1.4f\n", this->pool_index, this->max_temp); + + if(this->sensor_settings) { + // get or save any sensor specific optional values + TempSensor::sensor_options_t options; + if(sensor->get_optional(options) && !options.empty()) { + gcode->stream->printf(";Optional temp sensor specific settings:\nM305 S%d", this->pool_index); + for(auto &i : options) { + gcode->stream->printf(" %c%1.18f", i.first, i.second); + } + gcode->stream->printf("\n"); + } + } } else if( ( gcode->m == this->set_m_code || gcode->m == this->set_and_wait_m_code ) && gcode->has_letter('S')) { - // this only gets handled if it is not controlle dby the tool manager or is active in the toolmanager + // this only gets handled if it is not controlled by the tool manager or is active in the toolmanager this->active = true; // this is safe as old configs as well as single extruder configs the toolmanager will not be running so will return false @@ -242,36 +310,35 @@ void TemperatureControl::on_gcode_received(void *argument) } if(this->active) { - // Attach gcodes to the last block for on_gcode_execute - THEKERNEL->conveyor->append_gcode(gcode); - - // push an empty block if we have to wait, so the Planner can get things right, and we can prevent subsequent non-move gcodes from executing - if (gcode->m == this->set_and_wait_m_code) { - // ensure that no subsequent gcodes get executed with our M109 or similar - THEKERNEL->conveyor->queue_head_block(); - } - } - } - } -} - -void TemperatureControl::on_gcode_execute(void *argument) -{ - Gcode *gcode = static_cast(argument); - if( gcode->has_m) { - if (((gcode->m == this->set_m_code) || (gcode->m == this->set_and_wait_m_code)) - && gcode->has_letter('S') && this->active) { - float v = gcode->get_value('S'); - - if (v == 0.0) { - this->target_temperature = UNDEFINED; - this->heater_pin.set((this->o = 0)); - } else { - this->set_desired_temperature(v); - - if( gcode->m == this->set_and_wait_m_code && !this->waiting) { - THEKERNEL->pauser->take(); - this->waiting = true; + // required so temp change happens in order + THEKERNEL->conveyor->wait_for_empty_queue(); + + float v = gcode->get_value('S'); + + if (v == 0.0) { + this->target_temperature = UNDEFINED; + this->heater_pin.set((this->o = 0)); + } else { + this->set_desired_temperature(v); + // wait for temp to be reached, no more gcodes will be fetched until this is complete + if( gcode->m == this->set_and_wait_m_code) { + if(isinf(get_temperature()) && isinf(sensor->get_temperature())) { + THEKERNEL->streams->printf("Temperature reading is unreliable HALT asserted - reset or M999 required\n"); + THEKERNEL->call_event(ON_HALT, nullptr); + return; + } + + this->waiting = true; // on_second_tick will announce temps + while ( get_temperature() < target_temperature ) { + THEKERNEL->call_event(ON_IDLE, this); + // check if ON_HALT was called (usually by kill button) + if(THEKERNEL->is_halted() || this->target_temperature == UNDEFINED) { + THEKERNEL->streams->printf("Wait on temperature aborted by kill\n"); + break; + } + } + this->waiting = false; + } } } } @@ -292,18 +359,35 @@ void TemperatureControl::on_get_public_data(void *argument) pdr->set_data_ptr(&return_data); pdr->set_taken(); } - return; - }else if(!pdr->second_element_is(this->name_checksum)) return; + }else if(pdr->second_element_is(poll_controls_checksum)) { + // polling for all temperature controls + // add our data to the list which is passed in via the data_ptr + + std::vector *v= static_cast*>(pdr->get_data_ptr()); - // ok this is targeted at us, so send back the requested data - if(pdr->third_element_is(current_temperature_checksum)) { - this->public_data_return.current_temperature = this->get_temperature(); - this->public_data_return.target_temperature = (target_temperature == UNDEFINED) ? 0 : this->target_temperature; - this->public_data_return.pwm = this->o; - this->public_data_return.designator= this->designator; - pdr->set_data_ptr(&this->public_data_return); + struct pad_temperature t; + // setup data + t.current_temperature = this->get_temperature(); + t.target_temperature = (target_temperature <= 0) ? 0 : this->target_temperature; + t.pwm = this->o; + t.designator= this->designator; + t.id= this->name_checksum; + v->push_back(t); pdr->set_taken(); + + }else if(pdr->second_element_is(current_temperature_checksum)) { + // if targeted at us + if(pdr->third_element_is(this->name_checksum)) { + // ok this is targeted at us, so set the requ3sted data in the pointer passed into us + struct pad_temperature *t= static_cast(pdr->get_data_ptr()); + t->current_temperature = this->get_temperature(); + t->target_temperature = (target_temperature <= 0) ? 0 : this->target_temperature; + t->pwm = this->o; + t->designator= this->designator; + t->id= this->name_checksum; + pdr->set_taken(); + } } } @@ -317,6 +401,7 @@ void TemperatureControl::on_set_public_data(void *argument) if(!pdr->second_element_is(this->name_checksum)) return; // ok this is targeted at us, so set the temp + // NOTE unlike the M code this will set the temp now not when the queue is empty float t = *static_cast(pdr->get_data_ptr()); this->set_desired_temperature(t); pdr->set_taken(); @@ -329,14 +414,25 @@ void TemperatureControl::set_desired_temperature(float desired_temperature) desired_temperature = this->max_temp; } - if (desired_temperature == 1.0) + if (desired_temperature == 1.0F) desired_temperature = preset1; - else if (desired_temperature == 2.0) + else if (desired_temperature == 2.0F) desired_temperature = preset2; + float last_target_temperature= target_temperature; target_temperature = desired_temperature; - if (desired_temperature == 0.0) + if (desired_temperature <= 0.0F){ + // turning it off heater_pin.set((this->o = 0)); + + }else if(last_target_temperature <= 0.0F) { + // if it was off and we are now turning it on we need to initialize + this->lastInput= last_reading; + // set to whatever the output currently is See http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-initialization/ + this->iTerm= this->o; + if (this->iTerm > this->i_max) this->iTerm = this->i_max; + else if (this->iTerm < 0.0) this->iTerm = 0.0; + } } float TemperatureControl::get_temperature() @@ -347,26 +443,16 @@ float TemperatureControl::get_temperature() uint32_t TemperatureControl::thermistor_read_tick(uint32_t dummy) { float temperature = sensor->get_temperature(); - if(this->readonly) { - last_reading = temperature; - return 0; - } - - if (target_temperature > 0) { - if (isinf(temperature)) { - this->min_temp_violated = true; + if(!this->readonly && target_temperature > 2) { + if (isinf(temperature) || temperature < min_temp || temperature > max_temp) { + this->temp_violated = true; target_temperature = UNDEFINED; heater_pin.set((this->o = 0)); } else { pid_process(temperature); - if ((temperature > target_temperature) && waiting) { - THEKERNEL->pauser->release(); - waiting = false; - } } - } else { - heater_pin.set((this->o = 0)); } + last_reading = temperature; return 0; } @@ -399,21 +485,24 @@ void TemperatureControl::pid_process(float temperature) // regular PID control float error = target_temperature - temperature; - this->iTerm += (error * this->i_factor); - if (this->iTerm > this->i_max) this->iTerm = this->i_max; - else if (this->iTerm < 0.0) this->iTerm = 0.0; - if(this->lastInput < 0.0) this->lastInput = temperature; // set first time + float new_I = this->iTerm + (error * this->i_factor); + if (new_I > this->i_max) new_I = this->i_max; + else if (new_I < 0.0) new_I = 0.0; + if(!this->windup) this->iTerm= new_I; + float d = (temperature - this->lastInput); // calculate the PID output // TODO does this need to be scaled by max_pwm/256? I think not as p_factor already does that - this->o = (this->p_factor * error) + this->iTerm - (this->d_factor * d); + this->o = (this->p_factor * error) + new_I - (this->d_factor * d); if (this->o >= heater_pin.max_pwm()) this->o = heater_pin.max_pwm(); else if (this->o < 0) this->o = 0; + else if(this->windup) + this->iTerm = new_I; // Only update I term when output is not saturated. this->heater_pin.pwm(this->o); this->lastInput = temperature; @@ -422,7 +511,7 @@ void TemperatureControl::pid_process(float temperature) void TemperatureControl::on_second_tick(void *argument) { if (waiting) - THEKERNEL->streams->printf("%s:%3.1f /%3.1f @%d\n", designator.c_str(), get_temperature(), ((target_temperature == UNDEFINED) ? 0.0 : target_temperature), o); + THEKERNEL->streams->printf("%s:%3.1f /%3.1f @%d\n", designator.c_str(), get_temperature(), ((target_temperature <= 0) ? 0.0 : target_temperature), o); } void TemperatureControl::setPIDp(float p) diff --git a/src/modules/tools/temperaturecontrol/TemperatureControl.h b/src/modules/tools/temperaturecontrol/TemperatureControl.h index ce68ec51..aa5ef4a2 100644 --- a/src/modules/tools/temperaturecontrol/TemperatureControl.h +++ b/src/modules/tools/temperaturecontrol/TemperatureControl.h @@ -5,8 +5,8 @@ you should have received a copy of the gnu general public license along with smoothie. if not, see . */ -#ifndef temperaturecontrol_h -#define temperaturecontrol_h +#ifndef TEMPERATURECONTROL_H +#define TEMPERATURECONTROL_H #include "Module.h" #include "Pwm.h" @@ -21,7 +21,6 @@ class TemperatureControl : public Module { void on_module_loaded(); void on_main_loop(void* argument); - void on_gcode_execute(void* argument); void on_gcode_received(void* argument); void on_second_tick(void* argument); void on_get_public_data(void* argument); @@ -42,7 +41,7 @@ class TemperatureControl : public Module { int pool_index; float target_temperature; - float max_temp; + float max_temp, min_temp; float preset1; float preset2; @@ -62,19 +61,9 @@ class TemperatureControl : public Module { Pwm heater_pin; - struct { - bool use_bangbang:1; - bool waiting:1; - bool min_temp_violated:1; - bool link_to_tool:1; - bool active:1; - bool readonly:1; - }; - uint16_t set_m_code; uint16_t set_and_wait_m_code; uint16_t get_m_code; - struct pad_temperature public_data_return; std::string designator; @@ -90,6 +79,17 @@ class TemperatureControl : public Module { float i_factor; float d_factor; float PIDdt; + + struct { + bool use_bangbang:1; + bool waiting:1; + bool temp_violated:1; + bool link_to_tool:1; + bool active:1; + bool readonly:1; + bool windup:1; + bool sensor_settings:1; + }; }; #endif diff --git a/src/modules/tools/temperaturecontrol/TemperatureControlPool.cpp b/src/modules/tools/temperaturecontrol/TemperatureControlPool.cpp index aa33d998..debd8bbf 100644 --- a/src/modules/tools/temperaturecontrol/TemperatureControlPool.cpp +++ b/src/modules/tools/temperaturecontrol/TemperatureControlPool.cpp @@ -20,28 +20,22 @@ using namespace std; #define enable_checksum CHECKSUM("enable") -void TemperatureControlPool::load_tools(){ - +void TemperatureControlPool::load_tools() +{ vector modules; THEKERNEL->config->get_module_list( &modules, temperature_control_checksum ); - int cnt= 0; - for( auto cs : modules ){ + int cnt = 0; + for( auto cs : modules ) { // 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( controller ); + if( THEKERNEL->config->value(temperature_control_checksum, cs, enable_checksum )->as_bool() ) { + TemperatureControl *controller = new TemperatureControl(cs, cnt++); THEKERNEL->add_module(controller); } } // no need to create one of these if no heaters defined if(cnt > 0) { - PID_Autotuner* pidtuner = new PID_Autotuner(); - THEKERNEL->add_module( pidtuner ); + PID_Autotuner *pidtuner = new PID_Autotuner(); + THEKERNEL->add_module( pidtuner ); } } - - - - - diff --git a/src/modules/tools/temperaturecontrol/TemperatureControlPool.h b/src/modules/tools/temperaturecontrol/TemperatureControlPool.h index d6f931ab..c0d87678 100644 --- a/src/modules/tools/temperaturecontrol/TemperatureControlPool.h +++ b/src/modules/tools/temperaturecontrol/TemperatureControlPool.h @@ -8,6 +8,8 @@ #ifndef TEMPERATURECONTROLPOOL_H #define TEMPERATURECONTROLPOOL_H +#include + class TemperatureControlPool { public: void load_tools(); diff --git a/src/modules/tools/temperaturecontrol/TemperatureControlPublicAccess.h b/src/modules/tools/temperaturecontrol/TemperatureControlPublicAccess.h index 4792f425..fcedb8f0 100644 --- a/src/modules/tools/temperaturecontrol/TemperatureControlPublicAccess.h +++ b/src/modules/tools/temperaturecontrol/TemperatureControlPublicAccess.h @@ -13,11 +13,13 @@ #define target_temperature_checksum CHECKSUM("target_temperature") #define temperature_pwm_checksum CHECKSUM("temperature_pwm") #define pool_index_checksum CHECKSUM("pool_index") +#define poll_controls_checksum CHECKSUM("poll_controllers") struct pad_temperature { float current_temperature; float target_temperature; int pwm; + uint16_t id; std::string designator; }; #endif diff --git a/src/modules/tools/temperaturecontrol/Thermistor.cpp b/src/modules/tools/temperaturecontrol/Thermistor.cpp index ae987556..3e8724f1 100644 --- a/src/modules/tools/temperaturecontrol/Thermistor.cpp +++ b/src/modules/tools/temperaturecontrol/Thermistor.cpp @@ -5,19 +5,22 @@ You should have received a copy of the GNU General Public License along with Smoothie. If not, see . */ +#include "Thermistor.h" #include "libs/Kernel.h" -#include #include "libs/Pin.h" #include "Config.h" #include "checksumm.h" #include "Adc.h" #include "ConfigValue.h" #include "libs/Median.h" -#include "Thermistor.h" +#include "utils.h" +#include "StreamOutputPool.h" // a const list of predefined thermistors #include "predefined_thermistors.h" +#include + #include "MRI_Hooks.h" #define UNDEFINED -1 @@ -31,9 +34,19 @@ #define r1_checksum CHECKSUM("r1") #define r2_checksum CHECKSUM("r2") #define thermistor_pin_checksum CHECKSUM("thermistor_pin") +#define rt_curve_checksum CHECKSUM("rt_curve") +#define coefficients_checksum CHECKSUM("coefficients") +#define use_beta_table_checksum CHECKSUM("use_beta_table") + Thermistor::Thermistor() { + this->bad_config = false; + this->use_steinhart_hart= false; + this->beta= 0.0F; // not used by default + min_temp= 999; + max_temp= 0; + this->thermistor_number= 0; // not a predefined thermistor } Thermistor::~Thermistor() @@ -46,65 +59,363 @@ void Thermistor::UpdateConfig(uint16_t module_checksum, uint16_t name_checksum) // Values are here : http://reprap.org/wiki/Thermistor this->r0 = 100000; this->t0 = 25; - this->beta = 4066; this->r1 = 0; this->r2 = 4700; + this->beta = 4066; + // force use of beta perdefined thermistor table based on betas + bool use_beta_table= THEKERNEL->config->value(module_checksum, name_checksum, use_beta_table_checksum)->by_default(false)->as_bool(); + + bool found= false; + int cnt= 0; // load a predefined thermistor name if found - string thermistor = THEKERNEL->config->value(module_checksum, name_checksum, thermistor_checksum)->as_string(); - for (auto i : predefined_thermistors) { - if(thermistor.compare(i.name) == 0) { - this->beta = i.beta; - this->r0 = i.r0; - this->t0 = i.t0; - this->r1 = i.r1; - this->r2 = i.r2; - break; + string thermistor = THEKERNEL->config->value(module_checksum, name_checksum, thermistor_checksum)->by_default("")->as_string(); + if(!thermistor.empty()) { + if(!use_beta_table) { + for (auto& i : predefined_thermistors) { + cnt++; + if(thermistor.compare(i.name) == 0) { + this->c1 = i.c1; + this->c2 = i.c2; + this->c3 = i.c3; + this->r1 = i.r1; + this->r2 = i.r2; + use_steinhart_hart= true; + found= true; + break; + } + } + } + + // fall back to the old beta pre-defined table if not found above + if(!found) { + cnt= 0; + for (auto& i : predefined_thermistors_beta) { + cnt++; + if(thermistor.compare(i.name) == 0) { + this->beta = i.beta; + this->r0 = i.r0; + this->t0 = i.t0; + this->r1 = i.r1; + this->r2 = i.r2; + use_steinhart_hart= false; + cnt |= 0x80; // set MSB to indicate beta table + found= true; + break; + } + } + } + + if(!found) { + thermistor_number= 0; + }else{ + thermistor_number= cnt; } } // Preset values are overriden by specified values + if(!use_steinhart_hart) { + this->beta = THEKERNEL->config->value(module_checksum, name_checksum, beta_checksum)->by_default(this->beta)->as_number(); // Thermistor beta rating. See http://reprap.org/bin/view/Main/MeasuringThermistorBeta + } this->r0 = THEKERNEL->config->value(module_checksum, name_checksum, r0_checksum )->by_default(this->r0 )->as_number(); // Stated resistance eg. 100K this->t0 = THEKERNEL->config->value(module_checksum, name_checksum, t0_checksum )->by_default(this->t0 )->as_number(); // Temperature at stated resistance, eg. 25C - this->beta = THEKERNEL->config->value(module_checksum, name_checksum, beta_checksum)->by_default(this->beta)->as_number(); // Thermistor beta rating. See http://reprap.org/bin/view/Main/MeasuringThermistorBeta this->r1 = THEKERNEL->config->value(module_checksum, name_checksum, r1_checksum )->by_default(this->r1 )->as_number(); this->r2 = THEKERNEL->config->value(module_checksum, name_checksum, r2_checksum )->by_default(this->r2 )->as_number(); - // Thermistor math - j = (1.0 / beta); - k = (1.0 / (t0 + 273.15)); - // Thermistor pin for ADC readings this->thermistor_pin.from_string(THEKERNEL->config->value(module_checksum, name_checksum, thermistor_pin_checksum )->required()->as_string()); THEKERNEL->adc->enable_pin(&thermistor_pin); + + // specify the three Steinhart-Hart coefficients + // specified as three comma separated floats, no spaces + string coef= THEKERNEL->config->value(module_checksum, name_checksum, coefficients_checksum)->by_default("")->as_string(); + + // speficy three temp,resistance pairs, best to use 25° 150° 240° and the coefficients will be calculated + // specified as 25.0,100000.0,150.0,1355.0,240.0,203.0 which is temp in °C,resistance in ohms + string rtc= THEKERNEL->config->value(module_checksum, name_checksum, rt_curve_checksum)->by_default("")->as_string(); + if(!rtc.empty()) { + // use the http://en.wikipedia.org/wiki/Steinhart-Hart_equation instead of beta, as it is more accurate over the entire temp range + // we use three temps/resistor values taken from the thermistor R-C curve found in most datasheets + // eg http://sensing.honeywell.com/resistance-temperature-conversion-table-no-16, we take the resistance for 25°,150°,240° and the resistance in that table is 100000*R-T Curve coefficient + // eg http://www.atcsemitec.co.uk/gt-2_thermistors.html for the semitec is even easier as they give the resistance in column 8 of the R/T table + + // then calculate the three Steinhart-Hart coefficients + // format in config is T1,R1,T2,R2,T3,R3 if all three are not sepcified we revert to an invalid config state + std::vector trl= parse_number_list(rtc.c_str()); + if(trl.size() != 6) { + // punt we need 6 numbers, three pairs + THEKERNEL->streams->printf("Error in config need 6 numbers for Steinhart-Hart\n"); + this->bad_config= true; + return; + } + + // calculate the coefficients + std::tie(this->c1, this->c2, this->c3) = calculate_steinhart_hart_coefficients(trl[0], trl[1], trl[2], trl[3], trl[4], trl[5]); + + this->use_steinhart_hart= true; + + }else if(!coef.empty()) { + // the three Steinhart-Hart coefficients + // format in config is C1,C2,C3 if three are not specified we revert to an invalid config state + std::vector v= parse_number_list(coef.c_str()); + if(v.size() != 3) { + // punt we need 6 numbers, three pairs + THEKERNEL->streams->printf("Error in config need 3 Steinhart-Hart coefficients\n"); + this->bad_config= true; + return; + } + + this->c1= v[0]; + this->c2= v[1]; + this->c3= v[2]; + this->use_steinhart_hart= true; + + }else if(!use_steinhart_hart) { + // if using beta + calc_jk(); + + }else if(!found) { + THEKERNEL->streams->printf("Error in config need rt_curve, coefficients, beta or a valid predefined thermistor defined\n"); + this->bad_config= true; + return; + } + +} + +// print out predefined thermistors +void Thermistor::print_predefined_thermistors(StreamOutput* s) +{ + int cnt= 1; + s->printf("S/H table\n"); + for (auto& i : predefined_thermistors) { + s->printf("%d - %s\n", cnt++, i.name); + } + + cnt= 129; + s->printf("Beta table\n"); + for (auto& i : predefined_thermistors_beta) { + s->printf("%d - %s\n", cnt++, i.name); + } +} + +// calculate the coefficients from the supplied three Temp/Resistance pairs +// copied from https://github.com/MarlinFirmware/Marlin/blob/Development/Marlin/scripts/createTemperatureLookupMarlin.py +std::tuple Thermistor::calculate_steinhart_hart_coefficients(float t1, float r1, float t2, float r2, float t3, float r3) +{ + float l1 = logf(r1); + float l2 = logf(r2); + float l3 = logf(r3); + + float y1 = 1.0F / (t1 + 273.15F); + float y2 = 1.0F / (t2 + 273.15F); + float y3 = 1.0F / (t3 + 273.15F); + float x = (y2 - y1) / (l2 - l1); + float y = (y3 - y1) / (l3 - l1); + float c = (y - x) / ((l3 - l2) * (l1 + l2 + l3)); + float b = x - c * (powf(l1,2) + powf(l2,2) + l1 * l2); + float a = y1 - (b + powf(l1,2) * c) * l1; + + if(c < 0) { + THEKERNEL->streams->printf("WARNING: negative coefficient in calculate_steinhart_hart_coefficients. Something may be wrong with the measurements\n"); + c = -c; + } + return std::make_tuple(a, b, c); +} + +void Thermistor::calc_jk() +{ + // Thermistor math + if(beta > 0.0F) { + j = (1.0F / beta); + k = (1.0F / (t0 + 273.15F)); + }else{ + THEKERNEL->streams->printf("WARNING: beta cannot be 0\n"); + this->bad_config= true; + } } float Thermistor::get_temperature() { - return adc_value_to_temperature(new_thermistor_reading()); + if(bad_config) return infinityf(); + float t= adc_value_to_temperature(new_thermistor_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 Thermistor::get_raw() +{ + if(this->bad_config) { + THEKERNEL->streams->printf("WARNING: The config is bad for this temperature sensor\n"); + } + + int adc_value= new_thermistor_reading(); + const uint32_t max_adc_value= THEKERNEL->adc->get_max_value(); + + // resistance of the thermistor in ohms + float r = r2 / (((float)max_adc_value / adc_value) - 1.0F); + if (r1 > 0.0F) r = (r1 * r) / (r1 - r); + + THEKERNEL->streams->printf("adc= %d, resistance= %f\n", adc_value, r); + + float t; + if(this->use_steinhart_hart) { + THEKERNEL->streams->printf("S/H c1= %1.18f, c2= %1.18f, c3= %1.18f\n", c1, c2, c3); + float l = logf(r); + t= (1.0F / (this->c1 + this->c2 * l + this->c3 * powf(l,3))) - 273.15F; + THEKERNEL->streams->printf("S/H temp= %f, min= %f, max= %f, delta= %f\n", t, min_temp, max_temp, max_temp-min_temp); + }else{ + t= (1.0F / (k + (j * logf(r / r0)))) - 273.15F; + THEKERNEL->streams->printf("beta temp= %f, min= %f, max= %f, delta= %f\n", t, min_temp, max_temp, max_temp-min_temp); + } + + // if using a predefined thermistor show its name and which table it is from + if(thermistor_number != 0) { + string name= (thermistor_number&0x80) ? predefined_thermistors_beta[(thermistor_number&0x7F)-1].name : predefined_thermistors[thermistor_number-1].name; + THEKERNEL->streams->printf("Using predefined thermistor %d in %s table: %s\n", thermistor_number&0x7F, (thermistor_number&0x80)?"Beta":"S/H", name.c_str()); + } + + // reset the min/max + min_temp= max_temp= t; } -float Thermistor::adc_value_to_temperature(int adc_value) +float Thermistor::adc_value_to_temperature(uint32_t adc_value) { - if ((adc_value == 4095) || (adc_value == 0)) + const uint32_t max_adc_value= THEKERNEL->adc->get_max_value(); + if ((adc_value >= max_adc_value) || (adc_value == 0)) return infinityf(); - float r = r2 / ((4095.0 / adc_value) - 1.0); - if (r1 > 0) - r = (r1 * r) / (r1 - r); - return (1.0 / (k + (j * log(r / r0)))) - 273.15; + + // resistance of the thermistor in ohms + float r = r2 / (((float)max_adc_value / adc_value) - 1.0F); + if (r1 > 0.0F) r = (r1 * r) / (r1 - r); + + if(r > this->r0 * 8) return infinityf(); // 800k is probably open circuit + + float t; + if(this->use_steinhart_hart) { + float l = logf(r); + t= (1.0F / (this->c1 + this->c2 * l + this->c3 * powf(l,3))) - 273.15F; + }else{ + // use Beta value + t= (1.0F / (k + (j * logf(r / r0)))) - 273.15F; + } + + return t; } int Thermistor::new_thermistor_reading() { - int last_raw = THEKERNEL->adc->read(&thermistor_pin); - if (queue.size() >= queue.capacity()) { - uint16_t l; - queue.pop_front(l); - } - uint16_t r = last_raw; - queue.push_back(r); - for (int i=0; iadc->read(&thermistor_pin); } + +bool Thermistor::set_optional(const sensor_options_t& options) { + bool define_beta= false; + bool change_beta= false; + uint8_t define_shh= 0; + uint8_t predefined= 0; + + for(auto &i : options) { + switch(i.first) { + case 'B': this->beta= i.second; define_beta= true; break; + case 'R': this->r0= i.second; change_beta= true; break; + case 'X': this->t0= i.second; change_beta= true; break; + case 'I': this->c1= i.second; define_shh++; break; + case 'J': this->c2= i.second; define_shh++; break; + case 'K': this->c3= i.second; define_shh++; break; + case 'P': predefined= roundf(i.second); break; + } + } + + if(predefined != 0) { + if(define_beta || change_beta || define_shh != 0) { + // cannot use a predefined with any other option + this->bad_config= true; + return false; + } + + if(predefined & 0x80) { + // use the predefined beta table + uint8_t n= (predefined&0x7F)-1; + if(n >= sizeof(predefined_thermistors_beta) / sizeof(thermistor_beta_table_t)) { + // not a valid index + return false; + } + auto &i= predefined_thermistors_beta[n]; + this->beta = i.beta; + this->r0 = i.r0; + this->t0 = i.t0; + this->r1 = i.r1; + this->r2 = i.r2; + use_steinhart_hart= false; + calc_jk(); + thermistor_number= predefined; + this->bad_config= false; + return true; + + }else { + // use the predefined S/H table + uint8_t n= predefined-1; + if(n >= sizeof(predefined_thermistors) / sizeof(thermistor_table_t)) { + // not a valid index + return false; + } + auto &i= predefined_thermistors[n]; + this->c1 = i.c1; + this->c2 = i.c2; + this->c3 = i.c3; + this->r1 = i.r1; + this->r2 = i.r2; + use_steinhart_hart= true; + thermistor_number= predefined; + this->bad_config= false; + return true; + } + } + + bool error= false; + // if in Steinhart-Hart mode make sure B is specified, if in beta mode make sure all C1,C2,C3 are set and no beta settings + // this is needed if swapping between modes + if(use_steinhart_hart && define_shh == 0 && !define_beta) error= true; // if switching from SHH to beta need to specify new beta + if(!use_steinhart_hart && define_shh > 0 && (define_beta || change_beta)) error= true; // if in beta mode and switching to SHH malke sure no beta settings are set + if(!use_steinhart_hart && !(define_beta || change_beta) && define_shh != 3) error= true; // if in beta mode and switching to SHH must specify all three SHH + if(use_steinhart_hart && define_shh > 0 && (define_beta || change_beta)) error= true; // if setting SHH anfd already in SHH do not specify any beta values + + if(error) { + this->bad_config= true; + return false; + } + if(define_beta || change_beta) { + calc_jk(); + use_steinhart_hart= false; + }else if(define_shh > 0) { + use_steinhart_hart= true; + }else{ + return false; + } + + if(this->bad_config) this->bad_config= false; + + return true; +} + +bool Thermistor::get_optional(sensor_options_t& options) { + if(thermistor_number != 0) { + options['P']= thermistor_number; + return true; + } + + if(use_steinhart_hart) { + options['I']= this->c1; + options['J']= this->c2; + options['K']= this->c3; + + }else{ + options['B']= this->beta; + options['X']= this->t0; + options['R']= this->r0; + } + + return true; +}; diff --git a/src/modules/tools/temperaturecontrol/Thermistor.h b/src/modules/tools/temperaturecontrol/Thermistor.h index 2cb28e3a..e3497fc2 100644 --- a/src/modules/tools/temperaturecontrol/Thermistor.h +++ b/src/modules/tools/temperaturecontrol/Thermistor.h @@ -5,43 +5,69 @@ you should have received a copy of the gnu general public license along with smoothie. if not, see . */ -#ifndef thermistor_h -#define thermistor_h +#ifndef THERMISTOR_H +#define THERMISTOR_H #include "TempSensor.h" #include "RingBuffer.h" +#include "Pin.h" + +#include #define QUEUE_LEN 32 +class StreamOutput; class Thermistor : public TempSensor { public: Thermistor(); ~Thermistor(); - + // TempSensor interface. void UpdateConfig(uint16_t module_checksum, uint16_t name_checksum); float get_temperature(); - + bool set_optional(const sensor_options_t& options); + bool get_optional(sensor_options_t& options); + void get_raw(); + static std::tuple calculate_steinhart_hart_coefficients(float t1, float r1, float t2, float r2, float t3, float r3); + static void print_predefined_thermistors(StreamOutput*); + private: int new_thermistor_reading(); - float adc_value_to_temperature(int adc_value); + float adc_value_to_temperature(uint32_t adc_value); + void calc_jk(); - // Thermistor computation settings + // Thermistor computation settings using beta, not used if using Steinhart-Hart float r0; float t0; + + // on board resistor settings int r1; int r2; - float beta; - float j; - float k; + + union { + // this saves memory as we only use either beta or SHH + struct{ + float beta; + float j; + float k; + }; + struct{ + float c1; + float c2; + float c3; + }; + }; Pin thermistor_pin; - RingBuffer queue; // Queue of readings - uint16_t median_buffer[QUEUE_LEN]; - + float min_temp, max_temp; + struct { + bool bad_config:1; + bool use_steinhart_hart:1; + }; + uint8_t thermistor_number; }; #endif diff --git a/src/modules/tools/temperaturecontrol/predefined_thermistors.h b/src/modules/tools/temperaturecontrol/predefined_thermistors.h dissimilarity index 72% index 81778028..f2400d9f 100644 --- a/src/modules/tools/temperaturecontrol/predefined_thermistors.h +++ b/src/modules/tools/temperaturecontrol/predefined_thermistors.h @@ -1,19 +1,37 @@ - -typedef struct { - const char *name; - float beta; - float r0; - float t0; - int r1; - int r2; -} const thermistor_table_t; - -static const thermistor_table_t predefined_thermistors[] { - // name, beta, r0, t0, r1, r2 - {"EPCOS100K", 4066.0F, 100000.0F, 25.0F, 0, 4700}, - {"RRRF100K", 3960.0F, 100000.0F, 25.0F, 0, 4700}, - {"RRRF10K", 3964.0F, 10000.0F, 25.0F, 680, 1600}, - {"Honeywell100K", 3974.0F, 100000.0F, 25.0F, 0, 4700}, - {"Semitec", 4267.0F, 100000.0F, 25.0F, 0, 4700}, - {"HT100K", 3990.0F, 100000.0F, 25.0F, 0, 4700} -}; +typedef struct { + const char *name; + int r1; + int r2; + 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 + {"RRRF100K", 0, 4700, 3960.0F, 100000.0F, 25.0F}, + {"RRRF10K", 680, 1600, 3964.0F, 10000.0F, 25.0F}, + {"Honeywell100K", 0, 4700, 3974.0F, 100000.0F, 25.0F}, // 135-104LAG-J01 + {"Semitec", 0, 4700, 4267.0F, 100000.0F, 25.0F}, // 104GT-2 + {"HT100K", 0, 4700, 3990.0F, 100000.0F, 25.0F} +}; + +typedef struct { + const char *name; + int r1; + int r2; + float c1, c2, c3; +} const thermistor_table_t; + +// Use one of the following scripts to calcuate 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[] { + // name, r1, r2, c1, c2, c3 + {"EPCOS100K", 0, 4700, 0.000722378300319346F, 0.000216301852054578F, 9.2641025635702e-08F}, // B57540G0104F000 + {"Vishay100K", 0, 4700, 0.0007022607370F, 0.0002209155484F, 7.101626461e-08F }, // NTCS0603E3104FXT + {"Honeywell100K", 0, 4700, 0.000596153185928425F, 0.000231333192738335F, 6.19534004306738e-08F}, // 135-104LAG-J01 + {"Semitec", 0, 4700, 0.000811290160145459F, 0.000211355789144265F, 7.17614730463848e-08F}, // 104GT-2 + {"Honeywell-QAD", 0, 4700, 0.000827339299500986F, 0.000208786427208899F, 8.05595282332277e-08F} // 135-104QAD-J01 +}; diff --git a/src/modules/tools/temperatureswitch/TemperatureSwitch.cpp b/src/modules/tools/temperatureswitch/TemperatureSwitch.cpp index 82fd2398..510da512 100755 --- a/src/modules/tools/temperatureswitch/TemperatureSwitch.cpp +++ b/src/modules/tools/temperatureswitch/TemperatureSwitch.cpp @@ -26,6 +26,8 @@ Author: Michael Hackney, mhackney@eclecticangler.com #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") @@ -35,12 +37,19 @@ Author: Michael Hackney, mhackney@eclecticangler.com #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() { - this->temperatureswitch_state = false; - this->second_counter = 0; +} + +TemperatureSwitch::~TemperatureSwitch() +{ + THEKERNEL->unregister_for_event(ON_SECOND_TICK, this); + THEKERNEL->unregister_for_event(ON_GCODE_RECEIVED, this); } // Load module @@ -57,16 +66,14 @@ void TemperatureSwitch::on_module_loaded() delete this; } - -bool TemperatureSwitch::load_config(uint16_t modcs) +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 false; + 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()){ @@ -77,45 +84,40 @@ bool TemperatureSwitch::load_config(uint16_t modcs) designator= s[0]; } - if(designator == 0) return false; // no designator then not valid - - // create a new temperature switch module - TemperatureSwitch *ts= new TemperatureSwitch(); - - // get the list of temperature controllers and remove any that don't have designator == specified designator - vector tempcontrollers; - THEKERNEL->config->get_module_list(&tempcontrollers, temperature_control_checksum); - - // see what its designator is and add to list of it the one we specified - void *returned_temp; - for (auto controller : tempcontrollers) { - bool temp_ok = PublicData::get_value(temperature_control_checksum, controller, current_temperature_checksum, &returned_temp); - if (temp_ok) { - struct pad_temperature temp = *static_cast(returned_temp); - if (temp.designator[0] == designator) { - ts->temp_controllers.push_back(controller); - } - } - } - - // if we don't have any matching controllers, then not valid - if (ts->temp_controllers.empty()) { - delete ts; - return false; - } + if(designator == 0) return nullptr; // no designator then not valid // load settings from config file - s = THEKERNEL->config->value(temperatureswitch_checksum, modcs, temperatureswitch_switch_checksum)->by_default("")->as_string(); - if(s.empty()) { + 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 - s = THEKERNEL->config->value(temperatureswitch_checksum, modcs, temperatureswitch_type_checksum)->by_default("")->as_string(); - if(s.empty()) { + switchname = THEKERNEL->config->value(temperatureswitch_checksum, modcs, temperatureswitch_type_checksum)->by_default("")->as_string(); + if(switchname.empty()) { // no switch specified so invalid entry - delete this; - return false; + THEKERNEL->streams->printf("WARNING TEMPERATURESWITCH: no switch specified\n"); + return nullptr; } } - ts->temperatureswitch_switch_cs= get_checksum(s); // checksum of the switch to use + + // 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(); @@ -124,63 +126,116 @@ bool TemperatureSwitch::load_config(uint16_t modcs) 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); - return true; + 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; + 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 { - second_counter = 0; - float current_temp = this->get_highest_temperature(); - - if (current_temp >= this->temperatureswitch_threshold_temp) { - // temp >= threshold temp, turn the cooler switch on if it isn't already - if (!temperatureswitch_state) { - set_switch(true); - current_delay = temperatureswitch_cooldown_poll; - } - } else { - // temp < threshold temp, turn the cooler switch off if it isn't already - if (temperatureswitch_state) { - set_switch(false); - current_delay = temperatureswitch_heatup_poll; - } - } + 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() { - void *returned_temp; float high_temp = 0.0; - for (auto controller : temp_controllers) { - bool temp_ok = PublicData::get_value(temperature_control_checksum, controller, current_temperature_checksum, &returned_temp); - if (temp_ok) { - struct pad_temperature temp = *static_cast(returned_temp); + 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 (temp.current_temperature > high_temp) { - high_temp = temp.current_temperature; + 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) { - this->temperatureswitch_state = switch_state; - bool ok = PublicData::set_value(switch_checksum, this->temperatureswitch_switch_cs, state_checksum, &this->temperatureswitch_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"); + 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 index 768ae444..1c45c90d 100755 --- a/src/modules/tools/temperatureswitch/TemperatureSwitch.h +++ b/src/modules/tools/temperatureswitch/TemperatureSwitch.h @@ -26,11 +26,17 @@ 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: - bool load_config(uint16_t modcs); + 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(); @@ -38,8 +44,8 @@ class TemperatureSwitch : public Module // turn the switch on or off void set_switch(bool cooler_state); - // the set of temperature controllers that match the reuired designator prefix - vector temp_controllers; + // temperature has changed state + void set_state(STATE state); // temperatureswitch.hotend.threshold_temp float temperatureswitch_threshold_temp; @@ -61,8 +67,16 @@ class TemperatureSwitch : public Module // we are delaying for this many seconds uint16_t current_delay; - // is the switch currently on (1) or off (0)? - bool temperatureswitch_state; + // 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/ToolManager.cpp b/src/modules/tools/toolmanager/ToolManager.cpp index bc5a0223..7aea92ce 100644 --- a/src/modules/tools/toolmanager/ToolManager.cpp +++ b/src/modules/tools/toolmanager/ToolManager.cpp @@ -26,8 +26,6 @@ using namespace std; #include "libs/StreamOutput.h" #include "FileStream.h" -#include "modules/robot/RobotPublicAccess.h" - #define return_error_on_unhandled_gcode_checksum CHECKSUM("return_error_on_unhandled_gcode") ToolManager::ToolManager(){ @@ -52,7 +50,6 @@ void ToolManager::on_gcode_received(void *argument){ if( gcode->has_letter('T') ){ int new_tool = gcode->get_value('T'); - gcode->mark_as_taken(); if(new_tool >= (int)this->tools.size() || new_tool < 0){ // invalid tool if( return_error_on_unhandled_gcode ) { diff --git a/src/modules/tools/zprobe/DeltaCalibrationStrategy.cpp b/src/modules/tools/zprobe/DeltaCalibrationStrategy.cpp index bd39f41f..07e11dbd 100644 --- a/src/modules/tools/zprobe/DeltaCalibrationStrategy.cpp +++ b/src/modules/tools/zprobe/DeltaCalibrationStrategy.cpp @@ -12,11 +12,14 @@ #include "Conveyor.h" #include "ZProbe.h" #include "BaseSolution.h" +#include "StepperMotor.h" #include #include -#define radius_checksum CHECKSUM("radius") +#define radius_checksum CHECKSUM("radius") +#define initial_height_checksum CHECKSUM("initial_height") + // deprecated #define probe_radius_checksum CHECKSUM("probe_radius") @@ -29,6 +32,10 @@ bool DeltaCalibrationStrategy::handleConfig() 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; } @@ -54,6 +61,13 @@ bool DeltaCalibrationStrategy::handleGcode(Gcode *gcode) } 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, probe not triggered\n"); + } + return true; } } else if(gcode->has_m) { @@ -63,8 +77,6 @@ bool DeltaCalibrationStrategy::handleGcode(Gcode *gcode) return false; } - - // calculate the X and Y positions for the three towers given the radius from the center static std::tuple getCoordinates(float radius) { @@ -76,6 +88,57 @@ static std::tuple getCoordinates(float 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) +{ + // 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; + for(auto& i : pp) { + int s; + if(!zprobe->doProbeAt(s, i[0], i[1])) return false; + float z = zprobe->zsteps_to_mm(s); + gcode->stream->printf("X:%1.4f Y:%1.4f Z:%1.4f (%d) A:%1.4f B:%1.4f C:%1.4f\n", + i[0], i[1], z, s, + THEKERNEL->robot->actuators[0]->get_current_position()+z, + THEKERNEL->robot->actuators[1]->get_current_position()+z, + THEKERNEL->robot->actuators[2]->get_current_position()+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 + int s; + if(!zprobe->run_probe(s, false)) return NAN; + + return zprobe->zsteps_to_mm(s) + 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 @@ -117,15 +180,11 @@ bool DeltaCalibrationStrategy::calibrate_delta_endstops(Gcode *gcode) } } - // home - zprobe->home(); - - // find bed, run at fast rate - int s; - if(!zprobe->run_probe(s, true)) return false; - - float bedht = zprobe->zsteps_to_mm(s) - zprobe->getProbeHeight(); // distance to move from home to 5mm above bed - gcode->stream->printf("Bed ht is %f mm\n", bedht); + // 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); // move to start position zprobe->home(); @@ -133,6 +192,7 @@ bool DeltaCalibrationStrategy::calibrate_delta_endstops(Gcode *gcode) // get initial probes // probe the base of the X tower + int s; if(!zprobe->doProbeAt(s, t1x, t1y)) return false; float t1z = zprobe->zsteps_to_mm(s); gcode->stream->printf("T1-0 Z:%1.4f C:%d\n", t1z, s); @@ -222,12 +282,11 @@ bool DeltaCalibrationStrategy::calibrate_delta_radius(Gcode *gcode) float t1x, t1y, t2x, t2y, t3x, t3y; std::tie(t1x, t1y, t2x, t2y, t3x, t3y) = getCoordinates(this->probe_radius); - zprobe->home(); - // find bed, then move to a point 5mm above it - int s; - if(!zprobe->run_probe(s, true)) return false; - float bedht = zprobe->zsteps_to_mm(s) - zprobe->getProbeHeight(); // distance to move from home to 5mm above bed - gcode->stream->printf("Bed ht is %f mm\n", bedht); + // 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); zprobe->home(); zprobe->coordinated_move(NAN, NAN, -bedht, zprobe->getFastFeedrate(), true); // do a relative move from home to the point above the bed diff --git a/src/modules/tools/zprobe/DeltaCalibrationStrategy.h b/src/modules/tools/zprobe/DeltaCalibrationStrategy.h index 88f2769f..34c86004 100644 --- a/src/modules/tools/zprobe/DeltaCalibrationStrategy.h +++ b/src/modules/tools/zprobe/DeltaCalibrationStrategy.h @@ -20,8 +20,11 @@ private: 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/Plane3D.cpp b/src/modules/tools/zprobe/Plane3D.cpp index ee38e9b1..38ad0138 100644 --- a/src/modules/tools/zprobe/Plane3D.cpp +++ b/src/modules/tools/zprobe/Plane3D.cpp @@ -11,8 +11,7 @@ Plane3D::Plane3D(const Vector3 &v1, const Vector3 &v2, const Vector3 &v3) // ax+by+cz+d=0 // solve for d - Vector3 dv = normal.mul(v1); - d = -dv[0] - dv[1] - dv[2]; + d = -normal.dot(v1); } typedef union { float f; uint32_t u; } conv_t; @@ -21,7 +20,7 @@ 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.set(ca.f, cb.f, cc.f); + this->normal = Vector3(ca.f, cb.f, cc.f); this->d= cd.f; } diff --git a/src/modules/tools/zprobe/ThreePointStrategy.cpp b/src/modules/tools/zprobe/ThreePointStrategy.cpp index 8507be58..791c6246 100644 --- a/src/modules/tools/zprobe/ThreePointStrategy.cpp +++ b/src/modules/tools/zprobe/ThreePointStrategy.cpp @@ -39,6 +39,7 @@ 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 @@ -115,7 +116,13 @@ bool ThreePointStrategy::handleGcode(Gcode *gcode) { if(gcode->has_g) { // G code processing - if( gcode->g == 31 ) { // report status + 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{ @@ -127,6 +134,12 @@ bool ThreePointStrategy::handleGcode(Gcode *gcode) } else if( gcode->g == 32 ) { // three point probe // first wait for an empty queue i.e. no moves left THEKERNEL->conveyor->wait_for_empty_queue(); + if(!gcode->has_letter('K')) { // K will keep current compensation to test plane + // 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 { @@ -155,8 +168,9 @@ bool ThreePointStrategy::handleGcode(Gcode *gcode) this->plane= nullptr; // delete the compensationTransform in robot setAdjustFunction(false); + gcode->stream->printf("saved plane cleared\n"); }else{ - // smoothie specific way to restire a saved plane + // 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'); @@ -282,7 +296,7 @@ bool ThreePointStrategy::doProbing(StreamOutput *stream) 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].set(x, y, z); + v[i] = Vector3(x, y, z); } // if first point is not within tolerance report it, it should ideally be 0 @@ -309,6 +323,36 @@ bool ThreePointStrategy::doProbing(StreamOutput *stream) 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) { diff --git a/src/modules/tools/zprobe/ThreePointStrategy.h b/src/modules/tools/zprobe/ThreePointStrategy.h index c671442a..652f4c20 100644 --- a/src/modules/tools/zprobe/ThreePointStrategy.h +++ b/src/modules/tools/zprobe/ThreePointStrategy.h @@ -26,6 +26,7 @@ private: 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]; diff --git a/src/modules/tools/zprobe/ZGridStrategy.cpp b/src/modules/tools/zprobe/ZGridStrategy.cpp new file mode 100644 index 00000000..5fd9a1c1 --- /dev/null +++ b/src/modules/tools/zprobe/ZGridStrategy.cpp @@ -0,0 +1,699 @@ +/* + 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 + + 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 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) AHB0.dealloc(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->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->home_before_probe = THEKERNEL->config->value(leveling_strategy_checksum, ZGrid_leveling_checksum, home_before_probe_checksum)->by_default(HOMEXYZ)->as_number(); // Morgan default = 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) AHB0.dealloc(this->pData); + + // Allocate program memory for the pData grid + this->pData = (float *)AHB0.alloc(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_empty_queue(); + + 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; + + THEKERNEL->robot->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(); + + 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->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 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->setAdjustFunction(true); + + this->in_cal = false; + + return true; +} + + +void ZGridStrategy::normalize_grid() +{ + float min = 100.0F, // set large start value + norm_offset = 0; + + // find minimum value in offset grid + for (int i = 0; i < probe_points; i++) + if (this->pData[i] < min) + min = this->pData[i]; + + // creates addition offset to set minimum value to zero. + norm_offset = -min; + + // adds the offset to create a table of deltas, normalzed to minimum zero + for (int i = 0; i < probe_points; i++) + this->pData[i] += norm_offset; + + // add the offset to the current Z homing offset to preserve full probed offset. + this->setZoffset(getZhomeoffset() + norm_offset); +} + +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) +{ + char cmd[64]; + + // Assemble Gcode to add onto the queue. Also translate the position for non standard cartesian spaces (cal_offset) + snprintf(cmd, sizeof(cmd), "G0 X%1.3f Y%1.3f Z%1.3f F%1.1f", position[0] + this->cal_offset_x, position[1] + this->cal_offset_y, position[2], feed * 60); // use specified feedrate (mm/sec) + + //THEKERNEL->streams->printf("DEBUG: move: %s cent: %i\n", cmd, this->center_zero); + + Gcode gc(cmd, &(StreamOutput::NullStream)); + THEKERNEL->robot->on_gcode_received(&gc); // send to robot directly +} + + +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 + THEKERNEL->robot->compensationTransform= [this](float target[3]) { target[2] += this->getZOffset(target[0], target[1]); }; + }else{ + // clear it + THEKERNEL->robot->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..b20d0052 --- /dev/null +++ b/src/modules/tools/zprobe/ZGridStrategy.h @@ -0,0 +1,63 @@ +#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(); + + 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 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 index 3925cdf5..f243cda2 100644 --- a/src/modules/tools/zprobe/ZProbe.cpp +++ b/src/modules/tools/zprobe/ZProbe.cpp @@ -25,16 +25,19 @@ #include "EndstopsPublicAccess.h" #include "PublicData.h" #include "LevelingStrategy.h" +#include "StepTicker.h" // strategies we know about #include "DeltaCalibrationStrategy.h" #include "ThreePointStrategy.h" +#include "ZGridStrategy.h" #define enable_checksum CHECKSUM("enable") #define probe_pin_checksum CHECKSUM("probe_pin") #define debounce_count_checksum CHECKSUM("debounce_count") #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") @@ -66,7 +69,7 @@ void ZProbe::on_module_loaded() // register event-handlers register_for_event(ON_GCODE_RECEIVED); - THEKERNEL->slow_ticker->attach( THEKERNEL->stepper->get_acceleration_ticks_per_second() , this, &ZProbe::acceleration_tick ); + THEKERNEL->step_ticker->register_acceleration_tick_handler([this](){acceleration_tick(); }); } void ZProbe::on_config_reload(void *argument) @@ -93,6 +96,11 @@ void ZProbe::on_config_reload(void *argument) found= true; break; + case ZGrid_leveling_checksum: + this->strategies.push_back(new ZGridStrategy(this)); + found= true; + break; + // add other strategies here //case zheight_map_strategy: // this->strategies.push_back(new ZHeightMapStrategy(this)); @@ -118,6 +126,7 @@ void ZProbe::on_config_reload(void *argument) 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->max_z = THEKERNEL->config->value(gamma_max_checksum)->by_default(500)->as_number(); // maximum zprobe distance } @@ -126,6 +135,11 @@ bool ZProbe::wait_for_probe(int& steps) unsigned int debounce = 0; while(true) { THEKERNEL->call_event(ON_IDLE); + if(THEKERNEL->is_halted()){ + // aborted by kill + return false; + } + // if no stepper is moving, moves are finished and there was no touch if( !STEPPER[Z_AXIS]->is_moving() && (!is_delta || (!STEPPER[Y_AXIS]->is_moving() && !STEPPER[Z_AXIS]->is_moving())) ) { return false; @@ -159,58 +173,84 @@ bool ZProbe::wait_for_probe(int& steps) } } -// single probe and report amount moved -bool ZProbe::run_probe(int& steps, bool fast) +// single probe with custom feedrate +// returns boolean value indicating if probe was triggered +bool ZProbe::run_probe_feed(int& steps, float feedrate) { + // not a block move so disable the last tick setting + for ( int c = X_AXIS; c <= Z_AXIS; c++ ) { + STEPPER[c]->set_moved_last_block(false); + } + // Enable the motors THEKERNEL->stepper->turn_enable_pins_on(); - this->current_feedrate = (fast ? this->fast_feedrate : this->slow_feedrate) * Z_STEPS_PER_MM; // steps/sec + this->current_feedrate = feedrate * Z_STEPS_PER_MM; // steps/sec float maxz= this->max_z*2; // move Z down - STEPPER[Z_AXIS]->set_speed(0); // will be increased by acceleration tick - STEPPER[Z_AXIS]->move(true, maxz * Z_STEPS_PER_MM); // always probes down, no more than 2*maxz + STEPPER[Z_AXIS]->move(true, maxz * Z_STEPS_PER_MM, 0); // always probes down, no more than 2*maxz if(this->is_delta) { // for delta need to move all three actuators - STEPPER[X_AXIS]->set_speed(0); - STEPPER[X_AXIS]->move(true, maxz * STEPS_PER_MM(X_AXIS)); - STEPPER[Y_AXIS]->set_speed(0); - STEPPER[Y_AXIS]->move(true, maxz * STEPS_PER_MM(Y_AXIS)); + STEPPER[X_AXIS]->move(true, maxz * STEPS_PER_MM(X_AXIS), 0); + STEPPER[Y_AXIS]->move(true, maxz * STEPS_PER_MM(Y_AXIS), 0); } - // start acceration hrprocessing + // start acceleration processing this->running = true; bool r = wait_for_probe(steps); this->running = false; + STEPPER[X_AXIS]->move(0, 0); + STEPPER[Y_AXIS]->move(0, 0); + STEPPER[Z_AXIS]->move(0, 0); return r; } +// single probe with either fast or slow feedrate +// returns boolean value indicating if probe was triggered +bool ZProbe::run_probe(int& steps, bool fast) +{ + float feedrate = (fast ? this->fast_feedrate : this->slow_feedrate); + return run_probe_feed(steps, feedrate); + +} + bool ZProbe::return_probe(int steps) { // move probe back to where it was - float 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 + + 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 + } + this->current_feedrate = fr * Z_STEPS_PER_MM; // feedrate in steps/sec bool dir= steps < 0; steps= abs(steps); - STEPPER[Z_AXIS]->set_speed(0); // will be increased by acceleration tick - STEPPER[Z_AXIS]->move(dir, steps); + STEPPER[Z_AXIS]->move(dir, steps, 0); if(this->is_delta) { - STEPPER[X_AXIS]->set_speed(0); - STEPPER[X_AXIS]->move(dir, steps); - STEPPER[Y_AXIS]->set_speed(0); - STEPPER[Y_AXIS]->move(dir, steps); + STEPPER[X_AXIS]->move(dir, steps, 0); + STEPPER[Y_AXIS]->move(dir, steps, 0); } this->running = true; while(STEPPER[Z_AXIS]->is_moving() || (is_delta && (STEPPER[X_AXIS]->is_moving() || STEPPER[Y_AXIS]->is_moving())) ) { // wait for it to complete THEKERNEL->call_event(ON_IDLE); + if(THEKERNEL->is_halted()){ + // aborted by kill + break; + } } this->running = false; + STEPPER[X_AXIS]->move(0, 0); + STEPPER[Y_AXIS]->move(0, 0); + STEPPER[Z_AXIS]->move(0, 0); return true; } @@ -252,12 +292,18 @@ void ZProbe::on_gcode_received(void *argument) } if( gcode->g == 30 ) { // simple Z probe - gcode->mark_as_taken(); // first wait for an empty queue i.e. no moves left THEKERNEL->conveyor->wait_for_empty_queue(); int steps; - if(run_probe(steps)) { + bool probe_result; + if(gcode->has_letter('F')) { + probe_result = run_probe_feed(steps, gcode->get_value('F') / 60); + } else { + probe_result = run_probe(steps); + } + + if(probe_result) { gcode->stream->printf("Z:%1.4f C:%d\n", steps / Z_STEPS_PER_MM, steps); // move back to where it started, unless a Z is specified if(gcode->has_letter('Z')) { @@ -271,40 +317,70 @@ void ZProbe::on_gcode_received(void *argument) } } else { - // find a strategy to handle the gcode - for(auto s : strategies){ - if(s->handleGcode(gcode)) { - gcode->mark_as_taken(); + if(gcode->subcode == 0) { + // 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{ + // subcode selects which strategy to send the code to + // they are loaded in the order they are defined in config, 1 being the first, 2 being the second and so on. + int i= gcode->subcode-1; + if(gcode->subcode < strategies.size()) { + if(!strategies[i]->handleGcode(gcode)){ + gcode->stream->printf("strategy #%d did not handle G%d\n", i+1, gcode->g); + } return; + + }else{ + gcode->stream->printf("strategy #%d is not loaded\n", i+1); } } - gcode->stream->printf("No strategy found to handle G%d\n", gcode->g); } } else if(gcode->has_m) { // M code processing here - if(gcode->m == 119) { - int c = this->pin.get(); - gcode->stream->printf(" Probe: %d", c); - gcode->add_nl = true; - gcode->mark_as_taken(); - - }else { - for(auto s : strategies){ - if(s->handleGcode(gcode)) { - gcode->mark_as_taken(); - return; + 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'); + 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; + } } - } } } } -#define max(a,b) (((a) > (b)) ? (a) : (b)) // Called periodically to change the speed to match acceleration -uint32_t ZProbe::acceleration_tick(uint32_t dummy) +void ZProbe::acceleration_tick(void) { - if(!this->running) return(0); // nothing to do + if(!this->running) return; // nothing to do if(STEPPER[Z_AXIS]->is_moving()) accelerate(Z_AXIS); if(is_delta) { @@ -315,17 +391,17 @@ uint32_t ZProbe::acceleration_tick(uint32_t dummy) } } - return 0; + return; } void ZProbe::accelerate(int c) { uint32_t current_rate = STEPPER[c]->get_steps_per_second(); - uint32_t target_rate = int(floor(this->current_feedrate)); + uint32_t target_rate = floorf(this->current_feedrate); // Z may have a different acceleration to X and Y float acc= (c==Z_AXIS) ? THEKERNEL->planner->get_z_acceleration() : THEKERNEL->planner->get_acceleration(); if( current_rate < target_rate ) { - uint32_t rate_increase = int(floor((acc / THEKERNEL->stepper->get_acceleration_ticks_per_second()) * STEPS_PER_MM(c))); + uint32_t rate_increase = floorf((acc / THEKERNEL->acceleration_ticks_per_second) * STEPS_PER_MM(c)); current_rate = min( target_rate, current_rate + rate_increase ); } if( current_rate > target_rate ) { @@ -333,7 +409,7 @@ void ZProbe::accelerate(int c) } // steps per second - STEPPER[c]->set_speed(max(current_rate, THEKERNEL->stepper->get_minimum_steps_per_second())); + STEPPER[c]->set_speed(current_rate); } // issue a coordinated move directly to robot, and return when done diff --git a/src/modules/tools/zprobe/ZProbe.h b/src/modules/tools/zprobe/ZProbe.h index 4d966f43..f900fec2 100644 --- a/src/modules/tools/zprobe/ZProbe.h +++ b/src/modules/tools/zprobe/ZProbe.h @@ -29,10 +29,11 @@ public: void on_module_loaded(); void on_config_reload(void *argument); void on_gcode_received(void *argument); - uint32_t acceleration_tick(uint32_t dummy); + void acceleration_tick(void); bool wait_for_probe(int& steps); bool run_probe(int& steps, bool fast= false); + bool run_probe_feed(int& steps, float feedrate); bool return_probe(int steps); bool doProbeAt(int &steps, float x, float y); float probeDistance(float x, float y); @@ -41,9 +42,10 @@ public: void home(); bool getProbeStatus() { return this->pin.get(); } - float getSlowFeedrate() { return slow_feedrate; } - float getFastFeedrate() { return fast_feedrate; } - float getProbeHeight() { return probe_height; } + float getSlowFeedrate() const { return slow_feedrate; } + float getFastFeedrate() const { return fast_feedrate; } + float getProbeHeight() const { return probe_height; } + float getMaxZ() const { return max_z; } float zsteps_to_mm(float steps); private: @@ -52,6 +54,7 @@ private: volatile float current_feedrate; float slow_feedrate; float fast_feedrate; + float return_feedrate; float probe_height; float max_z; volatile struct { diff --git a/src/modules/utils/PlayLed/PlayLed.cpp b/src/modules/utils/PlayLed/PlayLed.cpp index 5a41de53..af06db91 100644 --- a/src/modules/utils/PlayLed/PlayLed.cpp +++ b/src/modules/utils/PlayLed/PlayLed.cpp @@ -3,16 +3,13 @@ /* * LED indicator: * off = not paused, nothing to do - * slow flash = paused * fast flash = halted * on = a block is being executed */ -#include "PauseButton.h" #include "modules/robot/Conveyor.h" #include "SlowTicker.h" #include "Config.h" -#include "Pauser.h" #include "checksumm.h" #include "ConfigValue.h" #include "Gcode.h" @@ -22,8 +19,6 @@ #define play_led_disable_checksum CHECKSUM("play_led_disable") PlayLed::PlayLed() { - - halted= false; cnt= 0; } @@ -35,7 +30,7 @@ void PlayLed::on_module_loaded() } on_config_reload(this); - this->register_for_event(ON_HALT); + THEKERNEL->slow_ticker->attach(12, this, &PlayLed::led_tick); } @@ -51,25 +46,15 @@ void PlayLed::on_config_reload(void *argument) uint32_t PlayLed::led_tick(uint32_t) { - if(this->halted) { + if(THEKERNEL->is_halted()) { led.set(!led.get()); return 0; } if(++cnt >= 6) { // 6 ticks ~ 500ms cnt= 0; - - if (THEKERNEL->pauser->paused()) { - led.set(!led.get()); - } else { - led.set(!THEKERNEL->conveyor->is_queue_empty()); - } + led.set(!THEKERNEL->conveyor->is_queue_empty()); } return 0; } - -void PlayLed::on_halt(void *arg) -{ - this->halted= (arg == nullptr); -} diff --git a/src/modules/utils/PlayLed/PlayLed.h b/src/modules/utils/PlayLed/PlayLed.h index 5d1da019..d139d8c9 100644 --- a/src/modules/utils/PlayLed/PlayLed.h +++ b/src/modules/utils/PlayLed/PlayLed.h @@ -12,14 +12,12 @@ public: void on_module_loaded(void); void on_config_reload(void *); - void on_halt(void *arg); private: uint32_t led_tick(uint32_t); Pin led; struct { uint8_t cnt:4; - bool halted:1; }; }; diff --git a/src/modules/utils/currentcontrol/CurrentControl.cpp b/src/modules/utils/currentcontrol/CurrentControl.cpp index 0eb79520..2e30f3b0 100644 --- a/src/modules/utils/currentcontrol/CurrentControl.cpp +++ b/src/modules/utils/currentcontrol/CurrentControl.cpp @@ -91,11 +91,18 @@ void CurrentControl::on_gcode_received(void *argument) } } else if(gcode->m == 500 || gcode->m == 503) { - gcode->stream->printf(";Motor currents:\nM907 "); + float currents[8]; + bool has_setting= false; for (int i = 0; i < 8; i++) { - float c = this->digipot->get_current(i); - if(c >= 0) - gcode->stream->printf("%c%1.5f ", alpha[i], c); + 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/ad5206.h b/src/modules/utils/currentcontrol/ad5206.h index ec9cd300..aca5da2b 100644 --- a/src/modules/utils/currentcontrol/ad5206.h +++ b/src/modules/utils/currentcontrol/ad5206.h @@ -8,8 +8,6 @@ #include #include -#define max(a,b) (((a) > (b)) ? (a) : (b)) - class AD5206 : public DigipotBase { public: AD5206(){ @@ -26,7 +24,7 @@ class AD5206 : public DigipotBase { currents[channel]= -1; return; } - current = min( max( current, 0.0L ), 2.0L ); + current = min( max( current, 0.0F ), 2.0F ); char adresses[6] = { 0x05, 0x03, 0x01, 0x00, 0x02, 0x04 }; currents[channel] = current; cs.set(0); diff --git a/src/modules/utils/currentcontrol/mcp4451.h b/src/modules/utils/currentcontrol/mcp4451.h index c9471a7b..499b9885 100644 --- a/src/modules/utils/currentcontrol/mcp4451.h +++ b/src/modules/utils/currentcontrol/mcp4451.h @@ -60,7 +60,7 @@ class MCP4451 : public DigipotBase { } char current_to_wiper( float current ){ - return char(ceil(float((this->factor*current)))); + return char(ceilf(float((this->factor*current)))); } mbed::I2C* i2c; 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..2ed27a61 --- /dev/null +++ b/src/modules/utils/motordrivercontrol/MotorDriverControl.cpp @@ -0,0 +1,443 @@ +#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 "Gcode.h" +#include "Config.h" +#include "checksumm.h" + +#include "mbed.h" // for SPI + +#include "drivers/TMC26X/TMC26X.h" +#include "drivers/DRV8711/drv8711.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 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; +} + +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) +{ + 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 ERROR: chip select not defined\n"); + return false; // if not defined then we can't use this instance + } + spi_cs_pin.set(1); + + 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]; + + str= THEKERNEL->config->value( motor_driver_control_checksum, cs, chip_checksum)->by_default("")->as_string(); + if(str.empty()) { + THEKERNEL->streams->printf("MotorDriverControl ERROR: chip type not defined\n"); + 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)); + + }else if(str == "TMC2660") { + chip= TMC2660; + tmc26x= new TMC26X(std::bind( &MotorDriverControl::sendSPI, this, _1, _2, _3)); + + }else{ + THEKERNEL->streams->printf("MotorDriverControl ERROR: Unknown chip type: %s\n", 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 = P0_18; miso = P0_17; sclk = P0_15; + } else if(spi_channel == 1) { + mosi = P0_9; miso = P0_8; sclk = P0_7; + } else { + THEKERNEL->streams->printf("MotorDriverControl ERROR: Unknown SPI Channel: %d\n", 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 overidden in config + switch(chip) { + case DRV8711: max_current= 4000; break; + case TMC2660: max_current= 4000; break; + } + + max_current= THEKERNEL->config->value(motor_driver_control_checksum, cs, max_current_checksum )->by_default((int)max_current)->as_number(); // in mA + //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(1000)->as_number(); // in 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(); + + // 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); + uint32_t reg= 0; + for(auto i : regs) { + switch(chip) { + case DRV8711: drv8711->set_raw_register(&StreamOutput::NullStream, ++reg, i); break; + case TMC2660: tmc26x->setRawRegister(&StreamOutput::NullStream, ++reg, i); 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); + } + + THEKERNEL->streams->printf("MotorDriverControl INFO: configured motor %c (%d): as %s, cs: %04X\n", designator, id, chip==TMC2660?"TMC2660":chip==DRV8711?"DRV8711":"UNKNOWN", (spi_cs_pin.port_number<<8)|spi_cs_pin.pin); + + 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; + } + + 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 + + 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() +{ + // send initialization sequence to chips + if(chip == DRV8711) { + drv8711->init(); + set_current(current); + set_microstep(microsteps); + + }else if(chip == TMC2660){ + tmc26x->init(); + 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; + } +} + +// 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; + } + 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; + } +} + +void MotorDriverControl::enable(bool on) +{ + switch(chip) { + case DRV8711: + drv8711->set_enable(on); + break; + + case TMC2660: + tmc26x->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; + } +} + +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; + } + 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; + } +} + +// 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..b403334d --- /dev/null +++ b/src/modules/utils/motordrivercontrol/MotorDriverControl.h @@ -0,0 +1,75 @@ +#pragma once + +#include "Module.h" +#include "Pin.h" + +#include + +namespace mbed { + class SPI; +} + +class DRV8711DRV; +class TMC26X; +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(); + 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 + }; + CHIP_TYPE chip; + + // one of these drivers + union { + DRV8711DRV *drv8711; + TMC26X *tmc26x; + }; + + //float current_factor; + uint32_t max_current; // in milliamps + uint32_t current; // in milliamps + 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..4b2fbfb9 --- /dev/null +++ b/src/modules/utils/motordrivercontrol/drivers/DRV8711/drv8711.cpp @@ -0,0 +1,449 @@ +#include "drv8711.h" + +#include "Kernel.h" +#include "StreamOutput.h" +#include "StreamOutputPool.h" + +#define REGWRITE 0x00 +#define REGREAD 0x80 + +DRV8711DRV::DRV8711DRV(std::function spi) : spi(spi) +{ + error_reported.reset(); +} + +void DRV8711DRV::init (uint16_t gain) +{ + this->gain= gain; + + // 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("Register Dump:\n"); + + // 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("ERROR: Overtemperature shutdown\n"); + error= true; + error_reported.set(0); + }else{ + error_reported.reset(0); + } + + + if(R_STATUS_REG.AOCP) { + if(!error_reported.test(1)) THEKERNEL->streams->printf("ERROR: Channel A over current shutdown\n"); + error= true; + error_reported.set(1); + }else{ + error_reported.reset(1); + } + + + if(R_STATUS_REG.BOCP) { + if(!error_reported.test(2)) THEKERNEL->streams->printf("ERROR: Channel B over current shutdown\n"); + error= true; + error_reported.set(2); + }else{ + error_reported.reset(2); + } + + if(R_STATUS_REG.APDF) { + if(!error_reported.test(3)) THEKERNEL->streams->printf("ERROR: Channel A predriver fault\n"); + error= true; + error_reported.set(3); + }else{ + error_reported.reset(3); + } + + + if(R_STATUS_REG.BPDF) { + if(!error_reported.test(4)) THEKERNEL->streams->printf("ERROR: Channel B predriver fault\n"); + 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..9d956433 --- /dev/null +++ b/src/modules/utils/motordrivercontrol/drivers/DRV8711/drv8711.h @@ -0,0 +1,149 @@ +#pragma once + +#include +#include + +class StreamOutput; + +class DRV8711DRV +{ +public: + DRV8711DRV(std::function spi); + + void init(uint16_t gain = 20) ; + + 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}; + + + // float _amps; + // uint8_t _microstepreg; +}; 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..a20bd6db --- /dev/null +++ b/src/modules/utils/motordrivercontrol/drivers/TMC26X/TMC26X.cpp @@ -0,0 +1,1134 @@ +/* + 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" + +//! 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 TCM260 +#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) : spi(spi) +{ + //we are not started yet + started = false; + //by default cool step is not enabled + cool_step_enabled = false; + error_reported.reset(); +} + +void TMC26X::setResistor(unsigned int resistor) +{ + //store the current sense resistor value for later use + this->resistor = resistor; +} + +/* + * configure the stepper driver + * just must be called. + */ +void TMC26X::init() +{ + //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); +} + +int TMC26X::getResistor() +{ + return this->resistor; +} + +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("Chip type TMC26X\n"); + + 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_stepped(), getCoolstepCurrent()); + } else { + readStatus(TMC26X_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(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("WARNING: Overtemperature Prewarning!\n"); + error_reported.set(0); + }else{ + error_reported.reset(0); + } + + if (this->getOverTemperature()&TMC26X_OVERTEMPERATURE_SHUTDOWN) { + if(!error_reported.test(1)) stream->printf("ERROR: Overtemperature Shutdown!\n"); + error=true; + error_reported.set(1); + }else{ + error_reported.reset(1); + } + + if (this->isShortToGroundA()) { + if(!error_reported.test(2)) stream->printf("ERROR: SHORT to ground on channel A!\n"); + error=true; + error_reported.set(2); + }else{ + error_reported.reset(2); + } + + if (this->isShortToGroundB()) { + if(!error_reported.test(3)) stream->printf("ERROR: SHORT to ground on channel B!\n"); + 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("ERROR: Channel A seems to be unconnected!\n"); + error=true; + error_reported.set(4); + }else{ + error_reported.reset(4); + } + + if (this->isOpenLoadB()) { + if(!error_reported.test(5)) stream->printf("ERROR: Channel B seems to be unconnected!\n"); + 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 0 writes them, 255 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..515b1ab0 --- /dev/null +++ b/src/modules/utils/motordrivercontrol/drivers/TMC26X/TMC26X.h @@ -0,0 +1,456 @@ +/* + 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; + +/*! + * \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 TCM262Stepper.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); + + /*! + * \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(); + + /*! + * \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_262_READOUT_STALLGUARD, or TMC_262_READOUT_CURRENT + * \sa TMC26X_READOUT_POSITION, TMC_262_READOUT_STALLGUARD, TMC_262_READOUT_CURRENT + */ + void readStatus(int8_t read_value); + + /*! + * \brief Returns the current sense resistor value in milliohm. + * The default value of ,15 Ohm will return 150. + */ + int getResistor(); + void setResistor(unsigned int resistor); + + /*! + * \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 beeded 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 +}; + diff --git a/src/modules/utils/panel/Panel.cpp b/src/modules/utils/panel/Panel.cpp index 1a6848f9..a2fa2e9c 100644 --- a/src/modules/utils/panel/Panel.cpp +++ b/src/modules/utils/panel/Panel.cpp @@ -20,7 +20,6 @@ #include "screens/MainMenuScreen.h" #include "SlowTicker.h" #include "Gcode.h" -#include "Pauser.h" #include "TemperatureControlPublicAccess.h" #include "ModifyValuesScreen.h" #include "PublicDataRequest.h" @@ -36,6 +35,7 @@ #include "checksumm.h" #include "ConfigValue.h" #include "Config.h" +#include "TemperatureControlPool.h" // for parse_pins in mbed #include "pinmap.h" @@ -63,6 +63,7 @@ #define hotend_temp_checksum CHECKSUM("hotend_temperature") #define bed_temp_checksum CHECKSUM("bed_temperature") +#define panel_display_message_checksum CHECKSUM("display_message") Panel* Panel::instance= nullptr; @@ -82,7 +83,6 @@ Panel::Panel() this->sd= nullptr; this->extmounter= nullptr; this->external_sd_enable= false; - this->halted= false; strcpy(this->playing_file, "Playing file"); } @@ -136,7 +136,6 @@ void Panel::on_module_loaded() // these need to be called here as they need the config cache loaded as they enumerate modules this->custom_screen= new CustomScreen(); - setup_temperature_screen(); // some panels may need access to this global info this->lcd->setPanel(this); @@ -165,7 +164,6 @@ void Panel::on_module_loaded() this->down_button.up_attach( this, &Panel::on_down ); this->click_button.up_attach( this, &Panel::on_select ); this->back_button.up_attach( this, &Panel::on_back ); - this->pause_button.up_attach( this, &Panel::on_pause ); //setting longpress_delay @@ -189,8 +187,7 @@ void Panel::on_module_loaded() // Register for events this->register_for_event(ON_IDLE); this->register_for_event(ON_MAIN_LOOP); - this->register_for_event(ON_GCODE_RECEIVED); - this->register_for_event(ON_HALT); + this->register_for_event(ON_SET_PUBLIC_DATA); // Refresh timer THEKERNEL->slow_ticker->attach( 20, this, &Panel::refresh_tick ); @@ -261,15 +258,19 @@ uint32_t Panel::encoder_tick(uint32_t dummy) return 0; } -void Panel::on_gcode_received(void *argument) +void Panel::on_set_public_data(void *argument) { - Gcode *gcode = static_cast(argument); - if ( gcode->has_m) { - if ( gcode->m == 117 ) { // set LCD message - this->message = get_arguments(gcode->get_command()); - if (this->message.size() > 20) this->message = this->message.substr(0, 20); - gcode->mark_as_taken(); - } + PublicDataRequest *pdr = static_cast(argument); + + if(!pdr->starts_with(panel_checksum)) return; + + if(!pdr->second_element_is(panel_display_message_checksum)) return; + + string *s = static_cast(pdr->get_data_ptr()); + if (s->size() > 20) { + this->message = s->substr(0, 20); + } else { + this->message= *s; } } @@ -369,7 +370,6 @@ void Panel::on_idle(void *argument) this->down_button.check_signal(but & BUTTON_DOWN); this->back_button.check_signal(but & BUTTON_LEFT); this->click_button.check_signal(but & BUTTON_SELECT); - this->pause_button.check_signal(but & BUTTON_PAUSE); } // If we are in menu mode and the position has changed @@ -430,16 +430,6 @@ uint32_t Panel::on_select(uint32_t dummy) return 0; } -uint32_t Panel::on_pause(uint32_t dummy) -{ - if (!THEKERNEL->pauser->paused()) { - THEKERNEL->pauser->take(); - } else { - THEKERNEL->pauser->release(); - } - return 0; -} - bool Panel::counter_change() { if ( this->counter_changed ) { @@ -594,69 +584,25 @@ bool Panel::is_playing() const return false; } -void Panel::set_playing_file(string f) -{ - // just copy the first 20 characters after the first / if there - size_t n = f.find_last_of('/'); - if (n == string::npos) n = 0; - strncpy(playing_file, f.substr(n + 1, 19).c_str(), sizeof(playing_file)); - playing_file[sizeof(playing_file) - 1] = 0; -} - -static float getTargetTemperature(uint16_t heater_cs) +bool Panel::is_suspended() const { void *returned_data; - bool ok = PublicData::get_value( temperature_control_checksum, heater_cs, current_temperature_checksum, &returned_data ); + bool ok = PublicData::get_value( player_checksum, is_suspended_checksum, &returned_data ); if (ok) { - struct pad_temperature temp = *static_cast(returned_data); - return temp.target_temperature; + bool b = *static_cast(returned_data); + return b; } - - return 0.0F; + return false; } -void Panel::setup_temperature_screen() +void Panel::set_playing_file(string f) { - // setup temperature screen - auto mvs= new ModifyValuesScreen(false); // does not delete itself on exit - this->temperature_screen= mvs; - - // enumerate heaters and add a menu item for each one - vector modules; - THEKERNEL->config->get_module_list( &modules, temperature_control_checksum ); - - int cnt= 0; - for(auto i : modules) { - if (!THEKERNEL->config->value(temperature_control_checksum, i, enable_checksum )->as_bool()) continue; - void *returned_data; - bool ok = PublicData::get_value( temperature_control_checksum, i, current_temperature_checksum, &returned_data ); - if (!ok) continue; - - this->temperature_modules.push_back(i); - struct pad_temperature t = *static_cast(returned_data); - - // rename if two of the known types - const char *name; - if(t.designator == "T") name= "Hotend"; - else if(t.designator == "B") name= "Bed"; - else name= t.designator.c_str(); - - mvs->addMenuItem(name, // menu name - [i]() -> float { return getTargetTemperature(i); }, // getter - [i](float t) { PublicData::set_value( temperature_control_checksum, i, &t ); }, // setter - 1.0F, // increment - 0.0F, // Min - 500.0F // Max - ); - cnt++; - } - - if(cnt== 0) { - // no heaters and probably no extruders either - delete mvs; - this->temperature_screen= nullptr; - } + // just copy the first 20 characters after the first / if there + size_t n = f.find_last_of('/'); + if (n == string::npos) n = 0; + strncpy(playing_file, f.substr(n + 1, 19).c_str(), sizeof(playing_file)); + playing_file[sizeof(playing_file) - 1] = 0; } bool Panel::mount_external_sd(bool on) @@ -715,8 +661,3 @@ void Panel::on_second_tick(void *arg) // TODO for panels with no sd card detect we need to poll to see if card is inserted - or not } } - -void Panel::on_halt(void *arg) -{ - halted= (arg == nullptr); -} diff --git a/src/modules/utils/panel/Panel.h b/src/modules/utils/panel/Panel.h index 4497ed60..b47fb737 100644 --- a/src/modules/utils/panel/Panel.h +++ b/src/modules/utils/panel/Panel.h @@ -35,9 +35,8 @@ class Panel : public Module { uint32_t button_tick(uint32_t dummy); uint32_t encoder_tick(uint32_t dummy); void on_idle(void* argument); - void on_halt(void* argument); void on_main_loop(void* argument); - void on_gcode_received(void* argument); + void on_set_public_data(void* argument); void on_second_tick(void* argument); void enter_screen(PanelScreen* screen); void reset_counter(); @@ -47,7 +46,6 @@ class Panel : public Module { uint32_t on_down(uint32_t dummy); uint32_t on_back(uint32_t dummy); uint32_t on_select(uint32_t dummy); - uint32_t on_pause(uint32_t dummy); uint32_t refresh_tick(uint32_t dummy); uint32_t encoder_check(uint32_t dummy); bool counter_change(); @@ -75,12 +73,12 @@ class Panel : public Module { // file playing from sd bool is_playing() const; + bool is_suspended() const; void set_playing_file(string f); const char* get_playing_file() { return playing_file; } string getMessage() { return message; } bool hasMessage() { return message.size() > 0; } - bool is_halted() const { return halted; } uint16_t get_screen_lines() const { return screen_lines; } @@ -88,14 +86,11 @@ class Panel : public Module { // TODO pass lcd into ctor of each sub screen LcdBase* lcd; PanelScreen* custom_screen; - PanelScreen* temperature_screen; - vector temperature_modules; // as panelscreen accesses private fields in Panel friend class PanelScreen; private: - void setup_temperature_screen(); // external SD card bool mount_external_sd(bool on); @@ -119,7 +114,6 @@ class Panel : public Module { Button down_button; Button back_button; Button click_button; - Button pause_button; int* counter; @@ -144,7 +138,6 @@ class Panel : public Module { bool menu_changed:1; bool control_value_changed:1; bool external_sd_enable:1; - bool halted:1; volatile bool counter_changed:1; volatile bool click_changed:1; volatile bool refresh_flag:1; diff --git a/src/modules/utils/panel/PanelScreen.cpp b/src/modules/utils/panel/PanelScreen.cpp index 6fa01670..f841e5cd 100644 --- a/src/modules/utils/panel/PanelScreen.cpp +++ b/src/modules/utils/panel/PanelScreen.cpp @@ -15,16 +15,15 @@ #include "LcdBase.h" #include "libs/StreamOutput.h" -#include -#include - using namespace std; +// static as it is shared by all screens +std::deque PanelScreen::command_queue; + PanelScreen::PanelScreen() {} PanelScreen::~PanelScreen() {} void PanelScreen::on_refresh() {} -void PanelScreen::on_main_loop() {} void PanelScreen::on_enter() {} @@ -69,26 +68,29 @@ void PanelScreen::send_gcode(const char *gm_code, char parameter, float value) send_gcode(g); } -// Helper to send commands, must be called from mainloop -// may contain multipe commands separated by \n +// Helper to send commands, may be called from on_idle will delegate all commands to on_main_loop +// may contain multiple commands separated by \n void PanelScreen::send_command(const char *gcstr) { string cmd(gcstr); - vector q; while (cmd.size() > 0) { size_t b = cmd.find_first_of("\n"); if ( b == string::npos ) { - q.push_back(cmd); + command_queue.push_back(cmd); break; } - q.push_back(cmd.substr( 0, b )); + command_queue.push_back(cmd.substr( 0, b )); cmd = cmd.substr(b + 1); } +} - // for each command send it - for (std::vector::iterator i = q.begin(); i != q.end(); ++i) { +void PanelScreen::on_main_loop() +{ + // for each command in queue send it + while(command_queue.size() > 0) { struct SerialMessage message; - message.message = *i; + message.message = command_queue.front(); + command_queue.pop_front(); message.stream = &(StreamOutput::NullStream); THEKERNEL->call_event(ON_CONSOLE_LINE_RECEIVED, &message ); } diff --git a/src/modules/utils/panel/PanelScreen.h b/src/modules/utils/panel/PanelScreen.h index 8558a39c..088c3c7a 100644 --- a/src/modules/utils/panel/PanelScreen.h +++ b/src/modules/utils/panel/PanelScreen.h @@ -9,6 +9,7 @@ #define PANELSCREEN_H #include +#include class Panel; @@ -36,7 +37,7 @@ protected: void send_gcode(std::string g); void send_gcode(const char *gm_code, char parameter, float value); void send_command(const char *gcstr); - + static std::deque command_queue; PanelScreen *parent; }; diff --git a/src/modules/utils/panel/panels/ST7565/AK4183.cpp b/src/modules/utils/panel/panels/ST7565/AK4183.cpp deleted file mode 100644 index 6b8c28de..00000000 --- a/src/modules/utils/panel/panels/ST7565/AK4183.cpp +++ /dev/null @@ -1,40 +0,0 @@ -/* - * AK4183.cpp - * - * Created on: 21-06-2013 - * Author: Wulfnor - */ - -#include "AK4183.h" - -AK4183::AK4183() { - this->i2c_address = TS_ADDRESS; - this->i2c = new mbed::I2C(p9, p10); // P0_0, P0_1 - - i2c->frequency(40000); - - penIRQ.from_string("3.25")->as_input(); -} - -AK4183::~AK4183() { - delete this->i2c; -} - -unsigned char AK4183::read_adc(unsigned char cmd){ - char data[2]; - data[0] = cmd; - i2c->write(this->i2c_address, data, 1); - i2c->read(this->i2c_address, data, 1); // Read from selected Register - return (~data[0]); -} - -bool AK4183::pen_touching(){ - return !penIRQ.get(); -} -int AK4183::get_x(){ - return read_adc(0xC2); -} - -int AK4183::get_y(){ - return read_adc(0xD2); -} diff --git a/src/modules/utils/panel/panels/ST7565/AK4183.h b/src/modules/utils/panel/panels/ST7565/AK4183.h deleted file mode 100644 index d967c9af..00000000 --- a/src/modules/utils/panel/panels/ST7565/AK4183.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * AK4183.h - * - * Created on: 21-06-2013 - * Author: Wulfnor - */ - -#ifndef AK4183_H_ -#define AK4183_H_ - -#include "libs/Kernel.h" -#include "mbed.h" -#include "libs/Pin.h" - -#define TS_ADDRESS 0x90 - -class AK4183 { -public: - AK4183(); - virtual ~AK4183(); - int get_x(); - int get_y(); - bool pen_touching(); -private: - Pin pen; - mbed::I2C* i2c; - char i2c_address; - Pin penIRQ; - unsigned char read_adc(unsigned char cmd); - int read_x(); - int read_y(); -}; - -#endif /* AK4183_H_ */ diff --git a/src/modules/utils/panel/screens/ControlScreen.cpp b/src/modules/utils/panel/screens/ControlScreen.cpp index 9a870d21..53ab8adc 100644 --- a/src/modules/utils/panel/screens/ControlScreen.cpp +++ b/src/modules/utils/panel/screens/ControlScreen.cpp @@ -14,7 +14,7 @@ #include "libs/nuts_bolts.h" #include "libs/utils.h" #include -#include "modules/robot/RobotPublicAccess.h" +#include "Robot.h" #include "PublicData.h" #include "checksumm.h" #include "LcdBase.h" @@ -125,15 +125,7 @@ void ControlScreen::enter_menu_control() void ControlScreen::get_current_pos(float *cp) { - void *returned_data; - - bool ok = PublicData::get_value( robot_checksum, current_position_checksum, &returned_data ); - if (ok) { - float *p = static_cast(returned_data); - cp[0] = p[0]; - cp[1] = p[1]; - cp[2] = p[2]; - } + THEKERNEL->robot->get_axis_position(cp); } void ControlScreen::set_current_pos(char axis, float p) diff --git a/src/modules/utils/panel/screens/CustomScreen.cpp b/src/modules/utils/panel/screens/CustomScreen.cpp index 28c3788c..08bb0f7e 100644 --- a/src/modules/utils/panel/screens/CustomScreen.cpp +++ b/src/modules/utils/panel/screens/CustomScreen.cpp @@ -25,8 +25,6 @@ using namespace std; CustomScreen::CustomScreen() { - this->command = nullptr; - //printf("Setting up CustomScreen\n"); vector modules; THEKERNEL->config->get_module_list( &modules, custom_menu_checksum ); @@ -81,15 +79,6 @@ void CustomScreen::clicked_menu_entry(uint16_t line) if (line == 0) { THEPANEL->enter_screen(this->parent); } else { - command = std::get<1>(menu_items[line-1]); + send_command(std::get<1>(menu_items[line-1])); // will be done in main loop } } - -// queuing commands needs to be done from main loop -void CustomScreen::on_main_loop() -{ - // issue command - if (this->command == nullptr) return; - send_command(this->command); - this->command = nullptr; -} diff --git a/src/modules/utils/panel/screens/CustomScreen.h b/src/modules/utils/panel/screens/CustomScreen.h index 7251bc24..161c4a9c 100644 --- a/src/modules/utils/panel/screens/CustomScreen.h +++ b/src/modules/utils/panel/screens/CustomScreen.h @@ -20,13 +20,11 @@ public: void on_refresh(); void on_enter(); - void on_main_loop(); void display_menu_line(uint16_t line); void clicked_menu_entry(uint16_t line); int idle_timeout_secs() { return 60; } private: - const char *command; std::vector > menu_items; }; diff --git a/src/modules/utils/panel/screens/ExtruderScreen.cpp b/src/modules/utils/panel/screens/ExtruderScreen.cpp index c4f9b8f0..5b5cf58a 100644 --- a/src/modules/utils/panel/screens/ExtruderScreen.cpp +++ b/src/modules/utils/panel/screens/ExtruderScreen.cpp @@ -25,7 +25,6 @@ using namespace std; ExtruderScreen::ExtruderScreen() { - this->command= nullptr; } void ExtruderScreen::on_enter() @@ -59,20 +58,12 @@ void ExtruderScreen::clicked_menu_entry(uint16_t line) { switch ( line ) { case 0: THEPANEL->enter_screen(this->parent); return; - case 1: command = "G91\nG1 E5 F100\nG90"; break; - case 2: command = "G91\nG1 E-5 F100\nG90"; break; + case 1: send_command("M120\nG91\nG1 E5 F100\nM121"); break; + case 2: send_command("M120\nG91\nG1 E-5 F100\nM121"); break; case 3: setupConfigSettings(); break; // lazy load } } -// queuing commands needs to be done from main loop -void ExtruderScreen::on_main_loop() -{ - if (this->command == nullptr) return; - send_command(this->command); - this->command= nullptr; -} - void ExtruderScreen::setupConfigSettings() { auto mvs= new ModifyValuesScreen(true); // self delete on exit diff --git a/src/modules/utils/panel/screens/ExtruderScreen.h b/src/modules/utils/panel/screens/ExtruderScreen.h index 30e21a34..19a45ca6 100644 --- a/src/modules/utils/panel/screens/ExtruderScreen.h +++ b/src/modules/utils/panel/screens/ExtruderScreen.h @@ -15,14 +15,12 @@ class ExtruderScreen : public PanelScreen { ExtruderScreen(); void on_refresh(); void on_enter(); - void on_main_loop(); void display_menu_line(uint16_t line); void clicked_menu_entry(uint16_t line); int idle_timeout_secs() { return 60; } private: void setupConfigSettings(); - const char *command; }; #endif diff --git a/src/modules/utils/panel/screens/FileScreen.cpp b/src/modules/utils/panel/screens/FileScreen.cpp index 309850ea..a60c2d41 100644 --- a/src/modules/utils/panel/screens/FileScreen.cpp +++ b/src/modules/utils/panel/screens/FileScreen.cpp @@ -116,11 +116,14 @@ void FileScreen::clicked_line(uint16_t line) } } -// only filter files that have a .g in them +// only filter files that have a .g, .ngc or .nc in them and does not start with a . bool FileScreen::filter_file(const char *f) { string fn= lc(f); - return (fn.find(".g") != string::npos); + return (fn.at(0) != '.') && + ((fn.find(".g") != string::npos) || + (fn.find(".ngc") != string::npos) || + (fn.find(".nc") != string::npos)); } // Find the "line"th file in the current folder @@ -132,8 +135,8 @@ string FileScreen::file_at(uint16_t line, bool& isdir) d = opendir(THEKERNEL->current_path.c_str()); if (d != NULL) { while ((p = readdir(d)) != NULL) { - // only filter files that have a .g in them and directories - if((p->d_isdir || filter_file(p->d_name)) && count++ == line ) { + // only filter files that have a .g in them and directories not starting with a . + if(((p->d_isdir && p->d_name[0] != '.') || filter_file(p->d_name)) && count++ == line ) { isdir= p->d_isdir; string fn= p->d_name; closedir(d); @@ -147,7 +150,7 @@ string FileScreen::file_at(uint16_t line, bool& isdir) return ""; } -// Count how many files there are in the current folder that have a .g in them +// Count how many files there are in the current folder that have a .g in them and does not start with a . uint16_t FileScreen::count_folder_content() { DIR *d; @@ -156,7 +159,7 @@ uint16_t FileScreen::count_folder_content() d = opendir(THEKERNEL->current_path.c_str()); if (d != NULL) { while ((p = readdir(d)) != NULL) { - if(p->d_isdir || filter_file(p->d_name)) count++; + if((p->d_isdir && p->d_name[0] != '.') || filter_file(p->d_name)) count++; } closedir(d); return count; diff --git a/src/modules/utils/panel/screens/JogScreen.cpp b/src/modules/utils/panel/screens/JogScreen.cpp index ab8f242a..d36c643a 100644 --- a/src/modules/utils/panel/screens/JogScreen.cpp +++ b/src/modules/utils/panel/screens/JogScreen.cpp @@ -29,7 +29,7 @@ JogScreen::JogScreen() void JogScreen::on_enter() { THEPANEL->enter_menu_mode(); - THEPANEL->setup_menu(4); + THEPANEL->setup_menu(5); this->refresh_menu(); } @@ -50,6 +50,7 @@ void JogScreen::display_menu_line(uint16_t line) case 1: THEPANEL->lcd->printf("Move 10.0mm \x7E"); break; case 2: THEPANEL->lcd->printf("Move 1.0mm \x7E"); break; case 3: THEPANEL->lcd->printf("Move 0.1mm \x7E"); break; + case 4: THEPANEL->lcd->printf("Move 0.01mm \x7E"); break; } } @@ -60,6 +61,7 @@ void JogScreen::clicked_menu_entry(uint16_t line) case 1: this->control_screen->set_jog_increment(10.0); break; case 2: this->control_screen->set_jog_increment(1.0); break; case 3: this->control_screen->set_jog_increment(0.1); break; + case 4: this->control_screen->set_jog_increment(0.01); break; } THEPANEL->enter_screen(this->control_screen); } diff --git a/src/modules/utils/panel/screens/MainMenuScreen.cpp b/src/modules/utils/panel/screens/MainMenuScreen.cpp index 4e53bcbb..2c4558d3 100644 --- a/src/modules/utils/panel/screens/MainMenuScreen.cpp +++ b/src/modules/utils/panel/screens/MainMenuScreen.cpp @@ -82,7 +82,7 @@ void MainMenuScreen::setupConfigureScreen() mvs->addMenuItem("Z Home Ofs", []() -> float { void *rd; PublicData::get_value( endstops_checksum, home_offset_checksum, &rd ); return rd==nullptr ? 0.0F : ((float*)rd)[2]; }, [this](float v) { send_gcode("M206", 'Z', v); }, - 0.1F + 0.01F ); mvs->addMenuItem("Contrast", @@ -118,7 +118,7 @@ void MainMenuScreen::display_menu_line(uint16_t line) { switch ( line ) { case 0: THEPANEL->lcd->printf("Watch"); break; - case 1: if(THEPANEL->is_halted()) THEPANEL->lcd->printf("Clear HALT"); else THEPANEL->lcd->printf(THEPANEL->is_playing() ? "Abort" : "Play"); break; + case 1: if(THEKERNEL->is_halted()) THEPANEL->lcd->printf("Clear HALT"); else THEPANEL->lcd->printf(THEPANEL->is_playing() ? "Abort" : "Play"); break; case 2: THEPANEL->lcd->printf("Jog"); break; case 3: THEPANEL->lcd->printf("Prepare"); break; case 4: THEPANEL->lcd->printf("Custom"); break; @@ -132,7 +132,7 @@ void MainMenuScreen::clicked_menu_entry(uint16_t line) switch ( line ) { case 0: THEPANEL->enter_screen(this->watch_screen); break; case 1: - if(THEPANEL->is_halted()){ + if(THEKERNEL->is_halted()){ send_command("M999"); THEPANEL->enter_screen(this->watch_screen); }else if(THEPANEL->is_playing()) abort_playing(); @@ -147,7 +147,8 @@ void MainMenuScreen::clicked_menu_entry(uint16_t line) void MainMenuScreen::abort_playing() { - PublicData::set_value(player_checksum, abort_play_checksum, NULL); + //PublicData::set_value(player_checksum, abort_play_checksum, NULL); + send_command("abort"); THEPANEL->enter_screen(this->watch_screen); } diff --git a/src/modules/utils/panel/screens/PrepareScreen.cpp b/src/modules/utils/panel/screens/PrepareScreen.cpp index 3208930b..6ebc256a 100644 --- a/src/modules/utils/panel/screens/PrepareScreen.cpp +++ b/src/modules/utils/panel/screens/PrepareScreen.cpp @@ -18,17 +18,18 @@ #include "PublicData.h" #include "TemperatureControlPublicAccess.h" #include "ModifyValuesScreen.h" +#include "TemperatureControlPool.h" + #include using namespace std; PrepareScreen::PrepareScreen() { - this->command = nullptr; - // Children screens - if(THEPANEL->temperature_screen != nullptr) { + std::vector controllers; + bool ok = PublicData::get_value(temperature_control_checksum, poll_controls_checksum, &controllers); + if (ok && controllers.size() > 0) { this->extruder_screen = (new ExtruderScreen())->set_parent(this); - THEPANEL->temperature_screen->set_parent(this); }else{ this->extruder_screen= nullptr; } @@ -38,7 +39,7 @@ void PrepareScreen::on_enter() { THEPANEL->enter_menu_mode(); // if no heaters or extruder then don't show related menu items - THEPANEL->setup_menu((THEPANEL->temperature_screen != nullptr) ? 9 : 5); + THEPANEL->setup_menu((this->extruder_screen != nullptr) ? 9 : 5); this->refresh_menu(); } @@ -56,7 +57,7 @@ void PrepareScreen::display_menu_line(uint16_t line) { switch ( line ) { case 0: THEPANEL->lcd->printf("Back" ); break; - case 1: THEPANEL->lcd->printf("Home All Axis" ); break; + case 1: THEPANEL->lcd->printf("Home All Axes" ); break; case 2: THEPANEL->lcd->printf("Set Home" ); break; case 3: THEPANEL->lcd->printf("Set Z0" ); break; case 4: THEPANEL->lcd->printf("Motors off" ); break; @@ -72,14 +73,14 @@ void PrepareScreen::clicked_menu_entry(uint16_t line) { switch ( line ) { case 0: THEPANEL->enter_screen(this->parent); break; - case 1: command = "G28"; break; - case 2: command = "G92 X0 Y0 Z0"; break; - case 3: command = "G92 Z0"; break; - case 4: command = "M84"; break; + case 1: send_command("G28"); break; + case 2: send_command("G92 X0 Y0 Z0"); break; + case 3: send_command("G92 Z0"); break; + case 4: send_command("M84"); break; case 5: this->preheat(); break; case 6: this->cooldown(); break; case 7: THEPANEL->enter_screen(this->extruder_screen); break; - case 8: THEPANEL->enter_screen(THEPANEL->temperature_screen); break; + case 8: setup_temperature_screen(); break; } } @@ -98,11 +99,52 @@ void PrepareScreen::cooldown() PublicData::set_value( temperature_control_checksum, bed_checksum, &t ); } -// queuing commands needs to be done from main loop -void PrepareScreen::on_main_loop() +static float getTargetTemperature(uint16_t heater_cs) +{ + struct pad_temperature temp; + bool ok = PublicData::get_value( temperature_control_checksum, current_temperature_checksum, heater_cs, &temp ); + + if (ok) { + return temp.target_temperature; + } + + return 0.0F; +} + +void PrepareScreen::setup_temperature_screen() { - // change actual axis value - if (this->command == nullptr) return; - send_command(this->command); - this->command = nullptr; + // setup temperature screen + auto mvs= new ModifyValuesScreen(true); // delete itself on exit + mvs->set_parent(this); + + int cnt= 0; + // returns enabled temperature controllers + std::vector controllers; + bool ok = PublicData::get_value(temperature_control_checksum, poll_controls_checksum, &controllers); + if (ok) { + for (auto &c : controllers) { + // rename if two of the known types + const char *name; + if(c.designator == "T") name= "Hotend"; + else if(c.designator == "B") name= "Bed"; + else name= c.designator.c_str(); + uint16_t i= c.id; + + mvs->addMenuItem(name, // menu name + [i]() -> float { return getTargetTemperature(i); }, // getter + [i](float t) { PublicData::set_value( temperature_control_checksum, i, &t ); }, // setter + 1.0F, // increment + 0.0F, // Min + 500.0F // Max + ); + cnt++; + } + } + + if(cnt > 0) { + THEPANEL->enter_screen(mvs); + }else{ + // no heaters and probably no extruders either + delete mvs; + } } diff --git a/src/modules/utils/panel/screens/PrepareScreen.h b/src/modules/utils/panel/screens/PrepareScreen.h index c6a695ce..4805ad08 100644 --- a/src/modules/utils/panel/screens/PrepareScreen.h +++ b/src/modules/utils/panel/screens/PrepareScreen.h @@ -17,7 +17,6 @@ public: void on_refresh(); void on_enter(); - void on_main_loop(); void display_menu_line(uint16_t line); void clicked_menu_entry(uint16_t line); int idle_timeout_secs() { return 60; } @@ -28,7 +27,6 @@ private: void setup_temperature_screen(); PanelScreen *extruder_screen; - const char *command; }; #endif diff --git a/src/modules/utils/panel/screens/ProbeScreen.cpp b/src/modules/utils/panel/screens/ProbeScreen.cpp index f8ae775b..0ef77a01 100644 --- a/src/modules/utils/panel/screens/ProbeScreen.cpp +++ b/src/modules/utils/panel/screens/ProbeScreen.cpp @@ -58,6 +58,10 @@ void ProbeScreen::on_refresh() THEPANEL->lcd->setCursor(0, 4); THEPANEL->lcd->printf("%20s", this->result.substr(20, 20).c_str()); } + if(this->result.size() > 40 && THEPANEL->get_screen_lines() > 5) { + THEPANEL->lcd->setCursor(0, 5); + THEPANEL->lcd->printf("%20s", this->result.substr(40, 20).c_str()); + } } } diff --git a/src/modules/utils/panel/screens/WatchScreen.cpp b/src/modules/utils/panel/screens/WatchScreen.cpp index 2eed0b5b..2d77f1a1 100644 --- a/src/modules/utils/panel/screens/WatchScreen.cpp +++ b/src/modules/utils/panel/screens/WatchScreen.cpp @@ -14,14 +14,16 @@ #include "libs/nuts_bolts.h" #include "libs/utils.h" #include "modules/tools/temperaturecontrol/TemperatureControlPublicAccess.h" -#include "modules/robot/RobotPublicAccess.h" +#include "Robot.h" #include "modules/robot/Conveyor.h" #include "modules/utils/player/PlayerPublicAccess.h" #include "NetworkPublicAccess.h" #include "PublicData.h" #include "SwitchPublicAccess.h" #include "checksumm.h" -#include "Pauser.h" +#include "TemperatureControlPool.h" + + #include #include #include @@ -51,8 +53,6 @@ static const uint8_t icons[] = { // 115x19 - 3 bytes each: he1, he2, he3, bed, f 0x0C, 0x00, 0x00, 0x06, 0x00, 0x00, 0x06, 0x00, 0x01, 0xFF, 0xFF, 0x80, 0x00, 0x00, 0x00 }; - - WatchScreen::WatchScreen() { speed_changed = false; @@ -77,6 +77,23 @@ void WatchScreen::on_enter() this->refresh_screen(false); THEPANEL->enter_control_mode(1, 0.5); THEPANEL->set_control_value(this->current_speed); + + // enumerate temperature controls + temp_controllers.clear(); + std::vector controllers; + bool ok = PublicData::get_value(temperature_control_checksum, poll_controls_checksum, &controllers); + if (ok) { + for (auto &c : controllers) { + temp_controllers.push_back(c.id); + } + } +} + +static struct pad_temperature getTemperatures(uint16_t heater_cs) +{ + struct pad_temperature temp; + PublicData::get_value( temperature_control_checksum, current_temperature_checksum, heater_cs, &temp ); + return temp; } void WatchScreen::on_refresh() @@ -123,22 +140,19 @@ void WatchScreen::on_refresh() // for LCDs with leds set them according to heater status bool bed_on= false, hotend_on= false, is_hot= false; uint8_t heon=0, hemsk= 0x01; // bit set for which hotend is on bit0: hotend1, bit1: hotend2 etc - for(auto m : THEPANEL->temperature_modules) { - // query each heater - void *p= getTemperatures(m); - struct pad_temperature *temp= static_cast(p); - if(temp != nullptr) { - if(temp->current_temperature > 50) is_hot= true; // anything is hot - if(temp->designator.front() == 'B' && temp->target_temperature > 0) bed_on= true; // bed on/off - if(temp->designator.front() == 'T') { // a hotend by convention - if(temp->target_temperature > 0){ - hotend_on= true;// hotend on/off (anyone) - heon |= hemsk; - } - hemsk <<= 1; + for(auto id : temp_controllers) { + struct pad_temperature c= getTemperatures(id); + if(c.current_temperature > 50) is_hot= true; // anything is hot + if(c.designator.front() == 'B' && c.target_temperature > 0) bed_on= true; // bed on/off + if(c.designator.front() == 'T') { // a hotend by convention + if(c.target_temperature > 0){ + hotend_on= true;// hotend on/off (anyone) + heon |= hemsk; } + hemsk <<= 1; } } + THEPANEL->lcd->setLed(LED_BED_ON, bed_on); THEPANEL->lcd->setLed(LED_HOTEND_ON, hotend_on); THEPANEL->lcd->setLed(LED_HOT, is_hot); @@ -171,30 +185,16 @@ void WatchScreen::on_main_loop() this->issue_change_speed = false; set_speed(); } -} - -void *WatchScreen::getTemperatures(uint16_t heater_cs) -{ - void *returned_data; - bool ok = PublicData::get_value( temperature_control_checksum, heater_cs, current_temperature_checksum, &returned_data ); - - if (ok) { - return returned_data; - } - - return nullptr; + PanelScreen::on_main_loop(); // in case any queued commands left } // fetch the data we are displaying void WatchScreen::get_current_status() { - void *returned_data; - bool ok; - // get fan status - ok = PublicData::get_value( switch_checksum, fan_checksum, 0, &returned_data ); + struct pad_switch s; + bool ok = PublicData::get_value( switch_checksum, fan_checksum, 0, &s ); if (ok) { - struct pad_switch s = *static_cast(returned_data); this->fan_state = s.state; } else { // fan probably disabled @@ -205,27 +205,13 @@ void WatchScreen::get_current_status() // fetch the data we are displaying float WatchScreen::get_current_speed() { - void *returned_data; - - bool ok = PublicData::get_value( robot_checksum, speed_override_percent_checksum, &returned_data ); - if (ok) { - float cs = *static_cast(returned_data); - return cs; - } - return 0.0; + // in percent + return 6000.0F / THEKERNEL->robot->get_seconds_per_minute(); } void WatchScreen::get_current_pos(float *cp) { - void *returned_data; - - bool ok = PublicData::get_value( robot_checksum, current_position_checksum, &returned_data ); - if (ok) { - float *p = static_cast(returned_data); - cp[0] = p[0]; - cp[1] = p[1]; - cp[2] = p[2]; - } + THEKERNEL->robot->get_axis_position(cp); } void WatchScreen::get_sd_play_info() @@ -249,9 +235,10 @@ void WatchScreen::display_menu_line(uint16_t line) // in menu mode switch ( line ) { case 0: - if(THEPANEL->temperature_modules.size() > 0) { + { + auto& tm= this->temp_controllers; + if(tm.size() > 0) { // only if we detected heaters in config - auto tm= THEPANEL->temperature_modules; int n= 0; if(tm.size() > 2) { // more than two temps we need to cycle between them @@ -264,18 +251,18 @@ void WatchScreen::display_menu_line(uint16_t line) for (size_t i = 0; i < 2; ++i) { size_t o= i+(n*2); if(o>tm.size()-1) break; - struct pad_temperature *temp= static_cast(getTemperatures(tm[o])); - if(temp == nullptr) continue; - int t= std::min(999, (int)roundf(temp->current_temperature)); - int tt= roundf(temp->target_temperature); + struct pad_temperature temp= getTemperatures(tm[o]); + int t= std::min(999, (int)roundf(temp.current_temperature)); + int tt= roundf(temp.target_temperature); THEPANEL->lcd->setCursor(off, 0); // col, row - off += THEPANEL->lcd->printf("%s:%03d/%03d ", temp->designator.substr(0, 2).c_str(), t, tt); + off += THEPANEL->lcd->printf("%s:%03d/%03d ", temp.designator.substr(0, 2).c_str(), t, tt); } }else{ //THEPANEL->lcd->printf("No Heaters"); } break; + } case 1: THEPANEL->lcd->printf("X%4d Y%4d Z%7.2f", (int)round(this->pos[0]), (int)round(this->pos[1]), this->pos[2]); break; case 2: THEPANEL->lcd->printf("%3d%% %2lu:%02lu %3u%% sd", this->current_speed, this->elapsed_time / 60, this->elapsed_time % 60, this->sd_pcnt_played); break; case 3: THEPANEL->lcd->printf("%19s", this->get_status()); break; @@ -284,23 +271,20 @@ void WatchScreen::display_menu_line(uint16_t line) const char *WatchScreen::get_status() { - if (THEPANEL->hasMessage()) { + if (THEPANEL->hasMessage()) return THEPANEL->getMessage().c_str(); - } - if (THEPANEL->is_halted()) { + if (THEKERNEL->is_halted()) return "HALTED Reset or M999"; - } - if (THEKERNEL->pauser->paused()) - return "Paused"; + if (THEPANEL->is_suspended()) + return "Suspended"; if (THEPANEL->is_playing()) return THEPANEL->get_playing_file(); - if (!THEKERNEL->conveyor->is_queue_empty()) { + if (!THEKERNEL->conveyor->is_queue_empty()) return "Printing"; - } const char *ip = get_network(); if (ip == NULL) { diff --git a/src/modules/utils/panel/screens/WatchScreen.h b/src/modules/utils/panel/screens/WatchScreen.h index bf723caf..32787ba7 100644 --- a/src/modules/utils/panel/screens/WatchScreen.h +++ b/src/modules/utils/panel/screens/WatchScreen.h @@ -30,7 +30,8 @@ private: void get_sd_play_info(); const char *get_status(); const char *get_network(); - void *getTemperatures(uint16_t heater_cs); + + std::vector temp_controllers; uint32_t update_counts; int current_speed; diff --git a/src/modules/utils/pausebutton/PauseButton.cpp b/src/modules/utils/pausebutton/PauseButton.cpp deleted file mode 100644 index 7d6d81b3..00000000 --- a/src/modules/utils/pausebutton/PauseButton.cpp +++ /dev/null @@ -1,119 +0,0 @@ -#include "libs/Kernel.h" -#include "PauseButton.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 "Pauser.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 pause_button_pin_checksum CHECKSUM("pause_button_pin") -#define kill_button_pin_checksum CHECKSUM("kill_button_pin") - -PauseButton::PauseButton() -{ - this->button_state = true; - this->killed = false; - this->do_kill= false; -} - -void PauseButton::on_module_loaded() -{ - this->pause_enable = THEKERNEL->config->value( pause_button_enable_checksum )->by_default(false)->as_bool(); - this->kill_enable = THEKERNEL->config->value( kill_button_enable_checksum )->by_default(false)->as_bool(); - this->pause_button.from_string( THEKERNEL->config->value( pause_button_pin_checksum )->by_default("2.12")->as_string())->as_input(); - this->kill_button.from_string( THEKERNEL->config->value( kill_button_pin_checksum )->by_default("nc")->as_string())->as_input(); - - if(this->kill_enable && this->kill_button.connected() && pause_button.equals(kill_button)) { - // kill button takes priority - this->pause_enable = false; - - } else if(this->kill_enable && !this->kill_button.connected() && !this->pause_enable && pause_button.connected()) { - // use pause button for kill button if kill buttin not specifically defined - this->kill_button = this->pause_button; - } - - this->register_for_event(ON_CONSOLE_LINE_RECEIVED); - - if( (this->pause_enable && this->pause_button.connected()) || (this->kill_enable && this->kill_button.connected()) ) { - THEKERNEL->slow_ticker->attach( 10, this, &PauseButton::button_tick ); - this->register_for_event(ON_IDLE); - } -} - -void PauseButton::on_idle(void *argument) -{ - if(do_kill) { - do_kill= false; - THEKERNEL->call_event(ON_HALT, nullptr); - THEKERNEL->streams->printf("Kill button pressed - reset or M999 to continue\r\n"); - } -} - -//TODO: Make this use InterruptIn -//Check the state of the button and act accordingly based on current pause state -// Note this is ISR so don't do anything nasty in here -uint32_t PauseButton::button_tick(uint32_t dummy) -{ - // If pause button changed - if(this->pause_enable && this->pause_button.connected()) { - bool newstate = this->pause_button.get(); - if(this->button_state != newstate) { - this->button_state = newstate; - // If button pressed - if( this->button_state ) { - if( THEKERNEL->pauser->paused() ) { - THEKERNEL->pauser->release(); - } else { - THEKERNEL->pauser->take(); - } - } - } - } - - if(!this->killed && this->kill_enable && this->kill_button.connected() && !this->kill_button.get()) { - this->killed = true; - // we can't call this in ISR, and we need to block on_main_loop so do it in on_idle - // THEKERNEL->call_event(ON_HALT); - this->do_kill= true; - } - - return 0; -} - -// When a new line is received, check if it is a command, and if it is, act upon it -void PauseButton::on_console_line_received( void *argument ) -{ - SerialMessage new_message = *static_cast(argument); - - if(this->killed && new_message.message == "M999") { - this->killed= false; - return; - } - - // ignore comments and blank lines and if this is a G code then also ignore it - char first_char = new_message.message[0]; - if(strchr(";( \n\rGMTN", first_char) != NULL) return; - - string cmd = shift_parameter(new_message.message); - - if (cmd == "freeze") { - if( !THEKERNEL->pauser->paused() ) { - THEKERNEL->pauser->take(); - } - - } else if (cmd == "unfreeze") { - if( THEKERNEL->pauser->paused() ) { - THEKERNEL->pauser->release(); - } - } -} - diff --git a/src/modules/utils/pausebutton/PauseButton.h b/src/modules/utils/pausebutton/PauseButton.h deleted file mode 100644 index 7ca62220..00000000 --- a/src/modules/utils/pausebutton/PauseButton.h +++ /dev/null @@ -1,27 +0,0 @@ -#ifndef PAUSEBUTTON_H -#define PAUSEBUTTON_H - -#include "libs/Pin.h" - -class PauseButton : public Module { - public: - PauseButton(); - - void on_module_loaded(); - void on_console_line_received( void *argument ); - void on_idle(void *argument); - uint32_t button_tick(uint32_t dummy); - - private: - Pin pause_button; - Pin kill_button; - struct { - bool pause_enable:1; - bool kill_enable:1; - bool button_state:1; - bool killed:1; - volatile bool do_kill:1; - }; -}; - -#endif diff --git a/src/modules/utils/player/Player.cpp b/src/modules/utils/player/Player.cpp index 0dd84e51..cb7f319a 100644 --- a/src/modules/utils/player/Player.cpp +++ b/src/modules/utils/player/Player.cpp @@ -17,7 +17,6 @@ #include "libs/StreamOutput.h" #include "Gcode.h" #include "checksumm.h" -#include "Pauser.h" #include "Config.h" #include "ConfigValue.h" #include "SDFAT.h" @@ -25,41 +24,59 @@ #include "modules/robot/Conveyor.h" #include "DirHandle.h" #include "PublicDataRequest.h" +#include "PublicData.h" #include "PlayerPublicAccess.h" +#include "TemperatureControlPublicAccess.h" +#include "TemperatureControlPool.h" +#include "ExtruderPublicAccess.h" -#define on_boot_gcode_checksum CHECKSUM("on_boot_gcode") -#define on_boot_gcode_enable_checksum CHECKSUM("on_boot_gcode_enable") +#include +#include +#include + +#include "mbed.h" + +#define on_boot_gcode_checksum CHECKSUM("on_boot_gcode") +#define on_boot_gcode_enable_checksum CHECKSUM("on_boot_gcode_enable") +#define after_suspend_gcode_checksum CHECKSUM("after_suspend_gcode") +#define before_resume_gcode_checksum CHECKSUM("before_resume_gcode") +#define leave_heaters_on_suspend_checksum CHECKSUM("leave_heaters_on_suspend") extern SDFAT mounter; -void Player::on_module_loaded() +Player::Player() { this->playing_file = false; - this->current_file_handler = NULL; + this->current_file_handler = nullptr; this->booted = false; + this->elapsed_secs = 0; + this->reply_stream = nullptr; + this->suspended= false; + this->suspend_loops= 0; +} + +void Player::on_module_loaded() +{ this->register_for_event(ON_CONSOLE_LINE_RECEIVED); this->register_for_event(ON_MAIN_LOOP); this->register_for_event(ON_SECOND_TICK); this->register_for_event(ON_GET_PUBLIC_DATA); this->register_for_event(ON_SET_PUBLIC_DATA); this->register_for_event(ON_GCODE_RECEIVED); - this->register_for_event(ON_HALT); this->on_boot_gcode = THEKERNEL->config->value(on_boot_gcode_checksum)->by_default("/sd/on_boot.gcode")->as_string(); this->on_boot_gcode_enable = THEKERNEL->config->value(on_boot_gcode_enable_checksum)->by_default(true)->as_bool(); - this->elapsed_secs = 0; - this->reply_stream = NULL; - this->halted= false; -} -void Player::on_halt(void *arg) -{ - halted= (arg == nullptr); + this->after_suspend_gcode = THEKERNEL->config->value(after_suspend_gcode_checksum)->by_default("")->as_string(); + this->before_resume_gcode = THEKERNEL->config->value(before_resume_gcode_checksum)->by_default("")->as_string(); + std::replace( this->after_suspend_gcode.begin(), this->after_suspend_gcode.end(), '_', ' '); // replace _ with space + std::replace( this->before_resume_gcode.begin(), this->before_resume_gcode.end(), '_', ' '); // replace _ with space + this->leave_heaters_on = THEKERNEL->config->value(leave_heaters_on_suspend_checksum)->by_default(false)->as_bool(); } void Player::on_second_tick(void *) { - if (!THEKERNEL->pauser->paused()) this->elapsed_secs++; + if(this->playing_file) this->elapsed_secs++; } // extract any options found on line, terminates args at the space before the first option (-v) @@ -83,12 +100,10 @@ void Player::on_gcode_received(void *argument) string args = get_arguments(gcode->get_command()); if (gcode->has_m) { if (gcode->m == 21) { // Dummy code; makes Octoprint happy -- supposed to initialize SD card - gcode->mark_as_taken(); mounter.remount(); gcode->stream->printf("SD card ok\r\n"); } else if (gcode->m == 23) { // select file - gcode->mark_as_taken(); this->filename = "/sd/" + args; // filename is whatever is in args this->current_stream = &(StreamOutput::NullStream); @@ -120,7 +135,6 @@ void Player::on_gcode_received(void *argument) this->elapsed_secs = 0; } else if (gcode->m == 24) { // start print - gcode->mark_as_taken(); if (this->current_file_handler != NULL) { this->playing_file = true; // this would be a problem if the stream goes away before the file has finished, @@ -130,11 +144,9 @@ void Player::on_gcode_received(void *argument) } } else if (gcode->m == 25) { // pause print - gcode->mark_as_taken(); this->playing_file = false; } else if (gcode->m == 26) { // Reset print. Slightly different than M26 in Marlin and the rest - gcode->mark_as_taken(); if(this->current_file_handler != NULL) { string currentfn = this->filename.c_str(); unsigned long old_size = this->file_size; @@ -154,17 +166,14 @@ void Player::on_gcode_received(void *argument) this->current_stream = &(StreamOutput::NullStream); } } - } else { gcode->stream->printf("No file loaded\r\n"); } } else if (gcode->m == 27) { // report print progress, in format used by Marlin - gcode->mark_as_taken(); progress_command("-b", gcode->stream); } else if (gcode->m == 32) { // select file and start print - gcode->mark_as_taken(); // Get filename this->filename = "/sd/" + args; // filename is whatever is in args including spaces this->current_stream = &(StreamOutput::NullStream); @@ -179,8 +188,25 @@ void Player::on_gcode_received(void *argument) gcode->stream->printf("file.open failed: %s\r\n", this->filename.c_str()); } else { this->playing_file = true; + + // get size of file + int result = fseek(this->current_file_handler, 0, SEEK_END); + if (0 != result) { + file_size = 0; + } else { + file_size = ftell(this->current_file_handler); + fseek(this->current_file_handler, 0, SEEK_SET); + } } + this->played_cnt = 0; + this->elapsed_secs = 0; + + } else if (gcode->m == 600) { // suspend print, Not entirely Marlin compliant, M600.1 will leave the heaters on + this->suspend_command((gcode->subcode == 1)?"h":"", gcode->stream); + + } else if (gcode->m == 601) { // resume print + this->resume_command("", gcode->stream); } } } @@ -188,7 +214,7 @@ void Player::on_gcode_received(void *argument) // When a new line is received, check if it is a command, and if it is, act upon it void Player::on_console_line_received( void *argument ) { - if(halted) return; // if in halted state ignore any commands + if(THEKERNEL->is_halted()) return; // if in halted state ignore any commands SerialMessage new_message = *static_cast(argument); @@ -206,8 +232,13 @@ void Player::on_console_line_received( void *argument ) this->play_command( possible_command, new_message.stream ); }else if (cmd == "progress"){ this->progress_command( possible_command, new_message.stream ); - }else if (cmd == "abort") + }else if (cmd == "abort") { this->abort_command( possible_command, new_message.stream ); + }else if (cmd == "suspend") { + this->suspend_command( possible_command, new_message.stream ); + }else if (cmd == "resume") { + this->resume_command( possible_command, new_message.stream ); + } } // Play a gcode file by considering each line as if it was received on the serial console @@ -218,7 +249,7 @@ void Player::play_command( string parameters, StreamOutput *stream ) // Get filename which is the entire parameter line upto any options found or entire line this->filename = absolute_from_relative(parameters); - if(this->playing_file) { + if(this->playing_file || this->suspended) { stream->printf("Currently printing, abort print first\r\n"); return; } @@ -289,7 +320,7 @@ void Player::progress_command( string parameters, StreamOutput *stream ) unsigned int pcnt = (file_size - (file_size - played_cnt)) * 100 / file_size; // If -b or -B is passed, report in the format used by Marlin and the others. if (!sdprinting) { - stream->printf("%u %% complete, elapsed time: %lu s", pcnt, this->elapsed_secs); + stream->printf("file: %s, %u %% complete, elapsed time: %lu s", this->filename.c_str(), pcnt, this->elapsed_secs); if(est > 0) { stream->printf(", est time: %lu s", est); } @@ -309,6 +340,7 @@ void Player::abort_command( string parameters, StreamOutput *stream ) stream->printf("Not currently playing\r\n"); return; } + suspended= false; playing_file = false; played_cnt = 0; file_size = 0; @@ -317,25 +349,26 @@ void Player::abort_command( string parameters, StreamOutput *stream ) fclose(current_file_handler); current_file_handler = NULL; if(parameters.empty()) { - // clear out the block queue - // I think this is a HACK... wait for queue !full as flushing a full queue doesn't work well - // as it means there is probably a gcode waiting to be pushed and will be as soon as I flush the queue this causes - // one more move but it is the last move queued so is completely wrong, this HACK means we stop cleanly but - // only after the current move has completed and maybe the next one. - while (THEKERNEL->conveyor->is_queue_full()) { - THEKERNEL->call_event(ON_IDLE); - } - + // clear out the block queue, will wait until queue is empty + // MUST be called in on_main_loop to make sure there are no blocked main loops waiting to put something on the queue THEKERNEL->conveyor->flush_queue(); // now the position will think it is at the last received pos, so we need to do FK to get the actuator position and reset the current position THEKERNEL->robot->reset_position_from_current_actuator_position(); } - stream->printf("Aborted playing or paused file\r\n"); + stream->printf("Aborted playing or paused file. Please turn any heaters off manually\r\n"); } void Player::on_main_loop(void *argument) { + if(suspended && suspend_loops > 0) { + // if we are suspended we need to allow main loop to cycle a few times then finish off the suspend processing + if(--suspend_loops == 0) { + suspend_part2(); + return; + } + } + if( !this->booted ) { this->booted = true; if( this->on_boot_gcode_enable ) { @@ -346,7 +379,7 @@ void Player::on_main_loop(void *argument) } if( this->playing_file ) { - if(halted) { + if(THEKERNEL->is_halted()) { abort_command("1", &(StreamOutput::NullStream)); return; } @@ -403,9 +436,9 @@ void Player::on_get_public_data(void *argument) if(!pdr->starts_with(player_checksum)) return; - if(pdr->second_element_is(is_playing_checksum)) { + if(pdr->second_element_is(is_playing_checksum) || pdr->second_element_is(is_suspended_checksum)) { static bool bool_data; - bool_data = this->playing_file; + bool_data = pdr->second_element_is(is_playing_checksum) ? this->playing_file : this->suspended; pdr->set_data_ptr(&bool_data); pdr->set_taken(); @@ -432,3 +465,198 @@ void Player::on_set_public_data(void *argument) pdr->set_taken(); } } + +/** +Suspend a print in progress +1. send pause to upstream host, or pause if printing from sd +1a. loop on_main_loop several times to clear any buffered commmands +2. wait for empty queue +3. save the current position, extruder position, temperatures - any state that would need to be restored +4. retract by specifed amount either on command line or in config +5. turn off heaters. +6. optionally run after_suspend gcode (either in config or on command line) + +User may jog or remove and insert filament at this point, extruding or retracting as needed + +*/ +void Player::suspend_command(string parameters, StreamOutput *stream ) +{ + if(suspended) { + stream->printf("Already suspended\n"); + return; + } + + stream->printf("Suspending print, waiting for queue to empty...\n"); + + // override the leave_heaters_on setting + this->override_leave_heaters_on= (parameters == "h"); + + suspended= true; + if( this->playing_file ) { + // pause an sd print + this->playing_file = false; + this->was_playing_file= true; + }else{ + // send pause to upstream host, we send it on all ports as we don't know which it is on + THEKERNEL->streams->printf("// action:pause\r\n"); + this->was_playing_file= false; + } + + // we need to allow main loop to cycle a few times to clear any buffered commands in the serial streams etc + suspend_loops= 10; +} + +// this completes the suspend +void Player::suspend_part2() +{ + // need to use streams here as the original stream may have changed + THEKERNEL->streams->printf("// Waiting for queue to empty (Host must stop sending)...\n"); + // wait for queue to empty + THEKERNEL->conveyor->wait_for_empty_queue(); + + THEKERNEL->streams->printf("// Saving current state...\n"); + + // save current XYZ position + THEKERNEL->robot->get_axis_position(this->saved_position); + + // save current extruder state + PublicData::set_value( extruder_checksum, save_state_checksum, nullptr ); + + // save state use M120 + THEKERNEL->robot->push_state(); + + // TODO retract by optional amount... + + this->saved_temperatures.clear(); + if(!this->leave_heaters_on && !this->override_leave_heaters_on) { + // save current temperatures, get a vector of all the controllers data + std::vector controllers; + bool ok = PublicData::get_value(temperature_control_checksum, poll_controls_checksum, &controllers); + if (ok) { + // query each heater and save the target temperature if on + for (auto &c : controllers) { + // TODO see if in exclude list + if(c.target_temperature > 0) { + this->saved_temperatures[c.id]= c.target_temperature; + } + } + } + + // turn off heaters that were on + for(auto& h : this->saved_temperatures) { + float t= 0; + PublicData::set_value( temperature_control_checksum, h.first, &t ); + } + } + + // execute optional gcode if defined + if(!after_suspend_gcode.empty()) { + struct SerialMessage message; + message.message = after_suspend_gcode; + message.stream = &(StreamOutput::NullStream); + THEKERNEL->call_event(ON_CONSOLE_LINE_RECEIVED, &message ); + } + + THEKERNEL->streams->printf("// Print Suspended, enter resume to continue printing\n"); +} + +/** +resume the suspended print +1. restore the temperatures and wait for them to get up to temp +2. optionally run before_resume gcode if specified +3. restore the position it was at and E and any other saved state +4. resume sd print or send resume upstream +*/ +void Player::resume_command(string parameters, StreamOutput *stream ) +{ + if(!suspended) { + stream->printf("Not suspended\n"); + return; + } + + stream->printf("resuming print...\n"); + + // wait for them to reach temp + if(!this->saved_temperatures.empty()) { + // set heaters to saved temps + for(auto& h : this->saved_temperatures) { + float t= h.second; + PublicData::set_value( temperature_control_checksum, h.first, &t ); + } + stream->printf("Waiting for heaters...\n"); + bool wait= true; + uint32_t tus= us_ticker_read(); // mbed call + while(wait) { + wait= false; + + bool timeup= false; + if((us_ticker_read() - tus) >= 1000000) { // print every 1 second + timeup= true; + tus= us_ticker_read(); // mbed call + } + + for(auto& h : this->saved_temperatures) { + struct pad_temperature temp; + if(PublicData::get_value( temperature_control_checksum, current_temperature_checksum, h.first, &temp )) { + if(timeup) + stream->printf("%s:%3.1f /%3.1f @%d ", temp.designator.c_str(), temp.current_temperature, ((temp.target_temperature == -1) ? 0.0 : temp.target_temperature), temp.pwm); + wait= wait || (temp.current_temperature < h.second); + } + } + if(timeup) stream->printf("\n"); + + if(wait) + THEKERNEL->call_event(ON_IDLE, this); + + if(THEKERNEL->is_halted()) { + // abort temp wait and rest of resume + THEKERNEL->streams->printf("Resume aborted by kill\n"); + THEKERNEL->robot->pop_state(); + this->saved_temperatures.clear(); + suspended= false; + return; + } + } + } + + // execute optional gcode if defined + if(!before_resume_gcode.empty()) { + stream->printf("Executing before resume gcode...\n"); + struct SerialMessage message; + message.message = before_resume_gcode; + message.stream = &(StreamOutput::NullStream); + THEKERNEL->call_event(ON_CONSOLE_LINE_RECEIVED, &message ); + } + + // Restore position + stream->printf("Restoring saved XYZ positions and state...\n"); + THEKERNEL->robot->pop_state(); + bool abs_mode= THEKERNEL->robot->absolute_mode; // what mode we were in + // force absolute mode for restoring position, then set to the saved relative/absolute mode + THEKERNEL->robot->absolute_mode= true; + { + char buf[128]; + int n = snprintf(buf, sizeof(buf), "G1 X%f Y%f Z%f", saved_position[0], saved_position[1], saved_position[2]); + string g(buf, n); + Gcode gcode(g, &(StreamOutput::NullStream)); + THEKERNEL->call_event(ON_GCODE_RECEIVED, &gcode ); + } + THEKERNEL->robot->absolute_mode= abs_mode; + + // restore extruder state + PublicData::set_value( extruder_checksum, restore_state_checksum, nullptr ); + + stream->printf("Resuming print\n"); + + if(this->was_playing_file) { + this->playing_file = true; + this->was_playing_file= false; + }else{ + // Send resume to host + THEKERNEL->streams->printf("// action:resume\r\n"); + } + + // clean up + this->saved_temperatures.clear(); + suspended= false; +} diff --git a/src/modules/utils/player/Player.h b/src/modules/utils/player/Player.h index b46c8811..cbcfb489 100644 --- a/src/modules/utils/player/Player.h +++ b/src/modules/utils/player/Player.h @@ -13,19 +13,20 @@ #include #include +#include +#include using std::string; class StreamOutput; class Player : public Module { public: - Player(){} + Player(); void on_module_loaded(); void on_console_line_received( void* argument ); void on_main_loop( void* argument ); void on_second_tick(void* argument); - void on_halt(void* argument); void on_get_public_data(void* argument); void on_set_public_data(void* argument); void on_gcode_received(void *argument); @@ -34,21 +35,33 @@ class Player : public Module { void play_command( string parameters, StreamOutput* stream ); void progress_command( string parameters, StreamOutput* stream ); void abort_command( string parameters, StreamOutput* stream ); + void suspend_command( string parameters, StreamOutput* stream ); + void resume_command( string parameters, StreamOutput* stream ); string extract_options(string& args); + void suspend_part2(); string filename; - + string after_suspend_gcode; + string before_resume_gcode; string on_boot_gcode; StreamOutput* current_stream; StreamOutput* reply_stream; + FILE* current_file_handler; - unsigned long file_size, played_cnt; + long file_size; + unsigned long played_cnt; unsigned long elapsed_secs; + float saved_position[3]; + std::map saved_temperatures; struct { bool on_boot_gcode_enable:1; bool booted:1; bool playing_file:1; - bool halted:1; + bool suspended:1; + bool was_playing_file:1; + bool leave_heaters_on:1; + bool override_leave_heaters_on:1; + uint8_t suspend_loops:4; }; }; diff --git a/src/modules/utils/player/PlayerPublicAccess.h b/src/modules/utils/player/PlayerPublicAccess.h index 40ae830f..bf813378 100644 --- a/src/modules/utils/player/PlayerPublicAccess.h +++ b/src/modules/utils/player/PlayerPublicAccess.h @@ -3,6 +3,7 @@ #define player_checksum CHECKSUM("player") #define is_playing_checksum CHECKSUM("is_playing") +#define is_suspended_checksum CHECKSUM("is_suspended") #define abort_play_checksum CHECKSUM("abort_play") #define get_progress_checksum CHECKSUM("progress") diff --git a/src/modules/utils/simpleshell/SimpleShell.cpp b/src/modules/utils/simpleshell/SimpleShell.cpp index 85248be2..0129f92b 100644 --- a/src/modules/utils/simpleshell/SimpleShell.cpp +++ b/src/modules/utils/simpleshell/SimpleShell.cpp @@ -17,21 +17,26 @@ #include "mri.h" #include "version.h" #include "PublicDataRequest.h" +#include "AppendFileStream.h" #include "FileStream.h" #include "checksumm.h" #include "PublicData.h" #include "Gcode.h" +#include "Robot.h" #include "modules/tools/temperaturecontrol/TemperatureControlPublicAccess.h" -#include "modules/robot/RobotPublicAccess.h" #include "NetworkPublicAccess.h" #include "platform_memory.h" #include "SwitchPublicAccess.h" #include "SDFAT.h" +#include "Thermistor.h" +#include "md5.h" #include "system_LPC17xx.h" #include "LPC17xx.h" +#include "mbed.h" // for wait_ms() + extern unsigned int g_maximumHeapAddress; #include @@ -66,6 +71,9 @@ const SimpleShell::ptentry_t SimpleShell::commands_table[] = { {"load", SimpleShell::load_command}, {"save", SimpleShell::save_command}, {"remount", SimpleShell::remount_command}, + {"calc_thermistor", SimpleShell::calc_thermistor_command}, + {"thermistors", SimpleShell::print_thermistors_command}, + {"md5sum", SimpleShell::md5sum_command}, // unknown command {NULL, NULL} @@ -155,17 +163,14 @@ void SimpleShell::on_gcode_received(void *argument) if (gcode->has_m) { if (gcode->m == 20) { // list sd card - gcode->mark_as_taken(); gcode->stream->printf("Begin file list\r\n"); ls_command("/sd", gcode->stream); gcode->stream->printf("End file list\r\n"); } else if (gcode->m == 30) { // remove file - gcode->mark_as_taken(); rm_command("/sd/" + args, gcode->stream); } else if(gcode->m == 501) { // load config override - gcode->mark_as_taken(); if(args.empty()) { load_command("/sd/config-override", gcode->stream); } else { @@ -173,7 +178,6 @@ void SimpleShell::on_gcode_received(void *argument) } } else if(gcode->m == 504) { // save to specific config override file - gcode->mark_as_taken(); if(args.empty()) { save_command("/sd/config-override", gcode->stream); } else { @@ -273,7 +277,7 @@ void SimpleShell::rm_command( string parameters, StreamOutput *stream ) void SimpleShell::mv_command( string parameters, StreamOutput *stream ) { string from = absolute_from_relative(shift_parameter( parameters )); - string to = shift_parameter(parameters); + string to = absolute_from_relative(shift_parameter(parameters)); int s = rename(from.c_str(), to.c_str()); if (s != 0) stream->printf("Could not rename %s to %s\r\n", from.c_str(), to.c_str()); else stream->printf("renamed %s to %s\r\n", from.c_str(), to.c_str()); @@ -332,6 +336,8 @@ void SimpleShell::cat_command( string parameters, StreamOutput *stream ) stream->puts(buffer.c_str()); buffer.clear(); if(linecnt > 80) linecnt = 0; + // we need to kick things or they die + THEKERNEL->call_event(ON_IDLE); } if ( newlines == limit ) { break; @@ -391,6 +397,8 @@ void SimpleShell::upload_command( string parameters, StreamOutput *stream ) // HACK ALERT to get around fwrite corruption close and re open for append fclose(fd); fd = fopen(upload_filename.c_str(), "a"); + // we need to kick things or they die + THEKERNEL->call_event(ON_IDLE); } } } @@ -443,18 +451,29 @@ void SimpleShell::save_command( string parameters, StreamOutput *stream ) filename = THEKERNEL->config_override_filename(); } - // replace stream with one that writes to config-override file - FileStream *gs = new FileStream(filename.c_str()); - if(!gs->is_open()) { - stream->printf("Unable to open File %s for write\n", filename.c_str()); - return; + THEKERNEL->conveyor->wait_for_empty_queue(); //just to be safe as it can take a while to run + + //remove(filename.c_str()); // seems to cause a hang every now and then + { + FileStream fs(filename.c_str()); + fs.printf("; DO NOT EDIT THIS FILE\n"); + // this also will truncate the existing file instead of deleting it } + // stream that appends to file + AppendFileStream *gs = new AppendFileStream(filename.c_str()); + // if(!gs->is_open()) { + // stream->printf("Unable to open File %s for write\n", filename.c_str()); + // return; + // } + + __disable_irq(); // issue a M500 which will store values in the file stream Gcode *gcode = new Gcode("M500", gs); THEKERNEL->call_event(ON_GCODE_RECEIVED, gcode ); delete gs; delete gcode; + __enable_irq(); stream->printf("Settings Stored to %s\r\n", filename.c_str()); } @@ -544,29 +563,37 @@ void SimpleShell::break_command( string parameters, StreamOutput *stream) void SimpleShell::get_command( string parameters, StreamOutput *stream) { string what = shift_parameter( parameters ); - void *returned_data; if (what == "temp") { + struct pad_temperature temp; string type = shift_parameter( parameters ); - bool ok = PublicData::get_value( temperature_control_checksum, get_checksum(type), current_temperature_checksum, &returned_data ); - - if (ok) { - struct pad_temperature temp = *static_cast(returned_data); - stream->printf("%s temp: %f/%f @%d\r\n", type.c_str(), temp.current_temperature, temp.target_temperature, temp.pwm); - } else { - stream->printf("%s is not a known temperature device\r\n", type.c_str()); - } + if(type.empty()) { + // scan all temperature controls + std::vector controllers; + bool ok = PublicData::get_value(temperature_control_checksum, poll_controls_checksum, &controllers); + if (ok) { + for (auto &c : controllers) { + stream->printf("%s (%d) temp: %f/%f @%d\r\n", c.designator.c_str(), c.id, c.current_temperature, c.target_temperature, c.pwm); + } - } else if (what == "pos") { - bool ok = PublicData::get_value( robot_checksum, current_position_checksum, &returned_data ); + } else { + stream->printf("no heaters found\r\n"); + } - if (ok) { - float *pos = static_cast(returned_data); - stream->printf("Position X: %f, Y: %f, Z: %f\r\n", pos[0], pos[1], pos[2]); + }else{ + bool ok = PublicData::get_value( temperature_control_checksum, current_temperature_checksum, get_checksum(type), &temp ); - } else { - stream->printf("get pos command failed\r\n"); + if (ok) { + stream->printf("%s temp: %f/%f @%d\r\n", type.c_str(), temp.current_temperature, temp.target_temperature, temp.pwm); + } else { + stream->printf("%s is not a known temperature device\r\n", type.c_str()); + } } + + } else if (what == "pos") { + float pos[3]; + THEKERNEL->robot->get_axis_position(pos); + stream->printf("Position X: %f, Y: %f, Z: %f\r\n", pos[0], pos[1], pos[2]); } } @@ -585,6 +612,46 @@ void SimpleShell::set_temp_command( string parameters, StreamOutput *stream) } } +void SimpleShell::print_thermistors_command( string parameters, StreamOutput *stream) +{ + Thermistor::print_predefined_thermistors(stream); +} + +void SimpleShell::calc_thermistor_command( string parameters, StreamOutput *stream) +{ + string s = shift_parameter( parameters ); + int saveto= -1; + // see if we have -sn as first argument + if(s.find("-s", 0, 2) != string::npos) { + // save the results to thermistor n + saveto= strtol(s.substr(2).c_str(), nullptr, 10); + }else{ + parameters= s; + } + + std::vector trl= parse_number_list(parameters.c_str()); + if(trl.size() == 6) { + // calculate the coefficients + float c1, c2, c3; + std::tie(c1, c2, c3) = Thermistor::calculate_steinhart_hart_coefficients(trl[0], trl[1], trl[2], trl[3], trl[4], trl[5]); + stream->printf("Steinhart Hart coefficients: I%1.18f J%1.18f K%1.18f\n", c1, c2, c3); + if(saveto == -1) { + stream->printf(" Paste the above in the M305 S0 command, then save with M500\n"); + }else{ + char buf[80]; + int n = snprintf(buf, sizeof(buf), "M305 S%d I%1.18f J%1.18f K%1.18f", saveto, c1, c2, c3); + string g(buf, n); + Gcode gcode(g, &(StreamOutput::NullStream)); + THEKERNEL->call_event(ON_GCODE_RECEIVED, &gcode ); + stream->printf(" Setting Thermistor %d to those settings, save with M500\n", saveto); + } + + }else{ + // give help + stream->printf("Usage: calc_thermistor T1,R1,T2,R2,T3,R3\n"); + } +} + // used to test out the get public data events for switch void SimpleShell::switch_command( string parameters, StreamOutput *stream) { @@ -605,6 +672,29 @@ void SimpleShell::switch_command( string parameters, StreamOutput *stream) } } +void SimpleShell::md5sum_command( string parameters, StreamOutput *stream ) +{ + string filename = absolute_from_relative(parameters); + + // Open file + FILE *lp = fopen(filename.c_str(), "r"); + if (lp == NULL) { + stream->printf("File not found: %s\r\n", filename.c_str()); + return; + } + MD5 md5; + uint8_t buf[64]; + do { + size_t n= fread(buf, 1, sizeof buf, lp); + if(n > 0) md5.update(buf, n); + } while(!feof(lp)); + + stream->printf("%s %s\n", md5.finalize().hexdigest().c_str(), filename.c_str()); + fclose(lp); +} + + + void SimpleShell::help_command( string parameters, StreamOutput *stream ) { stream->printf("Commands:\r\n"); @@ -631,5 +721,9 @@ void SimpleShell::help_command( string parameters, StreamOutput *stream ) stream->printf("net\r\n"); stream->printf("load [file] - loads a configuration override file from soecified name or config-override\r\n"); stream->printf("save [file] - saves a configuration override file as specified filename or as config-override\r\n"); + stream->printf("upload filename - saves a stream of text to the named file\r\n"); + stream->printf("calc_thermistor [-s0] T1,R1,T2,R2,T3,R3 - calculate the Steinhart Hart coefficients for a thermistor\r\n"); + stream->printf("thermistors - print out the predefined thermistors\r\n"); + stream->printf("md5sum file - prints md5 sum of the given file\r\n"); } diff --git a/src/modules/utils/simpleshell/SimpleShell.h b/src/modules/utils/simpleshell/SimpleShell.h index 68f22ea2..e1ec86d5 100644 --- a/src/modules/utils/simpleshell/SimpleShell.h +++ b/src/modules/utils/simpleshell/SimpleShell.h @@ -26,6 +26,7 @@ public: void on_console_line_received( void *argument ); void on_gcode_received(void *argument); void on_second_tick(void *); + static bool parse_command(const char *cmd, string args, StreamOutput *stream); private: static void ls_command(string parameters, StreamOutput *stream ); @@ -43,6 +44,10 @@ private: static void version_command(string parameters, StreamOutput *stream ); static void get_command(string parameters, StreamOutput *stream ); static void set_temp_command(string parameters, StreamOutput *stream ); + static void calc_thermistor_command( string parameters, StreamOutput *stream); + static void print_thermistors_command( string parameters, StreamOutput *stream); + static void md5sum_command( string parameters, StreamOutput *stream); + static void switch_command(string parameters, StreamOutput *stream ); static void mem_command(string parameters, StreamOutput *stream ); @@ -53,7 +58,6 @@ private: static void remount_command( string parameters, StreamOutput *stream); - bool parse_command(const char *cmd, string args, StreamOutput *stream); typedef void (*PFUNC)(string parameters, StreamOutput *stream); typedef struct { diff --git a/src/testframework/Readme.md b/src/testframework/Readme.md new file mode 100644 index 00000000..defffe5f --- /dev/null +++ b/src/testframework/Readme.md @@ -0,0 +1,57 @@ +# Test framework + +## Background + + +This is a frame work for writing unit tests for modules or libs. +It compiles only src/libs/... and a few select needed modules by default. +It runs on the target and the output is written to the serial UART, it runs well with MRI so you can also debug with GDB. + +You can edit a file to specify which module you are going to test by default the example has tools/temperaturecontrol selected. + +The unit tests go in a directory named src/testframework/unittests/*XXXX*/*YYYY* where *XXXX* is the module subdirectory (tools,utils) and *YYYY* is the modulename + +The Kernel and main are replaced with special versions for testing. + +The main way to test is to stimulate the modules by sending it commands via +on_gcode_received() (or other events it is registered for), and monitor its +actions my mocking the ON_PUBLIC_SET/GET calls it makes. A properly written +module does not access other modules any other way. + +Many HAL calls are made via THEKERNEL->xxx (eg THEKERNEL->adc) and these can be intercepted by mocks to provide the module with its external data. + +An example is provided here... + +`src/testframework/unittests/tools/temperatureswitch/TEST_TemperatureSwitch.cpp` + +## Usage + +You compile a unit test and framework using [rake](http://rake.rubyforge.org/)... + +```shell +> rake testing=1 +> rake upload +``` + +The unit test will run, the results are printed to the serial uart port, and then it is left in DFU mode so `rake upload` can be run again for the next test. + +You select which tests and modules you want to compile by creating a file called rakefile.defaults in the same directory as the Rakefile is found. + +```shell +TESTMODULES= %w(tools/temperatureswitch libs) +``` + +Here we enable unit tests for the tools/temperatureswitch module and any unit tests in src/testframework/unittests/tools/temperatureswitch/. +Also any unittests in the src/testframework/unittests/libs folder will be included + +In this case all files in src/libs/... are compiled anyway so we only need to enable unit tests in the src/testframework/unittests/libs folder. + +the tools/temperatureswitch both compiles all files in the src/modules/tools/temperatureswitch/... directory and enables the +unit tests in src/testframework/unittests/tools/temperatureswitch/... + +by default no other files in the src/modules/... directory tree are compiled unless specified above. + + + + + diff --git a/src/testframework/Test_kernel.cpp b/src/testframework/Test_kernel.cpp new file mode 100644 index 00000000..313cc19b --- /dev/null +++ b/src/testframework/Test_kernel.cpp @@ -0,0 +1,135 @@ +/* + 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 . +*/ + +/** +This is aprt of the Smoothie test framework, it generates a Mockable Kernl so kernel calls can be tested for +*/ + +#include "libs/Kernel.h" +#include "libs/Module.h" +#include "libs/Config.h" +#include "libs/nuts_bolts.h" +#include "libs/SlowTicker.h" +#include "libs/Adc.h" +#include "libs/StreamOutputPool.h" +#include +#include "checksumm.h" +#include "ConfigValue.h" + +#include "libs/StepTicker.h" +#include "libs/PublicData.h" +#include "modules/communication/SerialConsole.h" +#include "modules/communication/GcodeDispatch.h" +#include "modules/robot/Planner.h" +#include "modules/robot/Robot.h" +#include "modules/robot/Stepper.h" +#include "modules/robot/Conveyor.h" + +#include "Config.h" +#include "FirmConfigSource.h" + +#include +#include +#include +#include + +Kernel* Kernel::instance; + +// The kernel is the central point in Smoothie : it stores modules, and handles event calls +Kernel::Kernel(){ + instance= this; // setup the Singleton instance of the kernel + + // serial first at fixed baud rate (DEFAULT_SERIAL_BAUD_RATE) so config can report errors to serial + // Set to UART0, this will be changed to use the same UART as MRI if it's enabled + this->serial = new SerialConsole(USBTX, USBRX, DEFAULT_SERIAL_BAUD_RATE); + + // Config next, but does not load cache yet + // loads config from in memory source for test framework must be loaded by test + this->config = nullptr; + + this->streams = new StreamOutputPool(); + this->streams->append_stream(this->serial); + + this->current_path = "/"; + + this->slow_ticker = new SlowTicker(); + + // dummies (would be nice to refactor to not have to create a conveyor) + this->conveyor= new Conveyor(); + + // 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); + NVIC_SetPriority(UART0_IRQn, 5); +} + +// Add a module to Kernel. We don't actually hold a list of modules we just call its on_module_loaded +void Kernel::add_module(Module* module){ + module->on_module_loaded(); +} + +// Adds a hook for a given module and event +void Kernel::register_for_event(_EVENT_ENUM id_event, Module *mod){ + this->hooks[id_event].push_back(mod); +} + +static std::map<_EVENT_ENUM, std::function > event_callbacks; + +// Call a specific event with an argument +void Kernel::call_event(_EVENT_ENUM id_event, void * argument){ + for (auto m : hooks[id_event]) { + (m->*kernel_callback_functions[id_event])(argument); + } + if(event_callbacks.find(id_event) != event_callbacks.end()){ + event_callbacks[id_event](argument); + }else{ + printf("call_event for event: %d not handled\n", id_event); + } +} + +// These are used by tests to test for various things. basically mocks +bool Kernel::kernel_has_event(_EVENT_ENUM id_event, Module *mod) +{ + for (auto m : hooks[id_event]) { + if(m == mod) return true; + } + return false; +} + +void Kernel::unregister_for_event(_EVENT_ENUM id_event, Module *mod) +{ + for (auto i = hooks[id_event].begin(); i != hooks[id_event].end(); ++i) { + if(*i == mod) { + hooks[id_event].erase(i); + return; + } + } +} + +void test_kernel_setup_config(const char* start, const char* end) +{ + THEKERNEL->config= new Config(new FirmConfigSource("rom", start, end) ); + // Pre-load the config cache + THEKERNEL->config->config_cache_load(); +} + +void test_kernel_teardown() +{ + delete THEKERNEL->config; + THEKERNEL->config= nullptr; + event_callbacks.clear(); +} + +void test_kernel_trap_event(_EVENT_ENUM id_event, std::function fnc) +{ + event_callbacks[id_event]= fnc; +} + +void test_kernel_untrap_event(_EVENT_ENUM id_event) +{ + event_callbacks.erase(id_event); +} diff --git a/src/testframework/Test_kernel.h b/src/testframework/Test_kernel.h new file mode 100644 index 00000000..1c6a0c51 --- /dev/null +++ b/src/testframework/Test_kernel.h @@ -0,0 +1,9 @@ +#pragma once + +#include "Module.h" +#include + +void test_kernel_setup_config(const char* start, const char* end); +void test_kernel_teardown(); +void test_kernel_trap_event(_EVENT_ENUM id_event, std::function fnc); +void test_kernel_untrap_event(_EVENT_ENUM id_event); diff --git a/src/testframework/Test_main.cpp b/src/testframework/Test_main.cpp new file mode 100644 index 00000000..e1ee90d4 --- /dev/null +++ b/src/testframework/Test_main.cpp @@ -0,0 +1,35 @@ +#include +#include +#include + +#include "utils.h" +#include "SerialConsole.h" +#include "gpio.h" + +GPIO leds[5] = { + GPIO(P1_18), + GPIO(P1_19), + GPIO(P1_20), + GPIO(P1_21), + GPIO(P4_28) +}; + +#include "easyunit/testharness.h" +#include "easyunit/test.h" + +int main( ) +{ + Kernel* kernel = new Kernel(); + + printf("Starting tests...\n"); + + TestRegistry::runAndPrint(); + + kernel->serial->printf("Done\n"); + + // drop back into DFU upload + kernel->serial->printf("Entering DFU flash mode...\n"); + system_reset(true); + + for(;;) {} +} diff --git a/src/testframework/easyunit/defaulttestprinter.cpp b/src/testframework/easyunit/defaulttestprinter.cpp new file mode 100644 index 00000000..a7ee684a --- /dev/null +++ b/src/testframework/easyunit/defaulttestprinter.cpp @@ -0,0 +1,215 @@ +/* +EasyUnit : Simple C++ Unit testing framework +Copyright (C) 2004 Barthelemy Dagenais + +This library is free software; you can redistribute it and/or +modify it under the terms of the GNU Lesser General Public +License as published by the Free Software Foundation; either +version 2.1 of the License, or (at your option) any later version. + +This library 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 +Lesser General Public License for more details. + +You should have received a copy of the GNU Lesser General Public +License along with this library; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + +Barthelemy Dagenais +barthelemy@prologique.com +*/ + +#include "defaulttestprinter.h" + +#include "testpartresult.h" + +#include + + +DefaultTestPrinter::DefaultTestPrinter() +: testsTotal_(0),testFailuresTotal_(0),failuresTotal_(0), + level_(normal), showSuccessDetail_(false), output_(stdout) +{ +} + +DefaultTestPrinter::~DefaultTestPrinter() +{ +} + +void DefaultTestPrinter::print(const TestResult *testResult) +{ + int failures; + int successes; + int errors; + SimpleString state; + SimpleString name; + TestCase *testCase = testResult->getTestCases(); + int size = testResult->getTestCaseCount(); + + printHeader(testResult); + + if (testResult->getTestCaseRanCount() == 0) { + fprintf(output_,"\nNo test ran\n"); + } + + for (int i=0;iran()) { + + name = testCase->getName(); + failures = testCase->getFailuresCount(); + successes = testCase->getSuccessesCount(); + errors = testCase->getErrorsCount(); + + if (failures > 0 || errors > 0) { + state = "FAILED"; + } + else { + state = "SUCCEEDED"; + } + + fprintf(output_, "\n\nTest case \"%s\" %s with %d error(s), %d failure(s) and %d success(es): \n",name.asCharString(),state.asCharString(),errors,failures,successes); + + printTests(testCase); + } + + testCase = testCase->getNext(); + } +} + +void DefaultTestPrinter::setHeaderLevel(headerLevel level) +{ + level_ = level; +} + +void DefaultTestPrinter::showSuccessDetail(bool show) +{ + showSuccessDetail_ = show; +} + +void DefaultTestPrinter::setOutput(FILE *output) +{ + output_ = output; +} + +void DefaultTestPrinter::printHeader(const TestResult *testResult) +{ + fprintf(output_ , "-- EasyUnit Results --\n"); + + if (level_ != off) { + fprintf(output_ , "\nSUMMARY\n\n"); + fprintf(output_ , "Test summary: "); + + if (testResult->getErrors() > 0 || testResult->getFailures() > 0) { + fprintf(output_ , "FAIL\n"); + } + else { + fprintf(output_ , "SUCCESS\n"); + } + + if (level_ == normal) { + printNormalHeader(testResult); + } + else { + printCompleteHeader(testResult); + } + } + + fprintf(output_ , "\n"); + fprintf(output_ , "\nDETAILS"); +} + +void DefaultTestPrinter::printCompleteHeader(const TestResult *testResult) +{ + fprintf(output_ , "Number of test cases: %d\n",testResult->getTestCaseCount()); + fprintf(output_ , "Number of test cases ran: %d\n",testResult->getTestCaseRanCount()); + fprintf(output_ , "Test cases that succeeded: %d\n",testResult->getSuccesses()); + fprintf(output_ , "Test cases with errors: %d\n",testResult->getErrors()); + fprintf(output_ , "Test cases that failed: %d\n",testResult->getFailures()); + fprintf(output_ , "Number of tests ran: %d\n",testResult->getTestRanCount()); + fprintf(output_ , "Tests that succeeded: %d\n",testResult->getTotalSuccesses()); + fprintf(output_ , "Tests with errors: %d\n",testResult->getTotalErrors()); + fprintf(output_ , "Tests that failed: %d\n",testResult->getTotalFailures()); + +} + +void DefaultTestPrinter::printNormalHeader(const TestResult *testResult) +{ + fprintf(output_ , "Number of test cases ran: %d\n",testResult->getTestCaseRanCount()); + fprintf(output_ , "Test cases that succeeded: %d\n",testResult->getSuccesses()); + fprintf(output_ , "Test cases with errors: %d\n",testResult->getErrors()); + fprintf(output_ , "Test cases that failed: %d\n",testResult->getFailures()); +} + +void DefaultTestPrinter::printTests(TestCase *testCase) +{ + const char *indent = " "; + Test *test = testCase->getTests(); + int size = testCase->getTestsCount(); + SimpleString state; + + + + for (int i=0;igetFailuresCount() > 0 || test->getErrorsCount() > 0) { + state = "FAILED :"; + } + else { + state = "SUCCEEDED!"; + } + + fprintf(output_, "%s Test \"%s\" %s\n",indent,test->getTestName().asCharString(),state.asCharString()); + printResults(test); + test = test->getNext(); + } +} + +void DefaultTestPrinter::printResults(Test *test) +{ + const char *indent = " "; + TestPartResult *testPR = test->getTestPartResult(); + int size = test->getFailuresCount() + test->getSuccessesCount() + test->getErrorsCount(); + int type; + + for (int i=0;igetType(); + + if (type == failure) { + fprintf (output_, "%s%s%s%s%s%ld%s%s\n", + indent, + "Failure: \"", + testPR->getMessage().asCharString (), + "\" " , + "line ", + testPR->getLineNumber(), + " in ", + testPR->getFileName().asCharString ()); + } + else if (type == error) { + fprintf (output_, "%s%s%s%s%s%s\n", + indent, + "Error in ", + test->getTestName().asCharString(), + ": \"", + testPR->getMessage().asCharString (), + "\""); + } + else if (type == success && showSuccessDetail_) { + fprintf (output_, "%s%s%s%s%s%ld%s%s\n", + indent, + "Success: \"", + testPR->getMessage().asCharString (), + "\" " , + "line ", + testPR->getLineNumber(), + " in ", + testPR->getFileName().asCharString ()); + } + testPR = testPR->getNext(); + } +} + + + diff --git a/src/testframework/easyunit/defaulttestprinter.h b/src/testframework/easyunit/defaulttestprinter.h new file mode 100644 index 00000000..3824430d --- /dev/null +++ b/src/testframework/easyunit/defaulttestprinter.h @@ -0,0 +1,125 @@ +/* +EasyUnit : Simple C++ Unit testing framework +Copyright (C) 2004 Barthelemy Dagenais + +This library is free software; you can redistribute it and/or +modify it under the terms of the GNU Lesser General Public +License as published by the Free Software Foundation; either +version 2.1 of the License, or (at your option) any later version. + +This library 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 +Lesser General Public License for more details. + +You should have received a copy of the GNU Lesser General Public +License along with this library; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + +Barthelemy Dagenais +barthelemy@prologique.com +*/ + +#ifndef DEFAULTTESTPRINTER_H +#define DEFAULTTESTPRINTER_H + +#include "testprinter.h" +#include "testcase.h" +#include "test.h" +#include "testresult.h" +#include + + +/** + * Complete header level means that a header will be printed + * before the test details with all information available in + * the test result. + * + * Normal header level means that a header will be printed + * before the test details with the most useful information + * available in the test result. + * + * Off header level means that no header will be printed + * before the test details. + * + * Whatever the level, there will always be a clear indication + * telling if there was a failure/error or not at the global + * level. + */ +enum headerLevel {complete,normal,off}; + +/** + * This is the default testprinter used by easyunit testregistry + * when the user calls the runAndPrint() method without specifying + * a testprinter. + * + * This testprinter writes plain text result to any supplied file. + * The default file is the standard output. + * + * You may customize the outpur format by specifying the header level + * and if you wish the testprinter to print details about each success. + * + * The default header level is normal and by default, the testprinter + * does not print details about each success. + */ +class DefaultTestPrinter : public TestPrinter +{ + public: + + /** + * Default constructor that sets the header level + * to normal and the output source to the standard + * output. + */ + DefaultTestPrinter(); + + /** + * Empty destructor. + */ + virtual ~DefaultTestPrinter(); + /** + * Prints a header depending of the header level and + * details about each test to the output_. + * + * @param testResult Results of all tests that were ran. + */ + virtual void print(const TestResult *testResult); + + /** + * Set the header level of the printer. + * + * @param level Header level that will be used during print() + */ + void setHeaderLevel(headerLevel level); + + /** + * Set whether or not the printer should display the details + * of test that succeeded. + * + * @param show Set to true to display details about success + */ + void showSuccessDetail(bool show); + + /** + * Set the output to which the printer will print results. + * + * @param output Output used to print the results + */ + void setOutput(FILE *output); + + protected: + virtual void printHeader(const TestResult *testResult); + virtual void printTests(TestCase *testCase); + virtual void printResults(Test *test); + virtual void printCompleteHeader(const TestResult *testResult); + virtual void printNormalHeader(const TestResult *testResult); + int testsTotal_; + int testFailuresTotal_; + int failuresTotal_; + headerLevel level_; + bool showSuccessDetail_; + FILE *output_; +}; + +#endif // DEFAULTTESTPRINTER_H + diff --git a/src/testframework/easyunit/simplestring.cpp b/src/testframework/easyunit/simplestring.cpp new file mode 100644 index 00000000..326f530f --- /dev/null +++ b/src/testframework/easyunit/simplestring.cpp @@ -0,0 +1,153 @@ +/* +EasyUnit : Simple C++ Unit testing framework +Copyright (C) 2004 Barthelemy Dagenais + +This library is free software; you can redistribute it and/or +modify it under the terms of the GNU Lesser General Public +License as published by the Free Software Foundation; either +version 2.1 of the License, or (at your option) any later version. + +This library 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 +Lesser General Public License for more details. + +You should have received a copy of the GNU Lesser General Public +License along with this library; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + +Barthelemy Dagenais +barthelemy@prologique.com +*/ + +#include "simplestring.h" +#include +#include +#include + + +static const int DEFAULT_SIZE = 20; + +SimpleString::SimpleString () +: buffer(new char [1]) +{ + buffer [0] = '\0'; +} + + +SimpleString::SimpleString (const char *otherBuffer) +: buffer (new char [strlen (otherBuffer) + 1]) +{ + strcpy (buffer, otherBuffer); +} + +SimpleString::SimpleString (const SimpleString& other) +{ + buffer = new char [other.size() + 1]; + strcpy(buffer, other.buffer); +} + + +SimpleString SimpleString::operator= (const SimpleString& other) +{ + delete buffer; + buffer = new char [other.size() + 1]; + strcpy(buffer, other.buffer); + return *this; +} + +SimpleString SimpleString::operator+ (const SimpleString& other) +{ + SimpleString newS; + delete [] newS.buffer; + newS.buffer = new char[this->size()+other.size()+1]; + strcpy(newS.buffer,this->asCharString()); + newS.buffer= strcat(newS.buffer,other.asCharString()); + return newS; +} + +char *SimpleString::asCharString () const +{ + return buffer; +} + +int SimpleString::size() const +{ + return strlen (buffer); +} + +SimpleString::~SimpleString () +{ + delete [] buffer; +} + +bool operator== (const SimpleString& left, const SimpleString& right) +{ + return !strcmp (left.asCharString (), right.asCharString ()); +} + +bool operator!= (const SimpleString& left, const SimpleString& right) +{ + return !(left == right); +} + +SimpleString StringFrom (bool value) +{ + char buffer [sizeof ("false") + 1]; + sprintf (buffer, "%s", value ? "true" : "false"); + return SimpleString(buffer); +} + +SimpleString StringFrom (const char *value) +{ + return SimpleString(value); +} + +SimpleString StringFrom (long value) +{ + char buffer [DEFAULT_SIZE]; + sprintf (buffer, "%ld", value); + + return SimpleString(buffer); +} + +SimpleString StringFrom (int value) +{ + char buffer [DEFAULT_SIZE]; + sprintf (buffer, "%d", value); + + return SimpleString(buffer); +} + +SimpleString StringFrom (unsigned int value) +{ + char buffer [DEFAULT_SIZE]; + sprintf (buffer, "%u", value); + + return SimpleString(buffer); +} + +SimpleString StringFrom (double value) +{ + char buffer [DEFAULT_SIZE]; + sprintf (buffer, "%lf", value); + + return SimpleString(buffer); +} + +SimpleString StringFrom (float value) +{ + char buffer [DEFAULT_SIZE]; + sprintf (buffer, "%f", value); + + return SimpleString(buffer); +} + +SimpleString StringFrom (const SimpleString& value) +{ + return SimpleString(value); +} + + + + diff --git a/src/testframework/easyunit/simplestring.h b/src/testframework/easyunit/simplestring.h new file mode 100644 index 00000000..857a3a72 --- /dev/null +++ b/src/testframework/easyunit/simplestring.h @@ -0,0 +1,72 @@ +/* +EasyUnit : Simple C++ Unit testing framework +Copyright (C) 2004 Barthelemy Dagenais + +This library is free software; you can redistribute it and/or +modify it under the terms of the GNU Lesser General Public +License as published by the Free Software Foundation; either +version 2.1 of the License, or (at your option) any later version. + +This library 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 +Lesser General Public License for more details. + +You should have received a copy of the GNU Lesser General Public +License along with this library; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + +Barthelemy Dagenais +barthelemy@prologique.com + +This class was originally created by Michael Feathers and was modified +by Barthelemy Dagenais. +*/ + + +#ifndef SIMPLE_STRING +#define SIMPLE_STRING + + +/** + * SimpleString is a simple implementation of the std class String and is + * provided to ease the manipulation of strings without using any other + * libraries. + */ +class SimpleString +{ + friend bool operator== (const SimpleString& left, const SimpleString& right); + + friend bool operator!= (const SimpleString& left, const SimpleString& right); + + public: + SimpleString (); + SimpleString (const char *value); + SimpleString (const SimpleString& other); + ~SimpleString (); + + SimpleString operator= (const SimpleString& other); + + SimpleString operator+ (const SimpleString& other); + + char *asCharString () const; + int size() const; + + private: + char *buffer; +}; + +// Those functions are provided to ease the conversion between +// primary datatypes and SimpleString. Feel free to extend this list +// to support your own datatype. +SimpleString StringFrom (bool value); +SimpleString StringFrom (const char *value); +SimpleString StringFrom (long value); +SimpleString StringFrom (int value); +SimpleString StringFrom (unsigned int value); +SimpleString StringFrom (float value); +SimpleString StringFrom (double value); +SimpleString StringFrom (const SimpleString& other); + +#endif + diff --git a/src/testframework/easyunit/test.cpp b/src/testframework/easyunit/test.cpp new file mode 100644 index 00000000..a9453a40 --- /dev/null +++ b/src/testframework/easyunit/test.cpp @@ -0,0 +1,143 @@ +/* +EasyUnit : Simple C++ Unit testing framework +Copyright (C) 2004 Barthelemy Dagenais + +This library is free software; you can redistribute it and/or +modify it under the terms of the GNU Lesser General Public +License as published by the Free Software Foundation; either +version 2.1 of the License, or (at your option) any later version. + +This library 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 +Lesser General Public License for more details. + +You should have received a copy of the GNU Lesser General Public +License along with this library; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + +Barthelemy Dagenais +barthelemy@prologique.com +*/ + +#include "test.h" +#include "testregistry.h" + + +Test::Test(const SimpleString& testCaseName, const SimpleString& testName) +: testCaseName_(testCaseName), testName_(testName), testPartResult_(0), nextTest_(0), failuresCount_(0), + successesCount_(0) +{ + TestRegistry::addTest(this); +} + +Test::~Test() { + TestPartResult *tmp; + int size = failuresCount_ + successesCount_; + + for (int i = 0; igetNext(); + delete tmp; + } +} + +void Test::setUp() +{ +} + +void Test::tearDown() +{ +} + +void Test::run() +{ +} + + +TestCase* Test::getTestCase() const +{ + return testCase_; +} + + +void Test::setTestCase(TestCase *testCase) +{ + testCase_ = testCase; +} + +void Test::addTestPartResult(TestPartResult *testPartResult) +{ + TestPartResult *tmp; + int type = testPartResult->getType(); + + if (testPartResult_ == 0) { + testPartResult_ = testPartResult; + testPartResult_->setNext(testPartResult_); + } + else { + tmp = testPartResult_; + testPartResult_ = testPartResult; + testPartResult_->setNext(tmp->getNext()); + tmp->setNext(testPartResult_); + } + + if (type == failure) { + failuresCount_++; + } + else if (type == error) { + errorsCount_++; + } + else { + successesCount_++; + } +} + +TestPartResult* Test::getTestPartResult() const +{ + TestPartResult *tpr = testPartResult_; + + if (tpr != 0) { + tpr = tpr->getNext(); + } + + return tpr; +} + +int Test::getFailuresCount() const +{ + return failuresCount_; +} + +int Test::getSuccessesCount() const +{ + return successesCount_; +} + +int Test::getErrorsCount() const +{ + return errorsCount_; +} + +void Test::setNext(Test *nextTest) +{ + nextTest_ = nextTest; +} + + +Test* Test::getNext() const +{ + return nextTest_; +} + +const SimpleString& Test::getTestName() const +{ + return testName_; +} + +const SimpleString& Test::getTestCaseName() const +{ + return testCaseName_; +} + + diff --git a/src/testframework/easyunit/test.h b/src/testframework/easyunit/test.h new file mode 100644 index 00000000..d33e88b6 --- /dev/null +++ b/src/testframework/easyunit/test.h @@ -0,0 +1,433 @@ +/* +EasyUnit : Simple C++ Unit testing framework +Copyright (C) 2004 Barthelemy Dagenais + +This library is free software; you can redistribute it and/or +modify it under the terms of the GNU Lesser General Public +License as published by the Free Software Foundation; either +version 2.1 of the License, or (at your option) any later version. + +This library 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 +Lesser General Public License for more details. + +You should have received a copy of the GNU Lesser General Public +License along with this library; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + +Barthelemy Dagenais +barthelemy@prologique.com +*/ + +#ifndef TEST_H +#define TEST_H + +#include "testcase.h" +#include "testpartresult.h" + + + + + +/** + * EasyUnit namespace. + * This is the namespace containing all easyunit classes. + */ + +/** + * Test class containing all macros to do unit testing. + * A test object represents a test that will be executed. Once it has been + * executed, it reports all results in the testPartResult linked list. + * + * A failure occurs when a test fails (condition is false). + * An error occurs when an exception is thrown during a test. + * A success occurs if a test succeed (condition is true). + */ +class Test +{ + public: + + /** + * Main Test constructor. Used to create a test that will register itself + * with TestRegistry and with its test case. + * @param testCaseName Name of the test case this test belongs to + * @param testName Name of this test + */ + Test(const SimpleString& testCaseName, const SimpleString& testName); + + /** + * Main Test desctructor + * Delete the testPartResult linked list. This is why the user should + * only use the macro provided by easyunit to report a test result. + */ + virtual ~Test(); + + /** + * Fixtures that will be called after run(). + */ + virtual void tearDown(); + + /** + * Fixtures that will be called before run(). + */ + virtual void setUp(); + + /** + * Test code should be in this method. + * run() will be called by the Test's TestCase, hence subclasses of Test + * should override this method. + */ + virtual void run(); + + /** + * Set the TestCase this test belongs to. + * + * @param testCase The TestCase this test belongs to + */ + void setTestCase(TestCase *testCase); + + /** + * Get the TestCase this test belongs to. A test always belongs to + * only one TestCase. This is the TestCase identified by the first + * parameter of the test declaration. For example, if there is a + * test declared as TEST(TESTCASE1, TEST1), this test will be + * associated with the TestCase TESTCASE1. + * + * @return The TestCase this test belongs to + */ + TestCase* getTestCase() const; + + /** + * Add a testpartresult to the testpartresult list of this test. + * This method is used by the assertion macros to report success, + * failure or error. + * + * @param testPartResult The testpartresult to be added to the list + */ + virtual void addTestPartResult(TestPartResult *testPartResult); + + /** + * Get the testpartresult list of this test. If assertion macros + * and TEST and TESTF macros are used, there may be more than + * one successful testpartresult and no more than one error or failure. + * + * @return testPartResult The list of testpartresults of this test + */ + TestPartResult* getTestPartResult() const; + + /** + * Returns number of failures found in this test. + * If macro TEST or TESTF is used, failuresCount <= 1. + * If Test class is extended and ASSERT macros are used in different + * test methods, than failuresCount may be more than 1. + * + * @return Number of failures in this test + */ + int getFailuresCount() const; + + /** + * Returns number of successes found in this test. + * There may be more than one success since each ASSERT macro + * that succeeded generate a success. + * + * @return Number of successes in this test + */ + int getSuccessesCount() const; + + /** + * Returns number of errors found in this test. + * ErrorsCount <= 1, since exception are caught + * for the whole run() method. + * + * @return Number of errors in this test + */ + int getErrorsCount() const; + + + /** + * Set the next test in the linked list. + * + * @param nextTest Next test in the linked list + */ + void setNext(Test *nextTest); + + /** + * Get the next test in the linked list. + * + * @return The next test in the linked list + */ + Test* getNext() const; + + /** + * Get the name of the TestCase this test belongs to. The name of the + * TestCase is the first parameter of the test declaration. For example, + * if a test is declared as TEST(TESTCASE1, TEST1), this method will return + * "TESTCASE1". + * + * @return The TestCase name of this test + */ + const SimpleString& getTestCaseName() const; + + /** + * Get the name of this test. The name of the test is the second + * parameter of the test declaration. For example, + * if a test is declared as TEST(TESTCASE1, TEST1), this method will return + * "TEST1". + * + * @return The name of this test. + */ + const SimpleString& getTestName() const; + + protected: + SimpleString testCaseName_; + SimpleString testName_; + TestCase *testCase_; + TestPartResult *testPartResult_; + Test *nextTest_; + int failuresCount_; + int successesCount_; + int errorsCount_; +}; + + + + +/* + * Helper macros + */ + +#define EQUALS_DELTA(expected,actual,delta)\ + ((actual - expected) <= delta && actual >= expected) || ((expected - actual) <= delta && expected >= actual) + +#define TO_STRING_EQUALS_F(expected,actual)\ + StringFrom("Expected : ") + StringFrom(expected) + StringFrom(" but Actual : ") + StringFrom(actual) + +#define TO_STRING_EQUALS_S(expected,actual)\ + StringFrom(expected) + StringFrom(" == ") + StringFrom(actual) + +#define TO_S_E_DELTA_F(expected,actual,delta)\ + StringFrom("Expected : ") + StringFrom(expected) + StringFrom(" but Actual : ") + StringFrom(actual) + StringFrom(" with delta = ") + StringFrom(delta) + +#define TO_S_E_DELTA_S(expected,actual,delta)\ + StringFrom(expected) + StringFrom(" == ") + StringFrom(actual) + StringFrom(" with delta = ") + StringFrom(delta) + +/** + * Asserts that a condition is true. + * If the condition is not true, a failure is generated. + * @param condition Condition to fullfill for the assertion to pass + */ +#define ASSERT_TRUE(condition)\ + { if (condition) {\ + addTestPartResult(new TestPartResult(this, __FILE__,__LINE__,#condition,success));\ + } else {\ + addTestPartResult(new TestPartResult(this, __FILE__,__LINE__, #condition,failure)); return;\ + }} + +/** + * Asserts that a condition is true. + * If the condition is not true, a failure is generated. + * @param condition Condition to fullfill for the assertion to pass + * @param message Message that will be displayed if this assertion fails + */ +#define ASSERT_TRUE_M(condition,message)\ + { if (condition) {\ + addTestPartResult(new TestPartResult(this, __FILE__,__LINE__,#condition,success));\ + } else {\ + addTestPartResult(new TestPartResult(this, __FILE__,__LINE__, message,failure)); return;\ + }} + +/** + * Asserts that the two parameters are equals. Operator == must be defined. + * If the two parameters are not equals, a failure is generated. + * @param expected Expected value + * @param actual Actual value to be compared + */ +#define ASSERT_EQUALS(expected,actual)\ +{ if (expected == actual) {\ + addTestPartResult(new TestPartResult(this, __FILE__,__LINE__,TO_STRING_EQUALS_S(#expected,#actual),success));\ + } else {\ + addTestPartResult(new TestPartResult(this, __FILE__,__LINE__,TO_STRING_EQUALS_F(#expected,#actual),failure)); return;\ + }} + +/** + * Asserts that the two parameters are equals. Operator == must be defined. + * If the two parameters are not equals, a failure is generated. + * + * Parameters must be primitive data types or StringFrom (custom type) must + * be overloaded. + * + * @see SimpleString + * @param expected Expected value + * @param actual Actual value to be compared + */ +#define ASSERT_EQUALS_V(expected,actual)\ +{ if (expected == actual) {\ + addTestPartResult(new TestPartResult(this, __FILE__,__LINE__,TO_STRING_EQUALS_S(expected,actual),success));\ + } else {\ + addTestPartResult(new TestPartResult(this, __FILE__,__LINE__,TO_STRING_EQUALS_F(expected,actual),failure)); return;\ + }} + +/** + * Asserts that the two parameters are equals. Operator == must be defined. + * If the two parameters are not equals, a failure is generated. + * @param expected Expected value + * @param actual Actual value to be compared + * @param message Message that will be displayed if this assertion fails + */ +#define ASSERT_EQUALS_M(expected,actual,message)\ +{ if (expected == actual) {\ + addTestPartResult(new TestPartResult(this, __FILE__,__LINE__,#expected,success));\ + } else {\ + addTestPartResult(new TestPartResult(this, __FILE__,__LINE__,message,failure)); return;\ + }} + +/** + * Asserts that the two parameters are equals within a delta. Operators == and - must be defined. + * If the two parameters are not equals, a failure is generated. + * @param expected Expected value + * @param actual Actual value to be compared + * @param delta Delta accepted between the two values + */ +#define ASSERT_EQUALS_DELTA(expected,actual,delta)\ +{ if (EQUALS_DELTA(expected,actual,delta) ) {\ + addTestPartResult(new TestPartResult(this, __FILE__,__LINE__,TO_S_E_DELTA_S(#expected,#actual,#delta),success));\ + } else {\ + addTestPartResult(new TestPartResult(this, __FILE__,__LINE__,TO_S_E_DELTA_F(#expected,#actual,#delta),failure)); return;\ + }} + +/** + * Asserts that the two parameters are equals within a delta. Operators == and - must be defined. + * If the two parameters are not equals, a failure is generated. + * @param expected Expected value + * @param actual Actual value to be compared + * @param delta Delta accepted between the two values + * @param message Message that will be displayed if this assertion fails + */ +#define ASSERT_EQUALS_DELTA_M(expected,actual,delta,message)\ +{ if (EQUALS_DELTA(expected,actual,delta)) {\ + addTestPartResult(new TestPartResult(this, __FILE__,__LINE__,#expected,success));\ + } else {\ + addTestPartResult(new TestPartResult(this, __FILE__,__LINE__,message,failure)); return;\ + }} + +/** + * Asserts that the two parameters are equals within a delta. Operators == and - must be defined. + * If the two parameters are not equals, a failure is generated. + * + * Parameters must be primitive data types or StringFrom (custom type) must + * be overloaded. + * + * @see SimpleString + * @param expected Expected value + * @param actual Actual value to be compared + * @param delta Delta accepted between the two values + */ +#define ASSERT_EQUALS_DELTA_V(expected,actual,delta)\ +{ if (EQUALS_DELTA(expected,actual,delta)) {\ + addTestPartResult(new TestPartResult(this, __FILE__,__LINE__,TO_S_E_DELTA_S(expected,actual,delta),success));\ + } else {\ + addTestPartResult(new TestPartResult(this, __FILE__,__LINE__,TO_S_E_DELTA_F(expected,actual,delta),failure)); return;\ + }} + + +/** + * Make a test fails. + */ +#define FAIL()\ + { addTestPartResult(new TestPartResult(this, __FILE__, __LINE__,("Test failed."),failure)); return; } + +/** + * Make a test fails with the given message. + * @param text Failure message + */ +#define FAIL_M(text)\ + { addTestPartResult(new TestPartResult(this, __FILE__, __LINE__,text,failure)); return; } + + +/** + * Define a test in a TestCase. + * User should put his test code between brackets after using this macro. + * @param testCaseName TestCase name where the test belongs to + * @param testName Unique test name + */ +#define TEST(testCaseName, testName)\ + class testCaseName##testName##Test : public Test \ + { public: testCaseName##testName##Test() : Test (#testCaseName , #testName) {} \ + void run(); } \ + testCaseName##testName##Instance; \ + void testCaseName##testName##Test::run () + + +/** + * Define a test in a TestCase using test fixtures. + * User should put his test code between brackets after using this macro. + * + * This macro should only be used if test fixtures were declared earlier in + * this order: DECLARE, SETUP, TEARDOWN. + * @param testCaseName TestCase name where the test belongs to. Should be + * the same name of DECLARE, SETUP and TEARDOWN. + * @param testName Unique test name. + */ +#define TESTF(testCaseName, testName)\ + class testCaseName##testName##Test : public testCaseName##Declare##Test \ + { public: testCaseName##testName##Test() : testCaseName##Declare##Test (#testCaseName , #testName) {} \ + void run(); } \ + testCaseName##testName##Instance; \ + void testCaseName##testName##Test::run () + + +/** + * Setup code for test fixtures. + * This code is executed before each TESTF. + * + * User should put his setup code between brackets after using this macro. + * + * @param testCaseName TestCase name of the fixtures. + */ +#define SETUP(testCaseName)\ + void testCaseName##Declare##Test::setUp () + + +/** + * Teardown code for test fixtures. + * This code is executed after each TESTF. + * + * User should put his setup code between brackets after using this macro. + * + * @param testCaseName TestCase name of the fixtures. + */ +#define TEARDOWN(testCaseName)\ + void testCaseName##Declare##Test::tearDown () + + +/** + * Location to declare variables and objets. + * This is where user should declare members accessible by TESTF, + * SETUP and TEARDOWN. + * + * User should not use brackets after using this macro. User should + * not initialize any members here. + * + * @param testCaseName TestCase name of the fixtures + * @see END_DECLARE for more information. + */ +#define DECLARE(testCaseName)\ + class testCaseName##Declare##Test : public Test \ + { public: testCaseName##Declare##Test(const SimpleString& testCaseName, const SimpleString& testName) : Test (testCaseName , testName) {} \ + virtual void run() = 0; void setUp(); void tearDown(); \ + protected: + + +/** + * Ending macro used after DECLARE. + * + * User should use this macro after declaring members with + * DECLARE macro. + */ +#define END_DECLARE \ + }; + +#endif // TEST_H + + diff --git a/src/testframework/easyunit/testcase.cpp b/src/testframework/easyunit/testcase.cpp new file mode 100644 index 00000000..608f25d7 --- /dev/null +++ b/src/testframework/easyunit/testcase.cpp @@ -0,0 +1,170 @@ +#define ECPP +/* +EasyUnit : Simple C++ Unit testing framework +Copyright (C) 2004 Barthelemy Dagenais + +This library is free software; you can redistribute it and/or +modify it under the terms of the GNU Lesser General Public +License as published by the Free Software Foundation; either +version 2.1 of the License, or (at your option) any later version. + +This library 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 +Lesser General Public License for more details. + +You should have received a copy of the GNU Lesser General Public +License along with this library; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + +Barthelemy Dagenais +barthelemy@prologique.com +*/ + +#include "testcase.h" +#include "test.h" +#include "testresult.h" + +#ifndef ECPP +#include +#endif + + +TestCase::TestCase(const SimpleString& name, TestResult *testResult) +: name_(name), testResult_(testResult) +{ +} + +TestCase::~TestCase() +{ +} + +void TestCase::addTest(Test *test) +{ + Test *tmp; + + if (tests_ == 0) { + tests_ = test; + tests_->setNext(tests_); + } + else { + tmp = tests_; + tests_ = test; + tests_->setNext(tmp->getNext()); + tmp->setNext(tests_); + } + + testsCount_++; +} + +Test* TestCase::getTests() const +{ + Test *test = tests_; + + if (test != 0) { + test = test->getNext(); + } + + return test; +} + +void TestCase::run() +{ + Test *test = tests_->getNext(); + + runTests(test); + + ran_ = true; + + testResult_->addResult(this); +} + +int TestCase::getTestsCount() const +{ + return testsCount_; +} + +int TestCase::getFailuresCount() const +{ + return failuresCount_; +} + +int TestCase::getSuccessesCount() const +{ + return successesCount_; +} + +int TestCase::getErrorsCount() const +{ + return errorsCount_; +} + +bool TestCase::ran() const +{ + return ran_; +} + +const SimpleString& TestCase::getName() const +{ + return name_; +} + +void TestCase::updateCount(Test *test) +{ + if (test->getErrorsCount() > 0) { + errorsCount_++; + } + else if (test->getFailuresCount() > 0) { + failuresCount_++; + } + else { + successesCount_++; + } +} + +TestCase* TestCase::getNext() const +{ + return nextTestCase_; +} + +void TestCase::setNext(TestCase *testCase) +{ + nextTestCase_ = testCase; +} + +void TestCase::runTests(Test *test) +{ + + for (int i = 0; isetUp(); + runTest(test); + test->tearDown(); + updateCount(test); + test = test->getNext(); + } + +} + +#ifdef ECPP + +void TestCase::runTest(Test *test) +{ + test->run(); +} + +#else + +void TestCase::runTest(Test *test) +{ + try { + test->run(); + } + catch (std::exception &e) { + test->addTestPartResult(new TestPartResult(test,"",-1,e.what(),error)); + } + catch (...) { + test->addTestPartResult(new TestPartResult(test,"",-1,"Unexpected error occured",error)); + } +} +#endif + diff --git a/src/testframework/easyunit/testcase.h b/src/testframework/easyunit/testcase.h new file mode 100644 index 00000000..79438506 --- /dev/null +++ b/src/testframework/easyunit/testcase.h @@ -0,0 +1,154 @@ +/* +EasyUnit : Simple C++ Unit testing framework +Copyright (C) 2004 Barthelemy Dagenais + +This library is free software; you can redistribute it and/or +modify it under the terms of the GNU Lesser General Public +License as published by the Free Software Foundation; either +version 2.1 of the License, or (at your option) any later version. + +This library 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 +Lesser General Public License for more details. + +You should have received a copy of the GNU Lesser General Public +License along with this library; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + +Barthelemy Dagenais +barthelemy@prologique.com +*/ + +#include "simplestring.h" + +#ifndef TESTCASE_H +#define TESTCASE_H + + +class Test; +class TestResult; + +/** + * A TestCase is a collection of unit tests (instance of Test) and is + * always specified by the first parameter of a Test declaration. + */ +class TestCase +{ + public: + + /** + * Main TestCase constructor. + * + * @param name TestCase name + * @param testResult Pointer to the TestResult used to report results + * of executed Test + */ + TestCase(const SimpleString& name, TestResult *testResult); + + virtual ~TestCase(); + + /** + * Add a Test to the Test list. This method is used by TestRegistry. + * + * @param test Test instance to add to the Test list. + */ + void addTest(Test *test); + + /** + * Get the Test list. + * + * @return Test list + */ + Test* getTests() const; + + /** + * Execute all Tests in the Test list of this TestCase. In fact, it calls + * the run() method of all Tests. + */ + void run(); + + /** + * Get the Test list size (number of Tests in this TestCase). + * + * @return The Test list size + */ + int getTestsCount() const; + + /** + * Get the total number of failures reported by all Tests. + * + * @return The total number of failures reported by all Tests. 0 + * if no test were run or if no failures were reported. + */ + int getFailuresCount() const; + + /** + * Get the total number of successes reported by all Tests. + * + * @return The total number of successes reported by all Tests. 0 + * if no test were run or if no successes were reported. + */ + int getSuccessesCount() const; + + /** + * Get the total number of errors reported by all Tests. + * + * @return The total number of errors reported by all Tests. 0 + * if no test were run, if this is the embedded version or if + * no errors were reported. + */ + int getErrorsCount() const; + + /** + * Indicates whether or not this TestCase was executed. + * + * @return true if the method run() of this TestCase was called. false + * otherwise + */ + bool ran() const; + + /** + * Get the TestCase name. This name is specified by the first parameter + * of the Test declaration. For example, if a test was declared as + * TEST(TESTCASE1, TEST1), the TestCase name would be "TESTCASE1". + * + * @return The name of the TestCase + */ + const SimpleString& getName() const; + + /** + * Get the next TestCase in the list. + * + * @return The next TestCase in the TestCase linked list + */ + TestCase* getNext() const; + + /** + * Set the next TestCase in the list. + * + * @return The next TestCase in the TestCase linked list + */ + void setNext(TestCase *testCase); + + protected: + int failuresCount_{0}; + int successesCount_{0}; + int errorsCount_{0}; + int testsCount_{0}; + Test *tests_{0}; + SimpleString name_; + TestCase *nextTestCase_{0}; + TestResult *testResult_; + + private: + void updateCount(Test *test); + void runTests(Test *test); + void runTest(Test *test); + bool ran_{false}; + +}; + +#endif // TESTCASE_H + + diff --git a/src/testframework/easyunit/testharness.h b/src/testframework/easyunit/testharness.h new file mode 100644 index 00000000..e0a78ecd --- /dev/null +++ b/src/testframework/easyunit/testharness.h @@ -0,0 +1,38 @@ +/* +EasyUnit : Simple C++ Unit testing framework +Copyright (C) 2004 Barthelemy Dagenais + +This library is free software; you can redistribute it and/or +modify it under the terms of the GNU Lesser General Public +License as published by the Free Software Foundation; either +version 2.1 of the License, or (at your option) any later version. + +This library 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 +Lesser General Public License for more details. + +You should have received a copy of the GNU Lesser General Public +License along with this library; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + +Barthelemy Dagenais +barthelemy@prologique.com +*/ + +#ifndef TESTHARNESS_H +#define TESTHARNESS_H + +#include "test.h" +#include "testcase.h" +#include "testpartresult.h" +#include "testregistry.h" +#include "simplestring.h" +#include "testprinter.h" +#include "testresult.h" +#include "testrunner.h" +#include "defaulttestprinter.h" + +#endif + + diff --git a/src/testframework/easyunit/testpartresult.cpp b/src/testframework/easyunit/testpartresult.cpp new file mode 100644 index 00000000..f382c4a2 --- /dev/null +++ b/src/testframework/easyunit/testpartresult.cpp @@ -0,0 +1,67 @@ +/* +EasyUnit : Simple C++ Unit testing framework +Copyright (C) 2004 Barthelemy Dagenais + +This library is free software; you can redistribute it and/or +modify it under the terms of the GNU Lesser General Public +License as published by the Free Software Foundation; either +version 2.1 of the License, or (at your option) any later version. + +This library 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 +Lesser General Public License for more details. + +You should have received a copy of the GNU Lesser General Public +License along with this library; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + +Barthelemy Dagenais +barthelemy@prologique.com +*/ + +#include "testpartresult.h" +#include "test.h" + + +TestPartResult::TestPartResult (Test *test, + const SimpleString& fileName, + long lineNumber, + const SimpleString& message, + testType type) + : message_ (message), + test_ (test), + fileName_ (fileName), + lineNumber_ (lineNumber), + type_ (type) +{ +} + +void TestPartResult::setNext(TestPartResult *next) { + next_ = next; +} + +TestPartResult* TestPartResult::getNext() const { + return next_; +} + +testType TestPartResult::getType() const { + return type_; +} + +const SimpleString& TestPartResult::getMessage() const { + return message_; +} + +Test* TestPartResult::getTest() const { + return test_; +} + +const SimpleString& TestPartResult::getFileName() const { + return fileName_; +} + +long TestPartResult::getLineNumber() const { + return lineNumber_; +} + diff --git a/src/testframework/easyunit/testpartresult.h b/src/testframework/easyunit/testpartresult.h new file mode 100644 index 00000000..5f04a0b4 --- /dev/null +++ b/src/testframework/easyunit/testpartresult.h @@ -0,0 +1,131 @@ +/* +EasyUnit : Simple C++ Unit testing framework +Copyright (C) 2004 Barthelemy Dagenais + +This library is free software; you can redistribute it and/or +modify it under the terms of the GNU Lesser General Public +License as published by the Free Software Foundation; either +version 2.1 of the License, or (at your option) any later version. + +This library 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 +Lesser General Public License for more details. + +You should have received a copy of the GNU Lesser General Public +License along with this library; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + +Barthelemy Dagenais +barthelemy@prologique.com +*/ + +#ifndef TESTPARTRESULT_H +#define TESTPARTRESULT_H + +#include "simplestring.h" + + +/** + * This enumeration contains the three states a TestPartResult can take. + * + * A failure means that an assertion failed during the test. + * + * A success means that all assertion succeeded during the test. + * + * An error means that an exception was thrown during the test. + */ +enum testType {failure,success,error}; + +class Test; + +/** + * This class contains details about assertion processed during the + * execution of a Test. It contains the line and the file of the assertion, + * the result (success, failure or error), the condition (or message), and + * the Test class where the assertion was processed. + */ +class TestPartResult +{ + public: + /** + * Main constructor used to initialize all details about the result + * of an assertion. + * + * @param test The test where the assertion was processed + * @param fileName The file name where the assertion is located + * @param lineNumber The line number where the assertion is located + * @param message The assertion condition or message + * @param type The result of the assertion (failure, success or error) + */ + TestPartResult (Test *test, + const SimpleString& fileName, + long lineNumber, + const SimpleString& message, + testType type); + + /** + * Set the next TestPartResult in the list. + * + * @param next The next TestPartResult in the linked list + */ + void setNext(TestPartResult* next); + + /** + * Get the next TestPartResult in the list. + * + * @return The next TestPartResult in the linked list + */ + TestPartResult* getNext() const; + + /** + * Get the type of the TestPartResult. This represents the result + * of the assertion. + * + * @return The type of the TestPartResult (failure, success or error) + */ + testType getType() const; + + /** + * Get the message (or condition) of the assertion. + * + * @return The message (or condition) of the assertion + */ + const SimpleString& getMessage() const; + + /** + * Get the Test where the assertion is located. + * + * @return The Test where the assertion is located + */ + Test* getTest() const; + + /** + * Get the file name where the assertion is located. + * + * @return The file name where the assertion is located + */ + const SimpleString& getFileName() const; + + /** + * Get the line number where the assertion is located. + * + * @return The line number where the assertion is located + */ + long getLineNumber() const; + + + protected: + SimpleString message_; + Test *test_; + SimpleString fileName_; + long lineNumber_; + + private: + TestPartResult *next_; + testType type_; +}; + +#endif // TESTPARTRESULT_H + + diff --git a/src/testframework/easyunit/testprinter.h b/src/testframework/easyunit/testprinter.h new file mode 100644 index 00000000..4d2f8712 --- /dev/null +++ b/src/testframework/easyunit/testprinter.h @@ -0,0 +1,52 @@ +/* +EasyUnit : Simple C++ Unit testing framework +Copyright (C) 2004 Barthelemy Dagenais + +This library is free software; you can redistribute it and/or +modify it under the terms of the GNU Lesser General Public +License as published by the Free Software Foundation; either +version 2.1 of the License, or (at your option) any later version. + +This library 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 +Lesser General Public License for more details. + +You should have received a copy of the GNU Lesser General Public +License along with this library; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + +Barthelemy Dagenais +barthelemy@prologique.com +*/ + +#ifndef TESTPRINTER_H +#define TESTPRINTER_H + +#include "testresult.h" + + +/** + * A TestPrinter is a class used by the TestRegistry to print results + * of executed TestCases. This is an abstract class, so no default behavior + * for the print method is provided. + * + * @see DefaultTestPrinter + */ +class TestPrinter +{ + public: + virtual ~TestPrinter(){}; + /** + * Print the details of a given TestResult instance. This + * method must be overridden by subclasses since it is + * abstract. + * + * @param testResult TestResult instance that the user wish to print + */ + virtual void print(const TestResult *testResult) = 0; +}; + +#endif // TESTPRINTER_H + + diff --git a/src/testframework/easyunit/testregistry.cpp b/src/testframework/easyunit/testregistry.cpp new file mode 100644 index 00000000..aad38ba4 --- /dev/null +++ b/src/testframework/easyunit/testregistry.cpp @@ -0,0 +1,140 @@ +/* +EasyUnit : Simple C++ Unit testing framework +Copyright (C) 2004 Barthelemy Dagenais + +This library is free software; you can redistribute it and/or +modify it under the terms of the GNU Lesser General Public +License as published by the Free Software Foundation; either +version 2.1 of the License, or (at your option) any later version. + +This library 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 +Lesser General Public License for more details. + +You should have received a copy of the GNU Lesser General Public +License along with this library; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + +Barthelemy Dagenais +barthelemy@prologique.com +*/ + +#include "testregistry.h" +#include "defaulttestprinter.h" + + +int TestRegistry::nextName = 0; + +TestRegistry::TestRegistry() +: currentTC_(0), defaultPrinter_(new DefaultTestPrinter()),testCaseCount_(0), + defaultRunner_(new TestRunner()) +{ +} + +TestRegistry::~TestRegistry() +{ + TestCase *tmp; + for (int i = 0; igetNext(); + delete tmp; + } + + delete defaultPrinter_; + delete defaultRunner_; +} + +void TestRegistry::addTest(Test *test) +{ + instance().add(test); +} + +const TestResult* TestRegistry::run() +{ + return instance().runTests(instance().defaultRunner_); +} + +const TestResult* TestRegistry::run(TestRunner *runner) +{ + return instance().runTests(runner); +} + +const TestResult* TestRegistry::runAndPrint() +{ + return runAndPrint(instance().defaultPrinter_,instance().defaultRunner_); +} + +const TestResult* TestRegistry::runAndPrint(TestRunner *runner) +{ + return runAndPrint(instance().defaultPrinter_,runner); +} + +const TestResult* TestRegistry::runAndPrint(TestPrinter *printer) +{ + return runAndPrint(printer,instance().defaultRunner_); +} + + +const TestResult* TestRegistry::runAndPrint(TestPrinter *printer, TestRunner *runner) +{ + const TestResult *testResult = instance().runTests(runner); + printer->print(testResult); + return testResult; +} + + +TestRegistry& TestRegistry::instance() +{ + static TestRegistry registry; + return registry; +} + +void TestRegistry::add(Test *test) +{ + const SimpleString tcName = test->getTestCaseName(); + const SimpleString tName = test->getTestName(); + + if ((currentTC_ == 0) || (currentTC_->getName() != tcName)) { + addTestCase(new TestCase(tcName,&testResult_)); + } + + currentTC_->addTest(test); + +} + +const TestResult* TestRegistry::runTests(TestRunner *runner) +{ + TestCase *tc = currentTC_; + + if (tc != 0) { + tc = tc->getNext(); + runner->run(tc,testCaseCount_); + } + + testResult_.setTestCases(tc,testCaseCount_); + + return &testResult_; +} + + + +void TestRegistry::addTestCase(TestCase *testCase) +{ + TestCase *tmp; + + if (currentTC_ == 0) { + currentTC_ = testCase; + currentTC_->setNext(currentTC_); + } + else { + tmp = currentTC_; + currentTC_ = testCase; + currentTC_->setNext(tmp->getNext()); + tmp->setNext(currentTC_); + } + + testCaseCount_++; +} + + diff --git a/src/testframework/easyunit/testregistry.h b/src/testframework/easyunit/testregistry.h new file mode 100644 index 00000000..5d0d1df8 --- /dev/null +++ b/src/testframework/easyunit/testregistry.h @@ -0,0 +1,128 @@ +/* +EasyUnit : Simple C++ Unit testing framework +Copyright (C) 2004 Barthelemy Dagenais + +This library is free software; you can redistribute it and/or +modify it under the terms of the GNU Lesser General Public +License as published by the Free Software Foundation; either +version 2.1 of the License, or (at your option) any later version. + +This library 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 +Lesser General Public License for more details. + +You should have received a copy of the GNU Lesser General Public +License along with this library; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + +Barthelemy Dagenais +barthelemy@prologique.com +*/ + +#ifndef TESTREGISTRY_H +#define TESTREGISTRY_H + +#include "test.h" +#include "testcase.h" +#include "testprinter.h" +#include "simplestring.h" +#include "testrunner.h" +#include "testresult.h" + + +/** + * The TestRegistry is the main class used to register all tests, + * and create appropriate TestCase. It can then be used to run + * tests and print results. All methods that should be used by + * the user are static. + */ +class TestRegistry +{ + public: + TestRegistry(); + ~TestRegistry(); + + /** + * Add a test in the registry. If the previous TestCase was not the same + * as the one of the current test, a new TestCase is created. + * + * @param test Test to be added + */ + static void addTest (Test *test); + + /** + * Run all tests in the registry (default test runner) and return + * the test results. + * + * @return The test results + */ + static const TestResult* run(); + + /** + * Pass all tests in the registry to the TestRunner runner and + * return the results of all tests ran. + * + * @param runner The custom runner used to decided which test to run + * @return The test results of all tests ran + */ + static const TestResult* run(TestRunner *runner); + + /** + * Run all tests in the registry (default test runner) and return + * the test results. This will also print the results using the + * default test printer (normal level of details and to the standard + * output). + * + * @return The test results + */ + static const TestResult* runAndPrint(); + + /** + * Pass all tests in the registry to the TestRunner runner and + * return the results of all tests ran. This will also print the results + * using the default test printer (normal level of details and to the + * standard output). + * + * @param runner The custom runner used to decided which test to run + * @return The test results + */ + static const TestResult* runAndPrint(TestRunner *runner); + + /** + * Run all tests in the registry (default test runner) and return + * the test results. Results will also be given to + * to the TestPrinter printer. + * + * @param printer The custom printer used to print the test results + * @return The test results + */ + static const TestResult* runAndPrint(TestPrinter *printer); + + /** + * Pass all tests in the registry to the TestRunner runner and + * return the results of all tests ran. Results will also be given to + * to the TestPrinter printer. + * + * @param printer The custom printer used to print the test results + * @param runner The custom runner used to decided which test to run + * @return The test results + */ + static const TestResult* runAndPrint(TestPrinter *printer, TestRunner *runner); + + private: + static TestRegistry& instance(); + static int nextName; + void add(Test *test); + void addTestCase(TestCase *testCase); + const TestResult* runTests(TestRunner *runner); + TestCase *currentTC_; + TestPrinter *defaultPrinter_; + int testCaseCount_; + TestRunner *defaultRunner_; + TestResult testResult_; +}; + +#endif // TESTREGISTRY_H + + diff --git a/src/testframework/easyunit/testresult.cpp b/src/testframework/easyunit/testresult.cpp new file mode 100644 index 00000000..e23e58fc --- /dev/null +++ b/src/testframework/easyunit/testresult.cpp @@ -0,0 +1,117 @@ +/* +EasyUnit : Simple C++ Unit testing framework +Copyright (C) 2004 Barthelemy Dagenais + +This library is free software; you can redistribute it and/or +modify it under the terms of the GNU Lesser General Public +License as published by the Free Software Foundation; either +version 2.1 of the License, or (at your option) any later version. + +This library 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 +Lesser General Public License for more details. + +You should have received a copy of the GNU Lesser General Public +License along with this library; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + +Barthelemy Dagenais +barthelemy@prologique.com +*/ + +#include "testresult.h" + + +TestResult::TestResult() +{ +} + + +TestResult::~TestResult() +{ +} + +int TestResult::getTotalSuccesses() const +{ + return totalSuccesses_; +} + +int TestResult::getTotalErrors() const +{ + return totalErrors_; +} + +int TestResult::getTotalFailures() const +{ + return totalFailures_; +} + + +int TestResult::getSuccesses() const +{ + return successes_; +} + +int TestResult::getFailures() const +{ + return failures_; +} + +int TestResult::getErrors() const +{ + return errors_; +} + +int TestResult::getTestCaseCount() const +{ + return testCaseCount_; +} + +int TestResult::getTestRanCount() const +{ + return testRanCount_; +} + +int TestResult::getTestCaseRanCount() const +{ + return testCaseRanCount_; +} + +TestCase* TestResult::getTestCases() const +{ + return testCases_; +} + +void TestResult::setTestCases(TestCase *testCases, int testCaseCount) +{ + testCases_ = testCases; + testCaseCount_ = testCaseCount; +} + +void TestResult::addResult(TestCase *testCase) +{ + int tcSuccesses = testCase->getSuccessesCount(); + int tcErrors = testCase->getErrorsCount(); + int tcFailures = testCase->getFailuresCount(); + + testCaseRanCount_++; + + totalSuccesses_ += tcSuccesses; + totalErrors_ += tcErrors; + totalFailures_ += tcFailures; + testRanCount_ += testCase->getTestsCount(); + + if (tcErrors == 0 && tcFailures == 0) { + successes_++; + } + else if (tcErrors > 0) { + errors_++; + } + else { + failures_++; + } +} + + + diff --git a/src/testframework/easyunit/testresult.h b/src/testframework/easyunit/testresult.h new file mode 100644 index 00000000..ce44b113 --- /dev/null +++ b/src/testframework/easyunit/testresult.h @@ -0,0 +1,146 @@ +/* +EasyUnit : Simple C++ Unit testing framework +Copyright (C) 2004 Barthelemy Dagenais + +This library is free software; you can redistribute it and/or +modify it under the terms of the GNU Lesser General Public +License as published by the Free Software Foundation; either +version 2.1 of the License, or (at your option) any later version. + +This library 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 +Lesser General Public License for more details. + +You should have received a copy of the GNU Lesser General Public +License along with this library; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + +Barthelemy Dagenais +barthelemy@prologique.com +*/ + +#ifndef testresult_H +#define testresult_H + +#include "testcase.h" + + +class TestResult +{ +public: + TestResult(); + virtual ~TestResult(); + + + /** + * Get the total number of successes registered by all + * test cases ran. This is the sum of all TestCase->getSuccessesCount(). + * + *@return The number of successes registered by all testcases. + */ + int getTotalSuccesses() const; + + /** + * Get the total number of errors registered by all + * test cases ran. This is the sum of all TestCase->getErrorsCount(). + * + *@return The number of errors registered by all testcases. + */ + int getTotalErrors() const; + + /** + * Get the total number of failures registered by all + * test cases ran. This is the sum of all TestCase->getFailuresCount(). + * + * @return The number of failures registered by all testcases. + */ + int getTotalFailures() const; + + /** + * Get the number of testcases ran that succeeded. + * + * @return The number of testcases ran that succeeded. + */ + int getSuccesses() const; + + /** + * Get the number of testcases ran that failed. + * + * @return The number of testcases ran that failed. + */ + int getFailures() const; + + /** + * Get the number of testcases ran that reported an error. + * + * @return The number of testcases ran that reported an error. + */ + int getErrors() const; + + /** + * Get the number of testcases in the TestCase list. + * + * @return The size of the TestCase list + */ + int getTestCaseCount() const; + + /** + * Get the number of tests + * + * @return The number of tests ran that succeeded + */ + int getTestRanCount() const; + + /** + * Get the number of testcases ran. + * + * @return The number of testcases ran + */ + int getTestCaseRanCount() const; + + /** + * Get the TestCase list. This list contains all TestCase registered and + * not only those that were ran. + * + * @return The TestCase list + */ + TestCase* getTestCases() const; + + /** + * Set the TestCase list and the size of the list. + * + * @param testCases TestCase list + * @param testCaseCount size of the TestCase list + */ + void setTestCases(TestCase *testCases, int testCaseCount); + + /** + * Add a TestCase result. This is used by a TestCase after it has + * completed. + * + * @param testCase TestCase that ran and contains results to add to + * global results + */ + virtual void addResult(TestCase *testCase); + +protected: + int testCaseCount_{0}; + int testRanCount_{0}; + int testCaseRanCount_{0}; + + int totalSuccesses_{0}; + int totalErrors_{0}; + int totalFailures_{0}; + + int successes_{0}; + int errors_{0}; + int failures_{0}; + + TestCase* testCases_{0}; + +}; + + +#endif // testresult_H + diff --git a/src/testframework/easyunit/testrunner.cpp b/src/testframework/easyunit/testrunner.cpp new file mode 100644 index 00000000..09119a29 --- /dev/null +++ b/src/testframework/easyunit/testrunner.cpp @@ -0,0 +1,45 @@ +/* +EasyUnit : Simple C++ Unit testing framework +Copyright (C) 2004 Barthelemy Dagenais + +This library is free software; you can redistribute it and/or +modify it under the terms of the GNU Lesser General Public +License as published by the Free Software Foundation; either +version 2.1 of the License, or (at your option) any later version. + +This library 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 +Lesser General Public License for more details. + +You should have received a copy of the GNU Lesser General Public +License along with this library; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + +Barthelemy Dagenais +barthelemy@prologique.com +*/ + +#include "testrunner.h" + + +TestRunner::TestRunner() +{ +} + + +TestRunner::~TestRunner() +{ +} + +void TestRunner::run(TestCase *testCase, int size) +{ + for (int i=0; irun(); + testCase = testCase->getNext(); + } +} + + + + diff --git a/src/testframework/easyunit/testrunner.h b/src/testframework/easyunit/testrunner.h new file mode 100644 index 00000000..38498fb9 --- /dev/null +++ b/src/testframework/easyunit/testrunner.h @@ -0,0 +1,57 @@ +/* +EasyUnit : Simple C++ Unit testing framework +Copyright (C) 2004 Barthelemy Dagenais + +This library is free software; you can redistribute it and/or +modify it under the terms of the GNU Lesser General Public +License as published by the Free Software Foundation; either +version 2.1 of the License, or (at your option) any later version. + +This library 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 +Lesser General Public License for more details. + +You should have received a copy of the GNU Lesser General Public +License along with this library; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + +Barthelemy Dagenais +barthelemy@prologique.com +*/ + +#ifndef TestRunner_H +#define TestRunner_H + +#include "testcase.h" + + + +/** + * Test runner used to determine which test to run. + * + * User may extends this class to provide a custom test runner + * to TestRegistry. + */ +class TestRunner +{ +public: + TestRunner(); + virtual ~TestRunner(); + + /** + * Method used to run testcases by TestRegistry. + * + * User should override this method in order to provide custom + * behavior. + * + * @param testCase Linked list of testcases + * @param size Size of the linked list + */ + virtual void run(TestCase *testCase, int size); + +}; + + +#endif // TestRunner_H + diff --git a/src/testframework/prettyprint.hpp b/src/testframework/prettyprint.hpp new file mode 100644 index 00000000..6bf25431 --- /dev/null +++ b/src/testframework/prettyprint.hpp @@ -0,0 +1,445 @@ +// Copyright Louis Delacroix 2010 - 2014. +// Distributed under the Boost Software License, Version 1.0. +// (See accompanying file LICENSE_1_0.txt or copy at +// http://www.boost.org/LICENSE_1_0.txt) +// +// A pretty printing library for C++ +// +// Usage: +// Include this header, and operator<< will "just work". + +#ifndef H_PRETTY_PRINT +#define H_PRETTY_PRINT + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace pretty_print +{ + namespace detail + { + // SFINAE type trait to detect whether T::const_iterator exists. + + struct sfinae_base + { + using yes = char; + using no = yes[2]; + }; + + template + struct has_const_iterator : private sfinae_base + { + private: + template static yes & test(typename C::const_iterator*); + template static no & test(...); + public: + static const bool value = sizeof(test(nullptr)) == sizeof(yes); + using type = T; + }; + + template + struct has_begin_end : private sfinae_base + { + private: + template + static yes & f(typename std::enable_if< + std::is_same(&C::begin)), + typename C::const_iterator(C::*)() const>::value>::type *); + + template static no & f(...); + + template + static yes & g(typename std::enable_if< + std::is_same(&C::end)), + typename C::const_iterator(C::*)() const>::value, void>::type*); + + template static no & g(...); + + public: + static bool const beg_value = sizeof(f(nullptr)) == sizeof(yes); + static bool const end_value = sizeof(g(nullptr)) == sizeof(yes); + }; + + } // namespace detail + + + // Holds the delimiter values for a specific character type + + template + struct delimiters_values + { + using char_type = TChar; + const char_type * prefix; + const char_type * delimiter; + const char_type * postfix; + }; + + + // Defines the delimiter values for a specific container and character type + + template + struct delimiters + { + using type = delimiters_values; + static const type values; + }; + + + // Functor to print containers. You can use this directly if you want + // to specificy a non-default delimiters type. The printing logic can + // be customized by specializing the nested template. + + template , + typename TDelimiters = delimiters> + struct print_container_helper + { + using delimiters_type = TDelimiters; + using ostream_type = std::basic_ostream; + + template + struct printer + { + static void print_body(const U & c, ostream_type & stream) + { + using std::begin; + using std::end; + + auto it = begin(c); + const auto the_end = end(c); + + if (it != the_end) + { + for ( ; ; ) + { + stream << *it; + + if (++it == the_end) break; + + if (delimiters_type::values.delimiter != NULL) + stream << delimiters_type::values.delimiter; + } + } + } + }; + + print_container_helper(const T & container) + : container_(container) + { } + + inline void operator()(ostream_type & stream) const + { + if (delimiters_type::values.prefix != NULL) + stream << delimiters_type::values.prefix; + + printer::print_body(container_, stream); + + if (delimiters_type::values.postfix != NULL) + stream << delimiters_type::values.postfix; + } + + private: + const T & container_; + }; + + // Specialization for pairs + + template + template + struct print_container_helper::printer> + { + using ostream_type = print_container_helper::ostream_type; + + static void print_body(const std::pair & c, ostream_type & stream) + { + stream << c.first; + if (print_container_helper::delimiters_type::values.delimiter != NULL) + stream << print_container_helper::delimiters_type::values.delimiter; + stream << c.second; + } + }; + + // Specialization for tuples + + template + template + struct print_container_helper::printer> + { + using ostream_type = print_container_helper::ostream_type; + using element_type = std::tuple; + + template struct Int { }; + + static void print_body(const element_type & c, ostream_type & stream) + { + tuple_print(c, stream, Int<0>()); + } + + static void tuple_print(const element_type &, ostream_type &, Int) + { + } + + static void tuple_print(const element_type & c, ostream_type & stream, + typename std::conditional, std::nullptr_t>::type) + { + stream << std::get<0>(c); + tuple_print(c, stream, Int<1>()); + } + + template + static void tuple_print(const element_type & c, ostream_type & stream, Int) + { + if (print_container_helper::delimiters_type::values.delimiter != NULL) + stream << print_container_helper::delimiters_type::values.delimiter; + + stream << std::get(c); + + tuple_print(c, stream, Int()); + } + }; + + // Prints a print_container_helper to the specified stream. + + template + inline std::basic_ostream & operator<<( + std::basic_ostream & stream, + const print_container_helper & helper) + { + helper(stream); + return stream; + } + + + // Basic is_container template; specialize to derive from std::true_type for all desired container types + + template + struct is_container : public std::integral_constant::value && + detail::has_begin_end::beg_value && + detail::has_begin_end::end_value> { }; + + template + struct is_container : std::true_type { }; + + template + struct is_container : std::false_type { }; + + template + struct is_container> : std::true_type { }; + + template + struct is_container> : std::true_type { }; + + template + struct is_container> : std::true_type { }; + + + // Default delimiters + + template struct delimiters { static const delimiters_values values; }; + template const delimiters_values delimiters::values = { "[", ", ", "]" }; + template struct delimiters { static const delimiters_values values; }; + template const delimiters_values delimiters::values = { L"[", L", ", L"]" }; + + + // Delimiters for (multi)set and unordered_(multi)set + + template + struct delimiters< ::std::set, char> { static const delimiters_values values; }; + + template + const delimiters_values delimiters< ::std::set, char>::values = { "{", ", ", "}" }; + + template + struct delimiters< ::std::set, wchar_t> { static const delimiters_values values; }; + + template + const delimiters_values delimiters< ::std::set, wchar_t>::values = { L"{", L", ", L"}" }; + + template + struct delimiters< ::std::multiset, char> { static const delimiters_values values; }; + + template + const delimiters_values delimiters< ::std::multiset, char>::values = { "{", ", ", "}" }; + + template + struct delimiters< ::std::multiset, wchar_t> { static const delimiters_values values; }; + + template + const delimiters_values delimiters< ::std::multiset, wchar_t>::values = { L"{", L", ", L"}" }; + + template + struct delimiters< ::std::unordered_set, char> { static const delimiters_values values; }; + + template + const delimiters_values delimiters< ::std::unordered_set, char>::values = { "{", ", ", "}" }; + + template + struct delimiters< ::std::unordered_set, wchar_t> { static const delimiters_values values; }; + + template + const delimiters_values delimiters< ::std::unordered_set, wchar_t>::values = { L"{", L", ", L"}" }; + + template + struct delimiters< ::std::unordered_multiset, char> { static const delimiters_values values; }; + + template + const delimiters_values delimiters< ::std::unordered_multiset, char>::values = { "{", ", ", "}" }; + + template + struct delimiters< ::std::unordered_multiset, wchar_t> { static const delimiters_values values; }; + + template + const delimiters_values delimiters< ::std::unordered_multiset, wchar_t>::values = { L"{", L", ", L"}" }; + + + // Delimiters for pair and tuple + + template struct delimiters, char> { static const delimiters_values values; }; + template const delimiters_values delimiters, char>::values = { "(", ", ", ")" }; + template struct delimiters< ::std::pair, wchar_t> { static const delimiters_values values; }; + template const delimiters_values delimiters< ::std::pair, wchar_t>::values = { L"(", L", ", L")" }; + + template struct delimiters, char> { static const delimiters_values values; }; + template const delimiters_values delimiters, char>::values = { "(", ", ", ")" }; + template struct delimiters< ::std::tuple, wchar_t> { static const delimiters_values values; }; + template const delimiters_values delimiters< ::std::tuple, wchar_t>::values = { L"(", L", ", L")" }; + + + // Type-erasing helper class for easy use of custom delimiters. + // Requires TCharTraits = std::char_traits and TChar = char or wchar_t, and MyDelims needs to be defined for TChar. + // Usage: "cout << pretty_print::custom_delims(x)". + + struct custom_delims_base + { + virtual ~custom_delims_base() { } + virtual std::ostream & stream(::std::ostream &) = 0; + virtual std::wostream & stream(::std::wostream &) = 0; + }; + + template + struct custom_delims_wrapper : custom_delims_base + { + custom_delims_wrapper(const T & t_) : t(t_) { } + + std::ostream & stream(std::ostream & s) + { + return s << print_container_helper, Delims>(t); + } + + std::wostream & stream(std::wostream & s) + { + return s << print_container_helper, Delims>(t); + } + + private: + const T & t; + }; + + template + struct custom_delims + { + template + custom_delims(const Container & c) : base(new custom_delims_wrapper(c)) { } + + std::unique_ptr base; + }; + + template + inline std::basic_ostream & operator<<(std::basic_ostream & s, const custom_delims & p) + { + return p.base->stream(s); + } + + + // A wrapper for a C-style array given as pointer-plus-size. + // Usage: std::cout << pretty_print_array(arr, n) << std::endl; + + template + struct array_wrapper_n + { + typedef const T * const_iterator; + typedef T value_type; + + array_wrapper_n(const T * const a, size_t n) : _array(a), _n(n) { } + inline const_iterator begin() const { return _array; } + inline const_iterator end() const { return _array + _n; } + + private: + const T * const _array; + size_t _n; + }; + + + // A wrapper for hash-table based containers that offer local iterators to each bucket. + // Usage: std::cout << bucket_print(m, 4) << std::endl; (Prints bucket 5 of container m.) + + template + struct bucket_print_wrapper + { + typedef typename T::const_local_iterator const_iterator; + typedef typename T::size_type size_type; + + const_iterator begin() const + { + return m_map.cbegin(n); + } + + const_iterator end() const + { + return m_map.cend(n); + } + + bucket_print_wrapper(const T & m, size_type bucket) : m_map(m), n(bucket) { } + + private: + const T & m_map; + const size_type n; + }; + +} // namespace pretty_print + + +// Global accessor functions for the convenience wrappers + +template +inline pretty_print::array_wrapper_n pretty_print_array(const T * const a, size_t n) +{ + return pretty_print::array_wrapper_n(a, n); +} + +template pretty_print::bucket_print_wrapper +bucket_print(const T & m, typename T::size_type n) +{ + return pretty_print::bucket_print_wrapper(m, n); +} + + +// Main magic entry point: An overload snuck into namespace std. +// Can we do better? + +namespace std +{ + // Prints a container to the stream using default delimiters + + template + inline typename enable_if< ::pretty_print::is_container::value, + basic_ostream &>::type + operator<<(basic_ostream & stream, const T & container) + { + return stream << ::pretty_print::print_container_helper(container); + } +} + + + +#endif // H_PRETTY_PRINT diff --git a/src/testframework/unittests/libs/TEST_gcode.cpp b/src/testframework/unittests/libs/TEST_gcode.cpp new file mode 100644 index 00000000..e5888e35 --- /dev/null +++ b/src/testframework/unittests/libs/TEST_gcode.cpp @@ -0,0 +1,62 @@ +#include "utils.h" + +#include "Gcode.h" + +#include +#include +#include + +#include "easyunit/test.h" + +TEST(GCodeTest,subcode) +{ + Gcode gc1("G32 X1.2 Y2.3", nullptr); + + ASSERT_TRUE(gc1.has_g); + ASSERT_TRUE(!gc1.has_m); + ASSERT_EQUALS_V(32, gc1.g); + ASSERT_EQUALS_V(0, gc1.subcode); + ASSERT_EQUALS_V(2, gc1.get_num_args()); + ASSERT_TRUE(gc1.has_letter('X')); + ASSERT_TRUE(gc1.has_letter('Y')); + ASSERT_EQUALS_DELTA_V(1.2, gc1.get_value('X'), 0.001); + ASSERT_EQUALS_DELTA_V(2.3, gc1.get_value('Y'), 0.001); + + Gcode gc2("G32.2 X1.2 Y2.3", nullptr); + + ASSERT_TRUE(gc2.has_g); + ASSERT_TRUE(!gc2.has_m); + ASSERT_EQUALS_V(32, gc2.g); + ASSERT_EQUALS_V(2, gc2.subcode); + ASSERT_EQUALS_V(2, gc2.get_num_args()); + ASSERT_TRUE(gc2.has_letter('X')); + ASSERT_TRUE(gc2.has_letter('Y')); + ASSERT_EQUALS_DELTA_V(1.2, gc2.get_value('X'), 0.001); + ASSERT_EQUALS_DELTA_V(2.3, gc2.get_value('Y'), 0.001); + + // test equals + Gcode gc3("", nullptr); + gc3= gc2; + ASSERT_TRUE(gc3.has_g); + ASSERT_TRUE(!gc3.has_m); + ASSERT_EQUALS_V(32, gc3.g); + ASSERT_EQUALS_V(2, gc3.subcode); + ASSERT_EQUALS_V(2, gc3.get_num_args()); + ASSERT_TRUE(gc3.has_letter('X')); + ASSERT_TRUE(gc3.has_letter('Y')); + ASSERT_EQUALS_DELTA_V(1.2, gc3.get_value('X'), 0.001); + ASSERT_EQUALS_DELTA_V(2.3, gc3.get_value('Y'), 0.001); + + // test copy ctor + Gcode gc4(gc2); + ASSERT_TRUE(gc4.has_g); + ASSERT_TRUE(!gc4.has_m); + ASSERT_EQUALS_V(32, gc4.g); + ASSERT_EQUALS_V(2, gc4.subcode); + ASSERT_EQUALS_V(2, gc4.get_num_args()); + ASSERT_TRUE(gc4.has_letter('X')); + ASSERT_TRUE(gc4.has_letter('Y')); + ASSERT_EQUALS_DELTA_V(1.2, gc4.get_value('X'), 0.001); + ASSERT_EQUALS_DELTA_V(2.3, gc4.get_value('Y'), 0.001); + +} diff --git a/src/testframework/unittests/libs/TEST_utils.cpp b/src/testframework/unittests/libs/TEST_utils.cpp new file mode 100644 index 00000000..d3396542 --- /dev/null +++ b/src/testframework/unittests/libs/TEST_utils.cpp @@ -0,0 +1,47 @@ +#include "utils.h" + +#include +#include +#include + +#include "easyunit/test.h" + +TEST(UtilsTest,split) +{ + const char *s= "one two three"; + std::vector v= split(s, ' '); + ASSERT_TRUE(v.size() == 3); + ASSERT_TRUE(v[0] == "one"); + ASSERT_TRUE(v[1] == "two"); + ASSERT_TRUE(v[2] == "three"); +} + +TEST(UtilsTest,split_empty_string) +{ + const char *s= ""; + std::vector v= split(s, ' '); + + ASSERT_TRUE(v.size() == 1); + ASSERT_TRUE(v[0].empty()); + ASSERT_TRUE(v[0] == ""); +} + +TEST(UtilsTest,parse_number_list) +{ + const char *s= "1.1,2.2,3.3"; + std::vector v= parse_number_list(s); + ASSERT_TRUE(v.size() == 3); + ASSERT_TRUE(v[0] == 1.1F); + ASSERT_TRUE(v[1] == 2.2F); + ASSERT_TRUE(v[2] == 3.3F); +} + +TEST(UtilsTest,append_parameters) +{ + char buf[132]; + + int n= append_parameters(buf, {{'X', 1}, {'Y', 2}, {'Z', 3}}, sizeof(buf)); + //printf("%d - %s\n", n, buf); + ASSERT_TRUE(n == 24); + ASSERT_TRUE(strcmp(buf, "X1.0000 Y2.0000 Z3.0000 ") == 0); +} diff --git a/src/testframework/unittests/tools/switch/TEST_Switch.cpp b/src/testframework/unittests/tools/switch/TEST_Switch.cpp new file mode 100644 index 00000000..7201393e --- /dev/null +++ b/src/testframework/unittests/tools/switch/TEST_Switch.cpp @@ -0,0 +1,90 @@ +#include "Kernel.h" +#include "checksumm.h" +#include "utils.h" +#include "Test_kernel.h" +#include "PublicDataRequest.h" +#include "PublicData.h" +#include "SwitchPublicAccess.h" +#include "Gcode.h" +#include "Switch.h" +#include "mri.h" + +#include +#include + +#include "easyunit/test.h" + +// this declares any global variables the test needs +DECLARE(Switch) + Switch *ts; + uint16_t switch_id; +END_DECLARE + +// called before each test +SETUP(Switch) +{ + // create the module we want to test + switch_id= get_checksum("fan"); + ts = new Switch(switch_id); + //printf("...Setup Switch\n"); +} + +// called after each test +TEARDOWN(Switch) +{ + // delete the module + delete ts; + + // have kernel reset to a clean state + test_kernel_teardown(); + + //printf("...Teardown Switch\n"); +} + +// define various configs here, these are in the same formate they would appeat in the config file (comments removed for clarity) +const static char switch_config[]= "\ +switch.fan.enable true \n\ +switch.fan.input_on_command M106 \n\ +switch.fan.input_off_command M107 \n\ +switch.fan.output_pin 2.6 \n\ +switch.fan.output_type digital \n\ +"; + +static bool get_switch_state(Switch *ts, struct pad_switch& s) +{ + // inquire if the switch is on or off (simulate an ON_GET_PUBLIC_DATA) + PublicDataRequest pdr(switch_checksum, fan_checksum, 0); + pdr.set_data_ptr(&s, false); // caller providing storage + ts->on_get_public_data(&pdr); + return(pdr.is_taken() && !pdr.has_returned_data()); +} + +TESTF(Switch,set_on_off_with_gcode) +{ + // load config with required settings for this test + test_kernel_setup_config(switch_config, &switch_config[sizeof(switch_config)]); + + // make module load the config + ts->on_config_reload(nullptr); + + // make sure switch starts off + struct pad_switch s; + ASSERT_TRUE(get_switch_state(ts, s)); + ASSERT_EQUALS(s.name, switch_id); + ASSERT_TRUE(!s.state); + + // if we want to enter the debugger here + //__debugbreak(); + + // now turn it on + Gcode gc1("M106", (StreamOutput *)THEKERNEL->serial); + ts->on_gcode_received(&gc1); + ASSERT_TRUE(get_switch_state(ts, s)); + ASSERT_TRUE(s.state); + + // now turn it off + Gcode gc2("M107", (StreamOutput *)THEKERNEL->serial); + ts->on_gcode_received(&gc2); + ASSERT_TRUE(get_switch_state(ts, s)); + ASSERT_TRUE(!s.state); +} diff --git a/src/testframework/unittests/tools/temperatureswitch/TEST_TemperatureSwitch.cpp b/src/testframework/unittests/tools/temperatureswitch/TEST_TemperatureSwitch.cpp new file mode 100644 index 00000000..c5cf33fc --- /dev/null +++ b/src/testframework/unittests/tools/temperatureswitch/TEST_TemperatureSwitch.cpp @@ -0,0 +1,265 @@ +#include "TemperatureSwitch.h" +#include "Kernel.h" +#include "checksumm.h" +#include "utils.h" +#include "Test_kernel.h" +#include "PublicDataRequest.h" +#include "PublicData.h" +#include "TemperatureControlPublicAccess.h" +#include "SwitchPublicAccess.h" +#include "Gcode.h" + +#include +#include + +#include "easyunit/test.h" + +// this declares any global variables the test needs +DECLARE(TemperatureSwitch) + TemperatureSwitch *ts; +END_DECLARE + +// called before each test +SETUP(TemperatureSwitch) +{ + // create the module we want to test + ts = new TemperatureSwitch(); + //printf("...Setup TemperatureSwitch\n"); +} + +// called after each test +TEARDOWN(TemperatureSwitch) +{ + // delete the module + delete ts; + + // have kernel reset to a clean state + test_kernel_teardown(); + + //printf("...Teardown TemperatureSwitch\n"); +} + +// define various configs here, these are in the same formate they would appeat in the config file (comments removed for clarity) +const static char edge_low_config[]= "\ +temperatureswitch.psu_off.enable true \n\ +temperatureswitch.psu_off.designator T \n\ +temperatureswitch.psu_off.switch fan \n\ +temperatureswitch.psu_off.threshold_temp 50.0 \n\ +temperatureswitch.psu_off.heatup_poll 5 \n\ +temperatureswitch.psu_off.cooldown_poll 5 \n\ +temperatureswitch.psu_off.arm_mcode 1100 \n\ +temperatureswitch.psu_off.trigger falling \n\ +temperatureswitch.psu_off.inverted false \n\ +"; + +const static char level_config[]= "\ +temperatureswitch.psu_off.enable true \n\ +temperatureswitch.psu_off.designator T \n\ +temperatureswitch.psu_off.switch fan \n\ +temperatureswitch.psu_off.threshold_temp 50.0 \n\ +temperatureswitch.psu_off.heatup_poll 5 \n\ +temperatureswitch.psu_off.cooldown_poll 5 \n\ +"; + +// handle mock call to temperature control +// simulates one temperature control with T designator and id of 123 +static void on_get_public_data_tc1(void *argument) +{ + PublicDataRequest *pdr = static_cast(argument); + + if(!pdr->starts_with(temperature_control_checksum)) return; + if(!pdr->second_element_is(poll_controls_checksum)) return; + + std::vector *v= static_cast*>(pdr->get_data_ptr()); + + struct pad_temperature t; + // setup data + t.designator= "T"; + t.id= 123; + v->push_back(t); + pdr->set_taken(); +} + +// Handle temperature request and switch status request +static bool switch_state; +static bool switch_get_hit= false; +static int hitcnt= 0; +static float return_current_temp= 0; +static void on_get_public_data_tc2(void *argument) +{ + PublicDataRequest *pdr = static_cast(argument); + + if(pdr->starts_with(temperature_control_checksum)) { + // return a current temperature of return_current_temp when requested + if(!pdr->second_element_is(current_temperature_checksum)) return; + if(!pdr->third_element_is(123)) return; + + hitcnt++; + + struct pad_temperature *t= static_cast(pdr->get_data_ptr()); + t->current_temperature = return_current_temp; + t->target_temperature = 0; + t->pwm = 0; + t->designator= "T"; + t->id= 123; + pdr->set_taken(); + + } else if(pdr->starts_with(switch_checksum)){ + // return status of the fan switch + if(!pdr->second_element_is(get_checksum("fan"))) return; + + switch_get_hit= true; + + static struct pad_switch *pad= static_cast(pdr->get_data_ptr());; + pad->name = get_checksum("fan"); + pad->state = switch_state; + pad->value = 0; + pdr->set_taken(); + } +} + +// simulates the switch being turned on or off so we can test the state +static bool switch_set_hit= false; +static void on_set_public_data_switch(void *argument) +{ + PublicDataRequest *pdr = static_cast(argument); + + if(!pdr->starts_with(switch_checksum)) return; + if(!pdr->second_element_is(get_checksum("fan"))) return; + if(!pdr->third_element_is(state_checksum)) return; + + switch_set_hit= true; + + bool t = *static_cast(pdr->get_data_ptr()); + switch_state = t; + pdr->set_taken(); +} + +// stimulates the module with on_second ticks in which it asks for a temperature which we simulate above +static bool set_temp(TemperatureSwitch *nts, float t) +{ + // trap public data request to TemperatureControl and return a temperature + hitcnt= 0; + return_current_temp= t; + test_kernel_trap_event(ON_GET_PUBLIC_DATA, on_get_public_data_tc2); + + // tick 5 times for 5 seconds + for (int i = 0; i < 5; ++i) { + nts->on_second_tick(nullptr); + } + + // make sure the temp was read at least once + return (hitcnt > 0); +} + +TESTF(TemperatureSwitch,level_low_high) +{ + // load config with required settings for this test + test_kernel_setup_config(level_config, &level_config[sizeof(level_config)]); + + // trap public data request to TemperatureControl and return a mock tempcontrol + test_kernel_trap_event(ON_GET_PUBLIC_DATA, on_get_public_data_tc1); + + // make module load the config + uint16_t cs= get_checksum("psu_off"); + std::unique_ptr nts(ts->load_config(cs)); + + if(nts.get() == nullptr) { + FAIL_M("load config failed"); + } + + // stop handling this event + test_kernel_untrap_event(ON_GET_PUBLIC_DATA); + + // test it registered the event + ASSERT_TRUE(THEKERNEL->kernel_has_event(ON_GCODE_RECEIVED, nts.get())); + + // set the first temperature + ASSERT_TRUE(set_temp(nts.get(), 25)); + + // capture any call to the switch to turn it on or off + switch_state= false; + switch_set_hit= false; + switch_get_hit= false; + test_kernel_trap_event(ON_SET_PUBLIC_DATA, on_set_public_data_switch); + + // increase temp low -> high + ASSERT_TRUE(set_temp(nts.get(), 60)); + + ASSERT_TRUE(switch_get_hit); + // make sure switch was set + ASSERT_TRUE(switch_set_hit); + + // and make sure it was turned on + ASSERT_TRUE(switch_state); + + // now make sure it turns off when temp drops + ASSERT_TRUE(set_temp(nts.get(), 30)); + + // and make sure it was turned off + ASSERT_TRUE(!switch_state); +} + +TESTF(TemperatureSwitch,edge_high_low) +{ + // load config with required settings for this test + test_kernel_setup_config(edge_low_config, &edge_low_config[sizeof(edge_low_config)]); + + // trap public data request to TemperatureControl and return a mock tempcontrol + test_kernel_trap_event(ON_GET_PUBLIC_DATA, on_get_public_data_tc1); + + // make module load the config + uint16_t cs= get_checksum("psu_off"); + std::unique_ptr nts(ts->load_config(cs)); + + if(nts.get() == nullptr) { + FAIL_M("load config failed"); + } + + // stop handling this event + test_kernel_untrap_event(ON_GET_PUBLIC_DATA); + + // test it registered the event + ASSERT_TRUE(THEKERNEL->kernel_has_event(ON_GCODE_RECEIVED, nts.get())); + ASSERT_TRUE(!nts->is_armed()); + + // set initial temp low + ASSERT_TRUE(set_temp(nts.get(), 25)); + + // capture any call to the switch to turn it on or off + switch_state= true; + test_kernel_trap_event(ON_SET_PUBLIC_DATA, on_set_public_data_switch); + + // increase temp low -> high + ASSERT_TRUE(set_temp(nts.get(), 60)); + + // make sure it was not turned off + ASSERT_TRUE(switch_state); + + // drop temp + ASSERT_TRUE(set_temp(nts.get(), 30)); + + // and make sure it was still on + ASSERT_TRUE(switch_state); + + // now arm it + Gcode gc("M1100 S1", (StreamOutput *)THEKERNEL->serial, false); + nts->on_gcode_received(&gc); + + ASSERT_TRUE(nts->is_armed()); + + // increase temp low -> high + ASSERT_TRUE(set_temp(nts.get(), 60)); + + // make sure it was not turned off + ASSERT_TRUE(switch_state); + + // drop temp + ASSERT_TRUE(set_temp(nts.get(), 30)); + + // and make sure it was turned off + ASSERT_TRUE(!switch_state); + + // make sure it is not armed anymore + ASSERT_TRUE(!nts->is_armed()); +} diff --git a/travis_install b/travis_install new file mode 100755 index 00000000..6a5f2ed5 --- /dev/null +++ b/travis_install @@ -0,0 +1,79 @@ +#! /usr/bin/env bash +# Copyright 2012 Adam Green (http://mbed.org/users/AdamGreen/) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + + +# Logs the command to be run and then executes the command while logging the results. +RunAndLog () { + echo `date` Executing $@>>$LOGFILE + $@ 1>>$LOGFILE 2>$ERRORFILE + if [ "$?" != "0" ] ; then + cat $ERRORFILE >>$LOGFILE + echo `date` Failure forced early exit>>$LOGFILE + cat $LOGFILE + rm -f $ERRORFILE + popd >/dev/null + read -n 1 -sp "Press any key to continue..." dummy ; echo + exit 1 + fi +} + + +# Setup script variables. +ROOTDIR=$0 +ROOTDIR=${ROOTDIR%/*} +pushd $ROOTDIR +ROOTDIR=$PWD +LOGFILE=$ROOTDIR/linux_install.log +ERRORFILE=$ROOTDIR/linux_install.err +GCC4ARM_VERSION=gcc-arm-none-eabi-4_8-2014q1 +GCC4ARM_FILENAME=gcc-arm-none-eabi-4_8-2014q1-20140314-linux.tar.bz2 +GCC4ARM_URL=https://launchpad.net/gcc-arm-embedded/4.8/4.8-2014-q1-update/+download/$GCC4ARM_FILENAME +GCC4ARM_TAR=$ROOTDIR/$GCC4ARM_FILENAME +GCC4ARM_MD5=72b0d06ae16b303c25fd70b2883d3950 +GCC4ARM_EXTRACT=$ROOTDIR/$GCC4ARM_VERSION +GCC4ARM_DIR=$ROOTDIR/gcc-arm-none-eabi +GCC4ARM_BINDIR=$GCC4ARM_DIR/bin +BUILDSHELL_CMD=$ROOTDIR/BuildShell + + +echo Logging install results to $LOGFILE +echo `date` Starting $0 $*>$LOGFILE + +echo Downloading GNU Tools for ARM Embedded Processors... +rm $GCC4ARM_FILENAME >/dev/null 2>/dev/null +echo `date` Executing wget $GCC4ARM_URL>>$LOGFILE +wget $GCC4ARM_URL + +echo Validating md5 signature of GNU Tools for ARM Embedded Processors... +echo `date` Validating md5 signature of GNU Tools for ARM Embedded Processors>>$LOGFILE +archive_match=`md5sum $GCC4ARM_FILENAME | grep -c $GCC4ARM_MD5` +if [ "$archive_match" != "1" ] ; then + echo $GCC4ARM_FILENAME failed MD5 signature check.>>$LOGFILE + echo `date` Failure forced early exit>>$LOGFILE + cat $LOGFILE + rm -f $ERRORFILE + popd >/dev/null + read -n 1 -sp "Press any key to continue..." dummy ; echo + exit 1 +fi + +echo Extracting GNU Tools for ARM Embedded Processors... +rm -r $GCC4ARM_DIR >/dev/null 2>/dev/null +RunAndLog tar xf $GCC4ARM_TAR +RunAndLog mv $GCC4ARM_EXTRACT $GCC4ARM_DIR + +echo Cleaning up intermediate files... +RunAndLog rm $GCC4ARM_TAR +echo Installed build tools to $GCC4ARM_DIR \ No newline at end of file