updated Atmlien

HTequi-firmware/lib/Atm_lien/Atm_AccelStepper.cpp

@@ -7,15 +7,15 @@
 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
+    /*                       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,    LP_LIMIT_LOW,              -1,         -1,         -1,                  -1,     RUNNING,      STOP,              STOP,        LIMIT_LOW,                -1,            -1,            -1,              -1,   -1,
+    /*  LIMIT_HIGH */  ENT_LIMIT_HIGH,   LP_LIMIT_HIGH,              -1,         -1,         -1,                  -1,     RUNNING,      STOP,              STOP,               -1,        LIMIT_HIGH,            -1,            -1,              -1,   -1
   };
   // clang-format on
   Machine::begin( state_table, ELSE );
@@ -24,6 +24,7 @@ Atm_AccelStepper& Atm_AccelStepper::begin(int step_pin, int dir_pin) {
   stepper->setAcceleration(acceleration);
   idle_timer.set(ATM_TIMER_OFF);
   position_timer.set(POSITION_SEND_TIMER);
+
   return *this;
 }
 
@@ -47,37 +48,39 @@ int Atm_AccelStepper::event( int id ) {
     case EVT_EMERGENCY_STOP:
       return 0;
     case EVT_ON_LIMIT_LOW:
+      previous_state = limitLow_State;
       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;
+          return limitLow_State != previous_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;
+          if(limitLow_State){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 != previous_state;
       }
     case EVT_ON_LIMIT_HIGH:
+      previous_state = limitHigh_State;
       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;
+          return limitHigh_State != previous_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;
+          return limitHigh_State != previous_state;
       }
     case EVT_ON_TARGET:
       return runMode ? 0 : _currentStep == _targetStep;
