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

#include <OutputPin.h>

#include <ADC.h>

#include <Machine.h>

#include <AnalogPin.h>

#include <InputPin.h>



TorchHeightControl::TorchHeightControl(

     char*          data,

     String*          msgPtr,

     Machine*     machine )


     :     Axis(

               data,

               msgPtr ) {

     

     char        plasmaStartString[8];

     char        arcTransferString[8];

     char        arcVoltageString[8];


     char          xAxisName                    = 'X';

     char          yAxisName                    = 'Y';

     char          zAxisName                    = 'Z';

     char          torchRotationAxisName     = 'N';

     char          torchTiltAxisName          = 'N';


     proportionalComponent          = 0.0;

     integralComponent               = 0.0;


     xyArcLength                         = 0.0;

     float headRotationAngle          = 90.0;


     zArcOffset                         = 0.0;


     sscanf( data+3,

          "%s %s %s %c %c %c %c %c %f %f %f %f %f",

          plasmaStartString,

          arcTransferString,

          arcVoltageString,

          &xAxisName,

          &yAxisName,

          &zAxisName,

          &torchRotationAxisName,

          &torchTiltAxisName,

          &proportionalComponent,

          &integralComponent,

          &xyArcLength,

          &headRotationAngle,

          &zArcOffset );

     

     if ( zArcOffset == 0.0 ) {

          *msgPtr          += " No Z offset for arc vector";

          return; };


     // validate

     xAxis               = machine->findAxis( xAxisName, msgPtr );

     yAxis               = machine->findAxis( yAxisName, msgPtr );

     zAxis               = machine->findAxis( zAxisName, msgPtr );


     if ( ! xAxis || ! yAxis || ! zAxis )

          return;

     

     if ( torchRotationAxisName == 'N' )

          torchRotationAxis     = NULL;

     else

          torchRotationAxis     = machine->findAxis(

               torchRotationAxisName,

               msgPtr );


     plasmaStart     = new OutputPin( plasmaStartString, msgPtr );

     arcTransfer     = new InputPin( arcTransferString, msgPtr );

     arcVoltage          = new AnalogPin( arcVoltageString, msgPtr );

     if (     ! plasmaStart->valid()

          ||     ! arcTransfer->valid()

          ||     ! arcVoltage->valid() )     {


          delete plasmaStart;

          plasmaStart          = NULL;

     

          delete arcTransfer;

          arcTransfer          = NULL;

     

          delete arcVoltage;

          arcVoltage          = NULL;

          

          return; };

     

     plasmaStart->setPin( false);


     *msgPtr          += " Start";

     plasmaStart->fmtPin( msgPtr );


     *msgPtr          += " Transfer";

     arcTransfer->fmtPin( msgPtr );


     *msgPtr          += " Voltage";

     arcVoltage->fmtPin( msgPtr );

     

     ADC::addAnalogPin( arcVoltage );


     headRotationAngleRad     =

          headRotationAngle

          * M_PI

          / 180.0;

     headRotationSin               = sin( headRotationAngleRad );

     headRotationCos               = cos( headRotationAngleRad );

     xArcOffset                    = xyArcLength * headRotationSin;

     yArcOffset                    = xyArcLength * headRotationCos;

     

     integral                    = 0.0;

     setVoltage                    = 0.0;

     cuttingAngleRad               = 0.0; };



TorchHeightControl::~TorchHeightControl() {

     // Called automatically after derived class destructor

     if ( plasmaStart )

          delete plasmaStart;


     if ( arcTransfer )

          delete arcTransfer;


     if ( arcVoltage )

          delete arcVoltage; };


void

     TorchHeightControl::setArcVector(

          float          axyArcLength,

          float          aHeadRotationAngleRad,

          float          azArcOffset ) {


     xyArcLength                    = axyArcLength;

     headRotationAngleRad     = aHeadRotationAngleRad;

     headRotationSin               = sin( headRotationAngleRad );

     headRotationCos               = cos( headRotationAngleRad );

// Move to new position

//     xArcOffset                    = xyArcLength * headRotationSin;

//     yArcOffset                    = xyArcLength * headRotationCos;

     };


bool

     TorchHeightControl::isTorchAxis() {


     return true; };


void

     TorchHeightControl::startPlasma( bool on ) {


     if ( plasmaStart )

          plasmaStart->setPin( on ); };


bool

     TorchHeightControl::arcEstablished() {

     

     return arcTransfer ?

          arcTransfer->readPin() :

          false; };


void

     TorchHeightControl::interpolationIsr(

          Machine*     machine ) {


     bool     arcLengthChaged          = false;

     bool     headAngleChanged     = false;

     

     if ( arcEstablished() ) {

          float pV               = arcVoltage->analogValue();

          float error               = setVoltage - pV;

          integral               += error;

          

          float arcLengthScale     =

               error                    * proportionalComponent +

               integral               * integralComponent;


          if ( arcLengthScale != 1.0 ) {

               xyArcLength               *= arcLengthScale;

               arcLengthChaged     = true;


               float newZArcOffset          = zArcOffset * arcLengthScale;

               zAxis->interpolationMicrostepBuffer     +=

                    newZArcOffset - zArcOffset;

               zArcOffset               = newZArcOffset; }; };


     float     deltaX               = xAxis->interpolationMicrostepBuffer;

     float     deltaY               = yAxis->interpolationMicrostepBuffer;


     if ( deltaX || deltaY ) {

          float newCuttingAngleRad     = atan2( deltaY, deltaX );

          float deltaAngleRad               = newCuttingAngleRad - cuttingAngleRad;

          if ( deltaAngleRad ) {

               if ( torchRotationAxis ) // rotate the head

                    torchRotationAxis->interpolationMicrostepBuffer +=

                         deltaAngleRad

                         * torchRotationAxis->microStepsPerUnitDistanceF;

               cuttingAngleRad               =  newCuttingAngleRad;


               headRotationAngleRad     += deltaAngleRad;

               headAngleChanged          = true;

               headRotationSin               = sin( headRotationAngleRad );

               headRotationCos               = cos( headRotationAngleRad ); }; };


     if ( arcLengthChaged || headAngleChanged ) {

          float newxArcOffset          = xyArcLength * headRotationSin;

          xAxis->interpolationMicrostepBuffer     +=

               newxArcOffset - xArcOffset;

          xArcOffset                    = newxArcOffset;


          float newyArcOffset          = xyArcLength * headRotationCos;

          yAxis->interpolationMicrostepBuffer     +=

               newyArcOffset - yArcOffset;

          yArcOffset                    = newyArcOffset; }; };