Drive Data Capture

// Drive Data Capture Example:
// Demonstrates how to configure two different Drive Data Capture configurations on one axis, plot
// the data stored in the Drive Array by Drive Data Capture, and put that data into a text file.
// The two configurations use the same trigger (a PSO event), but have different inputs (primary
// feedback and auxiliary feedback). Data collection is configured programmatically to collect the
// applicable signals in a plot. The plot should show that the two axes are moving and the data
// read back from the Drive Array is being assigned into global variables $rglobal[0] and
// $rglobal[1].
//
// Hardware setup:
// X Axis - An XC4e connected to a motor with square-wave feedback.
// Y Axis - An XC4 connected to a motor with square-wave feedback.
// The auxiliary feedback pins of the two drives are connected to let the second axis echo its
// encoder signals to the first.
// Note: AuxiliaryFeedbackType on X should be changed to "IncrementalEncoderSquareWave".

// These defines are used by this program for Drive Data Capture. The starting address of the first
// array is arbitrary. The starting address of the second array is the byte address after the end
// of the first array. Because the Drive Data Capture positions are 8 bytes, the size of the array
// is DRIVE_DATA_CAPTURE_NUM_SAMPLES * 8 bytes.
#define DRIVE_DATA_CAPTURE_NUM_SAMPLES                2000
#define DRIVE_DATA_CAPTURE_FIRST_ARRAY_START_ADDRESS  800
#define DRIVE_DATA_CAPTURE_SECOND_ARRAY_START_ADDRESS (DRIVE_DATA_CAPTURE_FIRST_ARRAY_START_ADDRESS + (DRIVE_DATA_CAPTURE_NUM_SAMPLES * 8))

// These defines are used by this program for data collection.
#define DATA_COLLECTION_SAMPLE_TIME_MSEC              1     // Collect data at 1 kHz
#define DATA_COLLECTION_NUM_SAMPLES                   10000 // Collect up to 10 seconds worth of data

// These are other defines used by this program.
#define STRING_SIZE                                   1000

// Declare a global variable for the program to use.
var $driveDataCaptureSamples[2][DRIVE_DATA_CAPTURE_NUM_SAMPLES] as real

program
	var $axes[2] as axis = [X, Y]

	// Clear the controller global variables used by this program.
	$iglobal[0] = 0
	$rglobal[0] = 0
	$rglobal[1] = 0

	// Enable and home the axes before the PSO is configured.
	Enable($axes)
	Home($axes)
	Dwell(0.5)

	PsoSetup($axes, 50)

	DataCollectionSetup($axes[0])

	// The drive array might contain old data if this program runs many times without a controller
	// resest. To clear old data, write zeros to the drive array space allocated to Drive Data
	// Capture. The $driveDataCaptureSamples variable is still all zeros at this time.
	DriveArrayWrite($axes[0], $driveDataCaptureSamples[0], DRIVE_DATA_CAPTURE_FIRST_ARRAY_START_ADDRESS, DRIVE_DATA_CAPTURE_NUM_SAMPLES, DriveArrayType.DataCapturePositions)
	DriveArrayWrite($axes[0], $driveDataCaptureSamples[1], DRIVE_DATA_CAPTURE_SECOND_ARRAY_START_ADDRESS, DRIVE_DATA_CAPTURE_NUM_SAMPLES, DriveArrayType.DataCapturePositions)

	// Configure Drive Data Capture to use the PSO event as the trigger. To make sure each captured
	// value is the feedback value from precisely when the PSO event occurred, use the same PSO
	// event to trigger the two captures. Because the second axis does not have access to the PSO
	// event on the first axis, the program must use two configurations on one axis. It cannot use
	// one configuration on each of the two axes.
	DriveDataCaptureReset($axes[0], 0)
	DriveDataCaptureConfigureTrigger($axes[0], 0, DriveDataCaptureTrigger.PsoEvent)
	DriveDataCaptureConfigureInput($axes[0], 0, DriveDataCaptureInput.PrimaryFeedback)
	DriveDataCaptureConfigureArray($axes[0], 0, DRIVE_DATA_CAPTURE_FIRST_ARRAY_START_ADDRESS, DRIVE_DATA_CAPTURE_NUM_SAMPLES)
	DriveDataCaptureOn($axes[0], 0)
	DriveDataCaptureReset($axes[0], 1)
	DriveDataCaptureConfigureTrigger($axes[0], 1, DriveDataCaptureTrigger.PsoEvent)
	DriveDataCaptureConfigureInput($axes[0], 1, DriveDataCaptureInput.AuxiliaryFeedback)
	DriveDataCaptureConfigureArray($axes[0], 1, DRIVE_DATA_CAPTURE_SECOND_ARRAY_START_ADDRESS, DRIVE_DATA_CAPTURE_NUM_SAMPLES)
	DriveDataCaptureOn($axes[0], 1)

	AppDataCollectionSnapshot()

	// Move the axes forward. Expect a PSO Distance event every 50 counts.
	MoveLinear($axes, [CountsToUnits($axes[0], 49500), CountsToUnits($axes[1], 49500)], CountsToUnits($axes[0], 50000))

	// Process the data in the drive array from Drive Data Capture. Use global variables to plot the
	// data. Put the data in a text file.
	DriveDataCaptureProcessing($axes[0])

	Cleanup($axes)
