   pinMode(pinReset, INPUT);
   digitalWrite(pinReset, HIGH);
   PCintPort::attachInterrupt(pinReset, &reset, CHANGE);
+  
+  // Setup serial communication
   Serial.begin(115200);
   Serial.println("---------------------------------------");
 }
 
-uint8_t i;
+void loop() {
+  long steps = (halfSteps / 2);
+  double absolute_position_m = (halfSteps-halfStepsCorrection)*linearResolution/2;
+  
+  // read the analog in value:
+  double raw_angle = analogRead(1)/1024.0*potentiometerMaxRange;
+  double neutralAngle = 0.8*potentiometerMaxRange;
+  double angle = raw_angle - neutralAngle;
+  double correction_m = angle*potentiometerDistance; 
+    
+  Serial.print((absolute_position_m/linearRange)*1024 + 512);
+  Serial.print(",");
+  Serial.print(correction_m/linearRange*1024 + 512);
+  Serial.print(",");
+  Serial.print((absolute_position_m + correction_m)/linearRange*1024 + 512);
+  Serial.print(",");
+  Serial.print(angle/potentiometerUsedRange*1024 + 512);
+  Serial.print(",");
 
-void loop() {
-  long actualRotaryTicks = (rotaryHalfSteps / 2);
-    // read the analog in value:
-  Serial.print(rotaryHalfSteps*8+512); 
-  Serial.print(",");  
   //Serial.print("\t output = ");      
   //Serial.println(outputValue);
   delay(2);
     
-  for (int thisPin = 1; thisPin < 6; thisPin++) {
+  for (int thisPin = 0; thisPin < 2; thisPin++) {
   sensorValue = analogRead(thisPin);            
   
   // print the results to the serial monitor:

int sensorValue = 0;        // value read from the pot
int outputValue = 0;        // value output to the PWM (analog out)


void int0()
	{
	if ( micros() - int0time < threshold )
		return;
	int0history = int0signal;
	int0signal = bitRead(PIND,pinA);
	if ( int0history==int0signal )
		return;
	int0time = micros();
	if ( int0signal == int1signal )
		rotaryHalfSteps++;
	else
		rotaryHalfSteps--;
	}

void int1()
	{
	if ( micros() - int1time < threshold )
		return;
	int1history = int1signal;
	int1signal = bitRead(PIND,pinB);
	if ( int1history==int1signal )
		return;
	int1time = micros();
	}
void reset()
{
 rotaryHalfSteps = 0;
 }

void setup() {


//Parts of code taken from http://playground.arduino.cc/Main/RotaryEncoders J.Carter(of Earth)

// 'threshold' is the De-bounce Adjustment factor for the Rotary Encoder. halfSteps
volatile unsigned long threshold = 1000;

// 'halfStepshalfSteps' is the counter of half-steps. The actual
// number of steps will be equal to halfSteps / 2
//
volatile long halfSteps = 0;
long halfStepsCorrection = 0;

// Working variables for the interrupt routines
//
