CalculateAccelerationFromVelocity.cpp

 
#include "SampleAppsHelper.h" // Import our helper functions.
#include "rsi.h"              // Import our RapidCode Library.
 
using namespace RSI::RapidCode; // Import our RapidCode namespace
 
int main()
{
  const std::string SAMPLE_APP_NAME = "Math Blocks: Calculate Acceleration from Velocity";
 
  // Print a start message to indicate that the sample app has started
  SampleAppsHelper::PrintHeader(SAMPLE_APP_NAME);
 
  /* *NOTICE* The following constants must be configured before attempting to run with hardware. */
  const double VELOCITY = 1.0;       // the velocity to move the axis
  const double ACCELERATION = 0.123; // something small so we can check the MathBlock is working
 
  /* RAPIDCODE INITIALIZATION */
 
  // Create the Controller
  MotionController::CreationParameters params = SampleAppsHelper::GetCreationParameters();
  MotionController *controller = MotionController::Create(&params);
 
  int exitCode = -1; // Set the exit code to an error value.
  try                // Ensure that the controller is deleted if an error occurs.
  {
    SampleAppsHelper::CheckErrors(controller);
 
    // Prepare the controller and axes as defined in SampleAppsHelper.h
    SampleAppsHelper::SetupController(controller);
 
    /* SAMPLE APP BODY */
 
    /* Configure the controller object counts */
    // Add the two MathBlocks needed for this sample
    controller->MathBlockCountSet(SampleAppsConfig::MATH_BLOCK_COUNT + 2);
 
    //  Get the axis and make sure the axis is not moving and clear any faults
    Axis *axis = controller->AxisGet(SampleAppsConfig::AXIS_X_INDEX);
    SampleAppsHelper::CheckErrors(axis);
    axis->Abort();
    axis->ClearFaults();
 
    // Read the configuration of both MathBlocks
    const int SUBTRACTION_MATHBLOCK_INDEX = SampleAppsConfig::MATH_BLOCK_COUNT;
    MotionController::MathBlockConfig subtractionConfig = controller->MathBlockConfigGet(SUBTRACTION_MATHBLOCK_INDEX);
 
    // This index must be greater than the subtraction math block index, so the subtraction data is
    // one sample old
    const int PREVIOUS_VELOCITY_MATHBLOCK_INDEX = SUBTRACTION_MATHBLOCK_INDEX + 1;
    MotionController::MathBlockConfig previousVelocityConfig = controller->MathBlockConfigGet(PREVIOUS_VELOCITY_MATHBLOCK_INDEX);
 
    // Set the axis to use the command velocity as the input for the MathBlock
    RSIAxisAddressType INPUT_AXIS_ADDRESS_TYPE = RSIAxisAddressType::RSIAxisAddressTypeCOMMAND_VELOCITY;
 
    // Configure the first MathBlock to subtract the previous velocity from the current velocity
    // Current velocity:
    subtractionConfig.InputAddress0 = axis->AddressGet(INPUT_AXIS_ADDRESS_TYPE);
    subtractionConfig.InputDataType0 = RSIDataType::RSIDataTypeDOUBLE;
 
    // Previous velocity: (as was calculated by the second MathBlock, so we use its ProcessValue)
    subtractionConfig.InputAddress1 =
        controller->AddressGet(RSIControllerAddressType::RSIControllerAddressTypeMATHBLOCK_PROCESS_VALUE, PREVIOUS_VELOCITY_MATHBLOCK_INDEX);
    subtractionConfig.InputDataType1 = RSIDataType::RSIDataTypeDOUBLE;
 
    subtractionConfig.ProcessDataType = RSIDataType::RSIDataTypeDOUBLE;
    subtractionConfig.Operation = RSIMathBlockOperation::RSIMathBlockOperationSUBTRACT;
 
    // Write 1.0 to the first UserBuffer entry so the second MathBlock can use it for multiplication
    uint64_t userBufferAddr0 = controller->AddressGet(RSIControllerAddressType::RSIControllerAddressTypeUSER_BUFFER, 0);
    controller->MemoryDoubleSet(userBufferAddr0, 1.0);
 
    // Configure the second MathBlock to multiply the current velocity by 1.0 (which we'll use for
    // the previous sample's velocity)
    previousVelocityConfig.InputAddress0 = axis->AddressGet(INPUT_AXIS_ADDRESS_TYPE);
    previousVelocityConfig.InputDataType0 = RSIDataType::RSIDataTypeDOUBLE;
    previousVelocityConfig.InputAddress1 = userBufferAddr0;
    previousVelocityConfig.InputDataType1 = RSIDataType::RSIDataTypeDOUBLE;
    previousVelocityConfig.ProcessDataType = RSIDataType::RSIDataTypeDOUBLE;
    previousVelocityConfig.Operation = RSIMathBlockOperation::RSIMathBlockOperationMULTIPLY;
 
    // Set the MathBlock configurations
    controller->MathBlockConfigSet(SUBTRACTION_MATHBLOCK_INDEX, subtractionConfig);
    controller->MathBlockConfigSet(PREVIOUS_VELOCITY_MATHBLOCK_INDEX, previousVelocityConfig);
 
    // Wait a sample so we know the RMP is now processing the newly configured MathBlocks
    controller->SampleWait(1);
 
    // Set the axis to move with a very small acceleration so we can check the MathBlock is working
    axis->AmpEnableSet(true); // Enable the motor.
    axis->MoveVelocity(VELOCITY, ACCELERATION);
 
    // Wait several samples so we know the RMP is now processing the move command and accelerating
    controller->SampleWait(10);
 
    // Keep in mind firmware velocity is in counts per sample, so we need to convert to UserUnits
    // per second squared
    double calculatedVelocityDelta = controller->MathBlockProcessValueGet(SUBTRACTION_MATHBLOCK_INDEX).Double;
 
    // Reduce the velocity back to 0
    axis->MoveVelocity(0, ACCELERATION);
 
    axis->MotionDoneWait();    // Wait for the axis to finish moving.
    axis->AmpEnableSet(false); // Disable the motor.
 
    // Convert to UserUnits per second squared
    double calculatedAcceleration = calculatedVelocityDelta * controller->SampleRateGet() * controller->SampleRateGet() / axis->UserUnitsGet();
    std::cout << "Calculated acceleration from MathBlock: " << calculatedAcceleration << std::endl;
 
    // Check that the newly calculated acceleration is as expected
    if (std::abs(calculatedAcceleration - ACCELERATION) <= 0.000001)
    {
      std::cout << "The MathBlock is calculating the Axis' acceleration by subtracting previous velocity from current velocity." << std::endl;
      exitCode = 0;
    }
    else
    {
      std::cerr << "Error: The calculated acceleration does not match the expected value" << std::endl;
      exitCode = -1;
    }
  }
  catch (const std::exception &ex)
  {
    std::cerr << ex.what() << std::endl;
    exitCode = -1;
  }
  // Delete the controller as the program exits to ensure memory is deallocated in the correct order
  controller->Delete();
 
  // Print a message to indicate the sample app has finished and if it was successful or not
  SampleAppsHelper::PrintFooter(SAMPLE_APP_NAME, exitCode);
 
  return exitCode;
}