Roboteq

Brief Description of the Code:

This MicroBasic script for a Roboteq motor controller smooths encoder data for channels 1 and 2 using a moving average filter and transmits the smoothed data via CAN communication at 1 millisecond intervals.

What It Does:

1. Initialization:

   • Sets up variables and constants for the moving average calculation and timing.

2. Data Collection:

   • Continuously reads encoder data every 1ms from channels 1 and 2.

3. Moving Average Calculation:

   • Stores the latest encoder readings in a circular buffer.
   • After collecting 5 readings, calculates the average for each channel using the sum of the last 5 values.

4. Data Transmission:

   • Transmits the smoothed data via CAN communication.

' /////////// ////////// //////// //////// //////// ROBOTEQ \\\\\\\\\ \\\\\\\\\ \\\\\\\\\ \\\\\\\\\ \\\\\\\\
 
'Disclaimer:
'The script is tested and validated by Roboteq and is believed to be fault-free. 
'The possibility always exists, however, that the particular configuration and/or use 
'condition uncovers a fault that escaped our validation test coverage. Always 
'extensively test the script under your use conditions prior to deploying it in the field. 
 
'*******************************************************************************************
' This script is responsible for applying a moving average filter with a 1ms interval 
' for smoothing the encoder data of channels 1 and 2 and transmitting it via CAN communication.
 

option explicit
 
' Constants
 
#define WINDOW_SIZE 5     ' Number of samples for moving average
#define INTERVAL_MS 1     ' Time interval in milliseconds between data collection and averaging
 
' Variables
 
dim encoderDataCh1[WINDOW_SIZE] as integer  ' Array to store the last WINDOW_SIZE encoder readings for channel 1
dim encoderDataCh2[WINDOW_SIZE] as integer  ' Array to store the last WINDOW_SIZE encoder readings for channel 2
dim sumCh1 as integer                       ' Sum of the encoder readings for channel 1
dim sumCh2 as integer                       ' Sum of the encoder readings for channel 2
dim index as integer                        ' Index for the current position in the circular buffer
dim count as integer                        ' Count of the number of readings taken
dim smoothedDataCh1 as integer              ' Smoothed encoder data for channel 1
dim smoothedDataCh2 as integer              ' Smoothed encoder data for channel 2
dim i as integer                            ' Loop variable
 
'Initialization
 
count = 0
index = 0
sumCh1 = 0
sumCh2 = 0
i = 0
 

Main:
 
    'read encoder data from each channel
    encoderDataCh1[index] = getvalue(_c, 1)
    encoderDataCh2[index] = getvalue(_c, 2)
    'print(encoderDataCh1[index],"\n")
 

    ' Increment the index and count
    index = index + 1
 
    if index >= WINDOW_SIZE then               ' If index exceeds the window size,
        index = 0                              ' reset it to 0 (circular buffer)
    end if
 

    if count < WINDOW_SIZE then                ' If count is less than the window size,
        count = count + 1                      ' increment the count
    end if
    
 
    ' Apply the moving average filter
    if count = WINDOW_SIZE then
        gosub MovingAverageFilter
        'print(smoothedDataCh1,"\n")
        
        ' Transmit smoothed data via CAN saving in VAR 1 and VAR 2
        
        setcommand(_VAR, 1, smoothedDataCh1)
        setcommand(_VAR, 2, smoothedDataCh2)
        
        
    ' Reset count after the first average calculation
        count = 5 ' Ensure count stays at WINDOW_SIZE for subsequent calculations
    end if
 

    wait(INTERVAL_MS) 
 
goto main
 

' Subroutine for calculating the moving average
 
MovingAverageFilter:
 

    sumCh1 = 0
    sumCh2 = 0
    
 
    for i = 0 andwhile i <= (WINDOW_SIZE - 1) Evaluate i += 1  'Loop through all the values in the window size
    
        sumCh1 = sumCh1 + encoderDataCh1[i]
        sumCh2 = sumCh2 + encoderDataCh2[i]
        
            
    next
 
    smoothedDataCh1 = sumCh1 / WINDOW_SIZE                     'Calculate the average for channel 1
    smoothedDataCh2 = sumCh2 / WINDOW_SIZE                     'Calculate the average for channel 2
    
    'print(smoothedDataCh1,"\n")
 
return