Enhanced Throughput Module (ETM)

// Enhanced Throughput Module (ETM) Setup Program:
// Collects an ETM signal via an Analog Input and the axis Control Effort into arrays. Performs a linear regression
// using those arrays and reports the resulting slope as a suggested value for the EnhancedThroughputGain parameter.

// See the help topic Enhanced Throughput Module (ETM) Setup for instructions.

// Define the data collection array size.  Data is collected with a 9 msec period.
#define NUM_POINTS (36)

// Start the main program block
program
    var $myAxis as axis   				// The axis you are configuring with the ETM
    var $inputNumber as integer 			// The ETM is connected to this analog input number, typically 0 for Aerotech systems.
    var $etmTriggerLevel as real			// The ETM signal level that will trigger data collection (Volts)
    var $triggerDelay as integer			// The delay after trigger before data collection starts (msec)
    var $etmInitialValue as real 			// The initial reading of the ETM (Volts)
    var $triggerStart as integer			// The time, in milliseconds, when the triggering event occurred
    var $timedOut as integer				// A value of 1 indicates that a Wait command timed out, 0 means timeout did not occur
    var $etmSignal[NUM_POINTS] as real			// The array of ETM data after collection is triggered (Volts)
    var $controlEffort[NUM_POINTS] as real 		// The Control Effort after collection is triggered (Amps)
    var $t[NUM_POINTS] as real				// The time signal (seconds)
    var $gain[NUM_POINTS] as real			// A record of gain values from iterative testing
    var $count1, $count2 as integer			// Loop counters
    var $str as string					// A temporary string variable for messaging
    var $writeToFile as integer				// Indicates if the test data will be written to file
    var $fHandle as handle				// A file handle object for use if data is written to file
    var $rSquaredThreshold as real			// Threshold for judging the quality of the least squares fit
    var $viewInVisualizer as integer			// A true / false value to optionally view the measurement in the Visualize workspace
    var $continueIteration as integer			// A loop exit variable
    var $avg_x as real = 0  				// In a Cartesian coordinate graph, this is the average of the horizontal coordinate
    var $avg_y as real = 0  				// In a Cartesian coordinate graph, this is the average of the vertical coordinate
    var $slope as real      				// Slope of the best fit line; m in the equation y = m*x + b
    var $intercept as real  				// Intercept of the best fit line; b in the equation y = m*x + b
    var $numerator as real = 0.0			// Numerator of slope calculation
    var $denominator as real = 0.0			// Denominator of slope calculation
    var $rSquared as real   				// Coefficient of Determination (R^2) of the best fit line; qualifies goodness of fit
    var $ss_res as real = 0.0				// Sum of squares residual error 
    var $ss_tot as real = 0.0				// Sum of squares total error
    var $response as integer				// AppMessageBox response
    var $triggerTimeout as real				// Wait time for the trigger event. The program aborts if this time is exceeded.
    var $convergencePercent as real			// Convergence criteria for iterative gain calculations.
	
    // Input variables *******************************************************************************************************
    $myAxis = Y						// Enter the axis name associated with the ETM

    // Run options: write to file, trigger settings, goodness of fit threshold, and convergence criteria
    $writeToFile = 1					// 0 or 1;  Use a value of 1 to write the test data to a file
    $viewInVisualizer = 1				// 0 or 1;  Use a value of 1 to capture and view data in the Visualize workspace
    $etmTriggerLevel = 0.1				// ETM voltage level that will trigger data collection
    $triggerTimeout = 5.0       			// Wait time, in seconds, for the trigger event 
    $triggerDelay = 50					// Delay, in milliseconds after trigger and before data collection starts
    $rSquaredThreshold = 0.9    			// Linear regressions with R^2 less than this value are deemed poorly fitted
    $convergencePercent = 2.0				// Convergence threshold for exiting the measurement iteration loop
    // ***********************************************************************************************************************
	
    // Enable the axis if it is not already enabled.
    Enable($myAxis)
	
    // Get the ETM analog input number from the EnhancedThroughputChannel parameter
    $inputNumber = ParameterGetAxisValue($myAxis, AxisParameter.EnhancedThroughputChannel)
	
    // Get the initial ETM gain value from the parameter file and set it to eliminate any temporary override that might be in place.
    $gain[0] = ConfiguredParameterGetAxisValue($myAxis, AxisParameter.EnhancedThroughputGain)
    ParameterSetAxisValue($myAxis, AxisParameter.EnhancedThroughputGain, $gain[0])
	
    // Read the initial ETM signal level
    $etmInitialValue = AnalogInputGet($myAxis, $inputNumber)
	
    // Start the iterative measurement loop
    $count1 = 1
    $continueIteration = 1
    while ($continueIteration == 1)
        // Set up data collection in the Visualizer
        if ($viewInVisualizer == 1) && ($count1 == 1)
            ConfigureDataCollection($myAxis, $inputNumber)
        end
		
        // Prompt the user to trigger the system
        $str = "1. Read these instructions completely.\n\n2. Press OK.\n\n3. Within "
        $str = $str + RealToString($triggerTimeout,RealDisplayFormat.Decimal,1)
        $str = $str + " seconds, trigger the measurement with a moderate impact to the stage base in the ETM measurement direction."
        $response = AppMessageBox("Measurement #" + IntegerToString($count1) + ", ETM Gain: " + RealToString($gain[$count1-1],RealDisplayFormat.Decimal, 3), $str, ["OK", "Cancel"],  MessageSeverity.Information, 1)
        if $response == 1
            break
        end
	
        // Start data collection in the Visualizer
        if $viewInVisualizer == 1
            AppDataCollectionSnapshot()
        end
		
        // Initialize (reset) timer zero
        TimerClear(0)
		
        // Wait for the ETM signal to exceed the trigger level; time out after the specified number of seconds
        $timedOut = wait(Abs(AnalogInputGet($myAxis, $inputNumber)) > $etmTriggerLevel, $triggerTimeout * 1000)
		
        // Read timer zero after the wait is over
        $triggerStart = TimerRead(0, TimerMode.Standard)
		
        // In the case of a time out, display a message and stop the program
        if $timedOut == 1
            $str = "Timeout waiting for a " + RealToString($etmTriggerLevel) + " V ETM triggering event."
            AppMessageBox("Timeout", $str, ["OK"], MessageSeverity.Error, 1)
            ProgramExit()
        end
		
        // Wait after the triggering event to exclude the non-linear portion of the response from data collection
        wait(TimerRead(0, TimerMode.Standard) >= ($triggerStart + $triggerDelay))
		
        // Collect data in a fast loop.  The DriveGetItem.ServoLoopControlEffort request is relatively slow
        // compared to the AnalogInputGet and TimerRead commands, so it comes first in the loop. It is also
        // the reason that this loop takes several milliseconds per cycle.
        CriticalSectionStart()
        for $count2 = 0 to (NUM_POINTS-1)
            $controlEffort[$count2] = DriveGetItem($myAxis, DriveItem.ServoLoopControlEffort)
            $etmSignal[$count2] = AnalogInputGet($myAxis, $inputNumber)
            $t[$count2] = TimerRead(0, TimerMode.Standard)/1000
            Dwell(0.001) // dwell for 1msec to prevent starvation of the real time operating system
        end
        CriticalSectionEnd()
		
        // Stop Visualizer collection
        if $viewInVisualizer == 1
            AppDataCollectionStop()
        end
		
        // Calculate the least squares fit
        // First, calculate the average "x" and "y" values
        for $count2 = 0 to (NUM_POINTS-1)
            $avg_x = $avg_x + $etmSignal[$count2]
            $avg_y = $avg_y + $controlEffort[$count2]
        end
        $avg_x = $avg_x/NUM_POINTS
        $avg_y = $avg_y/NUM_POINTS
		
        // Second, calculate the slope
        for $count2 = 0 to (NUM_POINTS-1)
            $numerator = $numerator + ($etmSignal[$count2]-$avg_x) * ($controlEffort[$count2]-$avg_y)
            $denominator = $denominator + ($etmSignal[$count2]-$avg_x) * ($etmSignal[$count2]-$avg_x)
        end
        $slope = $numerator / $denominator
        $gain[$count1] = $slope
		
        // Third, calculate the intercept
        $intercept = $avg_y - $slope * $avg_x
		
        // Fourth, calculate the Coefficient of Determination (R^2)
        for $count2 = 0 to (NUM_POINTS-1)
            $ss_res = $ss_res + ($controlEffort[$count2] - ($slope * $etmSignal[$count2] + $intercept))*($controlEffort[$count2] - ($slope * $etmSignal[$count2] + $intercept))
            $ss_tot = $ss_tot + ($controlEffort[$count2] - $avg_y) * ($controlEffort[$count2] - $avg_y)
        end
        $rSquared = 1 - $ss_res / $ss_tot
		
        // Write to a CSV file
        if $writeToFile == 1
            var $fileName as string = "etmSetupData-" + AxisToString($myAxis) + "-" + TimeStamp() + ".csv"
            var $etmName as string = "ETM (" + AxisToString($myAxis) + ")"
            var $contEffortName as string = "ContEffort (" + AxisToString($myAxis) + ")"
	
            // Open the file
            $fHandle = FileOpenText($fileName, FileMode.Overwrite)
            // Write the header row and the array data to file
            WriteToCSV($fHandle, $t, $myAxis, $etmName, $etmSignal, $contEffortName, $controlEffort)
            // Close the file
            FileClose($fHandle)
        end
		
        // Check the exit criteria. If the gain change is less than the threshold or 10 measurements have occurred, then exit.
        if (Abs(($gain[$count1] - $gain[$count1-1])/$gain[$count1]) < ($convergencePercent/100.0)) || ($count1 == 10)
            $continueIteration = 0
        end
		
        // Display a notification message about this measurement
        $str = "Measurement #" + IntegerToString($count1)+"\n"
        $str = $str + "Active ETM Gain: " + RealToString($gain[$count1-1],RealDisplayFormat.Decimal, 3) + "\n"
        $str = $str + "New ETM Gain: " + RealToString($gain[$count1],RealDisplayFormat.Decimal, 3) + "\n"
        $str = $str + "Gain Change: " + RealToString(Abs(($gain[$count1]-$gain[$count1-1])/$gain[$count1])*100,RealDisplayFormat.Decimal, 1) + "%"
        AppMessageDisplay($str)
		
        // Set the new gain in the active controller for use during the next iteration
        ParameterSetAxisValue($myAxis, AxisParameter.EnhancedThroughputGain, $slope)
		
        // Updated the iteration counter
        $count1 = $count1 + 1
        Dwell(2.0)
		
    end
	
    // Report the results if program execution was not canceled by the user.
    if $response != 1
        $str = "Calculation Results:\n\n"
        $str = $str + "Goodness of Fit (R^2) = " + RealToString($rSquared, RealDisplayFormat.Decimal, 2) + "\n\n"
        $str = $str + "In Configure > Axes > Optimization, set the " + AxisToString($myAxis) + "-axis EnhancedThroughputGain to:"
        if $rSquared >= $rSquaredThreshold
            AppMessageInputBox("Results: Good Linear Fit", $str, RealToString($slope, RealDisplayFormat.Decimal, 3), MessageSeverity.Information, 1)
        else
            AppMessageInputBox("Results: Poor Linear Fit", $str, RealToString($slope, RealDisplayFormat.Decimal, 3), MessageSeverity.Warning, 1)
        end
    end
