   if ( micros() - int1time < threshold )
     return;
   int1history = int1signal;
-  int1signal = bitRead(PIND, pinB);
+  int1signal = bitRead(PIND, B_PIN);
   if ( int1history==int1signal )
     return;
   int1time = micros();
 }
-
 void reset()
 {
   halfStepsCorrection = halfSteps;
-  isAbsoluteReference = true;
+  isAbsoluteReferenceAvailable = true;
  }
  
-void computeFeedback()
-{
-  long steps = (halfSteps / 2);
-  //Serial.print("T");
-  //Serial.println(steps);
-  double relative_position_m = (halfSteps)*linearResolution/2;
-  double absolute_position_m = (halfSteps-halfStepsCorrection)*linearResolution/2;
-  
-  // read the analog in value:
-  double raw_angle = analogRead(potPin)/1024.0*potentiometerMaxRange;
-  double neutralAngle = 0.75*potentiometerMaxRange;
-  double angle = raw_angle - neutralAngle;
-  double correction_m = angle*potentiometerDistance;
-  Input1 = angle/potentiometerUsedRange;
-  Input2 = relative_position_m/linearRange;
-  Input3 = analogRead(tensionPin);
-  //Serial.print("P");
-  //Serial.println(Input1);
-  //Serial.print("A");
-  //Serial.println(Input2);
-  
-  InputRoll = StrToFloat(roll.value());
-
-}
-
-void process()
-{
-  //setMode();
-  computeFeedback();
-  if (myPID1.GetMode() ==MANUAL)
-  {
-    alphaSigned1 = StrToFloat(pwm1.value());
-  }
-  if (myPID1.GetMode() ==AUTOMATIC)
-  {
-    Setpoint1 = StrToFloat(setpos1.value());
-    myPID1.Compute();
-    alphaSigned1 = Output1;
-  }
-  if (myPID2.GetMode() ==MANUAL)
-  {
-    alphaSigned2 = StrToFloat(pwm2.value());
-  }
-  if (myPID2.GetMode() ==AUTOMATIC)
-  {
-    double offset_m = -0.0;
-    Setpoint2 = StrToFloat(setpos2.value()) + offset_m/linearRange;
-    myPID2.Compute();
-    alphaSigned2 = Output2;
-  }
-}
-
-void computePIDTuning()
-{
-  myPID1.SetTunings(StrToFloat(kpm1.value()), StrToFloat(kim1.value()), StrToFloat(kdm1.value()));
-  myPID2.SetTunings(StrToFloat(kpm2.value()), StrToFloat(kim2.value()), StrToFloat(kdm2.value()));
-  myPIDRoll.SetTunings(StrToFloat(kpmr.value()), StrToFloat(kimr.value()), StrToFloat(kdmr.value()));
-
-}
-
-
-

        if (feedback_request.isUpdated())
        {
          isFeedbackRequested = true;
          digitalWrite(ledPin, HIGH);
        }
          
      }
    }
  }  
}


void loop()
{
  
  int power1, power2;
  sensorValue = analogRead(analogInPin);

  process();
  
  power1 = alphaSigned1*127;
  power2 = -alphaSigned2*127;
  ST.motor(1, power1);
  ST.motor(2, power2);
  delay(10);
  if (isFeedbackRequested)
  {
    //lastWriteTime = millis();
    
    Serial.print((alphaSigned1+1)*512);
    Serial.print(", ");
    Serial.print((alphaSigned2+1)*512);
    Serial.print(", ");
    Serial.print((Input1+1)*512);
    Serial.print(", ");
    Serial.print((Input2+1)*512);
    Serial.print(", ");
    Serial.print(Input3);
    Serial.print(", ");
    Serial.println(Input3);
    
    //isConnectionAlive = false;
    isFeedbackRequested = false;
    digitalWrite(ledPin, LOW);
    if (isSerialControl)
    {
      //Serial.println("S");
    }
    else
    {
      if (isManualControl)
      {
        //Serial.println("M");
      }
    }

  }
  Serial.flush();
}

void setMode()
{  
  // If the analog value is changed, it means that the user wants to take control with the joystick
  // so fallback to joystick control
  if (fabs(initialSensorValue-sensorValue) > 10)
  {
    isSerialControl = false;
  }

  // Fallback to manual control if the analog voltage is close to zero (probably unplugged)
  // Place after hardware limit if possible
  // This avoids to go to full speed if one cable in the potentiometer is unplugged
  // Warning: if the midle cable of the potentiometer is unplugged, the signal is free, and the fault can not be detected
  // \todo invert polarity to be able to detect floating pin.
  isManualControl = false;//(sensorValue < 5)|(sensorValue >1023-5);
}

