   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()));
-
-}
-
-
-


// 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.
// It is as well derived from HBridgePWM.ino from robokite project
// https://github.com/baptistelabat/robokite/blob/master/HBridgePWM/HBridgePWM.ino
// The selector 1 3 5 6 have to be on on.
//
// 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)

#define NO_PORTB_PINCHANGES // PortB used for mySofwareSerial
#include <mySoftwareSerial.h>
#include <SabertoothSimplified.h>
#include <TinyGPS++.h>
#include <PID_v1.h>
#include <PinChangeInt.h>

mySoftwareSerial SWSerial(NOT_A_PIN, 8); // RX on no pin (unused), TX on pin 11 (to S1).
SabertoothSimplified ST(SWSerial); // Use SWSerial as the serial port.

const int analogInPin = A0;  // Analog input pin that the potentiometer is attached to
const int tensionPin = A2;
const int ledPin = 13;

int sensorValue = 0;        // Value read from the potentiometer
int initialSensorValue = 0;
double alphaSigned1 = 0;
double alphaSigned2 = 0;
String inputString = "";         // A string to hold incoming data
boolean stringComplete = false;  // Whether the string is complete
boolean isSerialControl = true; 
boolean isManualControl = false;
boolean isFeedbackRequested = false;
// Read the analog in value
// The range was reduced 
double sensorValueMin = 200;//0
double sensorValueMax = 800;//1023
double deadBand = 0.1; // Use to ensure zero speed. Should not be zero
double serialFrequency = 1;
long lastSerialInputTime = 0;
long lastWriteTime = 0;

TinyGPSPlus nmea;
// O stands for Opensource, R for Robokite
// The index is the place of the field in the NMEA message
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 setss1   (nmea, "ORSS1", 1);  // Set speed for Sabertooth output 1
TinyGPSCustom setss2   (nmea, "ORSS2", 1);  // Set speed 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
TinyGPSCustom roll     (nmea, "ORKST", 1);  // Kite STate roll (rotation in camera frame)
TinyGPSCustom elevation(nmea, "ORKST", 2);  // Kite STate elevation
TinyGPSCustom bearing  (nmea, "ORKST", 3);  // Kite STate bearing
TinyGPSCustom kpm1      (nmea, "ORKP1", 1);  // Proportional coefficient multiplicator
TinyGPSCustom kim1      (nmea, "ORKI1", 1);  // Integral coefficient multiplicator
TinyGPSCustom kdm1      (nmea, "ORKD1", 1);  // Derivative coefficient multiplicator
TinyGPSCustom kpm2      (nmea, "ORKP2", 1);  // Proportional coefficient multiplicator
TinyGPSCustom kim2      (nmea, "ORKI2", 1);  // Integral coefficient multiplicator
TinyGPSCustom kdm2      (nmea, "ORKD2", 1);  // Derivative coefficient multiplicator
TinyGPSCustom kpmr      (nmea, "ORKPR", 1);  // Proportional coefficient multiplicator
TinyGPSCustom kimr      (nmea, "ORKIR", 1);  // Integral coefficient multiplicator
TinyGPSCustom kdmr      (nmea, "ORKDR", 1);  // Derivative coefficient multiplicator
TinyGPSCustom feedback_request      (nmea, "ORFBR", 1);  // Feedback request

// Define Variables we'll be connecting to
double Setpoint1, Input1, Output1;
double Setpoint2, Input2, Output2;
double Input3;
double SetpointRoll, InputRoll, OutputRoll;

// 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);
PID myPIDRoll(&InputRoll, &OutputRoll, &SetpointRoll, 1, 0, 0, DIRECT);


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

// '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;

// Working variables for the interrupt routines
//
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;

uint8_t latest_interrupted_pin;
uint8_t interrupt_count[20] = {0}; // 20 possible arduino pins

int pinA = 2;
int pinB = 3;
int pinReset = 4;

boolean isAbsoluteReference = false;
boolean isConnectionAlive = false;

// These constants won't change.  They're used to give names
// to the pins used:
const int potPin = 1;  // Analog input pin that the potentiometer is attached to

int barrePotentiometerValue = 0;        // value read from the pot

double linearResolution = 0.005; //Resolution of the linear encoder (in meters)
double linearRange = 0.05; //Range of the sensor to normalize data (+/- linearRange/2)

double pi = 3.1415;
double potentiometerRangeDeg = 300; 
double potentiometerMaxRange = potentiometerRangeDeg * pi/180;
double potentiometerUsedRange = pi/3;
double potentiometerDistance = 0.05; //Distance from rotation axis to lever arm
//*********************************************************************************************