end

// Function: WriteToCSV
// Write three columns of data to a comma separated text file located in the Controller File System. 
// To download the resulting file to the computer's operating system:
//		- In the Configure Workspace, in the Controller section, select Controller Files
//		- Find and select the file in the file list at the right
//		- Press the Download button at the middle top
// To delete the resulting file from the Controller File system:
//		- In the Configure Workspace, in the Controller section, select Controller Files
//		- Find and select the file in the file list at the right
//		- Press the Delete button at the middle top
//
//	Arguments
//		$fHandle -> the handle of the previously opened file
//		$time[] -> the first array of data, with a header of Time (s)
//		$xName -> the name of the x data, used in the header row of the text file
//		$xData[] -> an array of data
//		$yName -> the name of the y data, used in the header row of the text file
//		$ydata[] -> a second array of data
//
//	Returns
//		Nothing
//
//  NOTE: the file must be closed separately with the FileClose(handle) command.
function WriteToCSV($fHandle as handle, $time[] as real, $myAx as axis, $xName as string, $xData[] as real, $yName as string, $yData[] as real)
    var $tempStr as string
    var $ii as integer

    // Write header information
    $tempStr = "EnhancedThroughputChannel:," + IntegerToString(ParameterGetAxisValue($myAx, AxisParameter.EnhancedThroughputChannel)) + "\n"
    FileTextWriteString($fHandle, $tempStr)
    $tempStr = "EnhancedThroughputCurrentClamp:," + RealToString(ParameterGetAxisValue($myAx, AxisParameter.EnhancedThroughputCurrentClamp)) + "\n"
    FileTextWriteString($fHandle, $tempStr)
    $tempStr = "EnhancedThroughputGain:," + RealToString(ParameterGetAxisValue($myAx, AxisParameter.EnhancedThroughputGain)) + "\n"
    FileTextWriteString($fHandle, $tempStr)
    $tempStr = "Time (s)," + $xName + "," + $yName + "\n"
    FileTextWriteString($fHandle, $tempStr)
	
    // Write data table
    $ii = 0
    foreach var $d in $xData
        $tempStr = RealToString($time[$ii]) + "," + RealToString($d) + "," + RealToString($yData[$ii]) + "\n"
        FileTextWriteString($fHandle, $tempStr)
        $ii = $ii + 1
    end
