Motion: Gantry

Gantry sample application.

This sample will configure two parallel axes (X1, X2) connected to the same load, and configure them so that X1 will be used to command linear motion, and X2 will be used to command yaw motion.

Precondition

This sample code presumes that the user has set the tuning paramters(PID, PIV, etc.) prior to running this program so that the motor can rotate in a stable manner.

Warning

This is a sample program to assist in the integration of the RMP motion controller with your application. It may not contain all of the logic and safety features that your application requires.

 
#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);
 
 
 
}