Bladeren bron

moved LOW to to Atm_AccelStepper

updated whole firmware, quite ready now
titi 5 jaren geleden
bovenliggende
commit
4b0326dc21
100 gewijzigde bestanden met toevoegingen van 2329 en 4047 verwijderingen
  1. 519 0
      HTequi-firmware/lib/Atm_lien/Atm_AccelStepper.cpp
  2. 190 0
      HTequi-firmware/lib/Atm_lien/Atm_AccelStepper.h
  3. 0 412
      HTequi-firmware/lib/Atm_lien/Atm_Teenstep.cpp
  4. 0 157
      HTequi-firmware/lib/Atm_lien/Atm_Teenstep.h
  5. 0 36
      HTequi-firmware/lib/Atm_lien/Atm_Teenstep_OSC.cpp
  6. 0 31
      HTequi-firmware/lib/Atm_lien/Atm_Teenstep_OSC.h
  7. 0 268
      HTequi-firmware/lib/Atm_lien/Atm_TeensyStep.cpp
  8. 0 120
      HTequi-firmware/lib/Atm_lien/Atm_TeensyStep.h
  9. 0 519
      HTequi-firmware/lib/Atm_lien/Atm_Tstepper.cpp
  10. 0 184
      HTequi-firmware/lib/Atm_lien/Atm_Tstepper.h
  11. 0 30
      HTequi-firmware/lib/Atm_lien/Atm_Tstepper_OSC.cpp
  12. 0 26
      HTequi-firmware/lib/Atm_lien/Atm_Tstepper_OSC.h
  13. 0 260
      HTequi-firmware/lib/Atm_lien/Atm_out.cpp
  14. 0 67
      HTequi-firmware/lib/Atm_lien/Atm_out.h
  15. 0 152
      HTequi-firmware/lib/Automaton-1.0.2/src/Atm_fade.cpp
  16. 0 39
      HTequi-firmware/lib/Automaton-1.0.2/src/Atm_fade.hpp
  17. 0 241
      HTequi-firmware/lib/Automaton-1.0.2/src/Atm_led.cpp
  18. 0 56
      HTequi-firmware/lib/Automaton-1.0.2/src/Atm_led.hpp
  19. 0 242
      HTequi-firmware/lib/Automaton-1.0.2/src/Atm_player.cpp
  20. 0 112
      HTequi-firmware/lib/Automaton-1.0.2/src/Atm_player.hpp
  21. 0 0
      HTequi-firmware/lib/Automaton-master/.clang-format
  22. 0 0
      HTequi-firmware/lib/Automaton-master/.gitattributes
  23. 0 0
      HTequi-firmware/lib/Automaton-master/.gitignore
  24. 0 0
      HTequi-firmware/lib/Automaton-master/LICENSE
  25. 0 0
      HTequi-firmware/lib/Automaton-master/README.md
  26. 0 0
      HTequi-firmware/lib/Automaton-master/examples/blink/blink.ino
  27. 0 0
      HTequi-firmware/lib/Automaton-master/examples/blink_modular/Atm_blink.cpp
  28. 0 0
      HTequi-firmware/lib/Automaton-master/examples/blink_modular/Atm_blink.h
  29. 0 0
      HTequi-firmware/lib/Automaton-master/examples/blink_modular/blink_modular.ino
  30. 0 0
      HTequi-firmware/lib/Automaton-master/examples/button/button.ino
  31. 5 0
      HTequi-firmware/lib/Automaton-master/examples/extern_LED/.vscode/settings.json
  32. 36 0
      HTequi-firmware/lib/Automaton-master/examples/extern_LED/Atm_led_mcp.cpp
  33. 28 0
      HTequi-firmware/lib/Automaton-master/examples/extern_LED/Atm_led_mcp.h
  34. 18 0
      HTequi-firmware/lib/Automaton-master/examples/extern_LED/main.cpp
  35. 0 0
      HTequi-firmware/lib/Automaton-master/examples/fade/fade.ino
  36. 0 0
      HTequi-firmware/lib/Automaton-master/examples/frere_jacques/frere_jacques.ino
  37. 0 0
      HTequi-firmware/lib/Automaton-master/examples/frere_jacques/musical_notes.h
  38. 0 0
      HTequi-firmware/lib/Automaton-master/examples/knight_rider1/knight_rider1.ino
  39. 0 0
      HTequi-firmware/lib/Automaton-master/examples/knight_rider2/Atm_sweep.cpp
  40. 0 0
      HTequi-firmware/lib/Automaton-master/examples/knight_rider2/Atm_sweep.h
  41. 0 0
      HTequi-firmware/lib/Automaton-master/examples/knight_rider2/knight_rider2.ino
  42. 0 0
      HTequi-firmware/lib/Automaton-master/examples/knight_rider3/knight_rider3.ino
  43. 0 0
      HTequi-firmware/lib/Automaton-master/examples/led_fuel_gauge/led_fuel_gauge.ino
  44. 0 0
      HTequi-firmware/lib/Automaton-master/examples/led_test/led_test.ino
  45. 0 0
      HTequi-firmware/lib/Automaton-master/examples/nuclear_missile_launcher/nuclear_missile_launcher.ino
  46. 0 0
      HTequi-firmware/lib/Automaton-master/examples/sos1/sos1.ino
  47. 0 0
      HTequi-firmware/lib/Automaton-master/examples/sos2/sos2.ino
  48. 0 0
      HTequi-firmware/lib/Automaton-master/examples/sos3/sos3.ino
  49. 0 0
      HTequi-firmware/lib/Automaton-master/extras/update.sh
  50. 0 0
      HTequi-firmware/lib/Automaton-master/keywords.txt
  51. 1 1
      HTequi-firmware/lib/Automaton-1.0.2/library.json
  52. 1 1
      HTequi-firmware/lib/Automaton-1.0.2/library.properties
  53. 54 0
      HTequi-firmware/lib/Automaton-master/platformio.ini
  54. 0 0
      HTequi-firmware/lib/Automaton-master/src/Atm_analog.cpp
  55. 0 0
      HTequi-firmware/lib/Automaton-master/src/Atm_analog.hpp
  56. 0 0
      HTequi-firmware/lib/Automaton-master/src/Atm_bit.cpp
  57. 0 0
      HTequi-firmware/lib/Automaton-master/src/Atm_bit.hpp
  58. 15 3
      HTequi-firmware/lib/Automaton-1.0.2/src/Atm_button.cpp
  59. 4 0
      HTequi-firmware/lib/Automaton-1.0.2/src/Atm_button.hpp
  60. 0 0
      HTequi-firmware/lib/Automaton-master/src/Atm_command.cpp
  61. 0 0
      HTequi-firmware/lib/Automaton-master/src/Atm_command.hpp
  62. 0 0
      HTequi-firmware/lib/Automaton-master/src/Atm_comparator.cpp
  63. 0 0
      HTequi-firmware/lib/Automaton-master/src/Atm_comparator.hpp
  64. 0 0
      HTequi-firmware/lib/Automaton-master/src/Atm_controller.cpp
  65. 0 0
      HTequi-firmware/lib/Automaton-master/src/Atm_controller.hpp
  66. 0 0
      HTequi-firmware/lib/Automaton-master/src/Atm_digital.cpp
  67. 0 0
      HTequi-firmware/lib/Automaton-master/src/Atm_digital.hpp
  68. 0 0
      HTequi-firmware/lib/Automaton-master/src/Atm_encoder.cpp
  69. 0 0
      HTequi-firmware/lib/Automaton-master/src/Atm_encoder.hpp
  70. 0 0
      HTequi-firmware/lib/Automaton-master/src/Atm_fan.cpp
  71. 0 0
      HTequi-firmware/lib/Automaton-master/src/Atm_fan.hpp
  72. 0 0
      HTequi-firmware/lib/Automaton-master/src/Atm_step.cpp
  73. 0 0
      HTequi-firmware/lib/Automaton-master/src/Atm_step.hpp
  74. 0 0
      HTequi-firmware/lib/Automaton-master/src/Atm_timer.cpp
  75. 0 0
      HTequi-firmware/lib/Automaton-master/src/Atm_timer.hpp
  76. 0 0
      HTequi-firmware/lib/Automaton-master/src/Automaton.cpp
  77. 3 3
      HTequi-firmware/lib/Automaton-1.0.2/src/Automaton.h
  78. 0 0
      HTequi-firmware/lib/Automaton-master/src/Machine.cpp
  79. 0 0
      HTequi-firmware/lib/Automaton-master/src/Machine.hpp
  80. 0 0
      HTequi-firmware/lib/Automaton-master/src/atm_connector.cpp
  81. 0 0
      HTequi-firmware/lib/Automaton-master/src/atm_connector.hpp
  82. 0 0
      HTequi-firmware/lib/Automaton-master/src/atm_counter.cpp
  83. 0 0
      HTequi-firmware/lib/Automaton-master/src/atm_counter.hpp
  84. 0 0
      HTequi-firmware/lib/Automaton-master/src/atm_serial_debug.hpp
  85. 8 0
      HTequi-firmware/lib/Automaton-1.0.2/src/atm_timer_millis.cpp
  86. 1 0
      HTequi-firmware/lib/Automaton-1.0.2/src/atm_timer_millis.hpp
  87. 129 91
      HTequi-firmware/src/blobcnc_low/main.cpp
  88. 2 2
      PCB/BLOBCNC_TOP/BLOBCNC_TOP-Edge.Cuts.gbr
  89. 499 236
      PCB/BLOBCNC_TOP/BLOBCNC_TOP-F.Cu.gbr
  90. 1 1
      PCB/BLOBCNC_TOP/BLOBCNC_TOP-NPTH.drl
  91. 53 49
      PCB/BLOBCNC_TOP/BLOBCNC_TOP-PTH.drl
  92. 2 2
      PCB/BLOBCNC_TOP/BLOBCNC_TOP-cache.lib
  93. 18 14
      PCB/BLOBCNC_TOP/BLOBCNC_TOP.bak
  94. 314 282
      PCB/BLOBCNC_TOP/BLOBCNC_TOP.kicad_pcb
  95. 6 5
      PCB/BLOBCNC_TOP/BLOBCNC_TOP.kicad_pcb-bak
  96. 4 4
      PCB/BLOBCNC_TOP/BLOBCNC_TOP.net
  97. 2 1
      PCB/BLOBCNC_TOP/BLOBCNC_TOP.sch
  98. 18 18
      PCB/BLOBCNC_TOP/CNC/blobcnc_TOP_CURT.gcode
  99. 398 382
      PCB/BLOBCNC_TOP/CNC/blobcnc_TOP_DRILL.gcode
  100. 0 0
      PCB/BLOBCNC_TOP/CNC/blobcnc_TOP_ISOL.gcode

+ 519 - 0
HTequi-firmware/lib/Atm_lien/Atm_AccelStepper.cpp

@@ -0,0 +1,519 @@
+#include "Atm_AccelStepper.h"
+
+/* Add optional parameters for the state machine to begin()
+ * Add extra initialization code
+ */
+
+Atm_AccelStepper& Atm_AccelStepper::begin(int step_pin, int dir_pin) {
+  // clang-format off
+  const static state_t state_table[] PROGMEM = {
+    /*                       ON_ENTER           ON_LOOP          ON_EXIT  EVT_DISABLE  EVT_ENABLE  EVT_ENABLED_TIMEOUT  EVT_MOVE  EVT_STOP  EVT_EMERGENCY_STOP  EVT_ON_LIMIT_LOW  EVT_ON_LIMIT_HIGH  EVT_ON_TARGET  EVT_HOMING_LOW  EVT_HOMING_HIGH  ELSE */
+    /*    DISABLE */     ENT_DISABLED,              -1,              -1,         -1,    ENABLED,                  -1,   RUNNING,        -1,                -1,               -1,                -1,            -1,    HOMING_LOW,     HOMING_HIGH,   -1,
+    /*     ENABLED */     ENT_ENABLED,              -1,              -1,    DISABLE,         -1,             DISABLE,   RUNNING,      STOP,              STOP,        LIMIT_LOW,        LIMIT_HIGH,            -1,    HOMING_LOW,     HOMING_HIGH,   -1,
+    /*     RUNNING */     ENT_RUNNING,      LP_RUNNING,              -1,    DISABLE,         -1,                  -1,   RUNNING,      STOP,              STOP,        LIMIT_LOW,        LIMIT_HIGH,       ENABLED,            -1,              -1,   -1,
+    /*        STOP */        ENT_STOP,         LP_STOP,              -1,    DISABLE,         -1,                  -1,   RUNNING,        -1,                -1,               -1,                -1,       ENABLED,            -1,              -1,   -1,
+    /*  HOMING_LOW */  ENT_HOMING_LOW,   LP_HOMING_LOW,  EXT_HOMING_LOW,    DISABLE,         -1,                  -1,        -1,      STOP,              STOP,        LIMIT_LOW,        LIMIT_HIGH,            -1,            -1,              -1,   -1,
+    /* HOMING_HIGH */ ENT_HOMING_HIGH,  LP_HOMING_HIGH, EXT_HOMING_HIGH,    DISABLE,         -1,                  -1,        -1,      STOP,              STOP,        LIMIT_LOW,        LIMIT_HIGH,            -1,            -1,              -1,   -1,
+    /*   LIMIT_LOW */   ENT_LIMIT_LOW,              -1,              -1,         -1,         -1,                  -1,   RUNNING,      STOP,              STOP,        LIMIT_LOW,                -1,            -1,            -1,              -1,   -1,
+    /*  LIMIT_HIGH */  ENT_LIMIT_HIGH,              -1,              -1,         -1,         -1,                  -1,   RUNNING,      STOP,              STOP,               -1,        LIMIT_HIGH,            -1,            -1,              -1,   -1
+  };
+  // clang-format on
+  Machine::begin( state_table, ELSE );
+  stepper = new AccelStepper(1, step_pin, dir_pin);
+  stepper->setMaxSpeed(max_speed);
+  stepper->setAcceleration(acceleration);
+  idle_timer.set(ATM_TIMER_OFF);
+  position_timer.set(POSITION_SEND_TIMER);
+  return *this;
+}
+
+/* Add C++ code for each internally handled event (input)
+ * The code must return 1 to trigger the event
+ */
+
+int Atm_AccelStepper::event( int id ) {
+  //updateLimitSwitch();
+  switch ( id ) {
+    case EVT_DISABLE:
+      return 0;
+    case EVT_ENABLE:
+      return 0;
+    case EVT_ENABLED_TIMEOUT:
+      return 0;
+    case EVT_MOVE:
+      return 0;
+    case EVT_STOP:
+      return 0;
+    case EVT_EMERGENCY_STOP:
+      return 0;
+    case EVT_ON_LIMIT_LOW:
+      switch(_limitLow_Mode) {
+        case 0:
+          //
+          Serial.println("no limit");
+          return 0;
+        case 1: //digital INPUT
+          // Serial.println("digital");
+          limitLow_State = digitalRead(_limitLow_Pin);
+          limitLow_State = _limitLow_Reversed ? !limitLow_State : limitLow_State;
+          return limitLow_State;
+        case 2:
+
+          int analogTemp = analogRead(_limitLow_Pin);
+          limitLow_State = (_limitLow_Thresholds[0] < analogTemp) && (analogTemp < _limitLow_Thresholds[1]);
+          limitLow_State = _limitLow_Reversed ? !limitLow_State : limitLow_State;
+          return limitLow_State;
+      }
+    case EVT_ON_LIMIT_HIGH:
+      switch(_limitHigh_Mode) {
+        case 0:
+          return 0;
+        case 1: //digital INPUT
+          limitHigh_State = digitalRead(_limitHigh_Pin);
+          limitHigh_State = _limitHigh_Reversed ? !limitHigh_State : limitHigh_State;
+          return limitHigh_State;
+        case 2:
+        //Serial.println("analog");
+          int analogTemp = analogRead(_limitHigh_Pin);
+          limitHigh_State = (_limitHigh_Thresholds[0] < analogTemp) && (analogTemp < _limitHigh_Thresholds[1]);
+          limitHigh_State = _limitHigh_Reversed ? !limitHigh_State : limitHigh_State;
+          return limitHigh_State;
+      }
+    case EVT_ON_TARGET:
+      return runMode ? 0 : _currentStep == _targetStep;
+    case EVT_HOMING_LOW:
+      return 0;
+    case EVT_HOMING_HIGH:
+      return 0;
+  }
+  return 0;
+}
+
+/* Add C++ code for each action
+ * This generates the 'output' for the state machine
+ *
+ * Available connectors:
+ *   push( connectors, ON_CHANGEPOSITION, 0, <v>, <up> );
+ *   push( connectors, ON_CHANGESTATE, 0, <v>, <up> );
+ *   push( connectors, ON_ONLIMITHIGH, 0, <v>, <up> );
+ *   push( connectors, ON_ONLIMITLOW, 0, <v>, <up> );
+ *   push( connectors, ON_ONTARGET, 0, <v>, <up> );
+ *   push( connectors, ON_STOP, 0, <v>, <up> );
+ */
+
+void Atm_AccelStepper::action( int id ) {
+  switch ( id ) {
+    case ENT_DISABLED:
+      push(connectors, ON_CHANGESTATE, 0,  state(), 0);
+      enabled = _enableReversed ? HIGH : LOW;
+      digitalWrite(_enablePin, enabled);
+      return;
+    case ENT_ENABLED:
+      _isHoming = 0 ;
+      if(last_trigger == EVT_ON_TARGET){push( connectors, ON_ONTARGET, 0, _currentStep, 0 );};
+      push(connectors, ON_CHANGESTATE, 0,  state(), 0);
+      enabled = _enableReversed ? LOW : HIGH ;
+      digitalWrite(_enablePin, enabled);
+
+
+      return;
+    case ENT_RUNNING:
+      push(connectors, ON_CHANGESTATE, 0,  state(), 0);
+      _isHoming = 0;
+      //push(connectors, ON_CHANGEPOSITION, 0,  _currentStep, stepper->speed());
+      position_timer.setFromNow(this, POSITION_SEND_TIMER);
+      return;
+    case LP_RUNNING:
+      stepper_update();
+      return;
+    case ENT_STOP:
+    push(connectors, ON_CHANGESTATE, 0,  state(), 0);
+      if (last_trigger == EVT_STOP) {
+        runMode = 0 ;
+        stepper->stop();
+        _targetStep = stepper->targetPosition();
+        push( connectors, ON_STOP, 0, 0, 0 );
+      }
+      if (last_trigger == EVT_EMERGENCY_STOP) {
+        _currentStep = stepper->currentPosition();
+        _targetStep = _currentStep ;
+        stepper->moveTo(_targetStep);
+        stepper->setSpeed(0);
+        push( connectors, ON_STOP, 0, 1, 0 );
+      }
+      return;
+    case LP_STOP:
+      stepper_update();
+      _currentStep = stepper->currentPosition();
+      return;
+    case ENT_HOMING_LOW:
+      push(connectors, ON_CHANGESTATE, 0,  state(), 0);
+      runMode = 1;
+      _isHoming = 1 ;
+      stepper->setSpeed(-1*homing_speed);
+      return;
+    case LP_HOMING_LOW:
+      stepper_update();
+      return;
+    case EXT_HOMING_LOW:
+      if(last_trigger == EVT_ON_LIMIT_LOW) {
+        stepper->setCurrentPosition(0);
+        _currentStep = 0;
+        Serial.println("homing low done");
+      }
+      else{Serial.println("homing low failed");}
+      _targetStep = _currentStep;
+
+      return;
+    case ENT_HOMING_HIGH:
+      push(connectors, ON_CHANGESTATE, 0,  state(), 0);
+      runMode = 1;
+      _isHoming = 2 ;
+      stepper->setSpeed(homing_speed);
+      return;
+    case LP_HOMING_HIGH:
+      stepper_update();
+      return;
+    case EXT_HOMING_HIGH:
+      if(last_trigger == EVT_ON_LIMIT_HIGH) {
+        _maxStep = stepper->currentPosition();
+        _currentStep = _maxStep;
+        Serial.println("homing high done");
+      }
+      else{Serial.println("homing high failed");}
+      _targetStep = _currentStep;
+      return;
+    case ENT_LIMIT_LOW:
+      push( connectors, ON_ONLIMITLOW, 0, 0, 0 );
+      //stop motor if going down, allow going up
+      if(_limitLow_Hard && (stepper->speed()<0) ) {trigger(EVT_EMERGENCY_STOP);}
+      else{
+        stepper_update();
+         switch(_isHoming) trigger.EVT_HOMINGtrigger(EVT_MOVE);}
+      return;
+    case ENT_LIMIT_HIGH:
+      push( connectors, ON_ONLIMITHIGH, 0, 1, 0 );
+      if(_limitHigh_Hard && (stepper->speed()>0)) {trigger(EVT_EMERGENCY_STOP);}
+      else{stepper_update(); trigger(EVT_MOVE);}
+      return;
+  }
+}
+
+/* Optionally override the default trigger() method
+ * Control how your machine processes triggers
+ */
+
+Atm_AccelStepper& Atm_AccelStepper::trigger( int event ) {
+  Machine::trigger( event );
+  return *this;
+}
+
+/* Optionally override the default state() method
+ * Control what the machine returns when another process requests its state
+ */
+
+int Atm_AccelStepper::state( void ) {
+  return Machine::state();
+}
+
+/* Nothing customizable below this line
+ ************************************************************************************************
+*/
+
+/* Still I'll customize a little just here
+*/
+
+void Atm_AccelStepper::stepper_update(void) {
+  switch (runMode) {
+    case 0: //positional modae
+      stepper->run();
+      break;
+    case 1: // speed mode
+      stepper->runSpeed();
+      break;
+  }
+  long int tempStep = stepper->currentPosition();
+  if (tempStep != _currentStep){
+    _currentStep =  tempStep;
+    if (position_timer.expired(this)){
+      push(connectors, ON_CHANGEPOSITION, 0,  _currentStep, stepper->speed());
+      position_timer.setFromNow(this, POSITION_SEND_TIMER);
+    }
+  }
+}
+
+Atm_AccelStepper& Atm_AccelStepper::setMaxSpeed( long int maxSpeed){
+  max_speed = maxSpeed ;
+  stepper->setMaxSpeed(max_speed);
+  return *this ;
+}
+
+Atm_AccelStepper& Atm_AccelStepper::setHomingSpeed(long int homingSpeed){
+  homing_speed = homingSpeed ;
+  return *this ;
+}
+
+Atm_AccelStepper& Atm_AccelStepper::setAcceleration(long int acc){
+  acceleration = acc ;
+  stepper->setAcceleration(max_speed);
+  return *this ;
+}
+
+Atm_AccelStepper& Atm_AccelStepper::position_refresh(long int refresh_ms){
+  POSITION_SEND_TIMER = refresh_ms ;
+  return *this ;
+}
+
+Atm_AccelStepper& Atm_AccelStepper::move( long int stepRel) {
+  _targetStep   = _currentStep + stepRel;
+  runMode = 0;
+  _isHoming = 0;
+  //Serial.println(_targetStep);
+  stepper->moveTo(_targetStep);
+  enable();
+  trigger( EVT_MOVE );
+  return *this;
+}
+
+Atm_AccelStepper& Atm_AccelStepper::moveTo( long int stepAbs) {
+  _targetStep   = stepAbs;
+  _isHoming = 0 ;
+  runMode = 0;
+  stepper->moveTo(_targetStep);
+  enable();
+  trigger( EVT_MOVE );
+  return *this;
+}
+
+Atm_AccelStepper& Atm_AccelStepper::rotate( long  int speed) {
+  runMode = 1;
+  _isHoming = 0 ;
+  stepper->setSpeed( speed);
+  enable();
+  trigger( EVT_MOVE );
+  return *this;
+}
+
+Atm_AccelStepper& Atm_AccelStepper::homing( bool direction ){
+  enable();
+  direction == 1 ? _isHoming = 2 : _isHoming = 1;
+  direction == 1 ? this->trigger(EVT_HOMING_HIGH) : this->trigger(EVT_HOMING_LOW);
+
+  return *this;
+}
+
+// Atm_AccelStepper& Atm_AccelStepper::rotationReversed(bool reversed){
+//   _rotationReversed = reversed ? -1 : 1 ;
+// }
+
+Atm_AccelStepper& Atm_AccelStepper::setEnablePin( int enablePin ){
+  _enablePin = enablePin ;
+  pinMode(_enablePin, OUTPUT);
+  return *this;
+}
+
+Atm_AccelStepper& Atm_AccelStepper::pinReversed( bool directionInvert,
+                              bool stepInvert, bool enableInvert){
+  stepper->setPinsInverted(directionInvert, stepInvert, enableInvert);
+  return *this;
+}
+
+Atm_AccelStepper& Atm_AccelStepper::limitLow_set(int mode,  int pin,  int reversed){
+  _limitLow_Mode = mode ;
+  _limitLow_Pin = pin ;
+  _limitLow_Reversed = reversed ;
+  if (_limitLow_Mode==1) {pinMode(_limitLow_Pin, INPUT_PULLUP);}
+  if (_limitLow_Mode==2) {pinMode(_limitLow_Pin, INPUT);}
+  return *this;
+}
+
+Atm_AccelStepper& Atm_AccelStepper::limitLow_isHard(bool hardlimit){
+  _limitLow_Hard = hardlimit;
+  return *this;
+}
+
+Atm_AccelStepper& Atm_AccelStepper::limitLow_setThresholds (int threshold_low, int threshold_high){
+  _limitLow_Thresholds[0] = threshold_low ;
+  _limitLow_Thresholds[1] = threshold_high ;
+  return *this;
+}
+
+Atm_AccelStepper& Atm_AccelStepper::limitHigh_set(int mode,  int pin,  int reversed){
+  _limitHigh_Mode = mode ;
+  _limitHigh_Pin = pin ;
+  _limitHigh_Reversed = reversed ;
+  if (_limitHigh_Mode==1) {pinMode(_limitHigh_Pin, INPUT_PULLUP);}
+  if (_limitHigh_Mode==2) {pinMode(_limitHigh_Pin, INPUT);}
+  return *this;
+}
+
+Atm_AccelStepper& Atm_AccelStepper::limitHigh_isHard(bool hardlimit){
+  _limitHigh_Hard = hardlimit;
+  return *this;
+}
+
+Atm_AccelStepper& Atm_AccelStepper::limitHigh_setThresholds (int threshold_low, int threshold_high){
+  _limitHigh_Thresholds[0] = threshold_low ;
+  _limitHigh_Thresholds[1] = threshold_high ;
+  return *this;
+}
+
+/* Public event methods
+ *
+ */
+
+Atm_AccelStepper& Atm_AccelStepper::disable() {
+  trigger( EVT_DISABLE );
+  return *this;
+}
+
+Atm_AccelStepper& Atm_AccelStepper::enable() {
+  trigger( EVT_ENABLE );
+  return *this;
+}
+
+// Atm_AccelStepper& Atm_AccelStepper::move() {
+//   trigger( EVT_MOVE );
+//   return *this;
+// }
+
+Atm_AccelStepper& Atm_AccelStepper::stop() {
+  trigger( EVT_STOP );
+  return *this;
+}
+
+Atm_AccelStepper& Atm_AccelStepper::emergency_stop() {
+  trigger( EVT_EMERGENCY_STOP );
+  return *this;
+}
+
+Atm_AccelStepper& Atm_AccelStepper::on_limit_low() {
+  trigger( EVT_ON_LIMIT_LOW );
+  return *this;
+}
+
+Atm_AccelStepper& Atm_AccelStepper::on_limit_high() {
+  trigger( EVT_ON_LIMIT_HIGH );
+  return *this;
+}
+
+Atm_AccelStepper& Atm_AccelStepper::on_target() {
+  trigger( EVT_ON_TARGET );
+  return *this;
+}
+
+
+
+/*
+ * onChangeposition() push connector variants ( slots 1, autostore 0, broadcast 0 )
+ */
+
+Atm_AccelStepper& Atm_AccelStepper::onChangeposition( Machine& machine, int event ) {
+  onPush( connectors, ON_CHANGEPOSITION, 0, 1, 1, machine, event );
+  return *this;
+}
+
+Atm_AccelStepper& Atm_AccelStepper::onChangeposition( atm_cb_push_t callback, int idx ) {
+  onPush( connectors, ON_CHANGEPOSITION, 0, 1, 1, callback, idx );
+  return *this;
+}
+
+/*
+ * onChangestate() push connector variants ( slots 1, autostore 0, broadcast 0 )
+ */
+
+Atm_AccelStepper& Atm_AccelStepper::onChangestate( Machine& machine, int event ) {
+  onPush( connectors, ON_CHANGESTATE, 0, 1, 1, machine, event );
+  return *this;
+}
+
+Atm_AccelStepper& Atm_AccelStepper::onChangestate( atm_cb_push_t callback, int idx ) {
+  onPush( connectors, ON_CHANGESTATE, 0, 1, 1, callback, idx );
+  return *this;
+}
+
+/*
+ * onOnlimithigh() push connector variants ( slots 1, autostore 0, broadcast 0 )
+ */
+
+Atm_AccelStepper& Atm_AccelStepper::onOnlimithigh( Machine& machine, int event ) {
+  onPush( connectors, ON_ONLIMITHIGH, 0, 1, 1, machine, event );
+  return *this;
+}
+
+Atm_AccelStepper& Atm_AccelStepper::onOnlimithigh( atm_cb_push_t callback, int idx ) {
+  onPush( connectors, ON_ONLIMITHIGH, 0, 1, 1, callback, idx );
+  return *this;
+}
+
+/*
+ * onOnlimitlow() push connector variants ( slots 1, autostore 0, broadcast 0 )
+ */
+
+Atm_AccelStepper& Atm_AccelStepper::onOnlimitlow( Machine& machine, int event ) {
+  onPush( connectors, ON_ONLIMITLOW, 0, 1, 1, machine, event );
+  return *this;
+}
+
+Atm_AccelStepper& Atm_AccelStepper::onOnlimitlow( atm_cb_push_t callback, int idx ) {
+  onPush( connectors, ON_ONLIMITLOW, 0, 1, 1, callback, idx );
+  return *this;
+}
+
+/*
+ * onOntarget() push connector variants ( slots 1, autostore 0, broadcast 0 )
+ */
+
+Atm_AccelStepper& Atm_AccelStepper::onOntarget( Machine& machine, int event ) {
+  onPush( connectors, ON_ONTARGET, 0, 1, 1, machine, event );
+  return *this;
+}
+
+Atm_AccelStepper& Atm_AccelStepper::onOntarget( atm_cb_push_t callback, int idx ) {
+  onPush( connectors, ON_ONTARGET, 0, 1, 1, callback, idx );
+  return *this;
+}
+
+/*
+ * onStop() push connector variants ( slots 1, autostore 0, broadcast 0 )
+ */
+
+Atm_AccelStepper& Atm_AccelStepper::onStop( Machine& machine, int event ) {
+  onPush( connectors, ON_STOP, 0, 1, 1, machine, event );
+  return *this;
+}
+
+Atm_AccelStepper& Atm_AccelStepper::onStop( atm_cb_push_t callback, int idx ) {
+  onPush( connectors, ON_STOP, 0, 1, 1, callback, idx );
+  return *this;
+}
+
+Atm_AccelStepper& Atm_AccelStepper::onOnhominglow( Machine& machine, int event ) {
+  onPush( connectors, ON_ONTARGET, 0, 1, 1, machine, event );
+  return *this;
+}
+
+Atm_AccelStepper& Atm_AccelStepper::onOnhominglow( atm_cb_push_t callback, int idx ) {
+  onPush( connectors, ON_ONTARGET, 0, 1, 1, callback, idx );
+  return *this;
+}
+
+Atm_AccelStepper& Atm_AccelStepper::onOnhominghigh( Machine& machine, int event ) {
+  onPush( connectors, ON_ONTARGET, 0, 1, 1, machine, event );
+  return *this;
+}
+
+Atm_AccelStepper& Atm_AccelStepper::onOnhominghigh( atm_cb_push_t callback, int idx ) {
+  onPush( connectors, ON_ONTARGET, 0, 1, 1, callback, idx );
+  return *this;
+}
+
+/* State trace method
+ * Sets the symbol table and the default logging method for serial monitoring
+ */
+
+Atm_AccelStepper& Atm_AccelStepper::trace( Stream & stream ) {
+  Machine::setTrace( &stream, atm_serial_debug::trace,
+    "ACCELSTEPPER\0EVT_DISABLE\0EVT_ENABLE\0EVT_ENABLED_TIMEOUT\0EVT_MOVE\0EVT_STOP\0EVT_EMERGENCY_STOP\0EVT_ON_LIMIT_LOW\0EVT_ON_LIMIT_HIGH\0EVT_ON_TARGET\0EVT_HOMING_LOW\0EVT_HOMING_HIGH\0ELSE\0DISABLED\0ENABLED\0RUNNING\0STOP\0HOMING_LOW\0HOMING_HIGH\0LIMIT_LOW\0LIMIT_HIGH" );
+  return *this;
+}

+ 190 - 0
HTequi-firmware/lib/Atm_lien/Atm_AccelStepper.h

@@ -0,0 +1,190 @@
+#pragma once
+
+#include <Automaton.h>
+#include <AccelStepper.h>
+
+class Atm_AccelStepper: public Machine {
+
+ public:
+  enum { DISABLE, ENABLED, RUNNING, STOP, HOMING_LOW, HOMING_HIGH, LIMIT_LOW, LIMIT_HIGH }; // STATES
+  enum { EVT_DISABLE, EVT_ENABLE, EVT_ENABLED_TIMEOUT, EVT_MOVE, EVT_STOP,
+          EVT_EMERGENCY_STOP, EVT_ON_LIMIT_LOW, EVT_ON_LIMIT_HIGH, EVT_ON_TARGET,
+          EVT_HOMING_LOW, EVT_HOMING_HIGH, ELSE }; // EVENTS
+  Atm_AccelStepper( void ) : Machine() {};
+  Atm_AccelStepper& begin( int step_pin, int dir_pin );
+  Atm_AccelStepper& trace( Stream & stream );
+  Atm_AccelStepper& trigger( int event );
+  int state( void );
+  Atm_AccelStepper& onChangeposition( Machine& machine, int event = 0 );
+  Atm_AccelStepper& onChangeposition( atm_cb_push_t callback, int idx = 0 );
+  Atm_AccelStepper& onChangestate( Machine& machine, int event = 0 );
+  Atm_AccelStepper& onChangestate( atm_cb_push_t callback, int idx = 0 );
+  Atm_AccelStepper& onOnlimithigh( Machine& machine, int event = 0 );
+  Atm_AccelStepper& onOnlimithigh( atm_cb_push_t callback, int idx = 0 );
+  Atm_AccelStepper& onOnlimitlow( Machine& machine, int event = 0 );
+  Atm_AccelStepper& onOnlimitlow( atm_cb_push_t callback, int idx = 0 );
+  Atm_AccelStepper& onOntarget( Machine& machine, int event = 0 );
+  Atm_AccelStepper& onOntarget( atm_cb_push_t callback, int idx = 0 );
+  Atm_AccelStepper& onStop( Machine& machine, int event = 0 );
+  Atm_AccelStepper& onStop( atm_cb_push_t callback, int idx = 0 );
+  Atm_AccelStepper& onOnhominglow( Machine& machine, int event = 0 );
+  Atm_AccelStepper& onOnhominglow( atm_cb_push_t callback, int idx = 0 );
+  Atm_AccelStepper& onOnhominghigh( Machine& machine, int event = 0 );
+  Atm_AccelStepper& onOnhominghigh( atm_cb_push_t callback, int idx = 0 );
+  Atm_AccelStepper& disable( void );
+  Atm_AccelStepper& enable( void );
+  //Atm_AccelStepper& move( void );
+  Atm_AccelStepper& stop( void );
+  Atm_AccelStepper& emergency_stop( void );
+  Atm_AccelStepper& on_limit_low( void );
+  Atm_AccelStepper& on_limit_high( void );
+  Atm_AccelStepper& on_target( void );
+
+  AccelStepper *stepper;
+
+  long int homing_speed = 100;
+  long int max_speed = 5000;
+  long int acceleration = 1000;
+  Atm_AccelStepper& setMaxSpeed( long int maxSpeed = 5000);
+  Atm_AccelStepper& setHomingSpeed( long int homingSpeed = 400);
+  Atm_AccelStepper& setAcceleration( long int acc = 1000);
+
+  Atm_AccelStepper& move(long int stepRel );
+  Atm_AccelStepper& moveTo(long int stepAbs );
+  Atm_AccelStepper& rotate(long int speed );
+  Atm_AccelStepper& homing( bool direction );
+  int runMode = 0; // 0 uses run() for positioning, 1 uses runSpeed() for constant speed
+  Atm_AccelStepper& position_refresh( long int refresh_ms = 1000);
+
+  // Atm_AccelStepper& rotationReversed(bool reversed);
+
+  Atm_AccelStepper& setEnablePin( int enablePin );
+  Atm_AccelStepper& pinReversed( bool directionInvert=false,  bool stepInvert=false, bool enableInvert=false );
+  bool enabled ;
+
+
+  Atm_AccelStepper& limitLow_set(int mode = 0, int pin = -1, int reversed=0);
+  Atm_AccelStepper& limitLow_isHard(bool hardlimit = 1);
+  Atm_AccelStepper& limitLow_setThresholds (int threshold_low=510, int threshold_high = 1024);
+  Atm_AccelStepper& limitHigh_set(int mode = 0, int pin = -1, int reversed=0);
+  Atm_AccelStepper& limitHigh_isHard(bool hardlimit = 1);
+  Atm_AccelStepper& limitHigh_setThresholds (int threshold_low=510, int threshold_high = 1024);
+  bool limitLow_State;
+  bool limitHigh_State;
+
+
+
+ private:
+  enum { ENT_DISABLED, ENT_ENABLED,ENT_RUNNING, LP_RUNNING, ENT_STOP, LP_STOP, ENT_HOMING_LOW, LP_HOMING_LOW, EXT_HOMING_LOW, ENT_HOMING_HIGH, LP_HOMING_HIGH, EXT_HOMING_HIGH, ENT_LIMIT_LOW, ENT_LIMIT_HIGH }; // ACTIONS
+  enum { ON_CHANGEPOSITION, ON_CHANGESTATE, ON_ONLIMITHIGH, ON_ONLIMITLOW, ON_ONTARGET, ON_STOP, CONN_MAX }; // CONNECTORS
+  atm_connector connectors[CONN_MAX];
+  int event( int id );
+  void action( int id );
+
+  void stepper_update(void);
+
+  long int _currentStep = 0;
+  long int _targetStep = 0;
+  long int _maxStep ;
+  atm_timer_millis position_timer ;
+  int POSITION_SEND_TIMER = 50 ;
+
+  // bool _rotationReversed = 0 ;
+
+  int _enablePin = -1;
+  bool _enableReversed = 0 ;
+  atm_timer_millis idle_timer ;
+  int IDLE_TIMEOUT_DURATION = 500000 ;
+
+  int _limitLow_Pin;
+  int _limitLow_Mode = 0; //0 no limit, 1 digital, 2 analog with thresholds
+  bool _limitLow_Reversed ; //invert logic of limit switches
+  int _limitLow_Thresholds[2] ; //analog value  range for two analog limits
+  bool _limitLow_Hard = 1;
+  int _limitHigh_Pin;
+  int _limitHigh_Mode; //0 no limit, 1 digital, 2 analog with thresholds
+  bool _limitHigh_Reversed ; //invert logic of limit switches
+  int _limitHigh_Thresholds[2] ; //analog value  range for two analog limits
+  bool _limitHigh_Hard = 1 ;
+  int _isHoming = 0 ;
+  void updateLimitSwitch();
+
+};
+
+/*
+Automaton::ATML::begin - Automaton Markup Language
+
+<?xml version="1.0" encoding="UTF-8"?>
+<machines>
+  <machine name="Atm_AccelStepper">
+    <states>
+      <DISABLED index="0" on_enter="ENT_DISABLED">
+        <EVT_ENABLE>ENABLED</EVT_ENABLE>
+      </DISABLED>
+      <ENABLED index="1" on_enter="ENT_ENABLED">
+        <EVT_DISABLE>DISABLED</EVT_DISABLE>
+        <EVT_ENABLED_TIMEOUT>DISABLED</EVT_ENABLED_TIMEOUT>
+        <EVT_STOP>STOP</EVT_STOP>
+        <EVT_EMERGENCY_STOP>STOP</EVT_EMERGENCY_STOP>
+      </ENABLED>
+      <RUNNING index="2" on_loop="LP_RUNNING">
+        <EVT_DISABLE>DISABLED</EVT_DISABLE>
+        <EVT_STOP>STOP</EVT_STOP>
+        <EVT_EMERGENCY_STOP>STOP</EVT_EMERGENCY_STOP>
+        <EVT_ON_LIMIT_LOW>LIMIT_LOW</EVT_ON_LIMIT_LOW>
+        <EVT_ON_LIMIT_HIGH>LIMIT_HIGH</EVT_ON_LIMIT_HIGH>
+        <EVT_ON_TARGET>ENABLED</EVT_ON_TARGET>
+      </RUNNING>
+      <STOP index="3">
+        <EVT_DISABLE>DISABLED</EVT_DISABLE>
+        <EVT_STOP>STOP</EVT_STOP>
+        <EVT_EMERGENCY_STOP>STOP</EVT_EMERGENCY_STOP>
+      </STOP>
+      <HOMING_LOW index="4" on_enter="ENT_HOMING_LOW" on_exit="EXT_HOMING_LOW">
+        <EVT_DISABLE>DISABLED</EVT_DISABLE>
+        <EVT_STOP>STOP</EVT_STOP>
+        <EVT_EMERGENCY_STOP>STOP</EVT_EMERGENCY_STOP>
+        <EVT_ON_LIMIT_LOW>ENABLED</EVT_ON_LIMIT_LOW>
+      </HOMING_LOW>
+      <HOMING_HIGH index="5" on_enter="ENT_HOMING_HIGH" on_exit="EXT_HOMING_HIGH">
+        <EVT_DISABLE>DISABLED</EVT_DISABLE>
+        <EVT_STOP>STOP</EVT_STOP>
+        <EVT_EMERGENCY_STOP>STOP</EVT_EMERGENCY_STOP>
+        <EVT_ON_LIMIT_HIGH>ENABLED</EVT_ON_LIMIT_HIGH>
+      </HOMING_HIGH>
+      <LIMIT_LOW index="6" on_enter="ENT_LIMIT_LOW">
+        <EVT_STOP>STOP</EVT_STOP>
+        <EVT_EMERGENCY_STOP>STOP</EVT_EMERGENCY_STOP>
+        <EVT_ON_LIMIT_LOW>LIMIT_LOW</EVT_ON_LIMIT_LOW>
+      </LIMIT_LOW>
+      <LIMIT_HIGH index="7" on_enter="ENT_LIMIT_HIGH">
+        <EVT_STOP>STOP</EVT_STOP>
+        <EVT_EMERGENCY_STOP>STOP</EVT_EMERGENCY_STOP>
+        <EVT_ON_LIMIT_HIGH>LIMIT_HIGH</EVT_ON_LIMIT_HIGH>
+      </LIMIT_HIGH>
+    </states>
+    <events>
+      <EVT_DISABLE index="0" access="MIXED"/>
+      <EVT_ENABLE index="1" access="MIXED"/>
+      <EVT_ENABLED_TIMEOUT index="2" access="PRIVATE"/>
+      <EVT_STOP index="3" access="MIXED"/>
+      <EVT_EMERGENCY_STOP index="4" access="MIXED"/>
+      <EVT_ON_LIMIT_LOW index="5" access="MIXED"/>
+      <EVT_ON_LIMIT_HIGH index="6" access="MIXED"/>
+      <EVT_ON_TARGET index="7" access="MIXED"/>
+    </events>
+    <connectors>
+      <CHANGEPOSITION autostore="0" broadcast="0" dir="PUSH" slots="1"/>
+      <CHANGESTATE autostore="0" broadcast="0" dir="PUSH" slots="1"/>
+      <ONLIMITHIGH autostore="0" broadcast="0" dir="PUSH" slots="1"/>
+      <ONLIMITLOW autostore="0" broadcast="0" dir="PUSH" slots="1"/>
+      <ONTARGET autostore="0" broadcast="0" dir="PUSH" slots="1"/>
+      <STOP autostore="0" broadcast="0" dir="PUSH" slots="1"/>
+    </connectors>
+    <methods>
+    </methods>
+  </machine>
+</machines>
+
+Automaton::ATML::end
+*/

+ 0 - 412
HTequi-firmware/lib/Atm_lien/Atm_Teenstep.cpp

@@ -1,412 +0,0 @@
-#include "Atm_Teenstep.h"
-
-/* Add optional parameters for the state machine to begin()
- * Add extra initialization code
- */
-
-Atm_Teenstep& Atm_Teenstep::begin(Stepper & motorRef, StepControl & stepControlRef) {
-  // clang-format off
-  const static state_t state_table[] PROGMEM = {
-    /*                             ON_ENTER     ON_LOOP          ON_EXIT  EVT_HOMING_LOW  EVT_HOMING_HIGH  EVT_MOVE_TIMEOUT  EVT_LIMIT_HIGH   EVT_LIMIT_LOW  EVT_EMERGENCYSTOP  EVT_STOP  EVT_ONTARGET  EVT_MOVE  EVT_DISABLE  EVT_ENABLE  ELSE */
-    /*       DISABLED */       ENT_DISABLED,         -1,              -1,     HOMING_LOW,     HOMING_HIGH,               -1,             -1,             -1,                -1,       -1,           -1,       -1,          -1,    ENABLED,   -1,
-    /*        ENABLED */        ENT_ENABLED,         -1,              -1,     HOMING_LOW,     HOMING_HIGH,               -1,             -1,             -1,                -1,       -1,           -1,  RUNNING,    DISABLED,         -1,   -1,
-    /*        RUNNING */        ENT_RUNNING, LP_RUNNING,              -1,     HOMING_LOW,     HOMING_HIGH,               -1,             -1,             -1,    EMERGENCY_STOP, STOPPING,      ENABLED,  RUNNING,          -1,         -1,   -1,
-    /*       STOPPING */       ENT_STOPPING,         -1,              -1,             -1,              -1,               -1, EMERGENCY_STOP, EMERGENCY_STOP,    EMERGENCY_STOP,       -1,           -1,  RUNNING,          -1,         -1,   -1,
-    /* EMERGENCY_STOP */ ENT_EMERGENCY_STOP,         -1,              -1,             -1,              -1,               -1,             -1,             -1,                -1,       -1,           -1,  RUNNING,          -1,    ENABLED,   -1,
-    /*    HOMING_HIGH */    ENT_HOMING_HIGH,         -1, EXT_HOMING_HIGH,             -1,              -1,               -1,        ENABLED,             -1,    EMERGENCY_STOP, STOPPING,           -1,       -1,          -1,         -1,   -1,
-    /*     HOMING_LOW */     ENT_HOMING_LOW,         -1,  EXT_HOMING_LOW,             -1,              -1,               -1,            -1,        ENABLED,    EMERGENCY_STOP, STOPPING,           -1,       -1,          -1,         -1,   -1,
-  };
-  // clang-format on
-  Machine::begin( state_table, ELSE );
-  this-> motor =   &motorRef;
-  this-> controller = &stepControlRef;
-  return *this;
-}
-
-/* Add C++ code for each internally handled event (input)
- * The code must return 1 to trigger the event
- */
-
-int Atm_Teenstep::event( int id ) {
-  updateLimitSwitch();
-  switch ( id ) {
-    case EVT_HOMING_LOW:
-      return 0;
-    case EVT_HOMING_HIGH:
-      return 0;
-    case EVT_MOVE_TIMEOUT:
-      return 0;
-    case EVT_LIMIT_HIGH:
-      return limitState[0];
-    case EVT_LIMIT_LOW:
-      return limitState[1];
-    case EVT_EMERGENCYSTOP:
-      return 0;
-    case EVT_STOP:
-      return 0;
-    case EVT_ONTARGET:
-      return _currentStep == _targetStep;
-    case EVT_MOVE:
-      return 0;
-    case EVT_DISABLE:
-      return 0;
-    case EVT_ENABLE:
-      return 0;
-  }
-  return 0;
-}
-
-/* Add C++ code for each action
- * This generates the 'output' for the state machine
- *
- * Available connectors:
- *   push( connectors, ON_CHANGE, 0, <v>, <up> );
- *   push( connectors, ON_CHANGEPOSITION, 0, <v>, <up> );
- *   push( connectors, ON_LIMITHIGH, 0, <v>, <up> );
- *   push( connectors, ON_LIMITLOW, 0, <v>, <up> );
- */
-
-void Atm_Teenstep::action( int id ) {
-  switch ( id ) {
-    long int tempStep ;
-    case ENT_DISABLED:
-      push(connectors, ON_CHANGE, 0,  state(), 0);
-      enabled = _enableReversed ? HIGH : LOW;
-      digitalWrite(_enablePin, enabled);
-      sendOSC();
-      return;
-    case ENT_ENABLED:
-      push(connectors, ON_CHANGE, 0,  state(), 0);
-      enabled = _enableReversed ? LOW : HIGH ;
-      digitalWrite(_enablePin, enabled);
-      sendOSC();
-      return;
-    case ENT_RUNNING:
-      push(connectors, ON_CHANGE, 0,  state(), 0);
-      push(connectors, ON_CHANGE, 0,  2, 0);
-      return;
-    case LP_RUNNING:
-      tempStep = motor->getPosition();
-      if (tempStep != _currentStep){
-        _currentStep =  tempStep;
-        push(connectors, ON_CHANGEPOSITION, 0,  _currentStep, 0);
-      }
-
-      //updateLimitSwitch();
-      sendOSC();
-      return;
-    case ENT_STOPPING:
-      push(connectors, ON_CHANGE, 0,  state(), 0);
-      sendOSC();
-      return;
-    case ENT_EMERGENCY_STOP:
-      push(connectors, ON_CHANGE, 0,  state(), 0);
-      controller->emergencyStop();
-      trigger(EVT_ENABLE);
-      sendOSC();
-      return;
-    case ENT_HOMING_HIGH:
-      push(connectors, ON_CHANGE, 0,  state(), 0);
-      if(_limitType) {
-        motor->setTargetRel(2147483647);
-        controller->moveAsync(*motor);
-        }
-      sendOSC();
-      return;
-    case EXT_HOMING_HIGH:
-      controller->emergencyStop();
-      if(last_trigger == EVT_LIMIT_HIGH){
-        _maxStep = motor->getPosition();
-        push(connectors, ON_LIMITHIGH, 0,  _maxStep, 0);
-        Serial.print("Stepper maxPos ");
-        Serial.println(motor->getPosition());
-        trigger(EVT_ENABLE);
-      }
-      else{
-        push(connectors, ON_LIMITLOW, 0,  1, 0);
-        Serial.println("homing failed ");
-        trigger(EVT_EMERGENCYSTOP);
-      }
-
-      return;
-    case ENT_HOMING_LOW:
-      push(connectors, ON_CHANGE, 0,  state(), 0);
-      if(_limitType) {
-        motor->setTargetRel(-2147483647);
-        controller->moveAsync(*motor);
-        }
-      sendOSC();
-      return;
-    case EXT_HOMING_LOW:
-      controller->emergencyStop();
-      if(last_trigger == EVT_LIMIT_LOW){
-        push(connectors, ON_LIMITLOW, 0,  1, 0);
-        motor->setPosition(0);
-        Serial.print("Stepper homed ");
-        Serial.println(motor->getPosition());
-        trigger(EVT_ENABLE);
-      }
-      else{
-        push(connectors, ON_LIMITHIGH, 0,  1, 0);
-        Serial.println("homing failed ");
-        trigger(EVT_EMERGENCYSTOP);
-      }
-
-
-      return;
-  }
-}
-
-/* Optionally override the default trigger() method
- * Control how your machine processes triggers
- */
-
-Atm_Teenstep& Atm_Teenstep::trigger( int event ) {
-  Machine::trigger( event );
-  return *this;
-}
-
-/* Optionally override the default state() method
- * Control what the machine returns when another process requests its state
- */
-
-int Atm_Teenstep::state( void ) {
-  return Machine::state();
-}
-
-/* CUSTOM METHODS
-********************************************************************************************************
-*/
-
-// Atm_TeensyStep& Atm_TeensyStep::enable( bool enable ){
-//
-//   return *this;
-// }
-
-/////////   ENABLE/DISABLE    ////////
-
-Atm_Teenstep& Atm_Teenstep::setEnablePin( int enablePin ){
-  _enablePin = enablePin ;
-  pinMode(_enablePin, OUTPUT);
-  return *this;
-}
-
-Atm_Teenstep& Atm_Teenstep::enableReversed( bool reverse ){
-  _enableReversed = reverse ;
-  return *this;
-}
-
-/////////   LIMITS    ////////
-
-Atm_Teenstep& Atm_Teenstep::setLimitPins( int limitPinLow){
-  _limitPin[0] = limitPinLow;
-  pinMode(_limitPin[0], INPUT);
-  return *this;
-}
-
-Atm_Teenstep& Atm_Teenstep::setLimitPins( int limitPinLow, int limitPinHigh){
- _limitPin[0] = limitPinLow;
- _limitPin[1] = limitPinHigh;
- pinMode(_limitPin[0], INPUT);
- pinMode(_limitPin[1], INPUT);
- return *this;
-}
-
-Atm_Teenstep& Atm_Teenstep::setLimitType( int limitType){
- _limitType = limitType;
- return *this;
-}
-
-Atm_Teenstep& Atm_Teenstep::limitReversed( bool reversed){
- _limitReversed = reversed;
- return *this;
-}
-
-Atm_Teenstep& Atm_Teenstep::limitThresholds( int limitThreshold0,
-       int limitThreshold1,  int limitThreshold2, int limitThreshold3){
- _limitThresholds[0] = limitThreshold0;
- _limitThresholds[1] = limitThreshold1;
- _limitThresholds[2] = limitThreshold2;
- _limitThresholds[3] = limitThreshold3;
- return *this;
-}
-
-void Atm_Teenstep::updateLimitSwitch(){
-  switch (_limitType) { // limitType!=0 means there is limit to check
-    case NONE:
-      return ;
-    case DIGITAL_1:
-      limitState[0] = digitalRead(_limitPin[0]);
-      limitState[0] = _limitReversed ? !limitState[0] : limitState[0];
-      return;
-    case DIGITAL_2:
-    limitState[0] = digitalRead(_limitPin[0]);
-    limitState[1] = digitalRead(_limitPin[1]);
-    limitState[0] = _limitReversed ? !limitState[0] : limitState[0];
-    limitState[1] = _limitReversed ? !limitState[1] : limitState[1];
-      return;
-    case ANALOG_1:
-    int read = analogRead(_limitPin[0]) ;
-    limitState[0] = _limitThresholds[0] < read && read < _limitThresholds[1] ;
-    limitState[1] = _limitThresholds[2] < read && read < _limitThresholds[3] ;
-      return;
-  }
-
-}
-
-
-Atm_Teenstep& Atm_Teenstep::onOSC( void ){
-//  _enableReversed = reverse ;
-  return *this;
-}
-
-Atm_Teenstep& Atm_Teenstep::sendOSC( void ){
-//  _enableReversed = reverse ;
-  return *this;
-}
-
-/* Nothing customizable below this line
- ************************************************************************************************
-*/
-
-/* Public event methods
- *
- */
- Atm_Teenstep& Atm_Teenstep::homing_low() {
-   trigger( EVT_HOMING_LOW );
-   return *this;
- }
-
- Atm_Teenstep& Atm_Teenstep::homing_high() {
-   trigger( EVT_HOMING_HIGH );
-   return *this;
- }
-
-Atm_Teenstep& Atm_Teenstep::move_timeout() {
-  trigger( EVT_MOVE_TIMEOUT );
-  return *this;
-}
-
-Atm_Teenstep& Atm_Teenstep::limit_high() {
-  trigger( EVT_LIMIT_HIGH );
-  return *this;
-}
-
-Atm_Teenstep& Atm_Teenstep::limit_low() {
-  trigger( EVT_LIMIT_LOW );
-  return *this;
-}
-
-Atm_Teenstep& Atm_Teenstep::emergencystop() {
-  trigger( EVT_EMERGENCYSTOP );
-  return *this;
-}
-
-Atm_Teenstep& Atm_Teenstep::stop() {
-  trigger( EVT_STOP );
-  return *this;
-}
-
-Atm_Teenstep& Atm_Teenstep::ontarget() {
-  trigger( EVT_ONTARGET );
-  return *this;
-}
-
-Atm_Teenstep& Atm_Teenstep::move(long int stepRel) {
-  _targetStep   = _currentStep + stepRel;
-  //Serial.println(_targetStep);
-  motor->setTargetAbs(_targetStep);
-  controller->moveAsync(*motor);
-  enable();
-  trigger( EVT_MOVE );
-  return *this;
-}
-
-Atm_Teenstep& Atm_Teenstep::moveTo(long int stepAbs) {
-  _targetStep   = stepAbs;
-  motor->setTargetAbs(_targetStep);
-  controller->moveAsync(*motor);
-  enable();
-  trigger( EVT_MOVE );
-  return *this;
-}
-
-Atm_Teenstep& Atm_Teenstep::disable() {
-  trigger( EVT_DISABLE );
-  return *this;
-}
-
-Atm_Teenstep& Atm_Teenstep::enable() {
-  trigger( EVT_ENABLE );
-}
-
-Atm_Teenstep& Atm_Teenstep::homing(bool direction) {
-  direction ? trigger( EVT_HOMING_HIGH ) : trigger(EVT_HOMING_LOW);
-  return *this;
-}
-
-/*
- * onChange() push connector variants ( slots 1, autostore 0, broadcast 0 )
- */
-
-Atm_Teenstep& Atm_Teenstep::onChange( Machine& machine, int event ) {
-  onPush( connectors, ON_CHANGE, 0, 1, 1, machine, event );
-  return *this;
-}
-
-Atm_Teenstep& Atm_Teenstep::onChange( atm_cb_push_t callback, int idx ) {
-  onPush( connectors, ON_CHANGE, 0, 1, 1, callback, idx );
-  return *this;
-}
-
-/*
- * onChangeposition() push connector variants ( slots 1, autostore 0, broadcast 0 )
- */
-
-Atm_Teenstep& Atm_Teenstep::onChangeposition( Machine& machine, int event ) {
-  onPush( connectors, ON_CHANGEPOSITION, 0, 1, 1, machine, event );
-  return *this;
-}
-
-Atm_Teenstep& Atm_Teenstep::onChangeposition( atm_cb_push_t callback, int idx ) {
-  onPush( connectors, ON_CHANGEPOSITION, 0, 1, 1, callback, idx );
-  return *this;
-}
-
-/*
- * onLimithigh() push connector variants ( slots 1, autostore 0, broadcast 0 )
- */
-
-Atm_Teenstep& Atm_Teenstep::onLimithigh( Machine& machine, int event ) {
-  onPush( connectors, ON_LIMITHIGH, 0, 1, 1, machine, event );
-  return *this;
-}
-
-Atm_Teenstep& Atm_Teenstep::onLimithigh( atm_cb_push_t callback, int idx ) {
-  onPush( connectors, ON_LIMITHIGH, 0, 1, 1, callback, idx );
-  return *this;
-}
-
-/*
- * onLimitlow() push connector variants ( slots 1, autostore 0, broadcast 0 )
- */
-
-Atm_Teenstep& Atm_Teenstep::onLimitlow( Machine& machine, int event ) {
-  onPush( connectors, ON_LIMITLOW, 0, 1, 1, machine, event );
-  return *this;
-}
-
-Atm_Teenstep& Atm_Teenstep::onLimitlow( atm_cb_push_t callback, int idx ) {
-  onPush( connectors, ON_LIMITLOW, 0, 1, 1, callback, idx );
-  return *this;
-}
-
-/* State trace method
- * Sets the symbol table and the default logging method for serial monitoring
- */
-
- Atm_Teenstep& Atm_Teenstep::trace( Stream & stream ) {
-   Machine::setTrace( &stream, atm_serial_debug::trace,
-     "TEENSTEP\0EVT_HOMING_LOW\0EVT_HOMING_HIGH\0EVT_MOVE_TIMEOUT\0EVT_LIMIT_HIGH\0EVT_LIMIT_LOW\0EVT_EMERGENCYSTOP\0EVT_STOP\0EVT_ONTARGET\0EVT_MOVE\0EVT_DISABLE\0EVT_ENABLE\0ELSE\0DISABLED\0ENABLED\0RUNNING\0STOPPING\0EMERGENCY_STOP\0HOMING_HIGH\0HOMING_LOW" );
-   return *this;
- }

+ 0 - 157
HTequi-firmware/lib/Atm_lien/Atm_Teenstep.h

@@ -1,157 +0,0 @@
-#pragma once
-
-#include <Automaton.h>
-#include <TeensyStep.h>
-
-class Atm_Teenstep: public Machine {
-
- public:
-   enum { DISABLED, ENABLED, RUNNING, STOPPING, EMERGENCY_STOP, HOMING_HIGH, HOMING_LOW }; // STATES
-   enum { EVT_HOMING_LOW, EVT_HOMING_HIGH, EVT_MOVE_TIMEOUT, EVT_LIMIT_HIGH, EVT_LIMIT_LOW, EVT_EMERGENCYSTOP, EVT_STOP, EVT_ONTARGET, EVT_MOVE, EVT_DISABLE, EVT_ENABLE, ELSE }; // EVENTS
-  Atm_Teenstep( void ) : Machine() {};
-  Atm_Teenstep& begin( Stepper & motorRef, StepControl & stepControlRef );
-  Atm_Teenstep& trace( Stream & stream );
-  Atm_Teenstep& trigger( int event );
-  int state( void );
-  Atm_Teenstep& onChange( Machine& machine, int event = 0 );
-  Atm_Teenstep& onChange( atm_cb_push_t callback, int idx = 0 );
-  Atm_Teenstep& onChangeposition( Machine& machine, int event = 0 );
-  Atm_Teenstep& onChangeposition( atm_cb_push_t callback, int idx = 0 );
-  Atm_Teenstep& onLimithigh( Machine& machine, int event = 0 );
-  Atm_Teenstep& onLimithigh( atm_cb_push_t callback, int idx = 0 );
-  Atm_Teenstep& onLimitlow( Machine& machine, int event = 0 );
-  Atm_Teenstep& onLimitlow( atm_cb_push_t callback, int idx = 0 );
-  Atm_Teenstep& homing_low( void );
-  Atm_Teenstep& homing_high( void );
-  Atm_Teenstep& move_timeout( void );
-  Atm_Teenstep& limit_high( void );
-  Atm_Teenstep& limit_low( void );
-  Atm_Teenstep& emergencystop( void );
-  Atm_Teenstep& stop( void );
-  Atm_Teenstep& ontarget( void );
-  Atm_Teenstep& move( void );
-  Atm_Teenstep& disable( void );
-  Atm_Teenstep& enable( void );
-
-
-  Atm_Teenstep& move( long int stepRel );
-  Atm_Teenstep& moveTo( long int stepAbs );
-  Atm_Teenstep& homing( bool direction );
-
-  Atm_Teenstep& setLimitType( int limitType = 0);
-  Atm_Teenstep& setLimitPins( int limitPinLow);
-  Atm_Teenstep& setLimitPins( int limitPinLow, int limitPinHigh);
-  Atm_Teenstep& limitReversed( bool reversed );
-  Atm_Teenstep& limitThresholds( int limitThreshold0, int limitThreshold1,
-                                  int limitThreshold2, int limitThreshold3);
-  bool limitState[2] ; // up to two limits, at least one for homing
-
-  virtual Atm_Teenstep& onOSC( void );
-  virtual Atm_Teenstep& sendOSC( void );
-
-  Stepper * motor;
-  StepControl * controller;
-
-  Atm_Teenstep& setEnablePin( int enablePin );
-  Atm_Teenstep& enableReversed( bool reverse );
-  bool enabled ;
-
-
-
- private:
-  enum { ENT_DISABLED, ENT_ENABLED, ENT_RUNNING, LP_RUNNING, ENT_STOPPING, ENT_EMERGENCY_STOP, ENT_HOMING_HIGH, EXT_HOMING_HIGH, ENT_HOMING_LOW, EXT_HOMING_LOW }; // ACTIONS
-  enum { ON_CHANGE, ON_CHANGEPOSITION, ON_LIMITHIGH, ON_LIMITLOW, CONN_MAX }; // CONNECTORS
-  atm_connector connectors[CONN_MAX];
-  int event( int id );
-  void action( int id );
-
-  atm_timer_millis idle_timer ;
-  int IDLE_TIMEOUT_DURATION = 500000 ;
-  atm_timer_millis moving_timer ;
-  int MOVING_TIMEOUT_DURATION = 12000000 ;
-  atm_counter homing_counterout ;
-
-  long int _currentStep = 0;
-  long int _targetStep = 0;
-  long int _maxStep ;
-
-  int _enablePin = -1;
-  bool _enableReversed = 0 ;
-
-  int _limitPin[2];
-  enum { NONE, DIGITAL_1, DIGITAL_2, ANALOG_1 };
-  int _limitType; //type of limit switch, from list over
-  bool _limitReversed ; //invert logic of limit switches
-  int _limitThresholds[4] ; //analog value  range for two analog limits
-  void updateLimitSwitch();
-};
-
-/*
-Automaton::ATML::begin - Automaton Markup Language
-
-<?xml version="1.0" encoding="UTF-8"?>
-<machines>
-  <machine name="Atm_Teenstep">
-    <states>
-      <DISABLED index="0" on_enter="ENT_DISABLED">
-        <EVT_ENABLE>ENABLED</EVT_ENABLE>
-      </DISABLED>
-      <ENABLED index="1" on_enter="ENT_ENABLED">
-        <EVT_MOVE>RUNNING</EVT_MOVE>
-        <EVT_DISABLE>DISABLED</EVT_DISABLE>
-      </ENABLED>
-      <RUNNING index="2" on_enter="ENT_RUNNING" on_loop="LP_RUNNING">
-        <EVT_LIMIT_HIGH>EMERGENCY_STOP</EVT_LIMIT_HIGH>
-        <EVT_LIMIT_LOW>EMERGENCY_STOP</EVT_LIMIT_LOW>
-        <EVT_EMERGENCYSTOP>EMERGENCY_STOP</EVT_EMERGENCYSTOP>
-        <EVT_STOP>STOPPING</EVT_STOP>
-        <EVT_ONTARGET>ENABLED</EVT_ONTARGET>
-        <EVT_MOVE>RUNNING</EVT_MOVE>
-      </RUNNING>
-      <STOPPING index="3" on_enter="ENT_STOPPING">
-        <EVT_LIMIT_HIGH>EMERGENCY_STOP</EVT_LIMIT_HIGH>
-        <EVT_LIMIT_LOW>EMERGENCY_STOP</EVT_LIMIT_LOW>
-        <EVT_EMERGENCYSTOP>EMERGENCY_STOP</EVT_EMERGENCYSTOP>
-        <EVT_MOVE>RUNNING</EVT_MOVE>
-      </STOPPING>
-      <EMERGENCY_STOP index="4" on_enter="ENT_EMERGENCY_STOP">
-        <EVT_MOVE>RUNNING</EVT_MOVE>
-        <EVT_ENABLE>ENABLED</EVT_ENABLE>
-      </EMERGENCY_STOP>
-      <HOMING_HIGH index="5" on_enter="ENT_HOMING_HIGH" on_exit="EXT_HOMING_HIGH">
-        <EVT_LIMIT_HIGH>ENABLED</EVT_LIMIT_HIGH>
-        <EVT_LIMIT_LOW>EMERGENCY_STOP</EVT_LIMIT_LOW>
-        <EVT_EMERGENCYSTOP>EMERGENCY_STOP</EVT_EMERGENCYSTOP>
-        <EVT_STOP>STOPPING</EVT_STOP>
-      </HOMING_HIGH>
-      <HOMING_LOW index="6" on_enter="ENT_HOMING_LOW" on_exit="EXT_HOMING_LOW">
-        <EVT_LIMIT_HIGH>EMERGENCY_STOP</EVT_LIMIT_HIGH>
-        <EVT_LIMIT_LOW>ENABLED</EVT_LIMIT_LOW>
-        <EVT_EMERGENCYSTOP>EMERGENCY_STOP</EVT_EMERGENCYSTOP>
-        <EVT_STOP>STOPPING</EVT_STOP>
-      </HOMING_LOW>
-    </states>
-    <events>
-      <EVT_MOVE_TIMEOUT index="0" access="MIXED"/>
-      <EVT_LIMIT_HIGH index="1" access="MIXED"/>
-      <EVT_LIMIT_LOW index="2" access="MIXED"/>
-      <EVT_EMERGENCYSTOP index="3" access="MIXED"/>
-      <EVT_STOP index="4" access="MIXED"/>
-      <EVT_ONTARGET index="5" access="MIXED"/>
-      <EVT_MOVE index="6" access="MIXED"/>
-      <EVT_DISABLE index="7" access="MIXED"/>
-      <EVT_ENABLE index="8" access="MIXED"/>
-    </events>
-    <connectors>
-      <CHANGE autostore="0" broadcast="0" dir="PUSH" slots="1"/>
-      <CHANGEPOSITION autostore="0" broadcast="0" dir="PUSH" slots="1"/>
-      <LIMITHIGH autostore="0" broadcast="0" dir="PUSH" slots="1"/>
-      <LIMITLOW autostore="0" broadcast="0" dir="PUSH" slots="1"/>
-    </connectors>
-    <methods>
-    </methods>
-  </machine>
-</machines>
-
-Automaton::ATML::end
-*/

+ 0 - 36
HTequi-firmware/lib/Atm_lien/Atm_Teenstep_OSC.cpp

@@ -1,36 +0,0 @@
-#include "Atm_Teenstep_OSC.h"
-
-Atm_Teenstep_OSC& Atm_Teenstep_OSC::begin(Atm_Teenstep& stepperMachineRef, EthernetUDP& udpRef, OSCBundle& bndl, const char* address) {
-  this-> stepperMachine = &stepperMachineRef;
-  _adress = address;
-  this->_udpRef =   &udpRef;
-  this->_bndl = &bndl ;
-  //stepperMachine->onChangeposition( sendPosition, 0);
-  return *this;
-}
-
-
-
-Atm_Teenstep_OSC& Atm_Teenstep_OSC::onOSC(OSCMessage& msg ){
-  Serial.println("OSC");
-  int patternOffset = msg.match(_adress) ;
-  if(patternOffset){
-    if(msg.fullMatch("/speedAcc", patternOffset)){
-      stepperMachine->motor->setMaxSpeed(msg.getInt(0));
-      stepperMachine->motor->setAcceleration(msg.getInt(1));
-    }
-    if(msg.fullMatch("/enable", patternOffset)){msg.getInt(0) ? stepperMachine->trigger(stepperMachine->EVT_ENABLE) : stepperMachine->trigger(stepperMachine->EVT_DISABLE);}
-    //if(msg.fullMatch("/home", patternOffset)){home(msg.getInt(0));}
-    if(msg.fullMatch("/move", patternOffset)){stepperMachine->move(msg.getInt(0));Serial.println(msg.getInt(0));}
-    if(msg.fullMatch("/moveTo", patternOffset)){stepperMachine->moveTo(msg.getInt(0));}
-    if(msg.fullMatch("/stop", patternOffset)){stepperMachine->stop();}
-    if(msg.fullMatch("/emergencyStop", patternOffset)){stepperMachine->emergencystop();}
-    if(msg.fullMatch("/home", patternOffset)){stepperMachine->homing(msg.getInt(0));}
-
-    return *this;
-  }
-}
-
-void Atm_Teenstep_OSC::sendPosition(int idx, int v, int up) {
-  _bndl->add(_adress).add(v);
-}

+ 0 - 31
HTequi-firmware/lib/Atm_lien/Atm_Teenstep_OSC.h

@@ -1,31 +0,0 @@
-#pragma once
-
-#include "Atm_Teenstep.h"
-
-#include <EthernetUdp.h>
-#include <OSCMessage.h>
-#include <OSCBundle.h>
-
-class Atm_Teenstep_OSC {
-
-  public:
-    Atm_Teenstep_OSC( void ){};
-    Atm_Teenstep_OSC& begin(Atm_Teenstep & stepperMachineRef,
-                              EthernetUDP& udpRef,
-                               OSCBundle& bndl,
-                               const char* address);
-    Atm_Teenstep* stepperMachine ;
-    const char* _adress = "/OSC";
-    EthernetUDP* _udpRef ;
-    Atm_Teenstep_OSC& onOSC(OSCMessage& msg );
-    Atm_Teenstep_OSC& sendOSC( void );
-
-    void sendPosition(int idx, int v, int up);
-
-  private:
-   // AccelStepper _motor;
-   // Stepper *_motor;       // STEP pin: 2, DIR pin: 3
-   // StepControl _controller;
-   OSCBundle* _bndl;
-
-};

+ 0 - 268
HTequi-firmware/lib/Atm_lien/Atm_TeensyStep.cpp

@@ -1,268 +0,0 @@
-#include "Atm_TeensyStep.h"
-
-
-/* Add optional parameters for the state machine to begin()
- * Add extra initialization code
- */
-
-Atm_TeensyStep& Atm_TeensyStep::begin(Stepper & motorRef, StepControl & stepControlRef,
-                  EthernetUDP& udpRef, OSCBundle& bndl, const char* address ) {
-  // clang-format off
-  const static state_t state_table[] PROGMEM = {
-    /*                 ON_ENTER     ON_LOOP       ON_EXIT  EVT_IDLE_TIMER  EVT_ON_TARGET  EVT_GOTO  ELSE */
-    /*     IDLE */     ENT_IDLE,         -1,     EXT_IDLE,             -1,            -1,  RUNNING,   -1,
-    /*    READY */    ENT_READY,         -1,           -1,           IDLE,            -1,  RUNNING,   -1,
-    /*  RUNNING */  ENT_RUNNING, LP_RUNNING,           -1,             -1,      STOPPING,  RUNNING,   -1,
-    /* STOPPING */ ENT_STOPPING,         -1, EXT_STOPPING,             -1,         READY,       -1,   -1,
-  };
-  // clang-format on
-  Machine::begin( state_table, ELSE );
-  this-> motor =   &motorRef;
-  this-> controller = &stepControlRef;
-  _adress = address;
-  this->_udpRef =   &udpRef;
-  this->_bndl = &bndl ;
-  return *this;
-}
-
-/* Add C++ code for each internally handled event (input)
- * The code must return 1 to trigger the event
- */
-
-int Atm_TeensyStep::event( int id ) {
-  switch ( id ) {
-    case EVT_IDLE_TIMER:
-      return 0;
-    case EVT_ON_TARGET:
-      //motor->get(targetStep);
-      return _currentStep == _targetStep ;
-      // return _motor.distanceToGo()== 0 ;
-  }
-  return 0;
-}
-
-/* Add C++ code for each action
- * This generates the 'output' for the state machine
- *
- * Available connectors:
- *   push( connectors, ON_ONCHANGE, 0, <v>, <up> );
- */
-
-void Atm_TeensyStep::action( int id ) {
-  String strg = _adress;
-  switch ( id ) {
-    case ENT_IDLE:
-      enable(0);
-      return;
-    case EXT_IDLE:
-      enable(1);
-      return;
-    case ENT_READY:
-
-      return;
-    case ENT_RUNNING:
-
-      // _controller.moveAsync(*_motor);
-      return;
-    case LP_RUNNING:
-      // _motor.run();
-      _currentStep = motor->getPosition();
-
-      strg += "/step";
-      _bndl->add(strg.c_str()).add(_currentStep);
-      updateLimitSwitch();
-      if(limitState[0] || limitState[1]) {emergencyStop();}
-     // Serial.println(_motor.currentPosition());
-      return;
-    case ENT_STOPPING:
-      return;
-    case EXT_STOPPING:
-      if(_isHoming && limitState[0]){motor->setPosition(0);}
-      if(_isHoming && limitState[1]){maxStep = motor->getPosition();;}
-      _isHoming=0;
-      return;
-  }
-}
-
-/* Optionally override the default trigger() method
- * Control how your machine processes triggers
- */
-
-Atm_TeensyStep& Atm_TeensyStep::trigger( int event ) {
-  Machine::trigger( event );
-  return *this;
-}
-
-Atm_TeensyStep& Atm_TeensyStep::onOSC(OSCMessage& msg ){
-  Serial.println("OSC");
-  int patternOffset = msg.match(_adress) ;
-  if(patternOffset){
-    if(msg.fullMatch("/speedAcc", patternOffset)){
-      motor->setMaxSpeed(msg.getInt(0));
-      motor->setAcceleration(msg.getInt(1));
-    }
-    if(msg.fullMatch("/enable", patternOffset)){enable(msg.getInt(0));}
-    if(msg.fullMatch("/home", patternOffset)){home(msg.getInt(0));}
-    if(msg.fullMatch("/move", patternOffset)){move(msg.getInt(0));}
-    if(msg.fullMatch("/moveTo", patternOffset)){moveTo(msg.getInt(0));}
-    if(msg.fullMatch("/stop", patternOffset)){stop();}
-    if(msg.fullMatch("/emergencyStop", patternOffset)){emergencyStop();}
-
-    return *this;
-  }
-}
-
-/* Optionally override the default state() method
- * Control what the machine returns when another process requests its state
- */
-
-int Atm_TeensyStep::state( void ) {
-  return Machine::state();
-}
-
-void Atm_TeensyStep::updateLimitSwitch(){
-  switch (_limitType) { // limitType!=0 means there is limit to check
-    case NONE:
-      return ;
-    case DIGITAL_1:
-      limitState[0] = digitalRead(_limitPin[0]);
-      limitState[0] = _limitReversed ? !limitState[0] : limitState[0];
-      return;
-    case DIGITAL_2:
-    limitState[0] = digitalRead(_limitPin[0]);
-    limitState[1] = digitalRead(_limitPin[1]);
-    limitState[0] = _limitReversed ? !limitState[0] : limitState[0];
-    limitState[1] = _limitReversed ? !limitState[1] : limitState[1];
-      return;
-    case ANALOG_1:
-    int read = analogRead(_limitPin[0]) ;
-    limitState[0] = _limitThresholds[0] < read && read < _limitThresholds[1] ;
-    limitState[1] = _limitThresholds[2] < read && read < _limitThresholds[3] ;
-      return;
-  }
-
-}
-
-/* Nothing customizable below this line
- ************************************************************************************************
-*/
-
-/* Public event methods
- *
- */
- Atm_TeensyStep& Atm_TeensyStep::setLimitPins( int limitPinLow){
-   _limitPin[0] = limitPinLow;
-   pinMode(_limitPin[0], INPUT);
-   return *this;
- }
-
-Atm_TeensyStep& Atm_TeensyStep::setLimitPins( int limitPinLow, int limitPinHigh){
-  _limitPin[0] = limitPinLow;
-  _limitPin[1] = limitPinHigh;
-  pinMode(_limitPin[0], INPUT);
-  pinMode(_limitPin[1], INPUT);
-  return *this;
-}
-
-Atm_TeensyStep& Atm_TeensyStep::setLimitType( int limitType){
-  _limitType = limitType;
-  return *this;
-}
-
-Atm_TeensyStep& Atm_TeensyStep::limitReversed( bool reversed){
-  _limitReversed = reversed;
-  return *this;
-}
-
-Atm_TeensyStep& Atm_TeensyStep::limitThresholds( int limitThreshold0,
-        int limitThreshold1,  int limitThreshold2, int limitThreshold3){
-  _limitThresholds[0] = limitThreshold0;
-  _limitThresholds[1] = limitThreshold1;
-  _limitThresholds[2] = limitThreshold2;
-  _limitThresholds[3] = limitThreshold3;
-  return *this;
-}
-
-Atm_TeensyStep& Atm_TeensyStep::enable( bool enable ){
-  enabled = enable ;
-  enabled = _enableReversed ? !enabled : enabled ;
-  digitalWrite(_enablePin, enabled);
-  return *this;
-}
-
-Atm_TeensyStep& Atm_TeensyStep::setEnablePin( int enablePin ){
-  _enablePin = enablePin ;
-  pinMode(_enablePin, OUTPUT);
-  return *this;
-}
-
-Atm_TeensyStep& Atm_TeensyStep::enableReversed( bool reverse ){
-  _enableReversed = reverse ;
-  return *this;
-}
-
-Atm_TeensyStep& Atm_TeensyStep::home( bool limit ){
-  _isHoming = 1 ;
-  limit ? move(-2147483647) : move(2147483647) ; // move to very far away until hit
-  return *this;
-}
-
-Atm_TeensyStep& Atm_TeensyStep::move(long int stepRel ) {
-  _targetStep   += stepRel;
-  motor->setTargetAbs(_targetStep);
-  controller->moveAsync(*motor);
-
-  // _motor.move(targetStep);
-  // _motor.run();
-  // Serial.println(_motor.distanceToGo());
-  // _motor.setMaxSpeed(5000);
-  // _motor.setAcceleration(6000);
-  trigger( EVT_GOTO );
-  return *this;
-}
-
-Atm_TeensyStep& Atm_TeensyStep::moveTo(long int stepAbs ) {
-  _targetStep   = stepAbs;
-  motor->setTargetAbs(_targetStep);
-  controller->moveAsync(*motor);
-  trigger( EVT_GOTO );
-  return *this;
-}
-
-Atm_TeensyStep& Atm_TeensyStep::stop(){
-  controller->stopAsync();
-  //trigger( EVT_STOPPING );
-  return *this;
-}
-
-Atm_TeensyStep& Atm_TeensyStep::emergencyStop(){
-  controller->emergencyStop();
-  //trigger( EVT_STOPPING );
-  return *this;
-}
-
-//Atm_TeensyStep& Atm_TeensyStep::onOSC(OSCMessage& msg)
-
-/*
- * onOnchange() push connector variants ( slots 1, autostore 0, broadcast 0 )
- */
-
-Atm_TeensyStep& Atm_TeensyStep::onOnchange( Machine& machine, int event ) {
-  onPush( connectors, ON_ONCHANGE, 0, 1, 1, machine, event );
-  return *this;
-}
-
-Atm_TeensyStep& Atm_TeensyStep::onOnchange( atm_cb_push_t callback, int idx ) {
-  onPush( connectors, ON_ONCHANGE, 0, 1, 1, callback, idx );
-  return *this;
-}
-
-/* State trace method
- * Sets the symbol table and the default logging method for serial monitoring
- */
-
-Atm_TeensyStep& Atm_TeensyStep::trace( Stream & stream ) {
-  Machine::setTrace( &stream, atm_serial_debug::trace,
-    "STEPPER\0EVT_IDLE_TIMER\0EVT_ON_TARGET\0EVT_GOTO\0ELSE\0IDLE\0READY\0RUNNING\0STOPPING" );
-  return *this;
-}

+ 0 - 120
HTequi-firmware/lib/Atm_lien/Atm_TeensyStep.h

@@ -1,120 +0,0 @@
-#pragma once
-
-#include <Automaton.h>
-#include <TeensyStep.h>
-
-#include <EthernetUdp.h>
-#include <OSCMessage.h>
-#include <OSCBundle.h>
-// #include <AccelStepper.h>
-class Atm_TeensyStep: public Machine {
-
- public:
-  enum { IDLE, READY, RUNNING, STOPPING }; // STATES
-  enum { EVT_IDLE_TIMER, EVT_ON_TARGET, EVT_GOTO, ELSE }; // EVENTS
-  Atm_TeensyStep( void ) : Machine()  {};
-  Atm_TeensyStep& begin( Stepper & motorRef, StepControl & stepControlRef,
-                    EthernetUDP& udpRef, OSCBundle& bndl, const char* address ) ;
-
-  Atm_TeensyStep& trace( Stream & stream );
-  Atm_TeensyStep& trigger( int event );
-  int state( void );
-  Atm_TeensyStep& onOnchange( Machine& machine, int event = 0 );
-  Atm_TeensyStep& onOnchange( atm_cb_push_t callback, int idx = 0 );
-  int enable_timeout; //in idle mode, how long before sleep (stepper disabled
-  Atm_TeensyStep& setLimitType( int limitType = 0);
-  Atm_TeensyStep& setLimitPins( int limitPinLow);
-  Atm_TeensyStep& setLimitPins( int limitPinLow, int limitPinHigh);
-  Atm_TeensyStep& limitReversed( bool reversed );
-  Atm_TeensyStep& limitThresholds( int limitThreshold0, int limitThreshold1,
-                                  int limitThreshold2, int limitThreshold3);
-  Atm_TeensyStep& setEnablePin( int enablePin );
-  Atm_TeensyStep& enableReversed( bool reverse );
-  bool enabled ;
-  Atm_TeensyStep& enable( bool enable );
-  Atm_TeensyStep& home( bool limit ); // 0(default) for home, 1 for max value
-  Atm_TeensyStep& move( long int stepRel );
-  Atm_TeensyStep& moveTo( long int stepAbs );
-  Atm_TeensyStep& stop();
-  Atm_TeensyStep& emergencyStop();
-  Stepper * motor;
-  StepControl * controller;
-
-  bool limitState[2] ; // up to two limits, at least one for homing
-  bool homed; //stepper has been homed
-  long int maxStep ;
-
-  const char* _adress = "/OSC";
-  EthernetUDP* _udpRef ;
-  Atm_TeensyStep& onOSC(OSCMessage& msg );
-
- private:
-  // AccelStepper _motor;
-  // Stepper *_motor;       // STEP pin: 2, DIR pin: 3
-  // StepControl _controller;
-  OSCBundle* _bndl;
-
-
-
-  enum { ENT_IDLE, EXT_IDLE, ENT_READY, ENT_RUNNING, LP_RUNNING,
-          ENT_STOPPING, EXT_STOPPING }; // ACTIONS
-  enum { ON_ONCHANGE, CONN_MAX }; // CONNECTORS
-  atm_connector connectors[CONN_MAX];
-  int event( int id );
-  void action( int id );
-
-  long int _currentStep ;
-  long int _targetStep ;
-
-  int _enablePin;
-  bool _enableReversed ;
-
-
-
-
-  int _limitPin[2];
-  enum { NONE, DIGITAL_1, DIGITAL_2, ANALOG_1 };
-  int _limitType; //type of limit switch, from list over
-  bool _limitReversed ; //invert logic of limit switches
-  int _limitThresholds[4] ; //analog value  range for two analog limits
-  void updateLimitSwitch();
-  bool _isHoming;
-};
-
-/*
-Automaton::ATML::begin - Automaton Markup Language
-
-<?xml version="1.0" encoding="UTF-8"?>
-<machines>
-  <machine name="Atm_stepper">
-    <states>
-      <IDLE index="0" on_enter="ENT_IDLE">
-        <EVT_GOTO>RUNNING</EVT_GOTO>
-      </IDLE>
-      <READY index="1" on_enter="ENT_READY">
-        <EVT_IDLE_TIMER>IDLE</EVT_IDLE_TIMER>
-        <EVT_GOTO>RUNNING</EVT_GOTO>
-      </READY>
-      <RUNNING index="2" on_enter="ENT_RUNNING" on_loop="LP_RUNNING">
-        <EVT_ON_TARGET>STOPPING</EVT_ON_TARGET>
-        <EVT_GOTO>RUNNING</EVT_GOTO>
-      </RUNNING>
-      <STOPPING index="3" on_enter="ENT_STOPPING" on_exit="EXT_STOPPING">
-        <EVT_ON_TARGET>READY</EVT_ON_TARGET>
-      </STOPPING>
-    </states>
-    <events>
-      <EVT_IDLE_TIMER index="0" access="PRIVATE"/>
-      <EVT_ON_TARGET index="1" access="PRIVATE"/>
-      <EVT_GOTO index="2" access="PUBLIC"/>
-    </events>
-    <connectors>
-      <ONCHANGE autostore="0" broadcast="0" dir="PUSH" slots="1"/>
-    </connectors>
-    <methods>
-    </methods>
-  </machine>
-</machines>
-
-Automaton::ATML::end
-*/

+ 0 - 519
HTequi-firmware/lib/Atm_lien/Atm_Tstepper.cpp

@@ -1,519 +0,0 @@
-#include "Atm_Tstepper.h"
-
-/* Add optional parameters for the state machine to begin()
- * Add extra initialization code
- */
-
-Atm_Tstepper& Atm_Tstepper::begin(Stepper & motorRef, StepControl & stepControlRef) {
-  // clang-format off
-  const static state_t state_table[] PROGMEM = {
-    /*                              ON_ENTER      ON_LOOP     ON_EXIT  EVT_HOMING_COUNTER  EVT_IDLE_TIMEOUT  EVT_MOVE_TIMEOUT  EVT_LIMIT_LOW  EVT_LIMIT_HIGH  EVT_EMERGENCY  EVT_STOP  EVT_ONTARGET  EVT_MOVE  EVT_DISABLE  EVT_ENABLE  EVT_HOMING  ELSE */
-    /*          DISABLED */     ENT_DISABLED,          -1,         -1,                 -1,               -1,               -1,            -1,             -1,            -1,       -1,           -1,       -1,          -1,    ENABLED,        -1,    -1,
-    /*           ENABLED */      ENT_ENABLED,          -1,         -1,                 -1,         DISABLED,               -1,            -1,             -1,            -1,       -1,           -1,  RUNNING,    DISABLED,         -1,    HOMING,    -1,
-    /*           RUNNING */      ENT_RUNNING,  LP_RUNNING,         -1,                 -1,               -1,     MOVE_TIMEOUT,     EMERGENCY,      EMERGENCY,     EMERGENCY, STOPPING,      ENABLED,  RUNNING,          -1,         -1,        -1,    -1,
-    /*          STOPPING */     ENT_STOPPING, LP_STOPPING,         -1,                 -1,               -1,               -1,     EMERGENCY,      EMERGENCY,     EMERGENCY,       -1,           -1,  RUNNING,          -1,         -1,        -1,    -1,
-    /*         EMERGENCY */    ENT_EMERGENCY,          -1,         -1,                 -1,               -1,               -1,            -1,             -1,            -1,       -1,           -1,       -1,          -1,         -1,        -1,    -1,
-    /*            HOMING */       ENT_HOMING,   LP_HOMING, EXT_HOMING,                 -1,               -1,     MOVE_TIMEOUT,     EMERGENCY,      EMERGENCY,     EMERGENCY, STOPPING, MOVE_TIMEOUT,       -1,          -1,         -1,        -1,    -1,
-    /*      MOVE_TIMEOUT */ ENT_MOVE_TIMEOUT,          -1,         -1,                 -1,               -1,               -1,     EMERGENCY,             -1,     EMERGENCY,       -1,           -1,       -1,          -1,         -1,        -1,    -1,
-    /* HOMING_COUNTEROUT */               -1,          -1,         -1,                 -1,               -1,               -1,            -1,             -1,            -1,       -1,           -1,       -1,          -1,         -1,        -1,    -1
-  };
-  // clang-format on
-  Machine::begin( state_table, ELSE );
-  this-> motor =   &motorRef;
-  this-> controller = &stepControlRef;
-  idle_timer.set(ATM_TIMER_OFF);
-  moving_timer.set(ATM_TIMER_OFF);
-  return *this;
-}
-
-/* Add C++ code for each internally handled event (input)
- * The code must return 1 to trigger the event
- */
-
-int Atm_Tstepper::event( int id ) {
-  switch ( id ) {
-    case EVT_HOMING_COUNTER:
-      return 0;
-    case EVT_IDLE_TIMEOUT:
-      return 0; //idle_timer.expired(this);
-    case EVT_MOVE_TIMEOUT:
-      return 0; //moving_timer.expired(this);
-    case EVT_LIMIT_LOW:
-      return limitState[0];//limitState[0];
-    case EVT_LIMIT_HIGH:
-      return limitState[1];///limitState[1];
-    case EVT_EMERGENCY:
-      return 0;
-    case EVT_STOP:
-      return 0;
-    case EVT_ONTARGET: //triggers if stepper reached _targetStep
-      return _currentStep == _targetStep ;
-    case EVT_MOVE:
-      return 0;
-    case EVT_DISABLE:
-      return 0;
-    case EVT_ENABLE:
-      return 0;
-    case EVT_HOMING:
-      return 0;
-  }
-  return 0;
-}
-
-/* Add C++ code for each action
- * This generates the 'output' for the state machine
- *
- * Available connectors:
- *   push( connectors, ON_ONCHANGE, 0, <v>, <up> );
- *   push( connectors, ON_ONCHANGEACCELERATION, 0, <v>, <up> );
- *   push( connectors, ON_ONCHANGEPOSITION, 0, <v>, <up> );
- *   push( connectors, ON_ONCHANGESTATE, 0, <v>, <up> );
- *   push( connectors, ON_ONHIGHLIMIT, 0, <v>, <up> );
- *   push( connectors, ON_ONHOMINGCOUNTEROUT, 0, <v>, <up> );
- *   push( connectors, ON_ONHOMINGTIMEOUT, 0, <v>, <up> );
- *   push( connectors, ON_ONIDLETIMEOUT, 0, <v>, <up> );
- *   push( connectors, ON_ONLOWLIMIT, 0, <v>, <up> );
- *   push( connectors, ON_ONMOVETIMEOUT, 0, <v>, <up> );
- *   push( connectors, ON_SPEED, 0, <v>, <up> );
- */
-
-void Atm_Tstepper::action( int id ) {
-  switch ( id ) {
-    long int tempStep ;
-    case ENT_DISABLED:
-      idle_timer.set(ATM_TIMER_OFF);
-      moving_timer.set(ATM_TIMER_OFF);
-      enabled = _enableReversed ? HIGH : LOW;
-      digitalWrite(_enablePin, enabled);
-      sendOSC();
-      return;
-    case ENT_ENABLED:
-      // idle_timer.set(IDLE_TIMEOUT_DURATION);
-      // moving_timer.set(ATM_TIMER_OFF);
-      enabled = _enableReversed ? LOW : HIGH ;
-      digitalWrite(_enablePin, enabled);
-      sendOSC();
-      return;
-    case ENT_RUNNING:
-      // idle_timer.set(ATM_TIMER_OFF);
-      // moving_timer.set(MOVING_TIMEOUT_DURATION);
-      return;
-    case LP_RUNNING:
-      tempStep = motor->getPosition();
-      connectors[ON_ONCHANGEPOSITION].push(_currentStep);
-      // if (tempStep != _currentStep){push( connectors, ON_ONCHANGEPOSITION, 0, _currentStep, 0 );} ;
-      _currentStep =  tempStep;
-      updateLimitSwitch();
-      sendOSC();
-      return;
-    case ENT_STOPPING:
-      idle_timer.set(ATM_TIMER_OFF);
-      moving_timer.set(ATM_TIMER_OFF);
-      sendOSC();
-      return;
-    case LP_STOPPING:
-      return;
-    case ENT_EMERGENCY:
-      idle_timer.set(ATM_TIMER_OFF);
-      moving_timer.set(ATM_TIMER_OFF);
-      controller->emergencyStop();
-      sendOSC();
-      return;
-    case ENT_HOMING:
-      // idle_timer.set(ATM_TIMER_OFF);
-      // moving_timer.set(MOVING_TIMEOUT_DURATION);
-      _limitType ? move(-2147483647) : trigger(EVT_STOP) ;
-      sendOSC();
-      return;
-    case LP_HOMING:
-      updateLimitSwitch();
-      sendOSC();
-      return;
-    case EXT_HOMING:
-      switch(next_trigger){
-        case EVT_LIMIT_LOW:
-          Serial.println("LOW");
-          move(10);
-          return;
-        case EVT_LIMIT_HIGH:
-          Serial.println("HIGH");
-          move(-10);
-          return;
-      }
-
-      return;
-    case ENT_MOVE_TIMEOUT:
-      idle_timer.set(ATM_TIMER_OFF);
-      moving_timer.set(ATM_TIMER_OFF);
-      emergency();
-      return;
-  }
-}
-
-/* Optionally override the default trigger() method
- * Control how your machine processes triggers
- */
-
-Atm_Tstepper& Atm_Tstepper::trigger( int event ) {
-  Machine::trigger( event );
-  return *this;
-}
-
-/* Optionally override the default state() method
- * Control what the machine returns when another process requests its state
- */
-
-int Atm_Tstepper::state( void ) {
-  return Machine::state();
-}
-
-/* CUSTOM METHODS
-********************************************************************************************************
-*/
-
-// Atm_TeensyStep& Atm_TeensyStep::enable( bool enable ){
-//
-//   return *this;
-// }
-
-/////////   ENABLE/DISABLE    ////////
-
-Atm_Tstepper& Atm_Tstepper::setEnablePin( int enablePin ){
-  _enablePin = enablePin ;
-  pinMode(_enablePin, OUTPUT);
-  return *this;
-}
-
-Atm_Tstepper& Atm_Tstepper::enableReversed( bool reverse ){
-  _enableReversed = reverse ;
-  return *this;
-}
-
-/////////   LIMITS    ////////
-
-Atm_Tstepper& Atm_Tstepper::setLimitPins( int limitPinLow){
-  _limitPin[0] = limitPinLow;
-  pinMode(_limitPin[0], INPUT);
-  return *this;
-}
-
-Atm_Tstepper& Atm_Tstepper::setLimitPins( int limitPinLow, int limitPinHigh){
- _limitPin[0] = limitPinLow;
- _limitPin[1] = limitPinHigh;
- pinMode(_limitPin[0], INPUT);
- pinMode(_limitPin[1], INPUT);
- return *this;
-}
-
-Atm_Tstepper& Atm_Tstepper::setLimitType( int limitType){
- _limitType = limitType;
- return *this;
-}
-
-Atm_Tstepper& Atm_Tstepper::limitReversed( bool reversed){
- _limitReversed = reversed;
- return *this;
-}
-
-Atm_Tstepper& Atm_Tstepper::limitThresholds( int limitThreshold0,
-       int limitThreshold1,  int limitThreshold2, int limitThreshold3){
- _limitThresholds[0] = limitThreshold0;
- _limitThresholds[1] = limitThreshold1;
- _limitThresholds[2] = limitThreshold2;
- _limitThresholds[3] = limitThreshold3;
- return *this;
-}
-
-void Atm_Tstepper::updateLimitSwitch(){
-  switch (_limitType) { // limitType!=0 means there is limit to check
-    case NONE:
-      return ;
-    case DIGITAL_1:
-      limitState[0] = digitalRead(_limitPin[0]);
-      limitState[0] = _limitReversed ? !limitState[0] : limitState[0];
-      return;
-    case DIGITAL_2:
-    limitState[0] = digitalRead(_limitPin[0]);
-    limitState[1] = digitalRead(_limitPin[1]);
-    limitState[0] = _limitReversed ? !limitState[0] : limitState[0];
-    limitState[1] = _limitReversed ? !limitState[1] : limitState[1];
-      return;
-    case ANALOG_1:
-    int read = analogRead(_limitPin[0]) ;
-    limitState[0] = _limitThresholds[0] < read && read < _limitThresholds[1] ;
-    limitState[1] = _limitThresholds[2] < read && read < _limitThresholds[3] ;
-      return;
-  }
-
-}
-
-int Atm_Tstepper::printPosition(){Serial.println(_currentStep);return 0;}
-
-Atm_Tstepper& Atm_Tstepper::onOSC( void ){
-//  _enableReversed = reverse ;
-  return *this;
-}
-
-Atm_Tstepper& Atm_Tstepper::sendOSC( void ){
-//  _enableReversed = reverse ;
-  return *this;
-}
-
-
-// Atm_Tstepper& Atm_Tstepper::move(long int stepRel ) {
-//
-//   // _motor.move(targetStep);
-//   // _motor.run();
-//   // Serial.println(_motor.distanceToGo());
-//   // _motor.setMaxSpeed(5000);
-//   // _motor.setAcceleration(6000);
-//   trigger( EVT_GOTO );
-//   return *this;
-// }
-
-/* Nothing customizable below this line
- ************************************************************************************************
-*/
-
-/* Public event methods
- *
- */
-
-Atm_Tstepper& Atm_Tstepper::homing_counter() {
-  trigger( EVT_HOMING_COUNTER );
-  return *this;
-}
-
-Atm_Tstepper& Atm_Tstepper::idle_timeout() {
-  trigger( EVT_IDLE_TIMEOUT );
-  return *this;
-}
-
-Atm_Tstepper& Atm_Tstepper::move_timeout() {
-  trigger( EVT_MOVE_TIMEOUT );
-  return *this;
-}
-
-Atm_Tstepper& Atm_Tstepper::limit_low() {
-  trigger( EVT_LIMIT_LOW );
-  return *this;
-}
-
-Atm_Tstepper& Atm_Tstepper::limit_high() {
-  trigger( EVT_LIMIT_HIGH );
-  return *this;
-}
-
-Atm_Tstepper& Atm_Tstepper::emergency() {
-  controller->emergencyStop();
-  trigger( EVT_EMERGENCY );
-  return *this;
-}
-
-Atm_Tstepper& Atm_Tstepper::stop() {
-  controller->stop();
-  trigger( EVT_STOP );
-  return *this;
-}
-
-Atm_Tstepper& Atm_Tstepper::ontarget() {
-  trigger( EVT_ONTARGET );
-  return *this;
-}
-
-Atm_Tstepper& Atm_Tstepper::move(long int stepRel) {
-  _targetStep   += stepRel;
-  motor->setTargetAbs(_targetStep);
-  controller->moveAsync(*motor);
-  enable();
-  trigger( EVT_MOVE );
-  return *this;
-}
-
-Atm_Tstepper& Atm_Tstepper::moveTo(long int stepAbs) {
-  _targetStep   = stepAbs;
-  motor->setTargetAbs(_targetStep);
-  controller->moveAsync(*motor);
-  enable();
-  trigger( EVT_MOVE );
-  return *this;
-}
-
-Atm_Tstepper& Atm_Tstepper::disable() {
-  trigger( EVT_DISABLE );
-  return *this;
-}
-
-Atm_Tstepper& Atm_Tstepper::enable() {
-  trigger( EVT_ENABLE );
-  return *this;
-}
-
-Atm_Tstepper& Atm_Tstepper::homing(int direction) {
-  trigger( EVT_HOMING );
-  return *this;
-}
-
-/*
- * onOnchange() push connector variants ( slots 1, autostore 0, broadcast 0 )
- */
-
-Atm_Tstepper& Atm_Tstepper::onOnchange( Machine& machine, int event ) {
-  onPush( connectors, ON_ONCHANGE, 0, 1, 1, machine, event );
-  return *this;
-}
-
-Atm_Tstepper& Atm_Tstepper::onOnchange( atm_cb_push_t callback, int idx ) {
-  onPush( connectors, ON_ONCHANGE, 0, 1, 1, callback, idx );
-  return *this;
-}
-
-/*
- * onOnchangeacceleration() push connector variants ( slots 1, autostore 0, broadcast 0 )
- */
-
-Atm_Tstepper& Atm_Tstepper::onOnchangeacceleration( Machine& machine, int event ) {
-  onPush( connectors, ON_ONCHANGEACCELERATION, 0, 1, 1, machine, event );
-  return *this;
-}
-
-Atm_Tstepper& Atm_Tstepper::onOnchangeacceleration( atm_cb_push_t callback, int idx ) {
-  onPush( connectors, ON_ONCHANGEACCELERATION, 0, 1, 1, callback, idx );
-  return *this;
-}
-
-/*
- * onOnchangeposition() push connector variants ( slots 1, autostore 0, broadcast 0 )
- */
-
-Atm_Tstepper& Atm_Tstepper::onOnchangeposition( Machine& machine, int event ) {
-  onPush( connectors, ON_ONCHANGEPOSITION, 0, 1, 1, machine, event );
-  return *this;
-}
-
-Atm_Tstepper& Atm_Tstepper::onOnchangeposition( atm_cb_push_t callback, int idx ) {
-  onPush( connectors, ON_ONCHANGEPOSITION, 0, 1, 1, callback, idx );
-  return *this;
-}
-
-/*
- * onOnchangestate() push connector variants ( slots 1, autostore 0, broadcast 0 )
- */
-
-Atm_Tstepper& Atm_Tstepper::onOnchangestate( Machine& machine, int event ) {
-  onPush( connectors, ON_ONCHANGESTATE, 0, 1, 1, machine, event );
-  return *this;
-}
-
-Atm_Tstepper& Atm_Tstepper::onOnchangestate( atm_cb_push_t callback, int idx ) {
-  onPush( connectors, ON_ONCHANGESTATE, 0, 1, 1, callback, idx );
-  return *this;
-}
-
-/*
- * onOnhighlimit() push connector variants ( slots 1, autostore 0, broadcast 0 )
- */
-
-Atm_Tstepper& Atm_Tstepper::onOnhighlimit( Machine& machine, int event ) {
-  onPush( connectors, ON_ONHIGHLIMIT, 0, 1, 1, machine, event );
-  return *this;
-}
-
-Atm_Tstepper& Atm_Tstepper::onOnhighlimit( atm_cb_push_t callback, int idx ) {
-  onPush( connectors, ON_ONHIGHLIMIT, 0, 1, 1, callback, idx );
-  return *this;
-}
-
-/*
- * onOnhomingcounterout() push connector variants ( slots 1, autostore 0, broadcast 0 )
- */
-
-Atm_Tstepper& Atm_Tstepper::onOnhomingcounterout( Machine& machine, int event ) {
-  onPush( connectors, ON_ONHOMINGCOUNTEROUT, 0, 1, 1, machine, event );
-  return *this;
-}
-
-Atm_Tstepper& Atm_Tstepper::onOnhomingcounterout( atm_cb_push_t callback, int idx ) {
-  onPush( connectors, ON_ONHOMINGCOUNTEROUT, 0, 1, 1, callback, idx );
-  return *this;
-}
-
-/*
- * onOnhomingtimeout() push connector variants ( slots 1, autostore 0, broadcast 0 )
- */
-
-Atm_Tstepper& Atm_Tstepper::onOnhomingtimeout( Machine& machine, int event ) {
-  onPush( connectors, ON_ONHOMINGTIMEOUT, 0, 1, 1, machine, event );
-  return *this;
-}
-
-Atm_Tstepper& Atm_Tstepper::onOnhomingtimeout( atm_cb_push_t callback, int idx ) {
-  onPush( connectors, ON_ONHOMINGTIMEOUT, 0, 1, 1, callback, idx );
-  return *this;
-}
-
-/*
- * onOnidletimeout() push connector variants ( slots 1, autostore 0, broadcast 0 )
- */
-
-Atm_Tstepper& Atm_Tstepper::onOnidletimeout( Machine& machine, int event ) {
-  onPush( connectors, ON_ONIDLETIMEOUT, 0, 1, 1, machine, event );
-  return *this;
-}
-
-Atm_Tstepper& Atm_Tstepper::onOnidletimeout( atm_cb_push_t callback, int idx ) {
-  onPush( connectors, ON_ONIDLETIMEOUT, 0, 1, 1, callback, idx );
-  return *this;
-}
-
-/*
- * onOnlowlimit() push connector variants ( slots 1, autostore 0, broadcast 0 )
- */
-
-Atm_Tstepper& Atm_Tstepper::onOnlowlimit( Machine& machine, int event ) {
-  onPush( connectors, ON_ONLOWLIMIT, 0, 1, 1, machine, event );
-  return *this;
-}
-
-Atm_Tstepper& Atm_Tstepper::onOnlowlimit( atm_cb_push_t callback, int idx ) {
-  onPush( connectors, ON_ONLOWLIMIT, 0, 1, 1, callback, idx );
-  return *this;
-}
-
-/*
- * onOnmovetimeout() push connector variants ( slots 1, autostore 0, broadcast 0 )
- */
-
-Atm_Tstepper& Atm_Tstepper::onOnmovetimeout( Machine& machine, int event ) {
-  onPush( connectors, ON_ONMOVETIMEOUT, 0, 1, 1, machine, event );
-  return *this;
-}
-
-Atm_Tstepper& Atm_Tstepper::onOnmovetimeout( atm_cb_push_t callback, int idx ) {
-  onPush( connectors, ON_ONMOVETIMEOUT, 0, 1, 1, callback, idx );
-  return *this;
-}
-
-/*
- * onSpeed() push connector variants ( slots 1, autostore 0, broadcast 0 )
- */
-
-Atm_Tstepper& Atm_Tstepper::onSpeed( Machine& machine, int event ) {
-  onPush( connectors, ON_SPEED, 0, 1, 1, machine, event );
-  return *this;
-}
-
-Atm_Tstepper& Atm_Tstepper::onSpeed( atm_cb_push_t callback, int idx ) {
-  onPush( connectors, ON_SPEED, 0, 1, 1, callback, idx );
-  return *this;
-}
-
-/* State trace method
- * Sets the symbol table and the default logging method for serial monitoring
- */
-
-Atm_Tstepper& Atm_Tstepper::trace( Stream & stream ) {
-  Machine::setTrace( &stream, atm_serial_debug::trace,
-    "TSTEPPER\0EVT_HOMING_COUNTER\0EVT_IDLE_TIMEOUT\0EVT_MOVE_TIMEOUT\0EVT_LIMIT_LOW\0EVT_LIMIT_HIGH\0EVT_EMERGENCY\0EVT_STOP\0EVT_ONTARGET\0EVT_MOVE\0EVT_DISABLE\0EVT_ENABLE\0ELSE\0DISABLED\0ENABLED\0RUNNING\0STOPPING\0EMERGENCY\0HOMING\0MOVE_TIMEOUT\0HOMING_COUNTEROUT" );
-  return *this;
-}

+ 0 - 184
HTequi-firmware/lib/Atm_lien/Atm_Tstepper.h

@@ -1,184 +0,0 @@
-#pragma once
-
-#include <Automaton.h>
-//#include <Atm_timer.h>
-
-#include <TeensyStep.h>
-
-class Atm_Tstepper: public Machine {
-
- public:
-  enum { DISABLED, ENABLED, RUNNING, STOPPING, EMERGENCY, HOMING, MOVE_TIMEOUT, HOMING_COUNTEROUT }; // STATES
-  enum { EVT_HOMING_COUNTER, EVT_IDLE_TIMEOUT, EVT_MOVE_TIMEOUT, EVT_LIMIT_LOW, EVT_LIMIT_HIGH, EVT_EMERGENCY, EVT_STOP, EVT_ONTARGET, EVT_MOVE, EVT_DISABLE, EVT_ENABLE, EVT_HOMING, ELSE }; // EVENTS
-  Atm_Tstepper( void ) : Machine() {};
-  Atm_Tstepper& begin( Stepper & motorRef, StepControl & stepControlRef );
-  Atm_Tstepper& trace( Stream & stream );
-  Atm_Tstepper& trigger( int event );
-  int state( void );
-  Atm_Tstepper& onOnchange( Machine& machine, int event = 0 );
-  Atm_Tstepper& onOnchange( atm_cb_push_t callback, int idx = 0 );
-  Atm_Tstepper& onOnchangeacceleration( Machine& machine, int event = 0 );
-  Atm_Tstepper& onOnchangeacceleration( atm_cb_push_t callback, int idx = 0 );
-  Atm_Tstepper& onOnchangeposition( Machine& machine, int event = 0 );
-  Atm_Tstepper& onOnchangeposition( atm_cb_push_t callback, int idx = 0 );
-  Atm_Tstepper& onOnchangestate( Machine& machine, int event = 0 );
-  Atm_Tstepper& onOnchangestate( atm_cb_push_t callback, int idx = 0 );
-  Atm_Tstepper& onOnhighlimit( Machine& machine, int event = 0 );
-  Atm_Tstepper& onOnhighlimit( atm_cb_push_t callback, int idx = 0 );
-  Atm_Tstepper& onOnhomingcounterout( Machine& machine, int event = 0 );
-  Atm_Tstepper& onOnhomingcounterout( atm_cb_push_t callback, int idx = 0 );
-  Atm_Tstepper& onOnhomingtimeout( Machine& machine, int event = 0 );
-  Atm_Tstepper& onOnhomingtimeout( atm_cb_push_t callback, int idx = 0 );
-  Atm_Tstepper& onOnidletimeout( Machine& machine, int event = 0 );
-  Atm_Tstepper& onOnidletimeout( atm_cb_push_t callback, int idx = 0 );
-  Atm_Tstepper& onOnlowlimit( Machine& machine, int event = 0 );
-  Atm_Tstepper& onOnlowlimit( atm_cb_push_t callback, int idx = 0 );
-  Atm_Tstepper& onOnmovetimeout( Machine& machine, int event = 0 );
-  Atm_Tstepper& onOnmovetimeout( atm_cb_push_t callback, int idx = 0 );
-  Atm_Tstepper& onSpeed( Machine& machine, int event = 0 );
-  Atm_Tstepper& onSpeed( atm_cb_push_t callback, int idx = 0 );
-  Atm_Tstepper& homing_counter( void );
-  Atm_Tstepper& idle_timeout( void );
-  Atm_Tstepper& move_timeout( void );
-  Atm_Tstepper& limit_low( void );
-  Atm_Tstepper& limit_high( void );
-  Atm_Tstepper& emergency( void );
-  Atm_Tstepper& stop( void );
-  Atm_Tstepper& ontarget( void );
-  Atm_Tstepper& move( long int stepRel );
-  Atm_Tstepper& moveTo( long int stepAbs );
-  Atm_Tstepper& disable( void );
-  Atm_Tstepper& enable( void );
-  Atm_Tstepper& homing( int direction );
-
-
-  Atm_Tstepper& setLimitType( int limitType = 0);
-  Atm_Tstepper& setLimitPins( int limitPinLow);
-  Atm_Tstepper& setLimitPins( int limitPinLow, int limitPinHigh);
-  Atm_Tstepper& limitReversed( bool reversed );
-  Atm_Tstepper& limitThresholds( int limitThreshold0, int limitThreshold1,
-                                  int limitThreshold2, int limitThreshold3);
-  bool limitState[2] ; // up to two limits, at least one for homing
-
-  virtual Atm_Tstepper& onOSC( void );
-  virtual Atm_Tstepper& sendOSC( void );
-
-  Stepper * motor;
-  StepControl * controller;
-
-  Atm_Tstepper& setEnablePin( int enablePin );
-  Atm_Tstepper& enableReversed( bool reverse );
-  bool enabled ;
-  // Atm_TeensyStep& enable( bool enable );
-
-  int printPosition();
-
- private:
-  enum { ENT_DISABLED, ENT_ENABLED, ENT_RUNNING, LP_RUNNING, ENT_STOPPING, LP_STOPPING, ENT_EMERGENCY, ENT_HOMING, LP_HOMING, EXT_HOMING, ENT_MOVE_TIMEOUT }; // ACTIONS
-  enum { ON_ONCHANGE, ON_ONCHANGEACCELERATION, ON_ONCHANGEPOSITION, ON_ONCHANGESTATE, ON_ONHIGHLIMIT, ON_ONHOMINGCOUNTEROUT, ON_ONHOMINGTIMEOUT, ON_ONIDLETIMEOUT, ON_ONLOWLIMIT, ON_ONMOVETIMEOUT, ON_SPEED, CONN_MAX }; // CONNECTORS
-  atm_connector connectors[CONN_MAX];
-  int event( int id );
-  void action( int id );
-
-  atm_timer_millis idle_timer ;
-  int IDLE_TIMEOUT_DURATION = 500000 ;
-  atm_timer_millis moving_timer ;
-  int MOVING_TIMEOUT_DURATION = 12000000 ;
-  atm_counter homing_counterout ;
-
-  long int _currentStep ;
-  long int _targetStep ;
-
-  int _enablePin = -1;
-  bool _enableReversed = 0 ;
-
-  int _limitPin[2];
-  enum { NONE, DIGITAL_1, DIGITAL_2, ANALOG_1 };
-  int _limitType; //type of limit switch, from list over
-  bool _limitReversed ; //invert logic of limit switches
-  int _limitThresholds[4] ; //analog value  range for two analog limits
-  void updateLimitSwitch();
-
-
-
-};
-
-/*
-Automaton::ATML::begin - Automaton Markup Language
-
-<?xml version="1.0" encoding="UTF-8"?>
-<machines>
-  <machine name="Atm_Tstepper">
-    <states>
-      <DISABLED index="0" on_enter="ENT_DISABLED">
-        <EVT_ENABLE>ENABLED</EVT_ENABLE>
-      </DISABLED>
-      <ENABLED index="1" on_enter="ENT_ENABLED">
-        <EVT_IDLE_TIMEOUT>DISABLED</EVT_IDLE_TIMEOUT>
-        <EVT_MOVE>RUNNING</EVT_MOVE>
-        <EVT_DISABLE>DISABLED</EVT_DISABLE>
-      </ENABLED>
-      <RUNNING index="2" on_enter="ENT_RUNNING" on_loop="LP_RUNNING">
-        <EVT_MOVE_TIMEOUT>MOVE_TIMEOUT</EVT_MOVE_TIMEOUT>
-        <EVT_LIMIT_LOW>EMERGENCY</EVT_LIMIT_LOW>
-        <EVT_LIMIT_HIGH>EMERGENCY</EVT_LIMIT_HIGH>
-        <EVT_EMERGENCY>EMERGENCY</EVT_EMERGENCY>
-        <EVT_STOP>STOPPING</EVT_STOP>
-        <EVT_ONTARGET>ENABLED</EVT_ONTARGET>
-        <EVT_MOVE>RUNNING</EVT_MOVE>
-      </RUNNING>
-      <STOPPING index="3" on_enter="ENT_STOPPING" on_loop="LP_STOPPING">
-        <EVT_LIMIT_LOW>EMERGENCY</EVT_LIMIT_LOW>
-        <EVT_LIMIT_HIGH>EMERGENCY</EVT_LIMIT_HIGH>
-        <EVT_EMERGENCY>EMERGENCY</EVT_EMERGENCY>
-        <EVT_MOVE>RUNNING</EVT_MOVE>
-      </STOPPING>
-      <EMERGENCY index="4" on_enter="ENT_EMERGENCY">
-      </EMERGENCY>
-      <HOMING index="5" on_enter="ENT_HOMING" on_loop="LP_HOMING" on_exit="EXT_HOMING">
-        <EVT_MOVE_TIMEOUT>MOVE_TIMEOUT</EVT_MOVE_TIMEOUT>
-        <EVT_LIMIT_LOW>ENABLED</EVT_LIMIT_LOW>
-        <EVT_LIMIT_HIGH>ENABLED</EVT_LIMIT_HIGH>
-        <EVT_EMERGENCY>EMERGENCY</EVT_EMERGENCY>
-        <EVT_STOP>STOPPING</EVT_STOP>
-        <EVT_ONTARGET>MOVE_TIMEOUT</EVT_ONTARGET>
-      </HOMING>
-      <MOVE_TIMEOUT index="6" on_enter="ENT_MOVE_TIMEOUT">
-        <EVT_MOVE_TIMEOUT>MOVE_TIMEOUT</EVT_MOVE_TIMEOUT>
-      </MOVE_TIMEOUT>
-      <HOMING_COUNTEROUT index="7">
-      </HOMING_COUNTEROUT>
-    </states>
-    <events>
-      <EVT_HOMING_COUNTER index="0" access="MIXED"/>
-      <EVT_IDLE_TIMEOUT index="1" access="MIXED"/>
-      <EVT_MOVE_TIMEOUT index="2" access="MIXED"/>
-      <EVT_LIMIT_LOW index="3" access="MIXED"/>
-      <EVT_LIMIT_HIGH index="4" access="MIXED"/>
-      <EVT_EMERGENCY index="5" access="MIXED"/>
-      <EVT_STOP index="6" access="MIXED"/>
-      <EVT_ONTARGET index="7" access="MIXED"/>
-      <EVT_MOVE index="8" access="MIXED"/>
-      <EVT_DISABLE index="9" access="MIXED"/>
-      <EVT_ENABLE index="10" access="MIXED"/>
-    </events>
-    <connectors>
-      <ONCHANGE autostore="0" broadcast="0" dir="PUSH" slots="1"/>
-      <ONCHANGEACCELERATION autostore="0" broadcast="0" dir="PUSH" slots="1"/>
-      <ONCHANGEPOSITION autostore="0" broadcast="0" dir="PUSH" slots="1"/>
-      <ONCHANGESTATE autostore="0" broadcast="0" dir="PUSH" slots="1"/>
-      <ONHIGHLIMIT autostore="0" broadcast="0" dir="PUSH" slots="1"/>
-      <ONHOMINGCOUNTEROUT autostore="0" broadcast="0" dir="PUSH" slots="1"/>
-      <ONHOMINGTIMEOUT autostore="0" broadcast="0" dir="PUSH" slots="1"/>
-      <ONIDLETIMEOUT autostore="0" broadcast="0" dir="PUSH" slots="1"/>
-      <ONLOWLIMIT autostore="0" broadcast="0" dir="PUSH" slots="1"/>
-      <ONMOVETIMEOUT autostore="0" broadcast="0" dir="PUSH" slots="1"/>
-      <SPEED autostore="0" broadcast="0" dir="PUSH" slots="1"/>
-    </connectors>
-    <methods>
-    </methods>
-  </machine>
-</machines>
-
-Automaton::ATML::end
-*/

+ 0 - 30
HTequi-firmware/lib/Atm_lien/Atm_Tstepper_OSC.cpp

@@ -1,30 +0,0 @@
-#include "Atm_Tstepper_OSC.h"
-
-Atm_Tstepper_OSC& Atm_Tstepper_OSC::begin(Atm_Tstepper & stepperMachineRef, EthernetUDP& udpRef, OSCBundle& bndl, const char* address) {
-  this-> stepperMachine = &stepperMachineRef;
-  _adress = address;
-  this->_udpRef =   &udpRef;
-  this->_bndl = &bndl ;
-  return *this;
-}
-
-
-
-Atm_Tstepper_OSC& Atm_Tstepper_OSC::onOSC(OSCMessage& msg ){
-  Serial.println("OSC");
-  int patternOffset = msg.match(_adress) ;
-  if(patternOffset){
-    if(msg.fullMatch("/speedAcc", patternOffset)){
-      stepperMachine->motor->setMaxSpeed(msg.getInt(0));
-      stepperMachine->motor->setAcceleration(msg.getInt(1));
-    }
-    if(msg.fullMatch("/enable", patternOffset)){msg.getInt(0) ? stepperMachine->trigger(stepperMachine->EVT_ENABLE) : stepperMachine->trigger(stepperMachine->EVT_DISABLE);}
-    //if(msg.fullMatch("/home", patternOffset)){home(msg.getInt(0));}
-    if(msg.fullMatch("/move", patternOffset)){stepperMachine->move(msg.getInt(0));}
-    if(msg.fullMatch("/moveTo", patternOffset)){stepperMachine->moveTo(msg.getInt(0));}
-    if(msg.fullMatch("/stop", patternOffset)){stepperMachine->stop();}
-    if(msg.fullMatch("/emergencyStop", patternOffset)){stepperMachine->emergency();}
-
-    return *this;
-  }
-}

+ 0 - 26
HTequi-firmware/lib/Atm_lien/Atm_Tstepper_OSC.h

@@ -1,26 +0,0 @@
-#pragma once
-
-#include "Atm_Tstepper.h"
-
-#include <EthernetUdp.h>
-#include <OSCMessage.h>
-#include <OSCBundle.h>
-
-class Atm_Tstepper_OSC {
-
-  public:
-    Atm_Tstepper_OSC( void ){};
-    Atm_Tstepper_OSC& begin(Atm_Tstepper & stepperMachineRef, EthernetUDP& udpRef, OSCBundle& bndl, const char* address);
-    Atm_Tstepper* stepperMachine ;
-    const char* _adress = "/OSC";
-    EthernetUDP* _udpRef ;
-    Atm_Tstepper_OSC& onOSC(OSCMessage& msg );
-    Atm_Tstepper& sendOSC( void );
-
-  private:
-   // AccelStepper _motor;
-   // Stepper *_motor;       // STEP pin: 2, DIR pin: 3
-   // StepControl _controller;
-   OSCBundle* _bndl;
-
-};

+ 0 - 260
HTequi-firmware/lib/Atm_lien/Atm_out.cpp

@@ -1,260 +0,0 @@
-#include "Atm_out.h"
-
-Atm_out& Atm_out::begin( int attached_pin, bool activeLow,
-            EthernetUDP& udpRef, OSCBundle& bndl, const char* address ) {
-  // clang-format off
-  static const state_t state_table[] PROGMEM = {
-    /*               ON_ENTER    ON_LOOP    ON_EXIT  EVT_ON_TIMER  EVT_OFF_TIMER EVT_WT_TIMER EVT_COUNTER  EVT_ON  EVT_OFF  EVT_BLINK  EVT_TOGGLE  EVT_TOGGLE_BLINK   ELSE */
-    /* IDLE      */  ENT_INIT, ATM_SLEEP,        -1,           -1,            -1,          -1,         -1,  WT_ON,      -1,  WT_START,         ON,         WT_START,    -1, // LED off
-    /* ON        */    ENT_ON, ATM_SLEEP,        -1,           -1,            -1,          -1,         -1,     -1,     OFF,  WT_START,        OFF,              OFF,    -1, // LED on
-    /* START     */    ENT_ON,        -1,        -1,    BLINK_OFF,            -1,          -1,         -1,  WT_ON,     OFF,        -1,        OFF,              OFF,    -1, // Start blinking
-    /* BLINK_OFF */   ENT_OFF,        -1,        -1,           -1,          LOOP,          -1,         -1,  WT_ON,     OFF,        -1,        OFF,              OFF,    -1,
-    /* LOOP      */        -1,        -1,        -1,           -1,            -1,          -1,       DONE,  WT_ON,     OFF,        -1,        OFF,              OFF, START,
-    /* DONE      */        -1,        -1, EXT_CHAIN,           -1,           OFF,          -1,         -1,  WT_ON,     OFF,  WT_START,        OFF,              OFF,    -1, // Wait after last blink
-    /* OFF       */   ENT_OFF,        -1,        -1,           -1,            -1,          -1,         -1,  WT_ON,     OFF,  WT_START,         -1,               -1,  IDLE, // All off -> IDLE
-    /* WT_ON     */        -1,        -1,        -1,           -1,            -1,          ON,         -1,  WT_ON,     OFF,  WT_START,         -1,               -1,    -1, // LEAD for ON
-    /* WT_START  */        -1,        -1,        -1,           -1,            -1,       START,         -1,  WT_ON,     OFF,  WT_START,         -1,               -1,    -1, // LEAD for BLINK
-  };
-  // clang-format on
-  Machine::begin( state_table, ELSE );
-  pin = attached_pin;
-  this->activeLow = activeLow;
-  level = 255;
-  toLow = 0;
-  toHigh = 255;
-  wrap = false;
-  pinMode( pin, OUTPUT );
-  digitalWrite( pin, activeLow ? HIGH : LOW );
-  on_timer.set( 500 );
-  off_timer.set( 500 );
-  pwm( 512, 1 );
-  lead_timer.set( 0 );
-  repeat_count = ATM_COUNTER_OFF;
-  counter.set( repeat_count );
-  while ( state() != 0 ) cycle();
-  _adress = address;
-  this->_udpRef =   &udpRef;
-  this->_bndl = &bndl ;
-  return *this;
-}
-
-Atm_out& Atm_out::pwm( uint16_t width, float freq ) {
-
-    if ( freq > -1 ) {
-		this->freq = freq;
-	} else {
-		freq = this->freq;
-	}
-	this->width = width;
-	float cycle_width = 1000 / freq;
-	on_timer.set( cycle_width / 1024 * this->width );
-	off_timer.set( cycle_width / 1024 * ( 1024 - this->width ) );
-	return *this;
-}
-
-Atm_out& Atm_out::frequency( float freq ) {
-
-	this->freq = freq;
-	float cycle_width = 1000 / freq;
-	on_timer.set( cycle_width / 1024 * this->width );
-	off_timer.set( cycle_width / 1024 * ( 1024 - this->width ) );
-	return *this;
-}
-
-int Atm_out::event( int id ) {
-  switch ( id ) {
-    case EVT_ON_TIMER:
-      return on_timer.expired( this );
-    case EVT_OFF_TIMER:
-      return off_timer.expired( this );
-    case EVT_WT_TIMER:
-      return lead_timer.expired( this );
-    case EVT_COUNTER:
-      return counter.expired();
-  }
-  return 0;
-}
-
-void Atm_out::action( int id ) {
-  switch ( id ) {
-    case ENT_INIT:
-      counter.set( repeat_count );
-      return;
-    case ENT_ON:
-      if ( on_timer.value > 0 ) { // Never turn if on_timer is zero (duty cycle 0 must be dark)
-        if ( activeLow ) {
-          digitalWrite( pin, LOW );
-        } else {
-          if ( level == toHigh ) {
-            digitalWrite( pin, HIGH );
-          } else {
-            analogWrite( pin, mapLevel( level ) );
-          }
-        }
-      }
-      return;
-    case ENT_OFF:
-      counter.decrement();
-      if ( !activeLow ) {
-        digitalWrite( pin, LOW );
-      } else {
-        if ( level == toHigh ) {
-          digitalWrite( pin, HIGH );
-        } else {
-          analogWrite( pin, mapLevel( level ) );
-        }
-      }
-      return;
-    case EXT_CHAIN:
-      onfinish.push( 0 );
-      return;
-  }
-}
-
-int Atm_out::mapLevel( int level ) {
-  if ( levelMapSize ) {
-    return levelMap[level];
-  } else {
-    return map( level, toLow, toHigh, 0, 255 );
-  }
-}
-
-Atm_out& Atm_out::on( void ) {
-  trigger( EVT_ON );
-  return *this;
-}
-
-Atm_out& Atm_out::off( void ) {
-  trigger( EVT_OFF );
-  return *this;
-}
-
-Atm_out& Atm_out::toggle( void ) {
-  trigger( EVT_TOGGLE );
-  return *this;
-}
-
-Atm_out& Atm_out::toggleBlink( void ) {
-  trigger( EVT_TOGGLE_BLINK );
-  return *this;
-}
-
-Atm_out& Atm_out::start( void ) {
-  trigger( EVT_BLINK );
-  return *this;
-}
-
-Atm_out& Atm_out::onFinish( Machine& machine, int event /* = 0 */ ) {
-  onfinish.set( &machine, event );
-  return *this;
-}
-
-Atm_out& Atm_out::onFinish( atm_cb_push_t callback, int idx /* = 0 */ ) {
-  onfinish.set( callback, idx );
-  return *this;
-}
-
-Atm_out& Atm_out::blink( uint32_t duration, uint32_t pause_duration, uint16_t repeat_count /* = ATM_COUNTER_OFF */ ) {
-  blink( duration );  // Time in which led is fully on
-  pause( pause_duration );
-  repeat( repeat_count );
-  return *this;
-}
-
-Atm_out& Atm_out::blink( uint32_t duration ) {
-  on_timer.set( duration );  // Time in which led is fully on
-  return *this;
-}
-
-Atm_out& Atm_out::blink( void ) {
-  trigger( EVT_BLINK );
-  return *this;
-}
-
-Atm_out& Atm_out::range( int toLow, int toHigh, bool wrap /* = false */ ) {
-  this->toLow = toLow;
-  this->toHigh = toHigh;
-  this->wrap = wrap;
-  level = toHigh;
-  return *this;
-}
-
-Atm_out& Atm_out::levels( unsigned char* map, int mapsize, bool wrap /* = false */ ) {
-  this->levelMap = map;
-  levelMapSize = mapsize;
-  range( 0, mapsize - 1, wrap );
-  return *this;
-}
-
-Atm_out& Atm_out::pause( uint32_t duration ) {  // Time in which led is fully off
-  off_timer.set( duration ? duration : 1 );     // Make sure off_timer is never 0 (work around)
-  return *this;
-}
-
-Atm_out& Atm_out::fade( int fade ) {
-  return *this;
-}  // Dummy for method compatibility with Atm_fade
-
-Atm_out& Atm_out::lead( uint32_t ms ) {
-  lead_timer.set( ms );
-  return *this;
-}
-
-Atm_out& Atm_out::repeat( uint16_t repeat ) {
-  counter.set( repeat_count = repeat );
-  return *this;
-}
-
-int Atm_out::brightness( int level /* = -1 */ ) {
-  if ( level > -1 ) {
-    this->level = level;
-    if ( current == ON || current == START ) {
-      analogWrite( pin, mapLevel( level ) );
-    }
-  }
-  return this->level;
-}
-
-int Atm_out::brighten( int v ) {
-  if ( abs( v ) == 1 ) {
-    int br = (int)this->level + v;
-    if ( br > toHigh )
-      br = wrap ? toLow : toHigh;
-    if ( br < toLow )
-      br = wrap ? toHigh : toLow;
-    brightness( br );
-  }
-  return this->level;
-}
-
-Atm_out& Atm_out::trigger( int event ) {
-  if ( event > ELSE ) {
-    brighten( event == EVT_BRUP ? 1 : -1 );
-  } else {
-    Machine::trigger( event );
-  }
-  return *this;
-}
-
-Atm_out& Atm_out::onOSC(OSCMessage& msg ){
-  Serial.println("OSC");
-  int patternOffset = msg.match(_adress) ;
-  if(patternOffset){
-
-    if(msg.fullMatch("/on", patternOffset)){trigger(EVT_ON);}
-    if(msg.fullMatch("/off", patternOffset)){trigger(EVT_OFF);}
-    if(msg.fullMatch("/toggle", patternOffset)){trigger(EVT_TOGGLE);}
-    if(msg.fullMatch("/blink", patternOffset)){trigger(EVT_BLINK);}
-    if(msg.fullMatch("/brightness", patternOffset)){brightness(msg.getInt(0));}
-
-    return *this;
-  }
-}
-
-Atm_out& Atm_out::trace( Stream& stream ) {
-  setTrace( &stream, atm_serial_debug::trace,
-            "LED\0EVT_ON_TIMER\0EVT_OFF_TIMER\0EVT_WT_TIMER\0EVT_COUNTER\0EVT_ON\0EVT_OFF\0EVT_"
-            "BLINK\0EVT_TOGGLE\0EVT_TOGGLE_BLINK\0ELSE\0"
-            "IDLE\0ON\0START\0BLINK_OFF\0LOOP\0DONE\0OFF\0WT_ON\0WT_START" );
-  return *this;
-}

+ 0 - 67
HTequi-firmware/lib/Atm_lien/Atm_out.h

@@ -1,67 +0,0 @@
-#pragma once
-
-#include <Automaton.h>
-
-#include <EthernetUdp.h>
-#include <OSCMessage.h>
-#include <OSCBundle.h>
-
-class Atm_out : public Machine {
- public:
-  enum { IDLE, ON, START, BLINK_OFF, LOOP, DONE, OFF, WT_ON, WT_START };
-  enum { EVT_ON_TIMER, EVT_OFF_TIMER, EVT_WT_TIMER, EVT_COUNTER, EVT_ON, EVT_OFF, EVT_BLINK, EVT_TOGGLE, EVT_TOGGLE_BLINK, ELSE, EVT_BRUP, EVT_BRDN }; // BRUP/BRDN pseudo
-  enum { EVT_START = EVT_BLINK };
-
-  Atm_out( void ) : Machine(){};
-  Atm_out& begin( int attached_pin, bool activeLow,
-          EthernetUDP& udpRef, OSCBundle& bndl, const char* address  );
-  Atm_out& blink( void );
-  Atm_out& blink( uint32_t duration );
-  Atm_out& blink( uint32_t duration, uint32_t pause_duration, uint16_t repeat_count = ATM_COUNTER_OFF );
-  Atm_out& pwm( uint16_t width, float freq = -1 );
-  Atm_out& frequency( float freq );
-  Atm_out& pause( uint32_t duration );
-  Atm_out& fade( int fade );
-  Atm_out& lead( uint32_t ms );
-  Atm_out& repeat( uint16_t repeat );
-  int brightness( int level = -1 );
-  Atm_out& on( void );
-  Atm_out& off( void );
-  Atm_out& toggle( void );
-  Atm_out& toggleBlink( void );
-  Atm_out& start( void );
-  Atm_out& trace( Stream& stream );
-  Atm_out& onFinish( Machine& machine, int event = 0 );
-  Atm_out& onFinish( atm_cb_push_t callback, int idx = 0 );
-  Atm_out& range( int toLow, int toHigh, bool wrap = false );
-  Atm_out& levels( unsigned char* map, int mapsize, bool wrap = false );
-  int brighten( int v = 1 );
-  Atm_out& trigger( int event );
-
-  const char* _adress = "/OSC";
-  EthernetUDP* _udpRef ;
-  Atm_out& onOSC(OSCMessage& msg );
-  OSCBundle* _bndl;
-
-
- private:
-  enum { ENT_INIT, ENT_ON, ENT_OFF, EXT_CHAIN };
-  uint8_t level;
-  short pin;
-  bool activeLow;
-  uint8_t toHigh, toLow;
-  bool wrap;
-  uint16_t repeat_count;
-  uint16_t width;
-  float freq;
-  atm_timer_millis on_timer, off_timer, lead_timer;
-  atm_counter counter;
-  atm_connector onfinish;
-  unsigned char* levelMap;
-  int levelMapSize;
-  int mapLevel( int level );
-
-
-  int event( int id );
-  void action( int id );
-};

+ 0 - 152
HTequi-firmware/lib/Automaton-1.0.2/src/Atm_fade.cpp

@@ -1,152 +0,0 @@
-#include "Atm_fade.hpp"
-
-Atm_fade& Atm_fade::begin( int attached_pin ) {
-  // clang-format off
-  const static state_t state_table[] PROGMEM = {
-    /*               ON_ENTER    ON_LOOP       ON_EXIT  EVT_CNT_FADE EVT_TM_FADE   EVT_TM_ON  EVT_TM_OFF   EVT_CNT_RPT  EVT_ON EVT_OFF EVT_BLINK  EVT_TOGGLE  EVT_TOGGLE_BLINK    ELSE  */
-    /* IDLE   */      ENT_OFF, ATM_SLEEP,           -1,           -1,         -1,         -1,         -1,           -1,OSTARTU,   IDLE,    START,    OSTARTU,            START,     -1,  // LED off
-    /* ON     */       ENT_ON, ATM_SLEEP,           -1,           -1,         -1,         -1,         -1,           -1,     -1,OSTARTD,    START,    OSTARTD,          OSTARTD,     -1,  // LED on
-    /* START  */      ENT_OFF,        -1,           -1,           -1,         -1,         -1,         -1,           -1,OSTARTU,   IDLE,    START,       IDLE,             IDLE, STARTU,  // Start fading
-    /* STARTU */    ENT_START,        -1,           -1,           -1,         -1,         -1,         UP,           -1,OSTARTU,   IDLE,    START,       IDLE,             IDLE,     -1,  
-    /* UP     */       ENT_UP,        -1,           -1,       STARTD,         UP,         -1,         -1,           -1,OSTARTU,   IDLE,    START,       IDLE,             IDLE,     -1,
-    /* STARTD */    ENT_START,        -1,           -1,           -1,         -1,       DOWN,         -1,           -1,OSTARTU,   IDLE,    START,       IDLE,             IDLE,     -1,
-    /* DOWN   */     ENT_DOWN,        -1,           -1,       REPEAT,       DOWN,         -1,         -1,           -1,OSTARTU,   IDLE,    START,       IDLE,             IDLE,     -1,
-    /* REPEAT */   ENT_REPEAT,        -1,           -1,           -1,         -1,         -1,         -1,         DONE,OSTARTU,   IDLE,    START,       IDLE,             IDLE, STARTU,
-    /* DONE   */     ENT_DONE,        -1,           -1,           -1,         -1,         -1,         -1,           -1,     -1,     -1,       -1,         -1,               -1,   IDLE,
-    /* OSTARTU*/    ENT_START,        -1,           -1,           -1,         -1,         -1,         -1,           -1,     -1,   IDLE,    START,       IDLE,             IDLE,    OUP,  
-    /* OUP    */       ENT_UP,        -1,           -1,           ON,        OUP,         -1,         -1,           -1,     -1,   IDLE,    START,       IDLE,             IDLE,     -1,
-    /* OSTARTD*/    ENT_START,        -1,           -1,           -1,         -1,         -1,         -1,           -1,OSTARTU,   IDLE,    START,       IDLE,             IDLE,  ODOWN,
-    /* ODOWN  */     ENT_DOWN,        -1,           -1,         IDLE,      ODOWN,         -1,         -1,           -1,OSTARTU,   IDLE,    START,       IDLE,             IDLE,     -1,
-
-  };
-  // clang-format on
-  Machine::begin( state_table, ELSE );
-  pin = attached_pin;
-  pinMode( pin, OUTPUT );
-  timer_fade.set( 0 );   // Number of ms per slope step (slope duration: rate * 32 ms)
-  timer_on.set( 500 );   // Plateau between slopes (in which led is fully on)
-  timer_off.set( 500 );  // Pause between slopes (in which led is fully off)
-  counter_fade.set( SLOPE_SIZE );
-  counter_repeat.set( ATM_COUNTER_OFF );
-  repeat_count = ATM_COUNTER_OFF;
-  return *this;
-}
-
-Atm_fade& Atm_fade::blink( uint32_t duration, uint32_t pause_duration, uint16_t repeat_count /* = ATM_COUNTER_OFF */ ) {
-  blink( duration );  // Time in which led is fully on
-  pause( pause_duration );
-  repeat( repeat_count );
-  return *this;
-}
-
-Atm_fade& Atm_fade::blink( uint32_t duration ) {
-  timer_on.set( duration );  // Plateau between slopes (in which led is fully on)
-  return *this;
-}
-
-Atm_fade& Atm_fade::blink( void ) {
-  trigger( EVT_BLINK );
-  return *this;
-}
-
-Atm_fade& Atm_fade::pause( uint32_t duration ) {  // Pause between slopes (in which led is fully off)
-  timer_off.set( duration ? duration : 1 );       // Make sure off_timer is never 0 (work around)
-  return *this;
-}
-
-Atm_fade& Atm_fade::fade( int fade ) {
-  timer_fade.set( fade >= 0 ? fade : ATM_TIMER_OFF );  // Number of ms per slope step (slope duration: rate * 32 ms)
-  return *this;
-}
-
-Atm_fade& Atm_fade::repeat( uint16_t repeat ) {
-  counter_repeat.set( repeat_count = repeat );
-  return *this;
-}
-
-int Atm_fade::event( int id ) {
-  switch ( id ) {
-    case EVT_TM_FADE:
-      return timer_fade.expired( this );
-    case EVT_TM_ON:
-      return timer_on.expired( this );
-    case EVT_TM_OFF:
-      return timer_off.expired( this );
-    case EVT_CNT_FADE:
-      return counter_fade.expired();
-    case EVT_CNT_RPT:
-      return counter_repeat.expired();
-  }
-  return 0;
-}
-
-void Atm_fade::action( int id ) {
-  switch ( id ) {
-    case ENT_ON:
-      analogWrite( pin, 255 );
-      return;
-    case ENT_REPEAT:
-      counter_repeat.decrement();
-      return;
-    case ENT_OFF:
-      counter_repeat.set( repeat_count );
-      analogWrite( pin, 0 );
-      return;
-    case ENT_START:
-      counter_fade.set( SLOPE_SIZE );
-      return;
-    case ENT_UP:
-      analogWrite( pin, slope[SLOPE_SIZE - counter_fade.value] );
-      counter_fade.decrement();
-      return;
-    case ENT_DOWN:
-      analogWrite( pin, slope[counter_fade.value - 1] );
-      counter_fade.decrement();
-      return;
-    case ENT_DONE:
-      onfinish.push( 0 );
-      return;    
-  }
-}
-
-Atm_fade& Atm_fade::on( void ) {
-  trigger( EVT_ON );
-  return *this;
-}
-
-Atm_fade& Atm_fade::off( void ) {
-  trigger( EVT_OFF );
-  return *this;
-}
-
-Atm_fade& Atm_fade::toggle( void ) {
-  trigger( EVT_TOGGLE );
-  return *this;
-}
-
-Atm_fade& Atm_fade::toggleBlink( void ) {
-  trigger( EVT_TOGGLE_BLINK );
-  return *this;
-}
-
-Atm_fade& Atm_fade::start( void ) {
-  trigger( EVT_BLINK );
-  return *this;
-}
-
-Atm_fade& Atm_fade::onFinish( Machine& machine, int event /* = 0 */ ) {
-  onfinish.set( &machine, event );
-  return *this;
-}
-
-Atm_fade& Atm_fade::onFinish( atm_cb_push_t callback, int idx /* = 0 */ ) {
-  onfinish.set( callback, idx );
-  return *this;
-}
-
-Atm_fade& Atm_fade::trace( Stream& stream ) {
-  setTrace( &stream, atm_serial_debug::trace,
-            "FADE\0EVT_CNT_FADE\0EVT_TM_FADE\0EVT_TM_ON\0EVT_TM_OFF\0EVT_CNT_RPT\0EVT_ON\0EVT_OFF\0EVT_BLINK\0EVT_TOGGLE\0EVT_TOGGLE_BLINK\0ELSE\0"
-            "IDLE\0ON\0START\0STARTU\0UP\0STARTD\0DOWN\0REPEAT\0DONE\0OSTARTU\0OUP\0OSTARTD\0ODOWN" );
-  return *this;
-}

+ 0 - 39
HTequi-firmware/lib/Automaton-1.0.2/src/Atm_fade.hpp

@@ -1,39 +0,0 @@
-#pragma once
-
-#include <Automaton.h>
-
-class Atm_fade : public Machine {
- public:
-  enum { IDLE, ON, START, STARTU, UP, STARTD, DOWN, REPEAT, DONE, OSTARTU, OUP, OSTARTD, ODOWN };
-  enum { EVT_CNT_FADE, EVT_TM_FADE, EVT_TM_ON, EVT_TM_OFF, EVT_CNT_RPT, EVT_ON, EVT_OFF, EVT_BLINK, EVT_TOGGLE, EVT_TOGGLE_BLINK, ELSE };
-  enum { EVT_START = EVT_BLINK };
-
-  Atm_fade( void ) : Machine(){};
-  Atm_fade& begin( int attached_pin );
-  Atm_fade& trace( Stream& stream );
-  Atm_fade& blink( uint32_t duration, uint32_t pause_duration, uint16_t repeat_count = ATM_COUNTER_OFF );
-  Atm_fade& blink( uint32_t duration );
-  Atm_fade& blink( void );
-  Atm_fade& pause( uint32_t duration );
-  Atm_fade& fade( int fade );
-  Atm_fade& repeat( uint16_t repeat );
-  Atm_fade& on( void );
-  Atm_fade& off( void );
-  Atm_fade& toggle( void );
-  Atm_fade& toggleBlink( void );
-  Atm_fade& start( void );
-  Atm_fade& onFinish( Machine& machine, int event = 0 );
-  Atm_fade& onFinish( atm_cb_push_t callback, int idx = 0 );
-
- private:
-  enum { ENT_REPEAT, ENT_OFF, ENT_ON, ENT_UP, ENT_DOWN, ENT_START, ENT_DONE };
-  static const uint8_t SLOPE_SIZE = 32;
-  uint8_t slope[SLOPE_SIZE] = {0, 0, 1, 1, 2, 2, 3, 4, 5, 6, 7, 8, 10, 12, 15, 18, 22, 26, 31, 37, 44, 54, 63, 76, 90, 108, 127, 153, 180, 217, 230, 255};
-  short pin;
-  uint16_t repeat_count;
-  atm_connector onfinish;
-  atm_timer_millis timer_fade, timer_on, timer_off;
-  atm_counter counter_fade, counter_repeat;
-  int event( int id );
-  void action( int id );
-};

+ 0 - 241
HTequi-firmware/lib/Automaton-1.0.2/src/Atm_led.cpp

@@ -1,241 +0,0 @@
-#include "Atm_led.hpp"
-
-Atm_led& Atm_led::begin( int attached_pin, bool activeLow ) {
-  // clang-format off
-  static const state_t state_table[] PROGMEM = {
-    /*               ON_ENTER    ON_LOOP    ON_EXIT  EVT_ON_TIMER  EVT_OFF_TIMER EVT_WT_TIMER EVT_COUNTER  EVT_ON  EVT_OFF  EVT_BLINK  EVT_TOGGLE  EVT_TOGGLE_BLINK   ELSE */
-    /* IDLE      */  ENT_INIT, ATM_SLEEP,        -1,           -1,            -1,          -1,         -1,  WT_ON,      -1,  WT_START,         ON,         WT_START,    -1, // LED off
-    /* ON        */    ENT_ON, ATM_SLEEP,        -1,           -1,            -1,          -1,         -1,     -1,     OFF,  WT_START,        OFF,              OFF,    -1, // LED on
-    /* START     */    ENT_ON,        -1,        -1,    BLINK_OFF,            -1,          -1,         -1,  WT_ON,     OFF,        -1,        OFF,              OFF,    -1, // Start blinking
-    /* BLINK_OFF */   ENT_OFF,        -1,        -1,           -1,          LOOP,          -1,         -1,  WT_ON,     OFF,        -1,        OFF,              OFF,    -1,
-    /* LOOP      */        -1,        -1,        -1,           -1,            -1,          -1,       DONE,  WT_ON,     OFF,        -1,        OFF,              OFF, START,
-    /* DONE      */        -1,        -1, EXT_CHAIN,           -1,           OFF,          -1,         -1,  WT_ON,     OFF,  WT_START,        OFF,              OFF,    -1, // Wait after last blink
-    /* OFF       */   ENT_OFF,        -1,        -1,           -1,            -1,          -1,         -1,  WT_ON,     OFF,  WT_START,         -1,               -1,  IDLE, // All off -> IDLE
-    /* WT_ON     */        -1,        -1,        -1,           -1,            -1,          ON,         -1,  WT_ON,     OFF,  WT_START,         -1,               -1,    -1, // LEAD for ON
-    /* WT_START  */        -1,        -1,        -1,           -1,            -1,       START,         -1,  WT_ON,     OFF,  WT_START,         -1,               -1,    -1, // LEAD for BLINK
-  };
-  // clang-format on
-  Machine::begin( state_table, ELSE );
-  pin = attached_pin;
-  this->activeLow = activeLow;
-  level = 255;
-  toLow = 0;
-  toHigh = 255;
-  wrap = false;
-  pinMode( pin, OUTPUT );
-  digitalWrite( pin, activeLow ? HIGH : LOW );
-  on_timer.set( 500 );
-  off_timer.set( 500 );
-  pwm( 512, 1 );
-  lead_timer.set( 0 );
-  repeat_count = ATM_COUNTER_OFF;
-  counter.set( repeat_count );
-  while ( state() != 0 ) cycle();
-  return *this;
-}
-
-Atm_led& Atm_led::pwm( uint16_t width, float freq ) {
-
-    if ( freq > -1 ) {
-		this->freq = freq;
-	} else {
-		freq = this->freq;
-	}
-	this->width = width;
-	float cycle_width = 1000 / freq;
-	on_timer.set( cycle_width / 1024 * this->width );
-	off_timer.set( cycle_width / 1024 * ( 1024 - this->width ) );
-	return *this;
-}
-
-Atm_led& Atm_led::frequency( float freq ) {
-
-	this->freq = freq;
-	float cycle_width = 1000 / freq;
-	on_timer.set( cycle_width / 1024 * this->width );
-	off_timer.set( cycle_width / 1024 * ( 1024 - this->width ) );
-	return *this;
-}
-
-int Atm_led::event( int id ) {
-  switch ( id ) {
-    case EVT_ON_TIMER:
-      return on_timer.expired( this );
-    case EVT_OFF_TIMER:
-      return off_timer.expired( this );
-    case EVT_WT_TIMER:
-      return lead_timer.expired( this );
-    case EVT_COUNTER:
-      return counter.expired();
-  }
-  return 0;
-}
-
-void Atm_led::action( int id ) {
-  switch ( id ) {
-    case ENT_INIT:
-      counter.set( repeat_count );
-      return;
-    case ENT_ON:
-      if ( on_timer.value > 0 ) { // Never turn if on_timer is zero (duty cycle 0 must be dark)
-        if ( activeLow ) {
-          digitalWrite( pin, LOW );
-        } else {
-          if ( level == toHigh ) {
-            digitalWrite( pin, HIGH );
-          } else {
-            analogWrite( pin, mapLevel( level ) );
-          }
-        }
-      }
-      return;
-    case ENT_OFF:
-      counter.decrement();
-      if ( !activeLow ) {
-        digitalWrite( pin, LOW );
-      } else {
-        if ( level == toHigh ) {
-          digitalWrite( pin, HIGH );
-        } else {
-          analogWrite( pin, mapLevel( level ) );
-        }
-      }
-      return;
-    case EXT_CHAIN:
-      onfinish.push( 0 );
-      return;
-  }
-}
-
-int Atm_led::mapLevel( int level ) {
-  if ( levelMapSize ) {
-    return levelMap[level];
-  } else {
-    return map( level, toLow, toHigh, 0, 255 );
-  }
-}
-
-Atm_led& Atm_led::on( void ) {
-  trigger( EVT_ON );
-  return *this;
-}
-
-Atm_led& Atm_led::off( void ) {
-  trigger( EVT_OFF );
-  return *this;
-}
-
-Atm_led& Atm_led::toggle( void ) {
-  trigger( EVT_TOGGLE );
-  return *this;
-}
-
-Atm_led& Atm_led::toggleBlink( void ) {
-  trigger( EVT_TOGGLE_BLINK );
-  return *this;
-}
-
-Atm_led& Atm_led::start( void ) {
-  trigger( EVT_BLINK );
-  return *this;
-}
-
-Atm_led& Atm_led::onFinish( Machine& machine, int event /* = 0 */ ) {
-  onfinish.set( &machine, event );
-  return *this;
-}
-
-Atm_led& Atm_led::onFinish( atm_cb_push_t callback, int idx /* = 0 */ ) {
-  onfinish.set( callback, idx );
-  return *this;
-}
-
-Atm_led& Atm_led::blink( uint32_t duration, uint32_t pause_duration, uint16_t repeat_count /* = ATM_COUNTER_OFF */ ) {
-  blink( duration );  // Time in which led is fully on
-  pause( pause_duration );
-  repeat( repeat_count );
-  return *this;
-}
-
-Atm_led& Atm_led::blink( uint32_t duration ) {
-  on_timer.set( duration );  // Time in which led is fully on
-  return *this;
-}
-
-Atm_led& Atm_led::blink( void ) {
-  trigger( EVT_BLINK );
-  return *this;
-}
-
-Atm_led& Atm_led::range( int toLow, int toHigh, bool wrap /* = false */ ) {
-  this->toLow = toLow;
-  this->toHigh = toHigh;
-  this->wrap = wrap;
-  level = toHigh;
-  return *this;
-}
-
-Atm_led& Atm_led::levels( unsigned char* map, int mapsize, bool wrap /* = false */ ) {
-  this->levelMap = map;
-  levelMapSize = mapsize;
-  range( 0, mapsize - 1, wrap );
-  return *this;
-}
-
-Atm_led& Atm_led::pause( uint32_t duration ) {  // Time in which led is fully off
-  off_timer.set( duration ? duration : 1 );     // Make sure off_timer is never 0 (work around)
-  return *this;
-}
-
-Atm_led& Atm_led::fade( int fade ) {
-  return *this;
-}  // Dummy for method compatibility with Atm_fade
-
-Atm_led& Atm_led::lead( uint32_t ms ) {
-  lead_timer.set( ms );
-  return *this;
-}
-
-Atm_led& Atm_led::repeat( uint16_t repeat ) {
-  counter.set( repeat_count = repeat );
-  return *this;
-}
-
-int Atm_led::brightness( int level /* = -1 */ ) {
-  if ( level > -1 ) {
-    this->level = level;
-    if ( current == ON || current == START ) {
-      analogWrite( pin, mapLevel( level ) );
-    }
-  }
-  return this->level;
-}
-
-int Atm_led::brighten( int v ) {
-  if ( abs( v ) == 1 ) {
-    int br = (int)this->level + v;
-    if ( br > toHigh )
-      br = wrap ? toLow : toHigh;
-    if ( br < toLow )
-      br = wrap ? toHigh : toLow;
-    brightness( br );
-  }
-  return this->level;
-}
-
-Atm_led& Atm_led::trigger( int event ) {
-  if ( event > ELSE ) {
-    brighten( event == EVT_BRUP ? 1 : -1 );
-  } else {
-    Machine::trigger( event );
-  }
-  return *this;
-}
-
-Atm_led& Atm_led::trace( Stream& stream ) {
-  setTrace( &stream, atm_serial_debug::trace,
-            "LED\0EVT_ON_TIMER\0EVT_OFF_TIMER\0EVT_WT_TIMER\0EVT_COUNTER\0EVT_ON\0EVT_OFF\0EVT_"
-            "BLINK\0EVT_TOGGLE\0EVT_TOGGLE_BLINK\0ELSE\0"
-            "IDLE\0ON\0START\0BLINK_OFF\0LOOP\0DONE\0OFF\0WT_ON\0WT_START" );
-  return *this;
-}

+ 0 - 56
HTequi-firmware/lib/Automaton-1.0.2/src/Atm_led.hpp

@@ -1,56 +0,0 @@
-#pragma once
-
-#include <Automaton.h>
-
-class Atm_led : public Machine {
- public:
-  enum { IDLE, ON, START, BLINK_OFF, LOOP, DONE, OFF, WT_ON, WT_START };
-  enum { EVT_ON_TIMER, EVT_OFF_TIMER, EVT_WT_TIMER, EVT_COUNTER, EVT_ON, EVT_OFF, EVT_BLINK, EVT_TOGGLE, EVT_TOGGLE_BLINK, ELSE, EVT_BRUP, EVT_BRDN }; // BRUP/BRDN pseudo
-  enum { EVT_START = EVT_BLINK };
-
-  Atm_led( void ) : Machine(){};
-  Atm_led& begin( int attached_pin, bool activeLow = false );
-  Atm_led& blink( void );
-  Atm_led& blink( uint32_t duration );
-  Atm_led& blink( uint32_t duration, uint32_t pause_duration, uint16_t repeat_count = ATM_COUNTER_OFF );
-  Atm_led& pwm( uint16_t width, float freq = -1 );
-  Atm_led& frequency( float freq ); 
-  Atm_led& pause( uint32_t duration );
-  Atm_led& fade( int fade );
-  Atm_led& lead( uint32_t ms );
-  Atm_led& repeat( uint16_t repeat );
-  int brightness( int level = -1 );
-  Atm_led& on( void );
-  Atm_led& off( void );
-  Atm_led& toggle( void );
-  Atm_led& toggleBlink( void );
-  Atm_led& start( void );
-  Atm_led& trace( Stream& stream );
-  Atm_led& onFinish( Machine& machine, int event = 0 );
-  Atm_led& onFinish( atm_cb_push_t callback, int idx = 0 );
-  Atm_led& range( int toLow, int toHigh, bool wrap = false );
-  Atm_led& levels( unsigned char* map, int mapsize, bool wrap = false );
-  int brighten( int v = 1 );
-  Atm_led& trigger( int event );
-
- private:
-  enum { ENT_INIT, ENT_ON, ENT_OFF, EXT_CHAIN };
-  uint8_t level;
-  short pin;
-  bool activeLow;
-  uint8_t toHigh, toLow;
-  bool wrap;
-  uint16_t repeat_count;
-  uint16_t width;
-  float freq;
-  atm_timer_millis on_timer, off_timer, lead_timer;
-  atm_counter counter;
-  atm_connector onfinish;
-  unsigned char* levelMap;
-  int levelMapSize;  
-  int mapLevel( int level );
-
-
-  int event( int id );
-  void action( int id );
-};

+ 0 - 242
HTequi-firmware/lib/Automaton-1.0.2/src/Atm_player.cpp

@@ -1,242 +0,0 @@
-#include "Atm_player.hpp"
-
-/* Add optional parameters for the state machine to begin()
- * Add extra initialization code
- */
-
-Atm_player& Atm_player::begin( int pin /* = - 1 */ ) {
-  // clang-format off
-  const static state_t state_table[] PROGMEM = {
-    /*             ON_ENTER    ON_LOOP  ON_EXIT  EVT_START  EVT_STOP  EVT_TOGGLE  EVT_TIMER  EVT_EOPAT  EVT_REPEAT   ELSE */
-    /*   IDLE */   ENT_IDLE, ATM_SLEEP,      -1,     START,       -1,      START,        -1,        -1,         -1,    -1,
-    /*  START */  ENT_START,        -1,      -1,        -1,       -1,         -1,        -1,        -1,         -1, SOUND,
-    /*  SOUND */  ENT_SOUND,        -1,      -1,        -1,     IDLE,       IDLE,     QUIET,        -1,         -1,    -1,
-    /*  QUIET */  ENT_QUIET,        -1,      -1,        -1,     IDLE,       IDLE,      NEXT,        -1,         -1,    -1,
-    /*   NEXT */   ENT_NEXT,        -1,      -1,        -1,     IDLE,       IDLE,        -1,    REPEAT,         -1, SOUND,
-    /* REPEAT */ ENT_REPEAT,        -1,      -1,        -1,     IDLE,       IDLE,        -1,        -1,     FINISH, START,
-    /* FINISH */ ENT_FINISH,        -1,      -1,        -1,     IDLE,         -1,        -1,        -1,       IDLE, START,
-  };
-  // clang-format on
-  Machine::begin( state_table, ELSE );
-  Atm_player::pin = pin;
-  speed( 100 );
-  pitch( 100 );
-  repeat( 1 );
-  play( 880, 50 );
-  return *this;
-}
-
-/* Add C++ code for each event (input)
- * The code must return 1 if the event should be triggered
- */
-
-int Atm_player::event( int id ) {
-  switch ( id ) {
-    case EVT_START:
-      return 0;
-    case EVT_TIMER:
-      return timer.expired( this );
-    case EVT_EOPAT:
-      if ( patternwidth == 32 ) {
-        return ( step * 3 * sizeof( uint32_t ) ) >= patternsize;
-      } else {
-        return ( step * 3 * sizeof( int ) ) >= patternsize;
-      }
-    case EVT_REPEAT:
-      return counter_repeat.expired();
-  }
-  return 0;
-}
-
-/* Add C++ code for each action
- * This generates the 'output' for the state machine
- */
-
-void Atm_player::action( int id ) {
-  switch ( id ) {
-    case ENT_FINISH:
-      push( connectors, ON_FINISH, 0, 0, 0 );
-      return;
-    case ENT_IDLE:
-#ifndef ATM_PLAYER_DISABLE_TONE    
-      if ( pin >= 0 ) noTone( pin );  // Tone takes up 7 bytes extra SRAM
-#endif
-      counter_repeat.set( repeatCount );
-      return;
-    case ENT_START:
-      step = 0;
-      counter_repeat.decrement();
-      return;
-    case ENT_SOUND:
-      if ( patternwidth == 32 ) {
-        uint32_t v = pattern32[step * 3] * (uint32_t)pitchFactor;
-        push( connectors, ON_NOTE, true, v & 0xFFFF, v >> 16 & 0xFFFF );
-#ifndef ATM_PLAYER_DISABLE_TONE    
-        if ( pin >= 0 ) tone( pin, pattern32[step * 3] * pitchFactor );
-#endif
-        timer.set( pattern32[step * 3 + 1] * speedFactor );
-      } else {
-        push( connectors, ON_NOTE, true, pattern16[step * 3] * pitchFactor, 1 );
-#ifndef ATM_PLAYER_DISABLE_TONE    
-        if ( pin >= 0 ) tone( pin, pattern16[step * 3] * pitchFactor );
-#endif
-        timer.set( pattern16[step * 3 + 1] * speedFactor );        
-      }
-      return;
-    case ENT_QUIET:
-      if ( patternwidth == 32 ) {
-        uint32_t v = pattern32[step * 3] * (uint32_t)pitchFactor;
-        push( connectors, ON_NOTE, false, v & 0xFFFF, v >> 16 & 0xFFFF );
-#ifndef ATM_PLAYER_DISABLE_TONE    
-        if ( pin >= 0 ) noTone( pin );
-#endif        
-        timer.set( pattern32[step * 3 + 2] * speedFactor );
-      } else {
-        push( connectors, ON_NOTE, false, pattern16[step * 3] * pitchFactor, 0 );
-#ifndef ATM_PLAYER_DISABLE_TONE    
-        if ( pin >= 0 ) noTone( pin );
-#endif
-        timer.set( pattern16[step * 3 + 2] * speedFactor );
-      }
-      return;
-    case ENT_NEXT:
-      step++;
-      return;
-    case ENT_REPEAT:
-      return;
-  }
-}
-
-/* How many times to repeat the pattern
- *
- */
-
-Atm_player& Atm_player::repeat( uint16_t v /* = -1 */) {
-  counter_repeat.set( repeatCount = v );
-  return *this;
-}
-
-Atm_player& Atm_player::speed( float v ) {
-  speedFactor = 100 / v;
-  return *this;
-}
-
-Atm_player& Atm_player::pitch( float v ) {
-  pitchFactor = v / 100;
-  return *this;
-}
-
-Atm_player& Atm_player::start( void ) {
-  trigger( EVT_START );
-  return *this;
-}
-
-Atm_player& Atm_player::stop( void ) {
-  trigger( EVT_STOP );
-  return *this;
-}
-
-Atm_player& Atm_player::toggle( void ) {
-  trigger( EVT_TOGGLE );
-  return *this;
-}
-
-/* Sets the pattern and pattern length (in bytes)
- *
- */
-
-Atm_player& Atm_player::play( int* pat, int patsize ) {
-  patternwidth = 16;
-  pattern16 = pat;
-  patternsize = patsize;
-  counter_repeat.set( repeatCount );
-  step = 0;
-  return *this;
-}
-
-Atm_player& Atm_player::play( uint32_t* pat, int patsize ) {
-  patternwidth = 32;
-  pattern32 = pat;
-  patternsize = patsize;
-  counter_repeat.set( repeatCount );
-  step = 0;
-  return *this;
-}
-
-Atm_player& Atm_player::play( int freq, int period, int pause /* = 0 */ ) {
-  patternwidth = 16;
-  stub[0] = freq;
-  stub[1] = period;
-  stub[2] = pause;
-  pattern16 = stub;
-  patternsize = 3 * sizeof( int );
-  step = 0;
-  return *this;
-}
-
-/* Optionally override the default trigger() method
- * Control what triggers your machine can and cannot process
- */
-
-Atm_player& Atm_player::trigger( int event ) {
-  Machine::trigger( event );
-  return *this;
-}
-
-/* Optionally override the default state() method
- * Control what the machine returns when another process requests its state()
- */
-
-int Atm_player::state( void ) {
-  return Machine::state();
-}
-
-/* Nothing customizable below this line
- ************************************************************************************************
-*/
-
-/* onFinish() push connector variants ( slots 1, autostore 0, broadcast 0 )
- *
- * Usage in action() handler: push( connectors, ON_FINISH, 0, v, up)
- */
-
-Atm_player& Atm_player::onFinish( Machine& machine, int event ) {
-  onPush( connectors, ON_FINISH, 0, 1, 1, machine, event );
-  return *this;
-}
-Atm_player& Atm_player::onFinish( atm_cb_push_t callback, int idx ) {
-  onPush( connectors, ON_FINISH, 0, 1, 1, callback, idx );
-  return *this;
-}
-
-/* onNote() push connector variants ( slots 2, autostore 0, broadcast 0 )
- *
- * Usage in action() handler: push( connectors, ON_NOTE, sub, v, up)
- */
-
-Atm_player& Atm_player::onNote( Machine& machine, int event ) {
-  onPush( connectors, ON_NOTE, 0, 2, 1, machine, event );
-  return *this;
-}
-Atm_player& Atm_player::onNote( atm_cb_push_t callback, int idx ) {
-  onPush( connectors, ON_NOTE, 0, 2, 1, callback, idx );
-  return *this;
-}
-Atm_player& Atm_player::onNote( int sub, Machine& machine, int event ) {
-  onPush( connectors, ON_NOTE, sub, 2, 0, machine, event );
-  return *this;
-}
-Atm_player& Atm_player::onNote( int sub, atm_cb_push_t callback, int idx ) {
-  onPush( connectors, ON_NOTE, sub, 2, 0, callback, idx );
-  return *this;
-}
-
-/* State trace method
- * Sets the symbol table and the default logging method for serial monitoring
- */
-
-Atm_player& Atm_player::trace( Stream& stream ) {
-  Machine::setTrace( &stream, atm_serial_debug::trace,
-                     "PLAYER\0EVT_START\0EVT_STOP\0EVT_TOGGLE\0EVT_TIMER\0EVT_EOPAT\0EVT_REPEAT\0ELSE\0IDLE\0START\0SOUND\0QUIET\0NEXT\0REPEAT\0FINISH" );
-  return *this;
-}

+ 0 - 112
HTequi-firmware/lib/Automaton-1.0.2/src/Atm_player.hpp

@@ -1,112 +0,0 @@
-#pragma once
-
-#include <Automaton.h>
-
-#if defined( _VARIANT_ARDUINO_DUE_X ) || defined( ARDUINO_FEATHER52 ) 
-#define ATM_PLAYER_DISABLE_TONE
-#endif
-
-class Atm_player : public Machine {
- public:
-  enum { IDLE, START, SOUND, QUIET, NEXT, REPEAT, FINISH };                          // STATES
-  enum { EVT_START, EVT_STOP, EVT_TOGGLE, EVT_TIMER, EVT_EOPAT, EVT_REPEAT, ELSE };  // EVENTS
-  Atm_player( void ) : Machine(){};
-  Atm_player& begin( int pin = -1 );
-  Atm_player& trace( Stream& stream );
-  Atm_player& trigger( int event );
-  int state( void );
-  Atm_player& play( int* pat, int size );
-  Atm_player& play( uint32_t* pat, int size );
-  Atm_player& play( int freq, int period, int pause = 0 );
-  Atm_player& repeat( uint16_t v = -1 );
-  Atm_player& speed( float v );
-  Atm_player& pitch( float v );
-  Atm_player& start( void );
-  Atm_player& stop( void );
-  Atm_player& toggle( void );
-  Atm_player& onFinish( Machine& machine, int event = 0 );
-  Atm_player& onFinish( atm_cb_push_t callback, int idx = 0 );
-  Atm_player& onNote( Machine& machine, int event = 0 );
-  Atm_player& onNote( atm_cb_push_t callback, int idx = 0 );
-  Atm_player& onNote( int sub, Machine& machine, int event = 0 );
-  Atm_player& onNote( int sub, atm_cb_push_t callback, int idx = 0 );
-
- private:
-  int pin;
-  int* pattern16; // Can also be 32 bit on some hardware (teensy 3.x)
-  uint32_t* pattern32;
-  uint16_t patternsize;
-  uint8_t patternwidth;
-  int step;
-  uint16_t repeatCount;
-  float speedFactor, pitchFactor;
-  int stub[3];
-  atm_timer_millis timer;
-  atm_counter counter_repeat;
-  enum { ENT_IDLE, ENT_START, ENT_SOUND, ENT_QUIET, ENT_NEXT, ENT_REPEAT, ENT_FINISH };  // ACTIONS
-  enum { ON_FINISH, ON_NOTE, CONN_MAX = 3 };                                             // CONNECTORS
-  atm_connector connectors[CONN_MAX];
-  int event( int id );
-  void action( int id );
-};
-
-/*
-Automaton::ATML::begin - Automaton Markup Language
-
-<?xml version="1.0" encoding="UTF-8"?>
-<machines>
-  <machine name="Atm_player">
-    <states>
-      <IDLE index="0" sleep="1" on_enter="ENT_IDLE">
-        <EVT_START>START</EVT_START>
-        <EVT_TOGGLE>START</EVT_TOGGLE>
-      </IDLE>
-      <START index="1" on_enter="ENT_START">
-        <ELSE>SOUND</ELSE>
-      </START>
-      <SOUND index="2" on_enter="ENT_SOUND">
-        <EVT_STOP>IDLE</EVT_STOP>
-        <EVT_TOGGLE>IDLE</EVT_TOGGLE>
-        <EVT_TIMER>QUIET</EVT_TIMER>
-      </SOUND>
-      <QUIET index="3" on_enter="ENT_QUIET">
-        <EVT_STOP>IDLE</EVT_STOP>
-        <EVT_TOGGLE>IDLE</EVT_TOGGLE>
-        <EVT_TIMER>NEXT</EVT_TIMER>
-      </QUIET>
-      <NEXT index="4" on_enter="ENT_NEXT">
-        <EVT_STOP>IDLE</EVT_STOP>
-        <EVT_TOGGLE>IDLE</EVT_TOGGLE>
-        <EVT_EOPAT>REPEAT</EVT_EOPAT>
-        <ELSE>SOUND</ELSE>
-      </NEXT>
-      <REPEAT index="5" on_enter="ENT_REPEAT">
-        <EVT_STOP>IDLE</EVT_STOP>
-        <EVT_TOGGLE>IDLE</EVT_TOGGLE>
-        <EVT_REPEAT>FINISH</EVT_REPEAT>
-        <ELSE>START</ELSE>
-      </REPEAT>
-      <FINISH index="6" on_enter="ENT_FINISH">
-        <EVT_REPEAT>IDLE</EVT_REPEAT>
-        <ELSE>START</ELSE>
-      </FINISH>
-    </states>
-    <events>
-      <EVT_START index="0"/>
-      <EVT_STOP index="1"/>
-      <EVT_TOGGLE index="2"/>
-      <EVT_TIMER index="3"/>
-      <EVT_EOPAT index="4"/>
-      <EVT_REPEAT index="5"/>
-    </events>
-    <connectors>
-      <FINISH autostore="0" broadcast="0" dir="PUSH" slots="1"/>
-      <NOTE autostore="0" broadcast="0" dir="PUSH" slots="2"/>
-    </connectors>
-    <methods>
-    </methods>
-  </machine>
-</machines>
-
-Automaton::ATML::end
-*/

HTequi-firmware/lib/Automaton-1.0.2/.clang-format → HTequi-firmware/lib/Automaton-master/.clang-format


HTequi-firmware/lib/Automaton-1.0.2/.gitattributes → HTequi-firmware/lib/Automaton-master/.gitattributes


HTequi-firmware/lib/Automaton-1.0.2/.gitignore → HTequi-firmware/lib/Automaton-master/.gitignore


HTequi-firmware/lib/Automaton-1.0.2/LICENSE → HTequi-firmware/lib/Automaton-master/LICENSE


HTequi-firmware/lib/Automaton-1.0.2/README.md → HTequi-firmware/lib/Automaton-master/README.md


HTequi-firmware/lib/Automaton-1.0.2/examples/blink/blink.ino → HTequi-firmware/lib/Automaton-master/examples/blink/blink.ino





HTequi-firmware/lib/Automaton-1.0.2/examples/button/button.ino → HTequi-firmware/lib/Automaton-master/examples/button/button.ino


+ 5 - 0
HTequi-firmware/lib/Automaton-master/examples/extern_LED/.vscode/settings.json

@@ -0,0 +1,5 @@
+{
+    "terminal.integrated.env.linux": {
+        "PATH": "/home/ian/.platformio/penv/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin:/usr/games:/usr/local/games:/snap/bin"
+    }
+}

+ 36 - 0
HTequi-firmware/lib/Automaton-master/examples/extern_LED/Atm_led_mcp.cpp

@@ -0,0 +1,36 @@
+/*
+ * Atm_led_mcp.cpp
+ *
+ *  Created on: 09.12.2017
+ *      Author: ian
+ */
+
+#include <Atm_led_mcp.h>
+#include <Atm_led.hpp>
+
+Atm_led_mcp::Atm_led_mcp(Adafruit_MCP23017& _gpio):
+	Atm_led(),			// base class ctor would also be called implicitly, but better style to mention this explicitly :)
+	gpio(_gpio) {
+	// nothing to see here
+}
+
+void Atm_led_mcp::initLED() {
+	gpio.pinMode(pin, OUTPUT);
+	gpio.digitalWrite(pin, activeLow ? HIGH : LOW);
+	Serial.printf("LED init on pin %c%x\n", activeLow?'~':' ', pin);
+}
+
+void Atm_led_mcp::switchOn() {
+	Serial.printf("LED ON on pin %c%x\n", activeLow?'~':' ', pin);
+	gpio.digitalWrite(pin, !activeLow);
+}
+
+void Atm_led_mcp::switchOff() {
+	Serial.printf("LED OFF on pin %c%x\n", activeLow?'~':' ', pin);
+	gpio.digitalWrite(pin, activeLow);
+}
+
+void Atm_led_mcp::setBrightness(int value) {
+	if (value == toHigh) switchOn(); else if(value==toLow) switchOff(); else
+	Serial.printf("ERROR: Setting brightness on GPIO expander is not possible (pin: %d)\n", pin);
+}

+ 28 - 0
HTequi-firmware/lib/Automaton-master/examples/extern_LED/Atm_led_mcp.h

@@ -0,0 +1,28 @@
+/*
+ * Atm_led_mcp.h
+ *
+ *  Created on: 09.12.2017
+ *      Author: ian
+ */
+
+#ifndef SRC_ATM_LED_MCP_H_
+#define SRC_ATM_LED_MCP_H_
+
+#include "Adafruit_MCP23017.h"
+#include <Atm_led.hpp>
+
+class Atm_led_mcp: public Atm_led {
+public:
+	Atm_led_mcp(Adafruit_MCP23017& _gpio);
+
+private:
+	Adafruit_MCP23017& gpio;
+
+protected:
+	virtual void initLED();
+	virtual void switchOn();
+	virtual void switchOff();
+    virtual void setBrightness(int value);
+};
+
+#endif /* SRC_ATM_LED_MCP_H_ */

+ 18 - 0
HTequi-firmware/lib/Automaton-master/examples/extern_LED/main.cpp

@@ -0,0 +1,18 @@
+#include <automaton.h>
+#include <atm_led_mcp.h>
+#include "Adafruit_MCP23017.h"
+
+
+Adafruit_MCP23017 ioext;
+Atm_led_mcp led(ioext);
+
+
+void setup() {
+  ioext.begin();
+  led.begin(6, true);
+  led.On();
+}
+
+void loop() {
+  automaton.run();
+}

HTequi-firmware/lib/Automaton-1.0.2/examples/fade/fade.ino → HTequi-firmware/lib/Automaton-master/examples/fade/fade.ino


HTequi-firmware/lib/Automaton-1.0.2/examples/frere_jacques/frere_jacques.ino → HTequi-firmware/lib/Automaton-master/examples/frere_jacques/frere_jacques.ino


HTequi-firmware/lib/Automaton-1.0.2/examples/frere_jacques/musical_notes.h → HTequi-firmware/lib/Automaton-master/examples/frere_jacques/musical_notes.h


HTequi-firmware/lib/Automaton-1.0.2/examples/knight_rider1/knight_rider1.ino → HTequi-firmware/lib/Automaton-master/examples/knight_rider1/knight_rider1.ino


HTequi-firmware/lib/Automaton-1.0.2/examples/knight_rider2/Atm_sweep.cpp → HTequi-firmware/lib/Automaton-master/examples/knight_rider2/Atm_sweep.cpp


HTequi-firmware/lib/Automaton-1.0.2/examples/knight_rider2/Atm_sweep.h → HTequi-firmware/lib/Automaton-master/examples/knight_rider2/Atm_sweep.h


HTequi-firmware/lib/Automaton-1.0.2/examples/knight_rider2/knight_rider2.ino → HTequi-firmware/lib/Automaton-master/examples/knight_rider2/knight_rider2.ino


HTequi-firmware/lib/Automaton-1.0.2/examples/knight_rider3/knight_rider3.ino → HTequi-firmware/lib/Automaton-master/examples/knight_rider3/knight_rider3.ino


HTequi-firmware/lib/Automaton-1.0.2/examples/led_fuel_gauge/led_fuel_gauge.ino → HTequi-firmware/lib/Automaton-master/examples/led_fuel_gauge/led_fuel_gauge.ino


HTequi-firmware/lib/Automaton-1.0.2/examples/led_test/led_test.ino → HTequi-firmware/lib/Automaton-master/examples/led_test/led_test.ino


HTequi-firmware/lib/Automaton-1.0.2/examples/nuclear_missile_launcher/nuclear_missile_launcher.ino → HTequi-firmware/lib/Automaton-master/examples/nuclear_missile_launcher/nuclear_missile_launcher.ino


HTequi-firmware/lib/Automaton-1.0.2/examples/sos1/sos1.ino → HTequi-firmware/lib/Automaton-master/examples/sos1/sos1.ino


HTequi-firmware/lib/Automaton-1.0.2/examples/sos2/sos2.ino → HTequi-firmware/lib/Automaton-master/examples/sos2/sos2.ino


HTequi-firmware/lib/Automaton-1.0.2/examples/sos3/sos3.ino → HTequi-firmware/lib/Automaton-master/examples/sos3/sos3.ino


HTequi-firmware/lib/Automaton-1.0.2/extras/update.sh → HTequi-firmware/lib/Automaton-master/extras/update.sh


HTequi-firmware/lib/Automaton-1.0.2/keywords.txt → HTequi-firmware/lib/Automaton-master/keywords.txt


+ 1 - 1
HTequi-firmware/lib/Automaton-1.0.2/library.json

@@ -1,6 +1,6 @@
 {
     "name": "Automaton",
-    "version": "1.0.2",
+    "version": "1.0.3",
     "keywords": "state, machine",
     "description": "A multi tasking table driven finite state machine framework for Arduino",
     "authors":

+ 1 - 1
HTequi-firmware/lib/Automaton-1.0.2/library.properties

@@ -1,5 +1,5 @@
 name=Automaton
-version=1.0.2
+version=1.0.3
 author=Tinkerspy <tinkerspy@myown.mailcan.com>
 maintainer=Tinkerspy <tinkerspy@myown.mailcan.com>
 sentence=A multi tasking table driven finite state machine framework

+ 54 - 0
HTequi-firmware/lib/Automaton-master/platformio.ini

@@ -0,0 +1,54 @@
+; PlatformIO Project Configuration File
+;
+;   Build options: build flags, source filter
+;   Upload options: custom upload port, speed and extra flags
+;   Library options: dependencies, extra library storages
+;   Advanced options: extra scripting
+;
+; Please visit documentation for the other options and examples
+; http://docs.platformio.org/page/projectconf.html
+
+[platformio]
+
+#env_default = esp12e
+env_default = d1_mini
+
+[common_env_data]
+
+# Automaton + PR #50 (https://github.com/tinkerspy/Automaton/pull/50)
+# Automaton-Esp8266 + PR #5 (https://github.com/tinkerspy/Automaton-Esp8266/pull/5)
+# 334 = Adafruit MCP23017 Arduino Library
+lib_deps_ext =	https://github.com/euphi/Automaton.git
+		https://github.com/euphi/Automaton-Esp8266.git
+		MFRC522
+		Adafruit MCP23017 Arduino Library
+
+lib_deps_int = Hash
+
+[env:esp12e]
+platform = espressif8266
+board = d1_mini
+framework = arduino
+
+lib_ldf_mode=chain+
+lib_deps= ${common_env_data.lib_deps_int}
+	  ${common_env_data.lib_deps_ext}
+
+# Build for ESP-ADC (in-circuit), 512kb Flash only!
+build_flags = -Wl,-Tesp8266.flash.512k64.ld                                                                                                                                                                                                                                    
+
+# ck: DTR connected to GPIO0, RTS connected to RESET
+upload_resetmethod = ck
+upload_speed = 460800
+
+
+[env:d1_mini]
+platform = espressif8266
+board = d1_mini
+framework = arduino
+
+lib_ldf_mode=chain+
+lib_deps= ${common_env_data.lib_deps_int}
+	  ${common_env_data.lib_deps_ext}
+
+upload_speed = 460800

HTequi-firmware/lib/Automaton-1.0.2/src/Atm_analog.cpp → HTequi-firmware/lib/Automaton-master/src/Atm_analog.cpp


HTequi-firmware/lib/Automaton-1.0.2/src/Atm_analog.hpp → HTequi-firmware/lib/Automaton-master/src/Atm_analog.hpp


HTequi-firmware/lib/Automaton-1.0.2/src/Atm_bit.cpp → HTequi-firmware/lib/Automaton-master/src/Atm_bit.cpp


HTequi-firmware/lib/Automaton-1.0.2/src/Atm_bit.hpp → HTequi-firmware/lib/Automaton-master/src/Atm_bit.hpp


+ 15 - 3
HTequi-firmware/lib/Automaton-1.0.2/src/Atm_button.cpp

@@ -28,7 +28,7 @@ Atm_button& Atm_button::begin( int attached_pin ) {
   timer_delay.set( ATM_TIMER_OFF );
   timer_repeat.set( ATM_TIMER_OFF );
   timer_auto.set( ATM_TIMER_OFF );
-  pinMode( pin, INPUT_PULLUP );
+  initButton();
   return *this;
 }
 
@@ -45,9 +45,9 @@ int Atm_button::event( int id ) {
     case EVT_AUTO:
       return timer_auto.expired( this );
     case EVT_PRESS:
-      return !digitalRead( pin );
+    	return isPressed();
     case EVT_RELEASE:
-      return digitalRead( pin );
+    	return isReleased();
     case EVT_COUNTER:
       return counter_longpress.expired();
   }
@@ -151,3 +151,15 @@ Atm_button& Atm_button::trace( Stream& stream ) {
             "AUTO_ST\0ELSE\0IDLE\0WAIT\0PRESSED\0REPEAT\0RELEASE\0LIDLE\0LWAIT\0LPRESSED\0LRELEASE\0WRELEASE\0AUTO" );
   return *this;
 }
+
+void Atm_button::initButton() {
+	pinMode( pin, INPUT_PULLUP );
+}
+
+bool Atm_button::isPressed() {
+	return !digitalRead( pin );
+}
+
+bool Atm_button::isReleased() {
+	return digitalRead( pin );
+}

+ 4 - 0
HTequi-firmware/lib/Automaton-1.0.2/src/Atm_button.hpp

@@ -35,4 +35,8 @@ class Atm_button : public Machine {
 
   int event( int id );
   void action( int id );
+
+  virtual void initButton();
+  virtual bool isPressed();
+  virtual bool isReleased();
 };

HTequi-firmware/lib/Automaton-1.0.2/src/Atm_command.cpp → HTequi-firmware/lib/Automaton-master/src/Atm_command.cpp


HTequi-firmware/lib/Automaton-1.0.2/src/Atm_command.hpp → HTequi-firmware/lib/Automaton-master/src/Atm_command.hpp


HTequi-firmware/lib/Automaton-1.0.2/src/Atm_comparator.cpp → HTequi-firmware/lib/Automaton-master/src/Atm_comparator.cpp


HTequi-firmware/lib/Automaton-1.0.2/src/Atm_comparator.hpp → HTequi-firmware/lib/Automaton-master/src/Atm_comparator.hpp


HTequi-firmware/lib/Automaton-1.0.2/src/Atm_controller.cpp → HTequi-firmware/lib/Automaton-master/src/Atm_controller.cpp


HTequi-firmware/lib/Automaton-1.0.2/src/Atm_controller.hpp → HTequi-firmware/lib/Automaton-master/src/Atm_controller.hpp


HTequi-firmware/lib/Automaton-1.0.2/src/Atm_digital.cpp → HTequi-firmware/lib/Automaton-master/src/Atm_digital.cpp


HTequi-firmware/lib/Automaton-1.0.2/src/Atm_digital.hpp → HTequi-firmware/lib/Automaton-master/src/Atm_digital.hpp


HTequi-firmware/lib/Automaton-1.0.2/src/Atm_encoder.cpp → HTequi-firmware/lib/Automaton-master/src/Atm_encoder.cpp


HTequi-firmware/lib/Automaton-1.0.2/src/Atm_encoder.hpp → HTequi-firmware/lib/Automaton-master/src/Atm_encoder.hpp


HTequi-firmware/lib/Automaton-1.0.2/src/Atm_fan.cpp → HTequi-firmware/lib/Automaton-master/src/Atm_fan.cpp


HTequi-firmware/lib/Automaton-1.0.2/src/Atm_fan.hpp → HTequi-firmware/lib/Automaton-master/src/Atm_fan.hpp


HTequi-firmware/lib/Automaton-1.0.2/src/Atm_step.cpp → HTequi-firmware/lib/Automaton-master/src/Atm_step.cpp


HTequi-firmware/lib/Automaton-1.0.2/src/Atm_step.hpp → HTequi-firmware/lib/Automaton-master/src/Atm_step.hpp


HTequi-firmware/lib/Automaton-1.0.2/src/Atm_timer.cpp → HTequi-firmware/lib/Automaton-master/src/Atm_timer.cpp


HTequi-firmware/lib/Automaton-1.0.2/src/Atm_timer.hpp → HTequi-firmware/lib/Automaton-master/src/Atm_timer.hpp


HTequi-firmware/lib/Automaton-1.0.2/src/Automaton.cpp → HTequi-firmware/lib/Automaton-master/src/Automaton.cpp


+ 3 - 3
HTequi-firmware/lib/Automaton-1.0.2/src/Automaton.h

@@ -91,9 +91,9 @@ class Factory : Automaton {
 #include <Atm_controller.hpp>
 #include <Atm_digital.hpp>
 #include <Atm_encoder.hpp>
-#include <Atm_fade.hpp>
-#include <Atm_led.hpp>
+//#include <Atm_fade.hpp>
+//#include <Atm_led.hpp>
 #include <Atm_fan.hpp>
 #include <Atm_step.hpp>
-#include <Atm_player.hpp>
+//#include <Atm_player.hpp>
 #include <Atm_timer.hpp>

HTequi-firmware/lib/Automaton-1.0.2/src/Machine.cpp → HTequi-firmware/lib/Automaton-master/src/Machine.cpp


HTequi-firmware/lib/Automaton-1.0.2/src/Machine.hpp → HTequi-firmware/lib/Automaton-master/src/Machine.hpp


HTequi-firmware/lib/Automaton-1.0.2/src/atm_connector.cpp → HTequi-firmware/lib/Automaton-master/src/atm_connector.cpp


HTequi-firmware/lib/Automaton-1.0.2/src/atm_connector.hpp → HTequi-firmware/lib/Automaton-master/src/atm_connector.hpp


HTequi-firmware/lib/Automaton-1.0.2/src/atm_counter.cpp → HTequi-firmware/lib/Automaton-master/src/atm_counter.cpp


HTequi-firmware/lib/Automaton-1.0.2/src/atm_counter.hpp → HTequi-firmware/lib/Automaton-master/src/atm_counter.hpp


HTequi-firmware/lib/Automaton-1.0.2/src/atm_serial_debug.hpp → HTequi-firmware/lib/Automaton-master/src/atm_serial_debug.hpp


+ 8 - 0
HTequi-firmware/lib/Automaton-1.0.2/src/atm_timer_millis.cpp

@@ -11,6 +11,14 @@ void atm_timer_millis::set( uint32_t v ) {
 }
 
 /*
+ * atm_timer_millis::setFromNow( Machine* machine, uint32_t v ) -
+ * Sets the timer to be expired v millis from now
+ */
+void atm_timer_millis::setFromNow( Machine* machine, uint32_t v ) {
+  value = millis() - machine->state_millis + v;
+}
+
+/*
  * atm_timer_millis::expired( this ) - Checks a millis timer for expiry (== 0)
  * This is a rollover-safe 32 bit unsigned integer comparison
  *

+ 1 - 0
HTequi-firmware/lib/Automaton-1.0.2/src/atm_timer_millis.hpp

@@ -11,5 +11,6 @@ class atm_timer_millis {
  public:
   uint32_t value;
   void set( uint32_t v );
+  void setFromNow( Machine* machine, uint32_t v );
   int expired( Machine* machine );
 };

+ 129 - 91
HTequi-firmware/src/blobcnc_low/main.cpp

@@ -3,8 +3,9 @@
 
 //Automaton and custom machines
 #include <Automaton.h>
-#include "Atm_Teenstep.h"
-#include "Atm_Teenstep_OSC.h"
+// #include "Atm_Teenstep.h"
+// #include "Atm_Teenstep_OSC.h"
+#include "Atm_AccelStepper.h"
 
 //osc
 #include <SPI.h>
@@ -14,6 +15,7 @@
 #include <OSCBundle.h>
 
 
+
 // Enter a MAC address and IP address for your controller below.
 // The IP address will be dependent on your local network:
 byte mac[] = {
@@ -23,10 +25,10 @@ byte mac[] = {
 IPAddress ip(192, 168, 1, 104);    //local IP of Arduino/Teensy
 unsigned int localPort = 8000;      // local port to listen on (not needed for multicast)
 
-IPAddress outIp(192, 168, 1, 102);
-const unsigned int outPort = 9000;
-// IPAddress ipMulti(239, 200, 200, 200); // ipMidi Multicast address
-// unsigned int portMulti = 9977;   // ipMidi Mutlicast port 1
+// IPAddress outIp(192, 168, 1, 102);
+// const unsigned int outPort = 9000;
+IPAddress ipMulti(239, 200, 200, 200); // ipMidi Multicast address
+unsigned int portMulti = 9977;   // ipMidi Mutlicast port 1
 
 
 // buffers for receiving and sending data
@@ -41,37 +43,42 @@ EthernetUDP Udp;
 OSCBundle bndl;
 
 /////////////////// STEPPER machines  ////////////////////////
-const int HIGH_SPEED = 800 ;
+const int HIGH_SPEED = 1200 ;
 const int HIGH_ACC = 800 ;
 const int LOW_SPEED = 100 ;
 const int LOW_ACC = 1000 ;
 
-Atm_Teenstep_OSC A_low_OSC;
-Atm_Teenstep A_low_step;
-Stepper A_low_stepper(8 , 7);
-StepControl A_low_controller ;
+// Atm_Teenstep_OSC A_low_OSC;
+// Atm_Teenstep A_low_step;
+// Stepper A_low_stepper(8 , 7);
+// StepControl A_low_controller ;
+Atm_AccelStepper A_low_step ;
+
+// Atm_Teenstep_OSC B_low_OSC;
+// Atm_Teenstep B_low_step;
+// Stepper B_low_stepper(32 , 31);
+// StepControl B_low_controller ;
+Atm_AccelStepper B_low_step ;
 
-Atm_Teenstep_OSC B_low_OSC;
-Atm_Teenstep B_low_step;
-Stepper B_low_stepper(32 , 31);
-StepControl B_low_controller ;
+// Atm_Teenstep_OSC TrBr_low_OSC;
+// Atm_Teenstep TrBr_low_step;
+// Stepper TrBr_low_stepper(2 , 1);
+// StepControl TrBr_low_controller ;
+Atm_AccelStepper TrBr_low_step ;
 
-Atm_Teenstep_OSC TrBr_low_OSC;
-Atm_Teenstep TrBr_low_step;
-Stepper TrBr_low_stepper(2 , 1);
-StepControl TrBr_low_controller ;
+// Atm_Teenstep_OSC LevBr_low_OSC;
+// Atm_Teenstep LevBr_low_step;
+// Stepper LevBr_low_stepper(5 , 4);
+// StepControl LevBr_low_controller ;
+Atm_AccelStepper LevBr_low_step ;
 
-Atm_Teenstep_OSC LevBr_low_OSC;
-Atm_Teenstep LevBr_low_step;
-Stepper LevBr_low_stepper(5 , 4);
-StepControl LevBr_low_controller ;
 
 
 /////////////////// Servo and pump  ////////////////////////
 
-const int filling_pwm_pin = 14;
-const int brush_pwm_pin = 20;
-const int crusher_pin = 16;
+const int filling_pwm_pin = 20;
+const int brush_pwm_pin = 14;
+const int crusher_pin = 20;
 
 
 void motorsOSC(OSCMessage &msg){
@@ -83,82 +90,87 @@ void motorsOSC(OSCMessage &msg){
 //////////////  Setup  /////////////////////
 
 void setup() {
- SPI.setSCK(27);
- Ethernet.init(10);//(10)
- teensyMAC(mac);
- Ethernet.begin(mac, ip);              // start the Ethernet and UDP:
- // Udp.beginMulticast(ipMulti, portMulti); // for modified Teensy Ethernet library
-  Udp.begin(localPort);
+ delay(2000);
 
  //  SERIAL
  Serial.begin(115200); // higher Baud rate for faster refresh, CHANGE IN SERIAL MONITOR
- delay(2000);
+ //delay(2000);
  Serial.println("Started Low");
 
+ // ETHERNET
+ SPI.setSCK(27);
+ Ethernet.init(10);//(10)
+ teensyMAC(mac);
+
+ Ethernet.begin(mac);              // start the Ethernet and UDP:
+ Udp.beginMulticast(ipMulti, portMulti); // for modified Teensy Ethernet library
+  // Udp.begin(localPort);
+
+Serial.print("Started ethernet, IP is ");
+Serial.println(Ethernet.localIP());
+
  //  STEPPERS
- A_low_step.trace( Serial );
- A_low_step.begin(A_low_stepper, A_low_controller)
-           .setEnablePin(6)//.enableReversed(1);//.enable(1);
-           .setLimitType(3).setLimitPins(A16).limitReversed(false).limitThresholds(600, 750, 950, 1200);
- A_low_stepper.setMaxSpeed(HIGH_SPEED);
- A_low_stepper.setAcceleration(HIGH_ACC);
- A_low_stepper.setInverseRotation(true);
- A_low_OSC.begin(A_low_step, Udp, bndl, "/A_low");
- A_low_step.onChangeposition([](int idx, int v, int up){bndl.add("/A_low_OSC/step").add(v);});
- A_low_step.onChange([](int idx, int v, int up){bndl.add("/A_low_OSC/state").add(v);});
- A_low_step.onLimitlow([](int idx, int v, int up){bndl.add("/A_low_OSC/limitLow").add(v);});
- A_low_step.onLimithigh([](int idx, int v, int up){bndl.add("/A_low_OSC/limitHigh").add(v);});
-
- B_low_step.trace( Serial );
- B_low_step.begin(B_low_stepper, B_low_controller)
-           .setEnablePin(30)//.enableReversed(1);//.enable(1);
-           .setLimitType(3).setLimitPins(A14).limitReversed(false).limitThresholds(600, 750, 950, 1200);
- B_low_stepper.setMaxSpeed(HIGH_SPEED);
- B_low_stepper.setAcceleration(HIGH_ACC);
- B_low_stepper.setInverseRotation(true);
- B_low_OSC.begin(B_low_step, Udp, bndl, "/B_low");
- B_low_step.onChangeposition([](int idx, int v, int up){bndl.add("/B_low_OSC/step").add(v);});
- B_low_step.onChange([](int idx, int v, int up){bndl.add("/B_low_OSC/state").add(v);});
- B_low_step.onLimitlow([](int idx, int v, int up){bndl.add("/B_low_OSC/limitLow").add(v);});
- B_low_step.onLimithigh([](int idx, int v, int up){bndl.add("/B_low_OSC/limitHigh").add(v);});
-
- TrBr_low_step.trace( Serial );
- TrBr_low_step.begin(TrBr_low_stepper, TrBr_low_controller)
-           .setEnablePin(0)//.enableReversed(1);//.enable(1);
-           .setLimitType(2).setLimitPins(17, 15).limitReversed(true).limitThresholds(600, 750, 950, 1200);
- TrBr_low_stepper.setMaxSpeed(HIGH_SPEED);
- TrBr_low_stepper.setAcceleration(HIGH_ACC);
- TrBr_low_stepper.setInverseRotation(true);
- TrBr_low_OSC.begin(TrBr_low_step, Udp, bndl, "/TrBr_low");
- TrBr_low_step.onChangeposition([](int idx, int v, int up){bndl.add("/TrBr_low_OSC/step").add(v);});
- TrBr_low_step.onChange([](int idx, int v, int up){bndl.add("/TrBr_low_OSC/state").add(v);});
- TrBr_low_step.onLimitlow([](int idx, int v, int up){bndl.add("/TrBr_low_OSC/limitLow").add(v);});
- TrBr_low_step.onLimithigh([](int idx, int v, int up){bndl.add("/TrBr_low_OSC/limitHigh").add(v);});
-
- LevBr_low_step.trace( Serial );
- LevBr_low_step.begin(LevBr_low_stepper, LevBr_low_controller)
-           .setEnablePin(3)//.enableReversed(1);//.enable(1);
-           .setLimitType(2).setLimitPins(39, 37).limitReversed(true).limitThresholds(600, 750, 950, 1200);
- LevBr_low_stepper.setMaxSpeed(HIGH_SPEED);
- LevBr_low_stepper.setAcceleration(HIGH_ACC);
- LevBr_low_stepper.setInverseRotation(true);
- LevBr_low_OSC.begin(LevBr_low_step, Udp, bndl, "/LevBr_low");
- LevBr_low_step.onChangeposition([](int idx, int v, int up){bndl.add("/LevBr_low_OSC/step").add(v);});
- LevBr_low_step.onChange([](int idx, int v, int up){bndl.add("/LevBr_low_OSC/state").add(v);});
- LevBr_low_step.onLimitlow([](int idx, int v, int up){bndl.add("/LevBr_low_OSC/limitLow").add(v);});
- LevBr_low_step.onLimithigh([](int idx, int v, int up){bndl.add("/LevBr_low_OSC/limitHigh").add(v);});
+
+ A_low_step.begin(8, 7).setEnablePin(6).pinReversed(1)//.trace(Serial)
+  .limitLow_set(2, 37, 0).limitLow_setThresholds(600, 720).limitLow_isHard(0)
+  .limitHigh_set(2, 37, 0).limitHigh_setThresholds(900, 1000).limitHigh_isHard(0)
+  .setMaxSpeed(HIGH_SPEED).setAcceleration(HIGH_ACC);
+ A_low_step.onChangeposition([](int idx, int v, int up){bndl.add("/A_low/step").add(v).add(up);});
+ A_low_step.onChangestate([](int idx, int v, int up){bndl.add("/A_low/state").add(v);});
+ A_low_step.onOnlimitlow([](int idx, int v, int up){bndl.add("/A_low/limitLow").add(v);});
+ A_low_step.onOnlimithigh([](int idx, int v, int up){bndl.add("/A_low/limitHigh").add(v);});
+ A_low_step.onOntarget([](int idx, int v, int up){bndl.add("/A_low/onTarget").add(v);});
+
+ B_low_step.begin(32 , 31).setEnablePin(30).pinReversed(0)//.trace(Serial)
+  .limitLow_set(2, 35, 0).limitLow_setThresholds(600, 720).limitLow_isHard(0)
+  .limitHigh_set(2, 35, 0).limitHigh_setThresholds(900, 1000).limitHigh_isHard(0)
+  .setMaxSpeed(HIGH_SPEED).setAcceleration(HIGH_ACC);
+ B_low_step.onChangeposition([](int idx, int v, int up){bndl.add("/B_low/step").add(v).add(up);});
+ B_low_step.onChangestate([](int idx, int v, int up){bndl.add("/B_low/state").add(v);});
+ B_low_step.onOnlimitlow([](int idx, int v, int up){bndl.add("/B_low/limitLow").add(v);});
+ B_low_step.onOnlimithigh([](int idx, int v, int up){bndl.add("/B_low/limitHigh").add(v);});
+ B_low_step.onOntarget([](int idx, int v, int up){bndl.add("/B_low/onTarget").add(v);});
+
+ TrBr_low_step.begin(2, 1).setEnablePin(0).pinReversed(1).trace(Serial)
+  .limitLow_set(1, 19, 1).limitLow_isHard(1)
+  .limitHigh_set(1, 17, 1).limitHigh_isHard(1)
+  .setMaxSpeed(HIGH_SPEED).setAcceleration(HIGH_ACC);
+ TrBr_low_step.onChangeposition([](int idx, int v, int up){bndl.add("/TrBr_low/step").add(v).add(up);});
+ TrBr_low_step.onChangestate([](int idx, int v, int up){bndl.add("/TrBr_low/state").add(v);});
+ TrBr_low_step.onOnlimitlow([](int idx, int v, int up){bndl.add("/TrBr_low/limitLow").add(v);});
+ TrBr_low_step.onOnlimithigh([](int idx, int v, int up){bndl.add("/TrBr_low/limitHigh").add(v);});
+ TrBr_low_step.onOntarget([](int idx, int v, int up){bndl.add("/TrBr_low/onTarget").add(v);});
+
+ LevBr_low_step.begin(5 , 4).setEnablePin(3).pinReversed(1)//.trace(Serial)
+  .limitLow_set(1, 15, 1).limitLow_isHard(1)
+  .limitHigh_set(1, 39, 1).limitHigh_isHard(1)
+  .setMaxSpeed(HIGH_SPEED).setAcceleration(HIGH_ACC);
+ LevBr_low_step.onChangeposition([](int idx, int v, int up){bndl.add("/LevBr_low/step").add(v).add(up);});
+ LevBr_low_step.onChangestate([](int idx, int v, int up){bndl.add("/LevBr_low/state").add(v);});
+ LevBr_low_step.onOnlimitlow([](int idx, int v, int up){bndl.add("/LevBr_low/limitLow").add(v);});
+ LevBr_low_step.onOnlimithigh([](int idx, int v, int up){bndl.add("/LevBr_low/limitHigh").add(v);});
+ LevBr_low_step.onOntarget([](int idx, int v, int up){bndl.add("/LevBr_low/onTarget").add(v);});
 
  //  Peristaltic pump
  pinMode(filling_pwm_pin, OUTPUT);
  pinMode(brush_pwm_pin, OUTPUT);
  pinMode(crusher_pin, OUTPUT);
 
+ Serial.println("Ended setup");
+ //TrBr_low_step.move(1000);
+ Udp.beginPacket(ipMulti, portMulti);
+ // Udp.beginPacket(outIp, outPort);
+ bndl.add("/test").add("youpi");
+ bndl.send(Udp); // send the bytes to the SLIP stream
+ Udp.endPacket(); // mark the end of the OSC Packet
+ bndl.empty(); //
 }
 
 void loop() {
+  delay(1);
+  Udp.beginPacket(ipMulti, portMulti);
+  // Udp.beginPacket(outIp, outPort);
   automaton.run();
-  // Udp.beginPacket(ipMulti, portMulti);
-  Udp.beginPacket(outIp, outPort);
   bndl.send(Udp); // send the bytes to the SLIP stream
   Udp.endPacket(); // mark the end of the OSC Packet
   bndl.empty(); // empty the bundle to free room for a new one
@@ -171,11 +183,37 @@ void loop() {
       msgIn.fill(Udp.read());
 
      if(!msgIn.hasError()){
-       Serial.println("got OSC");
-       A_low_OSC.onOSC(msgIn);
-       B_low_OSC.onOSC(msgIn);
-       TrBr_low_OSC.onOSC(msgIn);
-       LevBr_low_OSC.onOSC(msgIn);
+       //Serial.println("got OSC");
+       msgIn.dispatch("/A_low/move", [](OSCMessage &msg){A_low_step.move(msg.getInt(0));});
+       msgIn.dispatch("/A_low/moveTo", [](OSCMessage &msg){A_low_step.moveTo(msg.getInt(0));});
+       msgIn.dispatch("/A_low/rotate", [](OSCMessage &msg){A_low_step.rotate(msg.getInt(0));});
+       msgIn.dispatch("/A_low/stop", [](OSCMessage &msg){A_low_step.stop();});
+       msgIn.dispatch("/A_low/emergency_stop", [](OSCMessage &msg){A_low_step.emergency_stop();});
+       msgIn.dispatch("/A_low/homing", [](OSCMessage &msg){A_low_step.homing(msg.getInt(0));});
+
+       msgIn.dispatch("/B_low/move", [](OSCMessage &msg){B_low_step.move(msg.getInt(0));});
+       msgIn.dispatch("/B_low/moveTo", [](OSCMessage &msg){B_low_step.moveTo(msg.getInt(0));});
+       msgIn.dispatch("/B_low/rotate", [](OSCMessage &msg){B_low_step.rotate(msg.getInt(0));});
+       msgIn.dispatch("/B_low/stop", [](OSCMessage &msg){B_low_step.stop();});
+       msgIn.dispatch("/B_low/emergency_stop", [](OSCMessage &msg){B_low_step.emergency_stop();});
+       msgIn.dispatch("/B_low/homing", [](OSCMessage &msg){B_low_step.homing(msg.getInt(0));});
+
+       msgIn.dispatch("/TrBr_low/move", [](OSCMessage &msg){TrBr_low_step.move(msg.getInt(0));});
+       msgIn.dispatch("/TrBr_low/moveTo", [](OSCMessage &msg){TrBr_low_step.moveTo(msg.getInt(0));});
+       msgIn.dispatch("/TrBr_low/rotate", [](OSCMessage &msg){TrBr_low_step.rotate(msg.getInt(0));});
+       msgIn.dispatch("/TrBr_low/stop", [](OSCMessage &msg){TrBr_low_step.stop();});
+       msgIn.dispatch("/TrBr_low/emergency_stop", [](OSCMessage &msg){TrBr_low_step.emergency_stop();});
+       msgIn.dispatch("/TrBr_low/homing", [](OSCMessage &msg){TrBr_low_step.homing(msg.getInt(0));});
+
+       msgIn.dispatch("/LevBr_low/move", [](OSCMessage &msg){LevBr_low_step.move(msg.getInt(0));});
+       msgIn.dispatch("/LevBr_low/moveTo", [](OSCMessage &msg){LevBr_low_step.moveTo(msg.getInt(0));});
+       msgIn.dispatch("/LevBr_low/rotate", [](OSCMessage &msg){LevBr_low_step.rotate(msg.getInt(0));});
+       msgIn.dispatch("/LevBr_low/stop", [](OSCMessage &msg){LevBr_low_step.stop();});
+       msgIn.dispatch("/LevBr_low/emergency_stop", [](OSCMessage &msg){LevBr_low_step.emergency_stop();});
+       msgIn.dispatch("/LevBr_low/homing", [](OSCMessage &msg){LevBr_low_step.homing(msg.getInt(0));});
+       // B_low_OSC.onOSC(msgIn);
+       // TrBr_low_OSC.onOSC(msgIn);
+       // LevBr_low_OSC.onOSC(msgIn);
        motorsOSC(msgIn);
 
      }

+ 2 - 2
PCB/BLOBCNC_TOP/BLOBCNC_TOP-Edge.Cuts.gbr

@@ -1,11 +1,11 @@
 G04 #@! TF.GenerationSoftware,KiCad,Pcbnew,5.0.0+dfsg1-2*
-G04 #@! TF.CreationDate,2019-05-22T11:33:45+02:00*
+G04 #@! TF.CreationDate,2019-07-02T15:00:02+02:00*
 G04 #@! TF.ProjectId,BLOBCNC_TOP,424C4F42434E435F544F502E6B696361,rev?*
 G04 #@! TF.SameCoordinates,PXb438b80PY1b9d560*
 G04 #@! TF.FileFunction,Profile,NP*
 %FSLAX46Y46*%
 G04 Gerber Fmt 4.6, Leading zero omitted, Abs format (unit mm)*
-G04 Created by KiCad (PCBNEW 5.0.0+dfsg1-2) date Wed May 22 11:33:45 2019*
+G04 Created by KiCad (PCBNEW 5.0.0+dfsg1-2) date Tue Jul  2 15:00:02 2019*
 %MOMM*%
 %LPD*%
 G01*

+ 499 - 236
PCB/BLOBCNC_TOP/BLOBCNC_TOP-F.Cu.gbr

@@ -1,12 +1,12 @@
 G04 #@! TF.GenerationSoftware,KiCad,Pcbnew,5.0.0+dfsg1-2*
-G04 #@! TF.CreationDate,2019-05-22T11:33:45+02:00*
+G04 #@! TF.CreationDate,2019-07-02T15:00:02+02:00*
 G04 #@! TF.ProjectId,BLOBCNC_TOP,424C4F42434E435F544F502E6B696361,rev?*
 G04 #@! TF.SameCoordinates,PXb438b80PY1b9d560*
 G04 #@! TF.FileFunction,Copper,L1,Top,Signal*
 G04 #@! TF.FilePolarity,Positive*
 %FSLAX46Y46*%
 G04 Gerber Fmt 4.6, Leading zero omitted, Abs format (unit mm)*
-G04 Created by KiCad (PCBNEW 5.0.0+dfsg1-2) date Wed May 22 11:33:45 2019*
+G04 Created by KiCad (PCBNEW 5.0.0+dfsg1-2) date Tue Jul  2 15:00:02 2019*
 %MOMM*%
 %LPD*%
 G01*
@@ -50,17 +50,20 @@ G04 #@! TD*
 G04 #@! TA.AperFunction,ComponentPad*
 %ADD22O,1.600000X2.400000*%
 G04 #@! TD*
+G04 #@! TA.AperFunction,ComponentPad*
+%ADD23O,2.700000X2.000000*%
+G04 #@! TD*
 G04 #@! TA.AperFunction,ViaPad*
-%ADD23C,2.000000*%
+%ADD24C,2.000000*%
 G04 #@! TD*
 G04 #@! TA.AperFunction,Conductor*
-%ADD24C,1.500000*%
+%ADD25C,1.500000*%
 G04 #@! TD*
 G04 #@! TA.AperFunction,Conductor*
-%ADD25C,0.500000*%
+%ADD26C,0.500000*%
 G04 #@! TD*
 G04 #@! TA.AperFunction,Conductor*
-%ADD26C,0.254000*%
+%ADD27C,0.254000*%
 G04 #@! TD*
 G04 APERTURE END LIST*
 D10*
@@ -354,20 +357,6 @@ G04 #@! TO.P,SW1,6*
 G04 #@! TO.N,Net-(Dr1-Pad2)*
 X-77851000Y-33274000D03*
 G04 #@! TD*
-D12*
-G04 #@! TO.P,U1,1*
-G04 #@! TO.N,GNDPWR*
-X-102616000Y-4699000D03*
-G04 #@! TO.P,U1,2*
-G04 #@! TO.N,+36V*
-X-102616000Y-22479000D03*
-G04 #@! TO.P,U1,3*
-G04 #@! TO.N,GND*
-X-61976000Y-4699000D03*
-G04 #@! TO.P,U1,4*
-G04 #@! TO.N,+5V*
-X-61976000Y-22479000D03*
-G04 #@! TD*
 D18*
 G04 #@! TO.P,U2,5*
 G04 #@! TO.N,N/C*
@@ -533,6 +522,24 @@ G04 #@! TO.N,+5V*
 X-75311000Y-54864000D03*
 G04 #@! TD*
 D23*
+G04 #@! TO.P,U1,1*
+G04 #@! TO.N,GNDPWR*
+X-91186000Y-7239000D03*
+X-91186000Y-9779000D03*
+G04 #@! TO.P,U1,2*
+G04 #@! TO.N,+36V*
+X-91186000Y-17399000D03*
+X-91186000Y-19939000D03*
+G04 #@! TO.P,U1,3*
+G04 #@! TO.N,GND*
+X-73406000Y-7239000D03*
+X-73406000Y-9779000D03*
+G04 #@! TO.P,U1,4*
+G04 #@! TO.N,+5V*
+X-73406000Y-17399000D03*
+X-73406000Y-19939000D03*
+G04 #@! TD*
+D24*
 G04 #@! TO.N,GND*
 X-29972000Y-75184000D03*
 X-20828000Y-75184000D03*
@@ -545,7 +552,7 @@ X-110236000Y-51308000D03*
 X-16256000Y-19812000D03*
 X-23876000Y-19812000D03*
 G04 #@! TD*
-D24*
+D25*
 G04 #@! TO.N,+36V*
 X-109601000Y-15494000D02*
 X-102616000Y-22479000D01*
@@ -563,9 +570,15 @@ X-95631000Y-30734000D02*
 X-98171000Y-30734000D01*
 X-93091000Y-33274000D02*
 X-95631000Y-30734000D01*
+X-96576000Y-22479000D02*
+X-99816000Y-22479000D01*
+X-94036000Y-19939000D02*
+X-96576000Y-22479000D01*
+X-91186000Y-19939000D02*
+X-94036000Y-19939000D01*
+X-91186000Y-17399000D02*
+X-91186000Y-19939000D01*
 G04 #@! TO.N,GNDPWR*
-X-108331000Y-10414000D02*
-X-102616000Y-4699000D01*
 X-90591000Y-35219000D02*
 X-90591000Y-33274000D01*
 X-91186000Y-35814000D02*
@@ -576,15 +589,23 @@ X-98171000Y-33274000D02*
 X-96901000Y-33274000D01*
 X-96901000Y-33274000D02*
 X-94361000Y-35814000D01*
-X-90591000Y-33274000D02*
-X-90591000Y-28154000D01*
-X-108331000Y-10414000D02*
-X-108966000Y-10414000D01*
-X-90591000Y-28154000D02*
-X-108331000Y-10414000D01*
+X-91821000Y-10414000D02*
+X-91186000Y-9779000D01*
 X-110871000Y-10414000D02*
-X-108966000Y-10414000D01*
-D25*
+X-91821000Y-10414000D01*
+X-91186000Y-9779000D02*
+X-91186000Y-7239000D01*
+X-88336000Y-9779000D02*
+X-86995000Y-11120000D01*
+X-91186000Y-9779000D02*
+X-88336000Y-9779000D01*
+X-86995000Y-11120000D02*
+X-86995000Y-24765000D01*
+X-90591000Y-28361000D02*
+X-90591000Y-33274000D01*
+X-86995000Y-24765000D02*
+X-90591000Y-28361000D01*
+D26*
 G04 #@! TO.N,Net-(Dr1-Pad1)*
 X-63881000Y-37084000D02*
 X-60706000Y-37084000D01*
@@ -665,6 +686,8 @@ X-98471000Y-48514000D02*
 X-101265000Y-51308000D01*
 X-16256000Y-23749000D02*
 X-16256000Y-19812000D01*
+X-73406000Y-7239000D02*
+X-73406000Y-9779000D01*
 G04 #@! TO.N,+5V*
 X-61976000Y-28194000D02*
 X-60706000Y-29464000D01*
@@ -762,7 +785,15 @@ X-20215000Y-35665000D02*
 X-12446000Y-43434000D01*
 X-12446000Y-46609000D02*
 X-12446000Y-65659000D01*
-D24*
+X-73406000Y-17399000D02*
+X-73406000Y-19939000D01*
+X-73376000Y-21469000D02*
+X-73376000Y-28829000D01*
+X-73406000Y-21439000D02*
+X-73376000Y-21469000D01*
+X-73406000Y-19939000D02*
+X-73406000Y-21439000D01*
+D25*
 G04 #@! TO.N,Net-(Dr1-Pad11)*
 X-98171000Y-43434000D02*
 X-110236000Y-43434000D01*
@@ -797,7 +828,7 @@ X-10541000Y-28829000D02*
 X-8636000Y-26924000D01*
 X-8636000Y-26924000D02*
 X-4191000Y-26924000D01*
-D25*
+D26*
 G04 #@! TO.N,Net-(J4-Pad3)*
 X-15621000Y-47244000D02*
 X-14986000Y-46609000D01*
@@ -1066,7 +1097,7 @@ X-74610990Y-57338990D01*
 X-87236010Y-57338990D02*
 X-88011000Y-56564000D01*
 G04 #@! TD*
-D26*
+D27*
 G04 #@! TO.N,GND*
 G36*
 X-48824155Y-836576D02*
@@ -1086,15 +1117,10 @@ X-62614048Y-20340951D01*
 X-62663422Y-20414844D01*
 X-62663424Y-20414846D01*
 X-62809652Y-20633691D01*
-X-62868558Y-20929836D01*
-X-62913945Y-20938864D01*
-X-63454769Y-21300231D01*
-X-63816136Y-21841055D01*
-X-63943031Y-22479000D01*
-X-63816136Y-23116945D01*
-X-63454769Y-23657769D01*
-X-62913945Y-24019136D01*
-X-62861000Y-24029667D01*
+X-62878337Y-20979000D01*
+X-62860999Y-21066165D01*
+X-62861000Y-22391835D01*
+X-62861000Y-22391836D01*
 X-62860999Y-28106835D01*
 X-62878337Y-28194000D01*
 X-62809652Y-28539309D01*
@@ -1110,90 +1136,160 @@ X-68285156Y-28776577D01*
 X-68359049Y-28825951D01*
 X-68408423Y-28899844D01*
 X-70231000Y-30722421D01*
-X-72688575Y-28264847D01*
-X-72737951Y-28190951D01*
-X-73030690Y-27995348D01*
-X-73288835Y-27944000D01*
-X-73288839Y-27944000D01*
-X-73376000Y-27926663D01*
-X-73463161Y-27944000D01*
+X-72491000Y-28462422D01*
+X-72491000Y-21556159D01*
+X-72478053Y-21491070D01*
+X-72418055Y-21479136D01*
+X-71877231Y-21117769D01*
+X-71515864Y-20576945D01*
+X-71388969Y-19939000D01*
+X-71515864Y-19301055D01*
+X-71877231Y-18760231D01*
+X-72013768Y-18669000D01*
+X-71877231Y-18577769D01*
+X-71515864Y-18036945D01*
+X-71388969Y-17399000D01*
+X-71515864Y-16761055D01*
+X-71877231Y-16220231D01*
+X-72418055Y-15858864D01*
+X-72894969Y-15764000D01*
+X-73917031Y-15764000D01*
+X-74393945Y-15858864D01*
+X-74934769Y-16220231D01*
+X-75296136Y-16761055D01*
+X-75423031Y-17399000D01*
+X-75296136Y-18036945D01*
+X-74934769Y-18577769D01*
+X-74798232Y-18669000D01*
+X-74934769Y-18760231D01*
+X-75296136Y-19301055D01*
+X-75423031Y-19939000D01*
+X-75296136Y-20576945D01*
+X-74934769Y-21117769D01*
+X-74393945Y-21479136D01*
+X-74296498Y-21498519D01*
+X-74261000Y-21676984D01*
+X-74260999Y-27944000D01*
 X-87288841Y-27944000D01*
 X-87376001Y-27926663D01*
 X-87463161Y-27944000D01*
 X-87463165Y-27944000D01*
 X-87721310Y-27995348D01*
-X-87828544Y-28067000D01*
+X-87721312Y-27995349D01*
+X-87721311Y-27995349D01*
 X-87940155Y-28141576D01*
 X-87940156Y-28141577D01*
 X-88014049Y-28190951D01*
 X-88063423Y-28264844D01*
 X-89206000Y-29407422D01*
-X-89206000Y-28290407D01*
-X-89178867Y-28154000D01*
-X-89286359Y-27613600D01*
-X-89463374Y-27348678D01*
-X-89592471Y-27155471D01*
-X-89708112Y-27078202D01*
-X-106372315Y-10414000D01*
-X-102292314Y-6334000D01*
-X-102154969Y-6334000D01*
-X-101678055Y-6239136D01*
-X-101137231Y-5877769D01*
-X-100775864Y-5336945D01*
-X-100724642Y-5079434D01*
-X-63866124Y-5079434D01*
-X-63835144Y-5207355D01*
-X-63521922Y-5765317D01*
-X-63019020Y-6160942D01*
-X-62403000Y-6334000D01*
-X-62103000Y-6334000D01*
-X-62103000Y-4826000D01*
-X-61849000Y-4826000D01*
-X-61849000Y-6334000D01*
-X-61549000Y-6334000D01*
-X-60932980Y-6160942D01*
-X-60430078Y-5765317D01*
-X-60116856Y-5207355D01*
-X-60085876Y-5079434D01*
-X-60205223Y-4826000D01*
-X-61849000Y-4826000D01*
-X-62103000Y-4826000D01*
-X-63746777Y-4826000D01*
-X-63866124Y-5079434D01*
-X-100724642Y-5079434D01*
-X-100648969Y-4699000D01*
-X-100724641Y-4318566D01*
-X-63866124Y-4318566D01*
-X-63746777Y-4572000D01*
-X-62103000Y-4572000D01*
-X-62103000Y-3064000D01*
-X-61849000Y-3064000D01*
-X-61849000Y-4572000D01*
-X-60205223Y-4572000D01*
-X-60085876Y-4318566D01*
-X-60116856Y-4190645D01*
-X-60430078Y-3632683D01*
-X-60932980Y-3237058D01*
-X-61549000Y-3064000D01*
-X-61849000Y-3064000D01*
-X-62103000Y-3064000D01*
-X-62403000Y-3064000D01*
-X-63019020Y-3237058D01*
-X-63521922Y-3632683D01*
-X-63835144Y-4190645D01*
-X-63866124Y-4318566D01*
-X-100724641Y-4318566D01*
-X-100775864Y-4061055D01*
-X-101137231Y-3520231D01*
-X-101678055Y-3158864D01*
-X-102154969Y-3064000D01*
-X-103077031Y-3064000D01*
-X-103553945Y-3158864D01*
-X-104094769Y-3520231D01*
-X-104456136Y-4061055D01*
-X-104583031Y-4699000D01*
-X-104581646Y-4705961D01*
-X-108734531Y-8858846D01*
+X-89206000Y-28934685D01*
+X-86112112Y-25840798D01*
+X-85996471Y-25763529D01*
+X-85690359Y-25305400D01*
+X-85610000Y-24901407D01*
+X-85582867Y-24765000D01*
+X-85610000Y-24628593D01*
+X-85610000Y-11256401D01*
+X-85582868Y-11119999D01*
+X-85610000Y-10983597D01*
+X-85610000Y-10983593D01*
+X-85690359Y-10579600D01*
+X-85971105Y-10159434D01*
+X-75346124Y-10159434D01*
+X-75315144Y-10287355D01*
+X-75001922Y-10845317D01*
+X-74499020Y-11240942D01*
+X-73883000Y-11414000D01*
+X-73533000Y-11414000D01*
+X-73533000Y-9906000D01*
+X-73279000Y-9906000D01*
+X-73279000Y-11414000D01*
+X-72929000Y-11414000D01*
+X-72312980Y-11240942D01*
+X-71810078Y-10845317D01*
+X-71496856Y-10287355D01*
+X-71465876Y-10159434D01*
+X-71585223Y-9906000D01*
+X-73279000Y-9906000D01*
+X-73533000Y-9906000D01*
+X-75226777Y-9906000D01*
+X-75346124Y-10159434D01*
+X-85971105Y-10159434D01*
+X-85996472Y-10121471D01*
+X-86112113Y-10044202D01*
+X-87260200Y-8896115D01*
+X-87337471Y-8780471D01*
+X-87795600Y-8474359D01*
+X-88199593Y-8394000D01*
+X-88336000Y-8366867D01*
+X-88472407Y-8394000D01*
+X-89641349Y-8394000D01*
+X-89295864Y-7876945D01*
+X-89244642Y-7619434D01*
+X-75346124Y-7619434D01*
+X-75315144Y-7747355D01*
+X-75001922Y-8305317D01*
+X-74743009Y-8509000D01*
+X-75001922Y-8712683D01*
+X-75315144Y-9270645D01*
+X-75346124Y-9398566D01*
+X-75226777Y-9652000D01*
+X-73533000Y-9652000D01*
+X-73533000Y-7366000D01*
+X-73279000Y-7366000D01*
+X-73279000Y-9652000D01*
+X-71585223Y-9652000D01*
+X-71465876Y-9398566D01*
+X-71496856Y-9270645D01*
+X-71810078Y-8712683D01*
+X-72068991Y-8509000D01*
+X-71810078Y-8305317D01*
+X-71496856Y-7747355D01*
+X-71465876Y-7619434D01*
+X-71585223Y-7366000D01*
+X-73279000Y-7366000D01*
+X-73533000Y-7366000D01*
+X-75226777Y-7366000D01*
+X-75346124Y-7619434D01*
+X-89244642Y-7619434D01*
+X-89168969Y-7239000D01*
+X-89244641Y-6858566D01*
+X-75346124Y-6858566D01*
+X-75226777Y-7112000D01*
+X-73533000Y-7112000D01*
+X-73533000Y-5604000D01*
+X-73279000Y-5604000D01*
+X-73279000Y-7112000D01*
+X-71585223Y-7112000D01*
+X-71465876Y-6858566D01*
+X-71496856Y-6730645D01*
+X-71810078Y-6172683D01*
+X-72312980Y-5777058D01*
+X-72929000Y-5604000D01*
+X-73279000Y-5604000D01*
+X-73533000Y-5604000D01*
+X-73883000Y-5604000D01*
+X-74499020Y-5777058D01*
+X-75001922Y-6172683D01*
+X-75315144Y-6730645D01*
+X-75346124Y-6858566D01*
+X-89244641Y-6858566D01*
+X-89295864Y-6601055D01*
+X-89657231Y-6060231D01*
+X-90198055Y-5698864D01*
+X-90674969Y-5604000D01*
+X-91697031Y-5604000D01*
+X-92173945Y-5698864D01*
+X-92714769Y-6060231D01*
+X-93076136Y-6601055D01*
+X-93203031Y-7239000D01*
+X-93076136Y-7876945D01*
+X-92714769Y-8417769D01*
+X-92578232Y-8509000D01*
+X-92714769Y-8600231D01*
+X-93001263Y-9029000D01*
+X-108723560Y-9029000D01*
+X-108723560Y-8914000D01*
 X-108772843Y-8666235D01*
 X-108913191Y-8456191D01*
 X-109123235Y-8315843D01*
@@ -1212,15 +1308,71 @@ X-109371000Y-12561440D01*
 X-109123235Y-12512157D01*
 X-108913191Y-12371809D01*
 X-108772843Y-12161765D01*
-X-108734531Y-11969154D01*
-X-99592327Y-21111358D01*
+X-108723560Y-11914000D01*
+X-108723560Y-11799000D01*
+X-91957407Y-11799000D01*
+X-91821000Y-11826133D01*
+X-91684593Y-11799000D01*
+X-91280600Y-11718641D01*
+X-90824673Y-11414000D01*
+X-90674969Y-11414000D01*
+X-90198055Y-11319136D01*
+X-89965878Y-11164000D01*
+X-88909685Y-11164000D01*
+X-88380000Y-11693685D01*
+X-88379999Y-24191313D01*
+X-91473888Y-27285203D01*
+X-91589529Y-27362472D01*
+X-91757827Y-27614348D01*
+X-91895641Y-27820601D01*
+X-91999788Y-28344182D01*
+X-92015203Y-28321111D01*
+X-92015205Y-28321109D01*
+X-92092472Y-28205471D01*
+X-92208110Y-28128204D01*
+X-96466886Y-23869429D01*
+X-96439593Y-23864000D01*
+X-96035600Y-23783641D01*
+X-95577471Y-23477529D01*
+X-95500200Y-23361885D01*
+X-93462315Y-21324000D01*
+X-92406122Y-21324000D01*
+X-92173945Y-21479136D01*
+X-91697031Y-21574000D01*
+X-90674969Y-21574000D01*
+X-90198055Y-21479136D01*
+X-89657231Y-21117769D01*
+X-89295864Y-20576945D01*
+X-89168969Y-19939000D01*
+X-89295864Y-19301055D01*
+X-89657231Y-18760231D01*
+X-89793768Y-18669000D01*
+X-89657231Y-18577769D01*
+X-89295864Y-18036945D01*
+X-89168969Y-17399000D01*
+X-89295864Y-16761055D01*
+X-89657231Y-16220231D01*
+X-90198055Y-15858864D01*
+X-90674969Y-15764000D01*
+X-91697031Y-15764000D01*
+X-92173945Y-15858864D01*
+X-92714769Y-16220231D01*
+X-93076136Y-16761055D01*
+X-93203031Y-17399000D01*
+X-93076136Y-18036945D01*
+X-92730651Y-18554000D01*
+X-93899599Y-18554000D01*
+X-94036001Y-18526868D01*
+X-94172403Y-18554000D01*
+X-94172407Y-18554000D01*
+X-94576400Y-18634359D01*
+X-95034529Y-18940471D01*
+X-95111800Y-19056115D01*
+X-97149685Y-21094000D01*
 X-99679593Y-21094000D01*
 X-99816000Y-21066867D01*
 X-99952407Y-21094000D01*
-X-101445878Y-21094000D01*
-X-101678055Y-20938864D01*
-X-102154969Y-20844000D01*
-X-102292314Y-20844000D01*
+X-102042314Y-21094000D01*
 X-108525201Y-14611114D01*
 X-108602471Y-14495471D01*
 X-109060600Y-14189359D01*
@@ -1237,15 +1389,12 @@ X-112080380Y-17303966D01*
 X-111295678Y-17629000D01*
 X-110446322Y-17629000D01*
 X-109723916Y-17329770D01*
-X-104581646Y-22472039D01*
-X-104583031Y-22479000D01*
-X-104456136Y-23116945D01*
-X-104094769Y-23657769D01*
-X-103553945Y-24019136D01*
-X-103077031Y-24114000D01*
-X-102154969Y-24114000D01*
-X-101678055Y-24019136D01*
-X-101445878Y-23864000D01*
+X-103691798Y-23361888D01*
+X-103614529Y-23477529D01*
+X-103498889Y-23554797D01*
+X-103156400Y-23783641D01*
+X-102616000Y-23891133D01*
+X-102479593Y-23864000D01*
 X-100389685Y-23864000D01*
 X-94476000Y-29777686D01*
 X-94476000Y-29930315D01*
@@ -2089,15 +2238,10 @@ X-62614048Y-20340951D01*
 X-62663422Y-20414844D01*
 X-62663424Y-20414846D01*
 X-62809652Y-20633691D01*
-X-62868558Y-20929836D01*
-X-62913945Y-20938864D01*
-X-63454769Y-21300231D01*
-X-63816136Y-21841055D01*
-X-63943031Y-22479000D01*
-X-63816136Y-23116945D01*
-X-63454769Y-23657769D01*
-X-62913945Y-24019136D01*
-X-62861000Y-24029667D01*
+X-62878337Y-20979000D01*
+X-62860999Y-21066165D01*
+X-62861000Y-22391835D01*
+X-62861000Y-22391836D01*
 X-62860999Y-28106835D01*
 X-62878337Y-28194000D01*
 X-62809652Y-28539309D01*
@@ -2113,90 +2257,160 @@ X-68285156Y-28776577D01*
 X-68359049Y-28825951D01*
 X-68408423Y-28899844D01*
 X-70231000Y-30722421D01*
-X-72688575Y-28264847D01*
-X-72737951Y-28190951D01*
-X-73030690Y-27995348D01*
-X-73288835Y-27944000D01*
-X-73288839Y-27944000D01*
-X-73376000Y-27926663D01*
-X-73463161Y-27944000D01*
+X-72491000Y-28462422D01*
+X-72491000Y-21556159D01*
+X-72478053Y-21491070D01*
+X-72418055Y-21479136D01*
+X-71877231Y-21117769D01*
+X-71515864Y-20576945D01*
+X-71388969Y-19939000D01*
+X-71515864Y-19301055D01*
+X-71877231Y-18760231D01*
+X-72013768Y-18669000D01*
+X-71877231Y-18577769D01*
+X-71515864Y-18036945D01*
+X-71388969Y-17399000D01*
+X-71515864Y-16761055D01*
+X-71877231Y-16220231D01*
+X-72418055Y-15858864D01*
+X-72894969Y-15764000D01*
+X-73917031Y-15764000D01*
+X-74393945Y-15858864D01*
+X-74934769Y-16220231D01*
+X-75296136Y-16761055D01*
+X-75423031Y-17399000D01*
+X-75296136Y-18036945D01*
+X-74934769Y-18577769D01*
+X-74798232Y-18669000D01*
+X-74934769Y-18760231D01*
+X-75296136Y-19301055D01*
+X-75423031Y-19939000D01*
+X-75296136Y-20576945D01*
+X-74934769Y-21117769D01*
+X-74393945Y-21479136D01*
+X-74296498Y-21498519D01*
+X-74261000Y-21676984D01*
+X-74260999Y-27944000D01*
 X-87288841Y-27944000D01*
 X-87376001Y-27926663D01*
 X-87463161Y-27944000D01*
 X-87463165Y-27944000D01*
 X-87721310Y-27995348D01*
-X-87828544Y-28067000D01*
+X-87721312Y-27995349D01*
+X-87721311Y-27995349D01*
 X-87940155Y-28141576D01*
 X-87940156Y-28141577D01*
 X-88014049Y-28190951D01*
 X-88063423Y-28264844D01*
 X-89206000Y-29407422D01*
-X-89206000Y-28290407D01*
-X-89178867Y-28154000D01*
-X-89286359Y-27613600D01*
-X-89463374Y-27348678D01*
-X-89592471Y-27155471D01*
-X-89708112Y-27078202D01*
-X-106372315Y-10414000D01*
-X-102292314Y-6334000D01*
-X-102154969Y-6334000D01*
-X-101678055Y-6239136D01*
-X-101137231Y-5877769D01*
-X-100775864Y-5336945D01*
-X-100724642Y-5079434D01*
-X-63866124Y-5079434D01*
-X-63835144Y-5207355D01*
-X-63521922Y-5765317D01*
-X-63019020Y-6160942D01*
-X-62403000Y-6334000D01*
-X-62103000Y-6334000D01*
-X-62103000Y-4826000D01*
-X-61849000Y-4826000D01*
-X-61849000Y-6334000D01*
-X-61549000Y-6334000D01*
-X-60932980Y-6160942D01*
-X-60430078Y-5765317D01*
-X-60116856Y-5207355D01*
-X-60085876Y-5079434D01*
-X-60205223Y-4826000D01*
-X-61849000Y-4826000D01*
-X-62103000Y-4826000D01*
-X-63746777Y-4826000D01*
-X-63866124Y-5079434D01*
-X-100724642Y-5079434D01*
-X-100648969Y-4699000D01*
-X-100724641Y-4318566D01*
-X-63866124Y-4318566D01*
-X-63746777Y-4572000D01*
-X-62103000Y-4572000D01*
-X-62103000Y-3064000D01*
-X-61849000Y-3064000D01*
-X-61849000Y-4572000D01*
-X-60205223Y-4572000D01*
-X-60085876Y-4318566D01*
-X-60116856Y-4190645D01*
-X-60430078Y-3632683D01*
-X-60932980Y-3237058D01*
-X-61549000Y-3064000D01*
-X-61849000Y-3064000D01*
-X-62103000Y-3064000D01*
-X-62403000Y-3064000D01*
-X-63019020Y-3237058D01*
-X-63521922Y-3632683D01*
-X-63835144Y-4190645D01*
-X-63866124Y-4318566D01*
-X-100724641Y-4318566D01*
-X-100775864Y-4061055D01*
-X-101137231Y-3520231D01*
-X-101678055Y-3158864D01*
-X-102154969Y-3064000D01*
-X-103077031Y-3064000D01*
-X-103553945Y-3158864D01*
-X-104094769Y-3520231D01*
-X-104456136Y-4061055D01*
-X-104583031Y-4699000D01*
-X-104581646Y-4705961D01*
-X-108734531Y-8858846D01*
+X-89206000Y-28934685D01*
+X-86112112Y-25840798D01*
+X-85996471Y-25763529D01*
+X-85690359Y-25305400D01*
+X-85610000Y-24901407D01*
+X-85582867Y-24765000D01*
+X-85610000Y-24628593D01*
+X-85610000Y-11256401D01*
+X-85582868Y-11119999D01*
+X-85610000Y-10983597D01*
+X-85610000Y-10983593D01*
+X-85690359Y-10579600D01*
+X-85971105Y-10159434D01*
+X-75346124Y-10159434D01*
+X-75315144Y-10287355D01*
+X-75001922Y-10845317D01*
+X-74499020Y-11240942D01*
+X-73883000Y-11414000D01*
+X-73533000Y-11414000D01*
+X-73533000Y-9906000D01*
+X-73279000Y-9906000D01*
+X-73279000Y-11414000D01*
+X-72929000Y-11414000D01*
+X-72312980Y-11240942D01*
+X-71810078Y-10845317D01*
+X-71496856Y-10287355D01*
+X-71465876Y-10159434D01*
+X-71585223Y-9906000D01*
+X-73279000Y-9906000D01*
+X-73533000Y-9906000D01*
+X-75226777Y-9906000D01*
+X-75346124Y-10159434D01*
+X-85971105Y-10159434D01*
+X-85996472Y-10121471D01*
+X-86112113Y-10044202D01*
+X-87260200Y-8896115D01*
+X-87337471Y-8780471D01*
+X-87795600Y-8474359D01*
+X-88199593Y-8394000D01*
+X-88336000Y-8366867D01*
+X-88472407Y-8394000D01*
+X-89641349Y-8394000D01*
+X-89295864Y-7876945D01*
+X-89244642Y-7619434D01*
+X-75346124Y-7619434D01*
+X-75315144Y-7747355D01*
+X-75001922Y-8305317D01*
+X-74743009Y-8509000D01*
+X-75001922Y-8712683D01*
+X-75315144Y-9270645D01*
+X-75346124Y-9398566D01*
+X-75226777Y-9652000D01*
+X-73533000Y-9652000D01*
+X-73533000Y-7366000D01*
+X-73279000Y-7366000D01*
+X-73279000Y-9652000D01*
+X-71585223Y-9652000D01*
+X-71465876Y-9398566D01*
+X-71496856Y-9270645D01*
+X-71810078Y-8712683D01*
+X-72068991Y-8509000D01*
+X-71810078Y-8305317D01*
+X-71496856Y-7747355D01*
+X-71465876Y-7619434D01*
+X-71585223Y-7366000D01*
+X-73279000Y-7366000D01*
+X-73533000Y-7366000D01*
+X-75226777Y-7366000D01*
+X-75346124Y-7619434D01*
+X-89244642Y-7619434D01*
+X-89168969Y-7239000D01*
+X-89244641Y-6858566D01*
+X-75346124Y-6858566D01*
+X-75226777Y-7112000D01*
+X-73533000Y-7112000D01*
+X-73533000Y-5604000D01*
+X-73279000Y-5604000D01*
+X-73279000Y-7112000D01*
+X-71585223Y-7112000D01*
+X-71465876Y-6858566D01*
+X-71496856Y-6730645D01*
+X-71810078Y-6172683D01*
+X-72312980Y-5777058D01*
+X-72929000Y-5604000D01*
+X-73279000Y-5604000D01*
+X-73533000Y-5604000D01*
+X-73883000Y-5604000D01*
+X-74499020Y-5777058D01*
+X-75001922Y-6172683D01*
+X-75315144Y-6730645D01*
+X-75346124Y-6858566D01*
+X-89244641Y-6858566D01*
+X-89295864Y-6601055D01*
+X-89657231Y-6060231D01*
+X-90198055Y-5698864D01*
+X-90674969Y-5604000D01*
+X-91697031Y-5604000D01*
+X-92173945Y-5698864D01*
+X-92714769Y-6060231D01*
+X-93076136Y-6601055D01*
+X-93203031Y-7239000D01*
+X-93076136Y-7876945D01*
+X-92714769Y-8417769D01*
+X-92578232Y-8509000D01*
+X-92714769Y-8600231D01*
+X-93001263Y-9029000D01*
+X-108723560Y-9029000D01*
+X-108723560Y-8914000D01*
 X-108772843Y-8666235D01*
 X-108913191Y-8456191D01*
 X-109123235Y-8315843D01*
@@ -2215,15 +2429,71 @@ X-109371000Y-12561440D01*
 X-109123235Y-12512157D01*
 X-108913191Y-12371809D01*
 X-108772843Y-12161765D01*
-X-108734531Y-11969154D01*
-X-99592327Y-21111358D01*
+X-108723560Y-11914000D01*
+X-108723560Y-11799000D01*
+X-91957407Y-11799000D01*
+X-91821000Y-11826133D01*
+X-91684593Y-11799000D01*
+X-91280600Y-11718641D01*
+X-90824673Y-11414000D01*
+X-90674969Y-11414000D01*
+X-90198055Y-11319136D01*
+X-89965878Y-11164000D01*
+X-88909685Y-11164000D01*
+X-88380000Y-11693685D01*
+X-88379999Y-24191313D01*
+X-91473888Y-27285203D01*
+X-91589529Y-27362472D01*
+X-91757827Y-27614348D01*
+X-91895641Y-27820601D01*
+X-91999788Y-28344182D01*
+X-92015203Y-28321111D01*
+X-92015205Y-28321109D01*
+X-92092472Y-28205471D01*
+X-92208110Y-28128204D01*
+X-96466886Y-23869429D01*
+X-96439593Y-23864000D01*
+X-96035600Y-23783641D01*
+X-95577471Y-23477529D01*
+X-95500200Y-23361885D01*
+X-93462315Y-21324000D01*
+X-92406122Y-21324000D01*
+X-92173945Y-21479136D01*
+X-91697031Y-21574000D01*
+X-90674969Y-21574000D01*
+X-90198055Y-21479136D01*
+X-89657231Y-21117769D01*
+X-89295864Y-20576945D01*
+X-89168969Y-19939000D01*
+X-89295864Y-19301055D01*
+X-89657231Y-18760231D01*
+X-89793768Y-18669000D01*
+X-89657231Y-18577769D01*
+X-89295864Y-18036945D01*
+X-89168969Y-17399000D01*
+X-89295864Y-16761055D01*
+X-89657231Y-16220231D01*
+X-90198055Y-15858864D01*
+X-90674969Y-15764000D01*
+X-91697031Y-15764000D01*
+X-92173945Y-15858864D01*
+X-92714769Y-16220231D01*
+X-93076136Y-16761055D01*
+X-93203031Y-17399000D01*
+X-93076136Y-18036945D01*
+X-92730651Y-18554000D01*
+X-93899599Y-18554000D01*
+X-94036001Y-18526868D01*
+X-94172403Y-18554000D01*
+X-94172407Y-18554000D01*
+X-94576400Y-18634359D01*
+X-95034529Y-18940471D01*
+X-95111800Y-19056115D01*
+X-97149685Y-21094000D01*
 X-99679593Y-21094000D01*
 X-99816000Y-21066867D01*
 X-99952407Y-21094000D01*
-X-101445878Y-21094000D01*
-X-101678055Y-20938864D01*
-X-102154969Y-20844000D01*
-X-102292314Y-20844000D01*
+X-102042314Y-21094000D01*
 X-108525201Y-14611114D01*
 X-108602471Y-14495471D01*
 X-109060600Y-14189359D01*
@@ -2240,15 +2510,12 @@ X-112080380Y-17303966D01*
 X-111295678Y-17629000D01*
 X-110446322Y-17629000D01*
 X-109723916Y-17329770D01*
-X-104581646Y-22472039D01*
-X-104583031Y-22479000D01*
-X-104456136Y-23116945D01*
-X-104094769Y-23657769D01*
-X-103553945Y-24019136D01*
-X-103077031Y-24114000D01*
-X-102154969Y-24114000D01*
-X-101678055Y-24019136D01*
-X-101445878Y-23864000D01*
+X-103691798Y-23361888D01*
+X-103614529Y-23477529D01*
+X-103498889Y-23554797D01*
+X-103156400Y-23783641D01*
+X-102616000Y-23891133D01*
+X-102479593Y-23864000D01*
 X-100389685Y-23864000D01*
 X-94476000Y-29777686D01*
 X-94476000Y-29930315D01*
@@ -4137,6 +4404,7 @@ X-31408839Y-12069000D01*
 X-31496000Y-12051663D01*
 X-31583161Y-12069000D01*
 X-31583165Y-12069000D01*
+X-31832160Y-12118528D01*
 X-31841310Y-12120348D01*
 X-32060155Y-12266576D01*
 X-32060156Y-12266577D01*
@@ -4371,9 +4639,6 @@ X-43293663Y-4063999D01*
 X-43311000Y-3976839D01*
 X-43311000Y-3976835D01*
 X-43362348Y-3718690D01*
-X-43434385Y-3610880D01*
-X-43508576Y-3499845D01*
-X-43508577Y-3499844D01*
 X-43557951Y-3425951D01*
 X-43631844Y-3376577D01*
 X-44599422Y-2409000D01*
@@ -4610,6 +4875,7 @@ X-31408839Y-12069000D01*
 X-31496000Y-12051663D01*
 X-31583161Y-12069000D01*
 X-31583165Y-12069000D01*
+X-31832160Y-12118528D01*
 X-31841310Y-12120348D01*
 X-32060155Y-12266576D01*
 X-32060156Y-12266577D01*
@@ -4844,9 +5110,6 @@ X-43293663Y-4063999D01*
 X-43311000Y-3976839D01*
 X-43311000Y-3976835D01*
 X-43362348Y-3718690D01*
-X-43434385Y-3610880D01*
-X-43508576Y-3499845D01*
-X-43508577Y-3499844D01*
 X-43557951Y-3425951D01*
 X-43631844Y-3376577D01*
 X-44599422Y-2409000D01*

+ 1 - 1
PCB/BLOBCNC_TOP/BLOBCNC_TOP-NPTH.drl

@@ -1,5 +1,5 @@
 M48
-;DRILL file {KiCad 5.0.0+dfsg1-2} date Wed May 22 11:33:50 2019
+;DRILL file {KiCad 5.0.0+dfsg1-2} date Tue Jul  2 15:00:04 2019
 ;FORMAT={-:-/ absolute / metric / decimal}
 FMAT,2
 METRIC,TZ

+ 53 - 49
PCB/BLOBCNC_TOP/BLOBCNC_TOP-PTH.drl

@@ -1,5 +1,5 @@
 M48
-;DRILL file {KiCad 5.0.0+dfsg1-2} date Wed May 22 11:33:50 2019
+;DRILL file {KiCad 5.0.0+dfsg1-2} date Tue Jul  2 15:00:04 2019
 ;FORMAT={-:-/ absolute / metric / decimal}
 FMAT,2
 METRIC,TZ
@@ -13,6 +13,16 @@ G90
 G05
 M71
 T1
+X-77.851Y-54.864
+X-75.311Y-54.864
+X-93.091Y-33.274
+X-90.591Y-33.274
+X-77.851Y-33.274
+X-77.851Y-35.814
+X-77.851Y-38.354
+X-70.231Y-33.274
+X-70.231Y-35.814
+X-70.231Y-38.354
 X-90.551Y-54.864
 X-90.551Y-62.484
 X-88.011Y-54.864
@@ -23,18 +33,8 @@ X-82.931Y-54.864
 X-82.931Y-62.484
 X-80.391Y-54.864
 X-80.391Y-62.484
-X-77.851Y-54.864
 X-77.851Y-62.484
-X-75.311Y-54.864
 X-75.311Y-62.484
-X-93.091Y-33.274
-X-90.591Y-33.274
-X-77.851Y-33.274
-X-77.851Y-35.814
-X-77.851Y-38.354
-X-70.231Y-33.274
-X-70.231Y-35.814
-X-70.231Y-38.354
 T2
 X-110.236Y-51.308
 X-104.14Y-66.04
@@ -46,16 +46,28 @@ X-29.972Y-75.184
 X-23.876Y-19.812
 X-20.828Y-75.184
 X-16.256Y-19.812
+X-14.986Y-56.134
+X-12.446Y-56.134
+X-9.906Y-56.134
+X-46.736Y-5.334
+X-44.196Y-5.334
+X-41.656Y-5.334
 X-63.881Y-77.089
 X-63.881Y-79.629
 X-63.881Y-82.169
-X-14.986Y-46.609
-X-12.446Y-46.609
-X-9.906Y-46.609
-X-102.616Y-4.699
-X-102.616Y-22.479
-X-61.976Y-4.699
-X-61.976Y-22.479
+X-79.121Y-77.089
+X-79.121Y-79.629
+X-79.121Y-82.169
+X-28.956Y-23.749
+X-28.956Y-26.289
+X-28.956Y-28.829
+X-28.956Y-31.369
+X-28.956Y-33.909
+X-16.256Y-23.749
+X-16.256Y-26.289
+X-16.256Y-28.829
+X-16.256Y-31.369
+X-16.256Y-33.909
 X-28.956Y-5.334
 X-28.956Y-7.874
 X-28.956Y-10.414
@@ -66,44 +78,33 @@ X-16.256Y-7.874
 X-16.256Y-10.414
 X-16.256Y-12.954
 X-16.256Y-15.494
-X-14.986Y-65.659
-X-12.446Y-65.659
-X-9.906Y-65.659
-X-46.736Y-5.334
-X-44.196Y-5.334
-X-41.656Y-5.334
 X-94.996Y-77.089
 X-94.996Y-79.629
 X-94.996Y-82.169
-X-110.236Y-35.814
-X-110.236Y-38.354
-X-110.236Y-40.894
-X-110.236Y-43.434
-X-28.956Y-23.749
-X-28.956Y-26.289
-X-28.956Y-28.829
-X-28.956Y-31.369
-X-28.956Y-33.909
-X-16.256Y-23.749
-X-16.256Y-26.289
-X-16.256Y-28.829
-X-16.256Y-31.369
-X-16.256Y-33.909
-X-46.736Y-18.034
-X-44.196Y-18.034
-X-41.656Y-18.034
-X-14.986Y-56.134
-X-12.446Y-56.134
-X-9.906Y-56.134
-X-79.121Y-77.089
-X-79.121Y-79.629
-X-79.121Y-82.169
 X-110.236Y-56.134
 X-110.236Y-58.674
 X-110.236Y-61.214
 X-110.236Y-63.754
 X-110.236Y-66.294
 X-110.236Y-68.834
+X-91.186Y-7.239
+X-91.186Y-9.779
+X-91.186Y-17.399
+X-91.186Y-19.939
+X-73.406Y-7.239
+X-73.406Y-9.779
+X-73.406Y-17.399
+X-73.406Y-19.939
+X-14.986Y-46.609
+X-12.446Y-46.609
+X-9.906Y-46.609
+X-110.236Y-35.814
+X-110.236Y-38.354
+X-110.236Y-40.894
+X-110.236Y-43.434
+X-14.986Y-65.659
+X-12.446Y-65.659
+X-9.906Y-65.659
 X-42.799Y-70.104
 X-42.799Y-90.424
 X-40.259Y-70.104
@@ -116,6 +117,9 @@ X-32.639Y-70.104
 X-32.639Y-90.424
 X-30.099Y-70.104
 X-30.099Y-90.424
+X-46.736Y-18.034
+X-44.196Y-18.034
+X-41.656Y-18.034
 T3
 X-60.706Y-29.464
 X-60.706Y-32.004
@@ -163,11 +167,11 @@ X-85.471Y-43.434
 X-85.471Y-45.974
 X-85.471Y-48.514
 T5
-X-4.191Y-26.924
-X-4.191Y-32.004
 X-4.191Y-7.874
 X-4.191Y-12.954
 X-110.871Y-10.414
 X-110.871Y-15.494
+X-4.191Y-26.924
+X-4.191Y-32.004
 T0
 M30

+ 2 - 2
PCB/BLOBCNC_TOP/BLOBCNC_TOP-cache.lib

@@ -171,8 +171,8 @@ X 1B 11 700 -200 200 L 50 50 1 1 O
 X 1A 12 700 -100 200 L 50 50 1 1 O
 X 2A 13 700 0 200 L 50 50 1 1 O
 X 2B 14 700 100 200 L 50 50 1 1 O
-X GND 15 700 200 200 L 50 50 1 1 O
-X Vmot 16 700 300 200 L 50 50 1 1 O
+X GND 15 700 200 200 L 50 50 1 1 W
+X Vmot 16 700 300 200 L 50 50 1 1 W
 X MS1 2 -750 200 200 R 50 50 1 1 I
 X MS2 3 -750 100 200 R 50 50 1 1 I
 X MS3 4 -750 0 200 R 50 50 1 1 I

+ 18 - 14
PCB/BLOBCNC_TOP/BLOBCNC_TOP.bak

@@ -647,22 +647,8 @@ Wire Wire Line
 Wire Wire Line
 	6800 3700 7600 3700
 Wire Wire Line
-	7600 3700 7600 3200
-Wire Wire Line
-	7600 3200 8200 3200
-Wire Wire Line
 	6800 3850 7750 3850
 Wire Wire Line
-	7750 3850 7750 3500
-Wire Wire Line
-	7750 3500 8200 3500
-Wire Wire Line
-	6800 4000 9550 4000
-Wire Wire Line
-	9550 4000 9550 3600
-Wire Wire Line
-	9550 3600 9350 3600
-Wire Wire Line
 	9350 3500 9500 3500
 Wire Wire Line
 	9500 3500 9500 4150
@@ -1028,4 +1014,22 @@ Wire Wire Line
 Wire Wire Line
 	8050 2050 8100 2050
 NoConn ~ 6800 2950
+Wire Wire Line
+	6800 4000 7850 4000
+Wire Wire Line
+	7850 4000 7850 3200
+Wire Wire Line
+	7850 3200 8200 3200
+Wire Wire Line
+	7750 3850 7750 3500
+Wire Wire Line
+	7750 3500 8200 3500
+Wire Wire Line
+	7600 3700 7600 4050
+Wire Wire Line
+	7600 4050 9450 4050
+Wire Wire Line
+	9450 4050 9450 3600
+Wire Wire Line
+	9450 3600 9350 3600
 $EndSCHEMATC

+ 314 - 282
PCB/BLOBCNC_TOP/BLOBCNC_TOP.kicad_pcb

@@ -3,7 +3,7 @@
   (general
     (thickness 1.6)
     (drawings 5)
-    (tracks 251)
+    (tracks 263)
     (zones 0)
     (modules 22)
     (nets 45)
@@ -1167,41 +1167,6 @@
     )
   )
 
-  (module "Lien:DC-DC LStepdown M2596" (layer B.Cu) (tedit 5CE1D682) (tstamp 5CEDF184)
-    (at 106.68 42.545 270)
-    (path /5CE1C6E1)
-    (fp_text reference U1 (at 0 -0.5 270) (layer B.SilkS)
-      (effects (font (size 1 1) (thickness 0.15)) (justify mirror))
-    )
-    (fp_text value DC-DC_MP1584EN (at 0 2.54 270) (layer B.Fab)
-      (effects (font (size 1 1) (thickness 0.15)) (justify mirror))
-    )
-    (fp_line (start -10.16 21.59) (end 10.16 21.59) (layer B.SilkS) (width 0.15))
-    (fp_line (start 10.16 21.59) (end 10.16 -21.59) (layer B.SilkS) (width 0.15))
-    (fp_line (start 10.16 -21.59) (end -10.16 -21.59) (layer B.SilkS) (width 0.15))
-    (fp_line (start -10.16 -21.59) (end -10.16 21.59) (layer B.SilkS) (width 0.15))
-    (fp_text user IN- (at -6.35 20.32 270) (layer B.SilkS)
-      (effects (font (size 1 1) (thickness 0.15)) (justify mirror))
-    )
-    (fp_text user IN+ (at 6.35 20.32 270) (layer B.SilkS)
-      (effects (font (size 1 1) (thickness 0.15)) (justify mirror))
-    )
-    (fp_text user OUT- (at -5.08 -20.32 270) (layer B.SilkS)
-      (effects (font (size 1 1) (thickness 0.15)) (justify mirror))
-    )
-    (fp_text user OUT+ (at 5.08 -20.32 270) (layer B.SilkS)
-      (effects (font (size 1 1) (thickness 0.15)) (justify mirror))
-    )
-    (pad 1 thru_hole oval (at -8.89 20.32 270) (size 2 2.6) (drill 1) (layers *.Cu *.Mask)
-      (net 2 GNDPWR))
-    (pad 2 thru_hole oval (at 8.89 20.32 270) (size 2 2.6) (drill 1) (layers *.Cu *.Mask)
-      (net 1 +36V))
-    (pad 3 thru_hole oval (at -8.89 -20.32 270) (size 2 2.6) (drill 1) (layers *.Cu *.Mask)
-      (net 10 GND))
-    (pad 4 thru_hole oval (at 8.89 -20.32 270) (size 2 2.6) (drill 1) (layers *.Cu *.Mask)
-      (net 11 +5V))
-  )
-
   (module Lien:W5500 (layer B.Cu) (tedit 5CBF62F3) (tstamp 5CEDEEEF)
     (at 140.335 109.22 90)
     (path /5CE1C168)
@@ -1382,6 +1347,37 @@
     )
   )
 
+  (module Lien:DC-DC_MP1584EN (layer F.Cu) (tedit 5CD1FF7B) (tstamp 5D26F75F)
+    (at 106.68 42.545 270)
+    (path /5CE1C6E1)
+    (fp_text reference U1 (at 0 0.5 270) (layer F.SilkS)
+      (effects (font (size 1 1) (thickness 0.15)))
+    )
+    (fp_text value DC-DC_MP1584EN (at 0 -0.5 90) (layer F.Fab)
+      (effects (font (size 1 1) (thickness 0.15)))
+    )
+    (fp_line (start -8.509 11.176) (end 8.509 11.176) (layer F.SilkS) (width 0.15))
+    (fp_line (start 8.509 11.176) (end 8.509 -11.176) (layer F.SilkS) (width 0.15))
+    (fp_line (start 8.509 -11.176) (end -8.509 -11.176) (layer F.SilkS) (width 0.15))
+    (fp_line (start -8.509 -11.176) (end -8.509 11.176) (layer F.SilkS) (width 0.15))
+    (pad 1 thru_hole oval (at -6.35 8.89 270) (size 2 2.7) (drill 1) (layers *.Cu *.Mask)
+      (net 2 GNDPWR))
+    (pad 1 thru_hole oval (at -3.81 8.89 270) (size 2 2.7) (drill 1) (layers *.Cu *.Mask)
+      (net 2 GNDPWR))
+    (pad 2 thru_hole oval (at 3.81 8.89 270) (size 2 2.7) (drill 1) (layers *.Cu *.Mask)
+      (net 1 +36V))
+    (pad 2 thru_hole oval (at 6.35 8.89 270) (size 2 2.7) (drill 1) (layers *.Cu *.Mask)
+      (net 1 +36V))
+    (pad 3 thru_hole oval (at -6.35 -8.89 270) (size 2 2.7) (drill 1) (layers *.Cu *.Mask)
+      (net 10 GND))
+    (pad 3 thru_hole oval (at -3.81 -8.89 270) (size 2 2.7) (drill 1) (layers *.Cu *.Mask)
+      (net 10 GND))
+    (pad 4 thru_hole oval (at 3.81 -8.89 270) (size 2 2.7) (drill 1) (layers *.Cu *.Mask)
+      (net 11 +5V))
+    (pad 4 thru_hole oval (at 6.35 -8.89 270) (size 2 2.7) (drill 1) (layers *.Cu *.Mask)
+      (net 11 +5V))
+  )
+
   (gr_line (start 73.66 123.952) (end 73.66 28.956) (layer Edge.Cuts) (width 0.15))
   (gr_line (start 75.692 123.952) (end 73.66 123.952) (layer Edge.Cuts) (width 0.15))
   (gr_line (start 188.976 123.952) (end 75.692 123.952) (layer Edge.Cuts) (width 0.15))
@@ -1396,16 +1392,23 @@
   (segment (start 86.36 51.435) (end 89.16 51.435) (width 1.5) (layer F.Cu) (net 1))
   (segment (start 93.345 59.69) (end 90.805 59.69) (width 1.5) (layer F.Cu) (net 1))
   (segment (start 95.885 62.23) (end 93.345 59.69) (width 1.5) (layer F.Cu) (net 1))
-  (segment (start 80.645 39.37) (end 86.36 33.655) (width 1.5) (layer F.Cu) (net 2))
+  (segment (start 92.4 51.435) (end 89.16 51.435) (width 1.5) (layer F.Cu) (net 1))
+  (segment (start 94.94 48.895) (end 92.4 51.435) (width 1.5) (layer F.Cu) (net 1))
+  (segment (start 97.79 48.895) (end 94.94 48.895) (width 1.5) (layer F.Cu) (net 1))
+  (segment (start 97.79 46.355) (end 97.79 48.895) (width 1.5) (layer F.Cu) (net 1))
   (segment (start 98.385 64.175) (end 98.385 62.23) (width 1.5) (layer F.Cu) (net 2))
   (segment (start 97.79 64.77) (end 98.385 64.175) (width 1.5) (layer F.Cu) (net 2))
   (segment (start 94.615 64.77) (end 97.79 64.77) (width 1.5) (layer F.Cu) (net 2))
   (segment (start 90.805 62.23) (end 92.075 62.23) (width 1.5) (layer F.Cu) (net 2))
   (segment (start 92.075 62.23) (end 94.615 64.77) (width 1.5) (layer F.Cu) (net 2))
-  (segment (start 98.385 62.23) (end 98.385 57.11) (width 1.5) (layer F.Cu) (net 2))
-  (segment (start 80.645 39.37) (end 80.01 39.37) (width 1.5) (layer F.Cu) (net 2))
-  (segment (start 98.385 57.11) (end 80.645 39.37) (width 1.5) (layer F.Cu) (net 2))
-  (segment (start 78.105 39.37) (end 80.01 39.37) (width 1.5) (layer F.Cu) (net 2))
+  (segment (start 97.155 39.37) (end 97.79 38.735) (width 1.5) (layer F.Cu) (net 2))
+  (segment (start 78.105 39.37) (end 97.155 39.37) (width 1.5) (layer F.Cu) (net 2))
+  (segment (start 97.79 38.735) (end 97.79 36.195) (width 1.5) (layer F.Cu) (net 2))
+  (segment (start 100.64 38.735) (end 101.981 40.076) (width 1.5) (layer F.Cu) (net 2))
+  (segment (start 97.79 38.735) (end 100.64 38.735) (width 1.5) (layer F.Cu) (net 2))
+  (segment (start 101.981 40.076) (end 101.981 53.721) (width 1.5) (layer F.Cu) (net 2))
+  (segment (start 98.385 57.317) (end 98.385 62.23) (width 1.5) (layer F.Cu) (net 2))
+  (segment (start 101.981 53.721) (end 98.385 57.317) (width 1.5) (layer F.Cu) (net 2))
   (segment (start 125.095 66.04) (end 128.27 66.04) (width 0.5) (layer F.Cu) (net 3))
   (segment (start 113.03 59.69) (end 114.935 61.595) (width 0.5) (layer F.Cu) (net 3))
   (segment (start 103.505 59.69) (end 113.03 59.69) (width 0.5) (layer F.Cu) (net 3))
@@ -1457,6 +1460,7 @@
   (segment (start 172.72 52.705) (end 172.72 48.768) (width 0.5) (layer F.Cu) (net 10))
   (via (at 165.1 48.768) (size 2) (drill 1) (layers F.Cu B.Cu) (net 10))
   (segment (start 172.72 48.768) (end 165.1 48.768) (width 0.5) (layer B.Cu) (net 10))
+  (segment (start 115.57 36.195) (end 115.57 38.735) (width 0.5) (layer F.Cu) (net 10))
   (segment (start 127 57.15) (end 128.27 58.42) (width 0.5) (layer F.Cu) (net 11))
   (segment (start 127 51.435) (end 127 57.15) (width 0.5) (layer F.Cu) (net 11))
   (segment (start 121.255 58.42) (end 118.745 60.93) (width 0.5) (layer F.Cu) (net 11))
@@ -1505,6 +1509,10 @@
   (segment (start 176.53 72.39) (end 176.53 75.565) (width 0.5) (layer F.Cu) (net 11))
   (segment (start 168.761 64.621) (end 176.53 72.39) (width 0.5) (layer F.Cu) (net 11))
   (segment (start 176.53 75.565) (end 176.53 94.615) (width 0.5) (layer F.Cu) (net 11))
+  (segment (start 115.57 46.355) (end 115.57 48.895) (width 0.5) (layer F.Cu) (net 11))
+  (segment (start 115.6 50.425) (end 115.6 57.785) (width 0.5) (layer F.Cu) (net 11))
+  (segment (start 115.57 50.395) (end 115.6 50.425) (width 0.5) (layer F.Cu) (net 11))
+  (segment (start 115.57 48.895) (end 115.57 50.395) (width 0.5) (layer F.Cu) (net 11))
   (segment (start 90.805 72.39) (end 78.74 72.39) (width 1.5) (layer F.Cu) (net 12))
   (segment (start 78.74 69.85) (end 90.805 69.85) (width 1.5) (layer F.Cu) (net 13))
   (segment (start 90.805 67.31) (end 78.74 67.31) (width 1.5) (layer F.Cu) (net 14))
@@ -1654,203 +1662,227 @@
         (xy 140.151845 29.792576) (xy 140.151844 29.792577) (xy 140.077951 29.841951) (xy 140.028577 29.915844) (xy 134.055845 35.888577)
         (xy 133.981952 35.937951) (xy 133.932578 36.011844) (xy 133.932576 36.011846) (xy 133.786348 36.230691) (xy 133.717663 36.576)
         (xy 133.735001 36.663165) (xy 133.735 41.948421) (xy 126.435845 49.247577) (xy 126.361952 49.296951) (xy 126.312578 49.370844)
-        (xy 126.312576 49.370846) (xy 126.166348 49.589691) (xy 126.107442 49.885836) (xy 126.062055 49.894864) (xy 125.521231 50.256231)
-        (xy 125.159864 50.797055) (xy 125.032969 51.435) (xy 125.159864 52.072945) (xy 125.521231 52.613769) (xy 126.062055 52.975136)
-        (xy 126.115 52.985667) (xy 126.115001 57.062835) (xy 126.097663 57.15) (xy 126.166348 57.495309) (xy 126.192869 57.535)
+        (xy 126.312576 49.370846) (xy 126.166348 49.589691) (xy 126.097663 49.935) (xy 126.115001 50.022165) (xy 126.115 51.347835)
+        (xy 126.115 51.347836) (xy 126.115001 57.062835) (xy 126.097663 57.15) (xy 126.166348 57.495309) (xy 126.192869 57.535)
         (xy 121.342159 57.535) (xy 121.254999 57.517663) (xy 121.16784 57.535) (xy 121.167835 57.535) (xy 120.90969 57.586348)
         (xy 120.754616 57.689966) (xy 120.690845 57.732576) (xy 120.690844 57.732577) (xy 120.616951 57.781951) (xy 120.567577 57.855844)
-        (xy 118.745 59.678421) (xy 116.287425 57.220847) (xy 116.238049 57.146951) (xy 115.94531 56.951348) (xy 115.687165 56.9)
-        (xy 115.687161 56.9) (xy 115.6 56.882663) (xy 115.512839 56.9) (xy 101.687159 56.9) (xy 101.599999 56.882663)
-        (xy 101.512839 56.9) (xy 101.512835 56.9) (xy 101.25469 56.951348) (xy 101.147456 57.023) (xy 101.035845 57.097576)
-        (xy 101.035844 57.097577) (xy 100.961951 57.146951) (xy 100.912577 57.220844) (xy 99.77 58.363422) (xy 99.77 57.246407)
-        (xy 99.797133 57.11) (xy 99.689641 56.5696) (xy 99.512626 56.304678) (xy 99.383529 56.111471) (xy 99.267888 56.034202)
-        (xy 82.603685 39.37) (xy 86.683686 35.29) (xy 86.821031 35.29) (xy 87.297945 35.195136) (xy 87.838769 34.833769)
-        (xy 88.200136 34.292945) (xy 88.251358 34.035434) (xy 125.109876 34.035434) (xy 125.140856 34.163355) (xy 125.454078 34.721317)
-        (xy 125.95698 35.116942) (xy 126.573 35.29) (xy 126.873 35.29) (xy 126.873 33.782) (xy 127.127 33.782)
-        (xy 127.127 35.29) (xy 127.427 35.29) (xy 128.04302 35.116942) (xy 128.545922 34.721317) (xy 128.859144 34.163355)
-        (xy 128.890124 34.035434) (xy 128.770777 33.782) (xy 127.127 33.782) (xy 126.873 33.782) (xy 125.229223 33.782)
-        (xy 125.109876 34.035434) (xy 88.251358 34.035434) (xy 88.327031 33.655) (xy 88.251359 33.274566) (xy 125.109876 33.274566)
-        (xy 125.229223 33.528) (xy 126.873 33.528) (xy 126.873 32.02) (xy 127.127 32.02) (xy 127.127 33.528)
-        (xy 128.770777 33.528) (xy 128.890124 33.274566) (xy 128.859144 33.146645) (xy 128.545922 32.588683) (xy 128.04302 32.193058)
-        (xy 127.427 32.02) (xy 127.127 32.02) (xy 126.873 32.02) (xy 126.573 32.02) (xy 125.95698 32.193058)
-        (xy 125.454078 32.588683) (xy 125.140856 33.146645) (xy 125.109876 33.274566) (xy 88.251359 33.274566) (xy 88.200136 33.017055)
-        (xy 87.838769 32.476231) (xy 87.297945 32.114864) (xy 86.821031 32.02) (xy 85.898969 32.02) (xy 85.422055 32.114864)
-        (xy 84.881231 32.476231) (xy 84.519864 33.017055) (xy 84.392969 33.655) (xy 84.394354 33.661961) (xy 80.241469 37.814846)
+        (xy 118.745 59.678421) (xy 116.485 57.418422) (xy 116.485 50.512159) (xy 116.497947 50.44707) (xy 116.557945 50.435136)
+        (xy 117.098769 50.073769) (xy 117.460136 49.532945) (xy 117.587031 48.895) (xy 117.460136 48.257055) (xy 117.098769 47.716231)
+        (xy 116.962232 47.625) (xy 117.098769 47.533769) (xy 117.460136 46.992945) (xy 117.587031 46.355) (xy 117.460136 45.717055)
+        (xy 117.098769 45.176231) (xy 116.557945 44.814864) (xy 116.081031 44.72) (xy 115.058969 44.72) (xy 114.582055 44.814864)
+        (xy 114.041231 45.176231) (xy 113.679864 45.717055) (xy 113.552969 46.355) (xy 113.679864 46.992945) (xy 114.041231 47.533769)
+        (xy 114.177768 47.625) (xy 114.041231 47.716231) (xy 113.679864 48.257055) (xy 113.552969 48.895) (xy 113.679864 49.532945)
+        (xy 114.041231 50.073769) (xy 114.582055 50.435136) (xy 114.679502 50.454519) (xy 114.715 50.632984) (xy 114.715001 56.9)
+        (xy 101.687159 56.9) (xy 101.599999 56.882663) (xy 101.512839 56.9) (xy 101.512835 56.9) (xy 101.25469 56.951348)
+        (xy 101.254688 56.951349) (xy 101.254689 56.951349) (xy 101.035845 57.097576) (xy 101.035844 57.097577) (xy 100.961951 57.146951)
+        (xy 100.912577 57.220844) (xy 99.77 58.363422) (xy 99.77 57.890685) (xy 102.863888 54.796798) (xy 102.979529 54.719529)
+        (xy 103.285641 54.2614) (xy 103.366 53.857407) (xy 103.393133 53.721) (xy 103.366 53.584593) (xy 103.366 40.212401)
+        (xy 103.393132 40.075999) (xy 103.366 39.939597) (xy 103.366 39.939593) (xy 103.285641 39.5356) (xy 103.004895 39.115434)
+        (xy 113.629876 39.115434) (xy 113.660856 39.243355) (xy 113.974078 39.801317) (xy 114.47698 40.196942) (xy 115.093 40.37)
+        (xy 115.443 40.37) (xy 115.443 38.862) (xy 115.697 38.862) (xy 115.697 40.37) (xy 116.047 40.37)
+        (xy 116.66302 40.196942) (xy 117.165922 39.801317) (xy 117.479144 39.243355) (xy 117.510124 39.115434) (xy 117.390777 38.862)
+        (xy 115.697 38.862) (xy 115.443 38.862) (xy 113.749223 38.862) (xy 113.629876 39.115434) (xy 103.004895 39.115434)
+        (xy 102.979528 39.077471) (xy 102.863887 39.000202) (xy 101.7158 37.852115) (xy 101.638529 37.736471) (xy 101.1804 37.430359)
+        (xy 100.776407 37.35) (xy 100.64 37.322867) (xy 100.503593 37.35) (xy 99.334651 37.35) (xy 99.680136 36.832945)
+        (xy 99.731358 36.575434) (xy 113.629876 36.575434) (xy 113.660856 36.703355) (xy 113.974078 37.261317) (xy 114.232991 37.465)
+        (xy 113.974078 37.668683) (xy 113.660856 38.226645) (xy 113.629876 38.354566) (xy 113.749223 38.608) (xy 115.443 38.608)
+        (xy 115.443 36.322) (xy 115.697 36.322) (xy 115.697 38.608) (xy 117.390777 38.608) (xy 117.510124 38.354566)
+        (xy 117.479144 38.226645) (xy 117.165922 37.668683) (xy 116.907009 37.465) (xy 117.165922 37.261317) (xy 117.479144 36.703355)
+        (xy 117.510124 36.575434) (xy 117.390777 36.322) (xy 115.697 36.322) (xy 115.443 36.322) (xy 113.749223 36.322)
+        (xy 113.629876 36.575434) (xy 99.731358 36.575434) (xy 99.807031 36.195) (xy 99.731359 35.814566) (xy 113.629876 35.814566)
+        (xy 113.749223 36.068) (xy 115.443 36.068) (xy 115.443 34.56) (xy 115.697 34.56) (xy 115.697 36.068)
+        (xy 117.390777 36.068) (xy 117.510124 35.814566) (xy 117.479144 35.686645) (xy 117.165922 35.128683) (xy 116.66302 34.733058)
+        (xy 116.047 34.56) (xy 115.697 34.56) (xy 115.443 34.56) (xy 115.093 34.56) (xy 114.47698 34.733058)
+        (xy 113.974078 35.128683) (xy 113.660856 35.686645) (xy 113.629876 35.814566) (xy 99.731359 35.814566) (xy 99.680136 35.557055)
+        (xy 99.318769 35.016231) (xy 98.777945 34.654864) (xy 98.301031 34.56) (xy 97.278969 34.56) (xy 96.802055 34.654864)
+        (xy 96.261231 35.016231) (xy 95.899864 35.557055) (xy 95.772969 36.195) (xy 95.899864 36.832945) (xy 96.261231 37.373769)
+        (xy 96.397768 37.465) (xy 96.261231 37.556231) (xy 95.974737 37.985) (xy 80.25244 37.985) (xy 80.25244 37.87)
         (xy 80.203157 37.622235) (xy 80.062809 37.412191) (xy 79.852765 37.271843) (xy 79.605 37.22256) (xy 76.605 37.22256)
         (xy 76.357235 37.271843) (xy 76.147191 37.412191) (xy 76.006843 37.622235) (xy 75.95756 37.87) (xy 75.95756 40.87)
         (xy 76.006843 41.117765) (xy 76.147191 41.327809) (xy 76.357235 41.468157) (xy 76.605 41.51744) (xy 79.605 41.51744)
-        (xy 79.852765 41.468157) (xy 80.062809 41.327809) (xy 80.203157 41.117765) (xy 80.241469 40.925154) (xy 89.383673 50.067358)
-        (xy 89.296407 50.05) (xy 89.16 50.022867) (xy 89.023593 50.05) (xy 87.530122 50.05) (xy 87.297945 49.894864)
-        (xy 86.821031 49.8) (xy 86.683686 49.8) (xy 80.450799 43.567114) (xy 80.373529 43.451471) (xy 79.9154 43.145359)
-        (xy 79.795944 43.121598) (xy 79.31438 42.640034) (xy 78.529678 42.315) (xy 77.680322 42.315) (xy 76.89562 42.640034)
-        (xy 76.295034 43.24062) (xy 75.97 44.025322) (xy 75.97 44.874678) (xy 76.295034 45.65938) (xy 76.89562 46.259966)
-        (xy 77.680322 46.585) (xy 78.529678 46.585) (xy 79.252084 46.28577) (xy 84.394354 51.428039) (xy 84.392969 51.435)
-        (xy 84.519864 52.072945) (xy 84.881231 52.613769) (xy 85.422055 52.975136) (xy 85.898969 53.07) (xy 86.821031 53.07)
-        (xy 87.297945 52.975136) (xy 87.530122 52.82) (xy 88.586315 52.82) (xy 94.5 58.733686) (xy 94.5 58.886315)
-        (xy 94.4208 58.807115) (xy 94.343529 58.691471) (xy 93.8854 58.385359) (xy 93.481407 58.305) (xy 93.345 58.277867)
-        (xy 93.208593 58.305) (xy 91.975122 58.305) (xy 91.742945 58.149864) (xy 91.266031 58.055) (xy 90.343969 58.055)
-        (xy 89.867055 58.149864) (xy 89.326231 58.511231) (xy 88.964864 59.052055) (xy 88.837969 59.69) (xy 88.964864 60.327945)
-        (xy 89.326231 60.868769) (xy 89.462768 60.96) (xy 89.326231 61.051231) (xy 88.964864 61.592055) (xy 88.837969 62.23)
-        (xy 88.964864 62.867945) (xy 89.310349 63.385) (xy 79.932285 63.385) (xy 79.837765 63.321843) (xy 79.59 63.27256)
-        (xy 77.89 63.27256) (xy 77.642235 63.321843) (xy 77.432191 63.462191) (xy 77.291843 63.672235) (xy 77.24256 63.92)
-        (xy 77.24256 65.62) (xy 77.291843 65.867765) (xy 77.432191 66.077809) (xy 77.642235 66.218157) (xy 77.687619 66.227184)
-        (xy 77.669375 66.239375) (xy 77.341161 66.730582) (xy 77.225908 67.31) (xy 77.341161 67.889418) (xy 77.669375 68.380625)
-        (xy 77.967761 68.58) (xy 77.669375 68.779375) (xy 77.341161 69.270582) (xy 77.225908 69.85) (xy 77.341161 70.429418)
-        (xy 77.669375 70.920625) (xy 77.967761 71.12) (xy 77.669375 71.319375) (xy 77.341161 71.810582) (xy 77.225908 72.39)
-        (xy 77.341161 72.969418) (xy 77.669375 73.460625) (xy 78.160582 73.788839) (xy 78.593744 73.875) (xy 78.886256 73.875)
-        (xy 79.319418 73.788839) (xy 79.34013 73.775) (xy 89.310349 73.775) (xy 89.129941 74.045) (xy 88.987161 74.045)
-        (xy 88.9 74.027663) (xy 88.812839 74.045) (xy 88.812835 74.045) (xy 88.55469 74.096348) (xy 88.335845 74.242576)
-        (xy 88.335844 74.242577) (xy 88.261951 74.291951) (xy 88.212577 74.365844) (xy 78.95896 83.619462) (xy 78.886256 83.605)
-        (xy 78.593744 83.605) (xy 78.160582 83.691161) (xy 77.669375 84.019375) (xy 77.554698 84.191001) (xy 77.537919 84.187663)
-        (xy 77.450758 84.205) (xy 77.450754 84.205) (xy 77.192609 84.256348) (xy 76.973764 84.402576) (xy 76.973763 84.402577)
-        (xy 76.89987 84.451951) (xy 76.850496 84.525844) (xy 75.000847 86.375494) (xy 74.926951 86.42487) (xy 74.731348 86.71761)
-        (xy 74.68 86.975755) (xy 74.68 86.975758) (xy 74.662663 87.062919) (xy 74.68 87.15008) (xy 74.680001 90.082831)
-        (xy 74.68 90.082836) (xy 74.68 90.082839) (xy 74.662663 90.17) (xy 74.68 90.257161) (xy 74.680001 95.162831)
-        (xy 74.662662 95.25) (xy 74.68 95.337164) (xy 74.680001 102.147835) (xy 74.662663 102.235) (xy 74.731348 102.580309)
-        (xy 74.877576 102.799154) (xy 74.877578 102.799156) (xy 74.926952 102.873049) (xy 75.000845 102.922423) (xy 81.227577 109.149156)
-        (xy 81.276951 109.223049) (xy 81.350844 109.272423) (xy 81.350845 109.272424) (xy 81.46188 109.346615) (xy 81.56969 109.418652)
-        (xy 81.827835 109.47) (xy 81.827839 109.47) (xy 81.914999 109.487337) (xy 82.002159 109.47) (xy 92.785344 109.47)
-        (xy 92.909375 109.655625) (xy 92.931033 109.670096) (xy 92.770302 109.736673) (xy 92.591673 109.915301) (xy 92.495 110.14869)
-        (xy 92.495 110.83925) (xy 92.65375 110.998) (xy 93.853 110.998) (xy 93.853 110.978) (xy 94.107 110.978)
-        (xy 94.107 110.998) (xy 95.30625 110.998) (xy 95.465 110.83925) (xy 95.465 110.14869) (xy 95.368327 109.915301)
-        (xy 95.189698 109.736673) (xy 95.028967 109.670096) (xy 95.050625 109.655625) (xy 95.174656 109.47) (xy 108.660344 109.47)
-        (xy 108.784375 109.655625) (xy 108.806033 109.670096) (xy 108.645302 109.736673) (xy 108.466673 109.915301) (xy 108.37 110.14869)
-        (xy 108.37 110.83925) (xy 108.52875 110.998) (xy 109.728 110.998) (xy 109.728 110.978) (xy 109.982 110.978)
-        (xy 109.982 110.998) (xy 111.18125 110.998) (xy 111.34 110.83925) (xy 111.34 110.14869) (xy 111.243327 109.915301)
-        (xy 111.064698 109.736673) (xy 110.903967 109.670096) (xy 110.925625 109.655625) (xy 111.049656 109.47) (xy 123.900344 109.47)
-        (xy 124.024375 109.655625) (xy 124.046033 109.670096) (xy 123.885302 109.736673) (xy 123.706673 109.915301) (xy 123.61 110.14869)
-        (xy 123.61 110.83925) (xy 123.76875 110.998) (xy 124.968 110.998) (xy 124.968 110.978) (xy 125.222 110.978)
-        (xy 125.222 110.998) (xy 126.42125 110.998) (xy 126.58 110.83925) (xy 126.58 110.14869) (xy 126.483327 109.915301)
-        (xy 126.304698 109.736673) (xy 126.143967 109.670096) (xy 126.165625 109.655625) (xy 126.493839 109.164418) (xy 126.609092 108.585)
-        (xy 126.493839 108.005582) (xy 126.165625 107.514375) (xy 125.867239 107.315) (xy 126.165625 107.115625) (xy 126.493839 106.624418)
-        (xy 126.609092 106.045) (xy 126.493839 105.465582) (xy 126.165625 104.974375) (xy 125.98 104.850344) (xy 125.98 89.901578)
-        (xy 126.096579 89.785) (xy 126.355682 89.785) (xy 126.43732 89.90718) (xy 126.830658 90.17) (xy 126.43732 90.43282)
-        (xy 126.128556 90.894918) (xy 126.020132 91.44) (xy 126.128556 91.985082) (xy 126.43732 92.44718) (xy 126.899418 92.755944)
-        (xy 127.306912 92.837) (xy 127.394516 92.837) (xy 127.436348 93.047309) (xy 127.582576 93.266154) (xy 127.582578 93.266156)
-        (xy 127.631952 93.340049) (xy 127.705845 93.389423) (xy 141.355 107.038579) (xy 141.355001 119.927834) (xy 141.337663 120.015)
-        (xy 141.406348 120.360309) (xy 141.552576 120.579154) (xy 141.552578 120.579156) (xy 141.601952 120.653049) (xy 141.675845 120.702423)
-        (xy 143.457577 122.484156) (xy 143.506951 122.558049) (xy 143.580844 122.607423) (xy 143.580845 122.607424) (xy 143.79969 122.753652)
-        (xy 144.057835 122.805) (xy 144.057839 122.805) (xy 144.145 122.822337) (xy 144.232161 122.805) (xy 150.429839 122.805)
-        (xy 150.517 122.822337) (xy 150.604161 122.805) (xy 150.604165 122.805) (xy 150.86231 122.753652) (xy 151.155049 122.558049)
-        (xy 151.204425 122.484153) (xy 151.821156 121.867423) (xy 151.895049 121.818049) (xy 152.090652 121.52531) (xy 152.142 121.267165)
-        (xy 152.142 121.26716) (xy 152.159337 121.180001) (xy 152.142 121.092841) (xy 152.142 121.055059) (xy 152.435769 120.858769)
-        (xy 152.527 120.722232) (xy 152.618231 120.858769) (xy 153.159056 121.220136) (xy 153.797 121.347031) (xy 154.434945 121.220136)
-        (xy 154.975769 120.858769) (xy 155.086952 120.692371) (xy 155.270683 120.925922) (xy 155.828645 121.239144) (xy 155.956566 121.270124)
-        (xy 156.21 121.150777) (xy 156.21 119.507) (xy 156.464 119.507) (xy 156.464 121.150777) (xy 156.717434 121.270124)
-        (xy 156.845355 121.239144) (xy 157.312632 120.97683) (xy 157.338673 121.039698) (xy 157.517301 121.218327) (xy 157.75069 121.315)
-        (xy 158.59125 121.315) (xy 158.75 121.15625) (xy 158.75 119.507) (xy 159.004 119.507) (xy 159.004 121.15625)
-        (xy 159.16275 121.315) (xy 160.00331 121.315) (xy 160.236699 121.218327) (xy 160.415327 121.039698) (xy 160.512 120.806309)
-        (xy 160.512 119.66575) (xy 160.35325 119.507) (xy 159.004 119.507) (xy 158.75 119.507) (xy 156.464 119.507)
-        (xy 156.21 119.507) (xy 156.19 119.507) (xy 156.19 119.253) (xy 156.21 119.253) (xy 156.21 117.609223)
-        (xy 156.464 117.609223) (xy 156.464 119.253) (xy 158.75 119.253) (xy 158.75 117.60375) (xy 159.004 117.60375)
-        (xy 159.004 119.253) (xy 160.35325 119.253) (xy 160.512 119.09425) (xy 160.512 117.953691) (xy 160.415327 117.720302)
-        (xy 160.236699 117.541673) (xy 160.00331 117.445) (xy 159.16275 117.445) (xy 159.004 117.60375) (xy 158.75 117.60375)
-        (xy 158.59125 117.445) (xy 157.75069 117.445) (xy 157.517301 117.541673) (xy 157.338673 117.720302) (xy 157.312632 117.78317)
-        (xy 156.845355 117.520856) (xy 156.717434 117.489876) (xy 156.464 117.609223) (xy 156.21 117.609223) (xy 155.956566 117.489876)
-        (xy 155.828645 117.520856) (xy 155.270683 117.834078) (xy 155.086952 118.067629) (xy 154.975769 117.901231) (xy 154.682 117.704941)
-        (xy 154.682 116.825578) (xy 164.140156 107.367423) (xy 164.214049 107.318049) (xy 164.409652 107.02531) (xy 164.461 106.767165)
-        (xy 164.461 106.767161) (xy 164.478337 106.680001) (xy 164.461 106.592841) (xy 164.461 96.607165) (xy 164.478338 96.52)
-        (xy 164.409652 96.17469) (xy 164.214049 95.881951) (xy 163.92131 95.686348) (xy 163.663165 95.635) (xy 163.663164 95.635)
-        (xy 163.576 95.617662) (xy 163.488835 95.635) (xy 152.322079 95.635) (xy 145.7312 89.044122) (xy 145.759868 88.9)
-        (xy 145.651444 88.354918) (xy 145.34268 87.89282) (xy 144.949342 87.63) (xy 145.34268 87.36718) (xy 145.651444 86.905082)
-        (xy 145.759868 86.36) (xy 145.651444 85.814918) (xy 145.34268 85.35282) (xy 144.949342 85.09) (xy 145.34268 84.82718)
-        (xy 145.651444 84.365082) (xy 145.759868 83.82) (xy 145.651444 83.274918) (xy 145.34268 82.81282) (xy 144.949342 82.55)
-        (xy 145.34268 82.28718) (xy 145.346854 82.280932) (xy 158.245077 95.179156) (xy 158.294451 95.253049) (xy 158.368344 95.302423)
-        (xy 158.368345 95.302424) (xy 158.499917 95.390338) (xy 158.58719 95.448652) (xy 158.845335 95.5) (xy 158.845339 95.5)
-        (xy 158.9325 95.517337) (xy 159.019661 95.5) (xy 172.795344 95.5) (xy 172.919375 95.685625) (xy 173.410582 96.013839)
-        (xy 173.843744 96.1) (xy 174.136256 96.1) (xy 174.569418 96.013839) (xy 175.060625 95.685625) (xy 175.26 95.387239)
-        (xy 175.459375 95.685625) (xy 175.950582 96.013839) (xy 176.383744 96.1) (xy 176.676256 96.1) (xy 177.109418 96.013839)
-        (xy 177.600625 95.685625) (xy 177.615096 95.663967) (xy 177.681673 95.824698) (xy 177.860301 96.003327) (xy 178.09369 96.1)
-        (xy 178.78425 96.1) (xy 178.943 95.94125) (xy 178.943 94.742) (xy 179.197 94.742) (xy 179.197 95.94125)
-        (xy 179.35575 96.1) (xy 180.04631 96.1) (xy 180.279699 96.003327) (xy 180.458327 95.824698) (xy 180.555 95.591309)
-        (xy 180.555 94.90075) (xy 180.39625 94.742) (xy 179.197 94.742) (xy 178.943 94.742) (xy 178.923 94.742)
-        (xy 178.923 94.488) (xy 178.943 94.488) (xy 178.943 93.28875) (xy 179.197 93.28875) (xy 179.197 94.488)
-        (xy 180.39625 94.488) (xy 180.555 94.32925) (xy 180.555 93.638691) (xy 180.458327 93.405302) (xy 180.279699 93.226673)
-        (xy 180.04631 93.13) (xy 179.35575 93.13) (xy 179.197 93.28875) (xy 178.943 93.28875) (xy 178.78425 93.13)
-        (xy 178.09369 93.13) (xy 177.860301 93.226673) (xy 177.681673 93.405302) (xy 177.615096 93.566033) (xy 177.600625 93.544375)
-        (xy 177.415 93.420344) (xy 177.415 86.284656) (xy 177.600625 86.160625) (xy 177.615096 86.138967) (xy 177.681673 86.299698)
-        (xy 177.860301 86.478327) (xy 178.09369 86.575) (xy 178.78425 86.575) (xy 178.943 86.41625) (xy 178.943 85.217)
-        (xy 179.197 85.217) (xy 179.197 86.41625) (xy 179.35575 86.575) (xy 180.04631 86.575) (xy 180.279699 86.478327)
-        (xy 180.458327 86.299698) (xy 180.555 86.066309) (xy 180.555 85.37575) (xy 180.39625 85.217) (xy 179.197 85.217)
-        (xy 178.943 85.217) (xy 178.923 85.217) (xy 178.923 84.963) (xy 178.943 84.963) (xy 178.943 83.76375)
-        (xy 179.197 83.76375) (xy 179.197 84.963) (xy 180.39625 84.963) (xy 180.555 84.80425) (xy 180.555 84.113691)
-        (xy 180.458327 83.880302) (xy 180.279699 83.701673) (xy 180.04631 83.605) (xy 179.35575 83.605) (xy 179.197 83.76375)
-        (xy 178.943 83.76375) (xy 178.78425 83.605) (xy 178.09369 83.605) (xy 177.860301 83.701673) (xy 177.681673 83.880302)
-        (xy 177.615096 84.041033) (xy 177.600625 84.019375) (xy 177.415 83.895344) (xy 177.415 76.759656) (xy 177.600625 76.635625)
-        (xy 177.615096 76.613967) (xy 177.681673 76.774698) (xy 177.860301 76.953327) (xy 178.09369 77.05) (xy 178.78425 77.05)
-        (xy 178.943 76.89125) (xy 178.943 75.692) (xy 179.197 75.692) (xy 179.197 76.89125) (xy 179.35575 77.05)
-        (xy 180.04631 77.05) (xy 180.279699 76.953327) (xy 180.458327 76.774698) (xy 180.555 76.541309) (xy 180.555 75.85075)
-        (xy 180.39625 75.692) (xy 179.197 75.692) (xy 178.943 75.692) (xy 178.923 75.692) (xy 178.923 75.438)
-        (xy 178.943 75.438) (xy 178.943 74.23875) (xy 179.197 74.23875) (xy 179.197 75.438) (xy 180.39625 75.438)
-        (xy 180.555 75.27925) (xy 180.555 74.588691) (xy 180.458327 74.355302) (xy 180.279699 74.176673) (xy 180.04631 74.08)
-        (xy 179.35575 74.08) (xy 179.197 74.23875) (xy 178.943 74.23875) (xy 178.78425 74.08) (xy 178.09369 74.08)
-        (xy 177.860301 74.176673) (xy 177.681673 74.355302) (xy 177.615096 74.516033) (xy 177.600625 74.494375) (xy 177.415 74.370344)
-        (xy 177.415 72.477159) (xy 177.432337 72.389999) (xy 177.415 72.302839) (xy 177.415 72.302835) (xy 177.363652 72.04469)
-        (xy 177.291615 71.93688) (xy 177.217424 71.825845) (xy 177.217423 71.825844) (xy 177.168049 71.751951) (xy 177.094156 71.702578)
-        (xy 169.646 64.254422) (xy 169.646 57.770578) (xy 171.141727 56.274851) (xy 171.241231 56.423769) (xy 171.377768 56.515)
-        (xy 171.241231 56.606231) (xy 170.879864 57.147055) (xy 170.752969 57.785) (xy 170.879864 58.422945) (xy 171.241231 58.963769)
-        (xy 171.377768 59.055) (xy 171.241231 59.146231) (xy 170.879864 59.687055) (xy 170.752969 60.325) (xy 170.879864 60.962945)
-        (xy 171.241231 61.503769) (xy 171.377768 61.595) (xy 171.241231 61.686231) (xy 170.879864 62.227055) (xy 170.752969 62.865)
-        (xy 170.879864 63.502945) (xy 171.241231 64.043769) (xy 171.782055 64.405136) (xy 172.258969 64.5) (xy 173.181031 64.5)
-        (xy 173.657945 64.405136) (xy 174.198769 64.043769) (xy 174.560136 63.502945) (xy 174.687031 62.865) (xy 174.560136 62.227055)
-        (xy 174.214651 61.71) (xy 182.63756 61.71) (xy 182.63756 62.46) (xy 182.686843 62.707765) (xy 182.827191 62.917809)
-        (xy 183.037235 63.058157) (xy 183.285 63.10744) (xy 186.285 63.10744) (xy 186.532765 63.058157) (xy 186.742809 62.917809)
-        (xy 186.883157 62.707765) (xy 186.93244 62.46) (xy 186.93244 59.46) (xy 186.883157 59.212235) (xy 186.742809 59.002191)
-        (xy 186.532765 58.861843) (xy 186.285 58.81256) (xy 183.285 58.81256) (xy 183.037235 58.861843) (xy 182.920266 58.94)
-        (xy 179.199354 58.94) (xy 179.433529 58.783529) (xy 179.510799 58.667886) (xy 180.913686 57.265) (xy 183.150654 57.265)
-        (xy 183.57562 57.689966) (xy 184.360322 58.015) (xy 185.209678 58.015) (xy 185.99438 57.689966) (xy 186.594966 57.08938)
-        (xy 186.92 56.304678) (xy 186.92 55.455322) (xy 186.594966 54.67062) (xy 185.99438 54.070034) (xy 185.209678 53.745)
-        (xy 184.360322 53.745) (xy 183.57562 54.070034) (xy 183.150654 54.495) (xy 180.476407 54.495) (xy 180.34 54.467867)
-        (xy 180.203593 54.495) (xy 179.7996 54.575359) (xy 179.341471 54.881471) (xy 179.264202 54.997112) (xy 177.861315 56.4)
-        (xy 174.214651 56.4) (xy 174.560136 55.882945) (xy 174.687031 55.245) (xy 174.560136 54.607055) (xy 174.198769 54.066231)
-        (xy 174.032371 53.955048) (xy 174.265922 53.771317) (xy 174.579144 53.213355) (xy 174.610124 53.085434) (xy 174.490777 52.832)
-        (xy 172.847 52.832) (xy 172.847 52.852) (xy 172.593 52.852) (xy 172.593 52.832) (xy 170.949223 52.832)
-        (xy 170.829876 53.085434) (xy 170.860856 53.213355) (xy 171.174078 53.771317) (xy 171.407629 53.955048) (xy 171.241231 54.066231)
-        (xy 171.044941 54.36) (xy 171.007159 54.36) (xy 170.919999 54.342663) (xy 170.911861 54.344282) (xy 169.541 52.973422)
-        (xy 169.541 52.324566) (xy 170.829876 52.324566) (xy 170.949223 52.578) (xy 172.593 52.578) (xy 172.593 51.07)
-        (xy 172.847 51.07) (xy 172.847 52.578) (xy 174.490777 52.578) (xy 174.610124 52.324566) (xy 174.579144 52.196645)
-        (xy 174.265922 51.638683) (xy 173.76302 51.243058) (xy 173.147 51.07) (xy 172.847 51.07) (xy 172.593 51.07)
-        (xy 172.293 51.07) (xy 171.67698 51.243058) (xy 171.174078 51.638683) (xy 170.860856 52.196645) (xy 170.829876 52.324566)
-        (xy 169.541 52.324566) (xy 169.541 39.460578) (xy 171.141727 37.859851) (xy 171.241231 38.008769) (xy 171.377768 38.1)
-        (xy 171.241231 38.191231) (xy 170.879864 38.732055) (xy 170.752969 39.37) (xy 170.879864 40.007945) (xy 171.241231 40.548769)
-        (xy 171.377768 40.64) (xy 171.241231 40.731231) (xy 170.879864 41.272055) (xy 170.752969 41.91) (xy 170.879864 42.547945)
-        (xy 171.241231 43.088769) (xy 171.377768 43.18) (xy 171.241231 43.271231) (xy 170.879864 43.812055) (xy 170.752969 44.45)
-        (xy 170.879864 45.087945) (xy 171.241231 45.628769) (xy 171.782055 45.990136) (xy 172.258969 46.085) (xy 173.181031 46.085)
-        (xy 173.657945 45.990136) (xy 174.198769 45.628769) (xy 174.560136 45.087945) (xy 174.687031 44.45) (xy 174.560136 43.812055)
-        (xy 174.214651 43.295) (xy 182.63756 43.295) (xy 182.63756 43.41) (xy 182.686843 43.657765) (xy 182.827191 43.867809)
-        (xy 183.037235 44.008157) (xy 183.285 44.05744) (xy 186.285 44.05744) (xy 186.532765 44.008157) (xy 186.742809 43.867809)
-        (xy 186.883157 43.657765) (xy 186.93244 43.41) (xy 186.93244 40.41) (xy 186.883157 40.162235) (xy 186.742809 39.952191)
-        (xy 186.532765 39.811843) (xy 186.285 39.76256) (xy 183.285 39.76256) (xy 183.037235 39.811843) (xy 182.827191 39.952191)
-        (xy 182.686843 40.162235) (xy 182.63756 40.41) (xy 182.63756 40.525) (xy 177.929354 40.525) (xy 178.163529 40.368529)
-        (xy 178.2408 40.252885) (xy 180.278685 38.215) (xy 183.150654 38.215) (xy 183.57562 38.639966) (xy 184.360322 38.965)
-        (xy 185.209678 38.965) (xy 185.99438 38.639966) (xy 186.594966 38.03938) (xy 186.92 37.254678) (xy 186.92 36.405322)
-        (xy 186.594966 35.62062) (xy 185.99438 35.020034) (xy 185.209678 34.695) (xy 184.360322 34.695) (xy 183.57562 35.020034)
-        (xy 183.150654 35.445) (xy 179.841401 35.445) (xy 179.704999 35.417868) (xy 179.568597 35.445) (xy 179.568593 35.445)
-        (xy 179.1646 35.525359) (xy 178.706471 35.831471) (xy 178.6292 35.947115) (xy 176.591315 37.985) (xy 174.214651 37.985)
-        (xy 174.560136 37.467945) (xy 174.687031 36.83) (xy 174.560136 36.192055) (xy 174.198769 35.651231) (xy 174.032371 35.540048)
-        (xy 174.265922 35.356317) (xy 174.579144 34.798355) (xy 174.610124 34.670434) (xy 174.490777 34.417) (xy 172.847 34.417)
-        (xy 172.847 34.437) (xy 172.593 34.437) (xy 172.593 34.417) (xy 170.949223 34.417) (xy 170.829876 34.670434)
-        (xy 170.860856 34.798355) (xy 171.174078 35.356317) (xy 171.407629 35.540048) (xy 171.241231 35.651231) (xy 171.044941 35.945)
-        (xy 171.007161 35.945) (xy 170.92 35.927663) (xy 170.832839 35.945) (xy 170.800579 35.945) (xy 168.765145 33.909566)
-        (xy 170.829876 33.909566) (xy 170.949223 34.163) (xy 172.593 34.163) (xy 172.593 32.655) (xy 172.847 32.655)
-        (xy 172.847 34.163) (xy 174.490777 34.163) (xy 174.610124 33.909566) (xy 174.579144 33.781645) (xy 174.265922 33.223683)
-        (xy 173.76302 32.828058) (xy 173.147 32.655) (xy 172.847 32.655) (xy 172.593 32.655) (xy 172.293 32.655)
-        (xy 171.67698 32.828058) (xy 171.174078 33.223683) (xy 170.860856 33.781645) (xy 170.829876 33.909566) (xy 168.765145 33.909566)
-        (xy 164.771425 29.915847) (xy 164.722049 29.841951) (xy 164.496136 29.691) (xy 188.241 29.691) (xy 188.241001 123.242)
-        (xy 74.37 123.242) (xy 74.37 111.41075) (xy 92.495 111.41075) (xy 92.495 112.10131) (xy 92.591673 112.334699)
-        (xy 92.770302 112.513327) (xy 93.003691 112.61) (xy 93.69425 112.61) (xy 93.853 112.45125) (xy 93.853 111.252)
-        (xy 94.107 111.252) (xy 94.107 112.45125) (xy 94.26575 112.61) (xy 94.956309 112.61) (xy 95.189698 112.513327)
-        (xy 95.368327 112.334699) (xy 95.465 112.10131) (xy 95.465 111.41075) (xy 108.37 111.41075) (xy 108.37 112.10131)
-        (xy 108.466673 112.334699) (xy 108.645302 112.513327) (xy 108.878691 112.61) (xy 109.56925 112.61) (xy 109.728 112.45125)
-        (xy 109.728 111.252) (xy 109.982 111.252) (xy 109.982 112.45125) (xy 110.14075 112.61) (xy 110.831309 112.61)
-        (xy 111.064698 112.513327) (xy 111.243327 112.334699) (xy 111.34 112.10131) (xy 111.34 111.41075) (xy 123.61 111.41075)
-        (xy 123.61 112.10131) (xy 123.706673 112.334699) (xy 123.885302 112.513327) (xy 124.118691 112.61) (xy 124.80925 112.61)
-        (xy 124.968 112.45125) (xy 124.968 111.252) (xy 125.222 111.252) (xy 125.222 112.45125) (xy 125.38075 112.61)
-        (xy 126.071309 112.61) (xy 126.304698 112.513327) (xy 126.483327 112.334699) (xy 126.58 112.10131) (xy 126.58 111.41075)
-        (xy 126.42125 111.252) (xy 125.222 111.252) (xy 124.968 111.252) (xy 123.76875 111.252) (xy 123.61 111.41075)
-        (xy 111.34 111.41075) (xy 111.18125 111.252) (xy 109.982 111.252) (xy 109.728 111.252) (xy 108.52875 111.252)
-        (xy 108.37 111.41075) (xy 95.465 111.41075) (xy 95.30625 111.252) (xy 94.107 111.252) (xy 93.853 111.252)
-        (xy 92.65375 111.252) (xy 92.495 111.41075) (xy 74.37 111.41075) (xy 74.37 29.691) (xy 140.303864 29.691)
+        (xy 79.852765 41.468157) (xy 80.062809 41.327809) (xy 80.203157 41.117765) (xy 80.25244 40.87) (xy 80.25244 40.755)
+        (xy 97.018593 40.755) (xy 97.155 40.782133) (xy 97.291407 40.755) (xy 97.6954 40.674641) (xy 98.151327 40.37)
+        (xy 98.301031 40.37) (xy 98.777945 40.275136) (xy 99.010122 40.12) (xy 100.066315 40.12) (xy 100.596 40.649685)
+        (xy 100.596001 53.147313) (xy 97.502112 56.241203) (xy 97.386471 56.318472) (xy 97.218173 56.570348) (xy 97.080359 56.776601)
+        (xy 96.976212 57.300182) (xy 96.960797 57.277111) (xy 96.960795 57.277109) (xy 96.883528 57.161471) (xy 96.76789 57.084204)
+        (xy 92.509114 52.825429) (xy 92.536407 52.82) (xy 92.9404 52.739641) (xy 93.398529 52.433529) (xy 93.4758 52.317885)
+        (xy 95.513685 50.28) (xy 96.569878 50.28) (xy 96.802055 50.435136) (xy 97.278969 50.53) (xy 98.301031 50.53)
+        (xy 98.777945 50.435136) (xy 99.318769 50.073769) (xy 99.680136 49.532945) (xy 99.807031 48.895) (xy 99.680136 48.257055)
+        (xy 99.318769 47.716231) (xy 99.182232 47.625) (xy 99.318769 47.533769) (xy 99.680136 46.992945) (xy 99.807031 46.355)
+        (xy 99.680136 45.717055) (xy 99.318769 45.176231) (xy 98.777945 44.814864) (xy 98.301031 44.72) (xy 97.278969 44.72)
+        (xy 96.802055 44.814864) (xy 96.261231 45.176231) (xy 95.899864 45.717055) (xy 95.772969 46.355) (xy 95.899864 46.992945)
+        (xy 96.245349 47.51) (xy 95.076401 47.51) (xy 94.939999 47.482868) (xy 94.803597 47.51) (xy 94.803593 47.51)
+        (xy 94.3996 47.590359) (xy 93.941471 47.896471) (xy 93.8642 48.012115) (xy 91.826315 50.05) (xy 89.296407 50.05)
+        (xy 89.16 50.022867) (xy 89.023593 50.05) (xy 86.933686 50.05) (xy 80.450799 43.567114) (xy 80.373529 43.451471)
+        (xy 79.9154 43.145359) (xy 79.795944 43.121598) (xy 79.31438 42.640034) (xy 78.529678 42.315) (xy 77.680322 42.315)
+        (xy 76.89562 42.640034) (xy 76.295034 43.24062) (xy 75.97 44.025322) (xy 75.97 44.874678) (xy 76.295034 45.65938)
+        (xy 76.89562 46.259966) (xy 77.680322 46.585) (xy 78.529678 46.585) (xy 79.252084 46.28577) (xy 85.284202 52.317888)
+        (xy 85.361471 52.433529) (xy 85.477111 52.510797) (xy 85.8196 52.739641) (xy 86.36 52.847133) (xy 86.496407 52.82)
+        (xy 88.586315 52.82) (xy 94.5 58.733686) (xy 94.5 58.886315) (xy 94.4208 58.807115) (xy 94.343529 58.691471)
+        (xy 93.8854 58.385359) (xy 93.481407 58.305) (xy 93.345 58.277867) (xy 93.208593 58.305) (xy 91.975122 58.305)
+        (xy 91.742945 58.149864) (xy 91.266031 58.055) (xy 90.343969 58.055) (xy 89.867055 58.149864) (xy 89.326231 58.511231)
+        (xy 88.964864 59.052055) (xy 88.837969 59.69) (xy 88.964864 60.327945) (xy 89.326231 60.868769) (xy 89.462768 60.96)
+        (xy 89.326231 61.051231) (xy 88.964864 61.592055) (xy 88.837969 62.23) (xy 88.964864 62.867945) (xy 89.310349 63.385)
+        (xy 79.932285 63.385) (xy 79.837765 63.321843) (xy 79.59 63.27256) (xy 77.89 63.27256) (xy 77.642235 63.321843)
+        (xy 77.432191 63.462191) (xy 77.291843 63.672235) (xy 77.24256 63.92) (xy 77.24256 65.62) (xy 77.291843 65.867765)
+        (xy 77.432191 66.077809) (xy 77.642235 66.218157) (xy 77.687619 66.227184) (xy 77.669375 66.239375) (xy 77.341161 66.730582)
+        (xy 77.225908 67.31) (xy 77.341161 67.889418) (xy 77.669375 68.380625) (xy 77.967761 68.58) (xy 77.669375 68.779375)
+        (xy 77.341161 69.270582) (xy 77.225908 69.85) (xy 77.341161 70.429418) (xy 77.669375 70.920625) (xy 77.967761 71.12)
+        (xy 77.669375 71.319375) (xy 77.341161 71.810582) (xy 77.225908 72.39) (xy 77.341161 72.969418) (xy 77.669375 73.460625)
+        (xy 78.160582 73.788839) (xy 78.593744 73.875) (xy 78.886256 73.875) (xy 79.319418 73.788839) (xy 79.34013 73.775)
+        (xy 89.310349 73.775) (xy 89.129941 74.045) (xy 88.987161 74.045) (xy 88.9 74.027663) (xy 88.812839 74.045)
+        (xy 88.812835 74.045) (xy 88.55469 74.096348) (xy 88.335845 74.242576) (xy 88.335844 74.242577) (xy 88.261951 74.291951)
+        (xy 88.212577 74.365844) (xy 78.95896 83.619462) (xy 78.886256 83.605) (xy 78.593744 83.605) (xy 78.160582 83.691161)
+        (xy 77.669375 84.019375) (xy 77.554698 84.191001) (xy 77.537919 84.187663) (xy 77.450758 84.205) (xy 77.450754 84.205)
+        (xy 77.192609 84.256348) (xy 76.973764 84.402576) (xy 76.973763 84.402577) (xy 76.89987 84.451951) (xy 76.850496 84.525844)
+        (xy 75.000847 86.375494) (xy 74.926951 86.42487) (xy 74.731348 86.71761) (xy 74.68 86.975755) (xy 74.68 86.975758)
+        (xy 74.662663 87.062919) (xy 74.68 87.15008) (xy 74.680001 90.082831) (xy 74.68 90.082836) (xy 74.68 90.082839)
+        (xy 74.662663 90.17) (xy 74.68 90.257161) (xy 74.680001 95.162831) (xy 74.662662 95.25) (xy 74.68 95.337164)
+        (xy 74.680001 102.147835) (xy 74.662663 102.235) (xy 74.731348 102.580309) (xy 74.877576 102.799154) (xy 74.877578 102.799156)
+        (xy 74.926952 102.873049) (xy 75.000845 102.922423) (xy 81.227577 109.149156) (xy 81.276951 109.223049) (xy 81.350844 109.272423)
+        (xy 81.350845 109.272424) (xy 81.46188 109.346615) (xy 81.56969 109.418652) (xy 81.827835 109.47) (xy 81.827839 109.47)
+        (xy 81.914999 109.487337) (xy 82.002159 109.47) (xy 92.785344 109.47) (xy 92.909375 109.655625) (xy 92.931033 109.670096)
+        (xy 92.770302 109.736673) (xy 92.591673 109.915301) (xy 92.495 110.14869) (xy 92.495 110.83925) (xy 92.65375 110.998)
+        (xy 93.853 110.998) (xy 93.853 110.978) (xy 94.107 110.978) (xy 94.107 110.998) (xy 95.30625 110.998)
+        (xy 95.465 110.83925) (xy 95.465 110.14869) (xy 95.368327 109.915301) (xy 95.189698 109.736673) (xy 95.028967 109.670096)
+        (xy 95.050625 109.655625) (xy 95.174656 109.47) (xy 108.660344 109.47) (xy 108.784375 109.655625) (xy 108.806033 109.670096)
+        (xy 108.645302 109.736673) (xy 108.466673 109.915301) (xy 108.37 110.14869) (xy 108.37 110.83925) (xy 108.52875 110.998)
+        (xy 109.728 110.998) (xy 109.728 110.978) (xy 109.982 110.978) (xy 109.982 110.998) (xy 111.18125 110.998)
+        (xy 111.34 110.83925) (xy 111.34 110.14869) (xy 111.243327 109.915301) (xy 111.064698 109.736673) (xy 110.903967 109.670096)
+        (xy 110.925625 109.655625) (xy 111.049656 109.47) (xy 123.900344 109.47) (xy 124.024375 109.655625) (xy 124.046033 109.670096)
+        (xy 123.885302 109.736673) (xy 123.706673 109.915301) (xy 123.61 110.14869) (xy 123.61 110.83925) (xy 123.76875 110.998)
+        (xy 124.968 110.998) (xy 124.968 110.978) (xy 125.222 110.978) (xy 125.222 110.998) (xy 126.42125 110.998)
+        (xy 126.58 110.83925) (xy 126.58 110.14869) (xy 126.483327 109.915301) (xy 126.304698 109.736673) (xy 126.143967 109.670096)
+        (xy 126.165625 109.655625) (xy 126.493839 109.164418) (xy 126.609092 108.585) (xy 126.493839 108.005582) (xy 126.165625 107.514375)
+        (xy 125.867239 107.315) (xy 126.165625 107.115625) (xy 126.493839 106.624418) (xy 126.609092 106.045) (xy 126.493839 105.465582)
+        (xy 126.165625 104.974375) (xy 125.98 104.850344) (xy 125.98 89.901578) (xy 126.096579 89.785) (xy 126.355682 89.785)
+        (xy 126.43732 89.90718) (xy 126.830658 90.17) (xy 126.43732 90.43282) (xy 126.128556 90.894918) (xy 126.020132 91.44)
+        (xy 126.128556 91.985082) (xy 126.43732 92.44718) (xy 126.899418 92.755944) (xy 127.306912 92.837) (xy 127.394516 92.837)
+        (xy 127.436348 93.047309) (xy 127.582576 93.266154) (xy 127.582578 93.266156) (xy 127.631952 93.340049) (xy 127.705845 93.389423)
+        (xy 141.355 107.038579) (xy 141.355001 119.927834) (xy 141.337663 120.015) (xy 141.406348 120.360309) (xy 141.552576 120.579154)
+        (xy 141.552578 120.579156) (xy 141.601952 120.653049) (xy 141.675845 120.702423) (xy 143.457577 122.484156) (xy 143.506951 122.558049)
+        (xy 143.580844 122.607423) (xy 143.580845 122.607424) (xy 143.79969 122.753652) (xy 144.057835 122.805) (xy 144.057839 122.805)
+        (xy 144.145 122.822337) (xy 144.232161 122.805) (xy 150.429839 122.805) (xy 150.517 122.822337) (xy 150.604161 122.805)
+        (xy 150.604165 122.805) (xy 150.86231 122.753652) (xy 151.155049 122.558049) (xy 151.204425 122.484153) (xy 151.821156 121.867423)
+        (xy 151.895049 121.818049) (xy 152.090652 121.52531) (xy 152.142 121.267165) (xy 152.142 121.26716) (xy 152.159337 121.180001)
+        (xy 152.142 121.092841) (xy 152.142 121.055059) (xy 152.435769 120.858769) (xy 152.527 120.722232) (xy 152.618231 120.858769)
+        (xy 153.159056 121.220136) (xy 153.797 121.347031) (xy 154.434945 121.220136) (xy 154.975769 120.858769) (xy 155.086952 120.692371)
+        (xy 155.270683 120.925922) (xy 155.828645 121.239144) (xy 155.956566 121.270124) (xy 156.21 121.150777) (xy 156.21 119.507)
+        (xy 156.464 119.507) (xy 156.464 121.150777) (xy 156.717434 121.270124) (xy 156.845355 121.239144) (xy 157.312632 120.97683)
+        (xy 157.338673 121.039698) (xy 157.517301 121.218327) (xy 157.75069 121.315) (xy 158.59125 121.315) (xy 158.75 121.15625)
+        (xy 158.75 119.507) (xy 159.004 119.507) (xy 159.004 121.15625) (xy 159.16275 121.315) (xy 160.00331 121.315)
+        (xy 160.236699 121.218327) (xy 160.415327 121.039698) (xy 160.512 120.806309) (xy 160.512 119.66575) (xy 160.35325 119.507)
+        (xy 159.004 119.507) (xy 158.75 119.507) (xy 156.464 119.507) (xy 156.21 119.507) (xy 156.19 119.507)
+        (xy 156.19 119.253) (xy 156.21 119.253) (xy 156.21 117.609223) (xy 156.464 117.609223) (xy 156.464 119.253)
+        (xy 158.75 119.253) (xy 158.75 117.60375) (xy 159.004 117.60375) (xy 159.004 119.253) (xy 160.35325 119.253)
+        (xy 160.512 119.09425) (xy 160.512 117.953691) (xy 160.415327 117.720302) (xy 160.236699 117.541673) (xy 160.00331 117.445)
+        (xy 159.16275 117.445) (xy 159.004 117.60375) (xy 158.75 117.60375) (xy 158.59125 117.445) (xy 157.75069 117.445)
+        (xy 157.517301 117.541673) (xy 157.338673 117.720302) (xy 157.312632 117.78317) (xy 156.845355 117.520856) (xy 156.717434 117.489876)
+        (xy 156.464 117.609223) (xy 156.21 117.609223) (xy 155.956566 117.489876) (xy 155.828645 117.520856) (xy 155.270683 117.834078)
+        (xy 155.086952 118.067629) (xy 154.975769 117.901231) (xy 154.682 117.704941) (xy 154.682 116.825578) (xy 164.140156 107.367423)
+        (xy 164.214049 107.318049) (xy 164.409652 107.02531) (xy 164.461 106.767165) (xy 164.461 106.767161) (xy 164.478337 106.680001)
+        (xy 164.461 106.592841) (xy 164.461 96.607165) (xy 164.478338 96.52) (xy 164.409652 96.17469) (xy 164.214049 95.881951)
+        (xy 163.92131 95.686348) (xy 163.663165 95.635) (xy 163.663164 95.635) (xy 163.576 95.617662) (xy 163.488835 95.635)
+        (xy 152.322079 95.635) (xy 145.7312 89.044122) (xy 145.759868 88.9) (xy 145.651444 88.354918) (xy 145.34268 87.89282)
+        (xy 144.949342 87.63) (xy 145.34268 87.36718) (xy 145.651444 86.905082) (xy 145.759868 86.36) (xy 145.651444 85.814918)
+        (xy 145.34268 85.35282) (xy 144.949342 85.09) (xy 145.34268 84.82718) (xy 145.651444 84.365082) (xy 145.759868 83.82)
+        (xy 145.651444 83.274918) (xy 145.34268 82.81282) (xy 144.949342 82.55) (xy 145.34268 82.28718) (xy 145.346854 82.280932)
+        (xy 158.245077 95.179156) (xy 158.294451 95.253049) (xy 158.368344 95.302423) (xy 158.368345 95.302424) (xy 158.499917 95.390338)
+        (xy 158.58719 95.448652) (xy 158.845335 95.5) (xy 158.845339 95.5) (xy 158.9325 95.517337) (xy 159.019661 95.5)
+        (xy 172.795344 95.5) (xy 172.919375 95.685625) (xy 173.410582 96.013839) (xy 173.843744 96.1) (xy 174.136256 96.1)
+        (xy 174.569418 96.013839) (xy 175.060625 95.685625) (xy 175.26 95.387239) (xy 175.459375 95.685625) (xy 175.950582 96.013839)
+        (xy 176.383744 96.1) (xy 176.676256 96.1) (xy 177.109418 96.013839) (xy 177.600625 95.685625) (xy 177.615096 95.663967)
+        (xy 177.681673 95.824698) (xy 177.860301 96.003327) (xy 178.09369 96.1) (xy 178.78425 96.1) (xy 178.943 95.94125)
+        (xy 178.943 94.742) (xy 179.197 94.742) (xy 179.197 95.94125) (xy 179.35575 96.1) (xy 180.04631 96.1)
+        (xy 180.279699 96.003327) (xy 180.458327 95.824698) (xy 180.555 95.591309) (xy 180.555 94.90075) (xy 180.39625 94.742)
+        (xy 179.197 94.742) (xy 178.943 94.742) (xy 178.923 94.742) (xy 178.923 94.488) (xy 178.943 94.488)
+        (xy 178.943 93.28875) (xy 179.197 93.28875) (xy 179.197 94.488) (xy 180.39625 94.488) (xy 180.555 94.32925)
+        (xy 180.555 93.638691) (xy 180.458327 93.405302) (xy 180.279699 93.226673) (xy 180.04631 93.13) (xy 179.35575 93.13)
+        (xy 179.197 93.28875) (xy 178.943 93.28875) (xy 178.78425 93.13) (xy 178.09369 93.13) (xy 177.860301 93.226673)
+        (xy 177.681673 93.405302) (xy 177.615096 93.566033) (xy 177.600625 93.544375) (xy 177.415 93.420344) (xy 177.415 86.284656)
+        (xy 177.600625 86.160625) (xy 177.615096 86.138967) (xy 177.681673 86.299698) (xy 177.860301 86.478327) (xy 178.09369 86.575)
+        (xy 178.78425 86.575) (xy 178.943 86.41625) (xy 178.943 85.217) (xy 179.197 85.217) (xy 179.197 86.41625)
+        (xy 179.35575 86.575) (xy 180.04631 86.575) (xy 180.279699 86.478327) (xy 180.458327 86.299698) (xy 180.555 86.066309)
+        (xy 180.555 85.37575) (xy 180.39625 85.217) (xy 179.197 85.217) (xy 178.943 85.217) (xy 178.923 85.217)
+        (xy 178.923 84.963) (xy 178.943 84.963) (xy 178.943 83.76375) (xy 179.197 83.76375) (xy 179.197 84.963)
+        (xy 180.39625 84.963) (xy 180.555 84.80425) (xy 180.555 84.113691) (xy 180.458327 83.880302) (xy 180.279699 83.701673)
+        (xy 180.04631 83.605) (xy 179.35575 83.605) (xy 179.197 83.76375) (xy 178.943 83.76375) (xy 178.78425 83.605)
+        (xy 178.09369 83.605) (xy 177.860301 83.701673) (xy 177.681673 83.880302) (xy 177.615096 84.041033) (xy 177.600625 84.019375)
+        (xy 177.415 83.895344) (xy 177.415 76.759656) (xy 177.600625 76.635625) (xy 177.615096 76.613967) (xy 177.681673 76.774698)
+        (xy 177.860301 76.953327) (xy 178.09369 77.05) (xy 178.78425 77.05) (xy 178.943 76.89125) (xy 178.943 75.692)
+        (xy 179.197 75.692) (xy 179.197 76.89125) (xy 179.35575 77.05) (xy 180.04631 77.05) (xy 180.279699 76.953327)
+        (xy 180.458327 76.774698) (xy 180.555 76.541309) (xy 180.555 75.85075) (xy 180.39625 75.692) (xy 179.197 75.692)
+        (xy 178.943 75.692) (xy 178.923 75.692) (xy 178.923 75.438) (xy 178.943 75.438) (xy 178.943 74.23875)
+        (xy 179.197 74.23875) (xy 179.197 75.438) (xy 180.39625 75.438) (xy 180.555 75.27925) (xy 180.555 74.588691)
+        (xy 180.458327 74.355302) (xy 180.279699 74.176673) (xy 180.04631 74.08) (xy 179.35575 74.08) (xy 179.197 74.23875)
+        (xy 178.943 74.23875) (xy 178.78425 74.08) (xy 178.09369 74.08) (xy 177.860301 74.176673) (xy 177.681673 74.355302)
+        (xy 177.615096 74.516033) (xy 177.600625 74.494375) (xy 177.415 74.370344) (xy 177.415 72.477159) (xy 177.432337 72.389999)
+        (xy 177.415 72.302839) (xy 177.415 72.302835) (xy 177.363652 72.04469) (xy 177.291615 71.93688) (xy 177.217424 71.825845)
+        (xy 177.217423 71.825844) (xy 177.168049 71.751951) (xy 177.094156 71.702578) (xy 169.646 64.254422) (xy 169.646 57.770578)
+        (xy 171.141727 56.274851) (xy 171.241231 56.423769) (xy 171.377768 56.515) (xy 171.241231 56.606231) (xy 170.879864 57.147055)
+        (xy 170.752969 57.785) (xy 170.879864 58.422945) (xy 171.241231 58.963769) (xy 171.377768 59.055) (xy 171.241231 59.146231)
+        (xy 170.879864 59.687055) (xy 170.752969 60.325) (xy 170.879864 60.962945) (xy 171.241231 61.503769) (xy 171.377768 61.595)
+        (xy 171.241231 61.686231) (xy 170.879864 62.227055) (xy 170.752969 62.865) (xy 170.879864 63.502945) (xy 171.241231 64.043769)
+        (xy 171.782055 64.405136) (xy 172.258969 64.5) (xy 173.181031 64.5) (xy 173.657945 64.405136) (xy 174.198769 64.043769)
+        (xy 174.560136 63.502945) (xy 174.687031 62.865) (xy 174.560136 62.227055) (xy 174.214651 61.71) (xy 182.63756 61.71)
+        (xy 182.63756 62.46) (xy 182.686843 62.707765) (xy 182.827191 62.917809) (xy 183.037235 63.058157) (xy 183.285 63.10744)
+        (xy 186.285 63.10744) (xy 186.532765 63.058157) (xy 186.742809 62.917809) (xy 186.883157 62.707765) (xy 186.93244 62.46)
+        (xy 186.93244 59.46) (xy 186.883157 59.212235) (xy 186.742809 59.002191) (xy 186.532765 58.861843) (xy 186.285 58.81256)
+        (xy 183.285 58.81256) (xy 183.037235 58.861843) (xy 182.920266 58.94) (xy 179.199354 58.94) (xy 179.433529 58.783529)
+        (xy 179.510799 58.667886) (xy 180.913686 57.265) (xy 183.150654 57.265) (xy 183.57562 57.689966) (xy 184.360322 58.015)
+        (xy 185.209678 58.015) (xy 185.99438 57.689966) (xy 186.594966 57.08938) (xy 186.92 56.304678) (xy 186.92 55.455322)
+        (xy 186.594966 54.67062) (xy 185.99438 54.070034) (xy 185.209678 53.745) (xy 184.360322 53.745) (xy 183.57562 54.070034)
+        (xy 183.150654 54.495) (xy 180.476407 54.495) (xy 180.34 54.467867) (xy 180.203593 54.495) (xy 179.7996 54.575359)
+        (xy 179.341471 54.881471) (xy 179.264202 54.997112) (xy 177.861315 56.4) (xy 174.214651 56.4) (xy 174.560136 55.882945)
+        (xy 174.687031 55.245) (xy 174.560136 54.607055) (xy 174.198769 54.066231) (xy 174.032371 53.955048) (xy 174.265922 53.771317)
+        (xy 174.579144 53.213355) (xy 174.610124 53.085434) (xy 174.490777 52.832) (xy 172.847 52.832) (xy 172.847 52.852)
+        (xy 172.593 52.852) (xy 172.593 52.832) (xy 170.949223 52.832) (xy 170.829876 53.085434) (xy 170.860856 53.213355)
+        (xy 171.174078 53.771317) (xy 171.407629 53.955048) (xy 171.241231 54.066231) (xy 171.044941 54.36) (xy 171.007159 54.36)
+        (xy 170.919999 54.342663) (xy 170.911861 54.344282) (xy 169.541 52.973422) (xy 169.541 52.324566) (xy 170.829876 52.324566)
+        (xy 170.949223 52.578) (xy 172.593 52.578) (xy 172.593 51.07) (xy 172.847 51.07) (xy 172.847 52.578)
+        (xy 174.490777 52.578) (xy 174.610124 52.324566) (xy 174.579144 52.196645) (xy 174.265922 51.638683) (xy 173.76302 51.243058)
+        (xy 173.147 51.07) (xy 172.847 51.07) (xy 172.593 51.07) (xy 172.293 51.07) (xy 171.67698 51.243058)
+        (xy 171.174078 51.638683) (xy 170.860856 52.196645) (xy 170.829876 52.324566) (xy 169.541 52.324566) (xy 169.541 39.460578)
+        (xy 171.141727 37.859851) (xy 171.241231 38.008769) (xy 171.377768 38.1) (xy 171.241231 38.191231) (xy 170.879864 38.732055)
+        (xy 170.752969 39.37) (xy 170.879864 40.007945) (xy 171.241231 40.548769) (xy 171.377768 40.64) (xy 171.241231 40.731231)
+        (xy 170.879864 41.272055) (xy 170.752969 41.91) (xy 170.879864 42.547945) (xy 171.241231 43.088769) (xy 171.377768 43.18)
+        (xy 171.241231 43.271231) (xy 170.879864 43.812055) (xy 170.752969 44.45) (xy 170.879864 45.087945) (xy 171.241231 45.628769)
+        (xy 171.782055 45.990136) (xy 172.258969 46.085) (xy 173.181031 46.085) (xy 173.657945 45.990136) (xy 174.198769 45.628769)
+        (xy 174.560136 45.087945) (xy 174.687031 44.45) (xy 174.560136 43.812055) (xy 174.214651 43.295) (xy 182.63756 43.295)
+        (xy 182.63756 43.41) (xy 182.686843 43.657765) (xy 182.827191 43.867809) (xy 183.037235 44.008157) (xy 183.285 44.05744)
+        (xy 186.285 44.05744) (xy 186.532765 44.008157) (xy 186.742809 43.867809) (xy 186.883157 43.657765) (xy 186.93244 43.41)
+        (xy 186.93244 40.41) (xy 186.883157 40.162235) (xy 186.742809 39.952191) (xy 186.532765 39.811843) (xy 186.285 39.76256)
+        (xy 183.285 39.76256) (xy 183.037235 39.811843) (xy 182.827191 39.952191) (xy 182.686843 40.162235) (xy 182.63756 40.41)
+        (xy 182.63756 40.525) (xy 177.929354 40.525) (xy 178.163529 40.368529) (xy 178.2408 40.252885) (xy 180.278685 38.215)
+        (xy 183.150654 38.215) (xy 183.57562 38.639966) (xy 184.360322 38.965) (xy 185.209678 38.965) (xy 185.99438 38.639966)
+        (xy 186.594966 38.03938) (xy 186.92 37.254678) (xy 186.92 36.405322) (xy 186.594966 35.62062) (xy 185.99438 35.020034)
+        (xy 185.209678 34.695) (xy 184.360322 34.695) (xy 183.57562 35.020034) (xy 183.150654 35.445) (xy 179.841401 35.445)
+        (xy 179.704999 35.417868) (xy 179.568597 35.445) (xy 179.568593 35.445) (xy 179.1646 35.525359) (xy 178.706471 35.831471)
+        (xy 178.6292 35.947115) (xy 176.591315 37.985) (xy 174.214651 37.985) (xy 174.560136 37.467945) (xy 174.687031 36.83)
+        (xy 174.560136 36.192055) (xy 174.198769 35.651231) (xy 174.032371 35.540048) (xy 174.265922 35.356317) (xy 174.579144 34.798355)
+        (xy 174.610124 34.670434) (xy 174.490777 34.417) (xy 172.847 34.417) (xy 172.847 34.437) (xy 172.593 34.437)
+        (xy 172.593 34.417) (xy 170.949223 34.417) (xy 170.829876 34.670434) (xy 170.860856 34.798355) (xy 171.174078 35.356317)
+        (xy 171.407629 35.540048) (xy 171.241231 35.651231) (xy 171.044941 35.945) (xy 171.007161 35.945) (xy 170.92 35.927663)
+        (xy 170.832839 35.945) (xy 170.800579 35.945) (xy 168.765145 33.909566) (xy 170.829876 33.909566) (xy 170.949223 34.163)
+        (xy 172.593 34.163) (xy 172.593 32.655) (xy 172.847 32.655) (xy 172.847 34.163) (xy 174.490777 34.163)
+        (xy 174.610124 33.909566) (xy 174.579144 33.781645) (xy 174.265922 33.223683) (xy 173.76302 32.828058) (xy 173.147 32.655)
+        (xy 172.847 32.655) (xy 172.593 32.655) (xy 172.293 32.655) (xy 171.67698 32.828058) (xy 171.174078 33.223683)
+        (xy 170.860856 33.781645) (xy 170.829876 33.909566) (xy 168.765145 33.909566) (xy 164.771425 29.915847) (xy 164.722049 29.841951)
+        (xy 164.496136 29.691) (xy 188.241 29.691) (xy 188.241001 123.242) (xy 74.37 123.242) (xy 74.37 111.41075)
+        (xy 92.495 111.41075) (xy 92.495 112.10131) (xy 92.591673 112.334699) (xy 92.770302 112.513327) (xy 93.003691 112.61)
+        (xy 93.69425 112.61) (xy 93.853 112.45125) (xy 93.853 111.252) (xy 94.107 111.252) (xy 94.107 112.45125)
+        (xy 94.26575 112.61) (xy 94.956309 112.61) (xy 95.189698 112.513327) (xy 95.368327 112.334699) (xy 95.465 112.10131)
+        (xy 95.465 111.41075) (xy 108.37 111.41075) (xy 108.37 112.10131) (xy 108.466673 112.334699) (xy 108.645302 112.513327)
+        (xy 108.878691 112.61) (xy 109.56925 112.61) (xy 109.728 112.45125) (xy 109.728 111.252) (xy 109.982 111.252)
+        (xy 109.982 112.45125) (xy 110.14075 112.61) (xy 110.831309 112.61) (xy 111.064698 112.513327) (xy 111.243327 112.334699)
+        (xy 111.34 112.10131) (xy 111.34 111.41075) (xy 123.61 111.41075) (xy 123.61 112.10131) (xy 123.706673 112.334699)
+        (xy 123.885302 112.513327) (xy 124.118691 112.61) (xy 124.80925 112.61) (xy 124.968 112.45125) (xy 124.968 111.252)
+        (xy 125.222 111.252) (xy 125.222 112.45125) (xy 125.38075 112.61) (xy 126.071309 112.61) (xy 126.304698 112.513327)
+        (xy 126.483327 112.334699) (xy 126.58 112.10131) (xy 126.58 111.41075) (xy 126.42125 111.252) (xy 125.222 111.252)
+        (xy 124.968 111.252) (xy 123.76875 111.252) (xy 123.61 111.41075) (xy 111.34 111.41075) (xy 111.18125 111.252)
+        (xy 109.982 111.252) (xy 109.728 111.252) (xy 108.52875 111.252) (xy 108.37 111.41075) (xy 95.465 111.41075)
+        (xy 95.30625 111.252) (xy 94.107 111.252) (xy 93.853 111.252) (xy 92.65375 111.252) (xy 92.495 111.41075)
+        (xy 74.37 111.41075) (xy 74.37 29.691) (xy 140.303864 29.691)
       )
     )
     (filled_polygon
@@ -1999,55 +2031,55 @@
         (xy 158.707629 35.540048) (xy 158.541231 35.651231) (xy 158.179864 36.192055) (xy 158.052969 36.83) (xy 158.179864 37.467945)
         (xy 158.541231 38.008769) (xy 158.677768 38.1) (xy 158.541231 38.191231) (xy 158.179864 38.732055) (xy 158.052969 39.37)
         (xy 158.179864 40.007945) (xy 158.541231 40.548769) (xy 158.677768 40.64) (xy 158.541231 40.731231) (xy 158.344941 41.025)
-        (xy 157.567161 41.025) (xy 157.48 41.007663) (xy 157.392839 41.025) (xy 157.392835 41.025) (xy 157.13469 41.076348)
-        (xy 156.915845 41.222576) (xy 156.915844 41.222577) (xy 156.841951 41.271951) (xy 156.792577 41.345844) (xy 153.105845 45.032577)
-        (xy 153.031952 45.081951) (xy 152.982578 45.155844) (xy 152.982576 45.155846) (xy 152.836348 45.374691) (xy 152.767663 45.72)
-        (xy 152.785001 45.807166) (xy 152.785 58.053421) (xy 145.683422 65.155) (xy 145.424318 65.155) (xy 145.34268 65.03282)
-        (xy 144.949342 64.77) (xy 145.34268 64.50718) (xy 145.651444 64.045082) (xy 145.759868 63.5) (xy 145.651444 62.954918)
-        (xy 145.34268 62.49282) (xy 144.949342 62.23) (xy 145.34268 61.96718) (xy 145.651444 61.505082) (xy 145.759868 60.96)
-        (xy 145.651444 60.414918) (xy 145.34268 59.95282) (xy 144.932011 59.67842) (xy 144.987441 59.662059) (xy 145.41313 59.318026)
-        (xy 145.67476 58.837277) (xy 145.68972 58.76307) (xy 145.56722 58.547) (xy 143.637 58.547) (xy 143.637 58.567)
-        (xy 143.383 58.567) (xy 143.383 58.547) (xy 141.45278 58.547) (xy 141.33028 58.76307) (xy 141.34524 58.837277)
-        (xy 141.60687 59.318026) (xy 142.032559 59.662059) (xy 142.087989 59.67842) (xy 141.67732 59.95282) (xy 141.595682 60.075)
-        (xy 141.336579 60.075) (xy 140.585 59.323422) (xy 140.585 58.07693) (xy 141.33028 58.07693) (xy 141.45278 58.293)
-        (xy 143.383 58.293) (xy 143.383 57.023) (xy 143.637 57.023) (xy 143.637 58.293) (xy 145.56722 58.293)
-        (xy 145.68972 58.07693) (xy 145.67476 58.002723) (xy 145.41313 57.521974) (xy 144.987441 57.177941) (xy 144.4625 57.023)
-        (xy 143.637 57.023) (xy 143.383 57.023) (xy 142.5575 57.023) (xy 142.032559 57.177941) (xy 141.60687 57.521974)
-        (xy 141.34524 58.002723) (xy 141.33028 58.07693) (xy 140.585 58.07693) (xy 140.585 49.896578) (xy 142.02104 48.460538)
-        (xy 142.093744 48.475) (xy 142.386256 48.475) (xy 142.819418 48.388839) (xy 143.310625 48.060625) (xy 143.51 47.762239)
-        (xy 143.709375 48.060625) (xy 143.895 48.184656) (xy 143.895001 49.617165) (xy 143.946349 49.87531) (xy 144.141952 50.168049)
-        (xy 144.434691 50.363652) (xy 144.78 50.432338) (xy 145.12531 50.363652) (xy 145.418049 50.168049) (xy 145.613652 49.87531)
-        (xy 145.665 49.617165) (xy 145.665 48.184656) (xy 145.850625 48.060625) (xy 145.865096 48.038967) (xy 145.931673 48.199698)
-        (xy 146.110301 48.378327) (xy 146.34369 48.475) (xy 147.03425 48.475) (xy 147.193 48.31625) (xy 147.193 47.117)
-        (xy 147.447 47.117) (xy 147.447 48.31625) (xy 147.60575 48.475) (xy 148.29631 48.475) (xy 148.529699 48.378327)
-        (xy 148.708327 48.199698) (xy 148.805 47.966309) (xy 148.805 47.27575) (xy 148.64625 47.117) (xy 147.447 47.117)
-        (xy 147.193 47.117) (xy 147.173 47.117) (xy 147.173 46.863) (xy 147.193 46.863) (xy 147.193 45.66375)
-        (xy 147.447 45.66375) (xy 147.447 46.863) (xy 148.64625 46.863) (xy 148.805 46.70425) (xy 148.805 46.013691)
-        (xy 148.708327 45.780302) (xy 148.529699 45.601673) (xy 148.29631 45.505) (xy 147.60575 45.505) (xy 147.447 45.66375)
-        (xy 147.193 45.66375) (xy 147.03425 45.505) (xy 146.34369 45.505) (xy 146.110301 45.601673) (xy 145.931673 45.780302)
-        (xy 145.865096 45.941033) (xy 145.850625 45.919375) (xy 145.667074 45.79673) (xy 145.682337 45.719999) (xy 145.665 45.632839)
-        (xy 145.665 45.632835) (xy 145.613652 45.37469) (xy 145.541615 45.26688) (xy 145.467424 45.155845) (xy 145.467423 45.155844)
-        (xy 145.418049 45.081951) (xy 145.344156 45.032577) (xy 144.197425 43.885847) (xy 144.148049 43.811951) (xy 143.85531 43.616348)
-        (xy 143.597165 43.565) (xy 143.597161 43.565) (xy 143.51 43.547663) (xy 143.422839 43.565) (xy 141.057159 43.565)
-        (xy 140.969999 43.547663) (xy 140.882839 43.565) (xy 140.882835 43.565) (xy 140.62469 43.616348) (xy 140.51688 43.688385)
-        (xy 140.405845 43.762576) (xy 140.405844 43.762577) (xy 140.331951 43.811951) (xy 140.282577 43.885844) (xy 138.045 46.123422)
-        (xy 138.045 39.736578) (xy 142.02104 35.760538) (xy 142.093744 35.775) (xy 142.386256 35.775) (xy 142.819418 35.688839)
-        (xy 143.310625 35.360625) (xy 143.51 35.062239) (xy 143.709375 35.360625) (xy 144.200582 35.688839) (xy 144.633744 35.775)
-        (xy 144.926256 35.775) (xy 145.359418 35.688839) (xy 145.850625 35.360625) (xy 145.865096 35.338967) (xy 145.931673 35.499698)
-        (xy 146.110301 35.678327) (xy 146.34369 35.775) (xy 147.03425 35.775) (xy 147.193 35.61625) (xy 147.193 34.417)
-        (xy 147.447 34.417) (xy 147.447 35.61625) (xy 147.60575 35.775) (xy 148.29631 35.775) (xy 148.529699 35.678327)
-        (xy 148.708327 35.499698) (xy 148.805 35.266309) (xy 148.805 34.57575) (xy 148.64625 34.417) (xy 147.447 34.417)
-        (xy 147.193 34.417) (xy 147.173 34.417) (xy 147.173 34.163) (xy 147.193 34.163) (xy 147.193 32.96375)
-        (xy 147.447 32.96375) (xy 147.447 34.163) (xy 148.64625 34.163) (xy 148.805 34.00425) (xy 148.805 33.909566)
-        (xy 158.129876 33.909566) (xy 158.249223 34.163) (xy 159.893 34.163) (xy 159.893 32.655) (xy 160.147 32.655)
-        (xy 160.147 34.163) (xy 161.790777 34.163) (xy 161.910124 33.909566) (xy 161.879144 33.781645) (xy 161.565922 33.223683)
-        (xy 161.06302 32.828058) (xy 160.447 32.655) (xy 160.147 32.655) (xy 159.893 32.655) (xy 159.593 32.655)
-        (xy 158.97698 32.828058) (xy 158.474078 33.223683) (xy 158.160856 33.781645) (xy 158.129876 33.909566) (xy 148.805 33.909566)
-        (xy 148.805 33.313691) (xy 148.708327 33.080302) (xy 148.529699 32.901673) (xy 148.29631 32.805) (xy 147.60575 32.805)
-        (xy 147.447 32.96375) (xy 147.193 32.96375) (xy 147.03425 32.805) (xy 146.34369 32.805) (xy 146.110301 32.901673)
-        (xy 145.931673 33.080302) (xy 145.865096 33.241033) (xy 145.850625 33.219375) (xy 145.667074 33.09673) (xy 145.682337 33.019999)
-        (xy 145.665 32.932839) (xy 145.665 32.932835) (xy 145.613652 32.67469) (xy 145.541615 32.56688) (xy 145.467424 32.455845)
-        (xy 145.467423 32.455844) (xy 145.418049 32.381951) (xy 145.344156 32.332577) (xy 144.376578 31.365) (xy 163.717422 31.365)
+        (xy 157.567161 41.025) (xy 157.48 41.007663) (xy 157.392839 41.025) (xy 157.392835 41.025) (xy 157.14384 41.074528)
+        (xy 157.13469 41.076348) (xy 156.915845 41.222576) (xy 156.915844 41.222577) (xy 156.841951 41.271951) (xy 156.792577 41.345844)
+        (xy 153.105845 45.032577) (xy 153.031952 45.081951) (xy 152.982578 45.155844) (xy 152.982576 45.155846) (xy 152.836348 45.374691)
+        (xy 152.767663 45.72) (xy 152.785001 45.807166) (xy 152.785 58.053421) (xy 145.683422 65.155) (xy 145.424318 65.155)
+        (xy 145.34268 65.03282) (xy 144.949342 64.77) (xy 145.34268 64.50718) (xy 145.651444 64.045082) (xy 145.759868 63.5)
+        (xy 145.651444 62.954918) (xy 145.34268 62.49282) (xy 144.949342 62.23) (xy 145.34268 61.96718) (xy 145.651444 61.505082)
+        (xy 145.759868 60.96) (xy 145.651444 60.414918) (xy 145.34268 59.95282) (xy 144.932011 59.67842) (xy 144.987441 59.662059)
+        (xy 145.41313 59.318026) (xy 145.67476 58.837277) (xy 145.68972 58.76307) (xy 145.56722 58.547) (xy 143.637 58.547)
+        (xy 143.637 58.567) (xy 143.383 58.567) (xy 143.383 58.547) (xy 141.45278 58.547) (xy 141.33028 58.76307)
+        (xy 141.34524 58.837277) (xy 141.60687 59.318026) (xy 142.032559 59.662059) (xy 142.087989 59.67842) (xy 141.67732 59.95282)
+        (xy 141.595682 60.075) (xy 141.336579 60.075) (xy 140.585 59.323422) (xy 140.585 58.07693) (xy 141.33028 58.07693)
+        (xy 141.45278 58.293) (xy 143.383 58.293) (xy 143.383 57.023) (xy 143.637 57.023) (xy 143.637 58.293)
+        (xy 145.56722 58.293) (xy 145.68972 58.07693) (xy 145.67476 58.002723) (xy 145.41313 57.521974) (xy 144.987441 57.177941)
+        (xy 144.4625 57.023) (xy 143.637 57.023) (xy 143.383 57.023) (xy 142.5575 57.023) (xy 142.032559 57.177941)
+        (xy 141.60687 57.521974) (xy 141.34524 58.002723) (xy 141.33028 58.07693) (xy 140.585 58.07693) (xy 140.585 49.896578)
+        (xy 142.02104 48.460538) (xy 142.093744 48.475) (xy 142.386256 48.475) (xy 142.819418 48.388839) (xy 143.310625 48.060625)
+        (xy 143.51 47.762239) (xy 143.709375 48.060625) (xy 143.895 48.184656) (xy 143.895001 49.617165) (xy 143.946349 49.87531)
+        (xy 144.141952 50.168049) (xy 144.434691 50.363652) (xy 144.78 50.432338) (xy 145.12531 50.363652) (xy 145.418049 50.168049)
+        (xy 145.613652 49.87531) (xy 145.665 49.617165) (xy 145.665 48.184656) (xy 145.850625 48.060625) (xy 145.865096 48.038967)
+        (xy 145.931673 48.199698) (xy 146.110301 48.378327) (xy 146.34369 48.475) (xy 147.03425 48.475) (xy 147.193 48.31625)
+        (xy 147.193 47.117) (xy 147.447 47.117) (xy 147.447 48.31625) (xy 147.60575 48.475) (xy 148.29631 48.475)
+        (xy 148.529699 48.378327) (xy 148.708327 48.199698) (xy 148.805 47.966309) (xy 148.805 47.27575) (xy 148.64625 47.117)
+        (xy 147.447 47.117) (xy 147.193 47.117) (xy 147.173 47.117) (xy 147.173 46.863) (xy 147.193 46.863)
+        (xy 147.193 45.66375) (xy 147.447 45.66375) (xy 147.447 46.863) (xy 148.64625 46.863) (xy 148.805 46.70425)
+        (xy 148.805 46.013691) (xy 148.708327 45.780302) (xy 148.529699 45.601673) (xy 148.29631 45.505) (xy 147.60575 45.505)
+        (xy 147.447 45.66375) (xy 147.193 45.66375) (xy 147.03425 45.505) (xy 146.34369 45.505) (xy 146.110301 45.601673)
+        (xy 145.931673 45.780302) (xy 145.865096 45.941033) (xy 145.850625 45.919375) (xy 145.667074 45.79673) (xy 145.682337 45.719999)
+        (xy 145.665 45.632839) (xy 145.665 45.632835) (xy 145.613652 45.37469) (xy 145.541615 45.26688) (xy 145.467424 45.155845)
+        (xy 145.467423 45.155844) (xy 145.418049 45.081951) (xy 145.344156 45.032577) (xy 144.197425 43.885847) (xy 144.148049 43.811951)
+        (xy 143.85531 43.616348) (xy 143.597165 43.565) (xy 143.597161 43.565) (xy 143.51 43.547663) (xy 143.422839 43.565)
+        (xy 141.057159 43.565) (xy 140.969999 43.547663) (xy 140.882839 43.565) (xy 140.882835 43.565) (xy 140.62469 43.616348)
+        (xy 140.51688 43.688385) (xy 140.405845 43.762576) (xy 140.405844 43.762577) (xy 140.331951 43.811951) (xy 140.282577 43.885844)
+        (xy 138.045 46.123422) (xy 138.045 39.736578) (xy 142.02104 35.760538) (xy 142.093744 35.775) (xy 142.386256 35.775)
+        (xy 142.819418 35.688839) (xy 143.310625 35.360625) (xy 143.51 35.062239) (xy 143.709375 35.360625) (xy 144.200582 35.688839)
+        (xy 144.633744 35.775) (xy 144.926256 35.775) (xy 145.359418 35.688839) (xy 145.850625 35.360625) (xy 145.865096 35.338967)
+        (xy 145.931673 35.499698) (xy 146.110301 35.678327) (xy 146.34369 35.775) (xy 147.03425 35.775) (xy 147.193 35.61625)
+        (xy 147.193 34.417) (xy 147.447 34.417) (xy 147.447 35.61625) (xy 147.60575 35.775) (xy 148.29631 35.775)
+        (xy 148.529699 35.678327) (xy 148.708327 35.499698) (xy 148.805 35.266309) (xy 148.805 34.57575) (xy 148.64625 34.417)
+        (xy 147.447 34.417) (xy 147.193 34.417) (xy 147.173 34.417) (xy 147.173 34.163) (xy 147.193 34.163)
+        (xy 147.193 32.96375) (xy 147.447 32.96375) (xy 147.447 34.163) (xy 148.64625 34.163) (xy 148.805 34.00425)
+        (xy 148.805 33.909566) (xy 158.129876 33.909566) (xy 158.249223 34.163) (xy 159.893 34.163) (xy 159.893 32.655)
+        (xy 160.147 32.655) (xy 160.147 34.163) (xy 161.790777 34.163) (xy 161.910124 33.909566) (xy 161.879144 33.781645)
+        (xy 161.565922 33.223683) (xy 161.06302 32.828058) (xy 160.447 32.655) (xy 160.147 32.655) (xy 159.893 32.655)
+        (xy 159.593 32.655) (xy 158.97698 32.828058) (xy 158.474078 33.223683) (xy 158.160856 33.781645) (xy 158.129876 33.909566)
+        (xy 148.805 33.909566) (xy 148.805 33.313691) (xy 148.708327 33.080302) (xy 148.529699 32.901673) (xy 148.29631 32.805)
+        (xy 147.60575 32.805) (xy 147.447 32.96375) (xy 147.193 32.96375) (xy 147.03425 32.805) (xy 146.34369 32.805)
+        (xy 146.110301 32.901673) (xy 145.931673 33.080302) (xy 145.865096 33.241033) (xy 145.850625 33.219375) (xy 145.667074 33.09673)
+        (xy 145.682337 33.019999) (xy 145.665 32.932839) (xy 145.665 32.932835) (xy 145.613652 32.67469) (xy 145.418049 32.381951)
+        (xy 145.344156 32.332577) (xy 144.376578 31.365) (xy 163.717422 31.365)
       )
     )
   )

+ 6 - 5
PCB/BLOBCNC_TOP/BLOBCNC_TOP.kicad_pcb-bak

@@ -58,10 +58,11 @@
     (pad_size 1.524 1.524)
     (pad_drill 0.762)
     (pad_to_mask_clearance 0.2)
-    (aux_axis_origin 0 0)
+    (aux_axis_origin 188.976 28.956)
+    (grid_origin 188.976 28.956)
     (visible_elements FFFFFF7F)
     (pcbplotparams
-      (layerselection 0x010fc_ffffffff)
+      (layerselection 0x01000_7fffffff)
       (usegerberextensions false)
       (usegerberattributes false)
       (usegerberadvancedattributes false)
@@ -71,7 +72,7 @@
       (plotframeref false)
       (viasonmask false)
       (mode 1)
-      (useauxorigin false)
+      (useauxorigin true)
       (hpglpennumber 1)
       (hpglpenspeed 20)
       (hpglpendiameter 15.000000)
@@ -84,7 +85,7 @@
       (subtractmaskfromsilk false)
       (outputformat 1)
       (mirror false)
-      (drillshape 1)
+      (drillshape 0)
       (scaleselection 1)
       (outputdirectory ""))
   )
@@ -1639,7 +1640,7 @@
   (segment (start 125.73 74.93) (end 114.36501 86.29499) (width 0.5) (layer F.Cu) (net 44))
   (segment (start 101.73999 86.29499) (end 100.965 85.52) (width 0.5) (layer F.Cu) (net 44))
 
-  (zone (net 10) (net_name GND) (layer F.Cu) (tstamp 5CEDF2BE) (hatch edge 0.508)
+  (zone (net 10) (net_name GND) (layer F.Cu) (tstamp 5CE5177B) (hatch edge 0.508)
     (connect_pads (clearance 0.508))
     (min_thickness 0.254)
     (fill yes (arc_segments 16) (thermal_gap 0.508) (thermal_bridge_width 0.508))

+ 4 - 4
PCB/BLOBCNC_TOP/BLOBCNC_TOP.net

@@ -1,7 +1,7 @@
 (export (version D)
   (design
     (source "/home/titi/work/projets/Fresnoy/Hadrien Tequi/HTequi/PCB/BLOBCNC_TOP/BLOBCNC_TOP.sch")
-    (date "lun. 20 mai 2019 01:00:13 CEST")
+    (date "mar. 02 juil. 2019 14:54:09 CEST")
     (tool "Eeschema 5.0.0+dfsg1-2")
     (sheet (number 1) (name /) (tstamps /)
       (title_block
@@ -48,7 +48,7 @@
       (tstamp 5CE1C5A7))
     (comp (ref U1)
       (value DC-DC_MP1584EN)
-      (footprint "Lien:DC-DC LStepdown M2596")
+      (footprint Lien:DC-DC_MP1584EN)
       (libsource (lib Lien) (part DC-DC_MP1584EN) (description ""))
       (sheetpath (names /) (tstamps /))
       (tstamp 5CE1C6E1))
@@ -271,8 +271,8 @@
         (pin (num 12) (name 1A) (type output))
         (pin (num 13) (name 2A) (type output))
         (pin (num 14) (name 2B) (type output))
-        (pin (num 15) (name GND) (type output))
-        (pin (num 16) (name Vmot) (type output))))
+        (pin (num 15) (name GND) (type power_in))
+        (pin (num 16) (name Vmot) (type power_in))))
     (libpart (lib Lien) (part POL_MAX14870)
       (fields
         (field (name Reference) MotorDriver)

+ 2 - 1
PCB/BLOBCNC_TOP/BLOBCNC_TOP.sch

@@ -1,4 +1,5 @@
 EESchema Schematic File Version 4
+LIBS:BLOBCNC_TOP-cache
 EELAYER 26 0
 EELAYER END
 $Descr A4 11693 8268
@@ -74,7 +75,7 @@ U 1 1 5CE1C6E1
 P 2200 1300
 F 0 "U1" V 2715 1300 50  0000 C CNN
 F 1 "DC-DC_MP1584EN" V 2624 1300 50  0000 C CNN
-F 2 "Lien:DC-DC LStepdown M2596" H 2200 1300 50  0001 C CNN
+F 2 "Lien:DC-DC_MP1584EN" H 2200 1300 50  0001 C CNN
 F 3 "" H 2200 1300 50  0001 C CNN
 	1    2200 1300
 	0    1    -1   0   

+ 18 - 18
PCB/BLOBCNC_TOP/CNC/blobcnc_TOP_CURT.gcode

@@ -3,29 +3,29 @@ G21
 G90
 G94
 F100.00
-G00 Z0.2000
+G00 Z1.0000
 M03 S10000
 G4 P1
-G00 X0.7420Y-46.1855
+G00 X1.4500Y-46.1730
 G01 Z-1.8000
-G01 X0.7420Y0.7420
-G01 X-56.3455Y0.7420
-G00 Z0.2000
-G00 X-58.9955Y0.7420
+G01 X1.4500Y1.4500
+G01 X-56.3330Y1.4500
+G00 Z1.0000
+G00 X-58.9830Y1.4500
 G01 Z-1.8000
-G01 X-116.0830Y0.7420
-G01 X-116.0830Y-46.1855
-G00 Z0.2000
-G00 X-116.0830Y-48.8355
+G01 X-116.7660Y1.4500
+G01 X-116.7660Y-46.1730
+G00 Z1.0000
+G00 X-116.7660Y-48.8230
 G01 Z-1.8000
-G01 X-116.0830Y-95.7630
-G01 X-58.9955Y-95.7630
-G00 Z0.2000
-G00 X-56.3455Y-95.7630
+G01 X-116.7660Y-96.4460
+G01 X-58.9830Y-96.4460
+G00 Z1.0000
+G00 X-56.3330Y-96.4460
 G01 Z-1.8000
-G01 X0.7420Y-95.7630
-G01 X0.7420Y-48.8355
-G00 Z0.2000
-G00 Z0.2000
+G01 X1.4500Y-96.4460
+G01 X1.4500Y-48.8230
+G00 Z1.0000
+G00 Z1.0000
 G00 X0Y0
 M05

File diff suppressed because it is too large
+ 398 - 382
PCB/BLOBCNC_TOP/CNC/blobcnc_TOP_DRILL.gcode


+ 0 - 0
PCB/BLOBCNC_TOP/CNC/blobcnc_TOP_ISOL.gcode


Some files were not shown because too many files changed in this diff