<commit_before>/*=========================================================================
*
* Copyright Marius Staring, Stefan Klein, David Doria. 2011.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0.txt
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*=========================================================================*/
/** \file
 \brief This program converts and possibly casts images.

 \verbinclude castconvert.help
 */
/*
 * authors:       Marius Staring and Stefan Klein
 *
 * Thanks to Hans J. Johnson for a modification to this program. This
 * modification breaks down the program into smaller compilation units,
 * so that the compiler does not overflow.
 *
 * ENH: on 23-05-2006 we added multi-component support.
 * ENH: on 09-06-2006 we added support for extracting a specific DICOM serie.
 */

#include <iostream>
#include "castconverthelpers2.h"

#include "CommandLineArgumentHelper.h"
#include "itkCommandLineArgumentParser.h"

#include "itkImageFileReader.h"
#include "itkGDCMImageIO.h"

// Some non-standard IO Factories
#include "itkGE4ImageIOFactory.h"
#include "itkGE5ImageIOFactory.h"
#include "itkGEAdwImageIOFactory.h"
//#include "itkBrains2MaskImageIOFactory.h"
#include "itkPhilipsRECImageIOFactory.h"

/* Functions to do the actual conversion. */
extern int FileConverterScalar(
  const std::string & inputPixelComponentType,
  const std::string & outputPixelComponentType,
  const std::string & inputFileName,
  const std::string & outputFileName,
  const unsigned int inputDimension, bool useCompression );
extern int DicomFileConverterScalarA(
  const std::string & inputPixelComponentType,
  const std::string & outputPixelComponentType,
  const std::string & inputDirectoryName,
  const std::string & seriesUID,
  const std::vector<std::string> & restrictions,
  const std::string & outputFileName,
  const unsigned int inputDimension, bool useCompression );
extern int DicomFileConverterScalarB(
  const std::string & inputPixelComponentType,
  const std::string & outputPixelComponentType,
  const std::string & inputDirectoryName,
  const std::string & seriesUID,
  const std::vector<std::string> & restrictions,
  const std::string & outputFileName,
  const unsigned int inputDimension, bool useCompression );
extern int FileConverterMultiComponent(
  const std::string & inputPixelComponentType,
  const std::string & outputPixelComponentType,
  const unsigned int numberOfComponents,
  const std::string & inputFileName,
  const std::string & outputFileName,
  const unsigned int inputDimension, bool useCompression );

//-------------------------------------------------------------------------------------

int main( int argc, char **argv )
{
  /** Register some non-standard IO Factories to make the tool more useful.
   * Copied from the Insight Applications.
   */
  //itk::Brains2MaskImageIOFactory::RegisterOneFactory();
  itk::GE4ImageIOFactory::RegisterOneFactory();
  itk::GE5ImageIOFactory::RegisterOneFactory();
  itk::GEAdwImageIOFactory::RegisterOneFactory();
  itk::PhilipsRECImageIOFactory::RegisterOneFactory();

  itk::CommandLineArgumentParser::Pointer parser = itk::CommandLineArgumentParser::New();
  parser->SetCommandLineArguments( argc, argv );
  parser->SetProgramHelpText( GetHelpString() );

  parser->MarkArgumentAsRequired( "-in", "The input filename." );
  parser->MarkArgumentAsRequired( "-out", "The output filename." );

  /** Get the command line arguments. */
  std::string input = "";
  std::string outputFileName = "";
  std::string outputPixelComponentType = "";
  std::string seriesUID = "";
  std::vector<std::string> restrictions;
  std::string errorMessage = "";
  bool useCompression = false;
  bool argsRetrievedSuccessfully = GetCommandLineArguments( parser,
    input, outputFileName, outputPixelComponentType,
    seriesUID, restrictions, useCompression );

  itk::CommandLineArgumentParser::ReturnValue validateArguments = parser->CheckForRequiredArguments();

  if(validateArguments == itk::CommandLineArgumentParser::FAILED || !argsRetrievedSuccessfully)
  {
    return EXIT_FAILURE;
  }
  else if(validateArguments == itk::CommandLineArgumentParser::HELPREQUESTED)
  {
    return EXIT_SUCCESS;
  }

  /** Are we dealing with an image or a dicom series? */
  bool isDICOM = false;
  int returnValue2 = IsDICOM( input, errorMessage, isDICOM );
  if ( returnValue2 )
  {
    std::cout << errorMessage << std::endl;
    return returnValue2;
  }

  /** Get inputFileName or inputDirectoryName. */
  std::string inputFileName, inputDirectoryName;
  if ( !isDICOM ) inputFileName = input;
  else inputDirectoryName = input;

  /** TASK 2:
   * Typedefs and test reading to determine correct image types.
   * *******************************************************************
   */

  /** Initial image type. */
  const unsigned int Dimension = 3;
  typedef short      PixelType;

  /** Some typedef's. */
  typedef itk::Image< PixelType, Dimension >   ImageType;
  typedef itk::ImageFileReader< ImageType >    ReaderType;
  typedef itk::ImageIOBase                     ImageIOBaseType;
  typedef itk::GDCMImageIO                     GDCMImageIOType;

  /** Create a testReader. */
  ReaderType::Pointer testReader = ReaderType::New();

  /** Setup the testReader. */
  if ( !isDICOM )
  {
    /** Set the inputFileName in the testReader. */
    testReader->SetFileName( inputFileName.c_str() );
  }
  else
  {
    std::string fileName = "";
    int returnValue3 = GetFileNameFromDICOMDirectory(
      inputDirectoryName, fileName, seriesUID, restrictions, errorMessage );
    if ( returnValue3 )
    {
      std::cout << errorMessage << std::endl;
      return returnValue3;
    }

    /** Create a dicom ImageIO and set it in the testReader. */
    GDCMImageIOType::Pointer dicomIO = GDCMImageIOType::New();
    testReader->SetImageIO( dicomIO );

    /** Set the name of the 2D dicom image in the testReader. */
    testReader->SetFileName( fileName.c_str() );

  } // end isDICOM

  /** Generate all information. */
  try
  {
    testReader->GenerateOutputInformation();
  }
  catch( itk::ExceptionObject  &  err  )
  {
    std::cerr  << "ExceptionObject caught !"  << std::endl;
    std::cerr  << err <<  std::endl;
    return 1;
  }

  /** Extract the ImageIO from the testReader. */
  ImageIOBaseType::Pointer testImageIOBase = testReader->GetImageIO();

  /** Get the component type, number of components, dimension and pixel type. */
  unsigned int inputDimension = testImageIOBase->GetNumberOfDimensions();
  unsigned int numberOfComponents = testImageIOBase->GetNumberOfComponents();
  std::string inputPixelComponentType = testImageIOBase->GetComponentTypeAsString(
    testImageIOBase->GetComponentType() );
  std::string pixelType = testImageIOBase->GetPixelTypeAsString(
    testImageIOBase->GetPixelType() );

  /** TASK 3:
   * Do some preparations.
   * *******************************************************************
   */

  /** Check inputPixelType. */
  if ( inputPixelComponentType != "unsigned_char"
    && inputPixelComponentType != "char"
    && inputPixelComponentType != "unsigned_short"
    && inputPixelComponentType != "short"
    && inputPixelComponentType != "unsigned_int"
    && inputPixelComponentType != "int"
    && inputPixelComponentType != "unsigned_long"
    && inputPixelComponentType != "long"
    && inputPixelComponentType != "float"
    && inputPixelComponentType != "double" )
  {
    /** In this case an illegal inputPixelComponentType is found. */
    std::cerr << "The found inputPixelComponentType is \"" << inputPixelComponentType
      << "\", which is not supported." << std::endl;
    return 1;
  }

  /** Check outputPixelType. */
  if ( outputPixelComponentType == "" )
  {
    /** In this case this option is not given, and by default
    * we set it to the inputPixelComponentType.
    */
    outputPixelComponentType = inputPixelComponentType;
  }

  /** Get rid of the "_" in inputPixelComponentType and outputPixelComponentType. */
  ReplaceUnderscoreWithSpace( inputPixelComponentType );
  ReplaceUnderscoreWithSpace( outputPixelComponentType );

  /** TASK 4:
   * Now we are ready to check on image type and subsequently call the
   * correct ReadCastWrite-function.
   * *******************************************************************
   */

  try
  {
    if ( !isDICOM )
    {
      /**
       * ****************** Support for SCALAR pixel types. **********************************
       */
      if ( pixelType == "scalar" && numberOfComponents == 1 )
      {
        const int ret_value = FileConverterScalar(
          inputPixelComponentType, outputPixelComponentType,
          inputFileName, outputFileName, inputDimension, useCompression );
        if ( ret_value != 0 )
        {
          return ret_value;
        }
      } // end scalar support
      /**
       * ****************** Support for multi-component pixel types. **********************************
       */
      else if ( numberOfComponents > 1 )
      {
        const int ret_value = FileConverterMultiComponent(
          inputPixelComponentType, outputPixelComponentType, numberOfComponents,
          inputFileName, outputFileName, inputDimension, useCompression );
        if ( ret_value != 0 )
        {
          return ret_value;
        }
      } // end multi-component support
      else
      {
        std::cerr << "Pixel type is " << pixelType
          << ", component type is " << inputPixelComponentType
          << " and number of components equals " << numberOfComponents << "." << std::endl;
        std::cerr << "ERROR: This image type is not supported." << std::endl;
        return 1;
      }
    } // end NonDicom image
    else
    {
      /** In this case input is a DICOM series, from which we only support
       * SCALAR pixel types, with component type:
       * DICOMImageIO2: (unsigned) char, (unsigned) short, float
       * GDCMImageIO: (unsigned) char, (unsigned) short, (unsigned) int, double
       * It is also assumed that the dicom series consist of multiple
       * 2D images forming a 3D image.
       */

      if ( strcmp( pixelType.c_str(), "scalar" ) == 0 && numberOfComponents == 1 )
      {
        const int ret_value = DicomFileConverterScalarA(
          inputPixelComponentType, outputPixelComponentType,
          inputDirectoryName, seriesUID, restrictions,
          outputFileName, inputDimension, useCompression )
          || DicomFileConverterScalarB(
          inputPixelComponentType, outputPixelComponentType,
          inputDirectoryName, seriesUID, restrictions,
          outputFileName, inputDimension, useCompression );
        if ( ret_value != 0 )
        {
          return ret_value;
        }
      }
      else
      {
        std::cerr << "Pixel type is " << pixelType
          << ", component type is " << inputPixelComponentType
          << " and number of components equals " << numberOfComponents << "." << std::endl;
        std::cerr << "ERROR: This image type is not supported." << std::endl;
        return 1;
      }

    } // end isDICOM

  } // end try
  /** If any errors have occurred, catch and print the exception and return false. */
  catch ( itk::ExceptionObject & err )
  {
    std::cerr << "ExceptionObject caught !" << std::endl;
    std::cerr << err << std::endl;
    return 1;
  }

  /** End  program. Return success. */
  return 0;

}  // end main
<commit_msg>Add success message at the end of castconvert<commit_after>/*=========================================================================
*
* Copyright Marius Staring, Stefan Klein, David Doria. 2011.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0.txt
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*=========================================================================*/
/** \file
 \brief This program converts and possibly casts images.

 \verbinclude castconvert.help
 */
