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