@@ -112,7 +115,6 @@ void Atm_AccelStepper::action( int id ) {
       _isHoming = 0 ;
       if(last_trigger == EVT_ON_TARGET){push( connectors, ON_ONTARGET, 0, _currentStep, 0 );};
       push(connectors, ON_CHANGESTATE, 0,  state(), 0);
-      push(connectors, ON_CHANGEPOSITION, 0,  _currentStep, stepper->speed());
       enabled = _enableReversed ? LOW : HIGH ;
       digitalWrite(_enablePin, enabled);
 
@@ -120,21 +122,23 @@ void Atm_AccelStepper::action( int id ) {
       return;
     case ENT_RUNNING:
       push(connectors, ON_CHANGESTATE, 0,  state(), 0);
-      push(connectors, ON_CHANGEPOSITION, 0,  _currentStep, stepper->speed());
       _isHoming = 0;
-      stepper->setMaxSpeed(max_speed);
-      stepper->setAcceleration(acceleration);
+      //reset limit state so that they trigger again if we're stopped on it
+      // limitLow_State = 0;
+      // limitHigh_State = 0;
+      Serial.print("target ");
+      Serial.println(_targetStep);
+      stepper->moveTo(_targetStep);
+      // stepper->computeNewSpeed();
+      //stepper_update();
       //push(connectors, ON_CHANGEPOSITION, 0,  _currentStep, stepper->speed());
       position_timer.setFromNow(this, POSITION_SEND_TIMER);
       return;
     case LP_RUNNING:
       stepper_update();
-      Serial.println(stepper->speed());
-      if(stepper->speed() == 0) {trigger(EVT_ON_TARGET);}
       return;
     case ENT_STOP:
-      push(connectors, ON_CHANGESTATE, 0,  state(), 0);
-      push(connectors, ON_CHANGEPOSITION, 0,  _currentStep, stepper->speed());
+    push(connectors, ON_CHANGESTATE, 0,  state(), 0);
       if (last_trigger == EVT_STOP) {
         runMode = 0 ;
         stepper->stop();
@@ -148,16 +152,12 @@ void Atm_AccelStepper::action( int id ) {
         stepper->setSpeed(0);
         push( connectors, ON_STOP, 0, 1, 0 );
       }
-      stepper_update();
       return;
     case LP_STOP:
       stepper_update();
       _currentStep = stepper->currentPosition();
-
-      if(stepper->speed() == 0.) {Serial.println(stepper->speed());trigger(EVT_ON_TARGET);}
       return;
     case ENT_HOMING_LOW:
-      push(connectors, ON_CHANGEPOSITION, 0,  _currentStep, stepper->speed());
       push(connectors, ON_CHANGESTATE, 0,  state(), 0);
       runMode = 1;
       _isHoming = 1 ;
@@ -167,6 +167,9 @@ void Atm_AccelStepper::action( int id ) {
       stepper_update();
       return;
     case EXT_HOMING_LOW:
+      runMode = 0;
+      _isHoming = 0;
+      trigger(EVT_EMERGENCY_STOP);
       if(last_trigger == EVT_ON_LIMIT_LOW) {
         stepper->setCurrentPosition(0);
         _currentStep = 0;
@@ -178,7 +181,6 @@ void Atm_AccelStepper::action( int id ) {
       return;
     case ENT_HOMING_HIGH:
       push(connectors, ON_CHANGESTATE, 0,  state(), 0);
-      push(connectors, ON_CHANGEPOSITION, 0,  _currentStep, stepper->speed());
       runMode = 1;
       _isHoming = 2 ;
       stepper->setSpeed(homing_speed);
@@ -187,6 +189,9 @@ void Atm_AccelStepper::action( int id ) {
       stepper_update();
       return;
     case EXT_HOMING_HIGH:
+      runMode = 0;
+      _isHoming = 0;
+      trigger(EVT_EMERGENCY_STOP);
       if(last_trigger == EVT_ON_LIMIT_HIGH) {
         _maxStep = stepper->currentPosition();
         _currentStep = _maxStep;
@@ -196,42 +201,42 @@ void Atm_AccelStepper::action( int id ) {
       _targetStep = _currentStep;
       return;
     case ENT_LIMIT_LOW:
-      push( connectors, ON_ONLIMITLOW, 0, 0, 0 );
-      push(connectors, ON_CHANGEPOSITION, 0,  _currentStep, stepper->speed());
+      /*triggered by a change in limit state
+       if state is 0, we may leave this state for running
+       if state is 1 we stay in limit state loop, where moves are allowed only in
+            the free direction, until a trigger comes with state 0
+      */
+      push( connectors, ON_ONLIMITLOW, 0, limitLow_State, 0 );
+      if (!limitLow_State){trigger(EVT_MOVE);}
+      return;
+
+    case LP_LIMIT_LOW:
       //stop motor if going down, allow going up
-      if(_limitLow_Hard && (stepper->speed()<0) ) {trigger(EVT_EMERGENCY_STOP);}
-      else{ stepper_update(); _isHoming ? trigger(EVT_ENABLE):trigger(EVT_MOVE);}
-       //   switch(_isHoming) {
-       //     case 0 :
-       //      trigger(EVT_MOVE);
-       //      break;
-       //     case 1 :
-       //      trigger(EVT_HOMING_LOW);
-       //      break;
-       //     case 2 :
-       //      trigger(EVT_HOMING_HIGH);
-       //      break;
-       //   }
-       // }
+      stepper_update();
+      if(_limitLow_Hard && (_targetStep < _currentStep)) {
+          // Serial.println("youpi");
+          _currentStep = stepper->currentPosition();
+          //stepper->moveTo(_currentStep);
+          //_targetStep = _currentStep;
+          stepper->setSpeed(0);
+        }
+      //else{} // _isHoming ? trigger(EVT_STOP):
       return;
     case ENT_LIMIT_HIGH:
-      push( connectors, ON_ONLIMITHIGH, 0, 1, 0 );
-      push(connectors, ON_CHANGEPOSITION, 0,  _currentStep, stepper->speed());
-      if(_limitHigh_Hard && (stepper->speed()>0)) {trigger(EVT_EMERGENCY_STOP);}
-      else{ stepper_update(); _isHoming ? trigger(EVT_ENABLE):trigger(EVT_MOVE);}
-       //  stepper_update();
-       //   switch(_isHoming) {
-       //     case 0 :
-       //      trigger(EVT_MOVE);
-       //      break;
-       //     case 1 :
-       //      trigger(EVT_HOMING_HIGH);
-       //      break;
-       //     case 2 :
-       //      trigger(EVT_HOMING_HIGH);
-       //      break;
-       //   }
-       // }
+      push( connectors, ON_ONLIMITHIGH, 0, limitHigh_State, 0 );
+      if (!limitHigh_State){trigger(EVT_MOVE);};
+      return;
+    case LP_LIMIT_HIGH:
+      //stop motor if going down, allow going up
+      stepper_update();
+      if(_limitHigh_Hard && (_targetStep > _currentStep)) {
+          // Serial.println("youpi");
+          _currentStep = stepper->currentPosition();
+          //stepper->moveTo(_currentStep);
+          //_targetStep = _currentStep;
+          stepper->setSpeed(0);
+        }
+      // else{}
       return;
   }
 }
@@ -261,6 +266,8 @@ int Atm_AccelStepper::state( void ) {
 */
 
 void Atm_AccelStepper::stepper_update(void) {
+
+
   switch (runMode) {
     case 0: //positional modae
       stepper->run();
@@ -269,9 +276,12 @@ void Atm_AccelStepper::stepper_update(void) {
       stepper->runSpeed();
       break;
   }
+  // Serial.print("update ");
+  // Serial.println(stepper->speed());
   long int tempStep = stepper->currentPosition();
   if (tempStep != _currentStep){
     _currentStep =  tempStep;
+    //Serial.println(stepper->currentPosition());
     if (position_timer.expired(this)){
       push(connectors, ON_CHANGEPOSITION, 0,  _currentStep, stepper->speed());
       position_timer.setFromNow(this, POSITION_SEND_TIMER);
@@ -292,36 +302,53 @@ Atm_AccelStepper& Atm_AccelStepper::setHomingSpeed(long int homingSpeed){
 
 Atm_AccelStepper& Atm_AccelStepper::setAcceleration(long int acc){
   acceleration = acc ;
-  stepper->setAcceleration(max_speed);
+  stepper->setAcceleration(acceleration);
+  return *this ;
+}
+
+Atm_AccelStepper& Atm_AccelStepper::setPosition(long int position){
+  stepper->setCurrentPosition(position);
+  _currentStep = position ;
   return *this ;
 }
 
+long int Atm_AccelStepper::getPosition(){
+  return stepper->currentPosition();;
+}
+
+long int Atm_AccelStepper::distanceToGo(){
+  return stepper->distanceToGo();;
+}
+
+
+bool Atm_AccelStepper::isRunning(){
+  return stepper->isRunning();
+}
+
+float Atm_AccelStepper::getSpeed(){
+  return stepper->speed();
+}
+
 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, long int maxSpeed,
-                                              long int acc) {
+Atm_AccelStepper& Atm_AccelStepper::move( long int stepRel) {
   _targetStep   = _currentStep + stepRel;
   runMode = 0;
   _isHoming = 0;
   //Serial.println(_targetStep);
-  max_speed = maxSpeed;
-  acceleration = acc;
   stepper->moveTo(_targetStep);
   enable();
   trigger( EVT_MOVE );
   return *this;
 }
 
-Atm_AccelStepper& Atm_AccelStepper::moveTo( long int stepAbs, long int maxSpeed,
-                                              long int acc) {
+Atm_AccelStepper& Atm_AccelStepper::moveTo( long int stepAbs) {
   _targetStep   = stepAbs;
   _isHoming = 0 ;
   runMode = 0;
-  max_speed = maxSpeed;
-  acceleration = acc;
   stepper->moveTo(_targetStep);
   enable();
   trigger( EVT_MOVE );
@@ -337,9 +364,8 @@ Atm_AccelStepper& Atm_AccelStepper::rotate( long  int speed) {
   return *this;
 }
 
-Atm_AccelStepper& Atm_AccelStepper::homing( bool direction, long int speed ){
+Atm_AccelStepper& Atm_AccelStepper::homing( bool direction ){
   enable();
-  homing_speed = speed ;
   direction == 1 ? _isHoming = 2 : _isHoming = 1;
   direction == 1 ? this->trigger(EVT_HOMING_HIGH) : this->trigger(EVT_HOMING_LOW);
 

HTequi-firmware/lib/Atm_lien/Atm_AccelStepper.h

@@ -43,16 +43,21 @@ class Atm_AccelStepper: public Machine {
   AccelStepper *stepper;
 
   long int homing_speed = 100;
-  long int max_speed = 5000;
+  long int max_speed = 10000;
   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, long int maxSpeed = 5000, long int acc = 1000 );
-  Atm_AccelStepper& moveTo(long int stepAbs, long int maxSpeed = 5000, long int acc = 1000  );
+  Atm_AccelStepper& setMaxSpeed( long int maxSpeed );
+  Atm_AccelStepper& setHomingSpeed( long int homingSpeed);
+  Atm_AccelStepper& setAcceleration( long int acc);
+  Atm_AccelStepper& setPosition( long int position);
+  long int getPosition();
+  long int distanceToGo();
+  bool isRunning();
+  float getSpeed();
+
+  Atm_AccelStepper& move(long int stepRel );
+  Atm_AccelStepper& moveTo(long int stepAbs );
   Atm_AccelStepper& rotate(long int speed );
-  Atm_AccelStepper& homing( bool direction, 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);
 
@@ -71,12 +76,18 @@ class Atm_AccelStepper: public Machine {
   Atm_AccelStepper& limitHigh_setThresholds (int threshold_low=510, int threshold_high = 1024);
   bool limitLow_State;
   bool limitHigh_State;
-
+  bool previous_state = limitLow_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
+  // ACTIONS
+  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, LP_LIMIT_LOW, ENT_LIMIT_HIGH, LP_LIMIT_HIGH };
+  // CONNECTORS
+  enum { ON_CHANGEPOSITION, ON_CHANGESTATE, ON_ONLIMITHIGH,
+          ON_ONLIMITLOW, ON_ONTARGET, ON_STOP, CONN_MAX };
   atm_connector connectors[CONN_MAX];
   int event( int id );
   void action( int id );
@@ -87,7 +98,7 @@ class Atm_AccelStepper: public Machine {
   long int _targetStep = 0;
   long int _maxStep ;
   atm_timer_millis position_timer ;
-  int POSITION_SEND_TIMER = 100 ;
+  int POSITION_SEND_TIMER = 50 ;
 
   // bool _rotationReversed = 0 ;
 
@@ -100,91 +111,91 @@ class Atm_AccelStepper: public Machine {
   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;
+  bool _limitLow_Hard = 0;
   int _limitHigh_Pin;
-  int _limitHigh_Mode; //0 no limit, 1 digital, 2 analog with thresholds
+  int _limitHigh_Mode=0; //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 ;
+  bool _limitHigh_Hard = 0 ;
   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
-*/
+/*
+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
+*/

HTequi-firmware/lib/Atm_lien/Atm_Teenstep.cpp

@@ -1,5 +1,5 @@
 #if defined(__MK20DX128__) || defined(__MK20DX256__) || defined(__MK64FX512__) || defined(__MK66FX1M0__)
-#ifdef ATM_TEENSTEP_H
+
 #include "Atm_Teenstep.h"
 
 /* Add optional parameters for the state machine to begin()
@@ -414,4 +414,3 @@ Atm_Teenstep& Atm_Teenstep::onLimitlow( atm_cb_push_t callback, int idx ) {
  }
 
 #endif
-#endif

HTequi-firmware/lib/Atm_lien/Atm_Teenstep.h

@@ -1,7 +1,7 @@
 #pragma once
 
 #if defined(__MK20DX128__) || defined(__MK20DX256__) || defined(__MK64FX512__) || defined(__MK66FX1M0__)
-#ifdef ATM_TEENSTEP_H
+
 #include <Automaton.h>
 #include <TeensyStep.h>
 
@@ -89,7 +89,6 @@ class Atm_Teenstep: public Machine {
 };
 
 #endif
-#endif
 
 /*
 Automaton::ATML::begin - Automaton Markup Language

HTequi-firmware/src/blobcnc_feeder/main.cpp

@@ -171,6 +171,7 @@ void setup() {
   //Start serial
   Serial.begin(115200); // higher Baud rate for faster refresh, CHANGE IN SERIAL MONITOR
   Serial1.begin(9600);
+  // Serial2.begin(9600);
   delay(2000);
   Serial.println("Started Feeder");
   cmd.begin( Serial, cmd_buffer, sizeof( cmd_buffer ) )

HTequi-firmware/src/blobcnc_low/main.cpp

@@ -22,8 +22,8 @@ byte mac[] = {
  0x00, 0xAA, 0xBB, 0xCC, 0xDE, 0x02
 };
 
-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 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;

HTequi-firmware/src/blobcnc_top/main.cpp

@@ -3,8 +3,11 @@
 
 //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>
@@ -21,15 +24,15 @@
 // 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, 105);    //local IP of Arduino/Teensy
-unsigned int localPort = 8000;
+// IPAddress ip(192, 168, 1, 105);    //local IP of Arduino/Teensy
+// unsigned int localPort = 8000;
 
-IPAddress outIp(192, 168, 1, 102);
-const unsigned int outPort = 9000;
+// IPAddress outIp(192, 168, 1, 102);
+// const unsigned int outPort = 9000;
 
 // 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
+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
@@ -46,16 +49,17 @@ const int HIGH_ACC = 600 ;
 const int LOW_SPEED = 100 ;
 const int LOW_ACC = 1000 ;
 
-Atm_Teenstep_OSC X_top_OSC;
-Atm_Teenstep X_top_step;
-Stepper X_top_stepper(20 , 19);
-StepControl X_top_controller ;
-
-Atm_Teenstep_OSC Y_top_OSC;
-Atm_Teenstep Y_top_step;
-Stepper Y_top_stepper(22 , 21);
-StepControl Y_top_controller ;
+// Atm_Teenstep_OSC X_top_OSC;
+// Atm_Teenstep X_top_step;
+// Stepper X_top_stepper(20 , 19);
+// StepControl X_top_controller ;
+Atm_AccelStepper X_top_step;
 
+// Atm_Teenstep_OSC Y_top_OSC;
+// Atm_Teenstep Y_top_step;
+// Stepper Y_top_stepper(22 , 21);
+// StepControl Y_top_controller ;
+Atm_AccelStepper Y_top_step;
 
 /////////////////// Servo and pump  ////////////////////////
 
@@ -66,7 +70,7 @@ bool peristaltic_direction = 0 ;
 Servo pill_trap ;
 
 void motorsOSC(OSCMessage &msg){
-  msg.dispatch("/pill_trap", [](OSCMessage &msg){pill_trap.write(msg.getFloat(0));});
+  msg.dispatch("/pill_trap", [](OSCMessage &msg){pill_trap.write(msg.getInt(0));});
   msg.dispatch("/peristaltic", [](OSCMessage &msg){analogWrite(peristaltic_pwm_pin, msg.getInt(0));});
 }
 
@@ -147,14 +151,15 @@ void feederOSC(OSCMessage &msg){
 
 //////////////  Setup  /////////////////////
 
+
 void setup() {
   //  ETHERNET (not needed for feeder)
   //SPI.setSCK(27); //only bottom
   Ethernet.init(15);//(10)
   teensyMAC(mac);
-  Ethernet.begin(mac, ip);              // start the Ethernet and UDP:
-  // Udp.beginMulticast(ipMulti, portMulti); // for modified Teensy Ethernet library
-  Udp.begin(localPort);
+  Ethernet.begin(mac);              // start the Ethernet and UDP:
+  Udp.beginMulticast(ipMulti, portMulti); // for modified Teensy Ethernet library
+  // Udp.begin(localPort);
 
 
   //  SERIAL
@@ -167,37 +172,60 @@ void setup() {
 
   //  STEPPERS
 
-  Y_top_step.trace( Serial );
-  Y_top_step.begin(Y_top_stepper, Y_top_controller)
-            .setEnablePin(23).enableReversed(1)
-            //limit pin is on 7 and 8
-             .setLimitType(2).setLimitPins(8, 7).limitReversed(true);
-  Y_top_stepper.setMaxSpeed(600);
-  Y_top_stepper.setAcceleration(1500);
-  Y_top_stepper.setInverseRotation(false);
-  Y_top_OSC.begin(Y_top_step, Udp, bndl, "/Y_top");
-  //OSC feedback
-  Y_top_step.onChangeposition([](int idx, int v, int up){bndl.add("/Y_top_OSC/step").add(v);});
-  Y_top_step.onChange([](int idx, int v, int up){bndl.add("/Y_top_OSC/state").add(v);});
-  Y_top_step.onLimitlow([](int idx, int v, int up){bndl.add("/Y_top_OSC/limitLow").add(v);});
-  Y_top_step.onLimithigh([](int idx, int v, int up){bndl.add("/Y_top_OSC/limitHigh").add(v);});
-  // Y_top_step.onChangeposition([] ( int idx, int v, int up ) {
-  //      bndl.
-  //    });
-
-
-  X_top_step.trace( Serial );
-  X_top_step.begin(X_top_stepper, X_top_controller)
-            .setEnablePin(18)//.enableReversed(1);//.enable(1);
-            .setLimitType(2).setLimitPins(16, 17).limitReversed(true);
-  X_top_stepper.setMaxSpeed(HIGH_SPEED);
-  X_top_stepper.setAcceleration(HIGH_ACC);
-  X_top_stepper.setInverseRotation(false);
-  X_top_OSC.begin(X_top_step, Udp, bndl, "/X_top");
-  X_top_step.onChangeposition([](int idx, int v, int up){bndl.add("/X_top_OSC/step").add(v);});
-  X_top_step.onChange([](int idx, int v, int up){bndl.add("/X_top_OSC/state").add(v);});
-  X_top_step.onLimitlow([](int idx, int v, int up){bndl.add("/X_top_OSC/limitLow").add(v);});
-  X_top_step.onLimithigh([](int idx, int v, int up){bndl.add("/X_top_OSC/limitHigh").add(v);});
+  X_top_step.begin(20, 19).setEnablePin(18).pinReversed(0)//.trace(Serial)
+             .enable()
+   .limitLow_set(1, 17, 1).limitLow_isHard(1)
+   .limitHigh_set(1, 16, 1).limitHigh_isHard(1)
+   .setMaxSpeed(HIGH_SPEED).setAcceleration(HIGH_ACC);
+  X_top_step.onChangeposition([](int idx, int v, int up){bndl.add("/X_top/step").add(v).add(up);});
+  X_top_step.onChangestate([](int idx, int v, int up){bndl.add("/X_top/state").add(v);});
+  X_top_step.onOnlimitlow([](int idx, int v, int up){bndl.add("/X_top/limitLow").add(v);});
+  X_top_step.onOnlimithigh([](int idx, int v, int up){bndl.add("/X_top/limitHigh").add(v);});
+  X_top_step.onOntarget([](int idx, int v, int up){bndl.add("/X_top/onTarget").add(v);});
+
+  Y_top_step.begin(22, 21)//.pinReversed(1)//.trace(Serial)
+             .enable()
+   .limitLow_set(1, 7, 1).limitLow_isHard(1)
+   .limitHigh_set(1, 8, 1).limitHigh_isHard(1)
+   .setMaxSpeed(HIGH_SPEED).setAcceleration(HIGH_ACC);
+  Y_top_step.onChangeposition([](int idx, int v, int up){bndl.add("/Y_top/step").add(v).add(up);});
+  Y_top_step.onChangestate([](int idx, int v, int up){bndl.add("/Y_top/state").add(v);});
+  Y_top_step.onOnlimitlow([](int idx, int v, int up){bndl.add("/Y_top/limitLow").add(v);});
+  Y_top_step.onOnlimithigh([](int idx, int v, int up){bndl.add("/Y_top/limitHigh").add(v);});
+  Y_top_step.onOntarget([](int idx, int v, int up){bndl.add("/Y_top/onTarget").add(v);});
+
+
+  // Y_top_step.trace( Serial );
+  // Y_top_step.begin(Y_top_stepper, Y_top_controller)
+  //           .setEnablePin(23).enableReversed(1)
+  //           //limit pin is on 7 and 8
+  //            .setLimitType(2).setLimitPins(8, 7).limitReversed(true);
+  // Y_top_stepper.setMaxSpeed(600);
+  // Y_top_stepper.setAcceleration(1500);
+  // Y_top_stepper.setInverseRotation(false);
+  // Y_top_OSC.begin(Y_top_step, Udp, bndl, "/Y_top");
+  // //OSC feedback
+  // Y_top_step.onChangeposition([](int idx, int v, int up){bndl.add("/Y_top_OSC/step").add(v);});
+  // Y_top_step.onChange([](int idx, int v, int up){bndl.add("/Y_top_OSC/state").add(v);});
+  // Y_top_step.onLimitlow([](int idx, int v, int up){bndl.add("/Y_top_OSC/limitLow").add(v);});
+  // Y_top_step.onLimithigh([](int idx, int v, int up){bndl.add("/Y_top_OSC/limitHigh").add(v);});
+  // // Y_top_step.onChangeposition([] ( int idx, int v, int up ) {
+  // //      bndl.
+  // //    });
+  //
+  //
+  // X_top_step.trace( Serial );
+  // X_top_step.begin(X_top_stepper, X_top_controller)
+  //           .setEnablePin(18)//.enableReversed(1);//.enable(1);
+  //           .setLimitType(2).setLimitPins(16, 17).limitReversed(true);
+  // X_top_stepper.setMaxSpeed(HIGH_SPEED);
+  // X_top_stepper.setAcceleration(HIGH_ACC);
+  // X_top_stepper.setInverseRotation(false);
+  // X_top_OSC.begin(X_top_step, Udp, bndl, "/X_top");
+  // X_top_step.onChangeposition([](int idx, int v, int up){bndl.add("/X_top_OSC/step").add(v);});
+  // X_top_step.onChange([](int idx, int v, int up){bndl.add("/X_top_OSC/state").add(v);});
+  // X_top_step.onLimitlow([](int idx, int v, int up){bndl.add("/X_top_OSC/limitLow").add(v);});
+  // X_top_step.onLimithigh([](int idx, int v, int up){bndl.add("/X_top_OSC/limitHigh").add(v);});
   // X_top_step.onOnchangeposition();
 
 
@@ -206,13 +234,13 @@ void setup() {
   // FEEDERS
   Serial1.begin(9600);
   feeder1.begin( Serial1, feeder1_buffer, sizeof( feeder1_buffer ) )
-    .trace(Serial)
+    //.trace(Serial)
     .list( feeder1list )
     .onCommand( feeder1_callback );
 
   Serial2.begin(9600);
   feeder2.begin( Serial2, feeder2_buffer, sizeof( feeder2_buffer ) )
-    .trace(Serial)
+    //.trace(Serial)
     .list( feeder2list )
     .onCommand( feeder2_callback );
 
@@ -224,14 +252,16 @@ void setup() {
   digitalWrite(peristaltic_dir_pin, peristaltic_direction);
   pinMode(peristaltic_pwm_pin, OUTPUT);
 
+
 }
 
 
 
 void loop() {
+
+  Udp.beginPacket(ipMulti, portMulti);
   automaton.run();
-  // Udp.beginPacket(ipMulti, portMulti);
-  Udp.beginPacket(outIp, outPort);
+  // 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
@@ -244,10 +274,22 @@ void loop() {
       msgIn.fill(Udp.read());
 
      if(!msgIn.hasError()){
-       Serial.println("got OSC");
+       // Serial.println("got OSC");
        // msgIn.route("/blink", test);
-       X_top_OSC.onOSC(msgIn);
-       Y_top_OSC.onOSC(msgIn);
+       msgIn.dispatch("/X_top/move", [](OSCMessage &msg){X_top_step.move(msg.getInt(0), msg.getInt(1), msg.getInt(2));});
+       msgIn.dispatch("/X_top/moveTo", [](OSCMessage &msg){X_top_step.moveTo(msg.getInt(0), msg.getInt(1), msg.getInt(2));});
+       msgIn.dispatch("/X_top/rotate", [](OSCMessage &msg){X_top_step.rotate(msg.getInt(0));});
+       msgIn.dispatch("/X_top/stop", [](OSCMessage &msg){X_top_step.stop();});
+       msgIn.dispatch("/X_top/emergency_stop", [](OSCMessage &msg){X_top_step.emergency_stop();});
+       msgIn.dispatch("/X_top/homing", [](OSCMessage &msg){X_top_step.homing(msg.getInt(0), msg.getInt(1));});
+
+       msgIn.dispatch("/Y_top/move", [](OSCMessage &msg){Y_top_step.move(msg.getInt(0), msg.getInt(1), msg.getInt(2));});
+       msgIn.dispatch("/Y_top/moveTo", [](OSCMessage &msg){Y_top_step.moveTo(msg.getInt(0), msg.getInt(1), msg.getInt(2));});
+       msgIn.dispatch("/Y_top/rotate", [](OSCMessage &msg){Y_top_step.rotate(msg.getInt(0));});
+       msgIn.dispatch("/Y_top/stop", [](OSCMessage &msg){Y_top_step.stop();});
+       msgIn.dispatch("/Y_top/emergency_stop", [](OSCMessage &msg){Y_top_step.emergency_stop();});
+       msgIn.dispatch("/Y_top/homing", [](OSCMessage &msg){Y_top_step.homing(msg.getInt(0), msg.getInt(1));});
+
        feederOSC(msgIn);
        motorsOSC(msgIn);
        // Y_top_step.onOSC(msgIn);