end

// Configure a set of data collection signals.
function DataCollectionSetup($axis as axis)
	AppDataCollectionReset()

	AppDataCollectionConfigure(DATA_COLLECTION_NUM_SAMPLES, DATA_COLLECTION_SAMPLE_TIME_MSEC)

	// Collect the primary and auxiliary feedback to make sure that the first axis is tracking the
	// two axes correctly. AuxiliaryFeedbackType on X must be set to "IncrementalEncoderSquareWave"
	// for the feedback signals to be plotted correctly.
	AppDataCollectionAddAxisSignal($axis, AxisDataSignal.PositionFeedback)
	AppDataCollectionAddAxisSignal($axis, AxisDataSignal.AuxiliaryFeedback)
	// Collect the PSO counters because there is a PSO event when they reach the specified distance.
	AppDataCollectionAddAxisSignal($axis, AxisDataSignal.PsoCounter0)
	AppDataCollectionAddAxisSignal($axis, AxisDataSignal.PsoCounter1)
	// Collect the number of Drive Data Capture Samples to make sure that each configuration
	// captures the feedback value when a PSO event occurs.
	AppDataCollectionAddAxisSignal($axis, AxisDataSignal.DriveDataCaptureSamples, 0)
	AppDataCollectionAddAxisSignal($axis, AxisDataSignal.DriveDataCaptureSamples, 1)
	// Collect these global variables because the end of the program copies the captured values
	// into the global reals and the sample number into the global integer.
	AppDataCollectionAddSystemSignal(SystemDataSignal.GlobalInteger, 0)
	AppDataCollectionAddSystemSignal(SystemDataSignal.GlobalReal, 0)
	AppDataCollectionAddSystemSignal(SystemDataSignal.GlobalReal, 1)
end


// Configure PSO on the first axis to track feedback from each axis. Primary feedback from the
// second axis is being echoed out of the auxiliary encoder of the second axis into the auxiliary
// encoder of the first axis. This is to let the first axis to track the two axes.
function PsoSetup($axes[2] as axis, $psoEventDistance as integer)
	// Configure the second axis to echo its primary encoder signals out of the auxiliary encoder.
	DriveEncoderOutputOff($axes[1], EncoderOutputChannel.AuxiliaryEncoder)
	DriveEncoderOutputConfigureInput($axes[1], EncoderOutputChannel.AuxiliaryEncoder, EncoderInputChannel.PrimaryEncoder)
	DriveEncoderOutputOn($axes[1], EncoderOutputChannel.AuxiliaryEncoder)

	// Reset PSO before it is configured.
	PsoReset($axes[0])

	// Configure the PSO distance module to track the two axes. Primary feedback from the second
	// axis is being echoed out of the auxiliary encoder of the second axis into the auxiliary
	// encoder of the first axis.
	PsoDistanceConfigureInputs($axes[0], [PsoDistanceInput.XC4ePrimaryFeedback, PsoDistanceInput.XC4eAuxiliaryFeedback])
	PsoDistanceConfigureFixedDistance($axes[0], $psoEventDistance)
	PsoDistanceCounterOn($axes[0])
	PsoDistanceEventsOn($axes[0])

	// Configure the PSO waveform module to produce one 10 us pulse each time a PSO event occurs.
	PsoWaveformConfigurePulseFixedTotalTime($axes[0], 10)
	PsoWaveformConfigurePulseFixedOnTime($axes[0], 10)
	PsoWaveformConfigurePulseFixedCount($axes[0], 1)
	PsoWaveformApplyPulseConfiguration($axes[0])
	PsoWaveformOn($axes[0])

	// Configure the PSO output to output the waveform onto the auxiliary marker.
	PsoOutputConfigureSource($axes[0], PsoOutputSource.Waveform)
	PsoOutputConfigureOutput($axes[0], PsoOutputPin.XC4eAuxiliaryMarkerSingleEnded)
