The RMP Motion Controller APIs
Gantry.cpp
#include "rsi.h" // Import our RapidCode Library.
#include "HelperFunctions.h" // Import our SampleApp helper functions.
using namespace RSI::RapidCode;
#pragma region GantryDeclares
Axis *linearAxis;
Axis *yawAxis;
int linearAxisNumber = 0;
int yawAxisNumber = 1;
int defaultEncoderNumerator = 0; // 0 disables
int defaultEncoderDenominator = 0;
int gantryEncoderNumerator = 1; // we want 1/2
int gantryEncoderDenominator = 2;
double x1PrimaryCoeff = 1.0;
double x2PrimaryCoeff = 1.0;
double x1SecondaryCoeff = 1.0;
double x2SecondaryCoeff = -1.0;
double defaultPrimaryCoeff = 1.0;
double defaultSecondaryCoeff = 0.0;
uint64_t x1EncoderAddress;
uint64_t x2EncoderAddress;
uint64_t x1FilterPrimaryPointerAddress;
uint64_t x1FilterSecondaryPointerAddress;
uint64_t x2FilterPrimaryPointerAddress;
uint64_t x2FilterSecondaryPointerAddress;
uint64_t x1FilterPrimaryCoefficientAddress;
uint64_t x1FilterSecondaryCoefficientAddress;
uint64_t x2FilterPrimaryCoefficientAddress;
uint64_t x2FilterSecondaryCoefficientAddress;
uint64_t x1AxisLinkAddress;
uint64_t x2AxisLinkAddress;
#pragma endregion
#pragma region GantryMethods
void ReadAddressesFromMotionController()
{
x1EncoderAddress = linearAxis->AddressGet(RSIAxisAddressType::RSIAxisAddressTypeENCODER_PRIMARY);
x2EncoderAddress = yawAxis->AddressGet(RSIAxisAddressType::RSIAxisAddressTypeENCODER_PRIMARY);
x1FilterPrimaryPointerAddress = linearAxis->AddressGet(RSIAxisAddressType::RSIAxisAddressTypeFILTER_PRIMARY_POINTER);
x1FilterSecondaryPointerAddress = linearAxis->AddressGet(RSIAxisAddressType::RSIAxisAddressTypeFILTER_SECONDARY_POINTER);
x2FilterPrimaryPointerAddress = yawAxis->AddressGet(RSIAxisAddressType::RSIAxisAddressTypeFILTER_PRIMARY_POINTER);
x2FilterSecondaryPointerAddress = yawAxis->AddressGet(RSIAxisAddressType::RSIAxisAddressTypeFILTER_SECONDARY_POINTER);
x1FilterPrimaryCoefficientAddress = linearAxis->AddressGet(RSIAxisAddressType::RSIAxisAddressTypeFILTER_PRIMARY_COEFF);
x1FilterSecondaryCoefficientAddress = linearAxis->AddressGet(RSIAxisAddressType::RSIAxisAddressTypeFILTER_SECONDARY_COEFF);
x2FilterPrimaryCoefficientAddress = yawAxis->AddressGet(RSIAxisAddressType::RSIAxisAddressTypeFILTER_PRIMARY_COEFF);
x2FilterSecondaryCoefficientAddress = yawAxis->AddressGet(RSIAxisAddressType::RSIAxisAddressTypeFILTER_SECONDARY_COEFF);
x1AxisLinkAddress = linearAxis->AddressGet(RSIAxisAddressType::RSIAxisAddressTypeAXIS_LINK);
x2AxisLinkAddress = yawAxis->AddressGet(RSIAxisAddressType::RSIAxisAddressTypeAXIS_LINK);
}
void SetupEncoderMixing(bool enableGantry)
{
if (enableGantry)
{
// first scale encoders by half
linearAxis->EncoderRatioSet(RSIMotorFeedback::RSIMotorFeedbackPRIMARY, gantryEncoderNumerator, gantryEncoderDenominator);
yawAxis->EncoderRatioSet(RSIMotorFeedback::RSIMotorFeedbackPRIMARY, gantryEncoderNumerator, gantryEncoderDenominator);
mc->OS->Sleep(10);
// mix encoders (add on linear, subtract for yaw)
linearAxis->FeedbackPointerSet(RSIAxisPositionInput::RSIAxisPositionInputFIRST, x1EncoderAddress);
linearAxis->GantryTypeSet(RSIAxisGantryType::RSIAxisGantryTypeADD);
yawAxis->FeedbackPointerSet(RSIAxisPositionInput::RSIAxisPositionInputFIRST, x1EncoderAddress);
}
else
{
linearAxis->EncoderRatioSet(RSIMotorFeedback::RSIMotorFeedbackPRIMARY, defaultEncoderNumerator, defaultEncoderDenominator);
yawAxis->EncoderRatioSet(RSIMotorFeedback::RSIMotorFeedbackPRIMARY, defaultEncoderNumerator, defaultEncoderDenominator);
mc->OS->Sleep(10);
linearAxis->FeedbackPointerSet(RSIAxisPositionInput::RSIAxisPositionInputFIRST, x1EncoderAddress);
linearAxis->GantryTypeSet(RSIAxisGantryType::RSIAxisGantryTypeNONE);
yawAxis->FeedbackPointerSet(RSIAxisPositionInput::RSIAxisPositionInputFIRST, x2EncoderAddress);
yawAxis->GantryTypeSet(RSIAxisGantryType::RSIAxisGantryTypeNONE);
}
}
void SetupFilterMixing(bool enableGantry)
{
if (enableGantry)
{
// mix X1 filter
mc->MemorySet(x1FilterPrimaryPointerAddress, mc->FirmwareAddressGet(x1AxisLinkAddress));
mc->MemorySet(x1FilterSecondaryPointerAddress, mc->FirmwareAddressGet(x2AxisLinkAddress));
// mix X2 filter
mc->MemorySet(x2FilterPrimaryPointerAddress, mc->FirmwareAddressGet(x1AxisLinkAddress));
mc->MemorySet(x2FilterSecondaryPointerAddress, mc->FirmwareAddressGet(x2AxisLinkAddress));
// setup X1 filter mixing coefficients
mc->MemoryDoubleSet(x1FilterPrimaryCoefficientAddress, x1PrimaryCoeff);
mc->MemoryDoubleSet(x1FilterSecondaryCoefficientAddress, x1SecondaryCoeff);
// setup X2 filter mixing coefficients
mc->MemoryDoubleSet(x2FilterPrimaryCoefficientAddress, x2PrimaryCoeff);
mc->MemoryDoubleSet(x2FilterSecondaryCoefficientAddress, x2SecondaryCoeff);
}
else
{
// unmix X1 filter
mc->MemorySet(x1FilterPrimaryPointerAddress, mc->FirmwareAddressGet(x1AxisLinkAddress));
mc->MemorySet(x1FilterSecondaryPointerAddress, mc->FirmwareAddressGet(x1AxisLinkAddress));
// unmix X2 filter
mc->MemorySet(x2FilterPrimaryPointerAddress, mc->FirmwareAddressGet(x2AxisLinkAddress));
mc->MemorySet(x2FilterSecondaryPointerAddress, mc->FirmwareAddressGet(x2AxisLinkAddress));
// setup X1 filter defult coefficients
mc->MemoryDoubleSet(x1FilterPrimaryCoefficientAddress, defaultPrimaryCoeff);
mc->MemoryDoubleSet(x1FilterSecondaryCoefficientAddress, defaultSecondaryCoeff);
// setup X2 filter default coefficients
mc->MemoryDoubleSet(x2FilterPrimaryCoefficientAddress, defaultPrimaryCoeff);
mc->MemoryDoubleSet(x2FilterSecondaryCoefficientAddress, defaultSecondaryCoeff);
}
}
void GantryEnable(bool enable)
{
ReadAddressesFromMotionController();
linearAxis->Abort();
yawAxis->Abort();
SetupEncoderMixing(enable);
SetupFilterMixing(enable);
linearAxis->ClearFaults();
linearAxis->AmpEnableSet(true);
yawAxis->ClearFaults();
yawAxis->AmpEnableSet(true);
}
#pragma endregion
void gantryMain()
{
// Create and initialize MotionController
// Get Axis X0 and X1 respectively.
linearAxis = mc->AxisGet(linearAxisNumber);
yawAxis = mc->AxisGet(yawAxisNumber);
//Only need once
ReadAddressesFromMotionController();
//Enable when desired.
GantryEnable(true);
//Disable when finished.
GantryEnable(false);
}
RSI::RapidCode::MotionController::MemoryDoubleSet
void MemoryDoubleSet(uint64_t address, double dataDouble)
Write a 64-bit double value to controller memory.
RSI::RapidCode::MotionController::OS
RapidCodeOS * OS
Provides access to operating system (Windows) features.
Definition: rsi.h:3781
RSI::RapidCode::RapidCodeOS::Sleep
void Sleep(int32_t milliseconds)
Put the current thread to sleep.
RSI::RapidCode::RSIAxisPositionInput::RSIAxisPositionInputFIRST
@ RSIAxisPositionInputFIRST
Primary encoder.
RSI::RapidCode::RapidCodeMotion::ClearFaults
void ClearFaults()
Clear all faults for an Axis or MultiAxis.
RSI::RapidCode::Axis::FeedbackPointerSet
void FeedbackPointerSet(RSIAxisPositionInput input, uint64_t address)
Configure an axis for dual loop feedback.
RSI::RapidCode::MotionController::CreateFromSoftware
static MotionController * CreateFromSoftware()
Initialize and start the RMP EtherCAT controller.
RSI::RapidCode::Axis::GantryTypeSet
void GantryTypeSet(RSIAxisGantryType type)
Set the gantry type.
RSI::RapidCode
RSI::RapidCode::MotionController::MemorySet
void MemorySet(uint64_t address, int32_t data)
Write a value to controller memory.
RSI::RapidCode::MotionController::AxisGet
Axis * AxisGet(int32_t axisNumber)
AxisGet returns a pointer to an Axis object and initializes its internals.
RSI::RapidCode::RapidCodeMotion::Abort
void Abort()
Abort an axis.
RSI::RapidCode::RSIAxisGantryType::RSIAxisGantryTypeNONE
@ RSIAxisGantryTypeNONE
RSI::RapidCode::MotionController
The MotionController object represents the RMP INtime soft motion controller.
Definition: rsi.h:770
RSI::RapidCode::Axis
The Axis object manages a single physical axis on a motion controller.
Definition: rsi.h:6118
RSI::RapidCode::RapidCodeMotion::AmpEnableSet
void AmpEnableSet(bool enable)
Enable all amplifiers.
RSI::RapidCode::Axis::AddressGet
uint64_t AddressGet(RSIAxisAddressType addressType)
Get the an address for some location on the Axis.
HelperFunctions.CheckErrors
static void CheckErrors(RapidCodeObject rsiObject)
Check if the RapidCode Object has any errors.
Definition: HelperFunctions.cs:64
RSI::RapidCode::Axis::EncoderRatioSet
void EncoderRatioSet(RSIMotorFeedback encoder, int32_t numerator, int32_t denominator)
Sets the ratio to scale Encoder counts.
RSI::RapidCode::MotionController::FirmwareAddressGet
uint32_t FirmwareAddressGet(uint64_t hostAddress)
Convert a host controller address to a firmware address.