1962 lines
56 KiB
Plaintext
1962 lines
56 KiB
Plaintext
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");
|
|
}
|