|
@@ -0,0 +1,519 @@
|
|
|
|
+#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 idle_timer.expired(this);
|
|
|
|
+ case EVT_MOVE_TIMEOUT:
|
|
|
|
+ return 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;
|
|
|
|
+}
|