commit 88b719ac2fc6763bb9bc1ec4cb44af71e2c3bdde
Author: Hervé BECHER <hbdu68@gmail.com>
Date:   Sat Mar 16 19:57:51 2024 +0100

    Initial commit

diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..9994ca0
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,7 @@
+.DS_Store
+applet
+application.linux32
+application.linux64
+application.windows32
+application.windows64
+application.macosx
\ No newline at end of file
diff --git a/Buffer.pde b/Buffer.pde
new file mode 100644
index 0000000..08fdd44
--- /dev/null
+++ b/Buffer.pde
@@ -0,0 +1,255 @@
+import java.lang.reflect.Array;
+import java.util.Arrays;
+import java.util.Iterator;
+import java.util.LinkedList;
+import java.util.NoSuchElementException;
+import java.util.PrimitiveIterator;
+import java.util.function.Consumer;
+
+interface Buffer<T> extends Iterable<T> {
+  static <T> Buffer<T> from(T[] buffer) throws IllegalArgumentException {
+    if (buffer.length < 1) {
+      throw new IllegalArgumentException();
+    }
+
+    return new BoundedBufferArrayImpl<>(buffer);
+  }
+
+  static <T> Buffer<T> array(int capacity, Class<T> classOfT) throws IllegalArgumentException {
+    if (capacity < 1) {
+      throw new IllegalArgumentException();
+    }
+
+    var buffer = (T[]) Array.newInstance(classOfT, capacity);
+    return new BoundedBufferArrayImpl<>(buffer);
+  }
+
+  static <T> Buffer<T> boundedList(int capacity) throws IllegalArgumentException {
+    if (capacity < 1) {
+      throw new IllegalArgumentException();
+    }
+
+    return new BoundedBufferListImpl<>(capacity);
+  }
+
+  static <T> Buffer<T> unboundedList() throws IllegalArgumentException {
+    return new UnboundedBufferListImpl<>();
+  }
+
+  int capacity();
+
+  int size();
+
+  T get();
+
+  T get(int index);
+
+  void add(T value);
+
+  void clear();
+
+  default CappedIterator<T> cappedIterator(int maxCount) {
+    return new CappedIterator<>(iterator(), maxCount);
+  }
+
+  Iterator<T> descendingIterator();
+}
+
+static final class CappedIterator<T> implements Iterator<T> {
+  private final Iterator<T> iterator;
+  private final int maxCount;
+  private int count = 0;
+
+  CappedIterator(Iterator<T> iterator, int maxCount) {
+    this.iterator = iterator;
+    this.maxCount = maxCount;
+  }
+
+  public int count() {
+    return count;
+  }
+
+  public boolean hasNext() {
+    return count < maxCount && iterator.hasNext();
+  }
+
+  public T next() throws NoSuchElementException {
+    if (hasNext()) {
+      var next = iterator.next();
+      count++;
+      return next;
+    }
+
+    throw new NoSuchElementException();
+  }
+}
+
+static final class BoundedBufferArrayImpl<T> implements Buffer<T> {
+  private final T[] buffer;
+  private int size = 0, cursor = 0;
+
+  BoundedBufferArrayImpl(T[] buffer) throws IllegalArgumentException {
+    this.buffer = buffer;
+  }
+
+  public int capacity() {
+    return buffer.length;
+  }
+
+  public int size() {
+    return size;
+  }
+
+  public T get() {
+    return buffer[cursor];
+  }
+
+  public T get(int index) {
+    return buffer[(cursor + index) % buffer.length];
+  }
+
+  public void add(T value) {
+    cursor = Math.floorMod(cursor - 1, buffer.length);
+    buffer[cursor] = value;
+    size = Math.max(size + 1, buffer.length);
+  }
+
+  public void clear() {
+    size = 0;
+    Arrays.fill(buffer, null);
+  }
+
+  public Iterator<T> iterator() {
+    return new AscendingIterator();
+  }
+
+  public Iterator<T> descendingIterator() {
+    return new DescendingIterator();
+  }
+
+  private class AscendingIterator implements Iterator<T> {
+    private int pos = cursor, i = 0;
+
+    public boolean hasNext() {
+      return i < size;
+    }
+
+    public T next() throws NoSuchElementException {
+      if (hasNext()) {
+        var s = buffer[pos];
+        pos = (pos + 1) % buffer.length;
+
+        i++;
+
+        return s;
+      }
+
+      throw new NoSuchElementException();
+    }
+  }
+
+  private class DescendingIterator implements Iterator<T> {
+    private int pos = size - 1, i = 0;
+
+    public boolean hasNext() {
+      return i > 0;
+    }
+
+    public T next() throws NoSuchElementException {
+      if (hasNext()) {
+        var s = buffer[pos];
+        pos = Math.floorMod(pos - 1, buffer.length);
+
+        i--;
+
+        return s;
+      }
+
+      throw new NoSuchElementException();
+    }
+  }
+}
+
+static final class BoundedBufferListImpl<T> implements Buffer<T> {
+  private final LinkedList<T> buffer = new LinkedList<>();
+  private final int capacity;
+
+  BoundedBufferListImpl(int capacity) {
+    this.capacity = capacity;
+  }
+
+  public int capacity() {
+    return capacity;
+  }
+
+  public int size() {
+    return buffer.size();
+  }
+
+  public T get() {
+    return buffer.peekFirst();
+  }
+
+  public T get(int index) {
+    return buffer.get(index);
+  }
+
+  public void add(T value) {
+    if (size() == capacity) {
+      buffer.pollLast();
+    }
+
+    buffer.addFirst(value);
+  }
+
+  public void clear() {
+    buffer.clear();
+  }
+
+  public Iterator<T> iterator() {
+    return buffer.iterator();
+  }
+
+  public Iterator<T> descendingIterator() {
+    return buffer.descendingIterator();
+  }
+}
+
+static final class UnboundedBufferListImpl<T> implements Buffer<T> {
+  private final LinkedList<T> buffer = new LinkedList<>();
+
+  UnboundedBufferListImpl() {
+  }
+
+  public int capacity() {
+    return Integer.MAX_VALUE;
+  }
+
+  public int size() {
+    return buffer.size();
+  }
+
+  public T get() {
+    return buffer.peekFirst();
+  }
+
+  public T get(int index) {
+    return buffer.get(index);
+  }
+
+  public void add(T value) {
+    buffer.addFirst(value);
+  }
+
+  public void clear() {
+    buffer.clear();
+  }
+
+  public Iterator<T> iterator() {
+    return buffer.iterator();
+  }
+
+  public Iterator<T> descendingIterator() {
+    return buffer.descendingIterator();
+  }
+}
diff --git a/CruiseControlMX5.pde b/CruiseControlMX5.pde
new file mode 100644
index 0000000..6824a5b
--- /dev/null
+++ b/CruiseControlMX5.pde
@@ -0,0 +1,1961 @@
+import garciadelcastillo.dashedlines.*; //<>//
+import processing.serial.*;
+import controlP5.*;
+
+import java.util.Queue;
+import java.util.LinkedList;
+import java.util.concurrent.ConcurrentLinkedQueue;
+import java.util.regex.Pattern;
+
+
+static final Pattern FLOAT_REGEX = Pattern.compile("^-?(?:0|[1-9]\\d*)(?:\\.\\d+)?$");
+
+static final String COM_PORT = "COM7";
+static final int speedStart = 0, speedEnd = 240;
+static final int minSampleSize = 50, maxSampleSize = 600;
+static final float minTimeSpan = 5F, maxTimeSpan = 60F;
+
+static final float[] graphPos = new float[2];
+
+DashedLines dash;
+PFont consolas_16, consolas_16_bold;
+
+ControlP5 cp5;
+Textfield kpInput, kiInput, kdInput;
+Textfield errorRangeMinInput, errorRangeMaxInput, integralRangeMinInput, integralRangeMaxInput, derivativeRangeMinInput, derivativeRangeMaxInput, speedRangeMinInput, speedRangeMaxInput, accelerationRangeMinInput, accelerationRangeMaxInput, servoRangeMinInput, servoRangeMaxInput;
+Toggle autoScaleErrorToggle, autoScaleIntegralToggle, autoScaleDerivativeToggle, autoScaleSpeedToggle, autoScaleAccelerationToggle, autoScaleServoToggle;
+Slider sampleSizeSlider, timeSpanSlider;
+Toggle sampleOrTimeGraphToggle;
+Toggle smoothCurvesToggle;
+
+Rectangle kpArduinoRect, kiArduinoRect, kdArduinoRect;
+Rectangle errorGraphLabelRect, integralGraphLabelRect, derivativeGraphLabelRect, speedGraphLabelRect, accelerationGraphLabelRect, servoGraphLabelRect;
+Rectangle speedGaugeRect, graphRect, statusRect;
+Rectangle stateRect, switchFlagRect, speedDiffFlagRect, minSpeedFlagRect, maxSpeedFlagRect, brakeFlagRect, relayRect, satellitesRect;
+
+Serial serial;
+boolean hasIgnoredFirstPacket, hasReadSecondPacket;
+int timeZero;
+
+final Buffer<Sample> samples = Buffer.boundedList(maxSampleSize);
+final Queue<Sample> sampleQueue = new LinkedList<>();
+int sampleSize;
+float timeSpan;
+
+boolean showError = true, showIntegral = true, showDerivative = true, showSpeed, showAcceleration, showServo;
+boolean autoScaleError, autoScaleIntegral = true, autoScaleDerivative = true, autoScaleSpeed, autoScaleAcceleration = true, autoScaleServo = true;
+float errorRangeMin = -3F, errorRangeMax = 3F, integralRangeMin = Float.NaN, integralRangeMax = Float.NaN, derivativeRangeMin = Float.NaN, derivativeRangeMax = Float.NaN, speedRangeMin = 0F, speedRangeMax = 150F, accelerationRangeMin = Float.NaN, accelerationRangeMax = Float.NaN;
+int servoRangeMin = 1170, servoRangeMax = 1970;
+boolean sampleOrTimeGraph = false;
+boolean drawSmoothCurves;
+
+
+void setup() {
+  size(1600, 512);
+  pixelDensity(1);
+
+  windowTitle("Cruise Controller MX5");
+  
+  speedGaugeRect = Rectangle.xywh(32, 32, 300, 300);
+  graphRect = Rectangle.tlbr(speedGaugeRect.top, speedGaugeRect.right + 32, speedGaugeRect.top + 260, width - 32);
+  statusRect = Rectangle.xywh(-1, height - 32, width + 1, 32);
+
+  var splitRect = statusRect.splitVerticallyInto(8);
+  stateRect = splitRect[0];
+  switchFlagRect = splitRect[1];
+  speedDiffFlagRect = splitRect[2];
+  minSpeedFlagRect = splitRect[3];
+  maxSpeedFlagRect = splitRect[4];
+  brakeFlagRect = splitRect[5];
+  relayRect = splitRect[6];
+  satellitesRect = splitRect[7];
+
+  consolas_16 = createFont("Consolas", 16, true);
+  consolas_16_bold = createFont("Consolas Bold", 16, true);
+  textFont(consolas_16);
+
+  dash = new DashedLines(this);
+
+  errorGraphLabelRect = Rectangle.xywh(graphRect.left, graphRect.top - 23, floor(textWidth("[ERREUR]")), 18);
+  integralGraphLabelRect = Rectangle.xywh(errorGraphLabelRect.right + 16, errorGraphLabelRect.top, floor(textWidth("[INTÉGRAL]")), errorGraphLabelRect.height);
+  derivativeGraphLabelRect = Rectangle.xywh(integralGraphLabelRect.right + 16, errorGraphLabelRect.top, floor(textWidth("[DÉRIVATIF]")), errorGraphLabelRect.height);
+  speedGraphLabelRect = Rectangle.xywh(derivativeGraphLabelRect.right + 16, errorGraphLabelRect.top, floor(textWidth("[VITESSE]")), errorGraphLabelRect.height);
+  accelerationGraphLabelRect = Rectangle.xywh(speedGraphLabelRect.right + 16, errorGraphLabelRect.top, floor(textWidth("[ACCÉLÉRATION]")), errorGraphLabelRect.height);
+  servoGraphLabelRect = Rectangle.xywh(accelerationGraphLabelRect.right + 16, errorGraphLabelRect.top, floor(textWidth("[SERVO]")), errorGraphLabelRect.height);
+
+  cp5 = new ControlP5(this);
+  ControlFont font = new ControlFont(consolas_16);
+
+  kpInput = cp5.addTextfield("_kpInput")
+    .setPosition(87, 344)
+    .setSize(91, 24)
+    .setFont(font)
+    .setCaptionLabel("")
+    .setId(1);
+
+  kiInput = cp5.addTextfield("_kiInput")
+    .setPosition(87, 374)
+    .setSize(91, 24)
+    .setFont(font)
+    .setCaptionLabel("")
+    .setId(2);
+
+  kdInput = cp5.addTextfield("_kdInput")
+    .setPosition(87, 404)
+    .setSize(91, 24)
+    .setFont(font)
+    .setCaptionLabel("")
+    .setId(3);
+
+  sampleSizeSlider = cp5.addSlider("sampleSize")
+    .setPosition(graphRect.left + floor(textWidth("Nombre d'échantillons")) + 8, graphRect.bottom + 9)
+    .setSize((maxSampleSize - minSampleSize) / 3, 20)
+    .setSliderMode(Slider.FLEXIBLE)
+    .setRange(minSampleSize, maxSampleSize)
+    .setValue(100)
+    .setNumberOfTickMarks((maxSampleSize - minSampleSize) / 10 + 1)
+    .showTickMarks(false)
+    .snapToTickMarks(true)
+    .setFont(font)
+    .setCaptionLabel("");
+  var pos = sampleSizeSlider.getPosition();
+
+  sampleOrTimeGraphToggle = cp5.addToggle("sampleOrTimeGraph")
+    .setPosition(pos[0] + sampleSizeSlider.getWidth() + 16F, pos[1])
+    .setSize(50, 20)
+    .setMode(ControlP5.SWITCH)
+    .setCaptionLabel("");
+  pos = sampleOrTimeGraphToggle.getPosition();
+
+  timeSpanSlider = cp5.addSlider("timeSpan")
+    .setPosition(pos[0] + sampleOrTimeGraphToggle.getWidth() + 16F + floor(textWidth("Intervalle de temps")) + 8F, pos[1])
+    .setSize(int(maxTimeSpan - minTimeSpan) * 3, 20)
+    .setSliderMode(Slider.FLEXIBLE)
+    .setRange(minTimeSpan, maxTimeSpan)
+    .setValue(minTimeSpan * 2)
+    .setNumberOfTickMarks(int(maxTimeSpan - minTimeSpan) + 1)
+    .showTickMarks(false)
+    .snapToTickMarks(true)
+    .setFont(font)
+    .setCaptionLabel("");
+
+  //var saveSamplesButton = cp5.addButton("saveSamples")
+  //  .setPosition(width - 32 - 80, 8)
+  //  .setSize(80, 20)
+  //  .setFont(font)
+  //  .setCaptionLabel("Sauver");
+  //pos = saveSamplesButton.getPosition();
+
+  smoothCurvesToggle = cp5.addToggle("drawSmoothCurves")
+    .setPosition(width - 32 - 150, 8)
+    .setSize(150, 20)
+    .setFont(font)
+    .setCaptionLabel("Courbes lisses");
+  smoothCurvesToggle.getCaptionLabel().align(ControlP5.CENTER, ControlP5.CENTER);
+
+  errorRangeMinInput = cp5.addTextfield("setErrorRangeMin")
+    .setPosition(1323, 323)
+    .setSize(91, 22)
+    .setAutoClear(false)
+    .setFont(font)
+    .setCaptionLabel("");
+  if (!Float.isNaN(errorRangeMin)) {
+    errorRangeMinInput.setText(str(errorRangeMin));
+  }
+
+  errorRangeMaxInput = cp5.addTextfield("setErrorRangeMax")
+    .setPosition(1418, 323)
+    .setSize(91, 22)
+    .setAutoClear(false)
+    .setFont(font)
+    .setCaptionLabel("");
+  if (!Float.isNaN(errorRangeMax)) {
+    errorRangeMaxInput.setText(str(errorRangeMax));
+  }
+
+  integralRangeMinInput = cp5.addTextfield("setIntegralRangeMin")
+    .setPosition(1323, 347)
+    .setSize(91, 22)
+    .setAutoClear(false)
+    .setFont(font)
+    .setCaptionLabel("");
+  if (!Float.isNaN(integralRangeMin)) {
+    integralRangeMinInput.setText(str(integralRangeMin));
+  }
+
+  integralRangeMaxInput = cp5.addTextfield("setIntegralRangeMax")
+    .setPosition(1418, 347)
+    .setSize(91, 22)
+    .setAutoClear(false)
+    .setFont(font)
+    .setCaptionLabel("");
+  if (!Float.isNaN(integralRangeMax)) {
+    integralRangeMaxInput.setText(str(integralRangeMax));
+  }
+
+  derivativeRangeMinInput = cp5.addTextfield("setDerivativeRangeMin")
+    .setPosition(1323, 371)
+    .setSize(91, 22)
+    .setAutoClear(false)
+    .setFont(font)
+    .setCaptionLabel("");
+  if (!Float.isNaN(derivativeRangeMin)) {
+    derivativeRangeMinInput.setText(str(derivativeRangeMin));
+  }
+
+  derivativeRangeMaxInput = cp5.addTextfield("setDerivativeRangeMax")
+    .setPosition(1418, 371)
+    .setSize(91, 22)
+    .setAutoClear(false)
+    .setFont(font)
+    .setCaptionLabel("");
+  if (!Float.isNaN(derivativeRangeMax)) {
+    derivativeRangeMaxInput.setText(str(derivativeRangeMax));
+  }
+
+  speedRangeMinInput = cp5.addTextfield("setSpeedRangeMin")
+    .setPosition(1323, 395)
+    .setSize(91, 22)
+    .setAutoClear(false)
+    .setFont(font)
+    .setCaptionLabel("");
+  if (!Float.isNaN(speedRangeMin)) {
+    speedRangeMinInput.setText(str(speedRangeMin));
+  }
+
+  speedRangeMaxInput = cp5.addTextfield("setSpeedRangeMax")
+    .setPosition(1418, 395)
+    .setSize(91, 22)
+    .setAutoClear(false)
+    .setFont(font)
+    .setCaptionLabel("");
+  if (!Float.isNaN(speedRangeMax)) {
+    speedRangeMaxInput.setText(str(speedRangeMax));
+  }
+
+  accelerationRangeMinInput = cp5.addTextfield("setAccelerationRangeMin")
+    .setPosition(1323, 419)
+    .setSize(91, 22)
+    .setAutoClear(false)
+    .setFont(font)
+    .setCaptionLabel("");
+  if (!Float.isNaN(accelerationRangeMin)) {
+    accelerationRangeMinInput.setText(str(accelerationRangeMin));
+  }
+
+  accelerationRangeMaxInput = cp5.addTextfield("setAccelerationRangeMax")
+    .setPosition(1418, 419)
+    .setSize(91, 22)
+    .setAutoClear(false)
+    .setFont(font)
+    .setCaptionLabel("");
+  if (!Float.isNaN(accelerationRangeMax)) {
+    accelerationRangeMaxInput.setText(str(accelerationRangeMax));
+  }
+
+  servoRangeMinInput = cp5.addTextfield("setServoRangeMin")
+    .setPosition(1323, 443)
+    .setSize(91, 22)
+    .setAutoClear(false)
+    .setFont(font)
+    .setCaptionLabel("");
+  if (servoRangeMin != Integer.MIN_VALUE) {
+    servoRangeMinInput.setText(str(servoRangeMin));
+  }
+
+  servoRangeMaxInput = cp5.addTextfield("setServoRangeMax")
+    .setPosition(1418, 443)
+    .setSize(91, 22)
+    .setAutoClear(false)
+    .setFont(font)
+    .setCaptionLabel("");
+  if (servoRangeMax != Integer.MIN_VALUE) {
+    servoRangeMaxInput.setText(str(servoRangeMax));
+  }
+
+  autoScaleErrorToggle = cp5.addToggle("autoScaleError")
+    .setPosition(width - 32 - 50, 324)
+    .setSize(50, 20)
+    .setFont(font);
+  autoScaleErrorToggle.getCaptionLabel().align(ControlP5.CENTER, ControlP5.CENTER);
+  updateAutoScaleToggleLabel(autoScaleErrorToggle);
+
+  autoScaleIntegralToggle = cp5.addToggle("autoScaleIntegral")
+    .setPosition(width - 32 - 50, 348)
+    .setSize(50, 20)
+    .setFont(font);
+  autoScaleIntegralToggle.getCaptionLabel().align(ControlP5.CENTER, ControlP5.CENTER);
+  updateAutoScaleToggleLabel(autoScaleIntegralToggle);
+
+  autoScaleDerivativeToggle = cp5.addToggle("autoScaleDerivative")
+    .setPosition(width - 32 - 50, 372)
+    .setSize(50, 20)
+    .setFont(font);
+  autoScaleDerivativeToggle.getCaptionLabel().align(ControlP5.CENTER, ControlP5.CENTER);
+  updateAutoScaleToggleLabel(autoScaleDerivativeToggle);
+
+  autoScaleSpeedToggle = cp5.addToggle("autoScaleSpeed")
+    .setPosition(width - 32 - 50, 396)
+    .setSize(50, 20)
+    .setFont(font);
+  autoScaleSpeedToggle.getCaptionLabel().align(ControlP5.CENTER, ControlP5.CENTER);
+  updateAutoScaleToggleLabel(autoScaleSpeedToggle);
+
+  autoScaleAccelerationToggle = cp5.addToggle("autoScaleAcceleration")
+    .setPosition(width - 32 - 50, 420)
+    .setSize(50, 20)
+    .setFont(font);
+  autoScaleAccelerationToggle.getCaptionLabel().align(ControlP5.CENTER, ControlP5.CENTER);
+  updateAutoScaleToggleLabel(autoScaleAccelerationToggle);
+
+  autoScaleServoToggle = cp5.addToggle("autoScaleServo")
+    .setPosition(width - 32 - 50, 444)
+    .setSize(50, 20)
+    .setFont(font);
+  autoScaleServoToggle.getCaptionLabel().align(ControlP5.CENTER, ControlP5.CENTER);
+  updateAutoScaleToggleLabel(autoScaleServoToggle);
+
+  kpArduinoRect = Rectangle.xywh(186, 344, 91, 23);
+  kiArduinoRect = Rectangle.xywh(kpArduinoRect.left, kpArduinoRect.top + 30, kpArduinoRect.width, kpArduinoRect.height);
+  kdArduinoRect = Rectangle.xywh(kpArduinoRect.left, kiArduinoRect.top + 30, kpArduinoRect.width, kpArduinoRect.height);
+
+  println("Available serial ports");
+  printArray(Serial.list());
+  println("Trying %s".formatted(COM_PORT));
+
+  try {
+    serial = new Serial(this, COM_PORT, 115200);
+    serial.bufferUntil('\n');
+  }
+  catch (Exception e) {
+    println(e.getMessage());
+  }
+}
+
+void draw() {
+  var frameTimeStart = millis();
+
+  background(0);
+  textFont(consolas_16);
+
+  while (!sampleQueue.isEmpty()) {
+    samples.add(sampleQueue.poll());
+  }
+
+  var latestSample = samples.get();
+
+  drawSpeedGauge(latestSample);
+  drawKPID(latestSample);
+  drawGraph(frameTimeStart);
+  drawValues(latestSample);
+  drawStatusBar(latestSample);
+
+  pushStyle();
+
+  fill(200);
+  textAlign(LEFT, TOP);
+  text("%.1f fps / %d mspf".formatted(frameRate, millis() - frameTimeStart), 4, 4);
+
+  popStyle();
+}
+
+void drawSpeedGauge(Sample sample) {
+  push();
+
+  var speedGaugeDiameter = speedGaugeRect.width;
+  var speedGaugeRadius = speedGaugeDiameter / 2F;
+  var speedGaugeX = speedGaugeRect.centerX();
+  var speedGaugeY = speedGaugeRect.centerY();
+  var speedGaugeStart = PI - QUARTER_PI;
+  var speedGaugeEnd = TWO_PI + QUARTER_PI;
+  float speed = speedStart;
+
+  if (sample != null) {
+    speed = constrain(sample.speed, speedStart, speedEnd);
+
+    pushStyle();
+
+    stroke(159, 0, 0);
+    strokeWeight(8);
+    strokeCap(SQUARE);
+    noFill();
+
+    if (sample.speedMin > speedStart) {
+      arc(speedGaugeX, speedGaugeY, speedGaugeDiameter - 8F, speedGaugeDiameter - 8F, speedGaugeStart, map(sample.speedMin, speedStart, speedEnd, speedGaugeStart, speedGaugeEnd));
+    }
+
+    if (sample.speedMax < speedEnd) {
+      arc(speedGaugeX, speedGaugeY, speedGaugeDiameter - 8F, speedGaugeDiameter - 8F, map(sample.speedMax, speedStart, speedEnd, speedGaugeStart, speedGaugeEnd), speedGaugeEnd);
+    }
+
+    popStyle();
+  }
+
+  stroke(127, 180, 127);
+  strokeWeight(1);
+  noFill();
+  arc(speedGaugeX, speedGaugeY, speedGaugeDiameter - 16F, speedGaugeDiameter - 16F, speedGaugeStart, speedGaugeEnd);
+
+  textAlign(CENTER, CENTER);
+  textSize(18);
+
+  for (int i = speedStart, j = 0; i <= speedEnd; i += 10, j++) {
+    var a = map(i, speedStart, speedEnd, speedGaugeStart, speedGaugeEnd);
+    var bigTick = j % 3 == 0;
+    var cosA = cos(a);
+    var sinA = sin(a);
+
+    var tickX = speedGaugeX + speedGaugeRadius * cosA;
+    var tickY = speedGaugeY + speedGaugeRadius * sinA;
+    var tickX2 = speedGaugeX + (speedGaugeRadius - (bigTick ? 15 : 8)) * cosA;
+    var tickY2 = speedGaugeY + (speedGaugeRadius - (bigTick ? 15 : 8)) * sinA;
+
+    if (bigTick) {
+      stroke(255, 127, 0);
+    } else {
+      stroke(127, 180, 127);
+    }
+    strokeWeight(bigTick ? 3 : 1);
+    line(tickX, tickY, tickX2, tickY2);
+
+    if (bigTick) {
+      noStroke();
+      fill(255);
+      text(i, speedGaugeX + (speedGaugeRadius - 34) * cosA, speedGaugeY + (speedGaugeRadius - 34) * sinA);
+    }
+  }
+
+  stroke(127, 180, 127);
+  strokeWeight(3);
+  noFill();
+  arc(speedGaugeX, speedGaugeY, speedGaugeDiameter + 1, speedGaugeDiameter + 1, speedGaugeStart, speedGaugeEnd);
+
+  noStroke();
+  fill(255);
+  textSize(22);
+  text("KPH", speedGaugeX, speedGaugeY - 40);
+  textSize(16);
+
+  stroke(200);
+  strokeWeight(1);
+  fill(31);
+  rectMode(CENTER);
+  rect(speedGaugeX, speedGaugeY + 60, 66, 26);
+
+  rectMode(CORNER);
+
+  noStroke();
+  fill(255);
+  text(nf(speed, 0, 2), speedGaugeX, speedGaugeY + 60);
+
+  var speedAngle = map(speed, speedStart, speedEnd, speedGaugeStart, speedGaugeEnd);
+  stroke(255, 127, 0);
+  strokeWeight(4);
+  line(speedGaugeX - 20 * cos(speedAngle), speedGaugeY - 20 * sin(speedAngle), speedGaugeX + (speedGaugeRadius - 20) * cos(speedAngle), speedGaugeY + (speedGaugeRadius - 20) * sin(speedAngle));
+
+  stroke(0);
+  strokeWeight(4);
+  fill(127);
+  circle(speedGaugeX, speedGaugeY, 20);
+
+  if (sample != null) {
+    var consigne = sample.consigne;
+
+    if (consigne > 0) {
+      var consigneAngle = map(constrain(consigne, speedStart, speedEnd), speedStart, speedEnd, speedGaugeStart, speedGaugeEnd);
+
+      noStroke();
+      fill(60, 255, 60);
+      pushMatrix();
+      translate(speedGaugeX, speedGaugeY);
+      rotate(consigneAngle);
+      translate(speedGaugeRadius, 0);
+      triangle(4, 0, 12, 8, 12, -8);
+      stroke(60, 255, 60);
+      strokeWeight(3);
+      strokeCap(SQUARE);
+      line(8, 0, -8, 0);
+      strokeCap(ROUND);
+      popMatrix();
+
+      stroke(60, 255, 60);
+      strokeWeight(1);
+      fill(31);
+      rectMode(CENTER);
+      rect(speedGaugeX, speedGaugeY + 95, 66, 26);
+      rectMode(CORNER);
+
+      noStroke();
+      fill(60, 255, 60);
+      text(nf(consigne, 0, 2), speedGaugeX, speedGaugeY + 95);
+    }
+  }
+
+  pop();
+}
+
+void drawKPID(Sample sample) {
+  push();
+
+  noStroke();
+  fill(255);
+  textAlign(RIGHT, TOP);
+  text("kp", 81, 350);
+  text("ki", 81, 380);
+  text("kd", 81, 410);
+  textAlign(CENTER, BOTTOM);
+  text("Réglage", 133, 338);
+  text("Actuel", 232, 338);
+
+  stroke(127);
+  strokeWeight(1);
+  fill(63);
+  rect(kpArduinoRect.left, kpArduinoRect.top, kpArduinoRect.width, kpArduinoRect.height);
+  rect(kiArduinoRect.left, kiArduinoRect.top, kiArduinoRect.width, kiArduinoRect.height);
+  rect(kdArduinoRect.left, kdArduinoRect.top, kdArduinoRect.width, kdArduinoRect.height);
+
+  if (sample != null) {
+    fill(255);
+    textAlign(LEFT, CENTER);
+    clip(kpArduinoRect.left, kpArduinoRect.top, kpArduinoRect.width, kpArduinoRect.height);
+    text(str(sample.kp), kpArduinoRect.left + 4, kpArduinoRect.centerY());
+    clip(kiArduinoRect.left, kiArduinoRect.top, kiArduinoRect.width, kiArduinoRect.height);
+    text(str(sample.ki), kiArduinoRect.left + 4, kiArduinoRect.centerY());
+    clip(kdArduinoRect.left, kdArduinoRect.top, kdArduinoRect.width, kdArduinoRect.height);
+    text(str(sample.kd), kdArduinoRect.left + 4, kdArduinoRect.centerY());
+    noClip();
+  }
+
+  pop();
+}
+
+void drawGraph(int frameTimeStart) {
+  pushStyle();
+
+  textAlign(LEFT, BOTTOM);
+  fill(127, 127, 255);
+  text(showError ? "[ERREUR]" : " ERREUR", errorGraphLabelRect.left, errorGraphLabelRect.bottom);
+  fill(255, 255, 0);
+  text(showIntegral ? "[INTÉGRAL]" : " INTÉGRAL", integralGraphLabelRect.left, integralGraphLabelRect.bottom);
+  fill(255, 63, 63);
+  text(showDerivative ? "[DÉRIVATIF]" : " DÉRIVATIF", derivativeGraphLabelRect.left, derivativeGraphLabelRect.bottom);
+  fill(127, 255, 255);
+  text(showSpeed ? "[VITESSE]" : " VITESSE", speedGraphLabelRect.left, speedGraphLabelRect.bottom);
+  fill(127, 255, 127);
+  text(showAcceleration ? "[ACCÉLÉRATION]" : " ACCÉLÉRATION", accelerationGraphLabelRect.left, accelerationGraphLabelRect.bottom);
+  fill(255, 127, 255);
+  text(showServo ? "[SERVO]" : " SERVO", servoGraphLabelRect.left, servoGraphLabelRect.bottom);
+
+  popStyle();
+
+  push();
+
+  var maxTimestamp = frameTimeStart - timeZero;
+  var minTimestamp = maxTimestamp - timeSpan * 1000F;
+
+  int sampleSize;
+  if (sampleOrTimeGraph) {
+    sampleSize = this.sampleSize;
+  } else {
+    var i = 0;
+
+    for (var sample : samples) {
+      i++;
+
+      if (sample.timestamp < minTimestamp) {
+        break;
+      }
+    }
+
+    sampleSize = i;
+  }
+
+  translate(graphRect.left, graphRect.top);
+
+  final var graphMargin = 24;
+
+  stroke(100);
+  strokeWeight(1);
+  dash.pattern(5, 5);
+  dash.line(0, graphMargin - 1, graphRect.width, graphMargin - 1);
+  dash.line(0, graphRect.height - graphMargin + 1, graphRect.width, graphRect.height - graphMargin + 1);
+
+  dash.pattern(3, 7);
+
+  if (sampleOrTimeGraph) {
+    for (var iterator = samples.cappedIterator(sampleSize); iterator.hasNext(); ) {
+      var i = iterator.count();
+      var sample = iterator.next();
+
+      if (sample.drawTimeTick) {
+        var tickX = map(i, 0, sampleSize, graphRect.width, 0);
+
+        dash.line(tickX, graphRect.height - graphMargin, tickX, graphMargin);
+      }
+    }
+  } else {
+    var frameTimePast = frameTimeStart - timeSpan * 1000F;
+    var prevT = floor(map(-1, 0, graphRect.width - 1, frameTimePast, frameTimeStart) / 1000F);
+
+    for (var i = 0; i < graphRect.width; i++) {
+      var t = floor(map(i, 0, graphRect.width - 1, frameTimePast, frameTimeStart) / 1000F);
+
+      if (t != prevT) {
+        dash.line(i, graphRect.height - graphMargin, i, graphMargin);
+      }
+
+      prevT = t;
+    }
+  }
+
+  // ERREUR
+
+  if (showError) {
+    errorRangeMin = float(errorRangeMinInput.getText());
+    errorRangeMax = float(errorRangeMaxInput.getText());
+
+    var autoScale = autoScaleError || errorRangeMin == errorRangeMax;
+    var autoScaleMin = autoScale || Float.isNaN(errorRangeMin);
+    var autoScaleMax = autoScale || Float.isNaN(errorRangeMax);
+
+    var minValue = autoScaleMin ? Float.POSITIVE_INFINITY : errorRangeMin;
+    var maxValue = autoScaleMax ? Float.NEGATIVE_INFINITY : errorRangeMax;
+    var sampleCount = 0;
+
+    for (var iterator = samples.cappedIterator(sampleSize); iterator.hasNext(); ) {
+      var sample = iterator.next();
+
+      if (sample.state != 2) {
+        continue;
+      }
+
+      sampleCount++;
+
+      var value = sample.error;
+
+      if (autoScaleMin && value < minValue) {
+        minValue = value;
+      }
+
+      if (autoScaleMax && value > maxValue) {
+        maxValue = value;
+      }
+    }
+
+    if (sampleCount > 0) {
+      clip(0, graphMargin, graphRect.width, graphRect.height - 2 * graphMargin + 1);
+
+      if (minValue <= 0 && maxValue >= 0) {
+        var zero = minValue == maxValue ? graphRect.height / 2 : map(0, minValue, maxValue, graphRect.height - graphMargin, graphMargin);
+
+        stroke(63, 63, 127);
+        strokeWeight(1);
+        dash.pattern(3, 3);
+        dash.line(0, zero, graphRect.width, zero);
+      }
+
+      stroke(127, 127, 255);
+      strokeWeight(1);
+
+      var pointsDrawn = 0;
+
+      var i = 0;
+
+      for (var iterator = samples.cappedIterator(sampleSize); iterator.hasNext(); ) {
+        i = iterator.count();
+        var sample = iterator.next();
+
+        if (sample.state != 2) {
+          if (pointsDrawn > 0) {
+            if (drawSmoothCurves && pointsDrawn > 1) {
+              var nextSample1 = samples.get(i - 1);
+              var nextSample2 = samples.get(i - 2);
+
+              if (nextSample1 != null && nextSample2 != null) {
+                calculateGraphPosition(graphPos, graphMargin, i, sampleSize, nextSample1.error, minValue, maxValue, nextSample1.timestamp, minTimestamp, maxTimestamp);
+                var nextX1 = graphPos[0];
+                var nextY1 = graphPos[1];
+
+                calculateGraphPosition(graphPos, graphMargin, i, sampleSize, nextSample2.error, minValue, maxValue, nextSample2.timestamp, minTimestamp, maxTimestamp);
+                var nextX2 = graphPos[0];
+                var nextY2 = graphPos[1];
+
+                curveVertex(nextX1 - (nextX2 - nextX1), nextY1 - (nextY2 - nextY1));
+              }
+            }
+
+            endShape();
+
+            pointsDrawn = 0;
+          }
+
+          continue;
+        }
+
+        calculateGraphPosition(graphPos, graphMargin, i, sampleSize, sample.error, minValue, maxValue, sample.timestamp, minTimestamp, maxTimestamp);
+        var x = graphPos[0];
+        var y = graphPos[1];
+
+        if (pointsDrawn == 0) {
+          beginShape();
+        }
+
+        noFill();
+
+        if (drawSmoothCurves) {
+          if (pointsDrawn == 0) {
+            var previousSample = samples.get(i + 1);
+
+            if (previousSample != null) {
+              calculateGraphPosition(graphPos, graphMargin, i, sampleSize, previousSample.error, minValue, maxValue, previousSample.timestamp, minTimestamp, maxTimestamp);
+              var prevX = graphPos[0];
+              var prevY = graphPos[1];
+
+              curveVertex(x - prevX + x, y - prevY + y);
+            }
+          }
+
+          curveVertex(x, y);
+        } else {
+          vertex(x, y);
+        }
+
+        pointsDrawn++;
+      }
+
+      if (pointsDrawn > 0) {
+        if (drawSmoothCurves && pointsDrawn > 1) {
+          var nextSample1 = samples.get(i - 1);
+          var nextSample2 = samples.get(i - 2);
+
+          if (nextSample1 != null && nextSample2 != null) {
+            calculateGraphPosition(graphPos, graphMargin, i, sampleSize, nextSample1.error, minValue, maxValue, nextSample1.timestamp, minTimestamp, maxTimestamp);
+            var nextX1 = graphPos[0];
+            var nextY1 = graphPos[1];
+
+            calculateGraphPosition(graphPos, graphMargin, i, sampleSize, nextSample2.error, minValue, maxValue, nextSample2.timestamp, minTimestamp, maxTimestamp);
+            var nextX2 = graphPos[0];
+            var nextY2 = graphPos[1];
+
+            curveVertex(nextX1 - (nextX2 - nextX1), nextY1 - (nextY2 - nextY1));
+          }
+        }
+
+        endShape();
+      }
+
+      noClip();
+
+      fill(127, 127, 255);
+      textAlign(LEFT, TOP);
+      text(nfs(maxValue, 0, 3), 4, 6);
+      textAlign(LEFT, BOTTOM);
+      text(nfs(minValue, 0, 3), 4, graphRect.height - 3);
+      textAlign(LEFT);
+    }
+  }
+
+  // INTEGRAL
+
+  if (showIntegral) {
+    integralRangeMin = float(integralRangeMinInput.getText());
+    integralRangeMax = float(integralRangeMaxInput.getText());
+
+    var autoScale = autoScaleIntegral || integralRangeMin == integralRangeMax;
+    var autoScaleMin = autoScale || Float.isNaN(integralRangeMin);
+    var autoScaleMax = autoScale || Float.isNaN(integralRangeMax);
+
+    var minValue = autoScaleMin ? Float.POSITIVE_INFINITY : integralRangeMin;
+    var maxValue = autoScaleMax ? Float.NEGATIVE_INFINITY : integralRangeMax;
+    var sampleCount = 0;
+
+    for (var iterator = samples.cappedIterator(sampleSize); iterator.hasNext(); ) {
+      var sample = iterator.next();
+
+      if (sample.state != 2) {
+        continue;
+      }
+
+      sampleCount++;
+
+      var value = sample.integral;
+
+      if (autoScaleMin && value < minValue) {
+        minValue = value;
+      }
+
+      if (autoScaleMax && value > maxValue) {
+        maxValue = value;
+      }
+    }
+
+    if (sampleCount > 0) {
+      pushStyle();
+
+      stroke(255, 255, 0);
+      strokeWeight(1);
+
+      clip(0, graphMargin, graphRect.width, graphRect.height - 2 * graphMargin + 1);
+
+      var pointsDrawn = 0;
+
+      var i = 0;
+
+      for (var iterator = samples.cappedIterator(sampleSize); iterator.hasNext(); ) {
+        i = iterator.count();
+        var sample = iterator.next();
+
+        if (sample.state != 2) {
+          if (pointsDrawn > 0) {
+            if (drawSmoothCurves && pointsDrawn > 1) {
+              var nextSample1 = samples.get(i - 1);
+              var nextSample2 = samples.get(i - 2);
+
+              if (nextSample1 != null && nextSample2 != null) {
+                calculateGraphPosition(graphPos, graphMargin, i, sampleSize, nextSample1.integral, minValue, maxValue, nextSample1.timestamp, minTimestamp, maxTimestamp);
+                var nextX1 = graphPos[0];
+                var nextY1 = graphPos[1];
+
+                calculateGraphPosition(graphPos, graphMargin, i, sampleSize, nextSample2.integral, minValue, maxValue, nextSample2.timestamp, minTimestamp, maxTimestamp);
+                var nextX2 = graphPos[0];
+                var nextY2 = graphPos[1];
+
+                curveVertex(nextX1 - (nextX2 - nextX1), nextY1 - (nextY2 - nextY1));
+              }
+            }
+
+            endShape();
+
+            pointsDrawn = 0;
+          }
+
+          continue;
+        }
+
+        calculateGraphPosition(graphPos, graphMargin, i, sampleSize, sample.integral, minValue, maxValue, sample.timestamp, minTimestamp, maxTimestamp);
+        var x = graphPos[0];
+        var y = graphPos[1];
+
+        if (pointsDrawn == 0) {
+          beginShape();
+        }
+
+        noFill();
+
+        if (drawSmoothCurves) {
+          if (pointsDrawn == 0) {
+            var previousSample = samples.get(i + 1);
+
+            if (previousSample != null) {
+              calculateGraphPosition(graphPos, graphMargin, i, sampleSize, previousSample.integral, minValue, maxValue, previousSample.timestamp, minTimestamp, maxTimestamp);
+              var prevX = graphPos[0];
+              var prevY = graphPos[1];
+
+              curveVertex(x - prevX + x, y - prevY + y);
+            }
+          }
+
+          curveVertex(x, y);
+        } else {
+          vertex(x, y);
+        }
+
+        pointsDrawn++;
+      }
+
+      if (pointsDrawn > 0) {
+        if (drawSmoothCurves && pointsDrawn > 1) {
+          var nextSample1 = samples.get(i - 1);
+          var nextSample2 = samples.get(i - 2);
+
+          if (nextSample1 != null && nextSample2 != null) {
+            calculateGraphPosition(graphPos, graphMargin, i, sampleSize, nextSample1.integral, minValue, maxValue, nextSample1.timestamp, minTimestamp, maxTimestamp);
+            var nextX1 = graphPos[0];
+            var nextY1 = graphPos[1];
+
+            calculateGraphPosition(graphPos, graphMargin, i, sampleSize, nextSample2.integral, minValue, maxValue, nextSample2.timestamp, minTimestamp, maxTimestamp);
+            var nextX2 = graphPos[0];
+            var nextY2 = graphPos[1];
+
+            curveVertex(nextX1 - (nextX2 - nextX1), nextY1 - (nextY2 - nextY1));
+          }
+        }
+
+        endShape();
+      }
+
+      noClip();
+
+      fill(255, 255, 0);
+      textAlign(LEFT, TOP);
+      text(nfs(maxValue, 0, 3), 90, 6);
+      textAlign(LEFT, BOTTOM);
+      text(nfs(minValue, 0, 3), 90, graphRect.height - 3);
+
+      popStyle();
+    }
+  }
+
+  // DERIVATIF
+
+  if (showDerivative) {
+    derivativeRangeMin = float(derivativeRangeMinInput.getText());
+    derivativeRangeMax = float(derivativeRangeMaxInput.getText());
+
+    var autoScale = autoScaleDerivative || derivativeRangeMin == derivativeRangeMax;
+    var autoScaleMin = autoScale || Float.isNaN(derivativeRangeMin);
+    var autoScaleMax = autoScale || Float.isNaN(derivativeRangeMax);
+
+    var minValue = autoScaleMin ? Float.POSITIVE_INFINITY : derivativeRangeMin;
+    var maxValue = autoScaleMax ? Float.NEGATIVE_INFINITY : derivativeRangeMax;
+    var sampleCount = 0;
+
+    for (var iterator = samples.cappedIterator(sampleSize); iterator.hasNext(); ) {
+      var sample = iterator.next();
+
+      if (sample.state != 2) {
+        continue;
+      }
+
+      sampleCount++;
+
+      var value = sample.derivative;
+
+      if (autoScaleMin && value < minValue) {
+        minValue = value;
+      }
+
+      if (autoScaleMax && value > maxValue) {
+        maxValue = value;
+      }
+    }
+
+    if (sampleCount > 0) {
+      pushStyle();
+
+      stroke(255, 63, 63);
+      strokeWeight(1);
+
+      clip(0, graphMargin, graphRect.width, graphRect.height - 2 * graphMargin + 1);
+
+      var pointsDrawn = 0;
+
+      var i = 0;
+
+      for (var iterator = samples.cappedIterator(sampleSize); iterator.hasNext(); ) {
+        i = iterator.count();
+        var sample = iterator.next();
+
+        if (sample.state != 2) {
+          if (pointsDrawn > 0) {
+            if (drawSmoothCurves && pointsDrawn > 1) {
+              var nextSample1 = samples.get(i - 1);
+              var nextSample2 = samples.get(i - 2);
+
+              if (nextSample1 != null && nextSample2 != null) {
+                calculateGraphPosition(graphPos, graphMargin, i, sampleSize, nextSample1.derivative, minValue, maxValue, nextSample1.timestamp, minTimestamp, maxTimestamp);
+                var nextX1 = graphPos[0];
+                var nextY1 = graphPos[1];
+
+                calculateGraphPosition(graphPos, graphMargin, i, sampleSize, nextSample2.derivative, minValue, maxValue, nextSample2.timestamp, minTimestamp, maxTimestamp);
+                var nextX2 = graphPos[0];
+                var nextY2 = graphPos[1];
+
+                curveVertex(nextX1 - (nextX2 - nextX1), nextY1 - (nextY2 - nextY1));
+              }
+            }
+
+            endShape();
+
+            pointsDrawn = 0;
+          }
+
+          continue;
+        }
+
+        calculateGraphPosition(graphPos, graphMargin, i, sampleSize, sample.derivative, minValue, maxValue, sample.timestamp, minTimestamp, maxTimestamp);
+        var x = graphPos[0];
+        var y = graphPos[1];
+
+        if (pointsDrawn == 0) {
+          beginShape();
+        }
+
+        noFill();
+
+        if (drawSmoothCurves) {
+          if (pointsDrawn == 0) {
+            var previousSample = samples.get(i + 1);
+
+            if (previousSample != null) {
+              calculateGraphPosition(graphPos, graphMargin, i, sampleSize, previousSample.derivative, minValue, maxValue, previousSample.timestamp, minTimestamp, maxTimestamp);
+              var prevX = graphPos[0];
+              var prevY = graphPos[1];
+
+              curveVertex(x - prevX + x, y - prevY + y);
+            }
+          }
+
+          curveVertex(x, y);
+        } else {
+          vertex(x, y);
+        }
+
+        pointsDrawn++;
+      }
+
+      if (pointsDrawn > 0) {
+        if (drawSmoothCurves && pointsDrawn > 1) {
+          var nextSample1 = samples.get(i - 1);
+          var nextSample2 = samples.get(i - 2);
+
+          if (nextSample1 != null && nextSample2 != null) {
+            calculateGraphPosition(graphPos, graphMargin, i, sampleSize, nextSample1.derivative, minValue, maxValue, nextSample1.timestamp, minTimestamp, maxTimestamp);
+            var nextX1 = graphPos[0];
+            var nextY1 = graphPos[1];
+
+            calculateGraphPosition(graphPos, graphMargin, i, sampleSize, nextSample2.derivative, minValue, maxValue, nextSample2.timestamp, minTimestamp, maxTimestamp);
+            var nextX2 = graphPos[0];
+            var nextY2 = graphPos[1];
+
+            curveVertex(nextX1 - (nextX2 - nextX1), nextY1 - (nextY2 - nextY1));
+          }
+        }
+
+        endShape();
+      }
+
+      noClip();
+
+      fill(255, 63, 63);
+      textAlign(LEFT, TOP);
+      text(nfs(maxValue, 0, 3), 176, 6);
+      textAlign(LEFT, BOTTOM);
+      text(nfs(minValue, 0, 3), 176, graphRect.height - 3);
+
+      popStyle();
+    }
+  }
+
+  // VITESSE
+
+  if (showSpeed) {
+    speedRangeMin = float(speedRangeMinInput.getText());
+    speedRangeMax = float(speedRangeMaxInput.getText());
+
+    var autoScale = autoScaleSpeed || speedRangeMin == speedRangeMax;
+    var autoScaleMin = autoScale || Float.isNaN(speedRangeMin);
+    var autoScaleMax = autoScale || Float.isNaN(speedRangeMax);
+
+    var minValue = autoScaleMin ? Float.POSITIVE_INFINITY : speedRangeMin;
+    var maxValue = autoScaleMax ? Float.NEGATIVE_INFINITY : speedRangeMax;
+
+    var iterator = samples.cappedIterator(sampleSize);
+
+    while (iterator.hasNext()) {
+      var sample = iterator.next();
+
+      var value = sample.speed;
+
+      if (autoScaleMin && value < minValue) {
+        minValue = value;
+      }
+
+      if (autoScaleMax && value > maxValue) {
+        maxValue = value;
+      }
+    }
+
+    if (iterator.count() > 0) {
+      pushStyle();
+
+      clip(0, graphMargin, graphRect.width, graphRect.height - 2 * graphMargin + 1);
+
+      var isDrawingCurve = false;
+
+      // CONSIGNE
+
+      stroke(63, 127, 127);
+      strokeWeight(1);
+      dash.pattern(3, 3);
+
+      for (iterator = samples.cappedIterator(sampleSize); iterator.hasNext(); ) {
+        var i = iterator.count();
+        var sample = iterator.next();
+
+        if (sample.state == 0) {
+          if (isDrawingCurve) {
+            dash.endShape();
+            isDrawingCurve = false;
+          }
+          continue;
+        }
+
+        if (!isDrawingCurve) {
+          dash.beginShape();
+          isDrawingCurve = true;
+        }
+
+        calculateGraphPosition(graphPos, graphMargin, i, sampleSize, sample.consigne, minValue, maxValue, sample.timestamp, minTimestamp, maxTimestamp);
+        var x = graphPos[0];
+        var y = graphPos[1];
+
+        noFill();
+
+        dash.vertex(x, y);
+      }
+
+      if (isDrawingCurve) {
+        dash.endShape();
+        isDrawingCurve = false;
+      }
+
+      // VITESSE
+
+      stroke(127, 255, 255);
+      strokeWeight(1);
+
+      var pointsDrawn = 0;
+
+      var i = 0;
+
+      for (iterator = samples.cappedIterator(sampleSize); iterator.hasNext(); ) {
+        i = iterator.count();
+        var sample = iterator.next();
+
+        calculateGraphPosition(graphPos, graphMargin, i, sampleSize, sample.speed, minValue, maxValue, sample.timestamp, minTimestamp, maxTimestamp);
+        var x = graphPos[0];
+        var y = graphPos[1];
+
+        if (pointsDrawn == 0) {
+          beginShape();
+        }
+
+        noFill();
+
+        if (drawSmoothCurves) {
+          if (pointsDrawn == 0) {
+            var previousSample = samples.get(i + 1);
+
+            if (previousSample != null) {
+              calculateGraphPosition(graphPos, graphMargin, i, sampleSize, previousSample.speed, minValue, maxValue, previousSample.timestamp, minTimestamp, maxTimestamp);
+              var prevX = graphPos[0];
+              var prevY = graphPos[1];
+
+              curveVertex(x - prevX + x, y - prevY + y);
+            }
+          }
+
+          curveVertex(x, y);
+        } else {
+          vertex(x, y);
+        }
+
+        pointsDrawn++;
+      }
+
+      if (pointsDrawn > 0) {
+        if (drawSmoothCurves && pointsDrawn > 1) {
+          var nextSample1 = samples.get(i - 1);
+          var nextSample2 = samples.get(i - 2);
+
+          if (nextSample1 != null && nextSample2 != null) {
+            calculateGraphPosition(graphPos, graphMargin, i, sampleSize, nextSample1.speed, minValue, maxValue, nextSample1.timestamp, minTimestamp, maxTimestamp);
+            var nextX1 = graphPos[0];
+            var nextY1 = graphPos[1];
+
+            calculateGraphPosition(graphPos, graphMargin, i, sampleSize, nextSample2.speed, minValue, maxValue, nextSample2.timestamp, minTimestamp, maxTimestamp);
+            var nextX2 = graphPos[0];
+            var nextY2 = graphPos[1];
+
+            curveVertex(nextX1 - (nextX2 - nextX1), nextY1 - (nextY2 - nextY1));
+          }
+        }
+
+        endShape();
+      }
+
+      noClip();
+
+      fill(127, 255, 255);
+      textAlign(LEFT, TOP);
+      text(nfs(maxValue, 0, 3), 262, 6);
+      textAlign(LEFT, BOTTOM);
+      text(nfs(minValue, 0, 3), 262, graphRect.height - 3);
+
+      popStyle();
+    }
+  }
+
+  // ACCELERATION
+
+  if (showAcceleration) {
+    accelerationRangeMin = float(accelerationRangeMinInput.getText());
+    accelerationRangeMax = float(accelerationRangeMaxInput.getText());
+
+    var autoScale = autoScaleAcceleration || accelerationRangeMin == accelerationRangeMax;
+    var autoScaleMin = autoScale || Float.isNaN(accelerationRangeMin);
+    var autoScaleMax = autoScale || Float.isNaN(accelerationRangeMax);
+
+    var minValue = autoScaleMin ? Float.POSITIVE_INFINITY : accelerationRangeMin;
+    var maxValue = autoScaleMax ? Float.NEGATIVE_INFINITY : accelerationRangeMax;
+
+    var iterator = samples.cappedIterator(sampleSize);
+
+    while (iterator.hasNext()) {
+      var sample = iterator.next();
+
+      var value = sample.acceleration;
+
+      if (autoScaleMin && value < minValue) {
+        minValue = value;
+      }
+
+      if (autoScaleMax && value > maxValue) {
+        maxValue = value;
+      }
+    }
+
+    if (iterator.count() > 0) {
+      pushStyle();
+
+      stroke(127, 255, 127);
+      strokeWeight(1);
+
+      clip(0, graphMargin, graphRect.width, graphRect.height - 2 * graphMargin + 1);
+
+      var pointsDrawn = 0;
+
+      var i = 0;
+
+      for (iterator = samples.cappedIterator(sampleSize); iterator.hasNext(); ) {
+        i = iterator.count();
+        var sample = iterator.next();
+
+        calculateGraphPosition(graphPos, graphMargin, i, sampleSize, sample.acceleration, minValue, maxValue, sample.timestamp, minTimestamp, maxTimestamp);
+        var x = graphPos[0];
+        var y = graphPos[1];
+
+        if (pointsDrawn == 0) {
+          beginShape();
+        }
+
+        noFill();
+
+        if (drawSmoothCurves) {
+          if (pointsDrawn == 0) {
+            var previousSample = samples.get(i + 1);
+
+            if (previousSample != null) {
+              calculateGraphPosition(graphPos, graphMargin, i, sampleSize, previousSample.acceleration, minValue, maxValue, previousSample.timestamp, minTimestamp, maxTimestamp);
+              var prevX = graphPos[0];
+              var prevY = graphPos[1];
+
+              curveVertex(x - prevX + x, y - prevY + y);
+            }
+          }
+
+          curveVertex(x, y);
+        } else {
+          vertex(x, y);
+        }
+
+        pointsDrawn++;
+      }
+
+      if (pointsDrawn > 0) {
+        if (drawSmoothCurves && pointsDrawn > 1) {
+          var nextSample1 = samples.get(i - 1);
+          var nextSample2 = samples.get(i - 2);
+
+          if (nextSample1 != null && nextSample2 != null) {
+            calculateGraphPosition(graphPos, graphMargin, i, sampleSize, nextSample1.acceleration, minValue, maxValue, nextSample1.timestamp, minTimestamp, maxTimestamp);
+            var nextX1 = graphPos[0];
+            var nextY1 = graphPos[1];
+
+            calculateGraphPosition(graphPos, graphMargin, i, sampleSize, nextSample2.acceleration, minValue, maxValue, nextSample2.timestamp, minTimestamp, maxTimestamp);
+            var nextX2 = graphPos[0];
+            var nextY2 = graphPos[1];
+
+            curveVertex(nextX1 - (nextX2 - nextX1), nextY1 - (nextY2 - nextY1));
+          }
+        }
+
+        endShape();
+      }
+
+      noClip();
+
+      fill(127, 255, 127);
+      textAlign(LEFT, TOP);
+      text(nfs(maxValue, 0, 3), 348, 6);
+      textAlign(LEFT, BOTTOM);
+      text(nfs(minValue, 0, 3), 348, graphRect.height - 3);
+
+      popStyle();
+    }
+  }
+
+  // SERVO
+
+  if (showServo) {
+    servoRangeMin = toInt(servoRangeMinInput.getText());
+    servoRangeMax = toInt(servoRangeMaxInput.getText());
+
+    var autoScale = autoScaleServo || servoRangeMin == servoRangeMax;
+    var autoScaleMin = autoScale || servoRangeMin == Integer.MIN_VALUE;
+    var autoScaleMax = autoScale || servoRangeMax == Integer.MIN_VALUE;
+
+    var minValue = autoScaleMin ? Integer.MAX_VALUE : servoRangeMin;
+    var maxValue = autoScaleMax ? Integer.MIN_VALUE : servoRangeMax;
+
+    var iterator = samples.cappedIterator(sampleSize);
+
+    while (iterator.hasNext()) {
+      var sample = iterator.next();
+
+      var value = sample.servo;
+
+      if (autoScaleMin && value < minValue) {
+        minValue = value;
+      }
+
+      if (autoScaleMax && value > maxValue) {
+        maxValue = value;
+      }
+    }
+
+    if (iterator.count() > 0) {
+      pushStyle();
+
+      stroke(255, 127, 255);
+      strokeWeight(1);
+
+      clip(0, graphMargin, graphRect.width, graphRect.height - 2 * graphMargin + 1);
+
+      beginShape();
+
+      for (iterator = samples.cappedIterator(sampleSize); iterator.hasNext(); ) {
+        var i = iterator.count();
+        var sample = iterator.next();
+
+        calculateGraphPosition(graphPos, graphMargin, i, sampleSize, sample.servo, minValue, maxValue, sample.timestamp, minTimestamp, maxTimestamp);
+        var x = graphPos[0];
+        var y = graphPos[1];
+
+        noFill();
+
+        vertex(x, y);
+      }
+
+      endShape();
+
+      noClip();
+
+      fill(255, 127, 255);
+      textAlign(LEFT, TOP);
+      text(maxValue, 434, 6);
+      textAlign(LEFT, BOTTOM);
+      text(minValue, 434, graphRect.height - 3);
+
+      popStyle();
+    }
+  }
+
+  stroke(100);
+  strokeWeight(2);
+  noFill();
+  rect(0, 0, graphRect.width, graphRect.height);
+
+  pop();
+}
+
+void drawValues(Sample sample) {
+  push();
+
+  translate(graphRect.left, graphRect.bottom + 24);
+
+  fill(255);
+  text("Nombre d'échantillons", 0, 0);
+  text("Intervalle de temps", floor(textWidth("Nombre d'échantillons")) + 8F + sampleSizeSlider.getWidth() + 16F + sampleOrTimeGraphToggle.getWidth() + 16F, 0);
+
+  pushMatrix();
+
+  translate(0, 45);
+
+  pushMatrix();
+
+  var col1Text = floor(max(textWidth("Proportionnel"), max(textWidth("Intégral"), textWidth("Dérivatif"), textWidth("Erreur (km/h)"))));
+  var col1 = col1Text + 8;
+
+  translate(col1Text, 0);
+
+  textAlign(RIGHT);
+  fill(255);
+  text("Proportionnel", 0, 0);
+  text("Intégral", 0, 30);
+  text("Dérivatif", 0, 60);
+  text("Erreur (km/h)", 0, 90);
+  textAlign(LEFT);
+
+  translate(8, 0);
+
+  stroke(127);
+  strokeWeight(1);
+  fill(63);
+  rect(0, -17, 200, 23);
+  rect(0, 13, 200, 23);
+  rect(0, 43, 200, 23);
+  rect(0, 73, 200, 23);
+
+  var col2 = 200 + 24;
+  translate(col2, 0);
+
+  var col3Text = floor(max(textWidth("Vitesse"), max(textWidth("Accélération"), textWidth("Commande servo"))));
+  var col3 = col3Text + 8;
+
+  translate(col3Text, 0);
+
+  textAlign(RIGHT);
+  fill(255);
+  text("Vitesse", 0, 0);
+  text("Accélération", 0, 30);
+  text("Commande servo", 0, 60);
+  textAlign(LEFT);
+
+  translate(8, 0);
+
+  stroke(127);
+  strokeWeight(1);
+  fill(63);
+  rect(0, -17, 100, 23);
+  rect(0, 13, 100, 23);
+  rect(0, 43, 100, 23);
+
+  var col4 = 100 + 24;
+  translate(col4, 0);
+
+  var col5Text = floor(max(textWidth("Satellites"), textWidth("Seuil")));
+  var col5 = col5Text + 8;
+
+  translate(col5Text, 0);
+
+  textAlign(RIGHT);
+  fill(255);
+  text("Satellites", 0, 0);
+  text("Seuil", 0, 30);
+  textAlign(LEFT);
+
+  translate(8, 0);
+
+  stroke(127);
+  strokeWeight(1);
+  fill(63);
+  rect(0, -17, 50, 23);
+  rect(0, 13, 50, 23);
+
+  popMatrix();
+
+  if (sample != null) {
+    push();
+
+    fill(255);
+    textAlign(LEFT, CENTER);
+
+    translate(col1, 0);
+
+    clip(0, -18, 200, 23);
+    text(str(sample.proportional), 4F, -18F + 23F / 2F);
+
+    clip(0, 12, 200, 23);
+    text(str(sample.integral), 4F, 12F + 23F / 2F);
+
+    clip(0, 42, 200, 23);
+    text(str(sample.derivative), 4F, 42F + 23F / 2F);
+
+    clip(0, 72, 200, 23);
+    text(str(sample.error), 4F, 72F + 23F / 2F);
+
+    translate(col2 + col3, 0);
+
+    clip(0, -18, 100, 23);
+    text(str(sample.speed), 4F, -18F + 23F / 2F);
+
+    clip(0, 12, 100, 23);
+    text(str(sample.acceleration), 4F, 12F + 23F / 2F);
+
+    clip(0, 42, 100, 23);
+    text(sample.servo, 4F, 42F + 23F / 2F);
+
+    translate(col4 + col5, 0);
+
+    clip(0, -18, 50, 23);
+    text(sample.satellites, 4F, -18F + 23F / 2F);
+
+    clip(0, 12, 50, 23);
+    text(sample.minSatellites, 4F, 12F + 23F / 2F);
+
+    noClip();
+
+    pop();
+  }
+
+  popMatrix();
+
+  var colScalingLabel = floor(max(textWidth("Erreur"), max(textWidth("Intégral"), max(textWidth("Dérivatif"), max(textWidth("Vitesse"), max(textWidth("Accélération"), textWidth("Servo")))))));
+
+  translate(graphRect.width - autoScaleErrorToggle.getWidth() - 8F - (errorRangeMaxInput.getWidth() + 2F) - 2F - (errorRangeMinInput.getWidth() + 2F) - 8F - colScalingLabel, 0);
+
+  fill(255);
+
+  translate(colScalingLabel, 0);
+
+  textAlign(RIGHT);
+  text("Erreur", 0, 24);
+  text("Intégral", 0, 48);
+  text("Dérivatif", 0, 72);
+  text("Vitesse", 0, 96);
+  text("Accélération", 0, 120);
+  text("Servo", 0, 144);
+  textAlign(LEFT);
+
+  translate(8, 0);
+
+  textAlign(CENTER);
+  text("Graphe min/max", ((errorRangeMaxInput.getWidth() + 2F) + 2F + (errorRangeMinInput.getWidth() + 2F)) / 2F, 0);
+
+  pop();
+}
+
+void drawStatusBar(Sample sample) {
+  push();
+
+  stroke(63);
+  strokeWeight(1);
+
+  textAlign(CENTER, CENTER);
+  textFont(consolas_16_bold);
+  textSize(18);
+
+  String stateName;
+  if (sample != null) {
+    switch (sample.state) {
+    case 0:
+      stateName = "Standby";
+      fill(130, 130, 0);
+      break;
+    case 1:
+      stateName = "Approche";
+      fill(0, 0, 170);
+      break;
+    case 2:
+      stateName = "Régulation";
+      fill(0, 100, 0);
+      break;
+    default:
+      stateName = "État inconnu [%s]".formatted(sample.state);
+      noFill();
+    }
+  } else {
+    stateName = "Déconnecté / OFF";
+    fill(47);
+  }
+  rect(stateRect.left, stateRect.top, stateRect.width, stateRect.height);
+  fill(255);
+  text(stateName, stateRect.centerX(), stateRect.centerY());
+
+  if (sample == null) {
+    fill(47);
+  } else if (sample.switchFlag) {
+    fill(170, 0, 0);
+  } else {
+    fill(0, 100, 0);
+  }
+  rect(switchFlagRect.left, switchFlagRect.top, switchFlagRect.width, switchFlagRect.height);
+  fill(255);
+  text("SECU interrupteur", switchFlagRect.centerX(), switchFlagRect.centerY());
+
+  if (sample == null) {
+    fill(47);
+  } else if (sample.speedDiffFlag) {
+    fill(170, 0, 0);
+  } else {
+    fill(0, 100, 0);
+  }
+  rect(speedDiffFlagRect.left, speedDiffFlagRect.top, speedDiffFlagRect.width, speedDiffFlagRect.height);
+  fill(255);
+  text("SECU écart", speedDiffFlagRect.centerX(), speedDiffFlagRect.centerY());
+
+  if (sample == null) {
+    fill(47);
+  } else if (sample.minSpeedFlag) {
+    fill(170, 0, 0);
+  } else {
+    fill(0, 100, 0);
+  }
+  rect(minSpeedFlagRect.left, minSpeedFlagRect.top, minSpeedFlagRect.width, minSpeedFlagRect.height);
+  fill(255);
+  text("SECU vitesse min", minSpeedFlagRect.centerX(), minSpeedFlagRect.centerY());
+
+  if (sample == null) {
+    fill(47);
+  } else if (sample.maxSpeedFlag) {
+    fill(170, 0, 0);
+  } else {
+    fill(0, 100, 0);
+  }
+  rect(maxSpeedFlagRect.left, maxSpeedFlagRect.top, maxSpeedFlagRect.width, maxSpeedFlagRect.height);
+  fill(255);
+  text("SECU vitesse max", maxSpeedFlagRect.centerX(), maxSpeedFlagRect.centerY());
+
+  if (sample == null) {
+    fill(47);
+  } else if (sample.brakeFlag) {
+    fill(170, 0, 0);
+  } else {
+    fill(0, 100, 0);
+  }
+  rect(brakeFlagRect.left, brakeFlagRect.top, brakeFlagRect.width, brakeFlagRect.height);
+  fill(255);
+  text("Frein", brakeFlagRect.centerX(), brakeFlagRect.centerY());
+
+  if (sample == null) {
+    fill(47);
+  } else if (sample.relay) {
+    fill(0, 100, 0);
+  } else {
+    fill(130, 130, 0);
+  }
+  rect(relayRect.left, relayRect.top, relayRect.width, relayRect.height);
+  fill(255);
+  text("Relais", relayRect.centerX(), relayRect.centerY());
+
+  if (sample == null) {
+    fill(47);
+  } else if (sample.satellites < sample.minSatellites) {
+    fill(170, 0, 0);
+  } else {
+    fill(0, 100, 0);
+  }
+  rect(satellitesRect.left, satellitesRect.top, satellitesRect.width, satellitesRect.height);
+  fill(255);
+  text("Satellites", satellitesRect.centerX(), satellitesRect.centerY());
+
+  pop();
+}
+
+void mousePressed() {
+  if (mouseButton == LEFT) {
+    if (errorGraphLabelRect.contains(mouseX, mouseY)) {
+      showError = !showError;
+    } else if (integralGraphLabelRect.contains(mouseX, mouseY)) {
+      showIntegral = !showIntegral;
+    } else if (derivativeGraphLabelRect.contains(mouseX, mouseY)) {
+      showDerivative = !showDerivative;
+    } else if (speedGraphLabelRect.contains(mouseX, mouseY)) {
+      showSpeed = !showSpeed;
+    } else if (accelerationGraphLabelRect.contains(mouseX, mouseY)) {
+      showAcceleration = !showAcceleration;
+    } else if (servoGraphLabelRect.contains(mouseX, mouseY)) {
+      showServo = !showServo;
+    }
+  }
+}
+
+void controlEvent(ControlEvent c) {
+  switch (c.getId()) {
+  case 1:
+  case 2:
+  case 3:
+    sendKPID();
+    return;
+  case 4:
+    sendCommand(c.getStringValue());
+    return;
+  }
+
+  if (c.isAssignableFrom(Toggle.class)) {
+    var toggle = (Toggle) c.getController();
+
+    if (toggle.getName().startsWith("autoScale")) {
+      updateAutoScaleToggleLabel(toggle);
+    }
+  }
+}
+
+void serialEvent(Serial p) {
+  if (!hasIgnoredFirstPacket) {
+    p.clear();
+    hasIgnoredFirstPacket = true;
+    return;
+  }
+
+  handlePacket(p.readString());
+}
+
+static boolean isFloat(String value) {
+  return FLOAT_REGEX.matcher(value).matches();
+}
+
+static int toInt(String value) {
+  try {
+    return Integer.parseInt(value);
+  }
+  catch (NumberFormatException e) {
+    return Integer.MIN_VALUE;
+  }
+}
+
+static boolean intToBool(String value) {
+  return value.equals("1");
+}
+
+static int toInt(boolean value) {
+  return value ? 1 : 0;
+}
+
+float[] calculateGraphPosition(float graphMargin, int i, int sampleSize, float value, float minValue, float maxValue, float timestamp, float minTimestamp, float maxTimestamp) {
+  var pos = new float[2];
+  calculateGraphPosition(pos, graphMargin, i, sampleSize, value, minValue, maxValue, timestamp, minTimestamp, maxTimestamp);
+  return pos;
+}
+
+void calculateGraphPosition(float[] pos, float graphMargin, int i, int sampleSize, float value, float minValue, float maxValue, float timestamp, float minTimestamp, float maxTimestamp) {
+  pos[0] = sampleOrTimeGraph ? map(i, 0, sampleSize - 1, graphRect.width, 0) : map(timestamp, minTimestamp, maxTimestamp, 0, graphRect.width);
+  pos[1] = minValue == maxValue ? graphRect.height / 2 : map(value, minValue, maxValue, graphRect.height - graphMargin, graphMargin);
+}
+
+void updateAutoScaleToggleLabel(Toggle toggle) {
+  toggle.setCaptionLabel(toggle.getBooleanValue() ? "AUTO" : "MANU");
+}
+
+void sendKPID() {
+  var sample = samples.get();
+
+  if (sample == null) {
+    return;
+  }
+
+  var sb = new StringBuilder();
+
+  sb.append("kp").append('=');
+  if (isFloat(kpInput.getText())) {
+    sb.append(kpInput.getText());
+  } else {
+    sb.append(sample.kp);
+  }
+
+  sb.append(',');
+
+  sb.append("ki").append('=');
+  if (isFloat(kiInput.getText())) {
+    sb.append(kiInput.getText());
+  } else {
+    sb.append(sample.ki);
+  }
+
+  sb.append(',');
+
+  sb.append("kd").append('=');
+  if (isFloat(kdInput.getText())) {
+    sb.append(kdInput.getText());
+  } else {
+    sb.append(sample.kd);
+  }
+
+  sb.append('\n');
+
+  kpInput.setText("");
+  kiInput.setText("");
+  kdInput.setText("");
+
+  serial.write(sb.toString());
+}
+
+void sendCommand(String command) {
+  serial.write(command);
+  serial.write('\n');
+}
+
+void handlePacket(String packet) {
+  var timestamp = millis();
+
+  if (!hasReadSecondPacket) {
+    timeZero = timestamp;
+    hasReadSecondPacket = true;
+  }
+
+  timestamp -= timeZero;
+
+  var previousSample = samples.get();
+  var sample = new Sample(timestamp, previousSample == null || previousSample.timestamp / 1000 != timestamp / 1000);
+
+  var variables = packet.split(";");
+
+  for (int i = 0, max = variables.length - 1; i < max; i++) {
+    var variable = variables[i];
+    var split = variable.split(":", 2);
+
+    var name = split[0];
+    var value = split[1];
+
+    switch (name) {
+    case "SECUINTER":
+      sample.switchFlag = intToBool(value);
+      break;
+    case "SECUECART":
+      sample.speedDiffFlag = intToBool(value);
+      break;
+    case "SECUVMIN":
+      sample.minSpeedFlag = intToBool(value);
+      break;
+    case "SECUVMAX":
+      sample.maxSpeedFlag = intToBool(value);
+      break;
+    case "KP":
+      sample.kp = float(value);
+      break;
+    case "KI":
+      sample.ki = float(value);
+      break;
+    case "KD":
+      sample.kd = float(value);
+      break;
+    case "VITESSEMIN":
+      sample.speedMin = int(value);
+      break;
+    case "VITESSEMAX":
+      sample.speedMax = int(value);
+      break;
+    case "ETAT":
+      sample.state = int(value);
+      break;
+    case "CONSIGNE":
+      sample.consigne = float(value);
+      break;
+    case "SAT":
+      sample.satellites = int(value);
+      break;
+    case "SATMIN":
+      sample.minSatellites = int(value);
+      break;
+    case "VITESSE":
+      sample.speed = float(value);
+      break;
+    case "ACCEL":
+      sample.acceleration = float(value);
+      break;
+    case "ERREUR":
+      sample.error = float(value);
+      break;
+    case "PROPORTIONNEL":
+      sample.proportional = float(value);
+      break;
+    case "INTEGRAL":
+      sample.integral = float(value);
+      break;
+    case "DERIVATIF":
+      sample.derivative = float(value);
+      break;
+    case "SERVO":
+      sample.servo = int(value);
+      break;
+    case "FREIN":
+      sample.brakeFlag = intToBool(value);
+      break;
+      //case "MILLIS":
+      //  timestamp = int(value);
+      //  break;
+    case "RELAIS":
+      sample.relay = intToBool(value);
+      break;
+    }
+  }
+
+  sampleQueue.add(sample);
+}
+
+void resetSerial() {
+  if (serial != null) {
+    serial.stop();
+    serial = null;
+  }
+
+  hasReadSecondPacket = false;
+  timeZero = 0;
+
+  samples.clear();
+  sampleQueue.clear();
+
+  try {
+    serial = new Serial(this, COM_PORT, 115200);
+    serial.bufferUntil('\n');
+  }
+  catch (Exception e) {
+    println(e.getMessage());
+  }
+}
+
+void saveSamples() {
+  var table = new Table();
+
+  table.addColumn("timestamp");
+  table.addColumn("drawTimeTick");
+  table.addColumn("kp");
+  table.addColumn("ki");
+  table.addColumn("kd");
+  table.addColumn("proportional");
+  table.addColumn("integral");
+  table.addColumn("derivative");
+  table.addColumn("speed");
+  table.addColumn("acceleration");
+  table.addColumn("consigne");
+  table.addColumn("error");
+  table.addColumn("servo");
+  table.addColumn("state");
+  table.addColumn("satellites");
+  table.addColumn("minSatellites");
+  table.addColumn("speedMin");
+  table.addColumn("speedMax");
+  table.addColumn("switchFlag");
+  table.addColumn("speedDiffFlag");
+  table.addColumn("minSpeedFlag");
+  table.addColumn("maxSpeedFlag");
+  table.addColumn("brakeFlag");
+  table.addColumn("relay");
+
+  var iterator = samples.descendingIterator();
+
+  while (iterator.hasNext()) {
+    var sample = iterator.next();
+
+    var row = table.addRow();
+
+    row.setInt("timestamp", sample.timestamp);
+    row.setInt("drawTimeTick", toInt(sample.drawTimeTick));
+    row.setFloat("kp", sample.kp);
+    row.setFloat("ki", sample.ki);
+    row.setFloat("kd", sample.kd);
+    row.setFloat("proportional", sample.proportional);
+    row.setFloat("integral", sample.integral);
+    row.setFloat("derivative", sample.derivative);
+    row.setFloat("speed", sample.speed);
+    row.setFloat("acceleration", sample.acceleration);
+    row.setFloat("consigne", sample.consigne);
+    row.setFloat("error", sample.error);
+    row.setInt("servo", sample.servo);
+    row.setInt("state", sample.state);
+    row.setInt("satellites", sample.satellites);
+    row.setInt("minSatellites", sample.minSatellites);
+    row.setInt("speedMin", sample.speedMin);
+    row.setInt("speedMax", sample.speedMax);
+    row.setInt("switchFlag", toInt(sample.switchFlag));
+    row.setInt("speedDiffFlag", toInt(sample.speedDiffFlag));
+    row.setInt("minSpeedFlag", toInt(sample.minSpeedFlag));
+    row.setInt("maxSpeedFlag", toInt(sample.maxSpeedFlag));
+    row.setInt("brakeFlag", toInt(sample.brakeFlag));
+    row.setInt("relay", toInt(sample.relay));
+  }
+
+  saveTable(table, "data/samples.csv");
+}
diff --git a/Rectangle.pde b/Rectangle.pde
new file mode 100644
index 0000000..9678859
--- /dev/null
+++ b/Rectangle.pde
@@ -0,0 +1,76 @@
+static class Rectangle {
+  final float left, top, right, bottom, width, height;
+
+  Rectangle(float top, float left, float bottom, float right) {
+    this.top = top;
+    this.left = left;
+    this.bottom = bottom;
+    this.right = right;
+    this.width = right - left;
+    this.height = bottom - top;
+  }
+
+  static Rectangle tlbr(float top, float left, float bottom, float right) {
+    return new Rectangle(top, left, bottom, right);
+  }
+
+  static Rectangle xywh(float left, float top, float width, float height) {
+    return new Rectangle(top, left, top + height, left + width);
+  }
+
+  float centerX() {
+    return left + width / 2F;
+  }
+
+  float centerY() {
+    return top + height / 2F;
+  }
+
+  PVector center() {
+    return new PVector(centerX(), centerY());
+  }
+
+  boolean contains(float x, float y) {
+    return x >= left && x < right && y >= top && y < bottom;
+  }
+
+  Rectangle offset(float dx, float dy) {
+    return xywh(left + dx, top + dy, width, height);
+  }
+
+  Rectangle[] splitVerticallyAt(float dx) {
+    return new Rectangle[] {
+      new Rectangle(top, left, bottom, left + dx),
+      new Rectangle(top, left + dx, bottom, right)
+    };
+  }
+
+  Rectangle[] splitHorizontallyAt(float dy) {
+    return new Rectangle[] {
+      new Rectangle(top, left, top + dy, right),
+      new Rectangle(top + dy, left, bottom, right)
+    };
+  }
+
+  Rectangle[] splitVerticallyInto(int n) {
+    var count = floor(width / n);
+    var result = new Rectangle[count];
+
+    for (var i = 0; i < count; i++) {
+      result[i] = new Rectangle(top, lerp(left, right, (float) i / n), bottom, lerp(left, right, (float) (i + 1) / n));
+    }
+
+    return result;
+  }
+
+  Rectangle[] splitHorizontallyInto(int n) {
+    var count = floor(height / n);
+    var result = new Rectangle[count];
+
+    for (var i = 0; i < count; i++) {
+      result[i] = new Rectangle(lerp(top, bottom, (float) i / n), left, lerp(top, bottom, (float) (i + 1) / n), right);
+    }
+
+    return result;
+  }
+}
diff --git a/Sample.pde b/Sample.pde
new file mode 100644
index 0000000..8ba4130
--- /dev/null
+++ b/Sample.pde
@@ -0,0 +1,22 @@
+import java.util.Date;
+
+static class Sample {
+  final int timestamp;
+  final boolean drawTimeTick;
+  float proportional, integral, derivative;
+  float speed, acceleration;
+  float consigne;
+  float error;
+  int servo;
+  int state;
+  float kp, ki, kd;
+  int speedMin, speedMax;
+  int minSatellites, satellites;
+  boolean switchFlag, speedDiffFlag, minSpeedFlag, maxSpeedFlag, brakeFlag;
+  boolean relay;
+  
+  Sample(int timestamp, boolean drawTimeTick) {
+    this.timestamp = timestamp;
+    this.drawTimeTick = drawTimeTick;
+  }
+}