Gantry.cpp

 
#include "rsi.h" // Import our RapidCode Library. 
#include "HelperFunctionsCpp.h"                        // Import our SampleApp helper functions. 
using namespace RSI::RapidCode;
 
#pragma region GantryDeclares
MotionController *mc;;
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->FeedbackPointerSet(RSIAxisPositionInput::RSIAxisPositionInputSECOND, x2EncoderAddress);
        linearAxis->GantryTypeSet(RSIAxisGantryType::RSIAxisGantryTypeADD);
        yawAxis->FeedbackPointerSet(RSIAxisPositionInput::RSIAxisPositionInputFIRST, x1EncoderAddress);
        yawAxis->FeedbackPointerSet(RSIAxisPositionInput::RSIAxisPositionInputSECOND, x2EncoderAddress);
        yawAxis->GantryTypeSet(RSIAxisGantryType::RSIAxisGantryTypeSUBTRACT);
    }
    else
    {
        linearAxis->EncoderRatioSet(RSIMotorFeedback::RSIMotorFeedbackPRIMARY, defaultEncoderNumerator, defaultEncoderDenominator);
        yawAxis->EncoderRatioSet(RSIMotorFeedback::RSIMotorFeedbackPRIMARY, defaultEncoderNumerator, defaultEncoderDenominator);
        mc->OS->Sleep(10);
        linearAxis->FeedbackPointerSet(RSIAxisPositionInput::RSIAxisPositionInputFIRST, x1EncoderAddress);
        linearAxis->FeedbackPointerSet(RSIAxisPositionInput::RSIAxisPositionInputSECOND, x1EncoderAddress);
        linearAxis->GantryTypeSet(RSIAxisGantryType::RSIAxisGantryTypeNONE);
        yawAxis->FeedbackPointerSet(RSIAxisPositionInput::RSIAxisPositionInputFIRST, x2EncoderAddress);
        yawAxis->FeedbackPointerSet(RSIAxisPositionInput::RSIAxisPositionInputSECOND, 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
    mc = MotionController::CreateFromSoftware();
    HelperFunctionsCpp::CheckErrors(mc);
 
    // Get Axis X0 and X1 respectively.
    linearAxis = mc->AxisGet(linearAxisNumber);
    HelperFunctionsCpp::CheckErrors(linearAxis);
 
    yawAxis = mc->AxisGet(yawAxisNumber);
    HelperFunctionsCpp::CheckErrors(yawAxis);
 
    //Only need once
    ReadAddressesFromMotionController();
    //Enable when desired.
    GantryEnable(true);
 
 
    //Disable when finished.
    GantryEnable(false);
 
 
 
}