   digitalWrite(dirPin, LOW);  // set direction
 
   // Initialize position valiables
-  lastLastRawPos = analogRead(sensorPosPin);
-  lastRawPos = analogRead(sensorPosPin);
+//  lastLastRawPos = analogRead(sensorPosPin);
+//  lastRawPos = analogRead(sensorPosPin);
 }
 
 /*
     readPosCount() function
 */
-void readPosCount() {
-  // Get voltage output by MR sensor
-  rawPos = analogRead(sensorPosPin);  //current raw position from MR sensor
-  // Calculate differences between subsequent MR sensor readings
-  rawDiff = rawPos - lastRawPos;          //difference btwn current raw position and last raw position
-  lastRawDiff = rawPos - lastLastRawPos;  //difference btwn current raw position and last last raw position
-  rawOffset = abs(rawDiff);
-  lastRawOffset = abs(lastRawDiff);
-
-  // Update position record-keeping vairables
-  lastLastRawPos = lastRawPos;
-  lastRawPos = rawPos;
-
-  // Keep track of flips over 180 degrees
-  if ((lastRawOffset > flipThresh) && (!flipped)) { // enter this anytime the last offset is greater than the flip threshold AND it has not just flipped
-    if (lastRawDiff > 0) {       // check to see which direction the drive wheel was turning
-      flipNumber--;              // cw rotation
-    } else {                     // if(rawDiff < 0)
-      flipNumber++;              // ccw rotation
-    }
-    if (rawOffset > flipThresh) { // check to see if the data was good and the most current offset is above the threshold
-      updatedPos = rawPos + flipNumber * rawOffset; // update the pos value to account for flips over 180deg using the most current offset
-      tempOffset = rawOffset;
-    } else {                     // in this case there was a blip in the data and we want to use lastactualOffset instead
-      updatedPos = rawPos + flipNumber * lastRawOffset; // update the pos value to account for any flips over 180deg using the LAST offset
-      tempOffset = lastRawOffset;
-    }
-    flipped = true;            // set boolean so that the next time through the loop won't trigger a flip
-  } else {                        // anytime no flip has occurred
-    updatedPos = rawPos + flipNumber * tempOffset; // need to update pos based on what most recent offset is
-    flipped = false;
-  }
-    
-  //Serial.println(updatedPos);
-}
+//void readPosCount() {
+//  // Get voltage output by MR sensor
+//  rawPos = analogRead(sensorPosPin);  //current raw position from MR sensor
+//  // Calculate differences between subsequent MR sensor readings
+//  rawDiff = rawPos - lastRawPos;          //difference btwn current raw position and last raw position
+//  lastRawDiff = rawPos - lastLastRawPos;  //difference btwn current raw position and last last raw position
+//  rawOffset = abs(rawDiff);
+//  lastRawOffset = abs(lastRawDiff);
+//
+//  // Update position record-keeping vairables
+//  lastLastRawPos = lastRawPos;
+//  lastRawPos = rawPos;
+//
+//  // Keep track of flips over 180 degrees
+//  if ((lastRawOffset > flipThresh) && (!flipped)) { // enter this anytime the last offset is greater than the flip threshold AND it has not just flipped
+//    if (lastRawDiff > 0) {       // check to see which direction the drive wheel was turning
+//      flipNumber--;              // cw rotation
+//    } else {                     // if(rawDiff < 0)
+//      flipNumber++;              // ccw rotation
+//    }
+//    if (rawOffset > flipThresh) { // check to see if the data was good and the most current offset is above the threshold
+//      updatedPos = rawPos + flipNumber * rawOffset; // update the pos value to account for flips over 180deg using the most current offset
+//      tempOffset = rawOffset;
+//    } else {                     // in this case there was a blip in the data and we want to use lastactualOffset instead
+//      updatedPos = rawPos + flipNumber * lastRawOffset; // update the pos value to account for any flips over 180deg using the LAST offset
+//      tempOffset = lastRawOffset;
+//    }
+//    flipped = true;            // set boolean so that the next time through the loop won't trigger a flip
+//  } else {                        // anytime no flip has occurred
+//    updatedPos = rawPos + flipNumber * tempOffset; // need to update pos based on what most recent offset is
+//    flipped = false;
+//  }
+//    
+//  //Serial.println(updatedPos);
+//}
 
 
 /*
     calPosMeter()
 */
-void calPosMeter()
-{
-  double rh = 65.659;   //[mm]
-  double ts = -.0107 * updatedPos + 7.9513; // Compute the angle of the sector pulley (ts) in degrees based on updatedPos
-  xh = rh * (ts * 3.14159 / 180); // Compute the position of the handle based on ts
-  vh = -(.95 * .95) * lastLastVh + 2 * .95 * lastVh + (1 - .95) * (1 - .95) * (xh - lastXh) / .0001; // filtered velocity (2nd-order filter)
-  lastXh = xh;
-  lastLastVh = lastVh;
-  lastVh = vh;
-  
-  //Serial.println(xh);
-}
+//void calPosMeter()
+//{
+//  double rh = 65.659;   //[mm]
+//  double ts = -.0107 * updatedPos + 7.9513; // Compute the angle of the sector pulley (ts) in degrees based on updatedPos
+//  xh = rh * (ts * 3.14159 / 180); // Compute the position of the handle based on ts
+//  vh = -(.95 * .95) * lastLastVh + 2 * .95 * lastVh + (1 - .95) * (1 - .95) * (xh - lastXh) / .0001; // filtered velocity (2nd-order filter)
+//  lastXh = xh;
+//  lastLastVh = lastVh;
+//  lastVh = vh;
+//  
+//  //Serial.println(xh);
+//}
 
 // read joystick state
 uint8_t readJoystick() {

int32_t nextcall = 1000;
void loop() {
  // read the position in count
  readPosCount();
  calPosMeter();

  if(millis() > nextcall) {
    nextcall += 50;

	Serial.print(xh);
    Serial.print("\t");
	Serial.println(readJoystick()); // return joystick state
  }
  

/*
 * Arduino code to read joystick states
 */
//#include<math.h>

/*
    DECLARATION
