// © 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 <HallFeedback.h>

#include <InputPin.h>

#include <SysTick.h>

#include <BLDC3.h>

#include <PwmOutputPin.h>

#include <DRV8305Axis.h>

#include <Axis.h>

#include <MachineCommands.h>


#include <inc/hw_gpio.h>

#include <driverlib/interrupt.h>

#include <driverlib/timer.h>


//#define DebugDelete 1


HallFeedback::HallFeedback(

     char*               data,

     String*               msgPtr,

     DRV8305Axis*     aAxis ) {


     axis          = aAxis;

     bldc3          = axis->motor;


     hallA          = NULL;

     hallB          = NULL;

     hallC          = NULL;


     char          hallAPinString[8];

     char          hallBPinString[8];

     char          hallCPinString[8];


// Voltage PID

     angleProportion               = 0.0;

     voltageIntegral               = 0.0;

     angleDerivative               = 0.0;


// Angle PID

     angleIntegral          = 0.0;

//     angleIntegral          = 0.0;

//     angleDerivative          = 0.0;


     speedProportion          = 0.0;


     error                    = 0;

     priorError               = 0;

     integralError          = 0;

     expirationCounter     = 0;

     stallDetectVoltage     = 1024;     // 25%


     feedbackAngle          = 0;

     feedbackTorque          = 0;


     synchronized          = false;


     sscanf( data,

          " %s %s %s %f %f %f %f %lu %f",

          hallAPinString,

          hallBPinString,

          hallCPinString,


// Voltage PID

          &angleProportion,

          &voltageIntegral,

          &angleDerivative,


// Angle PID

          &angleIntegral,

//          &angleIntegral,

//          &angleDerivative,


          &stallDetectVoltage,

          &speedProportion );


     *msgPtr          += "\n Angle Proportion ";

     *msgPtr          += String( angleProportion );


     *msgPtr          += "\n Angle Integral ";

     *msgPtr          += String( angleIntegral );


     *msgPtr          += "\n Angle Derivative ";

     *msgPtr          += String( angleDerivative );


     *msgPtr          += "\n Voltage Integral ";

     *msgPtr          += String( voltageIntegral );


     *msgPtr          += "\n Stall Detect Voltage ";

     *msgPtr          += String( stallDetectVoltage );


     *msgPtr          += "\n Speed Proportion ";

     *msgPtr          += String( speedProportion );


     if (     angleProportion     == 0.0

          &&     voltageIntegral     == 0.0 ) {

          

          *msgPtr     += " PI Invalid";

          return; };


     *msgPtr          += "\n HallA";

     hallA     = new InputPin(

          hallAPinString,

          msgPtr );

     if ( hallA->valid() )

          hallA->fmtPin( msgPtr );

     else {

          *msgPtr          += " invalid";

          return; };


     *msgPtr          += "\n HallB";

     hallB     = new InputPin(

          hallBPinString,

          msgPtr );

     if ( hallB->valid() )

          hallB->fmtPin( msgPtr );

     else {

          *msgPtr          += " invalid";

          return; };


     *msgPtr          += "\n HallC";

     hallC     = new InputPin(

          hallCPinString,

          msgPtr );

     if ( hallC->valid() )

          hallC->fmtPin( msgPtr );

     else {

          delete hallC;

          hallC = NULL;

          *msgPtr          += " invalid";

          return; };


     // initialize pin interrupt callback

     hallA->stepCounter     = this;

     hallB->stepCounter     = this;

     hallC->stepCounter     = this;


     hallState     = HallStart;


     hallA->enableBothEdgeInterrupt();

     hallB->enableBothEdgeInterrupt();

     hallC->enableBothEdgeInterrupt();


     initPhaseTransitionTable(); };

     

void

     HallFeedback::initPhaseTransitionTable() {


     synchronized     = false;

     axis->enable8305( true );


     for ( int i = 0;

          i < 12;

          i++ ) {


          phaseTransition[ i ] = 0xFFFF; }; };


bool

     HallFeedback::valid() {

     

     return

          hallA

     &&     hallB

     &&     hallC; };


HallFeedback::~HallFeedback() {


#ifdef DebugDelete

     Serial.println( " delete HallFeedback" );

#endif


     if ( hallA )

          delete hallA;


     if ( hallB )

          delete hallB;


     if ( hallC )

          delete hallC; };


void

     HallFeedback::reportHallPinChange(

          String* msgPtr ) {

     

     if ( hallA )

          hallA->reportPinChange( msgPtr );


     if ( hallB )

          hallB->reportPinChange( msgPtr );


     if ( hallC )

          hallC->reportPinChange( msgPtr ); };


