 #define POT_RANGE_DEG      300 // 300 is the value for standard potentiometer
 #define POT_USED_RANGE_DEG  60 // To normalize and saturate rotation
 #define POT_OFFSET        0.05 // Distance from rotation axis to lever arm (in m)
-#define NEUTRAL_ANGLE_DEG  225 // Zero of the potentiometer
+#define NEUTRAL_ANGLE_DEG    90 // Zero of the potentiometer
 // Linear encoder
 #define LINEAR_RESOLUTION 0.005// Resolution of the linear encoder
 #define LINEAR_USED_RANGE 0.05 // To normalize and saturate translation motion
+#define PI 3.1415
 
 #define ORDER_RATE_ms 100
 long last_order_ms = 0;

    Serial.print(", ");
    Serial.print((power2+127)*4);
    Serial.print(", ");
    Serial.print((Input1+1)/2*1023);
    Serial.print(", ");
    Serial.print((Input2/127+1)/2*1023);
    Serial.print(", ");
    Serial.print((Input3+1)/2*1023);
    Serial.println("");
    isFeedbackRequested = false;
  }

  // Saturate the output to the maximum range (values are normalized between -1 and 1)
  myPID1.SetOutputLimits(-1, 1);
  myPID2.SetOutputLimits(-1, 1);
  
  for (int i=0;i<4;i++)
  {
    data[i] = 127;
  }
  data[0] = NEUTRAL_ANGLE_DEG*255./POT_RANGE_DEG+127;
}

/*