void setup()
{
  // Initialize software serial communication with Sabertooth 
  SWSerial.begin(9600);
  
  // Initialize serial communications at 9600 bps:
  Serial.begin(19200);
  Serial.println("Connected");
  
  // Initialize input and setpoint
  Input1 = 0;
  Input2 = 0;
  InputRoll = 0;
  Setpoint1 = 0;
  Setpoint2 = 0;
  SetpointRoll = 0;
  
  //turn the PID on
  myPID1.SetMode(MANUAL);
  myPID2.SetMode(MANUAL);
  myPIDRoll.SetMode(MANUAL);
  myPID1.SetOutputLimits(-1, 1);
  myPID2.SetOutputLimits(-1, 1);
  myPIDRoll.SetOutputLimits(-1, 1);
  
  //*****************************************************
  pinMode(pinA, INPUT);
  digitalWrite(pinA, HIGH);
  attachInterrupt(0, int0, CHANGE);
  //PCintPort::attachInterrupt(pinA, &int0, CHANGE);
  pinMode(pinB, INPUT);
  digitalWrite(pinB, HIGH);
  attachInterrupt(1, int1, CHANGE);
  //PCintPort::attachInterrupt(pinB, &int1, CHANGE);
  
  // Pin used to get absolute position
  pinMode(pinReset, INPUT);
  digitalWrite(pinReset, HIGH);
  PCintPort::attachInterrupt(pinReset, &reset, CHANGE);
  
  pinMode(ledPin, OUTPUT); 
  
}

void serialEvent() {
  isConnectionAlive = true;
  inputString="";
  Serial.flush();
  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;
    }
  }
  if (inputString[0] == 'i')
  {
    Serial.flush();
    isSerialControl = true;
    initialSensorValue = analogRead(analogInPin);// Read the joystick position to overide if joystick is moved
    if (inputString.substring(1)=="")
    {
      serialFrequency = 0;
    }
    else
    { 
      serialFrequency = StrToFloat(inputString.substring(1));
    } 
  }
  else
  {
    if (inputString == "o")
    {
      isSerialControl = false;
    }
    else
    {
      char c[50];
      inputString.toCharArray(c, 50);
      char *gpsStream = c;
      
      while (*gpsStream)
        if (nmea.encode(*gpsStream++))
      { 
        //Input1 = StrToFloat(elevation.value()); 
        lastSerialInputTime = millis();
        if (pwm1.isUpdated())
        { 
          myPID1.SetMode(MANUAL);

        if (feedback_request.isUpdated())
        {
          isFeedbackRequested = true;
          digitalWrite(LED_PIN, HIGH);
        }    
      }
    }
    // Clear the string:
    inputString = "";
    stringComplete = false;
  }
}

#define POT_RANGE_DEG      300
#define POT_USED_RANGE_DEG  60
#define POT_OFFSET        0.05 //Distance from rotation axis to lever arm

// Linear encoder
#define LINEAR_RESOLUTION 0.005
#define LINEAR_USED_RANGE 0.05
void computeFeedback()
{
  
  // Feedbacks are normalized between -1 and 1
  
  // Potentiomete angle
  float rawAngle_deg = analogRead(POT_PIN)/1024.0*POT_RANGE_DEG;
  Input1 = (rawAngle_deg - NEUTRAL_ANGLE_DEG)/POT_USED_RANGE_DEG;
  
  // Linear encoder position
  Input2 = halfSteps*LINEAR_RESOLUTION/2/LINEAR_USED_RANGE;
  // Correct for lever arm (potentiometer not on axis)
  Input2+= Input1* POT_OFFSET;
  
  // Line tension
  Input3 = analogRead(LINE_TENSION_PIN)/512 - 1;
}

void sendFeedback()
{
  if (isFeedbackRequested)
  {
    Serial.print((power1+128)*4);
    Serial.print(", ");
    Serial.print((power2+128)*4);
    Serial.print(", ");
    Serial.print((Input1+1)*512);
    Serial.print(", ");
    Serial.print((Input2+1)*512);
    Serial.print(", ");
    Serial.print((Input3+1)*512);
    Serial.println("");
    isFeedbackRequested = false;
  }
}

void computeOrder()
{
  if (myPID1.GetMode() == MANUAL)
  {
    power1 = atoi(pwm1.value());
  }
  if (myPID2.GetMode() == MANUAL)
  {
    power2 = atoi(pwm2.value());
  }
  if (myPID1.GetMode() == AUTOMATIC)
  {
    Setpoint1 = atoi(setpos1.value())/127.;
    myPID1.Compute();
    power1 = Output1*127;
  }
  if (myPID2.GetMode() == AUTOMATIC)
  {
    Setpoint2 = atoi(setpos2.value())/127.;
    Serial.println(Input2);
    myPID2.Compute();
    Serial.println(Output2);
    power2 = Output2*127;
  }
}

void sendOrder()
{
  // Order in the range -127/127
   ST.motor(1, power1);
   ST.motor(2, power2);
}

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