hbecher/cruise-control-mx5-processing
1
0
Fork 0
Code Activity

New version

Browse Source
This commit is contained in:
Hervé BECHER
2025-01-02 16:29:45 +01:00
parent 2c8f1f988c
commit 7cb389d4a3
2 changed files with 144 additions and 223 deletions
Download Patch File Download Diff File Expand all files Collapse all files

358
CruiseControlMX5.pde
View File

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

9
Sample.pde
View File

@@ -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;