APIs, concepts, guides, and more
Gantry.cpp
1
18#include "rsi.h" // Import our RapidCode Library.
19#include "SampleAppsHelper.h" // Import our SampleApp helper functions.
20using namespace RSI::RapidCode; // Import the RapidCode namespace
21
22#pragma region GantryDeclares
24Axis *linearAxis;
25Axis *yawAxis;
26
27int linearAxisNumber = 0;
28int yawAxisNumber = 1;
29
30int defaultEncoderNumerator = 0; // 0 disables
31int defaultEncoderDenominator = 0;
32int gantryEncoderNumerator = 1; // we want 1/2
33int gantryEncoderDenominator = 2;
34
35double x1PrimaryCoeff = 1.0;
36double x2PrimaryCoeff = 1.0;
37double x1SecondaryCoeff = 1.0;
38double x2SecondaryCoeff = -1.0;
39double defaultPrimaryCoeff = 1.0;
40double defaultSecondaryCoeff = 0.0;
41
42uint64_t x1EncoderAddress;
43uint64_t x2EncoderAddress;
44uint64_t x1FilterPrimaryPointerAddress;
45uint64_t x1FilterSecondaryPointerAddress;
46uint64_t x2FilterPrimaryPointerAddress;
47uint64_t x2FilterSecondaryPointerAddress;
48uint64_t x1FilterPrimaryCoefficientAddress;
49uint64_t x1FilterSecondaryCoefficientAddress;
50uint64_t x2FilterPrimaryCoefficientAddress;
51uint64_t x2FilterSecondaryCoefficientAddress;
52uint64_t x1AxisLinkAddress;
53uint64_t x2AxisLinkAddress;
54#pragma endregion
55
56#pragma region GantryMethods
57void ReadAddressesFromMotionController()
58{
59 x1EncoderAddress = linearAxis->AddressGet(RSIAxisAddressType::RSIAxisAddressTypeENCODER_PRIMARY);
60 x2EncoderAddress = yawAxis->AddressGet(RSIAxisAddressType::RSIAxisAddressTypeENCODER_PRIMARY);
61 x1FilterPrimaryPointerAddress = linearAxis->AddressGet(RSIAxisAddressType::RSIAxisAddressTypeFILTER_PRIMARY_POINTER);
62 x1FilterSecondaryPointerAddress = linearAxis->AddressGet(RSIAxisAddressType::RSIAxisAddressTypeFILTER_SECONDARY_POINTER);
63 x2FilterPrimaryPointerAddress = yawAxis->AddressGet(RSIAxisAddressType::RSIAxisAddressTypeFILTER_PRIMARY_POINTER);
64 x2FilterSecondaryPointerAddress = yawAxis->AddressGet(RSIAxisAddressType::RSIAxisAddressTypeFILTER_SECONDARY_POINTER);
65 x1FilterPrimaryCoefficientAddress = linearAxis->AddressGet(RSIAxisAddressType::RSIAxisAddressTypeFILTER_PRIMARY_COEFF);
66 x1FilterSecondaryCoefficientAddress = linearAxis->AddressGet(RSIAxisAddressType::RSIAxisAddressTypeFILTER_SECONDARY_COEFF);
67 x2FilterPrimaryCoefficientAddress = yawAxis->AddressGet(RSIAxisAddressType::RSIAxisAddressTypeFILTER_PRIMARY_COEFF);
68 x2FilterSecondaryCoefficientAddress = yawAxis->AddressGet(RSIAxisAddressType::RSIAxisAddressTypeFILTER_SECONDARY_COEFF);
69 x1AxisLinkAddress = linearAxis->AddressGet(RSIAxisAddressType::RSIAxisAddressTypeAXIS_LINK);
70 x2AxisLinkAddress = yawAxis->AddressGet(RSIAxisAddressType::RSIAxisAddressTypeAXIS_LINK);
71}
72
73void SetupEncoderMixing(bool enableGantry)
74{
75 if (enableGantry)
76 {
77 // first scale encoders by half
78 linearAxis->EncoderRatioSet(RSIMotorFeedback::RSIMotorFeedbackPRIMARY, gantryEncoderNumerator, gantryEncoderDenominator);
79 yawAxis->EncoderRatioSet(RSIMotorFeedback::RSIMotorFeedbackPRIMARY, gantryEncoderNumerator, gantryEncoderDenominator);
80 mc->OS->Sleep(10);
81 // mix encoders (add on linear, subtract for yaw)
82 linearAxis->FeedbackPointerSet(RSIAxisPositionInput::RSIAxisPositionInputFIRST, x1EncoderAddress);
83 linearAxis->FeedbackPointerSet(RSIAxisPositionInput::RSIAxisPositionInputSECOND, x2EncoderAddress);
84 linearAxis->GantryTypeSet(RSIAxisGantryType::RSIAxisGantryTypeADD);
85 yawAxis->FeedbackPointerSet(RSIAxisPositionInput::RSIAxisPositionInputFIRST, x1EncoderAddress);
86 yawAxis->FeedbackPointerSet(RSIAxisPositionInput::RSIAxisPositionInputSECOND, x2EncoderAddress);
87 yawAxis->GantryTypeSet(RSIAxisGantryType::RSIAxisGantryTypeSUBTRACT);
88 }
89 else
90 {
91 linearAxis->EncoderRatioSet(RSIMotorFeedback::RSIMotorFeedbackPRIMARY, defaultEncoderNumerator, defaultEncoderDenominator);
92 yawAxis->EncoderRatioSet(RSIMotorFeedback::RSIMotorFeedbackPRIMARY, defaultEncoderNumerator, defaultEncoderDenominator);
93 mc->OS->Sleep(10);
94 linearAxis->FeedbackPointerSet(RSIAxisPositionInput::RSIAxisPositionInputFIRST, x1EncoderAddress);
95 linearAxis->FeedbackPointerSet(RSIAxisPositionInput::RSIAxisPositionInputSECOND, x1EncoderAddress);
96 linearAxis->GantryTypeSet(RSIAxisGantryType::RSIAxisGantryTypeNONE);
97 yawAxis->FeedbackPointerSet(RSIAxisPositionInput::RSIAxisPositionInputFIRST, x2EncoderAddress);
98 yawAxis->FeedbackPointerSet(RSIAxisPositionInput::RSIAxisPositionInputSECOND, x2EncoderAddress);
99 yawAxis->GantryTypeSet(RSIAxisGantryType::RSIAxisGantryTypeNONE);
100 }
101}
102void SetupFilterMixing(bool enableGantry)
103{
104 if (enableGantry)
105 {
106 // mix X1 filter
107 mc->MemorySet(x1FilterPrimaryPointerAddress, mc->FirmwareAddressGet(x1AxisLinkAddress));
108 mc->MemorySet(x1FilterSecondaryPointerAddress, mc->FirmwareAddressGet(x2AxisLinkAddress));
109 // mix X2 filter
110 mc->MemorySet(x2FilterPrimaryPointerAddress, mc->FirmwareAddressGet(x1AxisLinkAddress));
111 mc->MemorySet(x2FilterSecondaryPointerAddress, mc->FirmwareAddressGet(x2AxisLinkAddress));
112
113 // setup X1 filter mixing coefficients
114 mc->MemoryDoubleSet(x1FilterPrimaryCoefficientAddress, x1PrimaryCoeff);
115 mc->MemoryDoubleSet(x1FilterSecondaryCoefficientAddress, x1SecondaryCoeff);
116 // setup X2 filter mixing coefficients
117 mc->MemoryDoubleSet(x2FilterPrimaryCoefficientAddress, x2PrimaryCoeff);
118 mc->MemoryDoubleSet(x2FilterSecondaryCoefficientAddress, x2SecondaryCoeff);
119 }
120 else
121 {
122 // unmix X1 filter
123 mc->MemorySet(x1FilterPrimaryPointerAddress, mc->FirmwareAddressGet(x1AxisLinkAddress));
124 mc->MemorySet(x1FilterSecondaryPointerAddress, mc->FirmwareAddressGet(x1AxisLinkAddress));
125 // unmix X2 filter
126 mc->MemorySet(x2FilterPrimaryPointerAddress, mc->FirmwareAddressGet(x2AxisLinkAddress));
127 mc->MemorySet(x2FilterSecondaryPointerAddress, mc->FirmwareAddressGet(x2AxisLinkAddress));
128 // setup X1 filter defult coefficients
129 mc->MemoryDoubleSet(x1FilterPrimaryCoefficientAddress, defaultPrimaryCoeff);
130 mc->MemoryDoubleSet(x1FilterSecondaryCoefficientAddress, defaultSecondaryCoeff);
131 // setup X2 filter default coefficients
132 mc->MemoryDoubleSet(x2FilterPrimaryCoefficientAddress, defaultPrimaryCoeff);
133 mc->MemoryDoubleSet(x2FilterSecondaryCoefficientAddress, defaultSecondaryCoeff);
134 }
135}
136void GantryEnable(bool enable)
137{
138 ReadAddressesFromMotionController();
139 linearAxis->Abort();
140 yawAxis->Abort();
141 SetupEncoderMixing(enable);
142 SetupFilterMixing(enable);
143 linearAxis->ClearFaults();
144 linearAxis->AmpEnableSet(true);
145 yawAxis->ClearFaults();
146 yawAxis->AmpEnableSet(true);
147}
148#pragma endregion
149
150void gantryMain()
151{
152
153 // Create and initialize MotionController
156
157 // Get Axis X0 and X1 respectively.
158 linearAxis = mc->AxisGet(linearAxisNumber);
160
161 yawAxis = mc->AxisGet(yawAxisNumber);
163
164 //Only need once
165 ReadAddressesFromMotionController();
166 //Enable when desired.
167 GantryEnable(true);
168
172
173 //Disable when finished.
174 GantryEnable(false);
175
176
177
178}
uint64_t AddressGet(RSIAxisAddressType addressType)
Get the an address for some location on the Axis.
void FeedbackPointerSet(RSIAxisPositionInput input, uint64_t address)
Configure an axis for dual loop feedback.
void EncoderRatioSet(RSIMotorFeedback encoder, int32_t numerator, int32_t denominator)
Sets the ratio to scale Encoder counts.
Represents a single axis of motion control. This class provides an interface for commanding motion,...
Definition rsi.h:5513
void GantryTypeSet(RSIAxisGantryType type)
Set the gantry type.
Axis * AxisGet(int32_t axisNumber)
AxisGet returns a pointer to an Axis object and initializes its internals.
void MemorySet(uint64_t address, int32_t data)
Write a value to controller memory.
uint32_t FirmwareAddressGet(uint64_t hostAddress)
Convert a host controller address to a firmware address.
void MemoryDoubleSet(uint64_t address, double dataDouble)
Write a 64-bit double value to controller memory.
static MotionController * CreateFromSoftware()
Initialize and start the RMP EtherCAT controller.
Represents the RMP soft motion controller. This class provides an interface to general controller con...
Definition rsi.h:794
RapidCodeOS * OS
Provides access to operating system (Windows) features.
Definition rsi.h:3664
void ClearFaults()
Clear all faults for an Axis or MultiAxis.
void AmpEnableSet(bool enable)
Enable all amplifiers.
void Abort()
Abort an axis.
void Sleep(int32_t milliseconds)
Put the current thread to sleep.
static void CheckErrors(RapidCodeObject *rsiObject)
Checks for errors in the given RapidCodeObject and throws an exception if any non-warning errors are ...