/*
 * authors:       Marius Staring and Stefan Klein
 *
 * Thanks to Hans J. Johnson for a modification to this program. This
 * modification breaks down the program into smaller compilation units,
 * so that the compiler does not overflow.
 *
 * ENH: on 23-05-2006 we added multi-component support.
 * ENH: on 09-06-2006 we added support for extracting a specific DICOM serie.
 */

#include <iostream>
#include "castconverthelpers2.h"

#include "CommandLineArgumentHelper.h"
#include "itkCommandLineArgumentParser.h"

#include "itkImageFileReader.h"
#include "itkGDCMImageIO.h"

// Some non-standard IO Factories
#include "itkGE4ImageIOFactory.h"
#include "itkGE5ImageIOFactory.h"
#include "itkGEAdwImageIOFactory.h"
//#include "itkBrains2MaskImageIOFactory.h"
#include "itkPhilipsRECImageIOFactory.h"

/* Functions to do the actual conversion. */
extern int FileConverterScalar(
  const std::string & inputPixelComponentType,
  const std::string & outputPixelComponentType,
  const std::string & inputFileName,
  const std::string & outputFileName,
  const unsigned int inputDimension, bool useCompression );
extern int DicomFileConverterScalarA(
  const std::string & inputPixelComponentType,
  const std::string & outputPixelComponentType,
  const std::string & inputDirectoryName,
  const std::string & seriesUID,
  const std::vector<std::string> & restrictions,
  const std::string & outputFileName,
  const unsigned int inputDimension, bool useCompression );
extern int DicomFileConverterScalarB(
  const std::string & inputPixelComponentType,
  const std::string & outputPixelComponentType,
  const std::string & inputDirectoryName,
  const std::string & seriesUID,
  const std::vector<std::string> & restrictions,
  const std::string & outputFileName,
  const unsigned int inputDimension, bool useCompression );
extern int FileConverterMultiComponent(
  const std::string & inputPixelComponentType,
  const std::string & outputPixelComponentType,
  const unsigned int numberOfComponents,
  const std::string & inputFileName,
  const std::string & outputFileName,
  const unsigned int inputDimension, bool useCompression );

//-------------------------------------------------------------------------------------

int main( int argc, char **argv )
{
  /** Register some non-standard IO Factories to make the tool more useful.
   * Copied from the Insight Applications.
   */
  //itk::Brains2MaskImageIOFactory::RegisterOneFactory();
  itk::GE4ImageIOFactory::RegisterOneFactory();
  itk::GE5ImageIOFactory::RegisterOneFactory();
  itk::GEAdwImageIOFactory::RegisterOneFactory();
  itk::PhilipsRECImageIOFactory::RegisterOneFactory();

  itk::CommandLineArgumentParser::Pointer parser = itk::CommandLineArgumentParser::New();
  parser->SetCommandLineArguments( argc, argv );
  parser->SetProgramHelpText( GetHelpString() );

  parser->MarkArgumentAsRequired( "-in", "The input filename." );
  parser->MarkArgumentAsRequired( "-out", "The output filename." );

  /** Get the command line arguments. */
  std::string input = "";
  std::string outputFileName = "";
  std::string outputPixelComponentType = "";
  std::string seriesUID = "";
  std::vector<std::string> restrictions;
  std::string errorMessage = "";
  bool useCompression = false;
  bool argsRetrievedSuccessfully = GetCommandLineArguments( parser,
    input, outputFileName, outputPixelComponentType,
    seriesUID, restrictions, useCompression );

  itk::CommandLineArgumentParser::ReturnValue validateArguments = parser->CheckForRequiredArguments();

  if(validateArguments == itk::CommandLineArgumentParser::FAILED || !argsRetrievedSuccessfully)
  {
    return EXIT_FAILURE;
  }
  else if(validateArguments == itk::CommandLineArgumentParser::HELPREQUESTED)
  {
    return EXIT_SUCCESS;
  }

  /** Are we dealing with an image or a dicom series? */
  bool isDICOM = false;
  int returnValue2 = IsDICOM( input, errorMessage, isDICOM );
  if ( returnValue2 )
  {
    std::cout << errorMessage << std::endl;
    return returnValue2;
  }

  /** Get inputFileName or inputDirectoryName. */
  std::string inputFileName, inputDirectoryName;
  if ( !isDICOM ) inputFileName = input;
  else inputDirectoryName = input;

  /** TASK 2:
   * Typedefs and test reading to determine correct image types.
   * *******************************************************************
   */

  /** Initial image type. */
  const unsigned int Dimension = 3;
  typedef short      PixelType;

  /** Some typedef's. */
  typedef itk::Image< PixelType, Dimension >   ImageType;
  typedef itk::ImageFileReader< ImageType >    ReaderType;
  typedef itk::ImageIOBase                     ImageIOBaseType;
  typedef itk::GDCMImageIO                     GDCMImageIOType;

  /** Create a testReader. */
  ReaderType::Pointer testReader = ReaderType::New();

  /** Setup the testReader. */
  if ( !isDICOM )
  {
    /** Set the inputFileName in the testReader. */
    testReader->SetFileName( inputFileName.c_str() );
  }
  else
  {
    std::string fileName = "";
    int returnValue3 = GetFileNameFromDICOMDirectory(
      inputDirectoryName, fileName, seriesUID, restrictions, errorMessage );
    if ( returnValue3 )
    {
      std::cout << errorMessage << std::endl;
      return returnValue3;
    }

    /** Create a dicom ImageIO and set it in the testReader. */
    GDCMImageIOType::Pointer dicomIO = GDCMImageIOType::New();
    testReader->SetImageIO( dicomIO );

    /** Set the name of the 2D dicom image in the testReader. */
    testReader->SetFileName( fileName.c_str() );

  } // end isDICOM

  /** Generate all information. */
  try
  {
    testReader->GenerateOutputInformation();
  }
  catch( itk::ExceptionObject  &  err  )
  {
    std::cerr  << "ExceptionObject caught !"  << std::endl;
    std::cerr  << err <<  std::endl;
    return 1;
  }

  /** Extract the ImageIO from the testReader. */
  ImageIOBaseType::Pointer testImageIOBase = testReader->GetImageIO();

  /** Get the component type, number of components, dimension and pixel type. */
  unsigned int inputDimension = testImageIOBase->GetNumberOfDimensions();
  unsigned int numberOfComponents = testImageIOBase->GetNumberOfComponents();
  std::string inputPixelComponentType = testImageIOBase->GetComponentTypeAsString(
    testImageIOBase->GetComponentType() );
  std::string pixelType = testImageIOBase->GetPixelTypeAsString(
    testImageIOBase->GetPixelType() );

  /** TASK 3:
   * Do some preparations.
   * *******************************************************************
   */

  /** Check inputPixelType. */
  if ( inputPixelComponentType != "unsigned_char"
    && inputPixelComponentType != "char"
    && inputPixelComponentType != "unsigned_short"
    && inputPixelComponentType != "short"
    && inputPixelComponentType != "unsigned_int"
    && inputPixelComponentType != "int"
    && inputPixelComponentType != "unsigned_long"
    && inputPixelComponentType != "long"
    && inputPixelComponentType != "float"
    && inputPixelComponentType != "double" )
  {
    /** In this case an illegal inputPixelComponentType is found. */
    std::cerr << "The found inputPixelComponentType is \"" << inputPixelComponentType
      << "\", which is not supported." << std::endl;
    return 1;
  }

  /** Check outputPixelType. */
  if ( outputPixelComponentType == "" )
  {
    /** In this case this option is not given, and by default
    * we set it to the inputPixelComponentType.
    */
    outputPixelComponentType = inputPixelComponentType;
  }

  /** Get rid of the "_" in inputPixelComponentType and outputPixelComponentType. */
  ReplaceUnderscoreWithSpace( inputPixelComponentType );
  ReplaceUnderscoreWithSpace( outputPixelComponentType );

  /** TASK 4:
   * Now we are ready to check on image type and subsequently call the
   * correct ReadCastWrite-function.
   * *******************************************************************
   */

  try
  {
    if ( !isDICOM )
    {
      /**
       * ****************** Support for SCALAR pixel types. **********************************
       */
      if ( pixelType == "scalar" && numberOfComponents == 1 )
      {
        const int ret_value = FileConverterScalar(
          inputPixelComponentType, outputPixelComponentType,
          inputFileName, outputFileName, inputDimension, useCompression );
        if ( ret_value != 0 )
        {
          return ret_value;
        }
      } // end scalar support
      /**
       * ****************** Support for multi-component pixel types. **********************************
       */
      else if ( numberOfComponents > 1 )
      {
        const int ret_value = FileConverterMultiComponent(
          inputPixelComponentType, outputPixelComponentType, numberOfComponents,
          inputFileName, outputFileName, inputDimension, useCompression );
        if ( ret_value != 0 )
        {
          return ret_value;
        }
      } // end multi-component support
      else
      {
        std::cerr << "Pixel type is " << pixelType
          << ", component type is " << inputPixelComponentType
          << " and number of components equals " << numberOfComponents << "." << std::endl;
        std::cerr << "ERROR: This image type is not supported." << std::endl;
        return 1;
      }
    } // end NonDicom image
    else
    {
      /** In this case input is a DICOM series, from which we only support
       * SCALAR pixel types, with component type:
       * DICOMImageIO2: (unsigned) char, (unsigned) short, float
       * GDCMImageIO: (unsigned) char, (unsigned) short, (unsigned) int, double
       * It is also assumed that the dicom series consist of multiple
       * 2D images forming a 3D image.
       */

      if ( strcmp( pixelType.c_str(), "scalar" ) == 0 && numberOfComponents == 1 )
      {
        const int ret_value = DicomFileConverterScalarA(
          inputPixelComponentType, outputPixelComponentType,
          inputDirectoryName, seriesUID, restrictions,
          outputFileName, inputDimension, useCompression )
          || DicomFileConverterScalarB(
          inputPixelComponentType, outputPixelComponentType,
          inputDirectoryName, seriesUID, restrictions,
          outputFileName, inputDimension, useCompression );
        if ( ret_value != 0 )
        {
          return ret_value;
        }
      }
      else
      {
        std::cerr << "Pixel type is " << pixelType
          << ", component type is " << inputPixelComponentType
          << " and number of components equals " << numberOfComponents << "." << std::endl;
        std::cerr << "ERROR: This image type is not supported." << std::endl;
        return 1;
      }

    } // end isDICOM

  } // end try
  /** If any errors have occurred, catch and print the exception and return false. */
  catch ( itk::ExceptionObject & err )
  {
    std::cerr << "ExceptionObject caught !" << std::endl;
    std::cerr << err << std::endl;
    return 1;
  }

  std::cout << "Successful conversion!" << std::endl;

  /** End  program. Return success. */
  return 0;

}  // end main
<|endoftext|>


