#include <ClosedLoopCounter.h>
#include <StepCounter.h>
#include <QuadratureCounter4X.h>
#include <StepDirectionCounter.h>
#include <InputPin.h>
#include <Timers.h>
#include <Machine.h>
#include <AxisTimer.h>
#include <Axis.h>
#include <inc/hw_gpio.h>
#include "driverlib/interrupt.h"
ClosedLoopCounter::ClosedLoopCounter(
StepCounter* aStepCounter,
float aAdjustmentFactor ) {
stepCounter = aStepCounter;
adjustmentFactor = aAdjustmentFactor;
stepped = 0.0; };
ClosedLoopCounter::~ClosedLoopCounter() {
#ifdef DebugDelete
Serial.println( " delete ClosedLoopCounter" );
#endif
if ( stepCounter )
delete stepCounter;
};
void
ClosedLoopCounter::interpolationIsr(
Machine* machine,
Axis* axis ) {
// Need to wait for more than one count to prevent hunting
// and allow axis to go idle
if ( abs( stepCounter->stepCount ) > 1 ) {
float scaledCount =
stepCounter->readScaledCount();
if ( scaledCount ) {
stepped -= scaledCount;
float maxCorrection =
fabs( scaledCount )
* adjustmentFactor;
float correction = stepped;
if ( correction ) {
if ( correction > maxCorrection )
correction = maxCorrection;
else if ( correction < -maxCorrection ) {
correction = -maxCorrection; };
AxisTimer::disableAxisInterrupts();
axis->moveMachinePositionAndTarget( -correction );
AxisTimer::enableAxisInterrupts(); }; }; };
if ( fabs( stepped / stepCounter->scale ) > 10.0 ) {
machine->stallDetected();
AxisTimer::disableAxisInterrupts();
axis->moveMachinePositionAndTarget( -stepped );
AxisTimer::enableAxisInterrupts(); }; };