float StrToFloat(String str){
  char carray[str.length() + 1]; // Determine the size of the array
  str.toCharArray(carray, sizeof(carray)); // Put str into an array
  return atof(carray);
}

void computeAlphaSigned()
{
  if (serialFrequency != 0)
  { // Fallbacks to zero as we received no message in the expected time (twice the time)
    if ((millis()-lastSerialInputTime) > 2*1/serialFrequency*1000)
    {
      alphaSigned1 = 0;
      alphaSigned2 = 0;
    }
  }

  // If not in Serial control overwrite the value 
  if (false == isSerialControl)
  {
    // Compute the value corresponding to the deadband
    double sensorDeadBandMax = sensorValueMin + (deadBand/2.0+0.5)*(sensorValueMax - sensorValueMin);
    double sensorDeadBandMin = sensorValueMin + (-deadBand/2.0+0.5)*(sensorValueMax - sensorValueMin);
    if (sensorValue < sensorDeadBandMin) //strictly to avoid alphaSigned equals zero
    {
      alphaSigned1 = (sensorValue-sensorValueMin)/(sensorDeadBandMax - sensorValueMin)-1;
      alphaSigned2 = 0;
    } 
    else
    {
      if (sensorValue > sensorDeadBandMax)
      {
        alphaSigned1 = (sensorValue-sensorDeadBandMax)/(sensorValueMax - sensorDeadBandMax);
        alphaSigned2 = 0;
      }
      else
      {
        alphaSigned1 = 0;
        alphaSigned2 = 0;
      }
    }
    if (isManualControl)
    {
      alphaSigned1 = 0;
      alphaSigned2 = 0;
    }
  }
  alphaSigned1 = min(1, max(-1, alphaSigned1));
  alphaSigned2 = min(1, max(-1, alphaSigned2));
}


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

// Copyright (c) 2013 Nautilabs
// https://github.com/baptistelabat/robokite

// This file used SoftwareSerial example (Copyright (c) 2012 Dimension Engineering LLC)
// for Sabertooth http://www.dimensionengineering.com/software/SabertoothSimplifiedArduinoLibrary/html/index.html
// See license.txt in the Sabertooth arduino library for license details.
//
//
// Using example in http://code.google.com/p/arduino-pinchangeint/wiki/Usage
// Parts of code taken from http://playground.arduino.cc/Main/RotaryEncoders J.Carter(of Earth)
//
// The Sabertooth selector 1 3 5 6 have to be on on.

// Includes
#define NO_PORTB_PINCHANGES // PortB used for mySofwareSerial
#include <mySoftwareSerial.h> // This is a modified version of SoftwareSerial to be able to use interrupt pin as well
#include <SabertoothSimplified.h> // From http://www.dimensionengineering.com/software/SabertoothSimplifiedArduinoLibrary/html/_sabertooth_simplified_8h_source.html
#include <TinyGPS++.h>    // From https://github.com/mikalhart/TinyGPSPlus
#include <PID_v1.h>       // From https://github.com/br3ttb/Arduino-PID-Library/tree/master/PID_v1
#include <PinChangeInt.h> // From http://code.google.com/p/arduino-pinchangeint/

// Port definition
#define   POT_PIN  1 // Potentiometer
#define     A_PIN  2 // Linear encoder one
#define     B_PIN  3 // Linear encoder other one
#define RESET_PIN  4 // Linear encoder absolute reference
#define    TX_PIN  8 // Plug to Sabertooth RX
#define    RX_PIN 10 // Do not plug
#define   LED_PIN 13 // Built-in led
const int LINE_TENSION_PIN = A2; // For line tension measurement

// String management
String inputString = "";         // a string to hold incoming data
boolean stringComplete = false;  // whether the string is complete

// Sabertooth connection
// Note: NOT_A_PIN (0) was previously used for RX which is not used, but this was causing problems
mySoftwareSerial SWSerial(RX_PIN, TX_PIN); // RX, TX. RX on no pin (unused), TX on pin 8 (to S1).
SabertoothSimplified ST(SWSerial); // Use SWSerial as the serial port.
int power1, power2;