<commit_before><commit_msg>fdo#40694 - Error in message when truncating col/row numbers in calc<commit_after><|endoftext|>


<commit_before>#include "cnn/training.h"

#include "cnn/gpu-ops.h"

namespace cnn {

using namespace std;

Trainer::~Trainer() {}

/**
@scale : proportional to the number of utterances trained in parallel
*/
float Trainer::clip_gradients(float nutt) {
  float gscale = 1;
  if (clipping_enabled) {
    float gg = model->gradient_l2_norm();
    if (gg > clip_threshold * nutt) {
      ++clips;
      gscale = (clip_threshold * nutt) / gg;
    }
  }
  return gscale;
}

void SimpleSGDTrainer::update(float nutt, real scale) {
    update(model->lookup_parameters_list(), model->parameters_list(), nutt, scale);
}

void SimpleSGDTrainer::update(const std::vector<LookupParameters*> &lookup_params, const std::vector<Parameters*> &params, float nutt, real scale) {
  const float gscale = clip_gradients(nutt);
  float nutt_scale = 1.0 / nutt;
  for (auto p : params) {
#if HAVE_CUDA
    gpu::sgd_update(p->values.d.size(), p->g.v, p->values.v, eta * scale * gscale * nutt_scale, lambda);
#else
    auto reg = (*p->values) * lambda;
    *p->values -= nutt_scale * ((eta * scale * gscale) * *p->g + reg);
#endif
    p->clear();
  }
  for (auto p : lookup_params) {
    for (auto i : p->non_zero_grads) {
#if HAVE_CUDA
      gpu::sgd_update(p->values[i].d.size(), p->grads[i].v, p->values[i].v, eta * scale * gscale * nutt_scale, lambda);
#else
      auto reg = (*p->values[i]) * lambda;
      *p->values[i] -= (*p->grads[i] * (eta * scale * gscale * nutt_scale) + reg);
#endif
    }
    p->clear();
  }
  ++updates;
}

void MomentumSGDTrainer::update(real nutt, real scale) {
  // executed on the first iteration to create vectors to
  // store the velocity
  if (!velocity_allocated) {
    vp = AllocateShadowParameters(*model);
    vlp = AllocateShadowLookupParameters(*model);
    velocity_allocated = true;
  }

  const float gscale = clip_gradients(nutt);
  float nutt_scale = 1.0 / nutt;
  unsigned pi = 0;
  for (auto p : model->parameters_list()) {
    Tensor& v = vp[pi++].h;
    auto reg = *p->values * lambda;
    (*v) = momentum * (*v) - (eta * scale * gscale*nutt_scale) * (*p->g);
    *p->values += *v - reg;
    p->clear();
  }
  pi = 0;
  for (auto p : model->lookup_parameters_list()) {
    vector<Tensor>& vx = vlp[pi++].h;
    for (auto i : p->non_zero_grads) {
      Tensor& v = vx[i];
      auto reg = (*p->values[i]) * lambda;
      (*v) = momentum * (*v) - (eta * scale * gscale*nutt_scale) * (*p->grads[i]);
      *p->values[i] += *v - reg;
    }
    p->clear();
  }
  ++updates;
}

void AdagradTrainer::update(real nutt, real scale) {
  unsigned pi;
  if (!shadow_params_allocated) {
    vp = AllocateShadowParameters(*model);
    vlp = AllocateShadowLookupParameters(*model);
    shadow_params_allocated = true;
  }

  pi = 0;
  const float gscale = clip_gradients(nutt);
  float nutt_scale = 1.0 / nutt;
  for (auto p : model->parameters_list()) {
    Tensor& v = vp[pi++].h;
    auto reg = (*p->values) * lambda;
    auto g2 = (*p->g).cwiseProduct(*p->g);
    (*v) += g2;
    auto delta = -(eta * scale * gscale * nutt_scale) * (*p->g).cwiseQuotient(((*v).array() + epsilon).matrix().cwiseSqrt());
    *p->values += delta - reg;
    p->clear();
  }

  pi = 0;
  for (auto p : model->lookup_parameters_list()) {
    vector<Tensor>& vx = vlp[pi++].h;
    for (auto i : p->non_zero_grads) {
      Tensor& v = vx[i];
      auto reg = (*p->values[i]) * lambda;
      auto g2 = (*p->grads[i]).cwiseProduct(*p->grads[i]);
      (*v) += g2;
      auto delta = -(eta * scale * gscale * nutt_scale) * (*p->grads[i]).cwiseQuotient(((*v).array() + epsilon).matrix().cwiseSqrt());
      *p->values[i] += delta - reg;
    }
    p->clear();
  }

  ++updates;
}

void AdadeltaTrainer::update(real nutt, real scale) {
  unsigned pi;
  if (!shadow_params_allocated) {
    hg = AllocateShadowParameters(*model);
    hlg = AllocateShadowLookupParameters(*model);
    hd = AllocateShadowParameters(*model);
    hld = AllocateShadowLookupParameters(*model);

    /*pi = 0;
    for (auto p : model->parameters_list()) {
      TensorTools::Constant(hg[pi].h, epsilon);
      TensorTools::Constant(hd[pi].h, epsilon);
      ++pi;
    }

    pi = 0;
    for (auto p : model->lookup_parameters_list()) {
      vector<Tensor>& hgx = hlg[pi].h;
      vector<Tensor>& hdx = hld[pi].h;
      for (unsigned i = 0; i < hgx.size(); ++i) {
        TensorTools::Constant(hgx[i], epsilon);
        TensorTools::Constant(hdx[i], epsilon);
      }
      ++pi;
    }*/

    shadow_params_allocated = true;
  }

  const float gscale = clip_gradients(nutt);
  float nutt_scale = 1.0 / nutt;
  pi = 0;
  for (auto p : model->parameters_list()) {
    auto& g = (scale * gscale * nutt_scale) * *p->g;
    Tensor& hgv = hg[pi].h;
    Tensor& hdv = hd[pi].h;
    auto reg = (*p->values) * lambda;
    auto g2 = g.cwiseProduct(g);
    *hgv = rho * *hgv + (1.0 - rho) * g2;
    auto num = -g.cwiseProduct(((*hdv).array() + epsilon).matrix().cwiseSqrt());
    auto den = ((*hgv).array() + epsilon).matrix().cwiseSqrt();
    auto delta = num.cwiseQuotient(den);
    auto d2 = delta.cwiseProduct(delta);
    *hdv = rho * *hdv + (1.0 - rho) * d2;
    *p->values += delta - reg;
    p->clear();
    pi++;
  }

  pi = 0;
  for (auto p : model->lookup_parameters_list()) {
    vector<Tensor>& hgvx = hlg[pi].h;
    vector<Tensor>& hdvx = hld[pi].h;
    for (auto i : p->non_zero_grads) {
      Tensor& hgv = hgvx[i];
      Tensor& hdv = hdvx[i];
      auto& g = scale * gscale * nutt_scale * *p->grads[i];
      auto reg = (*p->values[i]) * lambda;
      auto g2 = g.cwiseProduct(g);
      *hgv = rho * *hgv + (1.0 - rho) * g2;
      auto num = -g.cwiseProduct(((*hdv).array() + epsilon).matrix().cwiseSqrt());
      auto den = ((*hgv).array() + epsilon).matrix().cwiseSqrt();
      auto delta = num.cwiseQuotient(den);
      auto d2 = delta.cwiseProduct(delta);
      *hdv = rho * *hdv + (1.0 - rho) * d2;
      *p->values[i] += delta - reg;
    }
    p->clear();
    pi++;
  }
  ++updates;
}

void RmsPropTrainer::update(real nutt, real scale) {
  unsigned pi = 0;
  if (!shadow_params_allocated) {
    hg.resize(model->parameters_list().size());

    pi = 0;
    hlg.resize(model->lookup_parameters_list().size());
    for (auto p : model->lookup_parameters_list()) {
      hlg[pi++].resize(p->size());
    }

    shadow_params_allocated = true;
  }

  const float gscale = clip_gradients(nutt);
  float nutt_scale = 1.0 / nutt;
  pi = 0;
  for (auto p : model->parameters_list()) {
    real& d2 = hg[pi++];
    auto reg = (*p->values) * lambda;
    real g2 = (*p->g).squaredNorm();
    d2 = rho * d2 + (1.0 - rho) * g2;
    *p->values -= ((eta * scale * gscale * nutt_scale / sqrt(d2 + epsilon)) * *p->g + reg);
    p->clear();
  }

  pi = 0;
  for (auto p : model->lookup_parameters_list()) {
    vector<real>& hlgx = hlg[pi++];
    for (auto i : p->non_zero_grads) {
      real& d2 = hlgx[i];
      auto reg = (*p->values[i]) * lambda;
      real g2 = (*p->grads[i]).squaredNorm();
      d2 = rho * d2 + (1.0 - rho) * g2;
      *p->values[i] -= ((eta * scale * gscale * nutt_scale / sqrt(d2 + epsilon)) * *p->grads[i] + reg);
    }
    p->clear();
  }
  ++updates;
}

void AdamTrainer::update(real nutt, real scale) {
  unsigned pi;
  if (!shadow_params_allocated) {
    m = AllocateShadowParameters(*model);
    lm = AllocateShadowLookupParameters(*model);
    v = AllocateShadowParameters(*model);
    lv = AllocateShadowLookupParameters(*model);
    shadow_params_allocated = true;
  }

  const float gscale = clip_gradients();
  float nutt_scale = 1.0 / nutt;
  pi = 0;
  static unsigned t = 0;
  for (auto p : model->parameters_list()) {
    ++t;
    auto g_t = (scale * gscale * nutt_scale) * *p->g;
    auto m_t = *m[pi].h;
    auto v_t = *v[pi].h;
    auto reg = (*p->values) * lambda;
    m_t = beta_1 * m_t + (1 - beta_1) * g_t;
    auto g2 = g_t.cwiseProduct(g_t);
    v_t = beta_2 * v_t + (1 - beta_2) * g2;
    float s1 = 1 - pow(beta_1, t);
    float s2 = 1 - pow(beta_2, t);
    auto mhat = m_t / s1;
    auto vhat = v_t / s2;
    auto delta = (-eta * mhat).cwiseQuotient((vhat.array().sqrt() + eps).matrix());
    *p->values += delta - reg;
    p->clear();
    pi++;
  }

  pi = 0;
  for (auto p : model->lookup_parameters_list()) {
    vector<Tensor>& vm = lm[pi].h;
    vector<Tensor>& vv = lv[pi].h;
    for (auto i : p->non_zero_grads) {
      auto m_t = *vm[i];
      auto v_t = *vv[i];
      auto g_t = scale * gscale * nutt_scale * *p->grads[i];
      auto g2 = g_t.cwiseProduct(g_t);
      auto reg = (*p->values[i]) * lambda;
      m_t = beta_1 * m_t + (1 - beta_1) * g_t;
      v_t = beta_2 * v_t + (1 - beta_2) * g2;
      float s1 = 1 - pow(beta_1, t);
      float s2 = 1 - pow(beta_2, t);
      auto mhat = m_t / s1;
      auto vhat = v_t / s2;
      auto delta = (-eta * mhat).cwiseQuotient((vhat.array().sqrt() + eps).matrix());
      *p->values[i] += delta - reg;
    }
    p->clear();
    pi++;
  }
  ++updates;
}

} // namespace cnn
<commit_msg>updated code for cliping to consider the size of data, essentially making gradient clipping independent of number of observations<commit_after>#include "cnn/training.h"

