浏览代码

teenstep bugfixes
feeder quite working version

eLandon 6 年之前
父节点
当前提交
61a3756ad3

+ 70 - 21
HTequi-firmware/lib/Atm_lien/Atm_Teenstep.cpp

@@ -7,14 +7,14 @@
 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_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,               -1,             -1,             -1,                -1,       -1,           -1,       -1,          -1,    ENABLED,   -1,
-    /*        ENABLED */        ENT_ENABLED,         -1,              -1,               -1,             -1,             -1,                -1,       -1,           -1,  RUNNING,    DISABLED,         -1,   -1,
-    /*        RUNNING */        ENT_RUNNING, LP_RUNNING,              -1,               -1, EMERGENCY_STOP, EMERGENCY_STOP,    EMERGENCY_STOP, STOPPING,      ENABLED,  RUNNING,          -1,         -1,   -1,
-    /*       STOPPING */       ENT_STOPPING,         -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,  RUNNING,          -1,    ENABLED,   -1,
-    /*    HOMING_HIGH */    ENT_HOMING_HIGH,         -1, EXT_HOMING_HIGH,               -1,        ENABLED, EMERGENCY_STOP,    EMERGENCY_STOP, STOPPING,           -1,       -1,          -1,         -1,   -1,
-    /*     HOMING_LOW */     ENT_HOMING_LOW,         -1,  EXT_HOMING_LOW,               -1, EMERGENCY_STOP,        ENABLED,    EMERGENCY_STOP, STOPPING,           -1,       -1,          -1,         -1,   -1,
+    /*                             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, EMERGENCY_STOP, EMERGENCY_STOP,    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, EMERGENCY_STOP,    EMERGENCY_STOP, STOPPING,           -1,       -1,          -1,         -1,   -1,
+    /*     HOMING_LOW */     ENT_HOMING_LOW,         -1,  EXT_HOMING_LOW,             -1,              -1,               -1, EMERGENCY_STOP,        ENABLED,    EMERGENCY_STOP, STOPPING,           -1,       -1,          -1,         -1,   -1,
   };
   // clang-format on
   Machine::begin( state_table, ELSE );
