[JamesTerm]

Originally Posted by Tom Line

we have our shooter logic in a 10 ms timed loop (not a waited loop).

We calculate the rate based on 3 loops worth of data. Each loop we drop the oldest data point and add a new one.

10ms timed loop? is that because of vision tracking? Why does it need to be timed?

Originally Posted by Tom Line

For our 'enable to shoot' logic, we use an 8 sample average of rate fed into an IIR filter with a .5 constant.

Please, feel free to tell us what we can improve =). I'm an mechanical engineer masquerading as a controls engineer, and systems engineering was always one of my most hated classes :lol:

First thing I would like to mention from a previous time is that we had a problem with our encoder where the vibrations of the shooter caused the encoder itself to rupture the casing and then caused it to lose counts at higher speeds as the centripital force of the encoder wheel lost contact during certain phase intervals. We had to tape the outer casing down tight to keep this problem to a minimum, and then needed to use a priority averager to erase this symtom. The priority averager looks like this:

class Priority_Averager
{
	private:
		std::priority_queue<double> m_queue;
		const size_t m_SampleSize;
		const double m_PurgePercent;

		double m_CurrentBadApple_Percentage;
		size_t m_Iteration_Counter;
		void flush()
		{
			while (!m_queue.empty())
				m_queue.pop();
		}
	public:
	Priority_Averager(size_t SampleSize, double PurgePercent) : m_SampleSize(SampleSize),m_PurgePercent(PurgePercent),
		m_CurrentBadApple_Percentage(0.0),m_Iteration_Counter(0)
	{
	}
	double operator()(double newItem)
	{
		m_queue.push(newItem);
		double ret=m_queue.top();
		if (m_queue.size()>m_SampleSize)
			m_queue.pop();
		//Now to manage when to purge the bad apples
		m_Iteration_Counter++;
		if ((m_Iteration_Counter % m_SampleSize)==0)
		{
			m_CurrentBadApple_Percentage+=m_PurgePercent;
			//printf(" p=%.2f ",m_CurrentBadApple_Percentage);
			if (m_CurrentBadApple_Percentage >= 1.0)
			{
				//Time to purge all the bad apples
				flush();
				m_queue.push(ret);  //put one good apple back in to start the cycle over
				m_CurrentBadApple_Percentage-=1.0;
				//printf(" p=%.2f ",m_CurrentBadApple_Percentage);
			}
		}
		return ret;
	}
};

This got rid of that symptom but we still had the typical noise so we used a kalman filter:

void KalmanFilter::Reset()
{
	m_FirstRun=true;
    //initial values for the kalman filter
    m_x_est_last = 0.0;
    m_last = 0.0;
}

KalmanFilter::KalmanFilter(): m_Q(0.022),m_R(0.617)  //setup Q and R as the noise in the system
{
}

double KalmanFilter::operator()(double input)
{
	//For first run set the last value to the measured value
	if (m_FirstRun)
	{
		m_x_est_last=input;
		m_FirstRun=false;
	}
    //do a prediction
    double x_temp_est = m_x_est_last;
    double P_temp = m_last + m_Q;
    //calculate the Kalman gain
    double K = P_temp * (1.0/(P_temp + m_R));
    //the 'noisy' value we measured
    double z_measured = input;
    //correct
    double x_est = x_temp_est + K * (z_measured - x_temp_est); 
    double P = (1- K) * P_temp;
    
    //update our last's
    m_last = P;
    m_x_est_last = x_est;
    
	//Test for NAN
	if ((!(m_x_est_last>0.0)) && (!(m_x_est_last<0.0)))
		m_x_est_last=0;

    return x_est;
}

And the typical averager

// A templated averager, make sure the type being averaged can handle the +, -, and / functions
template<class T, unsigned NUMELEMENTS>
class Averager
{
public:
	Averager() : m_array(NULL), m_currIndex((unsigned)-1)
	{
		if (NUMELEMENTS > 1)
			m_array = new T[NUMELEMENTS];
	}
	virtual ~Averager() {if (m_array) delete[] m_array;}
	T GetAverage(T newItem)
	{
		if (!m_array)	// We are not really using the Averager
			return newItem;

		// If the first time called, set up the array and use this value
		if (m_currIndex == -1)
		{
			m_array[0] = newItem;
			m_currIndex = -2;
			m_sum = newItem;
			return newItem;
		}
		else if (m_currIndex < -1)
		{
			// We have not populated the array for the first time yet, still populating
			m_sum += newItem;
			int arrayIndex = (m_currIndex*-1)-1;
			m_array[arrayIndex] = newItem;

			// Still counting backwards unless we have filled all of the array
			if (arrayIndex == (NUMELEMENTS-1))	// This means we have filled the array
				m_currIndex = 0;				// Start taking from the array next time
			else
				--m_currIndex;

			// Return the average based on what we have counted so far
			return (m_sum / (arrayIndex+1));
		}
		else // 0 or greater, we have filled the array
		{
			m_sum += newItem;
			m_sum -= m_array[m_currIndex];
			m_array[m_currIndex] = newItem;
			++m_currIndex;
			if (m_currIndex == NUMELEMENTS)
				m_currIndex = 0;
			return (m_sum / NUMELEMENTS);
		}
	}

	void Reset(){m_currIndex=-1;}

private:
	T* m_array;
	int m_currIndex;
	T m_sum;
};

With the three averagers working together our encoder readings look like this:



Where the cyan line is the encoder reading trying to match the magenta ramp velocity. Hope these ideas may help out.