src/modules/tools/filamentdetector/FilamentDetector.cpp

   1
   2 /*
   3     Handles a filament detector that has an optical encoder wheel, that generates pulses as the filament
   4     moves through it.
   5     It also supports a "bulge" detector that triggers if the filament has a bulge in it
   6 */
   7
   8 #include "FilamentDetector.h"
   9 #include "Kernel.h"
  10 #include "Config.h"
  11 #include "checksumm.h"
  12 #include "ConfigValue.h"
  13 #include "SlowTicker.h"
  14 #include "PublicData.h"
  15 #include "StreamOutputPool.h"
  16 #include "StreamOutput.h"
  17 #include "SerialMessage.h"
  18 #include "FilamentDetector.h"
  19 #include "utils.h"
  20 #include "Gcode.h"
  21
  22 #include "InterruptIn.h" // mbed
  23 #include "us_ticker_api.h" // mbed
  24
  25 #define extruder_checksum CHECKSUM("extruder")
  26
  27 #define filament_detector_checksum  CHECKSUM("filament_detector")
  28 #define enable_checksum             CHECKSUM("enable")
  29 #define encoder_pin_checksum        CHECKSUM("encoder_pin")
  30 #define bulge_pin_checksum          CHECKSUM("bulge_pin")
  31 #define seconds_per_check_checksum  CHECKSUM("seconds_per_check")
  32 #define pulses_per_mm_checksum      CHECKSUM("pulses_per_mm")
  33
  34 FilamentDetector::FilamentDetector()
  35 {
  36     suspended= false;
  37     filament_out_alarm= false;
  38     bulge_detected= false;
  39     active= true;
  40     e_last_moved= NAN;
  41 }
  42
  43 FilamentDetector::~FilamentDetector()
  44 {
  45     if(encoder_pin != nullptr) delete encoder_pin;
  46 }
  47
  48 void FilamentDetector::on_module_loaded()
  49 {
  50     // if the module is disabled -> do nothing
  51     if(!THEKERNEL->config->value( filament_detector_checksum, enable_checksum )->by_default(false)->as_bool()) {
  52         // as this module is not needed free up the resource
  53         delete this;
  54         return;
  55     }
  56
  57     // encoder pin has to be interrupt enabled pin like 0.26, 0.27, 0.28
  58     Pin dummy_pin;
  59     dummy_pin.from_string( THEKERNEL->config->value(filament_detector_checksum, encoder_pin_checksum)->by_default("nc" )->as_string());
  60     this->encoder_pin= dummy_pin.interrupt_pin();
  61     if(this->encoder_pin == nullptr) {
  62         // was not a valid interrupt pin
  63         delete this;
  64         return;
  65     }
  66
  67     // set interrupt on rising edge
  68     this->encoder_pin->rise(this, &FilamentDetector::on_pin_rise);
  69     NVIC_SetPriority(EINT3_IRQn, 16); // set to low priority
  70
  71     // optional bulge detector
  72     bulge_pin.from_string( THEKERNEL->config->value(filament_detector_checksum, bulge_pin_checksum)->by_default("nc" )->as_string())->as_input();
  73     if(bulge_pin.connected()) {
  74         // input pin polling
  75         THEKERNEL->slow_ticker->attach( 100, this, &FilamentDetector::button_tick);
  76     }
  77
  78     // how many seconds between checks, must be long enough for several pulses to be detected, but not too long
  79     seconds_per_check= THEKERNEL->config->value(filament_detector_checksum, seconds_per_check_checksum)->by_default(2)->as_number();
  80
  81     // the number of pulses per mm of filament moving through the detector, can be fractional
  82     pulses_per_mm= THEKERNEL->config->value(filament_detector_checksum, pulses_per_mm_checksum)->by_default(1)->as_number();
  83
  84     // register event-handlers
  85     register_for_event(ON_SECOND_TICK);
  86     register_for_event(ON_MAIN_LOOP);
  87     register_for_event(ON_CONSOLE_LINE_RECEIVED);
  88     this->register_for_event(ON_GCODE_RECEIVED);
  89 }
  90
  91 void FilamentDetector::send_command(std::string msg, StreamOutput *stream)
  92 {
  93     struct SerialMessage message;
  94     message.message = msg;
  95     message.stream = stream;
  96     THEKERNEL->call_event(ON_CONSOLE_LINE_RECEIVED, &message );
  97 }
  98
  99 // needed to detect when we resume
 100 void FilamentDetector::on_console_line_received( void *argument )
 101 {
 102     if(!suspended) return;
 103
 104     SerialMessage new_message = *static_cast<SerialMessage *>(argument);
 105     string possible_command = new_message.message;
 106     string cmd = shift_parameter(possible_command);
 107     if(cmd == "resume") {
 108         this->pulses= 0;
 109         e_last_moved= NAN;
 110         suspended= false;
 111     }
 112 }
 113
 114 float FilamentDetector::get_emove()
 115 {
 116     float *rd;
 117     if(PublicData::get_value( extruder_checksum, (void **)&rd )) {
 118         return *(rd+5); // current position for extruder in mm
 119     }
 120     return NAN;
 121 }
 122
 123 void FilamentDetector::on_gcode_received(void *argument)
 124 {
 125     Gcode *gcode = static_cast<Gcode *>(argument);
 126     if (gcode->has_m) {
 127         if (gcode->m == 404) { // set filament detector parameters S seconds per check, P pulses per mm
 128             if(gcode->has_letter('S')){
 129                 seconds_per_check= gcode->get_value('S');
 130                 seconds_passed= 0;
 131             }
 132             if(gcode->has_letter('P')){
 133                 pulses_per_mm= gcode->get_value('P');
 134             }
 135             gcode->stream->printf("// pulses per mm: %f, seconds per check: %d\n", pulses_per_mm, seconds_per_check);
 136
 137         } else if (gcode->m == 405) { // disable filament detector
 138             active= false;
 139             e_last_moved= get_emove();
 140
 141         }else if (gcode->m == 406) { // enable filament detector
 142             this->pulses= 0;
 143             e_last_moved=  get_emove();
 144             active= true;
 145
 146         }else if (gcode->m == 407) { // display filament detector pulses and status
 147             float e_moved= get_emove();
 148             if(!isnan(e_moved)) {
 149                 float delta= e_moved - e_last_moved;
 150                 gcode->stream->printf("Extruder moved: %f mm\n", delta);
 151             }
 152
 153             gcode->stream->printf("Encoder pulses: %u\n", pulses.load());
 154             if(this->suspended) gcode->stream->printf("Filament detector triggered\n");
 155             gcode->stream->printf("Filament detector is %s\n", active?"enabled":"disabled");
 156         }
 157     }
 158 }
 159
 160 void FilamentDetector::on_main_loop(void *argument)
 161 {
 162     if (active && this->filament_out_alarm) {
 163         this->filament_out_alarm = false;
 164         if(bulge_detected){
 165             THEKERNEL->streams->printf("// Filament Detector has detected a bulge in the filament\n");
 166             bulge_detected= false;
 167         }else{
 168             THEKERNEL->streams->printf("// Filament Detector has detected a filament jam\n");
 169         }
 170
 171         if(!suspended) {
 172             this->suspended= true;
 173             // fire suspend command
 174             this->send_command( "M600", &(StreamOutput::NullStream) );
 175         }
 176     }
 177 }
 178
 179 void FilamentDetector::on_second_tick(void *argument)
 180 {
 181     if(++seconds_passed >= seconds_per_check) {
 182         seconds_passed= 0;
 183         check_encoder();
 184     }
 185 }
 186
 187 // encoder pin interrupt
 188 void FilamentDetector::on_pin_rise()
 189 {
 190     this->pulses++;
 191 }
 192
 193 void FilamentDetector::check_encoder()
 194 {
 195     if(suspended) return; // already suspended
 196     if(!active) return;  // not enabled
 197
 198     uint32_t pulse_cnt= this->pulses.exchange(0); // atomic load and reset
 199
 200     // get number of E steps taken and make sure we have seen enough pulses to cover that
 201     float e_moved= get_emove();
 202     if(isnan(e_last_moved)) {
 203         e_last_moved= e_moved;
 204         return;
 205     }
 206
 207     float delta= e_moved - e_last_moved;
 208     e_last_moved= e_moved;
 209     if(delta < 0) {
 210         // we ignore retracts for the purposes of jam detection
 211         return;
 212     }
 213
 214     // figure out how many pulses need to have happened to cover that e move
 215     uint32_t needed_pulses= floorf(delta*pulses_per_mm);
 216     // NOTE if needed_pulses is 0 then extruder did not move since last check, or not enough to register
 217     if(needed_pulses == 0) return;
 218
 219     if(pulse_cnt == 0) {
 220         // we got no pulses and E moved since last time so fire off alarm
 221         this->filament_out_alarm= true;
 222     }
 223 }
 224
 225 uint32_t FilamentDetector::button_tick(uint32_t dummy)
 226 {
 227     if(!bulge_pin.connected() || suspended) return 0;
 228
 229     if(bulge_pin.get()) {
 230         // we got a trigger from the bulge detector
 231         this->filament_out_alarm= true;
 232         this->bulge_detected= true;
 233     }
 234
 235     return 0;
 236 }