#include "cnn/gpu-ops.h"

namespace cnn {

using namespace std;

Trainer::~Trainer() {}

/**
@scale : proportional to the number of samples trained in parallel
*/
float Trainer::clip_gradients(float samples) {
  float gscale = 1;
  if (clipping_enabled) {
    float gg = model->gradient_l2_norm();
    if (gg > clip_threshold * samples) {
      ++clips;
      gscale = (clip_threshold * samples) / gg;
    }
  }
  return gscale;
}

void SimpleSGDTrainer::update(float nutt, real scale) {
    update(model->lookup_parameters_list(), model->parameters_list(), nutt, scale);
}

void SimpleSGDTrainer::update(const std::vector<LookupParameters*> &lookup_params, const std::vector<Parameters*> &params, float samples, real scale) {
  const float gscale = clip_gradients(samples);
  float nutt_scale = 1.0 / samples;
  for (auto p : params) {
#if HAVE_CUDA
    gpu::sgd_update(p->values.d.size(), p->g.v, p->values.v, eta * scale * gscale * nutt_scale, lambda);
#else
    auto reg = (*p->values) * lambda;
    *p->values -= nutt_scale * ((eta * scale * gscale) * *p->g + reg);
#endif
    p->clear();
  }
  for (auto p : lookup_params) {
    for (auto i : p->non_zero_grads) {
#if HAVE_CUDA
      gpu::sgd_update(p->values[i].d.size(), p->grads[i].v, p->values[i].v, eta * scale * gscale * nutt_scale, lambda);
#else
      auto reg = (*p->values[i]) * lambda;
      *p->values[i] -= (*p->grads[i] * (eta * scale * gscale * nutt_scale) + reg);
#endif
    }
    p->clear();
  }
  ++updates;
}

void MomentumSGDTrainer::update(real nutt, real scale) {
  // executed on the first iteration to create vectors to
  // store the velocity
  if (!velocity_allocated) {
    vp = AllocateShadowParameters(*model);
    vlp = AllocateShadowLookupParameters(*model);
    velocity_allocated = true;
  }

  const float gscale = clip_gradients(nutt);
  float nutt_scale = 1.0 / nutt;
  unsigned pi = 0;
  for (auto p : model->parameters_list()) {
    Tensor& v = vp[pi++].h;
    auto reg = *p->values * lambda;
    (*v) = momentum * (*v) - (eta * scale * gscale*nutt_scale) * (*p->g);
    *p->values += *v - reg;
    p->clear();
  }
  pi = 0;
  for (auto p : model->lookup_parameters_list()) {
    vector<Tensor>& vx = vlp[pi++].h;
    for (auto i : p->non_zero_grads) {
      Tensor& v = vx[i];
      auto reg = (*p->values[i]) * lambda;
      (*v) = momentum * (*v) - (eta * scale * gscale*nutt_scale) * (*p->grads[i]);
      *p->values[i] += *v - reg;
    }
    p->clear();
  }
  ++updates;
}

void AdagradTrainer::update(real nsamples, real scale) {
  unsigned pi;
  if (!shadow_params_allocated) {
    vp = AllocateShadowParameters(*model);
    vlp = AllocateShadowLookupParameters(*model);
    shadow_params_allocated = true;
  }

  pi = 0;
  const float gscale = clip_gradients(nsamples);
  float nutt_scale = 1.0 / nsamples;
  for (auto p : model->parameters_list()) {
    Tensor& v = vp[pi++].h;
    auto reg = (*p->values) * lambda;
    auto g2 = (*p->g).cwiseProduct(*p->g);
    (*v) += g2;
    auto delta = -(eta * scale * gscale * nutt_scale) * (*p->g).cwiseQuotient(((*v).array() + epsilon).matrix().cwiseSqrt());
    *p->values += delta - reg;
    p->clear();
  }

  pi = 0;
  for (auto p : model->lookup_parameters_list()) {
    vector<Tensor>& vx = vlp[pi++].h;
    for (auto i : p->non_zero_grads) {
      Tensor& v = vx[i];
      auto reg = (*p->values[i]) * lambda;
      auto g2 = (*p->grads[i]).cwiseProduct(*p->grads[i]);
      (*v) += g2;
      auto delta = -(eta * scale * gscale * nutt_scale) * (*p->grads[i]).cwiseQuotient(((*v).array() + epsilon).matrix().cwiseSqrt());
      *p->values[i] += delta - reg;
    }
    p->clear();
  }

  ++updates;
}

void AdadeltaTrainer::update(real nutt, real scale) {
  unsigned pi;
  if (!shadow_params_allocated) {
    hg = AllocateShadowParameters(*model);
    hlg = AllocateShadowLookupParameters(*model);
    hd = AllocateShadowParameters(*model);
    hld = AllocateShadowLookupParameters(*model);

    /*pi = 0;
    for (auto p : model->parameters_list()) {
      TensorTools::Constant(hg[pi].h, epsilon);
      TensorTools::Constant(hd[pi].h, epsilon);
      ++pi;
    }

    pi = 0;
    for (auto p : model->lookup_parameters_list()) {
      vector<Tensor>& hgx = hlg[pi].h;
      vector<Tensor>& hdx = hld[pi].h;
      for (unsigned i = 0; i < hgx.size(); ++i) {
        TensorTools::Constant(hgx[i], epsilon);
        TensorTools::Constant(hdx[i], epsilon);
      }
      ++pi;
    }*/

    shadow_params_allocated = true;
  }

  const float gscale = clip_gradients(nutt);
  float nutt_scale = 1.0 / nutt;
  pi = 0;
  for (auto p : model->parameters_list()) {
    auto& g = (scale * gscale * nutt_scale) * *p->g;
    Tensor& hgv = hg[pi].h;
    Tensor& hdv = hd[pi].h;
    auto reg = (*p->values) * lambda;
    auto g2 = g.cwiseProduct(g);
    *hgv = rho * *hgv + (1.0 - rho) * g2;
    auto num = -g.cwiseProduct(((*hdv).array() + epsilon).matrix().cwiseSqrt());
    auto den = ((*hgv).array() + epsilon).matrix().cwiseSqrt();
    auto delta = num.cwiseQuotient(den);
    auto d2 = delta.cwiseProduct(delta);
    *hdv = rho * *hdv + (1.0 - rho) * d2;
    *p->values += delta - reg;
    p->clear();
    pi++;
  }

  pi = 0;
  for (auto p : model->lookup_parameters_list()) {
    vector<Tensor>& hgvx = hlg[pi].h;
    vector<Tensor>& hdvx = hld[pi].h;
    for (auto i : p->non_zero_grads) {
      Tensor& hgv = hgvx[i];
      Tensor& hdv = hdvx[i];
      auto& g = scale * gscale * nutt_scale * *p->grads[i];
      auto reg = (*p->values[i]) * lambda;
      auto g2 = g.cwiseProduct(g);
      *hgv = rho * *hgv + (1.0 - rho) * g2;
      auto num = -g.cwiseProduct(((*hdv).array() + epsilon).matrix().cwiseSqrt());
      auto den = ((*hgv).array() + epsilon).matrix().cwiseSqrt();
      auto delta = num.cwiseQuotient(den);
      auto d2 = delta.cwiseProduct(delta);
      *hdv = rho * *hdv + (1.0 - rho) * d2;
      *p->values[i] += delta - reg;
    }
    p->clear();
    pi++;
  }
  ++updates;
}

void RmsPropTrainer::update(real nutt, real scale) {
  unsigned pi = 0;
  if (!shadow_params_allocated) {
    hg.resize(model->parameters_list().size());

    pi = 0;
    hlg.resize(model->lookup_parameters_list().size());
    for (auto p : model->lookup_parameters_list()) {
      hlg[pi++].resize(p->size());
    }

    shadow_params_allocated = true;
  }

  const float gscale = clip_gradients(nutt);
  float nutt_scale = 1.0 / nutt;
  pi = 0;
  for (auto p : model->parameters_list()) {
    real& d2 = hg[pi++];
    auto reg = (*p->values) * lambda;
    real g2 = (*p->g).squaredNorm();
    d2 = rho * d2 + (1.0 - rho) * g2;
    *p->values -= ((eta * scale * gscale * nutt_scale / sqrt(d2 + epsilon)) * *p->g + reg);
    p->clear();
  }

  pi = 0;
  for (auto p : model->lookup_parameters_list()) {
    vector<real>& hlgx = hlg[pi++];
    for (auto i : p->non_zero_grads) {
      real& d2 = hlgx[i];
      auto reg = (*p->values[i]) * lambda;
      real g2 = (*p->grads[i]).squaredNorm();
      d2 = rho * d2 + (1.0 - rho) * g2;
      *p->values[i] -= ((eta * scale * gscale * nutt_scale / sqrt(d2 + epsilon)) * *p->grads[i] + reg);
    }
    p->clear();
  }
  ++updates;
}

void AdamTrainer::update(real nutt, real scale) {
  unsigned pi;
  if (!shadow_params_allocated) {
    m = AllocateShadowParameters(*model);
    lm = AllocateShadowLookupParameters(*model);
    v = AllocateShadowParameters(*model);
    lv = AllocateShadowLookupParameters(*model);
    shadow_params_allocated = true;
  }

  const float gscale = clip_gradients();
  float nutt_scale = 1.0 / nutt;
  pi = 0;
  static unsigned t = 0;
  for (auto p : model->parameters_list()) {
    ++t;
    auto g_t = (scale * gscale * nutt_scale) * *p->g;
    auto m_t = *m[pi].h;
    auto v_t = *v[pi].h;
    auto reg = (*p->values) * lambda;
    m_t = beta_1 * m_t + (1 - beta_1) * g_t;
    auto g2 = g_t.cwiseProduct(g_t);
    v_t = beta_2 * v_t + (1 - beta_2) * g2;
    float s1 = 1 - pow(beta_1, t);
    float s2 = 1 - pow(beta_2, t);
    auto mhat = m_t / s1;
    auto vhat = v_t / s2;
    auto delta = (-eta * mhat).cwiseQuotient((vhat.array().sqrt() + eps).matrix());
    *p->values += delta - reg;
    p->clear();
    pi++;
  }

  pi = 0;
  for (auto p : model->lookup_parameters_list()) {
    vector<Tensor>& vm = lm[pi].h;
    vector<Tensor>& vv = lv[pi].h;
    for (auto i : p->non_zero_grads) {
      auto m_t = *vm[i];
      auto v_t = *vv[i];
      auto g_t = scale * gscale * nutt_scale * *p->grads[i];
      auto g2 = g_t.cwiseProduct(g_t);
      auto reg = (*p->values[i]) * lambda;
      m_t = beta_1 * m_t + (1 - beta_1) * g_t;
      v_t = beta_2 * v_t + (1 - beta_2) * g2;
      float s1 = 1 - pow(beta_1, t);
      float s2 = 1 - pow(beta_2, t);
      auto mhat = m_t / s1;
      auto vhat = v_t / s2;
      auto delta = (-eta * mhat).cwiseQuotient((vhat.array().sqrt() + eps).matrix());
      *p->values[i] += delta - reg;
    }
    p->clear();
    pi++;
  }
  ++updates;
}

} // namespace cnn
<|endoftext|>


