APIs, concepts, guides, and more
CalculateAccelerationFromVelocity.cpp
1
10#include "SampleAppsHelper.h" // Import our helper functions.
11#include "rsi.h" // Import our RapidCode Library.
12
13using namespace RSI::RapidCode; // Import our RapidCode namespace
14
15int main()
16{
17 const std::string SAMPLE_APP_NAME = "Math Blocks: Calculate Acceleration from Velocity";
18
19 // Print a start message to indicate that the sample app has started
20 SampleAppsHelper::PrintHeader(SAMPLE_APP_NAME);
21
23 /* *NOTICE* The following constants must be configured before attempting to run with hardware. */
24 const double VELOCITY = 1.0; // the velocity to move the axis
25 const double ACCELERATION = 0.123; // something small so we can check the MathBlock is working
26
27 /* RAPIDCODE INITIALIZATION */
28
29 // Create the Controller
31 MotionController *controller = MotionController::Create(&params);
32
33 int exitCode = -1; // Set the exit code to an error value.
34 try // Ensure that the controller is deleted if an error occurs.
35 {
37
38 // Prepare the controller and axes as defined in SampleAppsHelper.h
40
41 /* SAMPLE APP BODY */
42
43 /* Configure the controller object counts */
44 // Add the two MathBlocks needed for this sample
46
47 // Get the axis and make sure the axis is not moving and clear any faults
48 Axis *axis = controller->AxisGet(SampleAppsConfig::AXIS_X_INDEX);
50 axis->Abort();
51 axis->ClearFaults();
52
53 // Read the configuration of both MathBlocks
54 const int SUBTRACTION_MATHBLOCK_INDEX = SampleAppsConfig::MATH_BLOCK_COUNT;
55 MotionController::MathBlockConfig subtractionConfig = controller->MathBlockConfigGet(SUBTRACTION_MATHBLOCK_INDEX);
56
57 // This index must be greater than the subtraction math block index, so the subtraction data is
58 // one sample old
59 const int PREVIOUS_VELOCITY_MATHBLOCK_INDEX = SUBTRACTION_MATHBLOCK_INDEX + 1;
60 MotionController::MathBlockConfig previousVelocityConfig = controller->MathBlockConfigGet(PREVIOUS_VELOCITY_MATHBLOCK_INDEX);
61
62 // Set the axis to use the command velocity as the input for the MathBlock
63 RSIAxisAddressType INPUT_AXIS_ADDRESS_TYPE = RSIAxisAddressType::RSIAxisAddressTypeCOMMAND_VELOCITY;
64
65 // Configure the first MathBlock to subtract the previous velocity from the current velocity
66 // Current velocity:
67 subtractionConfig.InputAddress0 = axis->AddressGet(INPUT_AXIS_ADDRESS_TYPE);
68 subtractionConfig.InputDataType0 = RSIDataType::RSIDataTypeDOUBLE;
69
70 // Previous velocity: (as was calculated by the second MathBlock, so we use its ProcessValue)
71 subtractionConfig.InputAddress1 =
72 controller->AddressGet(RSIControllerAddressType::RSIControllerAddressTypeMATHBLOCK_PROCESS_VALUE, PREVIOUS_VELOCITY_MATHBLOCK_INDEX);
73 subtractionConfig.InputDataType1 = RSIDataType::RSIDataTypeDOUBLE;
74
75 subtractionConfig.ProcessDataType = RSIDataType::RSIDataTypeDOUBLE;
76 subtractionConfig.Operation = RSIMathBlockOperation::RSIMathBlockOperationSUBTRACT;
77
78 // Write 1.0 to the first UserBuffer entry so the second MathBlock can use it for multiplication
79 uint64_t userBufferAddr0 = controller->AddressGet(RSIControllerAddressType::RSIControllerAddressTypeUSER_BUFFER, 0);
80 controller->MemoryDoubleSet(userBufferAddr0, 1.0);
81
82 // Configure the second MathBlock to multiply the current velocity by 1.0 (which we'll use for
83 // the previous sample's velocity)
84 previousVelocityConfig.InputAddress0 = axis->AddressGet(INPUT_AXIS_ADDRESS_TYPE);
85 previousVelocityConfig.InputDataType0 = RSIDataType::RSIDataTypeDOUBLE;
86 previousVelocityConfig.InputAddress1 = userBufferAddr0;
87 previousVelocityConfig.InputDataType1 = RSIDataType::RSIDataTypeDOUBLE;
88 previousVelocityConfig.ProcessDataType = RSIDataType::RSIDataTypeDOUBLE;
89 previousVelocityConfig.Operation = RSIMathBlockOperation::RSIMathBlockOperationMULTIPLY;
90
91 // Set the MathBlock configurations
92 controller->MathBlockConfigSet(SUBTRACTION_MATHBLOCK_INDEX, subtractionConfig);
93 controller->MathBlockConfigSet(PREVIOUS_VELOCITY_MATHBLOCK_INDEX, previousVelocityConfig);
94
95 // Wait a sample so we know the RMP is now processing the newly configured MathBlocks
96 controller->SampleWait(1);
97
98 // Set the axis to move with a very small acceleration so we can check the MathBlock is working
99 axis->AmpEnableSet(true); // Enable the motor.
100 axis->MoveVelocity(VELOCITY, ACCELERATION);
101
102 // Wait several samples so we know the RMP is now processing the move command and accelerating
103 controller->SampleWait(10);
104
105 // Keep in mind firmware velocity is in counts per sample, so we need to convert to UserUnits
106 // per second squared
107 double calculatedVelocityDelta = controller->MathBlockProcessValueGet(SUBTRACTION_MATHBLOCK_INDEX).Double;
108
109 // Reduce the velocity back to 0
110 axis->MoveVelocity(0, ACCELERATION);
111
112 axis->MotionDoneWait(); // Wait for the axis to finish moving.
113 axis->AmpEnableSet(false); // Disable the motor.
114
115 // Convert to UserUnits per second squared
116 double calculatedAcceleration = calculatedVelocityDelta * controller->SampleRateGet() * controller->SampleRateGet() / axis->UserUnitsGet();
117 std::cout << "Calculated acceleration from MathBlock: " << calculatedAcceleration << std::endl;
118
119 // Check that the newly calculated acceleration is as expected
120 if (std::abs(calculatedAcceleration - ACCELERATION) <= 0.000001)
121 {
122 std::cout << "The MathBlock is calculating the Axis' acceleration by subtracting previous velocity from current velocity." << std::endl;
123 exitCode = 0;
124 }
125 else
126 {
127 std::cerr << "Error: The calculated acceleration does not match the expected value" << std::endl;
128 exitCode = -1;
129 }
130 }
131 catch (const std::exception &ex)
132 {
133 std::cerr << ex.what() << std::endl;
134 exitCode = -1;
135 }
136 // Delete the controller as the program exits to ensure memory is deallocated in the correct order
137 controller->Delete();
139
140 // Print a message to indicate the sample app has finished and if it was successful or not
141 SampleAppsHelper::PrintFooter(SAMPLE_APP_NAME, exitCode);
142
143 return exitCode;
144}
uint64_t AddressGet(RSIAxisAddressType addressType)
Get the an address for some location on the Axis.
double UserUnitsGet()
Get the number of counts per User Unit.
void MoveVelocity(double velocity)
Represents a single axis of motion control. This class provides an interface for commanding motion,...
Definition rsi.h:5513
Axis * AxisGet(int32_t axisNumber)
AxisGet returns a pointer to an Axis object and initializes its internals.
void MathBlockCountSet(int32_t mathBlockCount)
Set the number of processed MathBlocks in the MotionController.
void MemoryDoubleSet(uint64_t address, double dataDouble)
Write a 64-bit double value to controller memory.
uint64_t AddressGet(RSIControllerAddressType type)
Get the an address for some location on the MotionController.
static MotionController * Create()
Initialize and start the RMP EtherCAT controller.
void SampleWait(uint32_t samples)
Wait for controller firmware to execute samples.
void Delete(void)
Delete the MotionController and all its objects.
Represents the RMP soft motion controller. This class provides an interface to general controller con...
Definition rsi.h:794
MathBlockConfig MathBlockConfigGet(int32_t mathBlockNumber)
Get a MathBlock configuration.
FirmwareValue MathBlockProcessValueGet(int32_t mathBlockNumber)
Get a MathBlock process value.
void MathBlockConfigSet(int32_t mathBlockNumber, MathBlockConfig &config)
Set a MathBlock configuration.
void ClearFaults()
Clear all faults for an Axis or MultiAxis.
void AmpEnableSet(bool enable)
Enable all amplifiers.
void Abort()
Abort an axis.
int32_t MotionDoneWait()
Waits for a move to complete.
RSIAxisAddressType
Used to get firmware address used in User Limits, Recorders, etc.
Definition rsienums.h:433
static void PrintFooter(std::string sampleAppName, int exitCode)
Print a message to indicate the sample app has finished and if it was successful or not.
static void CheckErrors(RapidCodeObject *rsiObject)
Checks for errors in the given RapidCodeObject and throws an exception if any non-warning errors are ...
static void PrintHeader(std::string sampleAppName)
Print a start message to indicate that the sample app has started.
static void SetupController(MotionController *controller)
Setup the controller with user defined axis counts and configuration.
static MotionController::CreationParameters GetCreationParameters()
Returns a MotionController::CreationParameters object with user-defined parameters.
CreationParameters for MotionController::Create.
Definition rsi.h:855
RSIDataType ProcessDataType
Data type for processing.
Definition rsi.h:3642
MathBlock configuration structure.
Definition rsi.h:3635
uint64_t InputAddress0
Host memory address for Input0. Represents the left-hand side operand in math operations.
Definition rsi.h:3636
RSIDataType InputDataType0
Data type for Input0. This is the data type of the left-hand side operand in math operations.
Definition rsi.h:3637
uint64_t InputAddress1
Host memory address for Input1. Represents the right-hand side operand in math operations.
Definition rsi.h:3638
RSIDataType InputDataType1
Data type for Input1. This is the data type of the right-hand side operand in math operations.
Definition rsi.h:3639
RSIMathBlockOperation Operation
Math operation to be performed. (+, -, *, /, etc) use RSIMathBlockOperationNONE to disable a MathBloc...
Definition rsi.h:3643
static constexpr int AXIS_X_INDEX
Index of the first axis to use in the sample apps.
static constexpr int MATH_BLOCK_COUNT
Number of math block objects to configure on the controller.
double Double
Double precision (64-bit) floating-point.
Definition rsi.h:471