From 7cb389d4a34a54870d0359cdb7a185884b1690b9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Herv=C3=A9=20BECHER?= Date: Thu, 2 Jan 2025 16:29:45 +0100 Subject: [PATCH] New version --- CruiseControlMX5.pde | 358 +++++++++++++++++-------------------------- Sample.pde | 9 +- 2 files changed, 144 insertions(+), 223 deletions(-) diff --git a/CruiseControlMX5.pde b/CruiseControlMX5.pde index 9ca929b..5aaf739 100644 --- a/CruiseControlMX5.pde +++ b/CruiseControlMX5.pde @@ -10,7 +10,7 @@ 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 String COM_PORT = "COM5"; static final int speedStart = 0, speedEnd = 240; static final int minSampleSize = 50, maxSampleSize = 600; static final float minTimeSpan = 5F, maxTimeSpan = 60F; @@ -31,7 +31,7 @@ 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; +Rectangle stateRect, brakeFlagRect, deltaTRect; Serial serial; boolean hasIgnoredFirstPacket, hasReadSecondPacket; @@ -52,23 +52,18 @@ boolean drawSmoothCurves; void setup() { size(1600, 512); - pixelDensity(1); + pixelDensity(displayDensity()); 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); + var splitRect = statusRect.splitVerticallyInto(3); stateRect = splitRect[0]; - switchFlagRect = splitRect[1]; - speedDiffFlagRect = splitRect[2]; - minSpeedFlagRect = splitRect[3]; - maxSpeedFlagRect = splitRect[4]; - brakeFlagRect = splitRect[5]; - relayRect = splitRect[6]; - satellitesRect = splitRect[7]; + brakeFlagRect = splitRect[1]; + deltaTRect = splitRect[2]; consolas_16 = createFont("Consolas", 16, true); consolas_16_bold = createFont("Consolas Bold", 16, true); @@ -139,12 +134,13 @@ void setup() { .setFont(font) .setCaptionLabel(""); - //var saveSamplesButton = cp5.addButton("saveSamples") - // .setPosition(width - 32 - 80, 8) - // .setSize(80, 20) - // .setFont(font) - // .setCaptionLabel("Sauver"); - //pos = saveSamplesButton.getPosition(); + /*var saveSamplesButton = cp5.addButton("saveSamples"). + .setPosition(width - 32 - 80, 8) + .setSize(80, 20) + .setFont(font) + .setCaptionLabel("Sauver") + .setEnabled(false); + pos = saveSamplesButton.getPosition();*/ smoothCurvesToggle = cp5.addToggle("drawSmoothCurves") .setPosition(width - 32 - 150, 8) @@ -323,13 +319,7 @@ void setup() { 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()); - } + openSerial(); } void draw() { @@ -625,7 +615,7 @@ void drawGraph(int frameTimeStart) { for (var iterator = samples.cappedIterator(sampleSize); iterator.hasNext(); ) { var sample = iterator.next(); - if (sample.state != 2) { + if (!sample.state) { continue; } @@ -665,7 +655,7 @@ void drawGraph(int frameTimeStart) { i = iterator.count(); var sample = iterator.next(); - if (sample.state != 2) { + if (!sample.state) { if (pointsDrawn > 0) { if (drawSmoothCurves && pointsDrawn > 1) { var nextSample1 = samples.get(i - 1); @@ -772,7 +762,7 @@ void drawGraph(int frameTimeStart) { for (var iterator = samples.cappedIterator(sampleSize); iterator.hasNext(); ) { var sample = iterator.next(); - if (sample.state != 2) { + if (!sample.state) { continue; } @@ -805,7 +795,7 @@ void drawGraph(int frameTimeStart) { i = iterator.count(); var sample = iterator.next(); - if (sample.state != 2) { + if (!sample.state) { if (pointsDrawn > 0) { if (drawSmoothCurves && pointsDrawn > 1) { var nextSample1 = samples.get(i - 1); @@ -913,7 +903,7 @@ void drawGraph(int frameTimeStart) { for (var iterator = samples.cappedIterator(sampleSize); iterator.hasNext(); ) { var sample = iterator.next(); - if (sample.state != 2) { + if (!sample.state) { continue; } @@ -946,7 +936,7 @@ void drawGraph(int frameTimeStart) { i = iterator.count(); var sample = iterator.next(); - if (sample.state != 2) { + if (!sample.state) { if (pointsDrawn > 0) { if (drawSmoothCurves && pointsDrawn > 1) { var nextSample1 = samples.get(i - 1); @@ -1083,7 +1073,7 @@ void drawGraph(int frameTimeStart) { var i = iterator.count(); var sample = iterator.next(); - if (sample.state == 0) { + if (!sample.state) { if (isDrawingCurve) { dash.endShape(); isDrawingCurve = false; @@ -1411,7 +1401,7 @@ void drawValues(Sample sample) { var col2 = 200 + 24; translate(col2, 0); - var col3Text = floor(max(textWidth("Vitesse"), max(textWidth("Accélération"), textWidth("Commande servo")))); + var col3Text = floor(max(textWidth("Vitesse"), textWidth("Accélération"))); var col3 = col3Text + 8; translate(col3Text, 0); @@ -1420,7 +1410,6 @@ void drawValues(Sample sample) { fill(255); text("Vitesse", 0, 0); text("Accélération", 0, 30); - text("Commande servo", 0, 60); textAlign(LEFT); translate(8, 0); @@ -1430,20 +1419,21 @@ void drawValues(Sample sample) { 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 col5Text = floor(max(textWidth("Commande servo"), max(textWidth("Servo min"), max(textWidth("Servo max"), textWidth("Servo neutre"))))); var col5 = col5Text + 8; translate(col5Text, 0); textAlign(RIGHT); fill(255); - text("Satellites", 0, 0); - text("Seuil", 0, 30); + text("Commande servo", 0, 0); + text("Servo min", 0, 30); + text("Servo max", 0, 60); + text("Servo neutre", 0, 90); textAlign(LEFT); translate(8, 0); @@ -1453,6 +1443,8 @@ void drawValues(Sample sample) { fill(63); rect(0, -17, 50, 23); rect(0, 13, 50, 23); + rect(0, 43, 50, 23); + rect(0, 73, 50, 23); popMatrix(); @@ -1464,36 +1456,39 @@ void drawValues(Sample sample) { translate(col1, 0); - clip(0, -18, 200, 23); - text(str(sample.proportional), 4F, -18F + 23F / 2F); + clip(0, -17, 200, 23); + text(str(sample.proportional), 4F, -17F + 23F / 2F); - clip(0, 12, 200, 23); - text(str(sample.integral), 4F, 12F + 23F / 2F); + clip(0, 13, 200, 23); + text(str(sample.integral), 4F, 13F + 23F / 2F); - clip(0, 42, 200, 23); - text(str(sample.derivative), 4F, 42F + 23F / 2F); + clip(0, 43, 200, 23); + text(str(sample.derivative), 4F, 43F + 23F / 2F); - clip(0, 72, 200, 23); - text(str(sample.error), 4F, 72F + 23F / 2F); + clip(0, 73, 200, 23); + text(str(sample.error), 4F, 73F + 23F / 2F); translate(col2 + col3, 0); - clip(0, -18, 100, 23); - text(str(sample.speed), 4F, -18F + 23F / 2F); + clip(0, -17, 100, 23); + text(str(sample.speed), 4F, -17F + 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); + clip(0, 13, 100, 23); + text(str(sample.acceleration), 4F, 13F + 23F / 2F); translate(col4 + col5, 0); - clip(0, -18, 50, 23); - text(sample.satellites, 4F, -18F + 23F / 2F); + clip(0, -17, 100, 23); + text(str(sample.servo), 4F, -17F + 23F / 2F); - clip(0, 12, 50, 23); - text(sample.minSatellites, 4F, 12F + 23F / 2F); + clip(0, 13, 100, 23); + text(str(sample.servoMin), 4F, 13F + 23F / 2F); + + clip(0, 43, 100, 23); + text(str(sample.servoMax), 4F, 43F + 23F / 2F); + + clip(0, 73, 100, 23); + text(str(sample.servoNeutre), 4F, 73F + 23F / 2F); noClip(); @@ -1539,22 +1534,12 @@ void drawStatusBar(Sample sample) { 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: + if (sample.state) { stateName = "Régulation"; fill(0, 100, 0); - break; - default: - stateName = "État inconnu [%s]".formatted(sample.state); - noFill(); + } else { + stateName = "Standby"; + fill(130, 130, 0); } } else { stateName = "Déconnecté / OFF"; @@ -1564,50 +1549,6 @@ void drawStatusBar(Sample sample) { 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) { @@ -1621,25 +1562,16 @@ void drawStatusBar(Sample sample) { 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); + rect(deltaTRect.left, deltaTRect.top, deltaTRect.width, deltaTRect.height); fill(255); - text("Satellites", satellitesRect.centerX(), satellitesRect.centerY()); + if (sample != null) { + text("%d µs".formatted(sample.deltaT), deltaTRect.centerX(), deltaTRect.centerY()); + } else { + text("∆T (µs)", deltaTRect.centerX(), deltaTRect.centerY()); + } pop(); } @@ -1800,18 +1732,6 @@ void handlePacket(String packet) { 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; @@ -1827,18 +1747,12 @@ void handlePacket(String packet) { case "VITESSEMAX": sample.speedMax = int(value); break; - case "ETAT": - sample.state = int(value); + case "ACTIF": + sample.state = intToBool(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; @@ -1860,14 +1774,20 @@ void handlePacket(String packet) { case "SERVO": sample.servo = int(value); break; + case "SERVOMIN": + sample.servoMin = int(value); + break; + case "SERVOMAX": + sample.servoMax = int(value); + break; + case "SERVONEUTRE": + sample.servoNeutre = int(value); + break; case "FREIN": sample.brakeFlag = intToBool(value); break; - //case "MILLIS": - // timestamp = int(value); - // break; - case "RELAIS": - sample.relay = intToBool(value); + case "DELTAT": + sample.deltaT = int(float(value) * 1e6); break; } } @@ -1887,8 +1807,12 @@ void resetSerial() { samples.clear(); sampleQueue.clear(); + openSerial(); +} + +void openSerial() { try { - serial = new Serial(this, COM_PORT, 115200); + serial = new Serial(this, COM_PORT, 921600); serial.bufferUntil('\n'); } catch (Exception e) { @@ -1896,66 +1820,64 @@ void resetSerial() { } } +/* 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"); -} + 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("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("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/%s.csv".formatted(new Date())); + } + */ diff --git a/Sample.pde b/Sample.pde index 8ba4130..1380912 100644 --- a/Sample.pde +++ b/Sample.pde @@ -7,13 +7,12 @@ static class Sample { float speed, acceleration; float consigne; float error; - int servo; - int state; + int servo, servoMin, servoMax, servoNeutre; + boolean state; float kp, ki, kd; + int deltaT; int speedMin, speedMax; - int minSatellites, satellites; - boolean switchFlag, speedDiffFlag, minSpeedFlag, maxSpeedFlag, brakeFlag; - boolean relay; + boolean brakeFlag; Sample(int timestamp, boolean drawTimeTick) { this.timestamp = timestamp;