end


// Retrieve and process the valid Drive Data Capture samples from the drive array. The move was
// short enough that fewer than DRIVE_DATA_CAPTURE_NUM_SAMPLES values were captured and not all the
// memory locations in the drive array space allocated to each data capture configuration were
// used. Thus, this function should read only the memory locations in the drive array that have
// actual (valid) feedback values.
function DriveDataCaptureProcessing($axis as axis)
	var $driveDataCaptureNumberOfSamplesTaken[2] as integer
	var $loopIndex as integer

	// Read only the valid Drive Data Capture samples from the drive array.
	$driveDataCaptureNumberOfSamplesTaken[0] = DriveGetItem($axis, DriveItem.DriveDataCaptureSamples, 0)
	$driveDataCaptureNumberOfSamplesTaken[1] = DriveGetItem($axis, DriveItem.DriveDataCaptureSamples, 1)
	DriveArrayRead($axis, $driveDataCaptureSamples[0], DRIVE_DATA_CAPTURE_FIRST_ARRAY_START_ADDRESS, $driveDataCaptureNumberOfSamplesTaken[0], DriveArrayType.DataCapturePositions)
	DriveArrayRead($axis, $driveDataCaptureSamples[1], DRIVE_DATA_CAPTURE_SECOND_ARRAY_START_ADDRESS, $driveDataCaptureNumberOfSamplesTaken[1], DriveArrayType.DataCapturePositions)

	// This loop is in a critical section to make sure all the global variables are assigned their
	// values at the same time.
	CriticalSectionStart()

	// Loop over the data capture samples, assigning them into global variables that are being
	// plotted. This is for convenience because program variables cannot be plotted.
	for $loopIndex = 0 to (DRIVE_DATA_CAPTURE_NUM_SAMPLES - 1)
		// Set $iglobal[0] to the sample number.
		$iglobal[0] = $loopIndex

		// Set $rglobal[0] to the value of the sample from the first Drive Data Capture configuration.
		$rglobal[0] = $driveDataCaptureSamples[0][$loopIndex]

		// Set $rglobal[1] to the value of the sample from the second Drive Data Capture configuration.
		$rglobal[1] = $driveDataCaptureSamples[1][$loopIndex]

		Dwell(0.001)
	end

	CriticalSectionEnd()

	WriteSamplesToFile()
end

// Write the samples to a text file quickly and efficiently. To decrease file operation time and
// reduce the number of calls to FileTextWriteString(), this function uses a local string variable
// as a buffer to hold a large amount of text before the function writes it to the file.
function WriteSamplesToFile()
	var $loopIndex as integer
	var $fileHandle as handle
	var $buffer as string(STRING_SIZE)

	// Open the file and write the header to it.
	$fileHandle = FileOpenText("/DataCaptureSamples.csv", FileMode.Overwrite)
	FileTextWriteString($fileHandle, "SampleNumber,FirstAxisFeedback,SecondAxisFeedback\n")

	// This loop is in a critical section to reduce the amount of time it takes to execute.
	CriticalSectionStart()

	// Loop over the data capture samples and write them to the output file.
	for $loopIndex = 0 to (DRIVE_DATA_CAPTURE_NUM_SAMPLES - 1)
		$buffer = $buffer + IntegerToString($loopIndex) + "," + IntegerToString($driveDataCaptureSamples[0][$loopIndex]) + "," + IntegerToString($driveDataCaptureSamples[1][$loopIndex]) + "\n"
		
		// Write to the file when the string buffer is at 95% of its maximum size or the loop index
		// is a multiple of 50. This prevents the critical section from causing the application to
		// stop responding.
		if ((StringLength($buffer) > (STRING_SIZE * 0.95)) || (($loopIndex % 50) == 0))
			FileTextWriteString($fileHandle, $buffer)
			$buffer = ""
		end
	end

	// Write the remaining part of the string buffer to the file.
	FileTextWriteString($fileHandle, $buffer)

	CriticalSectionEnd()

	FileClose($fileHandle)
end


// Disable the axes and PSO.
function Cleanup($axes[2] as axis)
	// Wait for data collection to finish before the program continues.
	wait (StatusGetSystemItem(SystemStatusItem.DataCollectionStatus) & DataCollectionFlags.Done)

	Disable($axes)

	PsoOutputOff($axes[0])
end