#define TAPS 51
static float savGo[TAPS] =
{ -2.4637603,-1.6462135,-0.9167071,-0.2715726,0.2928581,0.7802535,1.1942819,1.5386117,1.8169113,2.0328488,2.1900928,2.2923116,2.3431735,2.3463468,2.3055000,2.2243013,2.1064191,1.9555218,1.7752777,1.5693551,1.3414224,1.0951481,0.8342003,0.5622474,0.2829579,0.0000000,-0.2829579,-0.5622474,-0.8342003,-1.0951481,-1.3414224,-1.5693551,-1.7752777,-1.9555218,-2.1064191,-2.2243013,-2.3055000,-2.3463468,-2.3431735,-2.2923116,-2.1900928,-2.0328488,-1.8169113,-1.5386117,-1.1942819,-0.7802535,-0.2928581,0.2715726,0.9167071,1.6462135,2.4637603,143793.0000000,143772.0000000,143751.0000000,143730.0000000,143709.0000000,143688.0000000,143667.0000000,143646.0000000,143625.0000000,143604.0000000,143583.0000000,143562.0000000,143541.0000000,143520.0000000,143499.0000000,143478.0000000,143457.0000000,143436.0000000,143415.0000000,143394.0000000,143373.0000000,143352.0000000,143331.0000000,143310.0000000,143289.0000000,143268.0000000,143247.0000000,143226.0000000,143205.0000000,143184.0000000,143163.0000000,143142.0000000,143121.0000000,143100.0000000,143079.0000000,143058.0000000,143037.0000000,143016.0000000,142995.0000000,142974.0000000,142953.0000000,142932.0000000,142911.0000000,142890.0000000,142869.0000000,142848.0000000,142827.0000000,142806.0000000,142785.0000000};
/*
* where the current Motor Speeds are stored in. units are degres per second
*/
static volatile signed int curMotorSpeeds[4]  = {0,0,0,0};
/*
* The last TAPS measurments. Where 0 is the most recent measurment and TAPS-1 is the oldest
*/
static volatile int32_t enocderCount[4][TAPS];

#define COUNTS_PER_REV	  3200
#define COUNTS_2_DEG COUNTS_PER_REV/360
#define DEGREE2RAD     pi/180


/*
 * THis gets the current motor speed from the encoders with calculation of savgo
 */
static void getMotorSpeed(int motorNum){
	int i;
	float sum = 0.0;
	for(i=TAPS;i>0;i--){
		enocderCount[motorNum][i] = enocderCount[motorNum][i-1];
	}
	enocderCount[motorNum][0] = getCurEncoder(motorNum);
	for(i=0;i<TAPS;i++){
		sum += enocderCount[motorNum][i]*savGo[i];
	}
	sum /= COUNTS_2_DEG;
	curMotorSpeeds[motorNum] = sum;
}