#include "rsi.h"
#include "HelperFunctions.h"
#pragma region GantryDeclares
int linearAxisNumber = 0;
int yawAxisNumber = 1;
int defaultEncoderNumerator = 0;
int defaultEncoderDenominator = 0;
int gantryEncoderNumerator = 1;
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)
{
linearAxis->
EncoderRatioSet(RSIMotorFeedback::RSIMotorFeedbackPRIMARY, gantryEncoderNumerator, gantryEncoderDenominator);
yawAxis->
EncoderRatioSet(RSIMotorFeedback::RSIMotorFeedbackPRIMARY, gantryEncoderNumerator, gantryEncoderDenominator);
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);
linearAxis->
FeedbackPointerSet(RSIAxisPositionInput::RSIAxisPositionInputFIRST, x1EncoderAddress);
linearAxis->
GantryTypeSet(RSIAxisGantryType::RSIAxisGantryTypeNONE);
yawAxis->
FeedbackPointerSet(RSIAxisPositionInput::RSIAxisPositionInputFIRST, x2EncoderAddress);
yawAxis->
GantryTypeSet(RSIAxisGantryType::RSIAxisGantryTypeNONE);
}
}
void SetupFilterMixing(bool enableGantry)
{
if (enableGantry)
{
mc->
MemoryDoubleSet(x1FilterSecondaryCoefficientAddress, x1SecondaryCoeff);
mc->
MemoryDoubleSet(x2FilterSecondaryCoefficientAddress, x2SecondaryCoeff);
}
else
{
mc->
MemoryDoubleSet(x1FilterPrimaryCoefficientAddress, defaultPrimaryCoeff);
mc->
MemoryDoubleSet(x1FilterSecondaryCoefficientAddress, defaultSecondaryCoeff);
mc->
MemoryDoubleSet(x2FilterPrimaryCoefficientAddress, defaultPrimaryCoeff);
mc->
MemoryDoubleSet(x2FilterSecondaryCoefficientAddress, defaultSecondaryCoeff);
}
}
void GantryEnable(bool enable)
{
ReadAddressesFromMotionController();
SetupEncoderMixing(enable);
SetupFilterMixing(enable);
}
#pragma endregion
void gantryMain()
{
linearAxis = mc->
AxisGet(linearAxisNumber);
yawAxis = mc->
AxisGet(yawAxisNumber);
ReadAddressesFromMotionController();
GantryEnable(true);
GantryEnable(false);
}