// © RiceMotion ( Robert Carl Rice ) 2012-2016 - All rights reserved


// This software makes use of tools and libraries obtained from open source projects or released for

// use by relevant hardware manufactures. However, this software is NOT a part of any open source project.

// It is released only on a "need to know" basis for beta testers of the "RiceCNC Interpolation Engine".

// Recipents of this source code must respect the confidential nature of this software and prevent any

// distribution that could result in counterfeit copies of the "RiceCNC Interpolation Engine".


// © RiceMotion ( Robert Carl Rice ) 2012-2016 - All rights reserved

#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(); }; };