414 lines
9.0 KiB
Plaintext
414 lines
9.0 KiB
Plaintext
#include "Plunger.h"
|
|
#include "AccelStepper.h"
|
|
#include "MultiStepper.h"
|
|
|
|
#include "config.h"
|
|
#ifdef HAS_STATES
|
|
#include <ArduinoJson.h>
|
|
#endif
|
|
|
|
AccelStepper stepper1(AccelStepper::DRIVER, PLUNGER_MOTOR_1_DIR_PIN, PLUNGER_MOTOR_1_STEP_PIN);
|
|
AccelStepper stepper2(AccelStepper::DRIVER, PLUNGER_MOTOR_2_DIR_PIN, PLUNGER_MOTOR_2_STEP_PIN);
|
|
|
|
MultiStepper steppers;
|
|
|
|
// #define HAS_PLUNGER_DEBUG
|
|
|
|
#ifdef HAS_PLUNGER_DEBUG
|
|
#define PLUNGER_DEBUG(A) Serial.println(A);
|
|
#else
|
|
#define PLUNGER_DEBUG(A)
|
|
#endif
|
|
|
|
#ifdef HAS_STATES
|
|
String Plunger::state()
|
|
{
|
|
const int capacity = JSON_OBJECT_SIZE(3);
|
|
StaticJsonDocument<capacity> doc;
|
|
doc[0] = id;
|
|
doc[1] = _state;
|
|
doc[2] = pFlags;
|
|
|
|
#ifdef HAS_PLUNGER_DEBUG
|
|
Serial.println("state : ");
|
|
Serial.println(_state);
|
|
Serial.print("pflags low : ");
|
|
Serial.println(pFlags);
|
|
Serial.print("limit high : ");
|
|
Serial.println(u1.value);
|
|
|
|
Serial.print("moving: ");
|
|
Serial.println(TEST(pFlags, MOVING));
|
|
Serial.print("retracting: ");
|
|
Serial.println(TEST(pFlags, RETRACTING));
|
|
Serial.print("freeing: ");
|
|
Serial.println(TEST(pFlags, FREEING));
|
|
Serial.print("done: ");
|
|
Serial.println(TEST(pFlags, DONE));
|
|
Serial.print("retracted: ");
|
|
Serial.println(TEST(pFlags, RETRACTED));
|
|
Serial.print("retracting on : ");
|
|
Serial.println(retracting);
|
|
#endif
|
|
|
|
/*
|
|
Serial.println("pflags");
|
|
Serial.print("moving: ");
|
|
Serial.println(TEST(pFlags, MOVING));
|
|
Serial.print("retracting: ");
|
|
Serial.println(TEST(pFlags, RETRACTING));
|
|
Serial.print("freeing: ");
|
|
Serial.println(TEST(pFlags, FREEING));
|
|
Serial.print("done: ");
|
|
Serial.println(TEST(pFlags, DONE));
|
|
Serial.print("retracted: ");
|
|
Serial.println(TEST(pFlags, RETRACTED));
|
|
Serial.print("retracting on : ");
|
|
Serial.println(retracting);*/
|
|
|
|
return doc.as<String>();
|
|
}
|
|
#endif
|
|
bool Plunger::change(short newState)
|
|
{
|
|
if (newState == _state)
|
|
{
|
|
return false;
|
|
}
|
|
_state = newState;
|
|
return true;
|
|
}
|
|
short Plunger::setSpeed(short val = 0)
|
|
{
|
|
speed = 1000 * val;
|
|
}
|
|
short Plunger::setup()
|
|
{
|
|
stepper1.setMaxSpeed(PLUNGER_BASE_SPEED);
|
|
stepper1.setSpeed(PLUNGER_BASE_SPEED);
|
|
stepper1.setAcceleration(5000);
|
|
steppers.addStepper(stepper1);
|
|
steppers.moveTo(positions);
|
|
stepper1.setCurrentPosition(positions[0]);
|
|
stepper1.setPinsInverted(false);
|
|
|
|
u1.setup();
|
|
u1.loop();
|
|
pFlags = 0;
|
|
retracting = false;
|
|
}
|
|
|
|
short Plunger::plunge(short force)
|
|
{
|
|
if (!change(PLUNGING))
|
|
{
|
|
if (!TEST(pFlags, DONE))
|
|
{
|
|
return TEST(pFlags, DONE);
|
|
}
|
|
else
|
|
{
|
|
if (force)
|
|
{
|
|
reset();
|
|
_state = PLUNGING;
|
|
}
|
|
else
|
|
{
|
|
return TEST(pFlags, DONE);
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
reset();
|
|
_state = PLUNGING;
|
|
}
|
|
|
|
SBI(pFlags, INITIATED);
|
|
zero();
|
|
|
|
if (u1.value)
|
|
{
|
|
SBI(pFlags, FREEING);
|
|
//goToPos(-PLUNGER_RETRACE_DISTANCE, PLUNGER_RETRACT_SPEED);
|
|
return TEST(pFlags, DONE);
|
|
}
|
|
else
|
|
{
|
|
SBI(pFlags, MOVING);
|
|
//goToPos(PLUNGE_PLUNG_DISTANCE, speed);
|
|
}
|
|
return TEST(pFlags, DONE);
|
|
}
|
|
short Plunger::retract()
|
|
{
|
|
u1.loop();
|
|
if (u1.value)
|
|
{
|
|
if (!retracting)
|
|
{
|
|
retracting = true;
|
|
zero();
|
|
stepper1.setMaxSpeed(PLUNGER_RETRACT_SPEED);
|
|
positions[0] += (u1.value ? PLUNGER_RETRACE_DISTANCE : -PLUNGER_RETRACE_DISTANCE);
|
|
positions[1] += (u1.value ? PLUNGER_RETRACE_DISTANCE : -PLUNGER_RETRACE_DISTANCE);
|
|
steppers.moveTo(positions);
|
|
PLUNGER_DEBUG("PLUNGER : collision!");
|
|
}
|
|
SBI(pFlags, RETRACTING);
|
|
return 1;
|
|
}
|
|
else
|
|
{
|
|
if (!steppers.hasToGo())
|
|
{
|
|
retracting = false;
|
|
}
|
|
return 0;
|
|
}
|
|
}
|
|
short Plunger::goToPos(long pos, long speed)
|
|
{
|
|
zero();
|
|
stepper1.setMaxSpeed(speed);
|
|
stepper1.setSpeed(speed);
|
|
Serial.println(speed);
|
|
positions[0] += pos;
|
|
positions[1] += pos;
|
|
steppers.moveTo(positions);
|
|
}
|
|
short Plunger::home(short force = false)
|
|
{
|
|
if (!change(HOMING))
|
|
{
|
|
PLUNGER_DEBUG("PLUNGER already homing");
|
|
if (!TEST(pFlags, DONE))
|
|
{
|
|
PLUNGER_DEBUG("PLUNGER already homing : not done yet");
|
|
return TEST(pFlags, DONE);
|
|
}
|
|
else
|
|
{
|
|
if (force == false)
|
|
{
|
|
return TEST(pFlags, DONE);
|
|
}
|
|
else
|
|
{
|
|
PLUNGER_DEBUG("PLUNGER already homing : doing it again");
|
|
reset();
|
|
_state = HOMING;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
reset();
|
|
_state = HOMING;
|
|
}
|
|
|
|
SBI(pFlags, INITIATED);
|
|
zero();
|
|
|
|
if (u1.value)
|
|
{
|
|
PLUNGER_DEBUG("PLUNGER : frooze up");
|
|
SBI(pFlags, RETRACTING);
|
|
retract();
|
|
return TEST(pFlags, DONE);
|
|
}
|
|
else
|
|
{
|
|
PLUNGER_DEBUG("PLUNGER : move");
|
|
SBI(pFlags, MOVING);
|
|
goToPos(PLUNGE_HOME_DISTANCE, PLUNGER_HOMING_SPEED);
|
|
}
|
|
return TEST(pFlags, DONE);
|
|
}
|
|
|
|
short Plunger::moveMotors(short val = 0)
|
|
{
|
|
_state = MANUAL;
|
|
goToPos(val, PLUNGER_MOVE_SPEED);
|
|
}
|
|
|
|
short Plunger::moveMotor1(short val = 0)
|
|
{
|
|
reset();
|
|
zero();
|
|
stepper1.setMaxSpeed(PLUNGER_MOVE_SPEED);
|
|
stepper1.moveTo(val);
|
|
_state = MANUAL;
|
|
}
|
|
short Plunger::moveMotor2(short val = 0)
|
|
{
|
|
reset();
|
|
zero();
|
|
stepper2.setMaxSpeed(PLUNGER_MOVE_SPEED);
|
|
stepper2.moveTo(val);
|
|
_state = MANUAL;
|
|
}
|
|
|
|
short Plunger::stop(short val)
|
|
{
|
|
_state = STOPPED;
|
|
pFlags = 0;
|
|
}
|
|
|
|
void Plunger::zero()
|
|
{
|
|
positions[0] = 0;
|
|
positions[1] = 0;
|
|
stepper1.setCurrentPosition(positions[0]);
|
|
stepper2.moveTo(positions);
|
|
}
|
|
short Plunger::reset(short val)
|
|
{
|
|
positions[0] = 0;
|
|
positions[1] = 0;
|
|
pFlags = 0;
|
|
_state = NONE;
|
|
retracting = false;
|
|
stepper1.setCurrentPosition(positions[0]);
|
|
}
|
|
short Plunger::loop()
|
|
{
|
|
AddonFnPtr ptr = this->check;
|
|
short ret = 1; (owner->*ptr)(0);
|
|
// u1.loop();
|
|
if (!ret)
|
|
{
|
|
SBI(pFlags, PAUSED);
|
|
return;
|
|
}
|
|
else
|
|
{
|
|
CBI(pFlags, PAUSED);
|
|
}
|
|
|
|
while (steppers.run() && !TEST(pFlags, PAUSED) && !TEST(pFlags, DONE))
|
|
{
|
|
// retract();
|
|
|
|
ret = (owner->*ptr)(0);
|
|
|
|
if (!ret)
|
|
{
|
|
SBI(pFlags, PAUSED);
|
|
break;
|
|
}
|
|
|
|
switch (_state)
|
|
{
|
|
case ERROR:
|
|
case MANUAL:
|
|
{
|
|
break;
|
|
}
|
|
case STOPPED:
|
|
{
|
|
break;
|
|
}
|
|
case HOMING:
|
|
case PLUNGING:
|
|
{
|
|
if (retract())
|
|
{
|
|
Serial.print('shit');
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
ret = (owner->*ptr)(0);
|
|
if (!ret)
|
|
{
|
|
SBI(pFlags, PAUSED);
|
|
return;
|
|
}
|
|
else
|
|
{
|
|
SBI(pFlags, PAUSED);
|
|
}
|
|
|
|
if (!steppers.hasToGo())
|
|
{
|
|
if (retracting)
|
|
{
|
|
retracting = false;
|
|
}
|
|
|
|
if (TEST(pFlags, RETRACTING))
|
|
{
|
|
SBI(pFlags, RETRACTED);
|
|
CBI(pFlags, RETRACTING);
|
|
}
|
|
|
|
switch (_state)
|
|
{
|
|
case ERROR:
|
|
case STOPPED:
|
|
{
|
|
_state = NONE;
|
|
pFlags = 0;
|
|
return;
|
|
}
|
|
break;
|
|
case MANUAL:
|
|
{
|
|
_state = NONE;
|
|
pFlags = 0;
|
|
break;
|
|
}
|
|
case PLUNGING:
|
|
case HOMING:
|
|
{
|
|
if (TEST(pFlags, DONE))
|
|
{
|
|
// PLUNGER_DEBUG("PLUNGER ::: SET DONE");
|
|
break;
|
|
}
|
|
|
|
if (TEST(pFlags, FREEING) && TEST(pFlags, RETRACTED))
|
|
{
|
|
PLUNGER_DEBUG("PLUNGER : clear retracted after freeing");
|
|
CBI(pFlags, FREEING);
|
|
CBI(pFlags, RETRACTED);
|
|
CBI(pFlags, RETRACTING);
|
|
}
|
|
|
|
if (!TEST(pFlags, MOVING) && !TEST(pFlags, RETRACTED) && !TEST(pFlags, RETRACTING))
|
|
{
|
|
PLUNGER_DEBUG("PLUNGER : goto");
|
|
SBI(pFlags, MOVING);
|
|
goToPos(_state == HOMING ? PLUNGE_HOME_DISTANCE : PLUNGE_PLUNG_DISTANCE, _state == HOMING ? PLUNGER_HOMING_SPEED : PLUNGER_PLUNGE_SPEED);
|
|
break;
|
|
}
|
|
|
|
if (!TEST(pFlags, FREEING) && TEST(pFlags, RETRACTED))
|
|
{
|
|
SBI(pFlags, DONE);
|
|
PLUNGER_DEBUG("PLUNGER : retracted, set done");
|
|
break;
|
|
}
|
|
|
|
if (TEST(pFlags, MOVING) && TEST(pFlags, RETRACTED))
|
|
{
|
|
PLUNGER_DEBUG("PLUNGER : moved & retracted");
|
|
SBI(pFlags, DONE);
|
|
return;
|
|
}
|
|
|
|
if (!TEST(pFlags, MOVING) && TEST(pFlags, RETRACTED))
|
|
{
|
|
PLUNGER_DEBUG("PLUNGER : not moved but retracted");
|
|
SBI(pFlags, DONE);
|
|
retracting = false;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
} |