// A function to rotate a vector
public void rotateVector(PVector v, float theta) {
float m = v.mag();
float a = v.heading2D();
a += theta;
v.x = m * PApplet.cos(a);
v.y = m * PApplet.sin(a);
}
public void avoid(ArrayList obstacles) {
// Make a vector that will be the position of the object
// relative to the Boid rotated in the direction of boid's velocity
PVector closestRotated = new PVector(sight + 1, sight + 1);
float closestDistance = 99999;
Obstacle avoid = null;
// Let's look at each obstacle
for (int i = 0; i < obstacles.size(); i++) {
Obstacle o = (Obstacle) obstacles.get(i);
float d = PVector.dist(loc, o.loc);
PVector dir = vel.get();
dir.normalize();
PVector diff = PVector.sub(o.loc, loc);
// Now we use the dot product to rotate the vector that points from boid to obstacle
// Velocity is the new x-axis
PVector rotated = new PVector(diff.dot(dir), diff.dot(getNormal(dir)));
// Is the obstacle in our path?
if (PApplet.abs(rotated.y) < (o.radius + r)) {
// Is it the closest obstacle?
if ((rotated.x > 0) && (rotated.x < closestRotated.x)) {
closestRotated = rotated;
avoid = o;
}
}
}
// Can we actually see the closest one?
if (PApplet.abs(closestRotated.x) < sight) {
// The desired vector should point away from the obstacle
// The closer to the obstacle, the more it should steer
PVector desired =
new PVector(closestRotated.x, -closestRotated.y * sight / closestRotated.x);
desired.normalize();
desired.mult(closestDistance);
desired.limit(maxspeed);
// Rotate back to the regular coordinate system
rotateVector(desired, vel.heading2D());
// Draw some debugging stuff
if (debug) {
stroke(0);
line(loc.x, loc.y, loc.x + desired.x * 10, loc.y + desired.y * 10);
avoid.highlight(true);
}
// Apply Reynolds steering rules
desired.sub(vel);
desired.limit(maxforce);
acc.add(desired);
}
}
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 controlEvent(ControlEvent theEvent) {
// PulldownMenu is if type ControlGroup.
// A controlEvent will be triggered from within the ControlGroup.
// therefore you need to check the originator of the Event with
// if (theEvent.isGroup())
// to avoid an error message from controlP5.
if (theEvent.isGroup()) {
if (theEvent.group().name() == "ruleChoiceList") {
// println(theEvent.group().value()+" from "+theEvent.group());
selectedRule = PApplet.parseInt(theEvent.group().value());
// has to be here for the controlp5 library
}
} else if (theEvent.isController()) {
// println(theEvent.controller().value()+" from "+theEvent.controller());
}
// switch(theEvent.controller().id()) {
// case(1):
// myColorRect = (int)(theEvent.controller().value());
// break;
// case(2):
// myColorBackground = (int)(theEvent.controller().value());
// break;
// case(3):
// println(theEvent.controller().stringValue());
// break;
// }
}
public void InitSerial(float portValue) {
// initialize the serial port selected in the listBox
println(
"initializing serial "
+ PApplet.parseInt(portValue)
+ " in serial.list()"); // for debugging
// grab the name of the serial port
String portPos = Serial.list()[PApplet.parseInt(portValue)];
// initialize the port
serial = new Serial(this, portPos, 115200);
// read bytes into a buffer until you get a linefeed (ASCII 10):
serial.bufferUntil('\n');
println("done init serial");
// initialized com port flag
init_com = 1;
}
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 render() {
// Draw a triangle rotated in the direction of velocity
float theta = vel.heading2D() + PApplet.radians(90);
fill(100);
stroke(0);
pushMatrix();
translate(loc.x, loc.y);
rotate(theta);
beginShape(PConstants.TRIANGLES);
vertex(0, -r * 2);
vertex(-r, r * 2);
vertex(r, r * 2);
endShape();
if (debug) {
stroke(50);
line(0, 0, 0, -sight);
}
popMatrix();
}
public static void main(String args[]) {
PApplet.main(new String[] {"--bgcolor=#FFFFFF", "lightArray"});
}
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 static void main(String args[]) {
PApplet.main(new String[] {"OneScreen"});
}
public void stop() {
mymod.stop();
super.stop();
}
public static void main(String args[]) {
PApplet.main(new String[] {"--bgcolor=#F0F0F0", "SimpleOpenNI_NITE_Hands"});
}
public static void main(String args[]) {
PApplet.main(new String[] {"--bgcolor=#ECE9D8", "fractalTree3"});
}
public static void main(String args[]) {
PApplet.main(new String[] {"--bgcolor=#FFFFFF", "sketch_aug05a"});
}
public static void main(String args[]) {
PApplet.main(new String[] {"--bgcolor=#FFFFFF", "PlanetScape"});
}
public static void main(String args[]) {
PApplet.main(
new String[] {
"--present", "--bgcolor=#666666", "--stop-color=#cccccc", "ObstacleAvoidance"
});
}
public static void main(String args[]) {
PApplet.main(new String[] {"black_circle_grow_2d_sequential"});
}
public static void main(String args[]) {
PApplet.main(new String[] {"--bgcolor=#FFFFFF", "mc1_hugh_rawlinson"});
}
public static void main(String args[]) {
PApplet.main(new String[] {"--bgcolor=#D4D0C8", "controlP5Example"});
}
// Close the sound engine
public void stop() {
Sonia.stop();
super.stop();
}
public static void main(String args[]) {
PApplet.main(new String[] {"example_20_4"});
}
public static void main(String args[]) {
PApplet.main(
new String[] {"--present", "--bgcolor=#666666", "--hide-stop", "basic_processing_sketch"});
}
public static void main(String args[]) {
PApplet.main(new String[] {"Pendulum"});
}
public static void main(String args[]) {
PApplet.main(
new String[] {"--present", "--bgcolor=#666666", "--stop-color=#cccccc", "randomLine"});
}
/*static int SKEL_HEAD
static int SKEL_LEFT_ANKLE
static int SKEL_LEFT_COLLAR
static int SKEL_LEFT_ELBOW
static int SKEL_LEFT_FINGERTIP
static int SKEL_LEFT_FOOT
static int SKEL_LEFT_HAND
static int SKEL_LEFT_HIP
static int SKEL_LEFT_KNEE
static int SKEL_LEFT_SHOULDER
static int SKEL_LEFT_WRIST
static int SKEL_NECK
static int SKEL_RIGHT_ANKLE
static int SKEL_RIGHT_COLLAR
static int SKEL_RIGHT_ELBOW
static int SKEL_RIGHT_FINGERTIP
static int SKEL_RIGHT_FOOT
static int SKEL_RIGHT_HAND
static int SKEL_RIGHT_HIP
static int SKEL_RIGHT_KNEE
static int SKEL_RIGHT_SHOULDER
static int SKEL_RIGHT_WRIST
static int SKEL_TORSO
static int SKEL_WAIST */
public static void main(String args[]) {
PApplet.main(new String[] {"--bgcolor=#FFFFFF", "kinectPong"});
}
public static void main(String args[]) {
PApplet.main(
new String[] {"--present", "--bgcolor=#666666", "--hide-stop", "flickr_uploader_get"});
}
public static void main(String args[]) {
PApplet.main(new String[] {"xhtml1"});
}
public static void main(String args[]) {
PApplet.main(new String[] {"--bgcolor=#F0F0F0", "OpenWheels"});
}
public static void main(String args[]) {
PApplet.main(new String[] {"--bgcolor=#f3f2f5", "portaPlay"});
}
public static void main(String args[]) {
PApplet.main(new String[] {"transparent_polychrome_star_burst_2d_sequential"});
}
