       moveSteer(Straight);    
     
 }
-int compareint (const void * a, const void * b)
-{
-  if ( *(int*)a <  *(int*)b ) return -1;
-  if ( *(int*)a == *(int*)b ) return 0;
-  if ( *(int*)a >  *(int*)b ) return 1;
-}
-int median_filter(int* readings)
-// on entry, readings holds the 5 most recent readings.
-// returned value is the median filter.
-// readings[0] is oldest; readings[4] is newest
-// on exit, all readings are shifted, with most recent reading in both [4] and [5]
-{
-  int sorted[5];
-  for (int i = 0; i < 5; i++)
-    sorted[i] = readings[i];
-   qsort(sorted, 5, sizeof(int), compareint);
-   for (int i = 0; i < 4; i++)
-    readings[i] = readings[i+1];
-  return sorted[2];
-
+
+#define SWAP(a,b) temp=(a);(a)=(b);(b)=temp;
+int median_filter(int arr[])
+{ // return the median of 5 numbers
+  // on exit, the order of the five numbers is unspecified.
+  // median filter has been tested and verified on 300 random arrays.
+	register int temp;
+	if (arr[0] > arr[1])
+	{
+		SWAP(arr[0],arr[1])
+	}
+	if (arr[1] > arr[2])
+	{
+		SWAP(arr[1],arr[2])
+		if (arr[0] > arr[1])
+		{
+			SWAP(arr[0],arr[1])
+		}
+	}
+	if (arr[4] < arr[3])
+	{
+		SWAP(arr[4],arr[3])	
+	}
+	if (arr[2] <= arr[3])
+		return arr[2];	
+	if (arr[4] <= arr[0])
+		return arr[0];
+	if (arr[0] < arr[3])
+	{
+		SWAP(arr[0],arr[3])	// arr[3] is the min
+	}
+	if (arr[2] > arr[4])
+	{
+		SWAP(arr[2],arr[4])	// arr[4] is the max
+	}
+	if (arr[0] >= arr[1])
+		return arr[0];	
+	if (arr[2] <= arr[1])
+		return arr[2];
+	return arr[1];
 }
 /*---------------------------------------------------------------------------------------*/ 
 void initialize()

const int FullThrottle =  179;   // 3.5 V
const int MinimumThrottle = 39;  // Throttle has no effect until 0.75 V

const int FullBrake = 225;  // start with a conservative value; could go as high as 255;  
const int MinimumBrake = 155; // start with a conservative value; could go as low as 127;

const int HardLeft = 223; // was 159; // start with a conservative value; could go as high as 255;
const int HalfLeft = 223; // was 159;


        initialize();
        
        CalibrateBrakes(400);
/*        
       CalibrateSteering(HardRight,3);     
       CalibrateSteering(HardLeft,3);
       delay(1000);
       CalibrateSteering(HardRight,1);
       CalibrateSteering(HardLeft,1);
       delay(1000);
       CalibrateSteering(HardRight,10);       
*/       
}	

/*---------------------------------------------------------------------------------------*/ 
void BrakeOn()
{
     analogWrite(DiskBrake, FullBrake);
}
void BrakeOff()
{
     analogWrite(DiskBrake, NoBrake);
}
void CalibrateBrakes( int delaySmall)
{
  // This routine establishes the correct values for NoBrake and FullBrake.
  // Verify that the value for FullBrake stops the vehicle.
  // It allows us to establish the constants to map the feedback to the commanded position.
  
