public void processSerialData() {
byte[] inBuf = new byte[20];
switch (serial.read()) {
case 'A':
// wait for all data arrived
while (serial.available() < 16) {}
// read all data
serial.readBytes(inBuf);
Acc_RAW = (inBuf[1] << 8) + (inBuf[0] & 0xFF);
Gyro_RAW = (inBuf[3] << 8) + (inBuf[2] & 0xFF);
// int intbit = 0;
// intbit = (inBuf[7] << 24) | ((inBuf[6] & 0xFF) << 16) | ((inBuf[5] & 0xFF) << 8) |
// (inBuf[4] & 0xFF);
// Angle = Float.intBitsToFloat(intbit);
int AngleInt = (inBuf[5] << 8) + (inBuf[4] & 0xFF);
Angle = PApplet.parseFloat(AngleInt) / 10;
Acc_Angle = (inBuf[7] << 8) + (inBuf[6] & 0xFF);
Gyro_Rate = (inBuf[9] << 8) + (inBuf[8] & 0xff);
Drive = (inBuf[11] << 8) + (inBuf[10] & 0xFF);
statusFlag = (inBuf[13] << 8) + (inBuf[12] & 0xFF);
int BatLevelInt = (inBuf[15] << 8) + (inBuf[14] & 0xFF);
Steer = (inBuf[17] << 8) + (inBuf[16] & 0xFF);
BatLevel = PApplet.parseFloat(BatLevelInt) / 10;
println(
"Acc="
+ Acc_RAW
+ " Gyro="
+ Gyro_RAW
+ " Angle="
+ Angle
+ " Acc_Angle="
+ Acc_Angle
+ " Gyro_Rate="
+ Gyro_Rate
+ " Drive="
+ Drive
+ " Status="
+ statusFlag);
break;
case 'E':
// wait for all data arrived
while (serial.available() < 6) {}
// read all data
serial.readBytes(inBuf);
int P = (inBuf[1] << 8) + (inBuf[0] & 0xFF);
int I = (inBuf[3] << 8) + (inBuf[2] & 0xFF);
int D = (inBuf[5] << 8) + (inBuf[4] & 0xFF);
conf_KP.setValue(P);
conf_KI.setValue(I);
conf_KD.setValue(D);
println("P=" + P + " I=" + I + " D=" + D);
break;
}
serial.clear();
}
public void draw() {
lights();
background(255);
noStroke();
fill(200);
translate(-width / 2, 0);
t += 0.001f;
int offset = 10;
float shapeSize = 5.0f;
for (int i = 1; i < 10; i++) {
// ellipse(noise(t+i*offset*0.01)*width, sin(float(frameCount + i*offset)/100)*height/2,
// shapeSize*i, shapeSize*i);
float x = noise(t + i * offset * 0.01f) * width;
float y = sin(PApplet.parseFloat(frameCount + i * offset) / 100) * height / 2;
pushMatrix();
translate(x, y);
sphere(shapeSize * i);
popMatrix();
}
}
public void draw() {
background(0);
// Strings composition
textFont(createFont("Arial bold", 24));
fill(c_red);
stroke(255);
text("OpenWheels GUI V1.0", 20, 30);
textSize(16);
// textAlign(CENTER);
fill(c_azure);
text("Acc_RAW: " + Acc_RAW, 150, 65);
AccSlider.setValue(Acc_RAW);
text("Gyro_RAW: " + Gyro_RAW, 150, 90);
GyroSlider.setValue(Gyro_RAW);
text("Acc_Angle: " + Acc_Angle + "\u00b0", 150, 115);
text("Gyro_Rate: " + Gyro_Rate + "\u00b0/sec", 150, 140);
text("Drive: " + Drive, 150, 165);
text("Steer: " + Steer, 150, 190);
text("BatLevel: " + nf(BatLevel, 1, 1) + "V", 150, 215);
text("Status: " + statusFlag, 150, 240);
graphGauge();
graphGrid();
// call the function that plot the angular of acc, with few screen settings
graphRoll(PApplet.parseFloat(Acc_Angle), VideoBuffer2, c_red); // xPos, YPos, YSpan
// call the function that plot the estimate angular, with few screen settings
graphRoll(Angle, VideoBuffer1, c_yellow); // xPos, YPos, YSpan
// call arduino for data every timePolling [msec]
int timePolling = 50; // 50msec=20Hz
time1 = millis();
if (init_com == 1) {
while (serial.available() > 0) processSerialData();
if ((time1 - time2) > timePolling) {
if (requestPID == true) {
serial.write('E');
requestPID = false;
} else if (writePID == true) {
int P = PApplet.parseInt(conf_KP.value());
int I = PApplet.parseInt(conf_KI.value());
int D = PApplet.parseInt(conf_KD.value());
char data[] = {0, 0, 0, 0, 0};
data[0] = 'W';
data[1] = PApplet.parseChar(P);
data[2] = PApplet.parseChar(I);
data[3] = PApplet.parseChar(D);
data[4] = ' ';
String str = new String(data);
serial.write(str);
println(str);
// println(" P=" + P + " I=" + I + " D=" + D);
writePID = false;
}
// else if (RUN==true ) {serial.write('A');}
else {
serial.write('A');
}
time2 = time1;
}
}
}
public void setup() {
minim = new Minim(this);
size(winWidth, winHeight, OPENGL);
background(bGround);
sm = new SceneManager(minim);
rulesChecker = new RulesChecker();
controlP5 = new ControlP5(this);
// ruleChoiceList = controlP5.addDropdownList("ruleChoiceList",850,100,100,100);
// customize(ruleChoiceList);
selectedRule = 0;
selectedCamera = 0;
gl = ((PGraphicsOpenGL) g).gl;
picker = new Picker(this);
oscP5 = new OscP5(this, port);
// TODO(sanjeet): Change the address
interfaceAddr = new NetAddress("127.0.0.1", port);
noStroke();
lines = loadStrings("fileFriedrich.txt"); // Hardcoded input file name
String[] tokens = split(lines[0], " ");
if (tokens.length != 1) {
println("Incorrect file format for number of cameras");
return;
}
int numOfCams = PApplet.parseInt(tokens[0]);
for (int i = 1; i < numOfCams + 1; i++) {
tokens = split(lines[i], " ");
float[] matrix = new float[16];
for (int j = 0; j < tokens.length; j++) {
matrix[j] = PApplet.parseFloat(tokens[j]);
}
cameras.add(new Cam(FloatBuffer.wrap(matrix))); // add all the cameras
}
tokens = split(lines[1 + numOfCams], " ");
if (tokens.length != 1) {
println("Incorrect file format for number of characters");
return;
}
int numOfChars = PApplet.parseInt(tokens[0]);
for (int i = 2 + numOfCams; i < lines.length; i++) {
tokens = split(lines[i], " ");
float[] matrix = new float[16];
for (int j = 0; j < tokens.length; j++) {
matrix[j] = PApplet.parseFloat(tokens[j]);
}
characters.add(new Character(FloatBuffer.wrap(matrix))); // add all the characters
}
characters.get(0).col = color(255, 255, 0);
characters.get(1).col = color(255, 0, 255);
timeline = new Timeline(sm);
// add initial tick to the begining of the timeline
timeline.addTick(cameras.get(0));
debug = new Debug(controlP5);
// title, start, end, initVal, xpos, ypos, width, height
// controlP5.addSlider("Timeline", 0,120,0,100,winHeight-50,winWidth-200,30);
}