void

     HallFeedback::readHallPhasing(

          char*          data,

          String*          msgPtr ) {


     for ( int i = 0;

          i < 12;

          i++ ) {


          snprintf( data, 10,

               " %x",

               phaseTransition[ i ] );

          *msgPtr          += String( data ); };


     *msgPtr          += "\n"; };


void

     HallFeedback::pinISR(

          InputPin* pin ) {


     switch ( hallState ) {


          case     HallStart :

          

          if          ( pin == hallA )

               hallState = pin->readPin() ?

                    S_A1

               :     S_A0;


          else if ( pin == hallB )

               hallState = pin->readPin() ?

                    S_B1

               :     S_B0;


          else if ( pin == hallC )

               hallState = pin->readPin() ?

                    S_C1

               :     S_C0;

          break;


          case     S_A1 :

          

          if          ( pin == hallA ) {

               hallState = S_A0;

               break; }


          case     CW_A1 :


          if          ( pin == hallA ) {

               if     ( ! pin->readPin() )

                    hallState = CCW_A0; }


          else if ( pin == hallB )

               hallState = pin->readPin() ?

                    CW_B1

               :     CCW_B0;


          else

               hallState = pin->readPin() ?

                    CCW_C1

               :     CW_C0;

          break;


          case     S_B1 :

          

          if          ( pin == hallB ) {

               hallState = S_B0;

               break; }


          case     CW_B1 :


          if          ( pin == hallB ) {

               if     ( ! pin->readPin() )

                    hallState = CCW_B0; }


          else if ( pin == hallC )

               hallState = pin->readPin() ?

                    CW_C1

               :     CCW_C0;


          else

               hallState = pin->readPin() ?

                    CCW_A1

               :     CW_A0;

          break;


          case     S_C1 :

          

          if          ( pin == hallC ) {

               hallState = S_C0;

               break; }


          case     CW_C1 :


          if          ( pin == hallC ) {

               if     ( ! pin->readPin() )

                    hallState = CCW_C0; }


          else if ( pin == hallA )

               hallState = pin->readPin() ?

                    CW_A1

               :     CCW_A0;


          else

               hallState = pin->readPin() ?

                    CCW_B1

               :     CW_B0;

          break;

     

          case     S_A0 :

          

          if          ( pin == hallA ) {

               hallState = S_A0;

               break; }


          case     CW_A0 :


          if          ( pin == hallA ) {

               if     ( pin->readPin() )

                    hallState = CCW_A1; }


          else if ( pin == hallB )

               hallState = pin->readPin() ?

                    CW_B1

               :     CCW_B0;


          else

               hallState = pin->readPin() ?

                    CW_C1

               :     CCW_C0;

          break;


          case     S_B0 :

          

          if          ( pin == hallB ) {

               hallState = S_B0;

               break; }


          case     CW_B0 :


          if          ( pin == hallB ) {

               if     ( pin->readPin() )

                    hallState = CCW_B1; }


          else if ( pin == hallC )

               hallState = pin->readPin() ?

                    CW_C1

               :     CCW_C0;


          else

               hallState = pin->readPin() ?

                    CW_A1

               :     CCW_A0;

          break;


          case     S_C0 :

          

          if          ( pin == hallC ) {

               hallState = S_C0;

               break; }


          case     CW_C0 :


          if          ( pin == hallC ) {

               if     ( pin->readPin() )

                    hallState = CCW_C1; }


          else if ( pin == hallA )

               hallState = pin->readPin() ?

                    CW_A1

               :     CCW_A0;


          else

               hallState = pin->readPin() ?

                    CW_B1

               :     CCW_B0;

          break;


          case     CCW_A1 :


          if          ( pin == hallA ) {

               if     ( ! pin->readPin() )

                    hallState = CW_A0; }


          else if ( pin == hallB )

               hallState = pin->readPin() ?

                    CW_B1

               :     CCW_B0;

          

          else

               hallState = pin->readPin() ?

                    CCW_C1

               :     CW_C0;

          break;


          case     CCW_B1 :


          if          ( pin == hallB ) {

               if     ( ! pin->readPin() )

                    hallState = CW_B0; }


          else if ( pin == hallC )

               hallState = pin->readPin() ?

                    CW_C1

               :     CCW_C0;


          else

               hallState = pin->readPin() ?

                    CCW_A1

               :     CW_A0;

          break;


          case     CCW_C1 :


          if          ( pin == hallC ) {

               if     ( ! pin->readPin() )

                    hallState = CW_C0; }


          else if ( pin == hallA )

               hallState = pin->readPin() ?

                    CW_A1

               :     CCW_A0;


          else

               hallState = pin->readPin() ?

                    CCW_B1

               :     CW_B0;

          break;

     

          case     CCW_A0 :


          if          ( pin == hallA ) {

               if     ( pin->readPin() )

                    hallState = CW_A1; }


          else if ( pin == hallB )

               hallState = pin->readPin() ?

                    CCW_B1

               :     CW_B0;


          else

               hallState = pin->readPin() ?

                    CCW_C1

               :     CW_C0;

          break;


          case     CCW_B0 :


          if          ( pin == hallB ) {

               if     ( pin->readPin() )

                    hallState = CW_B1; }


          else if ( pin == hallC )

               hallState = pin->readPin() ?

                    CCW_C1

               :     CW_C0;


          else

               hallState = pin->readPin() ?

                    CCW_A1

               :     CW_A0;

          break;


          case     CCW_C0 :


          if          ( pin == hallC ) {

               if     ( pin->readPin() )

                    hallState = CW_C1; }


          else if ( pin == hallA )

               hallState = pin->readPin() ?

                    CCW_A1

               :     CW_A0;


          else

               hallState = pin->readPin() ?

                    CCW_B1

               :     CW_B0;

          break; };


     if ( hallState >= HallStart )

          return;


     if ( synchronized )

          calculateFeedback();


     else if ( phaseTransition[ hallState ] == 0xFFFF ) { // initial transition

          phaseTransition[ hallState ] =

               bldc3->armMagneticAngle;

          if ( initCount() == 12 )

               synchronized     = true; }; };


