Fist hack at filament detector module

diff --git a/src/libs/Pin.cpp b/src/libs/Pin.cpp
index 97e45ba..d59fd17 100644 (file)
--- a/src/libs/Pin.cpp
+++ b/src/libs/Pin.cpp
@@ -3,7 +3,9 @@
 
 // mbed libraries for hardware pwm
 #include "PwmOut.h"
+#include "InterruptIn.h"
 #include "PinNames.h"
+#include "port_api.h"
 
 Pin::Pin(){
     this->inverting= false;
@@ -184,3 +186,20 @@ mbed::PwmOut* Pin::hardware_pwm()
     }
     return nullptr;
 }
+
+mbed::InterruptIn* Pin::interrupt_pin()
+{
+    if(!this->valid) return nullptr;
+
+    // set as input
+    as_input();
+
+    if (port_number == 0 || port_number == 2) {
+        PinName pinname = port_pin((PortName)port_number, pin);
+        return new mbed::InterruptIn(pinname);
+
+    }else{
+        this->valid= false;
+        return nullptr;
+    }
+}

diff --git a/src/libs/Pin.h b/src/libs/Pin.h
index 53ad20b..48a6c64 100644 (file)
--- a/src/libs/Pin.h
+++ b/src/libs/Pin.h
@@ -10,6 +10,7 @@
 
 namespace mbed {
     class PwmOut;
+    class InterruptIn;
 }
 
 class Pin {
@@ -64,6 +65,8 @@ class Pin {
 
         mbed::PwmOut *hardware_pwm();
 
+        mbed::InterruptIn *interrupt_pin();
+
         // these should be private, and use getters
         LPC_GPIO_TypeDef* port;
 

diff --git a/src/modules/tools/filamentdetector/FilamentDetector.cpp b/src/modules/tools/filamentdetector/FilamentDetector.cpp
new file mode 100644 (file)
index 0000000..e87da5c
--- /dev/null
+++ b/src/modules/tools/filamentdetector/FilamentDetector.cpp
@@ -0,0 +1,140 @@
+
+/*
+    Handles a filament detector that has an optical encoder wheel, that generates pulses as the filament
+    moves through it.
+    It also supports a "bulge" detector that triggers if the filament has a bulge in it
+*/
+
+#include "FilamentDetector.h"
+#include "Kernel.h"
+#include "Config.h"
+#include "checksumm.h"
+#include "ConfigValue.h"
+#include "SlowTicker.h"
+#include "PublicData.h"
+#include "StreamOutputPool.h"
+#include "StreamOutput.h"
+#include "SerialMessage.h"
+#include "FilamentDetector.h"
+#include "utils.h"
+
+#include "InterruptIn.h" // mbed
+#include "us_ticker_api.h" // mbed
+
+#define extruder_checksum CHECKSUM("extruder")
+
+#define filament_detector_checksum  CHECKSUM("filament_detector")
+#define enable_checksum             CHECKSUM("enable")
+#define encoder_pin_checksum        CHECKSUM("encoder_pin")
+#define seconds_per_check_checksum  CHECKSUM("seconds_per_check")
+#define pulses_per_mm_checksum      CHECKSUM("pulses_per_mm")
+
+FilamentDetector::FilamentDetector()
+{
+    suspended= false;
+    filament_out_alarm= false;
+}
+
+FilamentDetector::~FilamentDetector()
+{
+    if(encoder_pin != nullptr) delete encoder_pin;
+}
+
+void FilamentDetector::on_module_loaded()
+{
+    // if the module is disabled -> do nothing
+    if(!THEKERNEL->config->value( filament_detector_checksum, enable_checksum )->by_default(false)->as_bool()) {
+        // as this module is not needed free up the resource
+        delete this;
+        return;
+    }
+
+    // load settings
+    Pin dummy_pin;
+    dummy_pin.from_string( THEKERNEL->config->value(filament_detector_checksum, encoder_pin_checksum)->by_default("nc" )->as_string());
+    this->encoder_pin= dummy_pin.interrupt_pin();
+    if(this->encoder_pin == nullptr) {
+        // was not a valid interrupt pin
+        delete this;
+        return;
+    }
+
+    this->encoder_pin->rise(this, &FilamentDetector::on_pin_rise);
+    NVIC_SetPriority(EINT3_IRQn, 16); // set to low priority
+
+    seconds_per_check= THEKERNEL->config->value(filament_detector_checksum, seconds_per_check_checksum)->by_default(2)->as_number();
+    pulses_per_mm= THEKERNEL->config->value(filament_detector_checksum, pulses_per_mm_checksum)->by_default(1)->as_number();
+
+    // register event-handlers
+    register_for_event(ON_SECOND_TICK);
+    register_for_event(ON_MAIN_LOOP);
+    register_for_event(ON_CONSOLE_LINE_RECEIVED);
+}
+
+void FilamentDetector::send_command(std::string msg, StreamOutput *stream)
+{
+    struct SerialMessage message;
+    message.message = msg;
+    message.stream = stream;
+    THEKERNEL->call_event(ON_CONSOLE_LINE_RECEIVED, &message );
+}
+
+void FilamentDetector::on_console_line_received( void *argument )
+{
+    if(!suspended) return;
+
+    SerialMessage new_message = *static_cast<SerialMessage *>(argument);
+    string possible_command = new_message.message;
+    string cmd = shift_parameter(possible_command);
+    if(cmd == "resume") {
+        suspended= false;
+    }
+}
+
+void FilamentDetector::on_main_loop(void *argument)
+{
+    if (this->filament_out_alarm) {
+        this->filament_out_alarm = false;
+        THEKERNEL->streams->printf("// Filament Detector has detected a filament jam\n");
+        this->suspended= true;
+        // fire suspend command
+        this->send_command( "M600", &(StreamOutput::NullStream) );
+    }
+}
+
+void FilamentDetector::on_second_tick(void *argument)
+{
+    if(++seconds_passed >= seconds_per_check) {
+        seconds_passed= 0;
+        check_encoder();
+    }
+}
+
+// encoder pin interrupt
+void FilamentDetector::on_pin_rise()
+{
+    this->pulses++;
+}
+
+void FilamentDetector::check_encoder()
+{
+    uint32_t pulse_cnt= this->pulses.exchange(0); // atomic load and reset
+    if(suspended) return; // already suspended
+
+    // get number of E steps taken and make sure we have seen enough pulses to cover that
+    float *rd;
+    if(!PublicData::get_value( extruder_checksum, (void **)&rd )) return;
+    float e_moved= *(rd+5); // current position for extruder in mm
+    float delta= e_moved - e_last_moved;
+    e_last_moved= e_moved;
+
+    // figure out how many pulses need to have happened to cover that e move
+    uint32_t needed_pulses= floorf(delta*pulses_per_mm);
+    // NOTE if needed_pulses is 0 then extruder did not move since last check, or not enough to register
+    if(needed_pulses == 0) return;
+
+    if(pulse_cnt == 0) {
+        // we got no pulses and E moved since last time so fire off alarm
+        this->filament_out_alarm= true;
+    }
+}

diff --git a/src/modules/tools/filamentdetector/FilamentDetector.h b/src/modules/tools/filamentdetector/FilamentDetector.h
new file mode 100644 (file)
index 0000000..ac2aa69
--- /dev/null
+++ b/src/modules/tools/filamentdetector/FilamentDetector.h
@@ -0,0 +1,43 @@
+#pragma once
+
+#include "Module.h"
+
+#include <stdint.h>
+#include <atomic>
+#include <string>
+
+namespace mbed {
+    class InterruptIn;
+}
+
+class Pin;
+class StreamOutput;
+
+class FilamentDetector: public Module
+{
+public:
+    FilamentDetector();
+    ~FilamentDetector();
+    void on_module_loaded();
+    void on_main_loop(void* argument);
+    void on_second_tick(void* argument);
+    void on_console_line_received( void *argument );
+
+private:
+    void on_pin_rise();
+    void check_encoder();
+    void send_command(std::string msg, StreamOutput *stream);
+
+    mbed::InterruptIn *encoder_pin{0};
+    Pin *bulge_pin{0};
+    float e_last_moved{0};
+    std::atomic_uint pulses{0};
+    float pulses_per_mm{0};
+    uint8_t seconds_per_check{1};
+    uint8_t seconds_passed{0};
+
+    struct {
+        bool filament_out_alarm:1;
+        bool suspended:1;
+    };
+};