@@ -28,15 +28,20 @@ Atm_Teenstep& Atm_Teenstep::begin(Stepper & motorRef, StepControl & stepControlR
  */
 
 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 0;
-    case EVT_LIMIT_LOW:
       return limitState[0];
-    case EVT_EMERGENCYSTOP:
+    case EVT_LIMIT_LOW:
       return limitState[1];
+    case EVT_EMERGENCYSTOP:
+      return 0;
     case EVT_STOP:
       return 0;
     case EVT_ONTARGET:
@@ -81,7 +86,7 @@ void Atm_Teenstep::action( int id ) {
       //connectors[ON_ONCHANGEPOSITION].push(_currentStep);
       // if (tempStep != _currentStep){push( connectors, ON_ONCHANGEPOSITION, 0, _currentStep, 0 );} ;
       _currentStep =  tempStep;
-      updateLimitSwitch();
+      //updateLimitSwitch();
       sendOSC();
       return;
     case ENT_STOPPING:
@@ -89,21 +94,51 @@ void Atm_Teenstep::action( int id ) {
       return;
     case ENT_EMERGENCY_STOP:
       controller->emergencyStop();
+      trigger(EVT_ENABLE);
       sendOSC();
       return;
     case ENT_HOMING_HIGH:
-      _limitType ? move(2147483647) : trigger(EVT_ENABLE) ;
+      if(_limitType) {
+        motor->setTargetRel(2147483647);
+        controller->moveAsync(*motor);
+        }
       sendOSC();
       return;
     case EXT_HOMING_HIGH:
-      _maxStep = motor->getPosition();
+      controller->emergencyStop();
+      if(last_trigger == EVT_LIMIT_HIGH){
+        _maxStep = motor->getPosition();
+        Serial.print("Stepper maxPos ");
+        Serial.println(motor->getPosition());
+        trigger(EVT_ENABLE);
+      }
+      else{
+        Serial.println("homing failed ");
+        trigger(EVT_EMERGENCYSTOP);
+      }
+
       return;
     case ENT_HOMING_LOW:
-      _limitType ? move(-2147483647) : trigger(EVT_STOP) ;
+      if(_limitType) {
+        motor->setTargetRel(-2147483647);
+        controller->moveAsync(*motor);
+        }
       sendOSC();
       return;
     case EXT_HOMING_LOW:
-      motor->setPosition(0);
+      controller->emergencyStop();
+      if(last_trigger == EVT_LIMIT_LOW){
+        motor->setPosition(0);
+        Serial.print("Stepper homed ");
+        Serial.println(motor->getPosition());
+        trigger(EVT_ENABLE);
+      }
+      else{
+        Serial.println("homing failed ");
+        trigger(EVT_EMERGENCYSTOP);
+      }
+
+
       return;
   }
 }
@@ -223,6 +258,15 @@ Atm_Teenstep& Atm_Teenstep::sendOSC( void ){
 /* 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 );
@@ -283,6 +327,11 @@ Atm_Teenstep& Atm_Teenstep::enable() {
   return *this;
 }
 
+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 )
  */
@@ -343,8 +392,8 @@ Atm_Teenstep& Atm_Teenstep::onLimitlow( atm_cb_push_t callback, int idx ) {
  * 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_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;
-}
+ 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;
+ }

+ 5 - 3
HTequi-firmware/lib/Atm_lien/Atm_Teenstep.h

@@ -6,8 +6,8 @@
 class Atm_Teenstep: public Machine {
 
  public:
-  enum { DISABLED, ENABLED, RUNNING, STOPPING, EMERGENCY_STOP, HOMING_HIGH, HOMING_LOW }; // STATES
-  enum { EVT_MOVE_TIMEOUT, EVT_LIMIT_HIGH, EVT_LIMIT_LOW, EVT_EMERGENCYSTOP, EVT_STOP, EVT_ONTARGET, EVT_MOVE, EVT_DISABLE, EVT_ENABLE, ELSE }; // EVENTS
+   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 );
@@ -21,6 +21,8 @@ class Atm_Teenstep: public Machine {
   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 );
@@ -34,7 +36,7 @@ class Atm_Teenstep: public Machine {
 
   Atm_Teenstep& move( long int stepRel );
   Atm_Teenstep& moveTo( long int stepAbs );
-  Atm_Teenstep& homing( int direction );
+  Atm_Teenstep& homing( bool direction );
 
   Atm_Teenstep& setLimitType( int limitType = 0);
   Atm_Teenstep& setLimitPins( int limitPinLow);

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

@@ -24,6 +24,7 @@ Atm_Teenstep_OSC& Atm_Teenstep_OSC::onOSC(OSCMessage& msg ){
     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;
   }

+ 54 - 49
HTequi-firmware/src/blobcnc_feeder/main.cpp

@@ -2,7 +2,7 @@
 
 #include <Automaton.h>
 //#include "Atm_lien/Atm_stepper.h"
-#include "Atm_Tstepper.h"
+#include "Atm_Teenstep.h"
 #include "Atm_TeensyStep.h"
 #include <SPI.h>
 #include <Ethernet.h>
@@ -10,71 +10,74 @@
 #include <TeensyMAC.h>
 #include <OSCMessage.h>
 
-////////////////    Ethernet  /////////////////////////////
-// Enter a MAC address and IP address for your controller below.
-// The IP address will be dependent on your local network:
-byte mac[] = {0x00, 0xAA, 0xBB, 0xCC, 0xDE, 0x02};
-IPAddress ip(192, 168, 1, 204);    //local IP of Arduino/Teensy
-//unsigned int localPort = 8888;      // local port to listen on (not needed for multicast)
-IPAddress ipMulti(239, 200, 200, 200); // ipMidi Multicast address
-unsigned int portMulti = 9977;   // ipMidi Mutlicast port 1
-// buffers for receiving and sending data
-byte packetBuffer[UDP_TX_PACKET_MAX_SIZE];       // buffer to hold incoming packet
-byte sendBuffer1[] = {0x90, 0x14, 0x22};        // MIDI Note On to Multicast address
-byte sendBuffer2[] = {0x80, 0x14, 0x00};        // MIDI Note Off to Multicast address
-// An EthernetUDP instance to let us send and receive packets over UDP
-EthernetUDP Udp;
-
-///////////////////       OSC         ////////////////////////
-OSCBundle bndl;
+// ////////////////    Ethernet  /////////////////////////////
+// // Enter a MAC address and IP address for your controller below.
+// // The IP address will be dependent on your local network:
+// byte mac[] = {0x00, 0xAA, 0xBB, 0xCC, 0xDE, 0x02};
+// IPAddress ip(192, 168, 1, 204);    //local IP of Arduino/Teensy
+// //unsigned int localPort = 8888;      // local port to listen on (not needed for multicast)
+// IPAddress ipMulti(239, 200, 200, 200); // ipMidi Multicast address
+// unsigned int portMulti = 9977;   // ipMidi Mutlicast port 1
+// // buffers for receiving and sending data
+// byte packetBuffer[UDP_TX_PACKET_MAX_SIZE];       // buffer to hold incoming packet
+// byte sendBuffer1[] = {0x90, 0x14, 0x22};        // MIDI Note On to Multicast address
+// byte sendBuffer2[] = {0x80, 0x14, 0x00};        // MIDI Note Off to Multicast address
+// // An EthernetUDP instance to let us send and receive packets over UDP
+// EthernetUDP Udp;
+//
+// ///////////////////       OSC         ////////////////////////
+// OSCBundle bndl;
 
 /////////////////// STEPPER machines  ////////////////////////
 /*move are always meant in full steps on the motor output (after reduction)
 so all motors have 200steps per turn */
-const int BARREL_SPEED = 8000 ;
+const int BARREL_SPEED = 20 ;
 const int BARREL_ACC = 800 ;
 const int BARREL_DISTANCE = 10 ; //in full steps
 const int BARREL_REDUCTION_RATIO = 26.85;
 const int BARREL_MICROSTEP = 32;
 
-static uint16_t BARREL_THRESHOLD[] = {5} ; //for sensor to trig
+static uint16_t BARREL_THRESHOLD[] = {880} ; //for sensor to trig
 const int BARREL_INIBSENSOR = 5000;
 
-const int PILL_SPEED = 10 ;
+const int PILL_SPEED = 20 ;
 const int PILL_ACC = 1000 ;
 const int PILL_DISTANCE = 200 ; //in full steps
 const int PILL_MICROSTEP = 32;
 
 
 
-Atm_TeensyStep barrel_step;
+Atm_Teenstep barrel_step;
 Stepper barrel_stepper(3, 2);
 StepControl barrel_controller ;
 
-Atm_comparator barrel_sensor;
+Atm_analog barrel_sensor;
 
-Atm_TeensyStep pill_step;
+
+Atm_Teenstep pill_step;
 Stepper pill_stepper(6, 5);
 StepControl pill_controller ;
 
-Atm_comparator pill_sensor ;
-
+Atm_analog pill_sensor ;
+uint16_t avgbuffer[16];
 
 void barrel_homing(){
+  barrel_step.move(5*BARREL_MICROSTEP*BARREL_REDUCTION_RATIO);
+  //automaton.delay(BARREL_INIBSENSOR); //let the current position go
+  automaton.delay(BARREL_INIBSENSOR);
   barrel_step.move(BARREL_DISTANCE*BARREL_MICROSTEP*BARREL_REDUCTION_RATIO);
-  automaton.delay(BARREL_INIBSENSOR); //let the current position go
   bool foundHome = false ;
-  while(barrel_step.state() != barrel_step.STOPPING){
+  while(barrel_step.state() != barrel_step.ENABLED){
     // automaton.run();
-    barrel_step.cycle();
+    automaton.run();
+    Serial.println(barrel_sensor.state());
     if(barrel_sensor.state()<BARREL_THRESHOLD[0]){
-      barrel_step.emergencyStop();
+      barrel_step.emergencystop();
       barrel_stepper.setPosition(0);
       foundHome = true ;
       break ;
     }
   }
-  digitalWrite( 13, LOW );
   if (!foundHome){Serial.println("homing 0");}
   else{Serial.println("homing 1");}
 
@@ -82,23 +85,25 @@ void barrel_homing(){
 
 
 void barrel_move(int stepRel){
-  Serial.println(stepRel);
-  barrel_step.move(stepRel);
+  Serial.println(stepRel*BARREL_MICROSTEP*BARREL_REDUCTION_RATIO);
+  barrel_step.move(stepRel*BARREL_MICROSTEP*BARREL_REDUCTION_RATIO);
 }
 
 void pill_move(int stepRel){
-  Serial.println(stepRel);
-  pill_step.move(stepRel);
+  Serial.println(stepRel* PILL_MICROSTEP);
+  pill_step.move(stepRel* PILL_MICROSTEP);
 }
 
 void pill_next(){
-  pill_move(200 * PILL_MICROSTEP);
+  pill_step.move(PILL_DISTANCE * PILL_MICROSTEP);
 
   //make one turn, if nothing was seen by sensor try again up to 5 times
   bool foundPill = false ;
-  while(pill_step.state() != barrel_step.STOPPING){
+  while(pill_step.state() != pill_step.ENABLED){
+    int value = pill_sensor.state();
     automaton.run();
-    if(pill_sensor.state()<BARREL_THRESHOLD[0]){foundPill = true ;}
+    Serial.println(value);
+    if(value<650){foundPill = true ;break;}
   }
   if (!foundPill){Serial.println("pill 0");}
   else{Serial.println("pill 1");}
@@ -175,23 +180,23 @@ void setup() {
   // pinMode(4, OUTPUT);
   // pinMode(4, LOW);
   barrel_step.trace( Serial );
-  barrel_step.begin(barrel_stepper, barrel_controller, Udp, bndl, "/Y_top")
+  barrel_step.begin(barrel_stepper, barrel_controller)
         .setEnablePin(4).enableReversed(1);
         //.setLimitType(3).setLimitPins(A3).limitThresholds(800, 1000, 0, 100).homing(1);
-  barrel_stepper.setMaxSpeed(BARREL_SPEED);
-  barrel_stepper.setAcceleration(BARREL_ACC);
+  barrel_stepper.setMaxSpeed(BARREL_SPEED*BARREL_MICROSTEP);
+  barrel_stepper.setAcceleration(BARREL_ACC*BARREL_MICROSTEP);
   barrel_stepper.setInverseRotation(true);
-  barrel_sensor.begin(A3, 50).threshold(BARREL_THRESHOLD, 1)
-              .onChange( []( int idx, int v, int up ){Serial.println(up);} )
-              .onChange( true, []( int idx, int v, int up ){Serial.println(up);} )
-              .onChange( false, []( int idx, int v, int up ){Serial.println(up);} );
+  barrel_sensor.begin(A3, 50);//.threshold(BARREL_THRESHOLD, 1)
+              // .onChange( []( int idx, int v, int up ){Serial.println(up);} )
+              // .onChange( true, []( int idx, int v, int up ){Serial.println(up);} )
+              // .onChange( false, []( int idx, int v, int up ){Serial.println(up);} );
 
   pill_step.trace( Serial );
-  pill_step.begin(pill_stepper, pill_controller,  Udp, bndl, "/Y_top")
+  pill_step.begin(pill_stepper, pill_controller)
         .setEnablePin(7).enableReversed(1);
-  pill_stepper.setMaxSpeed(PILL_SPEED);
-  pill_stepper.setAcceleration(PILL_ACC);
-  pill_sensor.begin(A1, 50);
+  pill_stepper.setMaxSpeed(PILL_SPEED*PILL_MICROSTEP);
+  pill_stepper.setAcceleration(PILL_ACC*PILL_MICROSTEP);
+  pill_sensor.begin(A1, 50).average( avgbuffer, sizeof( avgbuffer ) );
   //stepper.onOnchange(Machine &machine, optional int event = 0)
   //stepper.cycle(1000);
   //barrel_step.move(10000);

+ 6 - 6
HTequi-firmware/src/blobcnc_top/main.cpp

@@ -87,8 +87,8 @@ void setup() {
   X_top_step.trace( Serial );
    // X_top_step.begin(X_top_stepper, X_top_controller, Udp, bndl, "/X_top")
   X_top_step.begin(X_top_stepper, X_top_controller)
-            .setEnablePin(18);//.enableReversed(1);//.enable(1);
-            //.setLimitType(3).setLimitPins(A3).limitReversed(true).limitThresholds(600, 750, 950, 1200);
+            .setEnablePin(18)//.enableReversed(1);//.enable(1);
+            .setLimitType(2).setLimitPins(A3, 16).limitReversed(true).limitThresholds(600, 750, 950, 1200);
   X_top_stepper.setMaxSpeed(HIGH_SPEED);
   X_top_stepper.setAcceleration(HIGH_ACC);
   X_top_stepper.setInverseRotation(true);
@@ -105,7 +105,7 @@ void setup() {
   Y_top_stepper.setInverseRotation(false);
   //stepper.onOnchange(Machine &machine, optional int event = 0)
   //stepper.cycle(1000);
-  X_top_step.move(-100000);
+  X_top_step.move(-100000).disable();
   Y_top_step.move(1000);
   //delay(2000);
 
@@ -161,9 +161,9 @@ void loop() {
 
   // Serial.print(analogRead(A3));
   // Serial.print("   ");
-  // Serial.print(Y_top_step.limitState[0]);
-  // Serial.print("   ");
-  // Serial.println(X_top_step.limitState[1]);
+   // Serial.print(X_top_step.limitState[0]);
+   // Serial.print("   ");
+   // Serial.println(X_top_step.limitState[1]);
 
   // OSCMessage msg("/analog/0");
   // msg.add((int32_t)analogRead(0));

+ 2 - 2
blob-CN/main.pd

@@ -55,9 +55,9 @@
 #X msg 733 282 /Y_top/emergencyStop;
 #X obj 1159 335 list prepend set;
 #X obj 1159 360 list trim;
-#X msg 1159 385 /X_top/move 1000;
-#X msg 563 279 /X_top/home 0;
+#X msg 1159 385 /X_top/home 0;
 #X msg 530 194 sendtyped /X_top/move i 1000;
+#X msg 563 279 /X_top/homing 0;
 #X connect 0 0 35 0;
 #X connect 2 0 35 0;
 #X connect 3 0 35 0;