void

     HallFeedback::calculateFeedback() {

     

     // New sample - reset counter

     expirationCounter          = 600;     // decremented @ 2 kHz rate


     uint32_t currentPhase     = bldc3->armMagneticAngle;

     uint32_t priorPhase          = phaseTransition[ hallState ];


     int32_t change     = currentPhase - priorPhase;

     if ( change == 0 )

          return;


     if          ( change >= BLDC3::MS50_180 )

          change     -= BLDC3::MS50_360;


     else if ( change < -BLDC3::MS50_180 )

          change     += BLDC3::MS50_360;


// error is positive if motor needs to go faster CW or slower CCW

     error               += change;

     phaseTransition[ hallState ] = currentPhase;

     

     integralError     += error;


     feedbackTorque     = abs (

               integralError     *     voltageIntegral );


     feedbackAngle =

               change               *     angleDerivative

          +     error               *     angleProportion

          +     integralError     *     angleIntegral;


     if ( feedbackAngle > BLDC3::MS50_90 )

          feedbackAngle     = BLDC3::MS50_90;

     

     else if ( feedbackAngle < -BLDC3::MS50_90 )

          feedbackAngle     = -BLDC3::MS50_90;

     

     feedbackAngle     -= error;


Serial.print( " e" );

Serial.print( error );

Serial.print( " E" );

Serial.print( integralError );

Serial.print( " <" );

Serial.print( feedbackAngle );

Serial.print( " t" );

Serial.println( feedbackTorque );

     };


bool

     HallFeedback::calculateHallFeedback(

          Machine*     machine ) {


     if ( expirationCounter == 0 )

          return

               false;

     expirationCounter--;

     

     bldc3->assertedVoltage          =

               feedbackTorque

          +     bldc3->holdTorque;


     bldc3->assertedLeadAngle     += feedbackAngle;


     int32_t overVoltage =

               bldc3->assertedVoltage

          -     stallDetectVoltage;


     if ( overVoltage > 0 ) {

          bldc3->assertedVoltage     -= overVoltage;

          axis->drv8305AxisState     = AXIS_VOLTAGE_LIMITED;

          axis->setFlag( VELOCITY_CHANGED );


          // at max torque either can't keep up with load

          // or motor is being driven overspeed by inertial load

          if ( feedbackTorque >= 0.0 ) {

               axis->mvStepsPerSecF     -=

                    overVoltage

               *     speedProportion;


               if ( axis->mvStepsPerSecF > 0 )

                    return

                         true; }


          else {

               axis->mvStepsPerSecF     +=

                    overVoltage

               *     speedProportion;


               if ( axis->mvStepsPerSecF < 0 )

                    return

                         true; };


          // Shutdown

          axis->drv8305AxisState               = AXIS_STALLED;

          machine->stallDetected();

          axis->setTargetVelocity( 0.0 );

          synchronized          = false;

          expirationCounter     = 0;

          axis->enable8305( false );

          return

               false; };

     

     return

          true; };

     

uint

     HallFeedback::initCount() {

     

     uint count     = 0;

     for ( int i = 0;

               i < 12;

               i++ ) {

          if ( phaseTransition[ i ] != 0xFFFF )

               count++; };


     if ( count == 12 ) {

          axis->drv8305AxisState     = AXIS_RUNNING; };


     return

          count; };