diff --git a/.vscode/launch.json b/.vscode/launch.json index 652cc97fd..9ce5345a6 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -49,7 +49,7 @@ "servertype": "jlink", "cwd": "${workspaceRoot}/g2core/", "executable": "./bin/MiniMillgShield-gShield/g2core.elf", - "device": "ATSAMS3X8E", + "device": "ATSAM3X8E", "svdFile": "${workspaceRoot}/Motate/MotateProject/motate/cmsis/TARGET_Atmel/sam3x/ATSAM3X8E.svd", "interface": "swd", "showDevDebugOutput": false, diff --git a/g2core/board/ArduinoDue.mk b/g2core/board/ArduinoDue.mk index 87ed83216..505fc3550 100755 --- a/g2core/board/ArduinoDue.mk +++ b/g2core/board/ArduinoDue.mk @@ -62,7 +62,7 @@ ifeq ("$(BASE_BOARD)","g2core-due") # Note: we call it "g2core-due" instead of "due" since the Motate built-in provides # a "due" BASE_BOARD. BOARD_PATH = ./board/ArduinoDue - SOURCE_DIRS += ${BOARD_PATH} device/step_dir_driver device/esc_spindle device/laser_toolhead + SOURCE_DIRS += ${BOARD_PATH} device/step_dir_driver device/esc_spindle device/laser_toolhead device/bme280 device/honeywell-trustability-ssc PLATFORM_BASE = ${MOTATE_PATH}/platform/atmel_sam diff --git a/g2core/board/ArduinoDue/0_hardware.cpp b/g2core/board/ArduinoDue/0_hardware.cpp index f6257b476..a3443cbae 100644 --- a/g2core/board/ArduinoDue/0_hardware.cpp +++ b/g2core/board/ArduinoDue/0_hardware.cpp @@ -58,6 +58,8 @@ #define SPINDLE_PWM_NUMBER 6 #endif +SPIBus_used_t spiBus; + #include "safety_manager.h" SafetyManager sm{}; @@ -115,8 +117,22 @@ void hardware_init() * hardware_periodic() - callback from the controller loop - TIME CRITICAL. */ +#if HAS_PRESSURE +float pressure = 0; +float pressure_threshold = 0.01; +#endif + stat_t hardware_periodic() { + + #if HAS_PRESSURE + float new_pressure = pressure_sensor.getPressure(PressureUnits::cmH2O); + if (std::abs(pressure - new_pressure) >= pressure_threshold) { + pressure = new_pressure; // only record if goes past threshold! + sr_request_status_report(SR_REQUEST_TIMED); + } + #endif + return STAT_OK; } @@ -214,7 +230,7 @@ stat_t hw_flash(nvObj_t *nv) return(STAT_OK); } -#if !HAS_LASER +#if HAS_PRESSURE // Stub in getSysConfig_3 // constexpr cfgItem_t sys_config_items_3[] = {}; constexpr cfgSubtableFromStaticArray sys_config_3{}; diff --git a/g2core/board/ArduinoDue/board_gpio.cpp b/g2core/board/ArduinoDue/board_gpio.cpp index f5d3946e6..b43baca36 100644 --- a/g2core/board/ArduinoDue/board_gpio.cpp +++ b/g2core/board/ArduinoDue/board_gpio.cpp @@ -88,12 +88,46 @@ gpioDigitalOutputPin> dout gpioDigitalInput* const d_in[] = {&din1, &din2, &din3, &din4, &din5, &din6, &din7, &din8, &din9}; gpioDigitalOutput* const d_out[] = {&dout1, &dout2, &dout3, &dout4, &dout5, &dout6, &dout7, &dout8, &dout9, &dout10, &dout11, &dout12, &dout13}; // not yet used - gpioAnalogInput* a_in[] = {}; +gpioAnalogInput* a_in[] = {}; // gpioAnalogOutput* a_out[A_OUT_CHANNELS]; +#if HAS_PRESSURE +Motate::SPIChipSelectPin pressure_cs{}; +// BME280 pressure_sensor{spiBus, pressure_cs}; +TruStabilitySSC pressure_sensor{spiBus, + pressure_cs, + /*min_output:*/ 1638, // 10% of 2^12 + /*max_output:*/ 14745, // 90% of 2^12 + /*min_value:*/ 0.0, // 0psi + /*max_value:*/ 15.0, // 15psi + PressureUnits::PSI}; +#endif + /************************************************************************************ **** CODE ************************************************************************** ************************************************************************************/ + +// Register a SysTick event to call start_sampling every temperature_sample_freq ms +const int16_t ain_sample_freq = 2; +int16_t ain_sample_counter = ain_sample_freq; +Motate::SysTickEvent ain_tick_event{ + [] { + if (!--ain_sample_counter) { + ai1.startSampling(); + ai2.startSampling(); + ai3.startSampling(); + ai4.startSampling(); + + #if HAS_PRESSURE + pressure_sensor.startSampling(); // has a timeout built in to prevent over-calling + #endif + + ain_sample_counter = ain_sample_freq; + } + }, + nullptr +}; + /* * gpio_reset() - reset inputs and outputs (no initialization) */ diff --git a/g2core/board/ArduinoDue/board_gpio.h b/g2core/board/ArduinoDue/board_gpio.h index f2eeec85a..f52f94260 100644 --- a/g2core/board/ArduinoDue/board_gpio.h +++ b/g2core/board/ArduinoDue/board_gpio.h @@ -96,5 +96,12 @@ extern gpioDigitalOutputPin> dout12; extern gpioDigitalOutputPin> dout13; +#if HAS_PRESSURE +// #include "bme280.h" +// extern BME280 pressure_sensor; + +#include "honeywell-trustability-ssc.h" +extern HoneywellTruStability pressure_sensor; +#endif // HAS_PRESSURE #endif // End of include guard: BOARD_GPIO_H_ONCE diff --git a/g2core/board/ArduinoDue/gShield-pinout.h b/g2core/board/ArduinoDue/gShield-pinout.h index 9440514ae..ae8aaa9f0 100644 --- a/g2core/board/ArduinoDue/gShield-pinout.h +++ b/g2core/board/ArduinoDue/gShield-pinout.h @@ -213,6 +213,8 @@ pin_number kSD_CardDetectPinNumber = -1; pin_number kInterlock_InPinNumber = -1; pin_number kOutputSAFE_PinNumber = -1; // SAFE signal +pin_number kPressure_ChipSelectPinNumber = kSocket2_SPISlaveSelectPinNumber; + pin_number kLED_USBRXPinNumber = 72; pin_number kLED_USBTXPinNumber = 73; diff --git a/g2core/board/ArduinoDue/hardware.h b/g2core/board/ArduinoDue/hardware.h index 4f69e87c4..cf85187a2 100644 --- a/g2core/board/ArduinoDue/hardware.h +++ b/g2core/board/ArduinoDue/hardware.h @@ -51,6 +51,10 @@ #define HAS_LASER 0 #endif +#ifndef HAS_PRESSURE +#define HAS_PRESSURE 0 +#endif + #if HAS_LASER #define MOTORS 5 // number of motors + one "laser" motor (used for pulsing the laser in sync) #else @@ -126,6 +130,11 @@ typedef TimerChannel<3,0> dda_timer_type; // stepper pulse generation in s typedef TimerChannel<4,0> exec_timer_type; // request exec timer in stepper.cpp typedef TimerChannel<5,0> fwd_plan_timer_type; // request exec timer in stepper.cpp +/**** SPI Setup ****/ +#include "MotateSPI.h" +typedef Motate::SPIBus SPIBus_used_t; +extern SPIBus_used_t spiBus; + // Pin assignments pin_number indicator_led_pin_num = Motate::kLED_USBRXPinNumber; diff --git a/g2core/board/gquintic.mk b/g2core/board/gquintic.mk index 92c71a799..b3998a196 100755 --- a/g2core/board/gquintic.mk +++ b/g2core/board/gquintic.mk @@ -60,7 +60,7 @@ ifeq ("$(BASE_BOARD)","gquintic") endif BOARD_PATH = ./board/gquintic - SOURCE_DIRS += ${BOARD_PATH} device/trinamic device/step_dir_hobbyservo device/max31865 device/i2c_eeprom device/i2c_multiplexer device/i2c_as5601 device/esc_spindle device/laser_toolhead + SOURCE_DIRS += ${BOARD_PATH} device/trinamic device/step_dir_hobbyservo device/max31865 device/i2c_eeprom device/i2c_multiplexer device/i2c_as5601 device/esc_spindle device/laser_toolhead device/bme280 device/honeywell-trustability-ssc PLATFORM_BASE = ${MOTATE_PATH}/platform/atmel_sam include $(PLATFORM_BASE).mk diff --git a/g2core/board/gquintic/0_hardware.cpp b/g2core/board/gquintic/0_hardware.cpp index ab5b4bc96..c35cb3ffc 100644 --- a/g2core/board/gquintic/0_hardware.cpp +++ b/g2core/board/gquintic/0_hardware.cpp @@ -35,23 +35,27 @@ #include "board_gpio.h" #include "safety_manager.h" +#include "kinematics.h" // for get_flow_volume + #include "MotateUtilities.h" #include "MotateUniqueID.h" #include "MotatePower.h" +#include "settings.h" + //Motate::ClockOutputPin external_clk_pin {16000000}; // 16MHz optimally Motate::OutputPin external_clk_pin {Motate::kStartLow}; HOT_DATA SPI_CS_PinMux_used_t spiCSPinMux; HOT_DATA SPIBus_used_t spiBus; -// HOT_DATA TWIBus_used_t twiBus; +HOT_DATA TWIBus_used_t twiBus; // // Define Multiplexers // HOT_DATA plex0_t plex0{twiBus, 0x0070L}; // HOT_DATA plex1_t plex1{twiBus, 0x0071L}; - +// #include "i2c_eeprom.h" // HOT_DATA I2C_EEPROM eeprom{twiBus, 0b01010000}; // alignas(4) uint8_t eeprom_buffer[128] HOT_DATA = "TestinglyABCDEFGHIJKLmnop"; // alignas(4) uint8_t eeprom_in_buffer[128] HOT_DATA = ""; @@ -123,7 +127,7 @@ ToolHead *toolhead_for_tool(uint8_t tool) { void hardware_init() { spiBus.init(); - // twiBus.init(); + twiBus.init(); board_hardware_init(); external_clk_pin = 0; // Force external clock to 0 for now. @@ -139,12 +143,22 @@ void hardware_init() */ // previous values of analog voltages +#if TEMPERATURE_OUTPUT_ON float ai_vv[A_IN_CHANNELS]; const float analog_change_threshold = 0.01; +#endif float angle_0 = 0.0; float angle_1 = 0.0; +#if HAS_PRESSURE +float pressure = 0; +float pressure_threshold = 0.01; + +float flow = 0; +float flow_threshold = 0.01; +#endif + // void read_encoder_0(bool worked /* = false*/, float angle /* = 0.0*/) { // if (worked) { // angle_0 = angle; @@ -159,6 +173,7 @@ float angle_1 = 0.0; // encoder_1.getAngleFraction(); // } +// Following is for testing internally // void first_part(bool worked = false); // void second_part(bool worked = false); // void third_part(bool worked = false); @@ -204,17 +219,25 @@ stat_t hardware_periodic() } #endif - // static uint8_t sent = false; - // if (!sent) { - // sent = 2; - // encoder_0.getAngleFraction(read_encoder_0); - // encoder_1.getAngleFraction(read_encoder_1); - // } else if (sent > 1 && sent < 80) { - // sent++; - // } + #if HAS_PRESSURE + float new_pressure = pressure_sensor1.getPressure(PressureUnits::cmH2O); + if (std::abs(pressure - new_pressure) >= pressure_threshold) { + pressure = new_pressure; // only record if goes past threshold! + sr_request_status_report(SR_REQUEST_TIMED); + } + + float new_flow = flow_sensor1.getFlow(FlowUnits::SLM); + if (std::abs(flow - new_flow) >= flow_threshold) { + flow = new_flow; // only record if goes past threshold! + sr_request_status_report(SR_REQUEST_TIMED); + } + #endif + - // if (sent == 3) { + // static uint8_t sent = 0; + // if (!sent) { // first_part(false); + // sent = 2; // } return STAT_OK; @@ -315,14 +338,39 @@ stat_t hw_flash(nvObj_t *nv) return(STAT_OK); } -#if !HAS_LASER +#if HAS_PRESSURE -// Stub in getSysConfig_3 -// constexpr cfgItem_t sys_config_items_3[] = {}; -constexpr cfgSubtableFromStaticArray sys_config_3{}; -const configSubtable * const getSysConfig_3() { return &sys_config_3; } +stat_t get_pressure(nvObj_t *nv) +{ + nv->value_flt = pressure_sensor1.getPressure(PressureUnits::cmH2O); + nv->valuetype = TYPE_FLOAT; + return (STAT_OK); +} + +stat_t get_flow(nvObj_t *nv) +{ + nv->value_flt = flow_sensor1.getFlow(FlowUnits::SLM); + nv->valuetype = TYPE_FLOAT; + return (STAT_OK); +} + +stat_t get_flow_pressure(nvObj_t *nv) +{ + nv->value_flt = flow_sensor1.getPressure(PressureUnits::cmH2O); + nv->valuetype = TYPE_FLOAT; + return (STAT_OK); +} -#else + +constexpr cfgItem_t sys_config_items_3[] = { + { "prs","prs1", _f0, 5, tx_print_nul, get_pressure, set_nul, nullptr, 0 }, + { "flow1","flow1prs", _f0, 5, tx_print_nul, get_flow_pressure, set_nul, nullptr, 0 }, + { "flow1","flow1slm", _f0, 5, tx_print_nul, get_flow, set_nul, nullptr, 0 }, + { "flow1","flow1vol", _f0, 5, tx_print_nul, get_flow_volume, set_nul, nullptr, 0 }, +}; +constexpr cfgSubtableFromStaticArray sys_config_3{sys_config_items_3}; + +#elif HAS_LASER stat_t set_pulse_duration(nvObj_t *nv) { @@ -385,10 +433,15 @@ constexpr cfgItem_t sys_config_items_3[] = { }; constexpr cfgSubtableFromStaticArray sys_config_3{sys_config_items_3}; -const configSubtable * const getSysConfig_3() { return &sys_config_3; } +#else // !HAS_LASER && !HAS_PRESSURE + +// Stub in getSysConfig_3 +constexpr cfgSubtableFromStaticArray sys_config_3{}; #endif +const configSubtable * const getSysConfig_3() { return &sys_config_3; } + /*********************************************************************************** * TEXT MODE SUPPORT * Functions to print variables from the cfgArray table diff --git a/g2core/board/gquintic/board_gpio.cpp b/g2core/board/gquintic/board_gpio.cpp index 63885d111..1a9d7cff1 100644 --- a/g2core/board/gquintic/board_gpio.cpp +++ b/g2core/board/gquintic/board_gpio.cpp @@ -111,24 +111,86 @@ gpioAnalogInput* const a_in[] = {&ai1, &ai2, &ai3, &ai4}; #endif // 'D' +// About chip selects: 0-4 are motors, 5-8 are skipped +// 8 is "CS1" on the board silk +// 9 is "CS2" on the board silk +// 12 is "CS3" on the board silk + +#if HAS_PRESSURE +// BME280 pressure_sensor{spiBus, spiCSPinMux.getCS(8)}; +// HoneywellTruStability pressure_sensor1{spiBus, +// spiCSPinMux.getCS(8), +// /*min_output:*/ 1638, // 10% of 2^14 +// /*max_output:*/ 14745, // 90% of 2^14 +// /*min_value:*/ 0.0, // 0psi +// /*max_value:*/ 15.0, // 15psi +// PressureUnits::PSI}; + +// ABPDANT030PG0D3 +// Last 8 translation: +// Source: https://sensing.honeywell.com/honeywell-sensing-basic-board-mount-pressure-abp-series-datasheet-32305128.pdf +// 030PG -> 0-30 PSI Gauge +// 0 -> I2C, Address 0x08 +// D -> 10% to 90% of 2^14 counts (digital only) temperature output enabled, sleep mode enabled +// 3 -> 3.3V version +HoneywellTruStability pressure_sensor1{twiBus, + 0x08, + /*min_output:*/ 1638, // 10% of 2^14 + /*max_output:*/ 14745, // 90% of 2^14 + /*min_value:*/ 0.0, // 0psi + /*max_value:*/ 30.0, // 15psi + PressureUnits::PSI}; + +// HSCMRRV001PD2A3 +// Last 8 translation: +// Source: https://sensing.honeywell.com/honeywell-sensing-trustability-hsc-series-high-accuracy-board-mount-pressure-sensors-50099148-a-en.pdf +// 001PD -> ±1 PSI Differential +// 2 -> I2C, Address 0x28 +// A -> 10% to 90% of 2^14 counts (digital) +// 3 -> 3.3V version +HoneywellTruStability flow_pressure_sensor1{twiBus, + 0x28, + /*min_output:*/ 1638, // 10% of 2^14 + /*max_output:*/ 14745, // 90% of 2^14 + /*min_value:*/ -1.0, // 0psi + /*max_value:*/ 1.0, // 15psi + PressureUnits::PSI}; + +// upstream_diameter_mm = 20, +// throat_diameter_mm = 7.25, +// air_density = 1.2431, +// discharge_coeffiecient = 0.7337153909 + +VenturiFlowSensor flow_sensor1{ + &flow_pressure_sensor1, + /* K= */ 0.03875590222 +}; +#endif // HAS_PRESSURE /************************************************************************************ **** CODE ************************************************************************** ************************************************************************************/ - // Register a SysTick event to call start_sampling every temperature_sample_freq ms - const int16_t ain_sample_freq = 1; - int16_t ain_sample_counter = ain_sample_freq; - Motate::SysTickEvent ain_tick_event {[] { - if (!--ain_sample_counter) { - ai1.startSampling(); - ai2.startSampling(); - ai3.startSampling(); - ai4.startSampling(); - ain_sample_counter = ain_sample_freq; - } - }, nullptr}; +// Register a SysTick event to call start_sampling every temperature_sample_freq ms +const int16_t ain_sample_freq = 2; +int16_t ain_sample_counter = ain_sample_freq; +Motate::SysTickEvent ain_tick_event{ + [] { + if (!--ain_sample_counter) { + ai1.startSampling(); + ai2.startSampling(); + ai3.startSampling(); + ai4.startSampling(); + + #if HAS_PRESSURE + pressure_sensor1.startSampling([](bool) { flow_pressure_sensor1.startSampling([](bool) { ; }); }); + #endif + ain_sample_counter = ain_sample_freq; + } + }, + nullptr +}; /* * gpio_reset() - reset inputs and outputs (no initialization) diff --git a/g2core/board/gquintic/board_gpio.h b/g2core/board/gquintic/board_gpio.h index 44b6eea47..a8972890b 100644 --- a/g2core/board/gquintic/board_gpio.h +++ b/g2core/board/gquintic/board_gpio.h @@ -194,4 +194,15 @@ extern gpioAnalogInputPin> ai4; extern gpioAnalogInput* const a_in[A_IN_CHANNELS]; +#if HAS_PRESSURE +// #include "bme280.h" +// extern BME280 pressure_sensor1; + +#include "honeywell-trustability-ssc.h" +// extern HoneywellTruStability pressure_sensor1; +extern HoneywellTruStability pressure_sensor1; +extern HoneywellTruStability flow_pressure_sensor1; // pressure sensor, used for flow measurement + +extern VenturiFlowSensor flow_sensor1; +#endif // HAS_PRESSURE #endif // End of include guard: BOARD_GPIO_H_ONCE diff --git a/g2core/board/gquintic/hardware.h b/g2core/board/gquintic/hardware.h index 85c8d1462..e04523b06 100644 --- a/g2core/board/gquintic/hardware.h +++ b/g2core/board/gquintic/hardware.h @@ -58,6 +58,10 @@ #define HAS_HOBBY_SERVO_MOTOR 0 #endif +#ifndef HAS_PRESSURE +#define HAS_PRESSURE 0 +#endif + #ifndef HAS_LASER #define HAS_LASER 0 #else @@ -102,7 +106,6 @@ #include "MotateTimers.h" // for TimerChanel<> and related... // Temporarily disabled: -// #include "i2c_eeprom.h" // #include "i2c_multiplexer.h" // #include "i2c_as5601.h" // For AS5601 @@ -144,8 +147,8 @@ using Motate::OutputPin; /**** Stepper DDA and dwell timer settings ****/ -#define FREQUENCY_DDA 200000UL // Hz step frequency. Interrupts actually fire at 2x (400 KHz) -// #define FREQUENCY_DDA 400000UL // Hz step frequency. Interrupts actually fire at 2x (300 KHz) +// #define FREQUENCY_DDA 200000UL // Hz step frequency. Interrupts actually fire at 2x (400 KHz) +#define FREQUENCY_DDA 400000UL // Hz step frequency. Interrupts actually fire at 2x (300 KHz) #define FREQUENCY_DWELL 1000UL // #define MIN_SEGMENT_MS ((float)0.125) // S70 can handle much much smaller segements @@ -168,8 +171,8 @@ typedef Motate::SPIChipSelectPinMux TWIBus_used_t; -// extern TWIBus_used_t twiBus; +typedef Motate::TWIBus TWIBus_used_t; +extern TWIBus_used_t twiBus; // using plex0_t = decltype(I2C_Multiplexer{twiBus, 0x0070L}); // extern HOT_DATA plex0_t plex0; diff --git a/g2core/config_app.cpp b/g2core/config_app.cpp index e171f1969..2fb29245d 100644 --- a/g2core/config_app.cpp +++ b/g2core/config_app.cpp @@ -999,14 +999,41 @@ constexpr cfgItem_t sys_config_items_2[] = { // kinematics controls #if KINEMATICS==KINE_FOUR_CABLE - { "sys","knfc", _f0, 4, tx_print_nul, kn_get_force, kn_set_force, nullptr, 0 }, - { "sys","knan", _f0, 0, tx_print_nul, kn_get_anchored, kn_set_anchored, nullptr, 0 }, - { "sys","knpa", _f0, 4, tx_print_nul, kn_get_pos_a, set_nul, nullptr, 0 }, - { "sys","knpb", _f0, 4, tx_print_nul, kn_get_pos_b, set_nul, nullptr, 0 }, - { "sys","knpc", _f0, 4, tx_print_nul, kn_get_pos_c, set_nul, nullptr, 0 }, - { "sys","knpd", _f0, 4, tx_print_nul, kn_get_pos_d, set_nul, nullptr, 0 }, + { "kn","knfc", _f0, 4, tx_print_nul, kn_get_force, kn_set_force, nullptr, 0 }, + { "kn","knan", _f0, 0, tx_print_nul, kn_get_anchored, kn_set_anchored, nullptr, 0 }, + { "kn","knpa", _f0, 4, tx_print_nul, kn_get_pos_a, set_nul, nullptr, 0 }, + { "kn","knpb", _f0, 4, tx_print_nul, kn_get_pos_b, set_nul, nullptr, 0 }, + { "kn","knpc", _f0, 4, tx_print_nul, kn_get_pos_c, set_nul, nullptr, 0 }, + { "kn","knpd", _f0, 4, tx_print_nul, kn_get_pos_d, set_nul, nullptr, 0 }, +#endif +#if KINEMATICS==KINE_PRESSURE + { "kn","knfc", _f0, 4, tx_print_nul, kn_get_force, kn_set_force, nullptr, 0 }, + { "kn","knft", _f0, 4, tx_print_nul, kn_get_target, kn_set_target, nullptr, 0 }, + { "kn","knan", _b0, 0, tx_print_nul, kn_get_anchored, kn_set_anchored, nullptr, 0 }, + { "kn","knepm", _f0, 4, tx_print_nul, kn_get_epm, kn_set_epm, nullptr, 0 }, + { "kn","knht", _f0, 4, tx_print_nul, kn_get_hold_time, kn_set_hold_time, nullptr, 0 }, + { "kn","knhr", _f0, 4, tx_print_nul, kn_get_hold_ratio, kn_set_hold_ratio, nullptr, 0 }, + { "kn","knp", _f0, 4, tx_print_nul, kn_get_p_factor, kn_set_p_factor, nullptr, 0 }, + { "kn","kni", _f0, 4, tx_print_nul, kn_get_i_factor, kn_set_i_factor, nullptr, 0 }, + { "kn","knd", _f0, 4, tx_print_nul, kn_get_d_factor, kn_set_d_factor, nullptr, 0 }, + { "kn","knp", _f0, 4, tx_print_nul, kn_get_p_factor, kn_set_p_factor, nullptr, 0 }, + { "kn","kni", _f0, 4, tx_print_nul, kn_get_i_factor, kn_set_i_factor, nullptr, 0 }, + { "kn","knd", _f0, 4, tx_print_nul, kn_get_d_factor, kn_set_d_factor, nullptr, 0 }, + { "kn","knev", _f0, 4, tx_print_nul, kn_get_e_value, set_nul, nullptr, 0 }, + { "kn","kniv", _f0, 4, tx_print_nul, kn_get_i_value, set_nul, nullptr, 0 }, + { "kn","kndv", _f0, 4, tx_print_nul, kn_get_d_value, set_nul, nullptr, 0 }, + { "kn","knbf", _f0, 4, tx_print_nul, kn_get_backoff_pressure, kn_set_backoff_pressure, nullptr, 0 }, + { "kn","knec", _i0, 0, tx_print_nul, kn_get_ec_value, set_nul, nullptr, 0 }, + { "kn","knuoc", _i0, 0, tx_print_nul, kn_get_uoc_value, set_nul, nullptr, 0 }, + { "kn","knumc", _i0, 0, tx_print_nul, kn_get_umc_value, set_nul, nullptr, 0 }, + { "kn","knpos1",_i0, 0, tx_print_nul, kn_get_pos_1, set_nul, nullptr, 0 }, + { "kn","knpos2",_i0, 0, tx_print_nul, kn_get_pos_2, set_nul, nullptr, 0 }, + { "kn","knpos3",_i0, 0, tx_print_nul, kn_get_pos_3, set_nul, nullptr, 0 }, + { "kn","knpos4",_i0, 0, tx_print_nul, kn_get_pos_4, set_nul, nullptr, 0 }, + { "kn","knpos5",_i0, 0, tx_print_nul, kn_get_pos_5, set_nul, nullptr, 0 }, #endif + // Communications and reporting parameters #ifdef __TEXT_MODE { "sys","tv", _iipn, 0, tx_print_tv, txt_get_tv, txt_set_tv, nullptr, TEXT_VERBOSITY }, @@ -1716,6 +1743,13 @@ constexpr cfgItem_t groups_config_items_1[] = { { "","ain7", _f0, 0, tx_print_nul, get_grp, set_grp,nullptr,0 }, { "","ain8", _f0, 0, tx_print_nul, get_grp, set_grp,nullptr,0 }, +#if KINEMATICS==KINE_PRESSURE || KINEMATICS==KINE_FOUR_CABLE +#define KINEMATICS_GROUPS 1 + { "","kn", _f0, 0, tx_print_nul, get_grp, set_grp,nullptr,0 }, +#else +#define KINEMATICS_GROUPS 0 +#endif + #define COORDINATE_OFFSET_GROUPS 9 { "","g54",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // coord offset groups { "","g55",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, @@ -1901,6 +1935,7 @@ cfgArraySynthesizer cfgArray {}; + DIGITAL_IN_GROUPS \ + DIGITAL_OUT_GROUPS \ + ANALOG_IN_GROUPS \ + + KINEMATICS_GROUPS \ + COORDINATE_OFFSET_GROUPS \ + TOOL_OFFSET_GROUPS \ + MACHINE_STATE_GROUPS \ diff --git a/g2core/device/bme280/bme280.h b/g2core/device/bme280/bme280.h new file mode 100644 index 000000000..84df2433b --- /dev/null +++ b/g2core/device/bme280/bme280.h @@ -0,0 +1,703 @@ +/* + * bme820.h - suppport for talking to the Bosch BME820 pressure/humidity/temperature sensor amp/ADC + * This file is part of the G2 project + * + * Copyright (c) 2020 Robert Giseburt + * + * This file ("the software") is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License, version 2 as published by the + * Free Software Foundation. You should have received a copy of the GNU General Public + * License, version 2 along with the software. If not, see . + * + * As a special exception, you may use this file as part of a software library without + * restriction. Specifically, if other files instantiate templates or use macros or + * inline functions from this file, or you compile this file and link it with other + * files to produce an executable, this file does not by itself cause the resulting + * executable to be covered by the GNU General Public License. This exception does not + * however invalidate any other reasons why the executable file might be covered by the + * GNU General Public License. + * + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY + * 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. + */ + +// Many thanks to Adafruit! +// More specifically, for their driver at +// https://github.com/adafruit/Adafruit_BME280_Library +// and their breakout board at https://adafru.it/2652 + +#ifndef bme820_h +#define bme820_h + +#include "MotateSPI.h" +#include "MotateBuffer.h" +#include "MotateUtilities.h" // for to/fromLittle/BigEndian +#include "util.h" // for fp_ZERO + +// Complete class for BME280 drivers. +template +struct BME280 final { + using SPIMessage = Motate::SPIMessage; + using SPIInterrupt = Motate::SPIInterrupt; + using SPIDeviceMode = Motate::SPIDeviceMode; + + // SPI and message handling properties + device_t _device; + SPIMessage _message; + + // Record if we're transmitting to prevent altering the buffers while they + // are being transmitted still. + volatile bool _transmitting = false; + + // We don't want to transmit until we're inited + bool _inited = false; + + // Record what register we just requested, so we know what register the + // the response is for (and to read the response.) + int16_t _active_register = -1; + + // Timer to keep track of when we need to do another periodic update + Motate::Timeout _check_timer; + + // Constructor - this is the only time we directly use the SBIBus + template + BME280(SPIBus_t &spi_bus, const chipSelect_t &_cs) + : _device{spi_bus.getDevice(_cs, 5000000, SPIDeviceMode::kSPIMode0 | SPIDeviceMode::kSPI8Bit, + 400, // min_between_cs_delay_ns + 400, // cs_to_sck_delay_ns + 80 // between_word_delay_ns + )} { + init(); + }; + + template + BME280(const Motate::PinOptions_t options, // completely ignored, but for compatibility with ADCPin + std::function &&_interrupt, SPIBus_t &spi_bus, const chipSelect_t &_cs) + : _device{spi_bus.getDevice(_cs, 5000000, SPIDeviceMode::kSPIMode0 | SPIDeviceMode::kSPI8Bit, + 400, // min_between_cs_delay_ns + 400, // cs_to_sck_delay_ns + 80 // between_word_delay_ns + )}, + _interrupt_handler{std::move(_interrupt)} { + init(); + }; + + // Prevent copying, and prevent moving (so we know if it happens) + BME280(const BME280 &) = delete; + BME280(BME280 &&other) : _device{std::move(other._device)} {}; + + + // ########### + // From here on we store actual values from the BME280, and marshall data + // from the in_buffer buffer to them, or from the values to the out_buffer. + + // Note that this includes _startNextReadWrite() and _doneReadingCallback(), + // which are what calls the functions to put data into the out_buffer and + // read data from the in_buffer, respectively. + + // Also, _init() is last, so it can setup a newly created BME280 object. + + enum { + BME280_REGISTER_DIG_T1 = 0x88, + BME280_REGISTER_DIG_T2 = 0x8A, + BME280_REGISTER_DIG_T3 = 0x8C, + + BME280_REGISTER_DIG_P1 = 0x8E, + BME280_REGISTER_DIG_P2 = 0x90, + BME280_REGISTER_DIG_P3 = 0x92, + BME280_REGISTER_DIG_P4 = 0x94, + BME280_REGISTER_DIG_P5 = 0x96, + BME280_REGISTER_DIG_P6 = 0x98, + BME280_REGISTER_DIG_P7 = 0x9A, + BME280_REGISTER_DIG_P8 = 0x9C, + BME280_REGISTER_DIG_P9 = 0x9E, + + BME280_REGISTER_DIG_H1 = 0xA1, + BME280_REGISTER_DIG_H2 = 0xE1, + BME280_REGISTER_DIG_H3 = 0xE3, + BME280_REGISTER_DIG_H4 = 0xE4, + BME280_REGISTER_DIG_H5 = 0xE5, + BME280_REGISTER_DIG_H6 = 0xE7, + + BME280_REGISTER_CHIPID = 0xD0, + BME280_REGISTER_VERSION = 0xD1, + BME280_REGISTER_SOFTRESET = 0xE0, + + BME280_REGISTER_CAL26 = 0xE1, // R calibration stored in 0xE1-0xF0 + + BME280_REGISTER_CONTROLHUMID = 0xF2, + BME280_REGISTER_STATUS = 0XF3, + BME280_REGISTER_CONTROL = 0xF4, + BME280_REGISTER_CONFIG = 0xF5, + BME280_REGISTER_PRESSUREDATA = 0xF7, + BME280_REGISTER_TEMPDATA = 0xFA, + BME280_REGISTER_HUMIDDATA = 0xFD + }; + + enum { + INITING, + NEED_CALIBRATION_READ, + NEEDS_CONFIGURED, + WAITING_FOR_SAMPLE, + NEEDS_SAMPLED, + } _state; + + bool _status_needs_read = false; + static const size_t bme280_status_data_size = 1; + struct bme280_status_t { + uint8_t address; + union { + uint8_t raw_status; + struct { + uint8_t im_update : 1; + int8_t unused_0 : 1; + uint8_t measuring : 1; + }; + }; + }; + alignas(4) bme280_status_t bme280_status; + void _postReadStatus() { + if (INITING == _state && bme280_status.im_update == 0) { + // it's done reading the calibration data into "image registers" + // and we were waiting for that to finish the first time + _state = NEED_CALIBRATION_READ; + } + + // if (WAITING_FOR_SAMPLE == _state && bme280_status.im_update == 0 && bme280_status.measuring == 0) { + // // it's done reading the calibration data into "image registers" + // // and we were waiting for that to finish the first time + // _state = NEEDS_SAMPLED; + // _sample_data_needs_read = true; + // } + }; + + bool _calibration0_needs_read = false; + static const size_t bme280_calib_data0_size = 25; + struct bme280_calib_data0_t { + uint8_t address; + union { + uint8_t raw_bme280_calib_data0[25]; + struct { + uint16_t dig_T1; ///< temperature compensation value + int16_t dig_T2; ///< temperature compensation value + int16_t dig_T3; ///< temperature compensation value + + uint16_t dig_P1; ///< pressure compensation value + int16_t dig_P2; ///< pressure compensation value + int16_t dig_P3; ///< pressure compensation value + int16_t dig_P4; ///< pressure compensation value + int16_t dig_P5; ///< pressure compensation value + int16_t dig_P6; ///< pressure compensation value + int16_t dig_P7; ///< pressure compensation value + int16_t dig_P8; ///< pressure compensation value + int16_t dig_P9; ///< pressure compensation value + + uint8_t dig_H1; ///< humidity compensation value + }; + }; + } __attribute__((packed)); + alignas(4) bme280_calib_data0_t bme280_calib_data0; + + + bool _calibration1_needs_read = false; + static const size_t bme280_calib_data1_size = 8; + struct bme280_calib_data1_t { + uint8_t address; + union { + uint8_t raw_bme280_calib_data1[8]; + struct { + int16_t dig_H2; ///< humidity compensation value + uint8_t dig_H3; ///< humidity compensation value + int8_t dig_H4; ///< humidity compensation value + int16_t dig_H5; ///< humidity compensation value + int8_t dig_H6; ///< humidity compensation value + }; + }; + } __attribute__((packed)); + alignas(4) bme280_calib_data1_t bme280_calib_data1; + + double dig_T1; + double dig_T2; + double dig_T3; + + double dig_P1; + double dig_P2; + double dig_P3; + double dig_P4; + double dig_P5; + double dig_P6; + double dig_P7; + double dig_P8; + double dig_P9; + + // double dig_H1; + // double dig_H2; + // double dig_H3; + // double dig_H4; + // double dig_H5; + // double dig_H6; + + void _postReadCalibration() { + using Motate::fromLittleEndian; + + // Now to rearrange the values we got into something usable + dig_T1 = fromLittleEndian((uint16_t)bme280_calib_data0.dig_T1); + dig_T2 = fromLittleEndian((uint16_t)bme280_calib_data0.dig_T2); + dig_T3 = fromLittleEndian((uint16_t)bme280_calib_data0.dig_T3); + + dig_P2 = fromLittleEndian((uint16_t)bme280_calib_data0.dig_P2); + dig_P3 = fromLittleEndian((uint16_t)bme280_calib_data0.dig_P3); + dig_P4 = fromLittleEndian((uint16_t)bme280_calib_data0.dig_P4); + dig_P1 = fromLittleEndian((uint16_t)bme280_calib_data0.dig_P1); + dig_P5 = fromLittleEndian((uint16_t)bme280_calib_data0.dig_P5); + dig_P6 = fromLittleEndian((uint16_t)bme280_calib_data0.dig_P6); + dig_P7 = fromLittleEndian((uint16_t)bme280_calib_data0.dig_P7); + dig_P8 = fromLittleEndian((uint16_t)bme280_calib_data0.dig_P8); + dig_P9 = fromLittleEndian((uint16_t)bme280_calib_data0.dig_P9); + + // the himidity values get mangled, as they are sized different than their memory layout + + // TODO - fix this is we read humidity!! + + // bme280_calib_data1.dig_H2 = fromBigEndian(bme280_calib_data1.dig_H2); + // bme280_calib_data1.dig_H4 = fromBigEndian(bme280_calib_data1.dig_H4); + // bme280_calib_data1.dig_H5 = fromBigEndian(bme280_calib_data1.dig_H5); + + _state = NEEDS_CONFIGURED; + } + + + bool _sample_data_needs_read = false; + static const size_t bme280_sample_data_size = 8; + struct bme280_sample_data_t { + uint8_t address; + union { + uint8_t raw_bme280_sample_data[8]; + struct { + uint8_t press_msb; + uint8_t press_lsb; + uint8_t press_xlsb; + uint8_t temp_msb; + uint8_t temp_lsb; + uint8_t temp_xlsb; + uint8_t hum_msb; + uint8_t hum_lsb; + }; + }; + } __attribute__((packed)); + alignas(4) bme280_sample_data_t bme280_sample_data; + + double temperature = 0; + double pressure = 0; + + void _postReadSampleData() { + double var1, var2, T; + uint32_t adc_T_int = ((uint32_t)bme280_sample_data.temp_msb << 12) | ((uint32_t)bme280_sample_data.temp_lsb << 4) | ((uint32_t)bme280_sample_data.temp_xlsb >> 4); + double adc_T = adc_T_int; + + uint32_t adc_P_int = ((uint32_t)bme280_sample_data.press_msb << 12) | ((uint32_t)bme280_sample_data.press_lsb << 4) | ((uint32_t)bme280_sample_data.press_xlsb >> 4); + double adc_P = adc_P_int; + + var1 = ((adc_T) / 16384.0 - (dig_T1) / 1024.0) * (dig_T2); + var2 = (((adc_T) / 131072.0 - (dig_T1) / 8192.0) * + ((adc_T) / 131072.0 - (dig_T1) / 8192.0)) * + (dig_T3); + // t_fine = (BME280_S32_t)(var1 + var2); + temperature = (var1 + var2) / 5120.0; + // return T; + + double p; + var1 = ((var1 + var2) / 2.0) - 64000.0; + var2 = var1 * var1 * (dig_P6) / 32768.0; + var2 = var2 + var1 * (dig_P5) * 2.0; + var2 = (var2 / 4.0) + ((dig_P4) * 65536.0); + var1 = ((dig_P3) * var1 * var1 / 524288.0 + (dig_P2) * var1) / 524288.0; + var1 = (1.0 + var1 / 32768.0) * (dig_P1); + if (fp_ZERO(var1)) { + pressure = 0; // avoid exception caused by division by zero + return; + } + p = 1048576.0 - adc_P; + p = (p - (var2 / 4096.0)) * 6250.0 / var1; + var1 = (dig_P9) * p * p / 2147483648.0; + var2 = p * (dig_P8) / 32768.0; + p = p + (var1 + var2 + (dig_P7)) / 16.0; + + pressure = p; + }; + + alignas(4) uint8_t _scribble_buffer[36]; + + + bool _configuration_is_ready_to_write = false; + static const size_t bme280_configuration_size = 2; + struct bme280_config_t { + uint8_t address; + union { + uint8_t raw_bme280_sample_data[2]; + struct { + uint8_t ctrl_meas; + uint8_t config; + }; + struct { + int8_t ctrl_meas_mode : 2; + int8_t ctrl_meas_osrs_p : 3; + int8_t ctrl_meas_osrs_t : 3; + // next byte + int8_t config_spi3w_en : 1; + int8_t unused_0 : 1; + int8_t config_filter : 3; + int8_t config_t_sb : 3; + }; + }; + } __attribute__((packed)); + alignas(4) bme280_config_t bme280_config; + + void _prepareConfiguration() { + bme280_config.ctrl_meas_mode = 0b11; // normal mode + bme280_config.ctrl_meas_osrs_p = 0b010; // pressure: 2x oversampling + bme280_config.ctrl_meas_osrs_t = 0b010; // temperature: 2x oversampling + + bme280_config.config_spi3w_en = 0b0; // stay in 4-wire mode + bme280_config.config_filter = 0b010; // filter coeffiecent: 4 + bme280_config.config_t_sb = 0b000; // 0.5ms standby between measurements + + _configuration_is_ready_to_write = true; + } + + void _postConfiguration() { + _state = WAITING_FOR_SAMPLE; + } + + void _startNextReadWrite() + { + if (_transmitting || !_inited) { return; } + _transmitting = true; // preemptively say we're transmitting .. as a mutex + + // We request the next register, and keep track of how long it is. + uint8_t next_reg = 0; + uint8_t *data_buffer = nullptr; + int8_t register_size; + + // We write before we read -- so we don't lose what we set in the registers when writing + + // check if we need to write registers + if (_configuration_is_ready_to_write) { next_reg = BME280_REGISTER_CONTROL & ~0x80; data_buffer = (uint8_t*)&bme280_config; register_size = bme280_configuration_size; _configuration_is_ready_to_write = false; } else + // if (_fault_low_needs_written) { next_reg = 0x80 | LFAULT_reg; data_buffer = (uint8_t*)&_fault_low; register_size = 2; _fault_low_needs_written = false; } else + + // check if we need to read reagisters + if (_calibration0_needs_read) { next_reg = BME280_REGISTER_DIG_T1; data_buffer = (uint8_t*)&bme280_calib_data0; register_size = bme280_calib_data0_size; _calibration0_needs_read = false; } else + if (_calibration1_needs_read) { next_reg = BME280_REGISTER_DIG_H2; data_buffer = (uint8_t*)&bme280_calib_data1; register_size = bme280_calib_data1_size; _calibration1_needs_read = false; } else + if (_sample_data_needs_read) { next_reg = BME280_REGISTER_PRESSUREDATA; data_buffer = (uint8_t*)&bme280_sample_data; register_size = bme280_sample_data_size; _sample_data_needs_read = false; } else + if (_status_needs_read) { next_reg = BME280_REGISTER_STATUS; data_buffer = (uint8_t*)&bme280_status; register_size = bme280_status_data_size; _status_needs_read = false; } else + + // otherwise we're done here + { + _active_register = -1; + _transmitting = false; // we're not really transmitting. + return; + } + + _active_register = next_reg; + *data_buffer = next_reg; + + if (next_reg & 0x80) { + // reading, prepare the address in the scribble buffer + _scribble_buffer[0] = data_buffer[0]; + _message.setup(_scribble_buffer, data_buffer, 1+register_size, SPIMessage::DeassertAfter, SPIMessage::EndTransaction); + + } else { + // writing + _message.setup(data_buffer, _scribble_buffer, 1+register_size, SPIMessage::DeassertAfter, SPIMessage::EndTransaction); + } + _device.queueMessage(&_message); + }; + + void _doneReadingCallback() + { + _transmitting = false; + + // Check to make sure it was a read, and handle it accordingly + switch (_active_register) { + case BME280_REGISTER_DIG_H2: _postReadCalibration(); break; + case BME280_REGISTER_STATUS: _postReadStatus(); break; + case (BME280_REGISTER_CONTROL & ~0x80): _postConfiguration(); break; + case BME280_REGISTER_PRESSUREDATA: _postReadSampleData(); break; + + default: + break; + } + + _active_register = -1; + _startNextReadWrite(); + }; + + void init() + { + _message.message_done_callback = [&] { this->_doneReadingCallback(); }; + + // Establish default values, and then prepare to read the registers we can to establish starting values + + _inited = true; + //_startNextReadWrite(); + _check_timer.set(0); + }; + + // interface to make this a drop-in replacement (after init) for an ADCPin + + std::function _interrupt_handler; + + void startSampling() + { + if (_check_timer.isPast()) { + if (INITING == _state) { + _status_needs_read = true; + _check_timer.set(0); + _startNextReadWrite(); + // _state is updated to NEED_CALIBRATION_READ in _postReadState() + } + else if (NEED_CALIBRATION_READ == _state) { + _calibration0_needs_read = true; + _calibration1_needs_read = true; + + _check_timer.set(0); + _startNextReadWrite(); + // _state is updated to NEEDS_CONFIGURED in _postReadCalibration() + } + else if (NEEDS_CONFIGURED == _state) { + _check_timer.set(0); + _prepareConfiguration(); + _startNextReadWrite(); + // _state is updated to WAITING_FOR_SAMPLE in _postConfiguration() + } + else if (NEEDS_SAMPLED == _state) { + _sample_data_needs_read = true; + + _check_timer.set(7); + _startNextReadWrite(); + _state = WAITING_FOR_SAMPLE; + } + else if (WAITING_FOR_SAMPLE == _state) { + _check_timer.set(0); + _startNextReadWrite(); + _state = NEEDS_SAMPLED; + } + } + }; + +// // getRaw is to return the last sampled value +// int32_t getRaw() { +// if (_fault_status.value) { +// return -_fault_status.value; +// } +// return _rtd_value; +// }; + +// float getPullupResistance() { +// return _pullup_resistance; +// } +// void setPullupResistance(const float r) { +// _pullup_resistance = r; +// } + +// // getValue is supposed to request a new value, block, and then return the result +// // PUNT - return the same as getRaw() +// int32_t getValue() { +// return getRaw(); +// }; +// // int32_t getBottom() { +// // return 0; +// // }; +// // float getBottomVoltage() { +// // return 0; +// // }; +// // int32_t getTop() { +// // return 32767; +// // }; +// // float getTopVoltage() { +// // return _vref; +// // }; + +// void setVoltageRange(const float vref, +// const float min_expected = 0, +// const float max_expected = -1, +// const float ideal_steps = 1) +// { +// // _vref = vref; + +// // All of the rest are ignored, but here for compatibility of interface +// }; +// float getVoltage() { +// float r = getRaw(); +// if (r < 0) { +// return r*1000.0; +// } +// return ((r*_pullup_resistance)/32768.0); +// }; +// operator float() { return getVoltage(); }; + +// float getResistance() { +// float r = getRaw(); +// if (r < 0) { +// return r*1000.0; +// } +// return (r*_pullup_resistance)/32768.0; +// } + +// void setInterrupts(const uint32_t interrupts) { +// // ignore this -- it's too dangerous to accidentally change the SPI interrupts +// }; + +// // We can only support interrupt inferface option 2: a function with a closure or function pointer +// void setInterruptHandler(std::function &&handler) { +// _interrupt_handler = std::move(handler); +// }; +// void setInterruptHandler(const std::function &handler) { +// _interrupt_handler = handler; +// }; + +}; + + +// A gpioAnalogInputPin subclass for the MAX31865 + +// template +// struct gpioAnalogInputPin> : gpioAnalogInput { +// protected: // so we know if anyone tries to reach in +// ioEnabled enabled; // -1=unavailable, 0=disabled, 1=enabled +// AnalogInputType_t type; + +// const uint8_t ext_pin_number; // external number to configure this pin ("ai" + ext_pin_number) +// uint8_t proxy_pin_number; // optional external number to access this pin ("ain" + proxy_pin_number) + +// using ADCPin_t = MAX31865; + +// ADCPin_t pin; // the actual pin object itself + +// public: +// // In constructor, simply forward all values to the pin +// // To get a different behavior, override this object. +// template +// gpioAnalogInputPin(const ioEnabled _enabled, const AnalogInputType_t _type, const uint8_t _ext_pin_number, const uint8_t _proxy_pin_number, T&&... additional_values) : +// gpioAnalogInput{}, +// enabled{_enabled}, +// type{_type}, +// ext_pin_number{_ext_pin_number}, +// proxy_pin_number{ _proxy_pin_number }, +// pin{Motate::kNormal, [&](bool e){this->adc_has_new_value(e);}, additional_values...} +// { +// // nothing to do here +// }; + +// // functions for use by other parts of the code, and are overridden + +// ioEnabled getEnabled() override +// { +// return enabled; +// }; +// bool setEnabled(const ioEnabled m) override +// { +// if (enabled == IO_UNAVAILABLE) { +// return false; +// } +// enabled = m; +// return true; +// }; + +// float getValue() override +// { +// if (enabled != IO_ENABLED) { +// return 0; +// } +// return pin.getVoltage(); +// }; +// float getResistance() override +// { +// if (enabled != IO_ENABLED) { +// return -1; +// } +// return pin.getResistance(); +// }; + +// AnalogInputType_t getType() override +// { +// return type; +// }; +// bool setType(const AnalogInputType_t t) override +// { +// // NOTE: Allow setting type to AIN_TYPE_EXTERNAL +// if (t == AIN_TYPE_INTERNAL) { +// return false; +// } +// type = t; +// return true; +// }; + +// AnalogCircuit_t getCircuit() override +// { +// return AIN_CIRCUIT_EXTERNAL; +// }; +// bool setCircuit(const AnalogCircuit_t c) override +// { +// // prevent setting circuit to anything but AIN_CIRCUIT_EXTERNAL +// if (c == AIN_CIRCUIT_EXTERNAL) { +// return true; +// } +// return false; +// }; + +// float getParameter(const uint8_t p) override +// { +// if (p == 0) { +// return pin.getPullupResistance(); +// } +// return 0; +// }; +// bool setParameter(const uint8_t p, const float v) override +// { +// if (p == 0) { +// pin.setPullupResistance(v); +// return true; +// } +// return false; +// }; + + +// void startSampling() override { +// pin.startSampling(); +// }; + + +// bool setExternalNumber(const uint8_t e) override +// { +// if (e == proxy_pin_number) { return true; } +// if (proxy_pin_number > 0) { +// // clear the old pin +// ain_r[proxy_pin_number-1]->setPin(nullptr); +// } +// proxy_pin_number = e; +// if (proxy_pin_number > 0) { +// // set the new pin +// ain_r[proxy_pin_number-1]->setPin(this); +// } +// return true; +// }; + +// const uint8_t getExternalNumber() override +// { +// return proxy_pin_number; +// }; + +// // support function for pin value update interrupt handling + +// void adc_has_new_value(bool err) { +// // float raw_adc_value = pin.getRaw(); +// // history.add_sample(raw_adc_value); +// }; +// }; + +#endif // bme820_h diff --git a/g2core/device/honeywell-trustability-ssc/honeywell-trustability-ssc.h b/g2core/device/honeywell-trustability-ssc/honeywell-trustability-ssc.h new file mode 100644 index 000000000..089750fd9 --- /dev/null +++ b/g2core/device/honeywell-trustability-ssc/honeywell-trustability-ssc.h @@ -0,0 +1,374 @@ +/* + * honeywell-trustability-ssc.h - suppport for talking to the Honeywell TruStability SSC line of pressure/temperature + * sensors This file is part of the G2 project + * + * Copyright (c) 2020 Robert Giseburt + * + * This file ("the software") is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License, version 2 as published by the + * Free Software Foundation. You should have received a copy of the GNU General Public + * License, version 2 along with the software. If not, see . + * + * As a special exception, you may use this file as part of a software library without + * restriction. Specifically, if other files instantiate templates or use macros or + * inline functions from this file, or you compile this file and link it with other + * files to produce an executable, this file does not by itself cause the resulting + * executable to be covered by the GNU General Public License. This exception does not + * however invalidate any other reasons why the executable file might be covered by the + * GNU General Public License. + * + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY + * 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. + */ + +#ifndef honeywell_trustability_ssc_h +#define honeywell_trustability_ssc_h + +#include "MotateBuffer.h" +#include "MotateSPI.h" +#include "MotateTWI.h" +#include "MotateUtilities.h" // for to/fromLittle/BigEndian +#include "util.h" // for fp_ZERO + +enum class PressureUnits { PSI, cmH2O, inH20, Pa, kPa }; + +// ToDo - move this to somewhere that is can be used by other pressure sensors +struct PressureSensor { + virtual double getPressure(const PressureUnits output_units) const; +}; + +// See https://en.wikipedia.org/wiki/Standard_litre_per_minute +enum class FlowUnits { LPM, SLM, NLPM }; +struct FlowSensor { + // get the pressure value from the underlying pressure sensor, if any + virtual double getPressure(const PressureUnits output_units) const; + + // get the flow value in the units requested + virtual double getFlow(const FlowUnits output_units) const; +}; + +/* VenturiFlowSensor + * Takes a differentail PressureSensor and some base parameters for a venturi tube and outputs flow values. + */ +class VenturiFlowSensor : public FlowSensor { + PressureSensor* ps; + + double K; // computed from the above + +/* + double upstream_diameter_mm; + double throat_diameter_mm; + double air_density; // kg / m^2 + double discharge_coeffiecient; // percent/100, IOW 0.0-1.0 +*/ + static double compute_k(double upstream_diameter_mm, double throat_diameter_mm, double air_density = 1.2431, double discharge_coeffiecient = 0.95) { + // "don't just do something, stand there" - we'll be verbose and let the compiler optimize it + double upstream_radius_m = (upstream_diameter_mm / 1000.0) / 2.0; // radius in meters + double area_upstream = upstream_radius_m * upstream_radius_m * M_PI; // area in m^2 + + double throat_radius_m = (throat_diameter_mm / 1000.0) / 2.0; // radius in meters + double area_throat = throat_radius_m * throat_radius_m * M_PI; // area in m^2 + + double area_ratio = area_upstream / area_throat; + + // K = C_disc * SQRT( 2 / dens ) * area_A/SQRT( (area_A/area_B)^2 - 1 ) * 1000 + + return discharge_coeffiecient * sqrt(2.0 / air_density) * area_upstream / sqrt((area_ratio * area_ratio) - 1) * + 1000.0; + } + + public: + VenturiFlowSensor(PressureSensor *ps_, double K_) + : ps{ps_}, + K{K_} { + } + + double getFlow(const FlowUnits output_units) const override { + double pressure_diff = ps->getPressure(PressureUnits::Pa); + double flow = K * sqrt(std::abs(pressure_diff)) * (pressure_diff < 0 ? -1 : 1); + + // is SLM right? + if (output_units == FlowUnits::LPM || output_units == FlowUnits::SLM) { + return flow * 60.0; + } + + // this is WRONG, but shuts the compiler up: + return 0.0; + } + + double getPressure(const PressureUnits output_units) const override { return ps->getPressure(output_units); } +}; + + +class HoneywellTruStabilityBase : virtual public PressureSensor { + public: + HoneywellTruStabilityBase(const uint16_t min_output, const uint16_t max_output, const double min_value, + const double max_value, const PressureUnits base_units) + : _min_output{min_output}, + _max_output{max_output}, + _min_value{min_value}, + _max_value{max_value}, + _base_units{base_units} {}; + + // Prevent copying, and prevent moving (so we know if it happens) + HoneywellTruStabilityBase(const HoneywellTruStabilityBase &) = delete; + + protected: + std::function _interrupt_handler; + + // Parameters of the sensor - for now, compile-time + const uint16_t _min_output; + const uint16_t _max_output; + const double _min_value; + const double _max_value; + const PressureUnits _base_units; + + double zero_offset; + + // ########### + // The HoneyWell TruStability SSC devices are really simple to talk to, and can be in SPI or I2C configurations: + + // SPI: See https://sensing.honeywell.com/spi-comms-digital-ouptu-pressure-sensors-tn-008202-3-en-final-30may12.pdf + // Read from 2 to 4 byts of data, interpret it, repeat. + // The bytes are: Status_and_BridgeData_MSB, BridgeData_LSB, TemperatureData_MSB, TemperatureData_LSB + + // I2C: See https://sensing.honeywell.com/i2c-comms-digital-output-pressure-sensors-tn-008201-3-en-final-30may12.pdf + // Same data as SPI + + enum { + INITING, + WAITING_FOR_SAMPLE, + NEEDS_SAMPLED, + } _state; + + bool _data_needs_read = false; + static const size_t _data_size = 4; + struct data_t { + union { + uint8_t raw_data[8]; + struct { + uint8_t bridge_msb : 6; + uint8_t status : 2; + uint8_t bridge_lsb : 8; + uint8_t temperature_msb : 8; + uint8_t temperature_lsb : 3; + uint8_t unused : 5; + }; + }; + }; + alignas(4) data_t _data; + + double temperature = 0; + double pressure = 0; + + static constexpr uint8_t STALE_DATA = 0b10; + + void _postReadSampleData() { + if (INITING != _state && _data.status == STALE_DATA) { + // we requested data too soon, the data is stale, try again sooner + _data_needs_read = true; + return; + } + + int32_t bridge_output = (_data.bridge_msb << 8) | _data.bridge_lsb; + // uint32_t temperature_output = (_data.temperature_msb << 3) | _data.temperature_lsb; + + // Formula + // ((ouput - output_min)*(pressure_max-pressure_min))/(outout_max-output_min)+pressure_min + + if (bridge_output < _min_output || bridge_output > _max_output) { + pressure = 0.0; + // error condition + } + + double temp_pressure = + ((double)(bridge_output - _min_output) * (double)(_max_value - _min_value)) / (_max_output - _min_output) + + _min_value; + + const double noise_min = ((_max_value - _min_value) * 0.0025); + + // if we're within 0.25% of zero, adjust the zero offset - slowly + if ((temp_pressure > -noise_min) && (temp_pressure < noise_min)) { + zero_offset = (zero_offset * 0.999) + (temp_pressure * 0.001); + } + + pressure = (pressure * 0.9) + ((temp_pressure - zero_offset) * 0.1); + // pressure = temp_pressure - zero_offset; + + if (_interrupt_handler) { + _interrupt_handler(true); + _interrupt_handler = nullptr; + } + }; + + public: + double getPressure(const PressureUnits output_units) const override { + if (output_units != _base_units) { + if (_base_units == PressureUnits::PSI && output_units == PressureUnits::cmH2O) { + // the only currently supported configuration + return pressure * 70.3069578296; + } + + if (_base_units == PressureUnits::PSI && output_units == PressureUnits::Pa) { + // the only currently supported configuration + return pressure * 6894.75729; + } + } + + return pressure; + }; +}; + +// empty template + +template +struct HoneywellTruStability : private HoneywellTruStabilityBase {}; // marked private to force an error + +// Final SPI class +template +struct HoneywellTruStability>> : public HoneywellTruStabilityBase { + using SPIMessage = Motate::SPIMessage; + using SPIInterrupt = Motate::SPIInterrupt; + using SPIDeviceMode = Motate::SPIDeviceMode; + + // SPI and message handling properties + device_t _device; + SPIMessage _message; + + // Record if we're transmitting to prevent altering the buffers while they + // are being transmitted still. + volatile bool _transmitting = false; + + // We don't want to transmit until we're inited + bool _inited = false; + + // Constructor - this is the only time we directly use the SBIBus + template + HoneywellTruStability(SPIBus_t &spi_bus, const chipSelect_t &_cs, const uint16_t min_output, const uint16_t max_output, + const double min_value, const double max_value, const PressureUnits base_units) + : HoneywellTruStabilityBase{min_output, max_output, min_value, max_value, base_units}, + _device{spi_bus.getDevice(_cs, 5000000, SPIDeviceMode::kSPIMode0 | SPIDeviceMode::kSPI8Bit, + 400, // min_between_cs_delay_ns + 400, // cs_to_sck_delay_ns + 80 // between_word_delay_ns + )} { + init(); + }; + + // Prevent copying, and prevent moving (so we know if it happens) + HoneywellTruStability(const HoneywellTruStability &) = delete; + HoneywellTruStability(HoneywellTruStability &&other) : _device{std::move(other._device)} {}; + + alignas(4) uint8_t _scribble_buffer[8]; + + void _startNextReadWrite() { + if (_transmitting || !_inited) { + return; + } + _transmitting = true; // preemptively say we're transmitting .. as a mutex + + // check if we need to read reagisters + if (!_data_needs_read) { + _transmitting = false; // we're not really transmitting. + return; + } + _data_needs_read = false; + + // reading, prepare the address in the scribble buffer + _message.setup(_scribble_buffer, (uint8_t *)&_data.raw_data, 4, SPIMessage::DeassertAfter, SPIMessage::EndTransaction); + _device.queueMessage(&_message); + }; + + void _doneReadingCallback() { + _transmitting = false; + _postReadSampleData(); + }; + + void init() { + _message.message_done_callback = [&] { this->_doneReadingCallback(); }; + + _inited = true; + }; + + + void startSampling(std::function &&handler) { + _interrupt_handler = std::move(handler); + + _data_needs_read = true; + _startNextReadWrite(); + }; +}; + +// Final TWI class +template +struct HoneywellTruStability>> : public HoneywellTruStabilityBase { + using TWIMessage = Motate::TWIMessage; + using TWIInterrupt = Motate::TWIInterrupt; + + // SPI and message handling properties + device_t _device; + TWIMessage _message; + + // Record if we're transmitting to prevent altering the buffers while they + // are being transmitted still. + volatile bool _transmitting = false; + + // We don't want to transmit until we're inited + bool _inited = false; + + // Constructor - this is the only time we directly use the TWIBus + template + HoneywellTruStability(TWIBus_t &twi_bus, const uint8_t &address, Ts... v, const uint16_t min_output, const uint16_t max_output, + const double min_value, const double max_value, const PressureUnits base_units) + : HoneywellTruStabilityBase{min_output, max_output, min_value, max_value, base_units}, + _device{twi_bus.getDevice({address, Motate::TWIDeviceAddressSize::k7Bit}, v...)} { + init(); + }; + + // Prevent copying, and prevent moving (so we know if it happens) + HoneywellTruStability(const HoneywellTruStability &) = delete; + HoneywellTruStability(HoneywellTruStability &&other) : _device{std::move(other._device)} {}; + + void _startNextReadWrite() { + if (_transmitting || !_inited) { + return; + } + _transmitting = true; // preemptively say we're transmitting .. as a mutex + + // check if we need to read reagisters + if (!_data_needs_read) { + _transmitting = false; // we're not really transmitting. + return; + } + _data_needs_read = false; + + using dir = TWIMessage::Direction; + + // reading, prepare the address in the scribble buffer + _message.setup((uint8_t *)&_data.raw_data, 4, dir::kRX); + _device.queueMessage(&_message); + }; + + void _doneReadingCallback() { + _transmitting = false; + _postReadSampleData(); + }; + + void init() { + _message.message_done_callback = [&](bool){ this->_doneReadingCallback(); }; + + _inited = true; + }; + + void startSampling(std::function &&handler) { + _interrupt_handler = std::move(handler); + _data_needs_read = true; + _startNextReadWrite(); + }; +}; + +#endif // honeywell_trustability_ssc_h diff --git a/g2core/device/max31865/max31865.h b/g2core/device/max31865/max31865.h index 394aa88c0..a5505db45 100644 --- a/g2core/device/max31865/max31865.h +++ b/g2core/device/max31865/max31865.h @@ -38,13 +38,13 @@ #include "MotateBuffer.h" #include "MotateUtilities.h" // for to/fromLittle/BigEndian -using Motate::SPIMessage; -using Motate::SPIInterrupt; -using Motate::SPIDeviceMode; - // Complete class for MAX31865 drivers. template struct MAX31865 final { + using SPIMessage = Motate::SPIMessage; + using SPIInterrupt = Motate::SPIInterrupt; + using SPIDeviceMode = Motate::SPIDeviceMode; + // SPI and message handling properties device_t _device; SPIMessage _message; diff --git a/g2core/kinematics.cpp b/g2core/kinematics.cpp index aa0faed01..082f48a46 100644 --- a/g2core/kinematics.cpp +++ b/g2core/kinematics.cpp @@ -130,6 +130,260 @@ stat_t kn_get_pos_d(nvObj_t *nv) return (STAT_OK); }; #endif // KINEMATICS==KINE_FOUR_CABLE +#if KINEMATICS==KINE_PRESSURE +#include "kinematics_pressure.h" +PressureKinematics pressure_kinematics; +KinematicsBase *kn = &pressure_kinematics; + +// volume +stat_t kn_get_force(nvObj_t *nv) +{ + nv->valuetype = TYPE_FLOAT; + nv->precision = 4; + nv->value_flt = pressure_kinematics.event_pressure_target; // read it as a float + + return (STAT_OK); +}; +stat_t kn_set_force(nvObj_t *nv) +{ + float value = nv->value_flt; // read it as a float + nv->precision = 4; + pressure_kinematics.event_pressure_target = value; + return (STAT_OK); +}; + +stat_t kn_get_target(nvObj_t *nv) +{ + nv->valuetype = TYPE_FLOAT; + nv->precision = 4; + nv->value_flt = pressure_kinematics.event_pressure_target; // read it as a float + + return (STAT_OK); +}; +stat_t kn_set_target(nvObj_t *nv) +{ + float value = nv->value_flt; // read it as a float + nv->precision = 4; + pressure_kinematics.event_pressure_target = value; + return (STAT_OK); +}; + +stat_t kn_get_epm(nvObj_t *nv) +{ + nv->valuetype = TYPE_FLOAT; + nv->precision = 4; + nv->value_flt = 60.0 / pressure_kinematics.seconds_between_events; + + return (STAT_OK); +}; +stat_t kn_set_epm(nvObj_t *nv) +{ + float value = nv->value_flt; // read it as a float + nv->precision = 4; + pressure_kinematics.seconds_between_events = 60.0 / value; + pressure_kinematics.seconds_to_hold_event = + pressure_kinematics.seconds_between_events / (pressure_kinematics.pressure_hold_release_ratio + 1); + + return (STAT_OK); +}; + + +stat_t kn_get_hold_time(nvObj_t *nv) +{ + nv->valuetype = TYPE_FLOAT; + nv->precision = 4; + nv->value_flt = pressure_kinematics.seconds_to_hold_event; + + return (STAT_OK); +}; +stat_t kn_set_hold_time(nvObj_t *nv) +{ + float value = nv->value_flt; // read it as a float + nv->precision = 4; + pressure_kinematics.seconds_to_hold_event = value; + pressure_kinematics.pressure_hold_release_ratio = + (pressure_kinematics.seconds_between_events / pressure_kinematics.seconds_to_hold_event) - 1; + + return (STAT_OK); +}; + + +stat_t kn_get_hold_ratio(nvObj_t *nv) +{ + nv->valuetype = TYPE_FLOAT; + nv->precision = 4; + nv->value_flt = pressure_kinematics.pressure_hold_release_ratio; + + return (STAT_OK); +}; +stat_t kn_set_hold_ratio(nvObj_t *nv) +{ + float value = nv->value_flt; // read it as a float + nv->precision = 4; + pressure_kinematics.pressure_hold_release_ratio = value; + pressure_kinematics.seconds_to_hold_event = + pressure_kinematics.seconds_between_events / (pressure_kinematics.pressure_hold_release_ratio + 1); + return (STAT_OK); +}; + + +stat_t kn_get_backoff_pressure(nvObj_t *nv) +{ + nv->valuetype = TYPE_FLOAT; + nv->precision = 4; + nv->value_flt = pressure_kinematics.reverse_target_pressure; + + return (STAT_OK); +}; +stat_t kn_set_backoff_pressure(nvObj_t *nv) +{ + float value = nv->value_flt; // read it as a float + nv->precision = 4; + pressure_kinematics.reverse_target_pressure = value; + return (STAT_OK); +}; + + +stat_t kn_get_e_value(nvObj_t *nv) +{ + nv->valuetype = TYPE_FLOAT; + nv->precision = 4; + nv->value_flt = pressure_kinematics.sensor_error_store; + + return (STAT_OK); +}; +stat_t kn_get_i_value(nvObj_t *nv) +{ + nv->valuetype = TYPE_FLOAT; + nv->precision = 4; + nv->value_flt = pressure_kinematics.sensor_integral_store; + + return (STAT_OK); +}; +stat_t kn_get_d_value(nvObj_t *nv) +{ + nv->valuetype = TYPE_FLOAT; + nv->precision = 4; + nv->value_flt = pressure_kinematics.sensor_derivative_store; + + return (STAT_OK); +}; + +stat_t kn_get_uoc_value(nvObj_t *nv) +{ + nv->valuetype = TYPE_INTEGER; + nv->value_int = pressure_kinematics.unable_to_obtian_error_counter; + + return (STAT_OK); +}; +stat_t kn_get_umc_value(nvObj_t *nv) +{ + nv->valuetype = TYPE_INTEGER; + nv->value_int = pressure_kinematics.unable_to_maintian_error_counter; + + return (STAT_OK); +}; +stat_t kn_get_ec_value(nvObj_t *nv) +{ + nv->valuetype = TYPE_INTEGER; + nv->value_int = pressure_kinematics.event_counter; + + return (STAT_OK); +}; + +stat_t kn_get_p_factor(nvObj_t *nv) +{ + nv->valuetype = TYPE_FLOAT; + nv->precision = 4; + nv->value_flt = pressure_kinematics.sensor_proportional_factor; + + return (STAT_OK); +}; +stat_t kn_set_p_factor(nvObj_t *nv) +{ + float value = nv->value_flt; // read it as a float + nv->precision = 4; + pressure_kinematics.sensor_proportional_factor = value; + return (STAT_OK); +}; + + +stat_t kn_get_i_factor(nvObj_t *nv) +{ + nv->valuetype = TYPE_FLOAT; + nv->precision = 4; + nv->value_flt = pressure_kinematics.sensor_inetgral_factor; + + return (STAT_OK); +}; +stat_t kn_set_i_factor(nvObj_t *nv) +{ + float value = nv->value_flt; // read it as a float + nv->precision = 4; + pressure_kinematics.sensor_inetgral_factor = value; + return (STAT_OK); +}; + + +stat_t kn_get_d_factor(nvObj_t *nv) +{ + nv->valuetype = TYPE_FLOAT; + nv->precision = 4; + nv->value_flt = pressure_kinematics.sensor_derivative_factor; + + return (STAT_OK); +}; +stat_t kn_set_d_factor(nvObj_t *nv) +{ + float value = nv->value_flt; // read it as a float + nv->precision = 4; + pressure_kinematics.sensor_derivative_factor = value; + return (STAT_OK); +}; + +// anchored + +stat_t kn_get_anchored(nvObj_t *nv) +{ + nv->valuetype = TYPE_BOOLEAN; + nv->value_flt = pressure_kinematics.anchored(); + + return (STAT_OK); +}; +stat_t kn_set_anchored(nvObj_t *nv) +{ + bool value = std::fabs(nv->value_flt) > 0.1; + pressure_kinematics.anchored(value); + return (STAT_OK); +}; + +stat_t _kn_get_pos(uint8_t joint, nvObj_t *nv) { + float position[AXES]; + pressure_kinematics.get_position(position); + + nv->valuetype = TYPE_FLOAT; + nv->precision = 4; + nv->value_flt = position[joint]; + + return (STAT_OK); +} + +stat_t kn_get_pos_1(nvObj_t *nv) { return _kn_get_pos(1-1, nv); } +stat_t kn_get_pos_2(nvObj_t *nv) { return _kn_get_pos(2-1, nv); } +stat_t kn_get_pos_3(nvObj_t *nv) { return _kn_get_pos(3-1, nv); } +stat_t kn_get_pos_4(nvObj_t *nv) { return _kn_get_pos(4-1, nv); } +stat_t kn_get_pos_5(nvObj_t *nv) { return _kn_get_pos(5-1, nv); } + +stat_t get_flow_volume(nvObj_t *nv) +{ + nv->valuetype = TYPE_FLOAT; + nv->precision = 4; + nv->value_flt = pressure_kinematics.volume_value[0]; + + return (STAT_OK); +}; + +#endif // KINEMATICS==KINE_PRESSURE // Concrete functions that involve kinematics diff --git a/g2core/kinematics.h b/g2core/kinematics.h index ee269f0b3..1a88fdec5 100644 --- a/g2core/kinematics.h +++ b/g2core/kinematics.h @@ -97,6 +97,55 @@ stat_t kn_get_pos_b(nvObj_t *nv); stat_t kn_get_pos_c(nvObj_t *nv); stat_t kn_get_pos_d(nvObj_t *nv); #endif +#if KINEMATICS==KINE_PRESSURE +// force +stat_t kn_get_force(nvObj_t *nv); +stat_t kn_set_force(nvObj_t *nv); + +stat_t kn_get_target(nvObj_t *nv); +stat_t kn_set_target(nvObj_t *nv); + +stat_t kn_get_epm(nvObj_t *nv); +stat_t kn_set_epm(nvObj_t *nv); + +stat_t kn_get_hold_time(nvObj_t *nv); +stat_t kn_set_hold_time(nvObj_t *nv); + +stat_t kn_get_hold_ratio(nvObj_t *nv); +stat_t kn_set_hold_ratio(nvObj_t *nv); + +stat_t kn_get_e_value(nvObj_t *nv); +stat_t kn_get_i_value(nvObj_t *nv); +stat_t kn_get_d_value(nvObj_t *nv); + +stat_t kn_get_uoc_value(nvObj_t *nv); +stat_t kn_get_umc_value(nvObj_t *nv); +stat_t kn_get_ec_value(nvObj_t *nv); + +stat_t kn_get_p_factor(nvObj_t *nv); +stat_t kn_set_p_factor(nvObj_t *nv); +stat_t kn_get_i_factor(nvObj_t *nv); +stat_t kn_set_i_factor(nvObj_t *nv); +stat_t kn_get_d_factor(nvObj_t *nv); +stat_t kn_set_d_factor(nvObj_t *nv); + +// anchored +stat_t kn_get_anchored(nvObj_t *nv); +stat_t kn_set_anchored(nvObj_t *nv); + +stat_t kn_get_backoff_pressure(nvObj_t *nv); +stat_t kn_set_backoff_pressure(nvObj_t *nv); + +// joint positions +stat_t kn_get_pos_1(nvObj_t *nv); +stat_t kn_get_pos_2(nvObj_t *nv); +stat_t kn_get_pos_3(nvObj_t *nv); +stat_t kn_get_pos_4(nvObj_t *nv); +stat_t kn_get_pos_5(nvObj_t *nv); + +stat_t get_flow_volume(nvObj_t *nv); + +#endif // KINEMATICS==KINE_PRESSURE void kn_config_changed(); void kn_forward_kinematics(const float steps[], float travel[]); diff --git a/g2core/kinematics_pressure.h b/g2core/kinematics_pressure.h new file mode 100644 index 000000000..2ebad4d63 --- /dev/null +++ b/g2core/kinematics_pressure.h @@ -0,0 +1,569 @@ +/* + * kinematics.h - inverse kinematics routines + * This file is part of the g2core project + * + * Copyright (c) 2013 - 2018 Alden S. Hart, Jr. + * + * This file ("the software") is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License, version 2 as published by the + * Free Software Foundation. You should have received a copy of the GNU General Public + * License, version 2 along with the software. If not, see . + * + * As a special exception, you may use this file as part of a software library without + * restriction. Specifically, if other files instantiate templates or use macros or + * inline functions from this file, or you compile this file and link it with other + * files to produce an executable, this file does not by itself cause the resulting + * executable to be covered by the GNU General Public License. This exception does not + * however invalidate any other reasons why the executable file might be covered by the + * GNU General Public License. + * + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY + * 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. + */ + +#ifndef KINEMATICS_FOUR_CABLE_H_ONCE +#define KINEMATICS_FOUR_CABLE_H_ONCE + +#include "g2core.h" +#include "config.h" +#include "canonical_machine.h" +#include "stepper.h" +#include "kinematics.h" +#include "util.h" +#include "settings.h" +#include "gpio.h" +#include "encoder.h" // for encoder grabbing + +#include + +#include "kinematics.h" + +template +struct PressureKinematics : KinematicsBase { + static const uint8_t joints = axes; // For cartesian we have one joint per axis + + double joint_vel[4]; + double joint_accel[4]; + double joint_jerk[4]; + + static constexpr uint8_t pressure_sensor_count = 1; + double raw_pressure_value[pressure_sensor_count]; // filtered value as read off the sensor + double zero_pressure_value[pressure_sensor_count]; // stored zero value for pressure + double pressure_pid_output[pressure_sensor_count]; // value after PID + double prev_pressure_pid_output[pressure_sensor_count]; // previous value after PID + + static constexpr uint8_t flow_sensor_count = 1; + double flow_value[flow_sensor_count]; // stored from last time they were read + double volume_value[flow_sensor_count]; // stored from last time they were read + double prev_volume_value[flow_sensor_count]; // stored from last time they were read + + double immediate_pressure_target = 0.0; + double sensor_proportional_factor = 550; + double sensor_integral_store = 0; + double sensor_inetgral_factor = 0.005; + double sensor_error_store = 0; + double sensor_derivative_factor = 3000; + double sensor_derivative_store = 0; + double derivative_contribution = 1.0/10.0; + + double reverse_target_pressure = 0; + + const float sensor_skip_detection_jump = 10000; + + double event_pressure_target = 0; + double seconds_between_events = 6.0; + double seconds_to_hold_event = 2; + double pressure_hold_release_ratio = 3; + + bool is_anchored = false; + + double prev_joint_position[4]; + double prev_joint_vel[4]; + double prev_joint_accel[4]; + double joint_min_limit[motors]; + double joint_max_limit[motors]; + + float start_velocities[motors]; + float end_velocities[motors]; + double target_accel[4] = {0.0, 0.0, 0.0, 0.0}; + bool last_switch_state[4]; + + enum class PressureState { Idle, Start, Hold, Release }; + PressureState pressure_state = PressureState::Idle; + + int32_t unable_to_obtian_error_counter = 0; + int32_t unable_to_maintian_error_counter = 0; + int32_t event_counter = 0; + + // use a timer to let the sensors be initied and their readings settle + Motate::Timeout sensor_settle_timer; + Motate::Timeout hold_pressure_timer; + Motate::Timeout inter_event_timer; + + #define ANCHOR_A_INPUT 1 + #define ANCHOR_B_INPUT 2 + #define ANCHOR_C_INPUT 3 + #define ANCHOR_D_INPUT 4 + + // setup the inputs array - compile-time for now + gpioDigitalInputReader * const anchor_inputs[4] = { + in_r[ANCHOR_A_INPUT-1], + in_r[ANCHOR_B_INPUT-1], + in_r[ANCHOR_C_INPUT-1], + in_r[ANCHOR_D_INPUT-1], + }; + + PressureSensor *const pressure_sensors[pressure_sensor_count] = {&pressure_sensor1}; + FlowSensor *const flow_sensors[flow_sensor_count] = {&flow_sensor1}; + + // 1 - let the sensors settle + // 2 - back the motors off 10mm (SKIP for now) + // 3 - read the sensors - record that as baseline + // 4 - start normal idle activity + + PressureKinematics() { + sensor_settle_timer.clear(); + hold_pressure_timer.clear(); + } + + // Joints are defined as these axes in order: + // 0 = X + // 1 = Y + // 2 = Z + // 3 = A + // 4 = B + // 5 = C + // 6 = U (maybe) + // 7 = V (maybe) + // 8 = W (maybe) + + float steps_per_unit[motors]; + float motor_offset[motors]; + bool needs_sync_encoders = true; // if true, we need to update the steps_offset + int8_t motor_map[motors]; // for each motor, which joint it maps from + + float joint_position[joints]; + + void configure(const float new_steps_per_unit[motors], const int8_t new_motor_map[motors]) override + { + for (uint8_t motor = 0; motor < motors; motor++) { + motor_map[motor] = new_motor_map[motor]; + int8_t joint = motor_map[motor]; + if (joint == -1) { + motor_offset[motor] = 0; + steps_per_unit[motor] = 1; + } else { + float steps = (joint_position[joint] * steps_per_unit[motor]) + motor_offset[motor]; + steps_per_unit[motor] = new_steps_per_unit[motor]; + motor_offset[motor] = steps - (joint_position[joint] * steps_per_unit[motor]); + } + + joint_max_limit[joint] = joint_position[joint] + cm->a[joint].travel_max; + joint_min_limit[joint] = joint_position[joint] + cm->a[joint].travel_min; + } + } + + void inverse_kinematics(const GCodeState_t &gm, const float target[axes], const float position[axes], const float start_velocity, + const float end_velocity, const float segment_time, float steps[motors]) override + { + // joint == axis in cartesian kinematics + for (uint8_t motor = 0; motor < motors; motor++) { + int8_t joint = motor_map[motor]; + if (joint == -1) { + continue; + } + + steps[motor] = (target[joint] * steps_per_unit[motor]) + motor_offset[motor]; + } + + for (uint8_t joint = 0; joint < joints; joint++) { + joint_position[joint] = target[joint]; + } + + last_segment_was_idle = false; + } + + void get_position(float position[axes]) override + { + for (uint8_t axis = 0; axis < axes; axis++) { + position[axis] = joint_position[axis]; + } + } + + float best_steps_per_unit[axes]; + + void forward_kinematics(const float steps[joints], float position[axes]) override + { + // Setup + for (uint8_t axis = 0; axis < axes; axis++) { + position[axis] = 0.0; + } + for (uint8_t motor = 0; motor < motors; motor++) { + int8_t joint = motor_map[motor]; + if (joint == -1) { + continue; + } + + auto axis = joint; // it's cartesian, baby! + best_steps_per_unit[axis] = -1.0; + + // If this motor has a better (or the only) resolution, then we use this motor's value + if (best_steps_per_unit[axis] < steps_per_unit[motor]) { + best_steps_per_unit[axis] = steps_per_unit[motor]; + position[axis] = (steps[motor]-motor_offset[motor]) / steps_per_unit[motor]; + } + + joint_position[joint] = position[joint]; + } + } + + void sync_encoders(const float step_position[motors], const float position[axes]) override { + // We need to make joint_offset[joint] adjust any given position so that if it's given as a target + // to inverse_kinematics then step_position[motor] will be given as the return steps[motor] + + // Why? Externally position[] may be unrelated to step_position[], so we need to adjust. + for (uint8_t motor = 0; motor < motors; motor++) { + int8_t joint = motor_map[motor]; + if (joint == -1) { + continue; + } + + // This, solved for motor_offset: step_position[motor] = (position[joint]] * steps_per_unit[motor]) + motor_offset[motor]; + motor_offset[motor] = step_position[motor] - (position[joint] * steps_per_unit[motor]); + + joint_max_limit[joint] = position[joint] + cm->a[joint].travel_max; + joint_min_limit[joint] = position[joint] + cm->a[joint].travel_min; + } + } + + bool read_sensors() { + for (uint8_t joint = 0; joint < pressure_sensor_count; joint++) { + raw_pressure_value[joint] = + pressure_sensors[joint]->getPressure(PressureUnits::cmH2O) - zero_pressure_value[joint]; + + double e = (immediate_pressure_target - raw_pressure_value[joint]); + + sensor_integral_store += e; + // if (sensor_integral_store > 10000) { + // sensor_integral_store = 10000; + // } else if (sensor_integral_store < -10000) { + // sensor_integral_store = -10000; + // } + + double p_v = e * sensor_proportional_factor; + double i_v = sensor_integral_store * sensor_inetgral_factor; + sensor_derivative_store = (e - sensor_error_store)*(derivative_contribution) + (sensor_derivative_store * (1.0-derivative_contribution)); + double d_v = sensor_derivative_store * sensor_derivative_factor; + sensor_error_store = e; + + double new_pressure_pid_output = p_v + i_v - d_v; + + prev_pressure_pid_output[joint] = pressure_pid_output[joint]; + pressure_pid_output[joint] = new_pressure_pid_output; + // *0.1 + prev_pressure_pid_output[joint] * 0.9; + + // read differential pressure from the volume sensors + flow_value[joint] = flow_sensors[joint]->getFlow(FlowUnits::SLM); + volume_value[joint] = volume_value[joint] + flow_value[joint] * MIN_SEGMENT_TIME; // SLM and MIN_SEGMENT_TIME are both in minutes - nice! + if ((raw_pressure_value[joint] < 0.1 && abs(flow_value[joint])<5.0) || volume_value[joint] < 0) { + volume_value[joint] = 0; + } + } + return true; + } + + bool anchored() { return is_anchored; } + void anchored(bool v) { + is_anchored = v; + + // if we are setting it to false, do NOT reset the cables + if (!is_anchored) { + return; + } + + // nothing to do yet + } + + // control and handle state machine changes + + void change_state_to_start() { + // this means the EPM timer expired - this shoutd be on an Idle -> Start transition + // we'll also accept a Release -> Start transition + // if not, we have an error to deal with + + if ((PressureState::Idle != pressure_state) && (PressureState::Release != pressure_state)) { + // we were unable to hold or obtain pressure - we need to move to release + change_state_to_release(); + } else if (fp_ZERO(event_pressure_target)) { + // no target pressure, go to (or stay in) idle instead + return change_state_to_release(); + } else { + pressure_state = PressureState::Start; + + immediate_pressure_target = event_pressure_target; + } + + // wipe out the PID integral, as in all the cases we reverse directions + sensor_integral_store = 0; + + // restart the timer + inter_event_timer.set(seconds_between_events * 1000.0); + event_counter++; + } + + void change_state_to_hold() { + // this should only go into hold if called from Start + if (PressureState::Start == pressure_state) { + pressure_state = PressureState::Hold; + hold_pressure_timer.set(seconds_to_hold_event*1000.0); + } + + + // Other possibilities: + // * Already in Hold: Still holding + // * In Release: pressure still high from Hold state + // * In Idle: Negative pressue used to Release is still in effect + } + + void change_state_to_release() { + // we have an error of some sort + if (PressureState::Start == pressure_state) { + unable_to_obtian_error_counter++; + } + else if (PressureState::Hold == pressure_state) { + if (!hold_pressure_timer.isPast()) { + unable_to_maintian_error_counter++; + } + } + + pressure_state = PressureState::Release; + immediate_pressure_target = reverse_target_pressure; + } + + void change_state_to_idle() { + if (PressureState::Release != pressure_state) { + // shouldn't happen? + } + + pressure_state = PressureState::Idle; + } + + // if we requested a move, return true, otherwise false + bool last_segment_was_idle = false; + bool over_pressure = false; + bool idle_task() override { + /* Notes about this situation: + * 1. This is called from Exec, which is called from Load, which is called (ignoring bootstrapping) from the stepper + * when a segment is over. + * 2. The currently running segment in the stepper subsystem (which may be a movement-free segment) has a target of + * the current joint_position[] (as it's known in this part of the code) and the start position of + * prev_joint_position[]. + * 3. The encoder was read during the last segment encoder_readings_taken[] times (may be zero). + * 4. If encoder_readings_taken[] is non-zero, then the last reading was taken at some point during the last segment, + * and should be somewhere between prev_joint_position[] and joint_position[]. + */ + + if (!read_sensors() || is_anchored) { + return false; // too soon - sensors are still settling + } + + if (!last_segment_was_idle) { + for (uint8_t joint = 0; joint < pressure_sensor_count; joint++) { + joint_vel[joint] = 0.0; + joint_accel[joint] = 0.0; + joint_jerk[joint] = 0.0; + pressure_pid_output[joint] = 0.0; + + sensor_error_store = 0.0; + sensor_integral_store = 0.0; + sensor_derivative_store = 0.0; + } + // change_state_to_calibrating(); + } + last_segment_was_idle = true; + + /* + immediate_pressure_target + float seconds_between_events = 6.0; + float seconds_to_hold_event = 2; + inter_event_timer + */ + + if (!inter_event_timer.isSet() || inter_event_timer.isPast()) { + change_state_to_start(); + } + + // check inputs to be sure we aren't anchored + // if we are anchored, then set the zero-position offsets + // the first segment that isn't anchored will use them + + const double segment_time = MIN_SEGMENT_TIME; // time in MINUTES + // const double segment_time_2 = segment_time * segment_time; // time in MINUTES + // const double segment_time_3 = segment_time * segment_time * segment_time; // time in MINUTES + + for (uint8_t joint = 0; joint < pressure_sensor_count; joint++) { + // capture the switch state + bool switch_state = anchor_inputs[joint]->getState(); + + // determine if we're NOW at or over pressure - we just call it "over_pressure" for brevity + bool at_pressure_detected = (raw_pressure_value[joint] > (immediate_pressure_target*0.80)); + + if ((PressureState::Hold == pressure_state) && hold_pressure_timer.isPast()) { + change_state_to_release(); + } else if (at_pressure_detected) { + change_state_to_hold(); // detects if already in hold, or not in Start + } + + if (switch_state) { + if (PressureState::Release == pressure_state) { + // stop the motion + // immediate_pressure_target = 0.1; + sensor_integral_store = 0; + + change_state_to_idle(); + } + if (!last_switch_state[joint]) { + joint_min_limit[joint] = joint_position[joint] + cm->a[AXIS_X].travel_min; + } + + } else if (!switch_state) { + if (last_switch_state[joint]) { + // just left the switch, record how far we can go + joint_max_limit[joint] = joint_position[joint] + cm->a[AXIS_X].travel_max; + } else { + // check to make sure we haven't gone too far + if (joint_position[joint] > joint_max_limit[joint]) { + change_state_to_release(); + } + } + } + + // prev_joint_accel[joint] = joint_accel[joint]; + start_velocities[joint] = std::abs(joint_vel[joint]); + + double jmax = cm->a[AXIS_X].jerk_max * JERK_MULTIPLIER; + const double vmax = cm->a[AXIS_X].velocity_max; + + prev_joint_position[joint] = joint_position[joint]; + double old_joint_vel = joint_vel[joint]; + double old_joint_accel = joint_accel[joint]; + + // treat pressure_pid_output[joint] as velocity, but we have to jerk-control it + + double requested_velocity = pressure_pid_output[joint]; + + // if we're releasing, target -vmax + if (PressureState::Release == pressure_state) { + requested_velocity = -vmax; + } + // if we're idle, target no velocity + else if (PressureState::Idle == pressure_state) { + requested_velocity = 0; + } + // limit velocity to stop at zero for polarity changes, and then limit to +- max + else if ((old_joint_vel < -1) && (requested_velocity > 1)) { + requested_velocity = 0; + } + else if ((old_joint_vel > 1) && (requested_velocity < -1)) { + requested_velocity = 0; + } + else if (requested_velocity < -vmax) { + requested_velocity = -vmax; + } else if (requested_velocity > vmax) { + requested_velocity = vmax; + } + + // double requested_accel = (requested_velocity - old_joint_vel) / segment_time - (jmax * segment_time)/2; + + // Notes: + // * velocity can be negative, that's valid + // * "maximum acceleration" is an absolute maximum - positive or negative + + // this will always be positive !! + double max_accel = sqrt(std::abs(requested_velocity - old_joint_vel) * jmax * 2.0); + double sign = 1.0; + // choose a jerk value that will not violate the max_acceleration withing two time segments + if ((requested_velocity - old_joint_vel) < 0.0) { + // want to accelerate in the negative direction + sign = -1.0; + } + + if ((std::abs(old_joint_accel) + jmax * segment_time * 4.0) < max_accel) { + joint_accel[joint] = (std::abs(old_joint_accel) + jmax * segment_time) * sign; + joint_jerk[joint] = jmax * sign; + } else { + joint_accel[joint] = (std::abs(old_joint_accel) - jmax * segment_time) * sign; + joint_jerk[joint] = -jmax * sign; + } + joint_vel[joint] = joint_vel[joint] + joint_accel[joint]*segment_time + jmax*segment_time*segment_time*0.5; + + // limit velocity + if (joint_vel[joint] < -vmax) { + joint_vel[joint] = -vmax; + } else if (joint_vel[joint] > vmax) { + joint_vel[joint] = vmax; + } + + // now that everything is done adjusting joint_vel[joint], we can recompute joint_accel[joint] + joint_accel[joint] = (joint_vel[joint] - old_joint_vel) / segment_time - jmax * segment_time * 0.5; + + // we check if we'll violate min or max position with the next position - last-change to stop driving into the wall + auto proposed_position = joint_position[joint] + ((old_joint_vel + joint_vel[joint]) * 0.5 * segment_time); + + // if the switch is closed, we may still have some room to stop cleanly + if (((switch_state) && (proposed_position < joint_min_limit[joint]) && (joint_vel[joint] < 0)) || + ((proposed_position > joint_max_limit[joint]) && (joint_vel[joint] > 0))) { + + if (joint_vel[joint] > 0) { + // we moved too far, give up on this pass + change_state_to_release(); + } + + // prevent the integral from winding up positive, pushing past the switch + sensor_integral_store = 0; + + joint_vel[joint] = joint_vel[joint] * 0.5; // drop the velocity hard -- we should probably do this more intelligently + + // this is a lie - we've certainly violated jerk, so don't punish the acceleration counter + joint_accel[joint] = 0; + } + + joint_position[joint] = joint_position[joint] + ((old_joint_vel + joint_vel[joint]) * 0.5 * segment_time); + end_velocities[joint] = std::abs(joint_vel[joint]); + + // sanity check, we can't do a reversal in the middle of a segment, + // so the start velocity and the end velocity have to have the same sign + // note: start_velocities[joint] and end_velocities[joint] are both ABS, so this sign change is lost there! + if (((old_joint_vel > 0) && (joint_vel[joint] < 0)) || ((old_joint_vel < 0) && (joint_vel[joint] > 0))) { + // solution: since we are reversing, we are going to start from zero + start_velocities[joint] = std::abs(old_joint_vel + joint_vel[joint])*0.5; + end_velocities[joint] = start_velocities[joint]; + } + + last_switch_state[joint] = switch_state; + } // for joint + + // convert them to steps + float target_steps[motors]; + for (uint8_t motor = 0; motor < motors; motor++) { + int8_t joint = motor_map[motor]; + if (joint == -1) { + continue; + } + + target_steps[motor] = (joint_position[joint] * steps_per_unit[motor]) + motor_offset[motor]; + } + + // tell the planner and runtime about them + mp_set_target_steps(target_steps, start_velocities, end_velocities, segment_time); + + return true; + } +}; + + +#endif // End of include Guard: KINEMATICS_FOUR_CABLE_H_ONCE diff --git a/g2core/report.cpp b/g2core/report.cpp index 4ff4e6739..14c504139 100644 --- a/g2core/report.cpp +++ b/g2core/report.cpp @@ -322,12 +322,12 @@ stat_t sr_request_status_report(cmStatusReportRequest request_type) // request a filtered report unless a verbose report has laready been requested // verbosity setting may override and make it verbose anyway sr.status_report_request = sr.status_report_request == SR_VERBOSE ? SR_VERBOSE : SR_FILTERED; - sr.status_report_systick.set(sr.status_report_interval, true); // true mean to not extend the timer + sr.status_report_systick.set(sr.status_report_interval, true); // true means to not extend the timer } else { //always trigger verbose report, regardless of verbosity setting sr.status_report_request = SR_VERBOSE; - sr.status_report_systick.set(sr.status_report_interval, true); // true mean to not extend the timer + sr.status_report_systick.set(sr.status_report_interval, true); // true means to not extend the timer } return (STAT_OK); } diff --git a/g2core/settings/settings_default.h b/g2core/settings/settings_default.h index 739196e02..013a3588d 100644 --- a/g2core/settings/settings_default.h +++ b/g2core/settings/settings_default.h @@ -246,6 +246,7 @@ #define KINE_CARTESIAN 0 #define KINE_CORE_XY 1 #define KINE_FOUR_CABLE 2 +#define KINE_PRESSURE 3 #ifndef KINEMATICS #define KINEMATICS KINE_CARTESIAN diff --git a/g2core/settings/settings_minimill.h b/g2core/settings/settings_minimill.h index 3b63b23c6..b28c1aea4 100644 --- a/g2core/settings/settings_minimill.h +++ b/g2core/settings/settings_minimill.h @@ -36,6 +36,7 @@ #define CHORDAL_TOLERANCE 0.01 // chordal accuracy for arc drawing (in mm) #define HAS_LASER 1 // We have a laser, but no shark (yet) +#define HAS_PRESSURE 0 #define SOFT_LIMIT_ENABLE 0 // 0=off, 1=on #define HARD_LIMIT_ENABLE 1 // 0=off, 1=on @@ -58,6 +59,7 @@ #define LASER_PULSE_DURATION 150 // in microseconds {th2pd:150} + /* M100 ({th2pd:150}) ; laser on period M100 ({th2mnp:100}) ; laser min pulses per mm @@ -74,6 +76,11 @@ M100 ({th2mxp:1500}) ; laser max pulses per mm #define DO7_POLARITY IO_ACTIVE_HIGH #define DO7_EXTERNAL_NUMBER 7 +#else + +// #define KINEMATICS KINE_CARTESIAN +#define KINEMATICS KINE_PRESSURE + #endif // HAS_LASER // Only used in Bantam mode @@ -119,12 +126,14 @@ M100 ({th2mxp:1500}) ; laser max pulses per mm #define STATUS_REPORT_VERBOSITY SR_FILTERED // one of: SR_OFF, SR_FILTERED, SR_VERBOSE #define STATUS_REPORT_MIN_MS 100 // milliseconds - enforces a viable minimum -#define STATUS_REPORT_INTERVAL_MS 250 // milliseconds - set $SV=0 to disable -#define STATUS_REPORT_DEFAULTS "posx", "posy", "posz", \ - "unit", "stat", "coor", "momo", "dist", \ - "home", "vel", "plan", "line", "path", \ - "frmo", "hold", "macs", "cycs", "spc", \ - "prbe", "in1", "in3", "in5", "in6", "feed" +#define STATUS_REPORT_INTERVAL_MS 100 // milliseconds - set $SV=0 to disable +#define STATUS_REPORT_DEFAULTS "knfc", "stat", "knft", "knht", "prs1", "flow1slm", "flow1vol", "flow1prs", "feed", "knev", "kniv", "kndv", "knec", "knuoc", "knumc", "knpos1" +// #define STATUS_REPORT_DEFAULTS "posx", "posy", "posz", +// "unit", "stat", "coor", "momo", "dist", +// "home", "vel", "plan", "line", "path", +// "frmo", "hold", "macs", "cycs", "spc", +// "prbe", "prs1", "feed" + // "in1", "in3", "in5", "in6" // "ofsx", "ofsy", "ofsz", // "g55x", "g55y", "g55z", @@ -151,7 +160,7 @@ M100 ({th2mxp:1500}) ; laser max pulses per mm #define M1_MOTOR_MAP AXIS_X_EXTERNAL // 1ma #define M1_STEP_ANGLE 1.8 // 1sa #define M1_TRAVEL_PER_REV 8 // 1tr -#define M1_MICROSTEPS 32 // 1mi 1,2,4,8,16,32 +#define M1_MICROSTEPS 8 // 1mi 1,2,4,8,16,32 #define M1_POLARITY 0 // 1po 0=normal, 1=reversed #define M1_POWER_MODE MOTOR_POWER_MODE // 1pm See enum cmMotorPowerMode in stepper.h #define M1_POWER_LEVEL MOTOR_POWER_LEVEL_XY // 0.00=off, 1.00=max @@ -204,23 +213,23 @@ M100 ({th2mxp:1500}) ; laser max pulses per mm // *** axis settings ********************************************************************************** -#define JERK_MAX 400 // 500 million mm/(min^3) -#define JERK_HIGH_SPEED 400 // 1000 million mm/(min^3) // Jerk during homing needs to stop *fast* -#define VELOCITY_MAX 4000 -#define LATCH_VELOCITY 25 // reeeeally slow for accuracy +#define JERK_MAX 800 // 500 million mm/(min^3) +#define JERK_HIGH_SPEED 4000 // 1000 million mm/(min^3) // Jerk during homing needs to stop *fast* +#define VELOCITY_MAX 9000 +#define LATCH_VELOCITY 500 // reeeeally slow for accuracy #define X_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values #define X_VELOCITY_MAX VELOCITY_MAX // xvm G0 max velocity in mm/min #define X_FEEDRATE_MAX X_VELOCITY_MAX // xfr G1 max feed rate in mm/min -#define X_TRAVEL_MIN 0 // xtn minimum travel for soft limits -#define X_TRAVEL_MAX 120 // xtr travel between switches or crashes +#define X_TRAVEL_MIN -5 // xtn minimum travel for soft limits +#define X_TRAVEL_MAX 110 // xtr travel between switches or crashes #define X_JERK_MAX JERK_MAX // xjm #define X_JERK_HIGH_SPEED JERK_HIGH_SPEED // xjh #define X_HOMING_INPUT 1 // xhi input used for homing or 0 to disable -#define X_HOMING_DIRECTION 1 // xhd 0=search moves negative, 1= search moves positive +#define X_HOMING_DIRECTION 0 // xhd 0=search moves negative, 1= search moves positive #define X_SEARCH_VELOCITY 1000 // xsv #define X_LATCH_VELOCITY LATCH_VELOCITY // xlv mm/min -#define X_LATCH_BACKOFF 4 // xlb mm +#define X_LATCH_BACKOFF 10 // xlb mm #define X_ZERO_BACKOFF 1 // xzb mm #define Y_AXIS_MODE AXIS_STANDARD