<commit_before><commit_msg>[ELF] - Remove unnecessary template #6. NFC.<commit_after><|endoftext|>


<commit_before><commit_msg>Formatting fix no functionality change.<commit_after><|endoftext|>


<commit_before>
/*
 * Copyright 2015 Cloudius Systems
 */

/*
 * This file is part of Scylla.
 *
 * Scylla is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Scylla is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Scylla.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "core/reactor.hh"
#include "core/app-template.hh"
#include "message/messaging_service.hh"
#include "gms/failure_detector.hh"
#include "gms/gossiper.hh"
#include "gms/application_state.hh"
#include "service/storage_service.hh"
#include "log.hh"
#include <chrono>

namespace bpo = boost::program_options;

int main(int ac, char ** av) {
    distributed<database> db;
    app_template app;
    app.add_options()
        ("seed", bpo::value<std::vector<std::string>>(), "IP address of seed node")
        ("listen-address", bpo::value<std::string>()->default_value("0.0.0.0"), "IP address to listen");
    return app.run_deprecated(ac, av, [&db, &app] {
        auto config = app.configuration();
        logging::logger_registry().set_logger_level("gossip", logging::log_level::trace);
        const gms::inet_address listen = gms::inet_address(config["listen-address"].as<std::string>());
        auto vv = std::make_shared<gms::versioned_value::factory>();
        service::init_storage_service(db).then([vv, listen, config] {
            return net::get_messaging_service().start(listen);
        }).then([config] {
            auto& server = net::get_local_messaging_service();
            auto port = server.port();
            auto listen = server.listen_address();
            print("Messaging server listening on ip %s port %d ...\n", listen, port);
            return gms::get_failure_detector().start();
        }).then([vv, config] {
            return gms::get_gossiper().start();
        }).then([vv, config] {
            std::set<gms::inet_address> seeds;
            for (auto s : config["seed"].as<std::vector<std::string>>()) {
                seeds.emplace(std::move(s));
            }

            std::cout << "Start gossiper service ...\n";
            auto& gossiper = gms::get_local_gossiper();
            gossiper.set_seeds(std::move(seeds));
            gossiper.set_cluster_name("Test Cluster");

            std::map<gms::application_state, gms::versioned_value> app_states = {
                { gms::application_state::LOAD, vv->load(0.5) },
            };

            using namespace std::chrono;
            auto now = high_resolution_clock::now().time_since_epoch();
            int generation_number = duration_cast<seconds>(now).count();
            return gossiper.start(generation_number, app_states);
        }).then([vv] {
            auto reporter = std::make_shared<timer<lowres_clock>>();
            reporter->set_callback ([reporter] {
                auto& gossiper = gms::get_local_gossiper();
                gossiper.dump_endpoint_state_map();
                auto& fd = gms::get_local_failure_detector();
                print("%s", fd);
            });
            reporter->arm_periodic(std::chrono::milliseconds(1000));

            auto app_state_adder = std::make_shared<timer<lowres_clock>>();
            app_state_adder->set_callback ([vv, app_state_adder] {
                static double load = 0.5;
                auto& gossiper = gms::get_local_gossiper();
                auto state = gms::application_state::LOAD;
                auto value = vv->load(load);
                gossiper.add_local_application_state(state, value);
                load += 0.0001;
            });
            app_state_adder->arm_periodic(std::chrono::seconds(1));
        });
    });
}
<commit_msg>tests: Fix gossip<commit_after>
/*
 * Copyright 2015 Cloudius Systems
 */

/*
 * This file is part of Scylla.
 *
 * Scylla is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Scylla is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Scylla.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "core/reactor.hh"
#include "core/app-template.hh"
#include "message/messaging_service.hh"
#include "gms/failure_detector.hh"
#include "gms/gossiper.hh"
#include "gms/application_state.hh"
#include "service/storage_service.hh"
#include "utils/fb_utilities.hh"
#include "locator/snitch_base.hh"
#include "log.hh"
#include <chrono>

namespace bpo = boost::program_options;

int main(int ac, char ** av) {
    distributed<database> db;
    app_template app;
    app.add_options()
        ("seed", bpo::value<std::vector<std::string>>(), "IP address of seed node")
        ("listen-address", bpo::value<std::string>()->default_value("0.0.0.0"), "IP address to listen");
    return app.run_deprecated(ac, av, [&db, &app] {
        auto config = app.configuration();
        logging::logger_registry().set_logger_level("gossip", logging::log_level::trace);
        const gms::inet_address listen = gms::inet_address(config["listen-address"].as<std::string>());
        utils::fb_utilities::set_broadcast_address(listen);
        auto vv = std::make_shared<gms::versioned_value::factory>();
        locator::i_endpoint_snitch::create_snitch("SimpleSnitch").then([&db] {
            return service::init_storage_service(db);
        }).then([vv, listen, config] {
            return net::get_messaging_service().start(listen);
        }).then([config] {
            auto& server = net::get_local_messaging_service();
            auto port = server.port();
            auto listen = server.listen_address();
            print("Messaging server listening on ip %s port %d ...\n", listen, port);
            return gms::get_failure_detector().start();
        }).then([vv, config] {
            return gms::get_gossiper().start();
        }).then([vv, config] {
            std::set<gms::inet_address> seeds;
            for (auto s : config["seed"].as<std::vector<std::string>>()) {
                seeds.emplace(std::move(s));
            }

            std::cout << "Start gossiper service ...\n";
            auto& gossiper = gms::get_local_gossiper();
            gossiper.set_seeds(std::move(seeds));
            gossiper.set_cluster_name("Test Cluster");

            std::map<gms::application_state, gms::versioned_value> app_states = {
                { gms::application_state::LOAD, vv->load(0.5) },
            };

            using namespace std::chrono;
            auto now = high_resolution_clock::now().time_since_epoch();
            int generation_number = duration_cast<seconds>(now).count();
            return gossiper.start(generation_number, app_states);
        }).then([vv] {
            auto reporter = std::make_shared<timer<lowres_clock>>();
            reporter->set_callback ([reporter] {
                auto& gossiper = gms::get_local_gossiper();
                gossiper.dump_endpoint_state_map();
                auto& fd = gms::get_local_failure_detector();
                print("%s", fd);
            });
            reporter->arm_periodic(std::chrono::milliseconds(1000));

            auto app_state_adder = std::make_shared<timer<lowres_clock>>();
            app_state_adder->set_callback ([vv, app_state_adder] {
                static double load = 0.5;
                auto& gossiper = gms::get_local_gossiper();
                auto state = gms::application_state::LOAD;
                auto value = vv->load(load);
                gossiper.add_local_application_state(state, value);
                load += 0.0001;
            });
            app_state_adder->arm_periodic(std::chrono::seconds(1));
        });
    });
}
<|endoftext|>


<commit_before>/*
   Copyright 2015 James Fong

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.

*/

#include "GameLayerMachine.hpp"
#include <algorithm>

