From 7560dbf90b9c3dcedd59f0dc196e402a3eebe8ce Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Fri, 4 Aug 2023 23:57:01 +0100 Subject: [PATCH] Filling out code for each module --- examples/yukon/CMakeLists.txt | 3 +- examples/yukon/yukon_switched_output.cmake | 13 ++ examples/yukon/yukon_switched_output.cpp | 121 +++++++++++ .../yukon/modules/audio_amp/audio_amp.cpp | 114 +++++++++++ .../yukon/modules/audio_amp/audio_amp.hpp | 83 +++++++- .../yukon/modules/bench_power/bench_power.cpp | 146 +++++++++++++ .../yukon/modules/bench_power/bench_power.hpp | 82 +++++++- .../yukon/modules/big_motor/big_motor.cpp | 118 +++++++++++ .../yukon/modules/big_motor/big_motor.hpp | 76 ++++++- .../yukon/modules/dual_motor/dual_motor.cpp | 110 ++++++++++ .../yukon/modules/dual_motor/dual_motor.hpp | 85 +++++++- .../modules/dual_switched/dual_switched.cpp | 193 ++++++++++++++++++ .../modules/dual_switched/dual_switched.hpp | 72 ++++++- .../yukon/modules/led_strip/led_strip.hpp | 2 +- libraries/yukon/modules/proto/proto.cpp | 79 +++++++ libraries/yukon/modules/proto/proto.hpp | 68 +++++- .../modules/quad_servo/quad_servo_direct.cpp | 33 +++ .../modules/quad_servo/quad_servo_direct.hpp | 41 +++- .../modules/quad_servo/quad_servo_reg.cpp | 104 ++++++++++ .../modules/quad_servo/quad_servo_reg.hpp | 69 ++++++- libraries/yukon/yukon.cmake | 2 +- 21 files changed, 1574 insertions(+), 40 deletions(-) create mode 100644 examples/yukon/yukon_switched_output.cmake create mode 100644 examples/yukon/yukon_switched_output.cpp diff --git a/examples/yukon/CMakeLists.txt b/examples/yukon/CMakeLists.txt index 812f98b19..a9b5df1de 100644 --- a/examples/yukon/CMakeLists.txt +++ b/examples/yukon/CMakeLists.txt @@ -1 +1,2 @@ -include(yukon_simple_enable.cmake) \ No newline at end of file +include(yukon_simple_enable.cmake) +include(yukon_switched_output.cmake) \ No newline at end of file diff --git a/examples/yukon/yukon_switched_output.cmake b/examples/yukon/yukon_switched_output.cmake new file mode 100644 index 000000000..a7e20bd48 --- /dev/null +++ b/examples/yukon/yukon_switched_output.cmake @@ -0,0 +1,13 @@ +set(OUTPUT_NAME yukon_switched_output) +add_executable(${OUTPUT_NAME} yukon_switched_output.cpp) + +target_link_libraries(${OUTPUT_NAME} + pico_stdlib + yukon + ) + +# enable usb output, disable uart output (so it doesn't confuse any connected servos) +pico_enable_stdio_usb(${OUTPUT_NAME} 1) +pico_enable_stdio_uart(${OUTPUT_NAME} 0) + +pico_add_extra_outputs(${OUTPUT_NAME}) diff --git a/examples/yukon/yukon_switched_output.cpp b/examples/yukon/yukon_switched_output.cpp new file mode 100644 index 000000000..ab22b8445 --- /dev/null +++ b/examples/yukon/yukon_switched_output.cpp @@ -0,0 +1,121 @@ +#include "pico/stdlib.h" + +#include "yukon.hpp" +using namespace pimoroni; +/* +Demonstrates how to create a Servo object and control it. +*/ + +const std::string OUTPUT_1_NAME = "Small Fan"; +const std::string OUTPUT_2_NAME = "Big Fan"; +const uint SLOT_ID = 1; + +Yukon yukon(12.1f); + +#define CHAR_CTRL_C (3) +#define CHAR_CTRL_D (4) + +int last_char = PICO_ERROR_TIMEOUT; + +bool check_for_ctrlc() { + int c = getchar_timeout_us(0); + if(c == CHAR_CTRL_C) { + printf("CTRL-C\n"); + return true; + } + return false; +} + +int main() { + stdio_init_all(); + yukon.change_logging(3); + + try { + // Initialise the servo + yukon.init(); + + while (!stdio_usb_connected()) { + sleep_ms(100); + } + while(!check_for_ctrlc()); + while(!check_for_ctrlc()); + printf("stdio_usb_connected()\n"); + + // Create a DualSwitchedModule and register it with a slot on Yukon + DualSwitchedModule switches; + yukon.register_with_slot(&switches, SLOT_ID); + + // Initialise Yukon's registered modules + yukon.initialise_modules(); + + // Turn on the module power + yukon.enable_main_output(); + + // Enable the switched outputs + switches.enable(1); + switches.enable(2); + + //uint offset = 0; + //bool sw_a_state = false; + //bool sw_b_state = false; + bool last_sw_a_state = false; + bool last_sw_b_state = false; + + while(!yukon.is_boot_pressed()) { + bool sw_a_state = yukon.is_pressed(0); + bool sw_b_state = yukon.is_pressed(1); + + if(sw_a_state && sw_a_state != last_sw_a_state) { + bool new_state = !switches.read_output(1); + switches.output(1, new_state); + yukon.set_led(0, new_state); + if(new_state) + logging.print(OUTPUT_1_NAME + " = On\n"); + else + logging.print(OUTPUT_1_NAME + " = Off\n"); + } + + if(sw_b_state && sw_b_state != last_sw_b_state) { + bool new_state = not switches.read_output(2); + switches.output(2, new_state); + yukon.set_led(1, new_state); + if(new_state) + logging.print(OUTPUT_2_NAME + " = On\n"); + else + logging.print(OUTPUT_2_NAME + " = Off\n"); + } + + last_sw_a_state = sw_a_state; + last_sw_b_state = sw_b_state; + + // Use the Yukon class to sleep, which lets it monitor voltage, current, and temperature + yukon.monitored_sleep_ms(100); + + if(stdio_usb_connected()) { + int c = getchar_timeout_us(0); + if(c != PICO_ERROR_TIMEOUT) { + if(c == CHAR_CTRL_C) { + printf("CTRL-C\n"); + if(last_char == CHAR_CTRL_C) { + printf("exiting\n"); + break; + } + } + if(c == CHAR_CTRL_D) { + printf("CTRL-D\n"); + printf("exiting\n"); + break; + } + last_char = c; + } + } + } + } + catch(const std::exception &e) { + printf(e.what()); + } + + // Put the board back into a safe state, regardless of how the program may have ended + yukon.reset(); + return 0; +} diff --git a/libraries/yukon/modules/audio_amp/audio_amp.cpp b/libraries/yukon/modules/audio_amp/audio_amp.cpp index b6760cd21..3a116f373 100644 --- a/libraries/yukon/modules/audio_amp/audio_amp.cpp +++ b/libraries/yukon/modules/audio_amp/audio_amp.cpp @@ -1,4 +1,6 @@ #include "audio_amp.hpp" +#include "../../logging.hpp" +#include "../../errors.hpp" namespace pimoroni { @@ -12,4 +14,116 @@ namespace pimoroni { return adc_level == ADC_FLOAT && slow1 == HIGH && slow2 == HIGH && slow3 == HIGH; } + AudioAmpModule::AudioAmpModule() : + YukonModule() { + } + + AudioAmpModule::~AudioAmpModule() { + } + + std::string AudioAmpModule::name() { + return AudioAmpModule::NAME; + } + + void AudioAmpModule::initialise(const SLOT& slot, SlotAccessor& accessor) { + // Configure strip and power pins + configure(); + + // Pass the slot and adc functions up to the parent now that module specific initialisation has finished + YukonModule::initialise(slot, accessor); + } + + void AudioAmpModule::configure() { + + } + + void AudioAmpModule::enable() { + + } + + void AudioAmpModule::disable() { + + } + + bool AudioAmpModule::is_enabled() { + return 0; // TODO + } + + void AudioAmpModule::exit_soft_shutdown() { + + } + + void AudioAmpModule::set_volume(float volume) { + + } + + float AudioAmpModule::read_temperature() { + return __read_adc2_as_temp(); + } + + void AudioAmpModule::monitor() { + float temperature = read_temperature(); + if(temperature > TEMPERATURE_THRESHOLD) { + throw OverTemperatureError(__message_header() + "Temperature of " + std::to_string(temperature) + "°C exceeded the user set level of " + std::to_string(TEMPERATURE_THRESHOLD) + "°C! Turning off output\n"); + } + + // Run some user action based on the latest readings + //if self.__monitor_action_callback is not None: + // self.__monitor_action_callback(pgood, temperature) + + max_temperature = MAX(temperature, max_temperature); + min_temperature = MIN(temperature, min_temperature); + avg_temperature += temperature; + count_avg += 1; + } + + std::vector> AudioAmpModule::get_readings() { + std::vector> values; + values.push_back(std::pair("T_max", max_temperature)); + values.push_back(std::pair("T_min", min_temperature)); + values.push_back(std::pair("T_avg", avg_temperature)); + return values; + } + + void AudioAmpModule::process_readings() { + if(count_avg > 0) { + avg_temperature /= count_avg; + } + } + + void AudioAmpModule::clear_readings() { + max_temperature = -std::numeric_limits::infinity(); + min_temperature = std::numeric_limits::infinity(); + avg_temperature = 0; + count_avg = 0; + } + + void AudioAmpModule::start_i2c() { + + } + + void AudioAmpModule::end_i2c() { + + } + + void AudioAmpModule::write_i2c_byte(uint8_t number) { + + } + + uint8_t AudioAmpModule::read_i2c_byte() { + return 0; // TODO + } + + void AudioAmpModule::write_i2c_reg(uint8_t reg, uint8_t data) { + + } + + uint8_t AudioAmpModule::read_i2c_reg(uint8_t reg) { + return 0; // TODO + } + + uint8_t AudioAmpModule::detect_i2c() { + return 0; // TODO + } + } diff --git a/libraries/yukon/modules/audio_amp/audio_amp.hpp b/libraries/yukon/modules/audio_amp/audio_amp.hpp index b18d08b32..7dc20f5ff 100644 --- a/libraries/yukon/modules/audio_amp/audio_amp.hpp +++ b/libraries/yukon/modules/audio_amp/audio_amp.hpp @@ -5,16 +5,91 @@ namespace pimoroni { class AudioAmpModule : public YukonModule { + //-------------------------------------------------- + // Constants + //-------------------------------------------------- public: static const std::string NAME; + static const uint8_t AMP_I2C_ADDRESS = 0x38; + static constexpr float TEMPERATURE_THRESHOLD = 50.0f; + + //-------------------------------------------------- + // Statics + //-------------------------------------------------- static bool is_module(uint adc_level, bool slow1, bool slow2, bool slow3); + TYPE_FUNCTION(AudioAmpModule) - virtual std::string name() { - return AudioAmpModule::NAME; - } - TYPE_FUNCTION(AudioAmpModule) + //-------------------------------------------------- + // Variables + //-------------------------------------------------- + public: + bool halt_on_not_pgood; + + //-------------------------------------------------- + private: + bool last_pgood1; + bool last_pgood2; + bool power_good_throughout1; + bool power_good_throughout2; + float max_temperature; + float min_temperature; + float avg_temperature; + float count_avg; + + //-------------------------------------------------- + public: + uint I2S_DATA; + uint I2S_CLK; + uint I2S_FS; + private: + TCA slow_sda; + TCA slow_scl; + TCA amp_en; + + + + //-------------------------------------------------- + // Constructors/Destructor + //-------------------------------------------------- + public: + AudioAmpModule(); + virtual ~AudioAmpModule(); + + + //-------------------------------------------------- + // Methods + //-------------------------------------------------- + public: + virtual std::string name(); + virtual void initialise(const SLOT& slot, SlotAccessor& accessor); + virtual void configure(); + + //-------------------------------------------------- + void enable(); + void disable(); + bool is_enabled(); + + void exit_soft_shutdown(); + void set_volume(float volume); + float read_temperature(); + + //-------------------------------------------------- + virtual void monitor(); + virtual std::vector> get_readings(); + virtual void process_readings(); + virtual void clear_readings(); + + //-------------------------------------------------- + private: + void start_i2c(); + void end_i2c(); + void write_i2c_byte(uint8_t number); + uint8_t read_i2c_byte(); + void write_i2c_reg(uint8_t reg, uint8_t data); + uint8_t read_i2c_reg(uint8_t reg); + uint8_t detect_i2c(); }; } diff --git a/libraries/yukon/modules/bench_power/bench_power.cpp b/libraries/yukon/modules/bench_power/bench_power.cpp index d1fe477a8..3e3e68c93 100644 --- a/libraries/yukon/modules/bench_power/bench_power.cpp +++ b/libraries/yukon/modules/bench_power/bench_power.cpp @@ -1,4 +1,6 @@ #include "bench_power.hpp" +#include "../../logging.hpp" +#include "../../errors.hpp" namespace pimoroni { @@ -13,4 +15,148 @@ namespace pimoroni { return slow1 == HIGH && slow2 == LOW && slow3 == LOW; } + BenchPowerModule::BenchPowerModule(bool halt_on_not_pgood) : + YukonModule(), + halt_on_not_pgood(halt_on_not_pgood), + voltage_pwm(nullptr), + power_en(nullptr), + power_good(nullptr) { + } + + BenchPowerModule::~BenchPowerModule() { + delete(voltage_pwm); + delete(power_en); + delete(power_good); + } + + std::string BenchPowerModule::name() { + return BenchPowerModule::NAME; + } + + void BenchPowerModule::initialise(const SLOT& slot, SlotAccessor& accessor) { + // Configure strip and power pins + configure(); + + // Pass the slot and adc functions up to the parent now that module specific initialisation has finished + YukonModule::initialise(slot, accessor); + } + + void BenchPowerModule::configure() { + + } + + void BenchPowerModule::enable() { + + } + + void BenchPowerModule::disable() { + + } + + bool BenchPowerModule::is_enabled() { + return 0; // TODO + } + + void BenchPowerModule::set_pwm(float percent) { + + } + + void BenchPowerModule::set_target_voltage(float voltage) { + + } + + void BenchPowerModule::set_target(float percent) { + + } + + float BenchPowerModule::read_voltage() { + // return (self.__shared_adc_voltage() * (100 + 22)) / 22 + float value = __read_adc1(); + if(value >= VOLTAGE_MID_MEASURE) { + return ((value - VOLTAGE_MID_MEASURE) * (VOLTAGE_MAX - VOLTAGE_MID)) / (VOLTAGE_MAX_MEASURE - VOLTAGE_MID_MEASURE) + VOLTAGE_MID; + } + else { + float voltage = ((value - VOLTAGE_MIN_MEASURE) * (VOLTAGE_MID - VOLTAGE_MIN)) / (VOLTAGE_MID_MEASURE - VOLTAGE_MIN_MEASURE) + VOLTAGE_MIN; + return MAX(voltage, 0.0); + } + } + + bool BenchPowerModule::read_power_good() { + return 0; // TODO + } + + float BenchPowerModule::read_temperature() { + return __read_adc2_as_temp(); + } + + void BenchPowerModule::monitor() { + bool pgood = read_power_good(); + if(!pgood) { + if(halt_on_not_pgood) { + throw FaultError(__message_header() + "Power is not good! Turning off output\n"); + } + } + + float temperature = read_temperature(); + if(temperature > TEMPERATURE_THRESHOLD) { + throw OverTemperatureError(__message_header() + "Temperature of " + std::to_string(temperature) + "°C exceeded the user set level of " + std::to_string(TEMPERATURE_THRESHOLD) + "°C! Turning off output\n"); + } + + float voltage_out = read_voltage(); + + if(last_pgood && !pgood) { + logging.warn(__message_header() + "Power is not good\n"); + } + else if(!last_pgood && pgood) { + logging.warn(__message_header() + "Power is good\n"); + } + + // Run some user action based on the latest readings + //if self.__monitor_action_callback is not None: + // self.__monitor_action_callback(pgood, temperature) + + last_pgood = pgood; + power_good_throughout = power_good_throughout && pgood; + + max_voltage_out = MAX(voltage_out, max_voltage_out); + min_voltage_out = MIN(voltage_out, min_voltage_out); + avg_voltage_out += voltage_out; + + max_temperature = MAX(temperature, max_temperature); + min_temperature = MIN(temperature, min_temperature); + avg_temperature += temperature; + + count_avg += 1; + } + + std::vector> BenchPowerModule::get_readings() { + std::vector> values; + values.push_back(std::pair("PGood", power_good_throughout)); + values.push_back(std::pair("VO_max", max_voltage_out)); + values.push_back(std::pair("VO_min", min_voltage_out)); + values.push_back(std::pair("VO_avg", avg_voltage_out)); + values.push_back(std::pair("T_max", max_temperature)); + values.push_back(std::pair("T_min", min_temperature)); + values.push_back(std::pair("T_avg", avg_temperature)); + return values; + } + + void BenchPowerModule::process_readings() { + if(count_avg > 0) { + avg_temperature /= count_avg; + avg_voltage_out /= count_avg; + } + } + + void BenchPowerModule::clear_readings() { + power_good_throughout = true; + max_voltage_out = -std::numeric_limits::infinity(); + min_voltage_out = std::numeric_limits::infinity(); + avg_voltage_out = 0; + max_temperature = -std::numeric_limits::infinity(); + min_temperature = std::numeric_limits::infinity(); + avg_temperature = 0; + count_avg = 0; + } + } diff --git a/libraries/yukon/modules/bench_power/bench_power.hpp b/libraries/yukon/modules/bench_power/bench_power.hpp index 0204abc69..74a534ecc 100644 --- a/libraries/yukon/modules/bench_power/bench_power.hpp +++ b/libraries/yukon/modules/bench_power/bench_power.hpp @@ -1,20 +1,94 @@ #pragma once #include "../common.hpp" +#include "pwm_cluster.hpp" +#include "io.hpp" namespace pimoroni { class BenchPowerModule : public YukonModule { + //-------------------------------------------------- + // Constants + //-------------------------------------------------- public: static const std::string NAME; + static constexpr float VOLTAGE_MAX = 12.3953f; + static constexpr float VOLTAGE_MID = 6.5052f; + static constexpr float VOLTAGE_MIN = 0.6713f; + static constexpr float VOLTAGE_MIN_MEASURE = 0.1477f; + static constexpr float VOLTAGE_MID_MEASURE = 1.1706f; + static constexpr float VOLTAGE_MAX_MEASURE = 2.2007f; + static constexpr float PWM_MIN = 0.3f; + static constexpr float PWM_MAX = 0.0f; + static constexpr float TEMPERATURE_THRESHOLD = 50.0f; + + //-------------------------------------------------- + // Statics + //-------------------------------------------------- static bool is_module(uint adc_level, bool slow1, bool slow2, bool slow3); + TYPE_FUNCTION(BenchPowerModule) - virtual std::string name() { - return BenchPowerModule::NAME; - } - TYPE_FUNCTION(BenchPowerModule) + //-------------------------------------------------- + // Variables + //-------------------------------------------------- + public: + bool halt_on_not_pgood; + + //-------------------------------------------------- + private: + bool last_pgood; + bool power_good_throughout; + float max_voltage_out; + float min_voltage_out; + float avg_voltage_out; + float max_temperature; + float min_temperature; + float avg_temperature; + float count_avg; + + //-------------------------------------------------- + private: + PWMCluster* voltage_pwm; + IO* power_en; + IO* power_good; + + + //-------------------------------------------------- + // Constructors/Destructor + //-------------------------------------------------- + public: + BenchPowerModule(bool halt_on_not_pgood = false); + virtual ~BenchPowerModule(); + + + //-------------------------------------------------- + // Methods + //-------------------------------------------------- + public: + virtual std::string name(); + virtual void initialise(const SLOT& slot, SlotAccessor& accessor); + virtual void configure(); + + //-------------------------------------------------- + void enable(); + void disable(); + bool is_enabled(); + private: + void set_pwm(float percent); + public: + void set_target_voltage(float voltage); + void set_target(float percent); + float read_voltage(); + bool read_power_good(); + float read_temperature(); + + //-------------------------------------------------- + virtual void monitor(); + virtual std::vector> get_readings(); + virtual void process_readings(); + virtual void clear_readings(); }; } diff --git a/libraries/yukon/modules/big_motor/big_motor.cpp b/libraries/yukon/modules/big_motor/big_motor.cpp index ee882627d..2aeaf85ed 100644 --- a/libraries/yukon/modules/big_motor/big_motor.cpp +++ b/libraries/yukon/modules/big_motor/big_motor.cpp @@ -1,4 +1,6 @@ #include "big_motor.hpp" +#include "../../logging.hpp" +#include "../../errors.hpp" namespace pimoroni { @@ -12,4 +14,120 @@ namespace pimoroni { return adc_level == ADC_LOW && slow1 == LOW && slow3 == HIGH; } + BigMotorModule::BigMotorModule(float frequency) : + YukonModule(), + frequency(frequency), + motor(nullptr), + encoder(nullptr) { + } + + BigMotorModule::~BigMotorModule() { + delete(motor); + delete(encoder); + } + + std::string BigMotorModule::name() { + return BigMotorModule::NAME; + } + + void BigMotorModule::initialise(const SLOT& slot, SlotAccessor& accessor) { + // Configure strip and power pins + configure(); + + // Pass the slot and adc functions up to the parent now that module specific initialisation has finished + YukonModule::initialise(slot, accessor); + } + + void BigMotorModule::configure() { + + } + + void BigMotorModule::enable() { + + } + + void BigMotorModule::disable() { + + } + + bool BigMotorModule::is_enabled() { + return 0; // TODO + } + + bool BigMotorModule::read_fault() { + return 0; // TODO + } + + bool BigMotorModule::read_current() { + // This needs more validation + return (fabsf(__read_adc1() - (3.3f / 2.0f))) / (SHUNT_RESISTOR * GAIN); + } + + float BigMotorModule::read_temperature() { + return __read_adc2_as_temp(); + } + + void BigMotorModule::monitor() { + bool fault = read_fault(); + if(!fault) { + throw FaultError(__message_header() + "Fault detected on motor driver! Turning off output\n"); + } + + float current = read_current(); + if(current > CURRENT_THRESHOLD) { + throw OverCurrentError(__message_header() + "Current of " + std::to_string(current) + "A exceeded the user set level of " + std::to_string(CURRENT_THRESHOLD) + "A! Turning off output\n"); + } + + float temperature = read_temperature(); + if(temperature > TEMPERATURE_THRESHOLD) { + throw OverTemperatureError(__message_header() + "Temperature of " + std::to_string(temperature) + "°C exceeded the user set level of " + std::to_string(TEMPERATURE_THRESHOLD) + "°C! Turning off output\n"); + } + + // Run some user action based on the latest readings + //if self.__monitor_action_callback is not None: + // self.__monitor_action_callback(pgood, temperature) + + fault_triggered = fault_triggered || fault; + + max_current = MAX(current, max_current); + min_current = MIN(current, min_current); + avg_current += current; + + max_temperature = MAX(temperature, max_temperature); + min_temperature = MIN(temperature, min_temperature); + avg_temperature += temperature; + + count_avg += 1; + } + + std::vector> BigMotorModule::get_readings() { + std::vector> values; + values.push_back(std::pair("Fault", fault_triggered)); + values.push_back(std::pair("C_max", max_current)); + values.push_back(std::pair("C_min", min_current)); + values.push_back(std::pair("C_avg", avg_current)); + values.push_back(std::pair("T_max", max_temperature)); + values.push_back(std::pair("T_min", min_temperature)); + values.push_back(std::pair("T_avg", avg_temperature)); + return values; + } + + void BigMotorModule::process_readings() { + if(count_avg > 0) { + avg_current /= count_avg; + avg_temperature /= count_avg; + } + } + + void BigMotorModule::clear_readings() { + fault_triggered = true; + max_current = -std::numeric_limits::infinity(); + min_current = std::numeric_limits::infinity(); + avg_current = 0; + max_temperature = -std::numeric_limits::infinity(); + min_temperature = std::numeric_limits::infinity(); + avg_temperature = 0; + count_avg = 0; + } + } diff --git a/libraries/yukon/modules/big_motor/big_motor.hpp b/libraries/yukon/modules/big_motor/big_motor.hpp index 2ebe26b82..3603902b3 100644 --- a/libraries/yukon/modules/big_motor/big_motor.hpp +++ b/libraries/yukon/modules/big_motor/big_motor.hpp @@ -1,20 +1,88 @@ #pragma once #include "../common.hpp" +#include "motor_cluster.hpp" +#include "encoder.hpp" + +using namespace motor; +using namespace encoder; namespace pimoroni { class BigMotorModule : public YukonModule { + //-------------------------------------------------- + // Constants + //-------------------------------------------------- public: static const std::string NAME; + static constexpr float DEFAULT_FREQUENCY = 25000.0f; + static constexpr float TEMPERATURE_THRESHOLD = 50.0f; + static constexpr float CURRENT_THRESHOLD = 25.0f; + static constexpr float SHUNT_RESISTOR = 0.001f; + static constexpr float GAIN = 80.0f; + + //-------------------------------------------------- + // Statics + //-------------------------------------------------- static bool is_module(uint adc_level, bool slow1, bool slow2, bool slow3); + TYPE_FUNCTION(BigMotorModule) - virtual std::string name() { - return BigMotorModule::NAME; - } - TYPE_FUNCTION(BigMotorModule) + //-------------------------------------------------- + // Variables + //-------------------------------------------------- + private: + float frequency; + + //-------------------------------------------------- + bool fault_triggered; + float max_current; + float min_current; + float avg_current; + float max_temperature; + float min_temperature; + float avg_temperature; + float count_avg; + + //-------------------------------------------------- + public: + MotorCluster* motor; + Encoder* encoder; + private: + TCA motor_en; + TCA motor_nfault; + + + //-------------------------------------------------- + // Constructors/Destructor + //-------------------------------------------------- + public: + BigMotorModule(float frequency = DEFAULT_FREQUENCY); + virtual ~BigMotorModule(); + + + //-------------------------------------------------- + // Methods + //-------------------------------------------------- + public: + virtual std::string name(); + virtual void initialise(const SLOT& slot, SlotAccessor& accessor); + virtual void configure(); + + //-------------------------------------------------- + void enable(); + void disable(); + bool is_enabled(); + bool read_fault(); + bool read_current(); + float read_temperature(); + + //-------------------------------------------------- + virtual void monitor(); + virtual std::vector> get_readings(); + virtual void process_readings(); + virtual void clear_readings(); }; } diff --git a/libraries/yukon/modules/dual_motor/dual_motor.cpp b/libraries/yukon/modules/dual_motor/dual_motor.cpp index 6138ebe38..0151d27af 100644 --- a/libraries/yukon/modules/dual_motor/dual_motor.cpp +++ b/libraries/yukon/modules/dual_motor/dual_motor.cpp @@ -1,4 +1,6 @@ #include "dual_motor.hpp" +#include "../../logging.hpp" +#include "../../errors.hpp" namespace pimoroni { @@ -12,4 +14,112 @@ namespace pimoroni { return adc_level == ADC_HIGH && slow2 == HIGH && slow3 == HIGH; } + DualMotorModule::DualMotorModule(float frequency) : + YukonModule(), + motor_type(DUAL), + frequency(frequency), + motors(nullptr) { + } + + DualMotorModule::~DualMotorModule() { + delete(motors); + } + + std::string DualMotorModule::name() { + return DualMotorModule::NAME; + } + + void DualMotorModule::initialise(const SLOT& slot, SlotAccessor& accessor) { + // Configure strip and power pins + configure(); + + // Pass the slot and adc functions up to the parent now that module specific initialisation has finished + YukonModule::initialise(slot, accessor); + } + + void DualMotorModule::configure() { + + } + + void DualMotorModule::enable() { + + } + + void DualMotorModule::disable() { + + } + + bool DualMotorModule::is_enabled() { + return 0; // TODO + } + + bool DualMotorModule::decay() { + return 0; // TODO + } + + void DualMotorModule::decay(bool val) { + + } + + bool DualMotorModule::toff() { + return 0; // TODO + } + + void DualMotorModule::toff(bool val) { + + } + + bool DualMotorModule::read_fault() { + return __read_adc1() <= FAULT_THRESHOLD; + } + + float DualMotorModule::read_temperature() { + return __read_adc2_as_temp(); + } + + void DualMotorModule::monitor() { + bool fault = read_fault(); + if(!fault) { + throw FaultError(__message_header() + "Fault detected on motor driver! Turning off output\n"); + } + + float temperature = read_temperature(); + if(temperature > TEMPERATURE_THRESHOLD) { + throw OverTemperatureError(__message_header() + "Temperature of " + std::to_string(temperature) + "°C exceeded the user set level of " + std::to_string(TEMPERATURE_THRESHOLD) + "°C! Turning off output\n"); + } + + // Run some user action based on the latest readings + //if self.__monitor_action_callback is not None: + // self.__monitor_action_callback(pgood, temperature) + + fault_triggered = fault_triggered || fault; + max_temperature = MAX(temperature, max_temperature); + min_temperature = MIN(temperature, min_temperature); + avg_temperature += temperature; + count_avg += 1; + } + + std::vector> DualMotorModule::get_readings() { + std::vector> values; + values.push_back(std::pair("Fault", fault_triggered)); + values.push_back(std::pair("T_max", max_temperature)); + values.push_back(std::pair("T_min", min_temperature)); + values.push_back(std::pair("T_avg", avg_temperature)); + return values; + } + + void DualMotorModule::process_readings() { + if(count_avg > 0) { + avg_temperature /= count_avg; + } + } + + void DualMotorModule::clear_readings() { + fault_triggered = true; + max_temperature = -std::numeric_limits::infinity(); + min_temperature = std::numeric_limits::infinity(); + avg_temperature = 0; + count_avg = 0; + } + } diff --git a/libraries/yukon/modules/dual_motor/dual_motor.hpp b/libraries/yukon/modules/dual_motor/dual_motor.hpp index 91cff08a4..edf302009 100644 --- a/libraries/yukon/modules/dual_motor/dual_motor.hpp +++ b/libraries/yukon/modules/dual_motor/dual_motor.hpp @@ -1,20 +1,97 @@ #pragma once #include "../common.hpp" +#include "motor_cluster.hpp" + +using namespace motor; namespace pimoroni { class DualMotorModule : public YukonModule { + //-------------------------------------------------- + // Constants + //-------------------------------------------------- public: static const std::string NAME; + static const uint NUM_MOTORS = 2; + static const uint NUM_STEPPERS = 1; + static constexpr float FAULT_THRESHOLD = 0.1f; + static constexpr float DEFAULT_FREQUENCY = 25000.0f; + static constexpr float TEMPERATURE_THRESHOLD = 50.0f; - static bool is_module(uint adc_level, bool slow1, bool slow2, bool slow3); - virtual std::string name() { - return DualMotorModule::NAME; - } + //-------------------------------------------------- + // Enums + //-------------------------------------------------- + public: + enum MotorType { + DUAL = 0, + STEPPER = 1 + }; + + //-------------------------------------------------- + // Statics + //-------------------------------------------------- + static bool is_module(uint adc_level, bool slow1, bool slow2, bool slow3); TYPE_FUNCTION(DualMotorModule) + + + //-------------------------------------------------- + // Variables + //-------------------------------------------------- + private: + const MotorType motor_type; + float frequency; + + //-------------------------------------------------- + private: + bool fault_triggered; + float max_temperature; + float min_temperature; + float avg_temperature; + float count_avg; + + //-------------------------------------------------- + private: + MotorCluster* motors; + TCA motors_decay; + TCA motors_toff; + TCA motors_en; + + + //-------------------------------------------------- + // Constructors/Destructor + //-------------------------------------------------- + public: + DualMotorModule(float frequency = DEFAULT_FREQUENCY); + virtual ~DualMotorModule(); + + +//-------------------------------------------------- + // Methods + //-------------------------------------------------- + public: + virtual std::string name(); + virtual void initialise(const SLOT& slot, SlotAccessor& accessor); + virtual void configure(); + + //-------------------------------------------------- + void enable(); + void disable(); + bool is_enabled(); + bool decay(); + void decay(bool val); + bool toff(); + void toff(bool val); + bool read_fault(); + float read_temperature(); + + //-------------------------------------------------- + virtual void monitor(); + virtual std::vector> get_readings(); + virtual void process_readings(); + virtual void clear_readings(); }; } diff --git a/libraries/yukon/modules/dual_switched/dual_switched.cpp b/libraries/yukon/modules/dual_switched/dual_switched.cpp index 3746c83fa..451abcc73 100644 --- a/libraries/yukon/modules/dual_switched/dual_switched.cpp +++ b/libraries/yukon/modules/dual_switched/dual_switched.cpp @@ -1,4 +1,6 @@ #include "dual_switched.hpp" +#include "../../logging.hpp" +#include "../../errors.hpp" namespace pimoroni { @@ -11,4 +13,195 @@ namespace pimoroni { return adc_level == ADC_FLOAT && slow1 == HIGH && slow2 == LOW && slow3 == HIGH; } + DualSwitchedModule::DualSwitchedModule(bool halt_on_not_pgood) : + YukonModule(), + halt_on_not_pgood(halt_on_not_pgood), + last_pgood1(false), + last_pgood2(false) { + //sw_output(nullptr, nullptr), TODO + //sw_enable(nullptr, nullptr) { + } + + DualSwitchedModule::~DualSwitchedModule() { + delete(sw_output[0]); + delete(sw_output[1]); + delete(sw_enable[0]); + delete(sw_enable[1]); + } + + std::string DualSwitchedModule::name() { + return DualSwitchedModule::NAME; + } + + void DualSwitchedModule::initialise(const SLOT& slot, SlotAccessor& accessor) { + // Create the switch and power control pin objects + sw_output[0] = new IO(slot.FAST1); + sw_output[1] = new IO(slot.FAST3); + sw_enable[0] = new IO(slot.FAST1); + sw_enable[1] = new IO(slot.FAST3); + power_good[0] = slot.SLOW1; + power_good[1] = slot.SLOW3; + + // Configure switch and power pins + configure(); + + // Pass the slot and adc functions up to the parent now that module specific initialisation has finished + YukonModule::initialise(slot, accessor); + } + + void DualSwitchedModule::configure() { + sw_output[0]->to_output(false); + sw_output[1]->to_output(false); + + sw_enable[0]->to_output(false); + sw_enable[1]->to_output(false); + + __accessor->set_slow_config(power_good[0], false); + __accessor->set_slow_config(power_good[1], false); + } + + void DualSwitchedModule::enable(uint output) { + if(!is_initialised()) { + throw std::runtime_error("Module not initialised\n"); + } + if(output < 1 || output > NUM_SWITCHES) { + throw std::runtime_error("switch index out of range. Expected 1 to 2\n"); + } + + sw_enable[output - 1]->value(true); + } + + void DualSwitchedModule::disable(uint output) { + if(!is_initialised()) { + throw std::runtime_error("Module not initialised\n"); + } + if(output < 1 || output > NUM_SWITCHES) { + throw std::runtime_error("switch index out of range. Expected 1 to 2\n"); + } + + sw_enable[output - 1]->value(false); + } + + bool DualSwitchedModule::is_enabled(uint output) { + if(!is_initialised()) { + throw std::runtime_error("Module not initialised\n"); + } + if(output < 1 || output > NUM_SWITCHES) { + throw std::runtime_error("switch index out of range. Expected 1 to 2\n"); + } + + return sw_enable[output - 1]->value(); + } + + void DualSwitchedModule::output(uint output, bool val) { + if(!is_initialised()) { + throw std::runtime_error("Module not initialised\n"); + } + if(output < 1 || output > NUM_SWITCHES) { + throw std::runtime_error("switch index out of range. Expected 1 to 2\n"); + } + + sw_output[output - 1]->value(val); + } + + bool DualSwitchedModule:: read_output(uint output) { + if(!is_initialised()) { + throw std::runtime_error("Module not initialised\n"); + } + if(output < 1 || output > NUM_SWITCHES) { + throw std::runtime_error("switch index out of range. Expected 1 to 2\n"); + } + + return sw_output[output - 1]->value(); + } + + bool DualSwitchedModule::read_power_good(uint output) { + if(!is_initialised()) { + throw std::runtime_error("Module not initialised\n"); + } + if(output < 1 || output > NUM_SWITCHES) { + throw std::runtime_error("switch index out of range. Expected 1 to 2\n"); + } + + return __accessor->get_slow_input(power_good[output - 1]); + } + + float DualSwitchedModule::read_temperature() { + return __read_adc2_as_temp(); + } + + void DualSwitchedModule::monitor() { + bool pgood1 = read_power_good(1); + if(!pgood1) { + if(halt_on_not_pgood) { + throw FaultError(__message_header() + "Power1 is not good! Turning off output\n"); + } + } + + bool pgood2 = read_power_good(2); + if(!pgood2) { + if(halt_on_not_pgood) { + throw FaultError(__message_header() + "Power2 is not good! Turning off output\n"); + } + } + + float temperature = read_temperature(); + if(temperature > TEMPERATURE_THRESHOLD) { + throw OverTemperatureError(__message_header() + "Temperature of " + std::to_string(temperature) + "°C exceeded the user set level of " + std::to_string(TEMPERATURE_THRESHOLD) + "°C! Turning off output\n"); + } + + if(last_pgood1 && !pgood1) { + logging.warn(__message_header() + "Power1 is not good\n"); + } + else if(!last_pgood1 && pgood1) { + logging.warn(__message_header() + "Power1 is good\n"); + } + + if(last_pgood2 && !pgood2) { + logging.warn(__message_header() + "Power2 is not good\n"); + } + else if(!last_pgood2 && pgood2) { + logging.warn(__message_header() + "Power2 is good\n"); + } + + // Run some user action based on the latest readings + //if self.__monitor_action_callback is not None: + // self.__monitor_action_callback(pgood, temperature) + + last_pgood1 = pgood1; + last_pgood2 = pgood2; + power_good_throughout1 = power_good_throughout1 && pgood1; + power_good_throughout2 = power_good_throughout2 && pgood2; + + max_temperature = MAX(temperature, max_temperature); + min_temperature = MIN(temperature, min_temperature); + avg_temperature += temperature; + count_avg += 1; + } + + std::vector> DualSwitchedModule::get_readings() { + std::vector> values; + values.push_back(std::pair("PGood1", power_good_throughout1)); + values.push_back(std::pair("PGood2", power_good_throughout2)); + values.push_back(std::pair("T_max", max_temperature)); + values.push_back(std::pair("T_min", min_temperature)); + values.push_back(std::pair("T_avg", avg_temperature)); + return values; + } + + void DualSwitchedModule::process_readings() { + if(count_avg > 0) { + avg_temperature /= count_avg; + } + } + + void DualSwitchedModule::clear_readings() { + power_good_throughout1 = true; + power_good_throughout2 = true; + max_temperature = -std::numeric_limits::infinity(); + min_temperature = std::numeric_limits::infinity(); + avg_temperature = 0; + count_avg = 0; + } + } diff --git a/libraries/yukon/modules/dual_switched/dual_switched.hpp b/libraries/yukon/modules/dual_switched/dual_switched.hpp index e24d6d684..4ed25d4fa 100644 --- a/libraries/yukon/modules/dual_switched/dual_switched.hpp +++ b/libraries/yukon/modules/dual_switched/dual_switched.hpp @@ -1,20 +1,82 @@ #pragma once #include "../common.hpp" +#include "io.hpp" namespace pimoroni { class DualSwitchedModule : public YukonModule { + //-------------------------------------------------- + // Constants + //-------------------------------------------------- public: - static const std::string NAME; + static const std::string NAME; // Required by all modules. Set in .cpp + static const uint NUM_SWITCHES = 2; + static constexpr float TEMPERATURE_THRESHOLD = 50.0f; + + //-------------------------------------------------- + // Statics + //-------------------------------------------------- static bool is_module(uint adc_level, bool slow1, bool slow2, bool slow3); + TYPE_FUNCTION(DualSwitchedModule) - virtual std::string name() { - return DualSwitchedModule::NAME; - } - TYPE_FUNCTION(DualSwitchedModule) + //-------------------------------------------------- + // Variables + //-------------------------------------------------- + public: + bool halt_on_not_pgood; + + //-------------------------------------------------- + private: + bool last_pgood1; + bool last_pgood2; + bool power_good_throughout1; + bool power_good_throughout2; + float max_temperature; + float min_temperature; + float avg_temperature; + float count_avg; + + //-------------------------------------------------- + private: + IO* sw_output[2]; + IO* sw_enable[2]; + TCA power_good[2]; + + + //-------------------------------------------------- + // Constructors/Destructor + //-------------------------------------------------- + public: + DualSwitchedModule(bool halt_on_not_pgood = false); + virtual ~DualSwitchedModule(); + + + //-------------------------------------------------- + // Methods + //-------------------------------------------------- + public: + virtual std::string name(); + virtual void initialise(const SLOT& slot, SlotAccessor& accessor); + virtual void configure(); + + //-------------------------------------------------- + void enable(uint output); + void disable(uint output); + bool is_enabled(uint output); + void output(uint output, bool val); + bool read_output(uint output); + + bool read_power_good(uint output); + float read_temperature(); + + //-------------------------------------------------- + virtual void monitor(); + virtual std::vector> get_readings(); + virtual void process_readings(); + virtual void clear_readings(); }; } diff --git a/libraries/yukon/modules/led_strip/led_strip.hpp b/libraries/yukon/modules/led_strip/led_strip.hpp index edaea219a..dc96ac478 100644 --- a/libraries/yukon/modules/led_strip/led_strip.hpp +++ b/libraries/yukon/modules/led_strip/led_strip.hpp @@ -86,7 +86,7 @@ namespace pimoroni { void enable(); void disable(); bool is_enabled(); - bool read_power_good() ; + bool read_power_good(); float read_temperature(); //-------------------------------------------------- diff --git a/libraries/yukon/modules/proto/proto.cpp b/libraries/yukon/modules/proto/proto.cpp index f3f38171f..03da4b18a 100644 --- a/libraries/yukon/modules/proto/proto.cpp +++ b/libraries/yukon/modules/proto/proto.cpp @@ -13,10 +13,89 @@ namespace pimoroni { return slow1 == HIGH && slow2 == HIGH && slow3 == LOW; } + ProtoPotModule::ProtoPotModule() : + YukonModule() { + } + + ProtoPotModule::~ProtoPotModule() { + } + + std::string ProtoPotModule::name() { + return ProtoPotModule::NAME; + } + + float ProtoPotModule::read() { + return __read_adc1() / 3.3f; + } + + const std::string ProtoPotModule2::NAME = "Proto Potentiometer 2"; bool ProtoPotModule2::is_module(uint adc_level, bool slow1, bool slow2, bool slow3) { return slow1 == HIGH && slow2 == HIGH && slow3 == LOW; } + ProtoPotModule2::ProtoPotModule2(float pot_resistance) : + YukonModule() { + + // Normalise the pull-up resistance + float pu = PULLUP / pot_resistance; + + // Pre-calculate values that are independent of the adc reading + sum = pu + 1; + const_a = (4 * pu) + 1; + const_b = (6 * pu) + 2; + const_c = (pu * pu) + (2 * pu) + 1; + } + + ProtoPotModule2::~ProtoPotModule2() { + } + + std::string ProtoPotModule2::name() { + return ProtoPotModule2::NAME; + } + + // ADC2 has a pull-up connected to simplify its use with modules that feature an onboard thermistor. + // Unfortunately, when connecting up a potentiometer, creating the below circuit, this has the + // effect of making the output non-linear. + // + // Vin --------------- + // | | + // | | | | + // rt | | | | rp + // | | | | + // | | + // --------------- Vout + // | + // | | + // rb | | + // | | + // | + // Gnd ------- + // + // Where rt is the top section of the potentiometer, rb is the bottom + // section of the potentiometer, and rp is the onboard pull-up. Also, + // The full resistance of the pot, R = rt + rb + // + // Below is the equation for calculating the output given a normalised input (e.g. 1V) + // + // o = rb + // --------------------------------- + // / 1 | + // | ---------------------- + rb | + // | / 1 \ / 1 \ | + // | | ------ | + | --- | | + // \ \ R - rb / \ rp / / + // + // We want to calculate the inverse of this though... which is this magic equation: + // https://www.symbolab.com/solver/equation-calculator/o%20%3D%20r%20%2F%20%5Cleft(%201%2F%20%5Cleft(1%2F%5Cleft(10000-r%5Cright)%2B1%2F5100%5Cright)%20%2B%20r%5Cright)?or=input + // where r is rb, and R and rp are set to 10k and 5.1k, respectively. + // + // This can be simplified by normalising the resistor values, by R, giving: + // https://www.symbolab.com/solver/equation-calculator/o%20%3D%20r%20%2F%20%5Cleft(%201%2F%20%5Cleft(1%2F%5Cleft(1-r%5Cright)%2B1%2F0.51%5Cright)%20%2B%20r%5Cright)?or=input + float ProtoPotModule2::read() { + float out = __read_adc2() / 3.3f; + return (-out + sum - sqrtf((const_a * (out * out)) - (const_b * out) + const_c)) / (2 * (-out + 1)); + } + } diff --git a/libraries/yukon/modules/proto/proto.hpp b/libraries/yukon/modules/proto/proto.hpp index c3192cc4d..08f477e60 100644 --- a/libraries/yukon/modules/proto/proto.hpp +++ b/libraries/yukon/modules/proto/proto.hpp @@ -5,29 +5,81 @@ namespace pimoroni { class ProtoPotModule : public YukonModule { + //-------------------------------------------------- + // Constants + //-------------------------------------------------- public: static const std::string NAME; + + //-------------------------------------------------- + // Statics + //-------------------------------------------------- static bool is_module(uint adc_level, bool slow1, bool slow2, bool slow3); + TYPE_FUNCTION(ProtoPotModule) - virtual std::string name() { - return ProtoPotModule::NAME; - } - TYPE_FUNCTION(ProtoPotModule) + //-------------------------------------------------- + // Constructors/Destructor + //-------------------------------------------------- + public: + ProtoPotModule(); + virtual ~ProtoPotModule(); + + + //-------------------------------------------------- + // Methods + //-------------------------------------------------- + public: + virtual std::string name(); + + //-------------------------------------------------- + float read(); }; class ProtoPotModule2 : public YukonModule { + //-------------------------------------------------- + // Constants + //-------------------------------------------------- public: static const std::string NAME; + private: + static constexpr float PULLUP = 5100.0f; + + //-------------------------------------------------- + // Statics + //-------------------------------------------------- static bool is_module(uint adc_level, bool slow1, bool slow2, bool slow3); + TYPE_FUNCTION(ProtoPotModule2) - virtual std::string name() { - return ProtoPotModule2::NAME; - } - TYPE_FUNCTION(ProtoPotModule2) + //-------------------------------------------------- + // Variables + //-------------------------------------------------- + private: + float sum; + float const_a; + float const_b; + float const_c; + + + //-------------------------------------------------- + // Constructors/Destructor + //-------------------------------------------------- + public: + ProtoPotModule2(float pot_resistance); + virtual ~ProtoPotModule2(); + + + //-------------------------------------------------- + // Methods + //-------------------------------------------------- + public: + virtual std::string name(); + + //-------------------------------------------------- + float read(); }; } diff --git a/libraries/yukon/modules/quad_servo/quad_servo_direct.cpp b/libraries/yukon/modules/quad_servo/quad_servo_direct.cpp index 8beed9724..7dd297697 100644 --- a/libraries/yukon/modules/quad_servo/quad_servo_direct.cpp +++ b/libraries/yukon/modules/quad_servo/quad_servo_direct.cpp @@ -13,4 +13,37 @@ namespace pimoroni { return slow1 == LOW && slow2 == LOW && slow3 == LOW; } + QuadServoDirectModule::QuadServoDirectModule() : + YukonModule(), + servos(nullptr) { + } + + QuadServoDirectModule::~QuadServoDirectModule() { + delete(servos); + } + + std::string QuadServoDirectModule::name() { + return QuadServoDirectModule::NAME; + } + + void QuadServoDirectModule::initialise(const SLOT& slot, SlotAccessor& accessor) { + // Configure strip and power pins + configure(); + + // Pass the slot and adc functions up to the parent now that module specific initialisation has finished + YukonModule::initialise(slot, accessor); + } + + void QuadServoDirectModule::configure() { + + } + + float QuadServoDirectModule::read_adc1() { + return __read_adc1(); + } + + float QuadServoDirectModule::read_adc2() { + return __read_adc2(); + } + } diff --git a/libraries/yukon/modules/quad_servo/quad_servo_direct.hpp b/libraries/yukon/modules/quad_servo/quad_servo_direct.hpp index 723833aba..e5140e05e 100644 --- a/libraries/yukon/modules/quad_servo/quad_servo_direct.hpp +++ b/libraries/yukon/modules/quad_servo/quad_servo_direct.hpp @@ -1,20 +1,53 @@ #pragma once #include "../common.hpp" +#include "servo_cluster.hpp" +using namespace servo; namespace pimoroni { class QuadServoDirectModule : public YukonModule { + //-------------------------------------------------- + // Constants + //-------------------------------------------------- public: static const std::string NAME; + static const uint NUM_SERVOS = 4; + + //-------------------------------------------------- + // Statics + //-------------------------------------------------- static bool is_module(uint adc_level, bool slow1, bool slow2, bool slow3); + TYPE_FUNCTION(QuadServoDirectModule) - virtual std::string name() { - return QuadServoDirectModule::NAME; - } - TYPE_FUNCTION(QuadServoDirectModule) + //-------------------------------------------------- + // Variables + //-------------------------------------------------- + private: + ServoCluster* servos; + + + //-------------------------------------------------- + // Constructors/Destructor + //-------------------------------------------------- + public: + QuadServoDirectModule(); + virtual ~QuadServoDirectModule(); + + + //-------------------------------------------------- + // Methods + //-------------------------------------------------- + public: + virtual std::string name(); + virtual void initialise(const SLOT& slot, SlotAccessor& accessor); + virtual void configure(); + + //-------------------------------------------------- + float read_adc1(); + float read_adc2(); }; } diff --git a/libraries/yukon/modules/quad_servo/quad_servo_reg.cpp b/libraries/yukon/modules/quad_servo/quad_servo_reg.cpp index ad6f848ca..c9a93eaf9 100644 --- a/libraries/yukon/modules/quad_servo/quad_servo_reg.cpp +++ b/libraries/yukon/modules/quad_servo/quad_servo_reg.cpp @@ -1,4 +1,6 @@ #include "quad_servo_reg.hpp" +#include "../../logging.hpp" +#include "../../errors.hpp" namespace pimoroni { @@ -11,4 +13,106 @@ namespace pimoroni { return adc_level == ADC_FLOAT && slow1 == LOW && slow2 == HIGH && slow3 == LOW; } + QuadServoRegModule::QuadServoRegModule(bool halt_on_not_pgood) : + YukonModule(), + halt_on_not_pgood(halt_on_not_pgood), + servos(nullptr) { + } + + QuadServoRegModule::~QuadServoRegModule() { + delete(servos); + } + + std::string QuadServoRegModule::name() { + return QuadServoRegModule::NAME; + } + + void QuadServoRegModule::initialise(const SLOT& slot, SlotAccessor& accessor) { + // Configure strip and power pins + configure(); + + // Pass the slot and adc functions up to the parent now that module specific initialisation has finished + YukonModule::initialise(slot, accessor); + } + + void QuadServoRegModule::configure() { + + } + + void QuadServoRegModule::enable() { + + } + + void QuadServoRegModule::disable() { + + } + + bool QuadServoRegModule::is_enabled() { + return 0; // TODO + } + + bool QuadServoRegModule::read_power_good() { + return 0; // TODO + } + + float QuadServoRegModule::read_temperature() { + return __read_adc2_as_temp(); + } + + void QuadServoRegModule::monitor() { + bool pgood = read_power_good(); + if(!pgood) { + if(halt_on_not_pgood) { + throw FaultError(__message_header() + "Power is not good! Turning off output\n"); + } + } + + float temperature = read_temperature(); + if(temperature > TEMPERATURE_THRESHOLD) { + throw OverTemperatureError(__message_header() + "Temperature of " + std::to_string(temperature) + "°C exceeded the user set level of " + std::to_string(TEMPERATURE_THRESHOLD) + "°C! Turning off output\n"); + } + + if(last_pgood && !pgood) { + logging.warn(__message_header() + "Power is not good\n"); + } + else if(!last_pgood && pgood) { + logging.warn(__message_header() + "Power is good\n"); + } + + // Run some user action based on the latest readings + //if self.__monitor_action_callback is not None: + // self.__monitor_action_callback(pgood, temperature) + + last_pgood = pgood; + power_good_throughout = power_good_throughout && pgood; + + max_temperature = MAX(temperature, max_temperature); + min_temperature = MIN(temperature, min_temperature); + avg_temperature += temperature; + count_avg += 1; + } + + std::vector> QuadServoRegModule::get_readings() { + std::vector> values; + values.push_back(std::pair("PGood", power_good_throughout)); + values.push_back(std::pair("T_max", max_temperature)); + values.push_back(std::pair("T_min", min_temperature)); + values.push_back(std::pair("T_avg", avg_temperature)); + return values; + } + + void QuadServoRegModule::process_readings() { + if(count_avg > 0) { + avg_temperature /= count_avg; + } + } + + void QuadServoRegModule::clear_readings() { + power_good_throughout = true; + max_temperature = -std::numeric_limits::infinity(); + min_temperature = std::numeric_limits::infinity(); + avg_temperature = 0; + count_avg = 0; + } + } diff --git a/libraries/yukon/modules/quad_servo/quad_servo_reg.hpp b/libraries/yukon/modules/quad_servo/quad_servo_reg.hpp index 2a01a233e..66258e5c3 100644 --- a/libraries/yukon/modules/quad_servo/quad_servo_reg.hpp +++ b/libraries/yukon/modules/quad_servo/quad_servo_reg.hpp @@ -1,20 +1,81 @@ #pragma once #include "../common.hpp" +#include "servo_cluster.hpp" +#include "io.hpp" + +using namespace servo; namespace pimoroni { class QuadServoRegModule : public YukonModule { + //-------------------------------------------------- + // Constants + //-------------------------------------------------- public: static const std::string NAME; + static const uint NUM_SERVOS = 4; + static constexpr float TEMPERATURE_THRESHOLD = 50.0f; + + //-------------------------------------------------- + // Statics + //-------------------------------------------------- static bool is_module(uint adc_level, bool slow1, bool slow2, bool slow3); + TYPE_FUNCTION(QuadServoRegModule) - virtual std::string name() { - return QuadServoRegModule::NAME; - } - TYPE_FUNCTION(QuadServoRegModule) + //-------------------------------------------------- + // Variables + //-------------------------------------------------- +public: + bool halt_on_not_pgood; + + //-------------------------------------------------- + private: + bool last_pgood; + bool power_good_throughout; + float max_temperature; + float min_temperature; + float avg_temperature; + float count_avg; + + //-------------------------------------------------- + public: + ServoCluster* servos; + private: + IO* power_en; + IO* power_good; + + + //-------------------------------------------------- + // Constructors/Destructor + //-------------------------------------------------- + public: + QuadServoRegModule(bool halt_on_not_pgood = false); + virtual ~QuadServoRegModule(); + + + //-------------------------------------------------- + // Methods + //-------------------------------------------------- + public: + virtual std::string name(); + virtual void initialise(const SLOT& slot, SlotAccessor& accessor); + virtual void configure(); + + //-------------------------------------------------- + void enable(); + void disable(); + bool is_enabled(); + bool read_power_good(); + float read_temperature(); + + //-------------------------------------------------- + virtual void monitor(); + virtual std::vector> get_readings(); + virtual void process_readings(); + virtual void clear_readings(); }; } diff --git a/libraries/yukon/yukon.cmake b/libraries/yukon/yukon.cmake index 399e0cebf..071d8dcff 100644 --- a/libraries/yukon/yukon.cmake +++ b/libraries/yukon/yukon.cmake @@ -17,4 +17,4 @@ target_sources(yukon INTERFACE target_include_directories(yukon INTERFACE ${CMAKE_CURRENT_LIST_DIR}) # Pull in pico libraries that we need -target_link_libraries(yukon INTERFACE pico_stdlib pico_graphics tca9555 hardware_adc io plasma) +target_link_libraries(yukon INTERFACE pico_stdlib pico_graphics tca9555 hardware_adc io plasma servo motor encoder)