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()
{
  // Print a start message to indicate that the sample app has started
  const std::string SAMPLE_APP_NAME = "Math Blocks: Calculate Acceleration from Velocity";
  SampleAppsHelper::PrintHeader(SAMPLE_APP_NAME);
 
  /* CONSTANTS */
  /* *NOTICE* The following constants must be configured before attempting to run with hardware. */
  const int NUM_AXES = 1;            // The number of axes to configure
  const int AXIS_INDEX = 0;          // The index of the axis to configure
  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);
 
  // Variables to store initial object counts to restore later (starts at -1 to indicate it has not been set)
  int initialMathBlockCount = -1;
 
  // Set the exit code to an error value.
  int exitCode = -1;  
  try   // Ensure that the controller is deleted if an error occurs.
  {
    // Prepare the controller as defined in SampleAppsHelper.h depending on the configuration
    SampleAppsHelper::CheckErrors(controller);
    SampleAppsHelper::SetupController(controller, NUM_AXES);
 
    // Save initial object counts to restore later
    initialMathBlockCount = controller->MathBlockCountGet();
 
    /* SAMPLE APP BODY */
 
    /* Configure the controller object counts */
    // Normally you would set the number of axes here, but for samples that is handled in the SampleAppsHelper::SetupController function
    // controller->AxisCountSet(NUM_AXES); 
 
    // Add the two MathBlocks needed for this sample
    const int subtractionMathBlockIndex = initialMathBlockCount;
    const int previousVelocityMathBlockIndex = initialMathBlockCount + 1;
    controller->MathBlockCountSet(initialMathBlockCount + 2);
 
    //  Get the axis and make sure the axis is not moving and clear any faults
    Axis *axis = controller->AxisGet(AXIS_INDEX);
    SampleAppsHelper::CheckErrors(axis);
    axis->Abort();
    axis->ClearFaults();
 
    // Read the configuration of both MathBlocks
    MotionController::MathBlockConfig subtractionConfig = controller->MathBlockConfigGet(subtractionMathBlockIndex);
 
    // This index must be greater than the subtraction math block index, so the subtraction data is
    // one sample old
    MotionController::MathBlockConfig previousVelocityConfig = controller->MathBlockConfigGet(previousVelocityMathBlockIndex);
 
    // 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, previousVelocityMathBlockIndex);
    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(subtractionMathBlockIndex, subtractionConfig);
    controller->MathBlockConfigSet(previousVelocityMathBlockIndex, 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(subtractionMathBlockIndex).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;
  }
  // Restore the object counts to the original values
  if (initialMathBlockCount != -1) { controller->MathBlockCountSet(initialMathBlockCount); }
  // Note: The axis count is handled by the SampleAppsHelper::Cleanup function
 
  // Clean up the controller and any other objects as needed
  SampleAppsHelper::Cleanup(controller);
 
  // 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;
}