namespace vse {

GameLayerMachine::GameLayerMachine(
    Ogre::Root* ogreRoot,
    Ogre::RenderWindow* ogreWindow,
    SDL_Window* sdlWindow,
    CEGUI::OgreRenderer* ceguiRenderer,
    CEGUI::Window* ceguiWindow)
: mOgreRoot(ogreRoot)
, mOgreWindow(ogreWindow)
, mSdlWindow(sdlWindow)
, mCeguiRenderer(ceguiRenderer)
, mCeguiWindow(ceguiWindow) {

}

GameLayerMachine::~GameLayerMachine() {}

void GameLayerMachine::addTop(GameLayer* addMe) {
    addMe->onBegin(mOgreRoot, mOgreWindow, mSdlWindow, mCeguiRenderer, mCeguiWindow);

    for(std::vector<GameLayer*>::iterator iter = mLayers.begin(); iter != mLayers.end(); ++ iter) {
        GameLayer* layer = *iter;

        layer->onAddedAbove(addMe);
    }
    mLayers.push_back(addMe);
}
/*
void GameLayerMachine::addGlobal(GameLayer* addMe) {
}
void GameLayerMachine::removeGlobal(GameLayer* removeMe) {
}
*/
void GameLayerMachine::addAbove(GameLayer* addMe, GameLayer* caller) {
    // Find where the caller is located
    std::vector<GameLayer*>::iterator location = mLayers.end();
    for(std::vector<GameLayer*>::iterator iter = mLayers.begin(); iter != mLayers.end(); ++ iter) {
        GameLayer* layer = *iter;

        if(layer == caller) {
            location = iter;
            break;
        }
    }

    // The caller is valid
    assert(location != mLayers.end());

    // Insert the new game layer into the next location ("One layer above")
    ++ location;
    mLayers.insert(location, addMe); // Inserting here is safe
    addMe->onBegin(mOgreRoot, mOgreWindow, mSdlWindow, mCeguiRenderer, mCeguiWindow);

    // Inform all layers "below" this one that a new layer was added above them (this should logically include the caller)
    for(std::vector<GameLayer*>::iterator iter = mLayers.begin(); iter != location; ++ iter) {
        GameLayer* layer = *iter;

        layer->onAddedAbove(addMe);
    }
}
void GameLayerMachine::remove(GameLayer* removeMe) {
    // Find the layer to remove
    std::vector<GameLayer*>::iterator location = mLayers.end();
    for(std::vector<GameLayer*>::iterator iter = mLayers.begin(); iter != mLayers.end(); ++ iter) {
        GameLayer* layer = *iter;

        if(layer == removeMe) {
            location = iter;
            break;
        }
    }

    // This layer was found
    assert(location != mLayers.end());

    // Inform layer that it is going to be removed
    removeMe->onEnd();

    // Inform all layers "below" this one that a layer above them was removed
    for(std::vector<GameLayer*>::iterator iter = mLayers.begin(); iter != location; ++ iter) {
        GameLayer* layer = *iter;

        layer->onRemovedAbove(removeMe);
    }

    // Remove layer
    mLayers.erase(location);
}
void GameLayerMachine::removeAll() {
    std::vector<GameLayer*>::reverse_iterator iter = mLayers.rbegin();
    while(iter != mLayers.rend()) {
        GameLayer* layer = *iter;
        this->remove(layer);
        ++ iter
    }
}

// Ticks
void GameLayerMachine::onTick(float tps) {
    for(std::vector<GameLayer*>::reverse_iterator iter = mLayers.rbegin(); iter != mLayers.rend(); ++ iter) {
        GameLayer* layer = *iter;

        layer->onTick(tps);
    }
}

// Key handling
void GameLayerMachine::onKeyPress(const SDL_KeyboardEvent& event, bool repeat) {
    for(std::vector<GameLayer*>::reverse_iterator iter = mLayers.rbegin(); iter != mLayers.rend(); ++ iter) {
        GameLayer* layer = *iter;

        // Blocking
        if(layer->onKeyPress(event, repeat)) {
            break;
        }
    }
}
void GameLayerMachine::onKeyRelease(const SDL_KeyboardEvent& event) {
    for(std::vector<GameLayer*>::reverse_iterator iter = mLayers.rbegin(); iter != mLayers.rend(); ++ iter) {
        GameLayer* layer = *iter;

        // Blocking
        if(layer->onKeyRelease(event)) {
            break;
        }
    }
}
void GameLayerMachine::onTextInput(const SDL_TextInputEvent& event) {
    for(std::vector<GameLayer*>::reverse_iterator iter = mLayers.rbegin(); iter != mLayers.rend(); ++ iter) {
        GameLayer* layer = *iter;

        // Blocking
        if(layer->onTextInput(event)) {
            break;
        }
    }
}

// Mouse handling
void GameLayerMachine::onMouseMove(const SDL_MouseMotionEvent& event) {
    for(std::vector<GameLayer*>::reverse_iterator iter = mLayers.rbegin(); iter != mLayers.rend(); ++ iter) {
        GameLayer* layer = *iter;

        // Blocking
        if(layer->onMouseMove(event)) {
            break;
        }
    }
}
void GameLayerMachine::onMousePress(const SDL_MouseButtonEvent& event) {
    for(std::vector<GameLayer*>::reverse_iterator iter = mLayers.rbegin(); iter != mLayers.rend(); ++ iter) {
        GameLayer* layer = *iter;

        // Blocking
        if(layer->onMousePress(event)) {
            break;
        }
    }
}
void GameLayerMachine::onMouseRelease(const SDL_MouseButtonEvent& event) {
    for(std::vector<GameLayer*>::reverse_iterator iter = mLayers.rbegin(); iter != mLayers.rend(); ++ iter) {
        GameLayer* layer = *iter;

        // Blocking
        if(layer->onMouseRelease(event)) {
            break;
        }
    }
}
void GameLayerMachine::onMouseWheel(const SDL_MouseWheelEvent& event) {
    for(std::vector<GameLayer*>::reverse_iterator iter = mLayers.rbegin(); iter != mLayers.rend(); ++ iter) {
        GameLayer* layer = *iter;

        // Blocking
        if(layer->onMouseWheel(event)) {
            break;
        }
    }
}

}
<commit_msg>Add missing token<commit_after>/*
   Copyright 2015 James Fong

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.

*/

#include "GameLayerMachine.hpp"
#include <algorithm>

namespace vse {

GameLayerMachine::GameLayerMachine(
    Ogre::Root* ogreRoot,
    Ogre::RenderWindow* ogreWindow,
    SDL_Window* sdlWindow,
    CEGUI::OgreRenderer* ceguiRenderer,
    CEGUI::Window* ceguiWindow)
: mOgreRoot(ogreRoot)
, mOgreWindow(ogreWindow)
, mSdlWindow(sdlWindow)
, mCeguiRenderer(ceguiRenderer)
, mCeguiWindow(ceguiWindow) {

}

GameLayerMachine::~GameLayerMachine() {}

void GameLayerMachine::addTop(GameLayer* addMe) {
    addMe->onBegin(mOgreRoot, mOgreWindow, mSdlWindow, mCeguiRenderer, mCeguiWindow);

    for(std::vector<GameLayer*>::iterator iter = mLayers.begin(); iter != mLayers.end(); ++ iter) {
        GameLayer* layer = *iter;

        layer->onAddedAbove(addMe);
    }
    mLayers.push_back(addMe);
}
/*
void GameLayerMachine::addGlobal(GameLayer* addMe) {
}
void GameLayerMachine::removeGlobal(GameLayer* removeMe) {
}
*/
void GameLayerMachine::addAbove(GameLayer* addMe, GameLayer* caller) {
    // Find where the caller is located
    std::vector<GameLayer*>::iterator location = mLayers.end();
    for(std::vector<GameLayer*>::iterator iter = mLayers.begin(); iter != mLayers.end(); ++ iter) {
        GameLayer* layer = *iter;

        if(layer == caller) {
            location = iter;
            break;
        }
    }

    // The caller is valid
    assert(location != mLayers.end());

    // Insert the new game layer into the next location ("One layer above")
    ++ location;
    mLayers.insert(location, addMe); // Inserting here is safe
    addMe->onBegin(mOgreRoot, mOgreWindow, mSdlWindow, mCeguiRenderer, mCeguiWindow);

    // Inform all layers "below" this one that a new layer was added above them (this should logically include the caller)
    for(std::vector<GameLayer*>::iterator iter = mLayers.begin(); iter != location; ++ iter) {
        GameLayer* layer = *iter;

        layer->onAddedAbove(addMe);
    }
}
void GameLayerMachine::remove(GameLayer* removeMe) {
    // Find the layer to remove
    std::vector<GameLayer*>::iterator location = mLayers.end();
    for(std::vector<GameLayer*>::iterator iter = mLayers.begin(); iter != mLayers.end(); ++ iter) {
        GameLayer* layer = *iter;

        if(layer == removeMe) {
            location = iter;
            break;
        }
    }

    // This layer was found
    assert(location != mLayers.end());

    // Inform layer that it is going to be removed
    removeMe->onEnd();

    // Inform all layers "below" this one that a layer above them was removed
    for(std::vector<GameLayer*>::iterator iter = mLayers.begin(); iter != location; ++ iter) {
        GameLayer* layer = *iter;

        layer->onRemovedAbove(removeMe);
    }

    // Remove layer
    mLayers.erase(location);
}
void GameLayerMachine::removeAll() {
    std::vector<GameLayer*>::reverse_iterator iter = mLayers.rbegin();
    while(iter != mLayers.rend()) {
        GameLayer* layer = *iter;
        this->remove(layer);
        ++ iter;
    }
}

// Ticks
void GameLayerMachine::onTick(float tps) {
    for(std::vector<GameLayer*>::reverse_iterator iter = mLayers.rbegin(); iter != mLayers.rend(); ++ iter) {
        GameLayer* layer = *iter;

        layer->onTick(tps);
    }
}

// Key handling
void GameLayerMachine::onKeyPress(const SDL_KeyboardEvent& event, bool repeat) {
    for(std::vector<GameLayer*>::reverse_iterator iter = mLayers.rbegin(); iter != mLayers.rend(); ++ iter) {
        GameLayer* layer = *iter;

        // Blocking
        if(layer->onKeyPress(event, repeat)) {
            break;
        }
    }
}
void GameLayerMachine::onKeyRelease(const SDL_KeyboardEvent& event) {
    for(std::vector<GameLayer*>::reverse_iterator iter = mLayers.rbegin(); iter != mLayers.rend(); ++ iter) {
        GameLayer* layer = *iter;

        // Blocking
        if(layer->onKeyRelease(event)) {
            break;
        }
    }
}
void GameLayerMachine::onTextInput(const SDL_TextInputEvent& event) {
    for(std::vector<GameLayer*>::reverse_iterator iter = mLayers.rbegin(); iter != mLayers.rend(); ++ iter) {
        GameLayer* layer = *iter;

        // Blocking
        if(layer->onTextInput(event)) {
            break;
        }
    }
}

// Mouse handling
void GameLayerMachine::onMouseMove(const SDL_MouseMotionEvent& event) {
    for(std::vector<GameLayer*>::reverse_iterator iter = mLayers.rbegin(); iter != mLayers.rend(); ++ iter) {
        GameLayer* layer = *iter;

        // Blocking
        if(layer->onMouseMove(event)) {
            break;
        }
    }
}
void GameLayerMachine::onMousePress(const SDL_MouseButtonEvent& event) {
    for(std::vector<GameLayer*>::reverse_iterator iter = mLayers.rbegin(); iter != mLayers.rend(); ++ iter) {
        GameLayer* layer = *iter;

        // Blocking
        if(layer->onMousePress(event)) {
            break;
        }
    }
}
void GameLayerMachine::onMouseRelease(const SDL_MouseButtonEvent& event) {
    for(std::vector<GameLayer*>::reverse_iterator iter = mLayers.rbegin(); iter != mLayers.rend(); ++ iter) {
        GameLayer* layer = *iter;

        // Blocking
        if(layer->onMouseRelease(event)) {
            break;
        }
    }
}
void GameLayerMachine::onMouseWheel(const SDL_MouseWheelEvent& event) {
    for(std::vector<GameLayer*>::reverse_iterator iter = mLayers.rbegin(); iter != mLayers.rend(); ++ iter) {
        GameLayer* layer = *iter;

        // Blocking
        if(layer->onMouseWheel(event)) {
            break;
        }
    }
}

}
<|endoftext|>


<commit_before>#include "BtuLinear.h"

BtuLinear::BtuLinear():
    m_depthPid(DEP_K_C, DEP_TAU_I, DEP_TAU_D, PID_FREQ, DEPTH_MIN, DEPTH_MAX, VEL_MIN, VEL_MAX, 0),
    m_posAPid(SP_K_C, SP_TAU_I, SP_TAU_D, PID_FREQ, POS_MIN, POS_MAX, VEL_MIN, VEL_MAX, 0),
    m_posBPid(SP_K_C, SP_TAU_I, SP_TAU_D, PID_FREQ, POS_MIN, POS_MAX, VEL_MIN, VEL_MAX, 0),
    m_velAPid(VEL_K_C, VEL_TAU_I, VEL_TAU_D, PID_FREQ, VEL_MIN, VEL_MAX, -1, 1, 0),
    m_velBPid(VEL_K_C, VEL_TAU_I, VEL_TAU_D, PID_FREQ, VEL_MIN, VEL_MAX, -1, 1, 0),
	m_pressureSensor(PIN_IMU_SDA, PIN_IMU_SCL),
    m_actAPwm(PIN_ACTA_PWM),
    m_actBPwm(PIN_ACTB_PWM),
    m_actAPot(PIN_ACTA_POT),
    m_actBPot(PIN_ACTB_POT),
    m_actADir(PIN_ACTA_DIR),
    m_actBDir(PIN_ACTB_DIR)
{};