end

// Function: IntegerToStringZeroPadded
// Convert integers into a string with zero padding for integers less than 10.
//
//   Arguments
//   		$value -> an integer value to be converted to a string
//
//   Returns
//			For single digit integers, returns a two-character zero-padded string
//          For multi-digit integers, returns the integer as a string
//
function IntegerToStringZeroPadded($value as integer) as string
    var $str as string
	
    if Abs($value) < 10
        $str = "0" + IntegerToString(Abs($value))
        if $value != 0
            if Abs($value)/$value < 0
                $str = "-" + $str
            end
        end
    else
        $str = IntegerToString($value)
    end
    return $str
end

// Function: TimeStamp
// Read and format the current time in a return string
//
//   Arguments
//   		None
//
//   Returns
//			The date and time as a string in 'yyyyMMddhhmmss' format
//
function TimeStamp() as string
    var $dateTimeRaw as integer = DateTimeGet()
        var $str as string
	
        // Year
        $str = IntegerToString(DateTimeExtractYear($dateTimeRaw))
        // Month
        $str = $str + IntegerToStringZeroPadded(DateTimeExtractMonth($dateTimeRaw))
        // Day
        $str = $str + IntegerToStringZeroPadded(DateTimeExtractDay($dateTimeRaw))
        // Hour
        $str = $str + IntegerToStringZeroPadded(DateTimeExtractHour($dateTimeRaw))
        // Minute
        $str = $str + IntegerToStringZeroPadded(DateTimeExtractMinute($dateTimeRaw))
        // Second
        $str = $str + IntegerToStringZeroPadded(DateTimeExtractSecond($dateTimeRaw))
	
        // Return the date-time string
        return $str
end

// Function: ConfigureDataCollection
// Set up the measurement duration and signals in the Visualizer
//
//   Arguments
//   		$myAx -> The name of the axis connected to the ETM
//			$inputNum -> The number of the analog input connected to the ETM
//
//   Returns
//			None
//
function ConfigureDataCollection($myAx as axis, $inputNum as integer)
    AppDataCollectionReset()
    AppDataCollectionConfigure(10000, 1)
    AppDataCollectionAddAxisSignal($myAx, AxisDataSignal.PositionError)
    AppDataCollectionAddAxisSignal($myAx, AxisDataSignal.ServoLoopControlEffort)
    if $inputNum == 0
        AppDataCollectionAddAxisSignal($myAx, AxisDataSignal.AnalogInput0)
    elseif $inputNum == 1
        AppDataCollectionAddAxisSignal($myAx, AxisDataSignal.AnalogInput1)
    elseif $inputNum == 2
        AppDataCollectionAddAxisSignal($myAx, AxisDataSignal.AnalogInput2)
    else
        AppDataCollectionAddAxisSignal($myAx, AxisDataSignal.AnalogInput3)
    end
end