// NMEA protocol for robust messages
TinyGPSPlus nmea;
// O stands for Opensource, R for Robokite
// The index is the place of the field in the NMEA message
TinyGPSCustom feedback_request      (nmea, "ORFBR", 1);  // Feedback request
TinyGPSCustom pwm1     (nmea, "ORPW1", 1);  // Dimentionless voltage setpoint (Pulse Width Modulation) for Sabertooth output 1
TinyGPSCustom pwm2     (nmea, "ORPW2", 1);  // Dimentionless voltage setpoint (Pulse Width Modulation) for Sabertooth output 2
TinyGPSCustom setpos1  (nmea, "ORSP1", 1);  // Position setpoint for Sabertooth output 1
TinyGPSCustom setpos2  (nmea, "ORSP2", 1);  // Position setpoint for Sabertooth output 2
boolean isFeedbackRequested = false;

// PID for robust control
// Define Variables we'll be connecting to
double Setpoint1, Input1, Output1;
double Setpoint2, Input2, Output2;
double Input3;
// Specify the links and initial tuning parameters (Kp, Ki, Kd)
PID myPID1(&Input1, &Output1, &Setpoint1, 1, 0, 0, DIRECT);
PID myPID2(&Input2, &Output2, &Setpoint2, 1, 0, 0, DIRECT);

// Define global variables to deal with encoder interrupt
volatile unsigned long threshold = 1000;// 'threshold' is the De-bounce Adjustment factor for the Rotary Encoder. halfSteps
volatile unsigned long int0time = 0;
volatile unsigned long int1time = 0;
volatile uint8_t int0signal = 0;
volatile uint8_t int1signal = 0;
volatile uint8_t int0history = 0;
volatile uint8_t int1history = 0;
// 'halfSteps' is the counter of half-steps. The actual
// number of steps will be equal to halfSteps / 2
volatile long halfSteps = 0;
long halfStepsCorrection = 0;
boolean isAbsoluteReferenceAvailable = false;

// Hardware specific parameters
// Potentiometer
#define POT_RANGE_DEG      300 // 300 is the value for standard potentiometer
#define POT_USED_RANGE_DEG  60 // To 
#define POT_OFFSET        0.05 // Distance from rotation axis to lever arm
#define NEUTRAL_ANGLE_DEG  225
// Linear encoder
#define LINEAR_RESOLUTION 0.005
#define LINEAR_USED_RANGE 0.05

void setup()
{
  pinMode(LED_PIN, OUTPUT); 
  
  // Initialize software serial communication with Sabertooth 
  SWSerial.begin(9600);
  
  // Initialize serial communications with computer
  Serial.begin(57600);
  
  // Reserve bytes for the inputString:
  inputString.reserve(200);
  
  // Initialize input and setpoint
  Input1 = 0;
  Input2 = 0;
  Input3 = 0;

  Setpoint1 = 0;
  Setpoint2 = 0;
  
  // Turn the PID on
  myPID1.SetMode(MANUAL);
  myPID2.SetMode(MANUAL);
  myPID1.SetOutputLimits(-1, 1);
  myPID2.SetOutputLimits(-1, 1);
  
    //*****************************************************
  pinMode(A_PIN, INPUT);
  digitalWrite(A_PIN, HIGH);
  attachInterrupt(0, int0, CHANGE);
  //PCintPort::attachInterrupt(pinA, &int0, CHANGE);
  pinMode(B_PIN, INPUT);
  digitalWrite(B_PIN, HIGH);
  attachInterrupt(1, int1, CHANGE);
  //PCintPort::attachInterrupt(pinB, &int1, CHANGE);
  
  // Pin used to get absolute position
  pinMode(RESET_PIN, INPUT);
  digitalWrite(RESET_PIN, HIGH);
  PCintPort::attachInterrupt(RESET_PIN, &reset, CHANGE);
}

/*
  SerialEvent occurs whenever a new data comes in the
 hardware serial RX.  This routine is run between each
 time loop() runs, so using delay inside loop can delay
 response.  Multiple bytes of data may be available.
 */
void serialEvent()
{
  while (Serial.available()) {
    // Get the new byte:
    char inChar = (char)Serial.read(); 
    // Add it to the inputString:
    inputString += inChar;
    // If the incoming character is a newline, set a flag
    // so the main loop can do something about it:
    if (inChar == '\n') {
      stringComplete = true;
    } 
  }
}

void loop()
{
   processSerialInput();
   computeFeedback();
   sendFeedback();
   computeOrder();
   sendOrder();
   delay(10);
}

void processSerialInput()
{
  // Print the string when a newline arrives:
  if (stringComplete)
  {   
    // Serial.println(inputString);
    char ctab[inputString.length()+1];
    inputString.toCharArray(ctab, sizeof(ctab));
    char *gpsStream = ctab;
    
    while (*gpsStream)
    {
      if (nmea.encode(*gpsStream++))
      {
        if (pwm1.isUpdated())
        { 
          myPID1.SetMode(MANUAL);