BtuLinear::~BtuLinear(){}

void BtuLinear::init() {
    m_mode = 2;                   // vel control
    m_kc = DEP_K_C;
    m_tauI = DEP_TAU_I;
    m_tauD = DEP_TAU_D;

    m_v_kc = VEL_K_C;
    m_v_tauI = VEL_TAU_I;
    m_v_tauD = VEL_TAU_D;

    // m_motorServo.calibrate(SERVO_PWM_WIDTH, SERVO_DEGREE_WIDTH);
    m_pressureSensor.MS5837Init();
    m_pressureSensor.MS5837Start();
    wait(0.1);                    // remnant from old BTU class TODO: check if can be removed

    m_oldPosA = getActPosition(ACT_A);
    m_oldPosB = getActPosition(ACT_B);

    for(int i = 0; i < AVG_WINDOW_WIDTH; i++) {
        m_avg_windowA[i] = 0;
        m_avg_windowB[i] = 0;
    }
    m_avg_windowPtr = 0;
    m_avg_windowSize = 0;
    m_currentAvgA = 0;

    m_currentAvgB = 0;
    m_currentVoltage = 0;
}

float BtuLinear::getPressure() {
    return m_pressureSensor.MS5837_Pressure();
}

void BtuLinear::stop() {
    return;
}

void BtuLinear::update(int mode, float kc, float tauI, float tauD) {
    updateMode(mode);
    m_kc = kc;
    m_tauI = tauI;
    m_tauD = tauD;
    m_depthPid.setTunings(kc, tauI, tauD);
}

void BtuLinear::updatePosTunings(float kc, float tauI, float tauD) {
    m_p_kc = kc;
    m_p_tauI = tauI;
    m_p_tauD = tauD;
    m_posAPid.setTunings(kc, tauI, tauD);
    m_posBPid.setTunings(kc, tauI, tauD);
}

void BtuLinear::updateVelTunings(float kc, float tauI, float tauD) {
    m_v_kc = kc;
    m_v_tauI = tauI;
    m_v_tauD = tauD;
    m_velAPid.setTunings(kc, tauI, tauD);
    m_velBPid.setTunings(kc, tauI, tauD);
}

void BtuLinear::updateMode(int mode) {
    if(m_mode != mode) {
        stop();
        m_mode = mode;
        m_currentVoltage = 0;
        m_currentAvgA = 0;
        m_currentAvgB = 0;
        m_avg_windowSize = 0;
        m_depthPid.reset();
        m_posAPid.reset();
        m_posBPid.reset();
        m_velAPid.reset();
        m_velBPid.reset();
    }
}

void BtuLinear::runCycle(float setVal) {
    switch (m_mode) {

    case VOLTAGE_CTRL_MODE:
        voltageControl(setVal);
        break;

    case VELOCITY_CTRL_MODE:
        velocityControl(setVal);
        break;

    case DEPTH_CTRL_MODE:
        depthControl(setVal);
        break;

    case POSITION_CTRL_MODE:
        positionControl(setVal);
        break;
    }
}

void BtuLinear::updateAndRunCycle(int mode, float value) {
    updateMode(mode);
    runCycle(value);
}

// void BTU::positionControl(float setPosDeg) {
//     float setPos = clip(setPosDeg, -SERVO_DEGREE_WIDTH, SERVO_DEGREE_WIDTH);
//     m_motorServo.position(setPos);
// }

void BtuLinear::voltageControlHelper(float setDuty, int ctrl) {
    // PwmOut* actPwm;
    // DigitalOut* actDir;
    // if(ctrl == ACT_A) {
    //     actPwm = &m_actAPwm;
    //     actDir = &m_actADir;
    // } else {
    //     actPwm = &m_actBPwm;
    //     actDir = &m_actBDir;
    // }
    if(ctrl == ACT_A) {
        if(setDuty > 0) {
            m_actAPwm = setDuty;
            m_actADir = 1;
        } else {
            m_actADir = 0;
            m_actAPwm = -setDuty;
        }
    }

    if(ctrl == ACT_B) {
        if(setDuty > 0) {
            m_actBPwm = setDuty;
            m_actBDir = 1;
        } else {
            m_actBDir = 0;
            m_actBPwm = -setDuty;
        }
    }
    // if(setDuty > 0) {
    //     *actPwm = setDuty;
    //     *actDir = 1;
    // } else {
    //     *actDir = 0;
    //     *actPwm = -setDuty;
    // }
}

void BtuLinear::voltageControl(float setDuty) {
    voltageControlHelper(setDuty, ACT_A);
    voltageControlHelper(setDuty, ACT_B);
}

void BtuLinear::updatePositionReadings() {
    float aPosition = m_actAPot;
    float bPosition = m_actBPot;

    float aOldPos = m_avg_windowA[m_avg_windowPtr];
    float bOldPos = m_avg_windowB[m_avg_windowPtr];
    m_avg_windowA[m_avg_windowPtr] = aPosition;
    m_avg_windowB[m_avg_windowPtr] = bPosition;
    m_avg_windowPtr = (m_avg_windowPtr+1) % AVG_WINDOW_WIDTH;
    if(m_avg_windowSize >= AVG_WINDOW_WIDTH) {
        // buffer is full
    	m_currentAvgA = m_currentAvgA + (aPosition / AVG_WINDOW_WIDTH)- (aOldPos / AVG_WINDOW_WIDTH);
        m_currentAvgB = m_currentAvgB + (bPosition / AVG_WINDOW_WIDTH)- (bOldPos / AVG_WINDOW_WIDTH);
    } else {
    	// buffer is still filling up
        m_avg_windowSize++;
        m_currentAvgA = 0;
        m_currentAvgB = 0;
        for(int i = 0; i < m_avg_windowSize; i++) {
            m_currentAvgA = (m_avg_windowA[i] / m_avg_windowSize);
            m_currentAvgB = (m_avg_windowB[i] / m_avg_windowSize);
        }
    }
}

float BtuLinear::getActPosition(int act) {
    // updatePositionReadings();
    // float position;
    // if(act == ACT_A) {
    //     position = m_currentAvgA;
    // } else {
    //     position = m_currentAvgB;
    // }
    float position;
    if(act == ACT_A) {
        position = m_actAPot;
    } else {
        position = m_actBPot;
    }
    float scaledPos = (position - POT_MIN) / (POT_MAX - POT_MIN);
    return scaledPos;
}

void BtuLinear::velocityControl(float setVel) {
    velocityControlHelper(setVel, ACT_A);
    velocityControlHelper(setVel, ACT_B);
}

float btu_abs(float a) {
    return (a >= 0) ? a : -a;
}

void BtuLinear::velocityControlHelper(float setVelocity, int ctrl) {
    // float pos = m_motorServo.readPosition();
    if(ctrl != ACT_A && ctrl != ACT_B) {
        return;
    }
    float pos;
    float deltaVolt;
    float cmdVolt;
    float setVel = setVelocity;

    if(ctrl == ACT_A) {
        pos = getActPosition(ACT_A);
        m_velAPid.setProcessValue((pos - m_oldPosA) / PID_FREQ);
        m_velAPid.setSetPoint(setVel);
        deltaVolt = m_velAPid.compute();
        m_oldPosA = pos;
    } else if(ctrl == ACT_B){
        pos = getActPosition(ACT_B);
        m_velBPid.setProcessValue((pos - m_oldPosB) / PID_FREQ);
        m_velBPid.setSetPoint(setVel);
        deltaVolt = m_velBPid.compute();
        m_oldPosB = pos;
    }
    m_currentVoltage += deltaVolt;
    if ((getActPosition(ctrl) <= 0.01 && setVel < 0) || (getActPosition(ctrl) >= 0.99 && setVel > 0)) {
        m_currentVoltage = 0;
    }
    cmdVolt = m_currentVoltage;
    if(btu_abs(cmdVolt) <= VOLTAGE_THRESHOLD) {
        cmdVolt = 0;
    }
    voltageControlHelper(cmdVolt, ctrl);

}

void BtuLinear::positionControlHelper(float setPosDeg, int ctrl) {
    if(ctrl == ACT_A) {
        m_posAPid.setSetPoint(setPosDeg);
        // m_specPid.setProcessValue(m_motorServo.readPosition());
        m_posAPid.setProcessValue(getActPosition(ACT_A));
        float cmdVelA = m_posAPid.compute();
        // m_specCmd = (int)(cmdVel >= 0); // just for test recording purposes
        velocityControlHelper(cmdVelA, ACT_A);
    } else {
        m_posBPid.setSetPoint(setPosDeg);
        // m_specPid.setProcessValue(m_motorServo.readPosition());
        m_posBPid.setProcessValue(getActPosition(ACT_B));
        float cmdVelB = m_posBPid.compute();
        // m_specCmd = (int)(cmdVel >= 0); // just for test recording purposes
        velocityControlHelper(cmdVelB, ACT_B);
    }

}

void BtuLinear::positionControl(float setPos) {
    positionControlHelper(setPos, ACT_A);
    positionControlHelper(setPos, ACT_B);
}

void BtuLinear::depthControlHelper(float cmdVelocity) {
    velocityControlHelper(cmdVelocity, ACT_A);
    positionControlHelper(getActPosition(ACT_A), ACT_B);
}

void BtuLinear::depthControl(float setDepthMeters) {
    m_depthPid.setSetPoint(setDepthMeters);

    float curDepth = getDepth();

    m_depthPid.setProcessValue(curDepth);

    float cmdVel = m_depthPid.compute();
    depthControlHelper(cmdVel);
    // velocityControl(cmdVel);
}

float BtuLinear::getDepth() {
    float pvDepth = getPressure();
    float pvDepthMeters = (pvDepth - P_ATMOS_MBAR) / P_WATER_SURFACE_MBAR;
    return pvDepthMeters;
}

// float BTU::getServoPos() {
// 	return m_motorServo.readPosition();
// }
<commit_msg>clipped commandable voltage<commit_after>#include "BtuLinear.h"

