import processing.serial.*;
JSONObject json;
Serial port; // The serial port
char[] teapotPacket = new char[14]; // InvenSense Teapot packet
int serialCount = 0; // current packet byte position
int synced = 0;
int interval = 0;
float[] K = {0, 0, 0};
int motor;
PFont Font1, Font2;
PGraphics Curve;
PGraphics Info;
int guard_h = 80;
int guard_v = 30;
int tick_s = 10;
int font_size = 32;
int info_h = 400;
int info_v = 200;
String[] Mot = { "YAW", "PITCH", "ROLL" };
class PPoint {
float angle, time, cmd;
PPoint (float a, float t, float c) {
angle = a;
time = t;
cmd = c;
}
}
PPoint[][] datas = new PPoint[2][255];
int nb_trace = 0;
int nb_points = 0;
int prev_seq = 0;
void setup() {
size(1920, 1080);
surface.setTitle("PID Drawer");
// Uncomment the following two lines to see the available fonts
String[] fontList = PFont.list();
printArray(fontList);
Font1 = createFont("FreeSans Bold", font_size);
Font2 = createFont("FreeSans", font_size);
Curve = createGraphics(width, height);
Info = createGraphics(400, 200);
// display serial port list for debugging/clarity
println(Serial.list());
// get the first available port (use EITHER this OR the specific port code below)
String portName = Serial.list()[0];
// get a specific serial port (use EITHER this OR the first-available code above)
//String portName = "COM4";
// open the serial port
port = new Serial(this, portName, 115200);
println(port);
println(datas[0][0]);
}
void draw() {
// black background
background(0);
Curve.beginDraw();
Curve.textFont(Font2);
// Horizontal line
Curve.stroke(255);
// Horizontal line
Curve.line(guard_h, height / 2, width - guard_h, height / 2);
for (int i = 0; i < 20; i = i+1) {
Curve.line(guard_h + i * 90, height / 2 - tick_s / 2, guard_h + i * 90, height / 2 + tick_s / 2);
}
fill(255);
Curve.triangle(width - guard_h, -10 + height / 2,
width - guard_h + 10, height / 2,
width - guard_h, +10 + height / 2);
// Vertial line
Curve.line(guard_h, guard_v, guard_h, height - guard_v);
for (int i = 0; i <= 12; i = i+1) {
Curve.line(guard_h - tick_s / 2, i * 80 + guard_v, guard_h + tick_s / 2, i * 80 + guard_v);
Curve.text(nfp(60 - i * 10, 0), 0, i * 80 + guard_v + font_size / 4);
}
Curve.triangle(guard_h - 10, guard_v, guard_h + 10, guard_v, guard_h, guard_v - 10);
Curve.triangle(guard_h - 10, height - guard_v, guard_h + 10, height - guard_v, guard_h, height - guard_v + 10);
Curve.endDraw();
image(Curve, 0, 0);
Info.beginDraw();
Info.stroke(255, 0, 0);
Info.fill(255, 255, 255);
Info.rect(0, 0, info_h, info_v);
Info.fill(1);
Info.textFont(Font1);
Info.text("Motor:", 20, 40);
Info.text("Kp:", 20, 80);
Info.text("Ki:", 20, 120);
Info.text("Kd:", 20, 160);
Info.textFont(Font2);
Info.text(Mot[0], 130, 40);
Info.text(nf(0.0, 0, 4), 130, 80);
Info.text(nf(0.0, 0, 4), 130, 120);
Info.text(nf(0.0, 0, 4), 130, 160);
Info.endDraw();
image(Info, width - info_h - guard_h, guard_v);
}
void serialEvent(Serial port) {
interval = millis();
while (port.available() > 0) {
int ch = port.read();
print((char)ch);
if (synced == 0 && ch != '$') return; // initial synchronization - also used to resync/realign if needed
synced = 1;
if ((serialCount == 1 && ch != 2)
|| (serialCount == 12 && ch != '\r')
|| (serialCount == 13 && ch != '\n')) {
serialCount = 0;
synced = 0;
println("Dump Frame");
return;
}
if (serialCount > 0 || ch == '$') {
teapotPacket[serialCount++] = (char)ch;
if (serialCount == 14) {
serialCount = 0; // restart packet byte position
println("Complete Packet");
if (teapotPacket[10] == 0x1) {
println("Reset is requested");
Curve.beginDraw();
Curve.clear();
Curve.endDraw();
nb_trace = 0;
nb_points = 0;
prev_seq = 0;
}
if ( prev_seq != teapotPacket[9]) {
println("New Seq");
nb_trace++;
}
if (nb_trace >= 2) {
println("Reset Seq");
nb_trace = 0;
}
PPoint p = new PPoint(teapotPacket[6], teapotPacket[7], teapotPacket[8]);
println(nb_trace); println(nb_points);
datas[nb_trace][nb_points] = p;
motor = teapotPacket[2];
K[0] = teapotPacket[3];
K[1] = teapotPacket[4];
K[2] = teapotPacket[5];
nb_points++;
if (nb_points >= 254) {
// Overflow
nb_points = 254;
}
}
}
}
}
void keyPressed() {
json = new JSONObject();
print("Pressed");
if (key == 'a') {
K[0] += 0.1;
}
else if (key == 'q') {
K[1] += 0.1;
}
else if (key == 'w') {
K[2] += 0.1;
}
else if (key == 'z') {
K[0] += 0.01;
}
else if (key == 's') {
K[1] += 0.01;
}
else if (key == 'x') {
K[2] += 0.01;
}
else if (key == 'e') {
K[0] += 0.001;
}
else if (key == 'd') {
K[1] += 0.001;
}
else if (key == 'c') {
K[2] += 0.001;
}
else if (key == 'r') {
K[0] += 0.0001;
}
else if (key == 'f') {
K[1] += 0.0001;
}
else if (key == 'v') {
K[2] += 0.0001;
}
else if (key == 'A') {
K[0] -= 0.1;
}
else if (key == 'Q') {
K[1] -= 0.1;
}
else if (key == 'W') {
K[2] -= 0.1;
}
else if (key == 'Z') {
K[0] -= 0.01;
}
else if (key == 'S') {
K[1] -= 0.01;
}
else if (key == 'X') {
K[2] -= 0.01;
}
else if (key == 'E') {
K[0] -= 0.001;
}
else if (key == 'D') {
K[1] -= 0.001;
}
else if (key == 'C') {
K[2] -= 0.001;
}
else if (key == 'R') {
K[0] -= 0.0001;
}
else if (key == 'F') {
K[1] -= 0.0001;
}
else if (key == 'V') {
K[2] -= 0.0001;
}
else if (key == '1') {
motor = 0;
}
else if (key == '2') {
motor = 1;
}
else if (key == '3') {
motor = 2;
}
json.setInt("motor", motor);
json.setFloat("Kp", K[0]);
json.setFloat("Ki", K[1]);
json.setFloat("Kd", K[2]);
print(json.toString());
port.write(json.toString());
}```
1 Like