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()
{
x1FilterPrimaryPointerAddress = linearAxis->AddressGet(RSIAxisAddressType::RSIAxisAddressTypeFILTER_PRIMARY_POINTER);
x1FilterSecondaryPointerAddress = linearAxis->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);
}
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)
}
else
{
linearAxis->EncoderRatioSet(RSIMotorFeedback::RSIMotorFeedbackPRIMARY, defaultEncoderNumerator, defaultEncoderDenominator);
yawAxis->EncoderRatioSet(RSIMotorFeedback::RSIMotorFeedbackPRIMARY, defaultEncoderNumerator, defaultEncoderDenominator);
mc->OS->Sleep(10);
}
}
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
SampleAppsCPP::HelperFunctions::CheckErrors(mc);
// Get Axis X0 and X1 respectively.
linearAxis = mc->AxisGet(linearAxisNumber);
SampleAppsCPP::HelperFunctions::CheckErrors(linearAxis);
yawAxis = mc->AxisGet(yawAxisNumber);
SampleAppsCPP::HelperFunctions::CheckErrors(yawAxis);
//Only need once
ReadAddressesFromMotionController();
//Enable when desired.
GantryEnable(true);
//Disable when finished.
GantryEnable(false);
}