#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, -1, -1, -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 );
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();
bool changed = 0 ;
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:
// previous_state = limitLow_State;
switch(_limitLow_Mode) {
case 0:
break;
case 1: //digital INPUT
// Serial.println("digital");
limitLow_State = digitalRead(_limitLow_Pin);
limitLow_State = _limitLow_Reversed ? !limitLow_State : limitLow_State;
break;
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;
if(limitLow_State){
delay(3);
analogTemp = analogRead(_limitLow_Pin);
limitLow_State = (_limitLow_Thresholds[0] < analogTemp) && (analogTemp < _limitLow_Thresholds[1]);
limitLow_State = _limitLow_Reversed ? !limitLow_State : limitLow_State;
}
break;
}
changed = limitLow_State != limitLow_State_prev ? 1 : 0 ;
limitLow_State_prev = limitLow_State ;
return changed ;
case EVT_ON_LIMIT_HIGH:
// previous_state = limitHigh_State;
switch(_limitHigh_Mode) {
case 0:
break;
case 1: //digital INPUT
limitHigh_State = digitalRead(_limitHigh_Pin);
limitHigh_State = _limitHigh_Reversed ? !limitHigh_State : limitHigh_State;
break;
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;
if(limitHigh_State){
delay(3);
analogTemp = analogRead(_limitHigh_Pin);
limitHigh_State = (_limitHigh_Thresholds[0] < analogTemp) && (analogTemp < _limitHigh_Thresholds[1]);
limitHigh_State = _limitHigh_Reversed ? !limitHigh_State : limitHigh_State;
}
break;
}
changed = limitHigh_State != limitHigh_State_prev ? 1 : 0;
limitHigh_State_prev = limitHigh_State;
return changed;
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 ;
stepper_update();
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 ;
//reset limit state so that they trigger again if we're stopped on it
limitLow_State = 0;
limitHigh_State = 0;
digitalWrite(_enablePin, enabled);
return;
case ENT_RUNNING:
push(connectors, ON_CHANGESTATE, 0, state(), 0);
push(connectors, ON_CHANGEPOSITION, 0, _currentStep, stepper->speed());
_isHoming = 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();
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());
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) {
stepper->setSpeed(0);
_currentStep = stepper->currentPosition();
_targetStep = _currentStep ;
stepper->moveTo(_targetStep);
push( connectors, ON_STOP, 0, 1, 0 );
}
return;
case LP_STOP:
stepper_update();
push(connectors, ON_CHANGEPOSITION, 0, _currentStep, stepper->speed());
if(stepper->speed() == 0.) {trigger(EVT_ON_TARGET);}
// _currentStep = stepper->currentPosition();
return;
case ENT_HOMING_LOW:
push(connectors, ON_CHANGESTATE, 0, state(), 0);
push(connectors, ON_CHANGEPOSITION, 0, _currentStep, stepper->speed());
runMode = 1;
_isHoming = 1 ;
stepper->setSpeed(-1*homing_speed);
return;
case LP_HOMING_LOW:
stepper_update();
push(connectors, ON_CHANGEPOSITION, 0, _currentStep, stepper->speed());
return;
case EXT_HOMING_LOW:
runMode = 0;
_isHoming = 0;
push(connectors, ON_CHANGEPOSITION, 0, _currentStep, stepper->speed());
if(last_trigger == EVT_ON_LIMIT_LOW) {
stepper->setCurrentPosition(0);
_currentStep = 0;
push(connectors, ON_CHANGEPOSITION, 0, _currentStep, stepper->speed());
Serial.println("homing low done");
}
else{Serial.println("homing low failed");}
trigger(EVT_EMERGENCY_STOP);
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);
return;
case LP_HOMING_HIGH:
stepper_update();
push(connectors, ON_CHANGEPOSITION, 0, _currentStep, stepper->speed());
return;
case EXT_HOMING_HIGH:
push(connectors, ON_CHANGEPOSITION, 0, _currentStep, stepper->speed());
runMode = 0;
_isHoming = 0;
trigger(EVT_EMERGENCY_STOP);
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:
/*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 );
push(connectors, ON_CHANGEPOSITION, 0, _currentStep, stepper->speed());
if (!limitLow_State){trigger(EVT_MOVE);}
return;
case LP_LIMIT_LOW:
//stop motor if going down, allow going up
// if(_limitLow_Hard && (_targetStep < _currentStep)) {
if(_limitLow_Hard && (stepper->speed()<0.)) {
// Serial.println("youpi");
_currentStep = stepper->currentPosition();
stepper->moveTo(_currentStep);
_targetStep = _currentStep;
stepper->setSpeed(0);
}
stepper_update();
//else{} // _isHoming ? trigger(EVT_STOP):
return;
case ENT_LIMIT_HIGH:
push( connectors, ON_ONLIMITHIGH, 0, limitHigh_State, 0 );
push(connectors, ON_CHANGEPOSITION, 0, _currentStep, stepper->speed());
if (!limitHigh_State){trigger(EVT_MOVE);};
return;
case LP_LIMIT_HIGH:
//stop motor if going down, allow going up
if(_limitHigh_Hard && ((stepper->speed()>0.))) {
// Serial.println("youpi");
_currentStep = stepper->currentPosition();
stepper->moveTo(_currentStep);
_targetStep = _currentStep;
stepper->setSpeed(0);
}
stepper_update();
// else{}
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;
}
// 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);
}
}
}
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(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) {
_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;
}