BtuLinear::BtuLinear():
    m_depthPid(DEP_K_C, DEP_TAU_I, DEP_TAU_D, PID_FREQ, DEPTH_MIN, DEPTH_MAX, VEL_MIN, VEL_MAX, 0),
    m_posAPid(SP_K_C, SP_TAU_I, SP_TAU_D, PID_FREQ, POS_MIN, POS_MAX, VEL_MIN, VEL_MAX, 0),
    m_posBPid(SP_K_C, SP_TAU_I, SP_TAU_D, PID_FREQ, POS_MIN, POS_MAX, VEL_MIN, VEL_MAX, 0),
    m_velAPid(VEL_K_C, VEL_TAU_I, VEL_TAU_D, PID_FREQ, VEL_MIN, VEL_MAX, -1, 1, 0),
    m_velBPid(VEL_K_C, VEL_TAU_I, VEL_TAU_D, PID_FREQ, VEL_MIN, VEL_MAX, -1, 1, 0),
	m_pressureSensor(PIN_IMU_SDA, PIN_IMU_SCL),
    m_actAPwm(PIN_ACTA_PWM),
    m_actBPwm(PIN_ACTB_PWM),
    m_actAPot(PIN_ACTA_POT),
    m_actBPot(PIN_ACTB_POT),
    m_actADir(PIN_ACTA_DIR),
    m_actBDir(PIN_ACTB_DIR)
{};

BtuLinear::~BtuLinear(){}

void BtuLinear::init() {
    m_mode = 2;                   // vel control
    m_kc = DEP_K_C;
    m_tauI = DEP_TAU_I;
    m_tauD = DEP_TAU_D;

    m_v_kc = VEL_K_C;
    m_v_tauI = VEL_TAU_I;
    m_v_tauD = VEL_TAU_D;

    // m_motorServo.calibrate(SERVO_PWM_WIDTH, SERVO_DEGREE_WIDTH);
    m_pressureSensor.MS5837Init();
    m_pressureSensor.MS5837Start();
    wait(0.1);                    // remnant from old BTU class TODO: check if can be removed

    m_oldPosA = getActPosition(ACT_A);
    m_oldPosB = getActPosition(ACT_B);

    for(int i = 0; i < AVG_WINDOW_WIDTH; i++) {
        m_avg_windowA[i] = 0;
        m_avg_windowB[i] = 0;
    }
    m_avg_windowPtr = 0;
    m_avg_windowSize = 0;
    m_currentAvgA = 0;

    m_currentAvgB = 0;
    m_currentVoltage = 0;
}

float BtuLinear::getPressure() {
    return m_pressureSensor.MS5837_Pressure();
}

void BtuLinear::stop() {
    return;
}

void BtuLinear::update(int mode, float kc, float tauI, float tauD) {
    updateMode(mode);
    m_kc = kc;
    m_tauI = tauI;
    m_tauD = tauD;
    m_depthPid.setTunings(kc, tauI, tauD);
}

void BtuLinear::updatePosTunings(float kc, float tauI, float tauD) {
    m_p_kc = kc;
    m_p_tauI = tauI;
    m_p_tauD = tauD;
    m_posAPid.setTunings(kc, tauI, tauD);
    m_posBPid.setTunings(kc, tauI, tauD);
}

void BtuLinear::updateVelTunings(float kc, float tauI, float tauD) {
    m_v_kc = kc;
    m_v_tauI = tauI;
    m_v_tauD = tauD;
    m_velAPid.setTunings(kc, tauI, tauD);
    m_velBPid.setTunings(kc, tauI, tauD);
}

void BtuLinear::updateMode(int mode) {
    if(m_mode != mode) {
        stop();
        m_mode = mode;
        m_currentVoltage = 0;
        m_currentAvgA = 0;
        m_currentAvgB = 0;
        m_avg_windowSize = 0;
        m_depthPid.reset();
        m_posAPid.reset();
        m_posBPid.reset();
        m_velAPid.reset();
        m_velBPid.reset();
    }
}

void BtuLinear::runCycle(float setVal) {
    switch (m_mode) {

    case VOLTAGE_CTRL_MODE:
        voltageControl(setVal);
        break;

    case VELOCITY_CTRL_MODE:
        velocityControl(setVal);
        break;

    case DEPTH_CTRL_MODE:
        depthControl(setVal);
        break;

    case POSITION_CTRL_MODE:
        positionControl(setVal);
        break;
    }
}

void BtuLinear::updateAndRunCycle(int mode, float value) {
    updateMode(mode);
    runCycle(value);
}

// void BTU::positionControl(float setPosDeg) {
//     float setPos = clip(setPosDeg, -SERVO_DEGREE_WIDTH, SERVO_DEGREE_WIDTH);
//     m_motorServo.position(setPos);
// }

void BtuLinear::voltageControlHelper(float setDuty, int ctrl) {
    // PwmOut* actPwm;
    // DigitalOut* actDir;
    // if(ctrl == ACT_A) {
    //     actPwm = &m_actAPwm;
    //     actDir = &m_actADir;
    // } else {
    //     actPwm = &m_actBPwm;
    //     actDir = &m_actBDir;
    // }
    if(ctrl == ACT_A) {
        if(setDuty > 0) {
            m_actAPwm = setDuty;
            m_actADir = 1;
        } else {
            m_actADir = 0;
            m_actAPwm = -setDuty;
        }
    }

    if(ctrl == ACT_B) {
        if(setDuty > 0) {
            m_actBPwm = setDuty;
            m_actBDir = 1;
        } else {
            m_actBDir = 0;
            m_actBPwm = -setDuty;
        }
    }
    // if(setDuty > 0) {
    //     *actPwm = setDuty;
    //     *actDir = 1;
    // } else {
    //     *actDir = 0;
    //     *actPwm = -setDuty;
    // }
}

void BtuLinear::voltageControl(float setDuty) {
    voltageControlHelper(setDuty, ACT_A);
    voltageControlHelper(setDuty, ACT_B);
}

void BtuLinear::updatePositionReadings() {
    float aPosition = m_actAPot;
    float bPosition = m_actBPot;

    float aOldPos = m_avg_windowA[m_avg_windowPtr];
    float bOldPos = m_avg_windowB[m_avg_windowPtr];
    m_avg_windowA[m_avg_windowPtr] = aPosition;
    m_avg_windowB[m_avg_windowPtr] = bPosition;
    m_avg_windowPtr = (m_avg_windowPtr+1) % AVG_WINDOW_WIDTH;
    if(m_avg_windowSize >= AVG_WINDOW_WIDTH) {
        // buffer is full
    	m_currentAvgA = m_currentAvgA + (aPosition / AVG_WINDOW_WIDTH)- (aOldPos / AVG_WINDOW_WIDTH);
        m_currentAvgB = m_currentAvgB + (bPosition / AVG_WINDOW_WIDTH)- (bOldPos / AVG_WINDOW_WIDTH);
    } else {
    	// buffer is still filling up
        m_avg_windowSize++;
        m_currentAvgA = 0;
        m_currentAvgB = 0;
        for(int i = 0; i < m_avg_windowSize; i++) {
            m_currentAvgA = (m_avg_windowA[i] / m_avg_windowSize);
            m_currentAvgB = (m_avg_windowB[i] / m_avg_windowSize);
        }
    }
}

float BtuLinear::getActPosition(int act) {
    // updatePositionReadings();
    // float position;
    // if(act == ACT_A) {
    //     position = m_currentAvgA;
    // } else {
    //     position = m_currentAvgB;
    // }
    float position;
    if(act == ACT_A) {
        position = m_actAPot;
    } else {
        position = m_actBPot;
    }
    float scaledPos = (position - POT_MIN) / (POT_MAX - POT_MIN);
    return scaledPos;
}

void BtuLinear::velocityControl(float setVel) {
    velocityControlHelper(setVel, ACT_A);
    velocityControlHelper(setVel, ACT_B);
}

float btu_abs(float a) {
    return (a >= 0) ? a : -a;
}

void BtuLinear::velocityControlHelper(float setVelocity, int ctrl) {
    // float pos = m_motorServo.readPosition();
    if(ctrl != ACT_A && ctrl != ACT_B) {
        return;
    }
    float pos;
    float deltaVolt;
    float cmdVolt;
    float setVel = setVelocity;

    if(ctrl == ACT_A) {
        pos = getActPosition(ACT_A);
        m_velAPid.setProcessValue((pos - m_oldPosA) / PID_FREQ);
        m_velAPid.setSetPoint(setVel);
        deltaVolt = m_velAPid.compute();
        m_oldPosA = pos;
    } else if(ctrl == ACT_B){
        pos = getActPosition(ACT_B);
        m_velBPid.setProcessValue((pos - m_oldPosB) / PID_FREQ);
        m_velBPid.setSetPoint(setVel);
        deltaVolt = m_velBPid.compute();
        m_oldPosB = pos;
    }
    m_currentVoltage = utility::clip(m_currentVoltage + deltaVolt, -1.0, 1.0);
    if ((getActPosition(ctrl) <= 0.01 && setVel < 0) || (getActPosition(ctrl) >= 0.99 && setVel > 0)) {
        m_currentVoltage = 0;
    }
    cmdVolt = m_currentVoltage;
    if(btu_abs(cmdVolt) <= VOLTAGE_THRESHOLD) {
        cmdVolt = 0;
    }
    voltageControlHelper(cmdVolt, ctrl);

}

void BtuLinear::positionControlHelper(float setPosDeg, int ctrl) {
    if(ctrl == ACT_A) {
        m_posAPid.setSetPoint(setPosDeg);
        // m_specPid.setProcessValue(m_motorServo.readPosition());
        m_posAPid.setProcessValue(getActPosition(ACT_A));
        float cmdVelA = m_posAPid.compute();
        // m_specCmd = (int)(cmdVel >= 0); // just for test recording purposes
        velocityControlHelper(cmdVelA, ACT_A);
    } else {
        m_posBPid.setSetPoint(setPosDeg);
        // m_specPid.setProcessValue(m_motorServo.readPosition());
        m_posBPid.setProcessValue(getActPosition(ACT_B));
        float cmdVelB = m_posBPid.compute();
        // m_specCmd = (int)(cmdVel >= 0); // just for test recording purposes
        velocityControlHelper(cmdVelB, ACT_B);
    }

}

void BtuLinear::positionControl(float setPos) {
    positionControlHelper(setPos, ACT_A);
    positionControlHelper(setPos, ACT_B);
}

void BtuLinear::depthControlHelper(float cmdVelocity) {
    velocityControlHelper(cmdVelocity, ACT_A);
    positionControlHelper(getActPosition(ACT_A), ACT_B);
}

void BtuLinear::depthControl(float setDepthMeters) {
    m_depthPid.setSetPoint(setDepthMeters);

    float curDepth = getDepth();

    m_depthPid.setProcessValue(curDepth);

    float cmdVel = m_depthPid.compute();
    depthControlHelper(cmdVel);
    // velocityControl(cmdVel);
}

float BtuLinear::getDepth() {
    float pvDepth = getPressure();
    float pvDepthMeters = (pvDepth - P_ATMOS_MBAR) / P_WATER_SURFACE_MBAR;
    return pvDepthMeters;
}

// float BTU::getServoPos() {
// 	return m_motorServo.readPosition();
// }
<|endoftext|>