Miguel Pardo

INTERFACE AND APLICATION PROGRAMMING


Goals:
Write an application that interfaces with an input &/or output device

At the lab

For this assignment I’m going to use an Arduino and a freeIMU.

The first reason to make that is because I saw them on a table in the lab and I still didn’t play with them. I guess someone had to use it.

The second reason is because it is directly related to my final project. For my final project I’m going to use an accelerometer. I’m actually going to design a board with a tiny accelerometer on it.

Actually for the final project I want also to design a design a radio board and connect it to this new accelerometer board. The reason to do that is for visualizing the movement of a boat.

So Francisco borrows me his Arduino UNO kit and I start playing with it. The first thing I need to do is thinking which language program I’m going to use from the entire list that Neil explain us during the class. Since I am not a programmer I’m wondering which one of them would be useful, simple and functional for me. He said once Processing is pretty compatible with Arduino because this last one is derived from the integrated development environment for the Processing programming language. Why not? I decide I will focus a bit on Processing.

I am finding lots of libraries for Processing that interacts with Arduino and an IMU. My idea is finding one that fits my necessities. An to do that I choose the same libraries that come with the freeIMU. Both of them are very interesting and appropriate for my final project.

To start working with them is not that evident in the beginning because it takes me a while to realize that every time you want to use the library you have to know which usbnumber I am using.

To do that you should follow these instructions:

Open you terminal, and type: ls /dev/tty* Then look for your usb-number which is in the 4 line aprox. When you see it, just mark it down.

Now it's time to work with Processor and use that serial number in your code. When you have copied that number in the code you will be able to run the library called FreeIMU_cube.

This is the code:
*/
import processing.serial.*;
import processing.opengl.*;
Serial myPort; // Create object from Serial class
final String serialPort = "/dev/tty.usbmodem641"; // replace this with your serial port. On windows you will need something
like "COM1".
float [] q = new float [4];
float [] hq = null;
float [] Euler = new float [3]; // psixxx, theta, phi
int lf = 10; // 10 is '\n' in ASCII
byte[] inBuffer = new byte[22]; // this is the number of chars on each line from the Arduino (including /r/n)
PFont font;
final int VIEW_SIZE_X = 800, VIEW_SIZE_Y = 600;
final int burst = 32;
int count = 0;
void myDelay(int time) {
try {
Thread.sleep(time);
} catch (InterruptedException e) { }
}
void setup()
{
size(VIEW_SIZE_X, VIEW_SIZE_Y, OPENGL);
myPort = new Serial(this, serialPort, 115200);
// The font must be located in the sketch's "data" directory to load successfully
font = loadFont("CourierNew36.vlw");
println("Waiting IMU..");
myPort.clear();
while (myPort.available() == 0) {
myPort.write("v");
myDelay(1000);
}
println(myPort.readStringUntil('\n'));
myPort.write("q" + char(burst));
myPort.bufferUntil('\n');
}
float decodeFloat(String inString) {
byte [] inData = new byte[4];
if(inString.length() == 8) {
inData[0] = (byte) unhex(inString.substring(0, 2));
inData[1] = (byte) unhex(inString.substring(2, 4));
inData[2] = (byte) unhex(inString.substring(4, 6));
inData[3] = (byte) unhex(inString.substring(6, 8));
}
int intbits = (inData[3] << 24) | ((inData[2] & 0xff) << 16) | ((inData[1] & 0xff) << 8) | (inData[0] & 0xff);
return Float.intBitsToFloat(intbits);
}
void serialEvent(Serial p) {
if(p.available() >= 18) {
String inputString = p.readStringUntil('\n');
//print(inputString);
if (inputString != null && inputString.length() > 0) {
String [] inputStringArr = split(inputString, ",");
if(inputStringArr.length >= 5) { // q1,q2,q3,q4,\r\n so we have 5 elements
q[0] = decodeFloat(inputStringArr[0]);
q[1] = decodeFloat(inputStringArr[1]);
q[2] = decodeFloat(inputStringArr[2]);
q[3] = decodeFloat(inputStringArr[3]);
}
}
count = count + 1;
if(burst == count) { // ask more data when burst completed
p.write("q" + char(burst));
count = 0;
}
}
}
void buildBoxShape() {
//box(a, b, c);
float a=40;
float b=10;
float c=60;
a=a/2;
b=b/2;
c=c/2;
noStroke();
beginShape(QUADS);
//Z+ (to the drawing area)
fill(#00ff00);
vertex(-a, -b, c);
vertex(a, -b, c);
vertex(a, b, c);
vertex(-a, b, c);
//Z-
fill(#0000ff);
vertex(-a, -b, -c);
vertex(a, -b, -c);
vertex(a, b, -c);
vertex(-a, b, -c);
//X-
fill(#ff0000);
vertex(-a, -b, -c);
vertex(-a, -b, c);
vertex(-a, b, c);
vertex(-a, b, -c);
//X+
fill(#ffff00);
vertex(a, -b, -c);
vertex(a, -b, c);
vertex(a, b, c);
vertex(a, b, -c);
//Y-
fill(#ff00ff);
vertex(-a, -b, -c);
vertex(a, -b, -c);
vertex(a, -b, c);
vertex(-a, -b, c);
//Y+
fill(#00ffff);
vertex(-a, b, -c);
vertex(a, b, -c);
vertex(a, b, c);
vertex(-a, b, c);
endShape();
}
void drawCube() {
pushMatrix();
translate(VIEW_SIZE_X/2, VIEW_SIZE_Y/2 + 50, 0);
scale(5,5,5);
// a demonstration of the following is at
// http://www.varesano.net/blog/fabio/ahrs-sensor-fusion-orientation-filter-3d-graphical-rotating-cube
rotateZ(-Euler[2]);
rotateX(-Euler[1]);
rotateY(-Euler[0]);
buildBoxShape();
popMatrix();
}
void draw() {
background(#000000);
fill(#ffffff);
if(hq != null) { // use home quaternion
quaternionToEuler(quatProd(hq, q), Euler);
text("Disable home position by pressing \"n\"", 20, VIEW_SIZE_Y - 30);
}
else {
quaternionToEuler(q, Euler);
text("Point FreeIMU's X axis to your monitor then press \"h\"", 20, VIEW_SIZE_Y - 30);
}
textFont(font, 20);
textAlign(LEFT, TOP);
text("Q:\n" + q[0] + "\n" + q[1] + "\n" + q[2] + "\n" + q[3], 20, 20);
text("Euler Angles:\nYaw (psi) : " + degrees(Euler[0]) + "\nPitch (theta): " + degrees(Euler[1]) + "\nRoll (phi) : " +
degrees(Euler[2]), 200, 20);
drawCube();
//myPort.write("q" + 1);
}
void keyPressed() {
if(key == 'h') {
println("pressed h");
// set hq the home quaternion as the quatnion conjugate coming from the sensor fusion
hq = quatConjugate(q);
}
else if(key == 'n') {
println("pressed n");
hq = null;
}
}
// See Sebastian O.H. Madwick report
// "An efficient orientation filter for inertial and intertial/magnetic sensor arrays" Chapter 2 Quaternion representation
void quaternionToEuler(float [] q, float [] euler) {
euler[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1); // psi
euler[1] = -asin(2 * q[1] * q[3] + 2 * q[0] * q[2]); // theta
euler[2] = atan2(2 * q[2] * q[3] - 2 * q[0] * q[1], 2 * q[0] * q[0] + 2 * q[3] * q[3] - 1); // phi
}
float [] quatProd(float [] a, float [] b) {
float [] q = new float[4];
q[0] = a[0] * b[0] - a[1] * b[1] - a[2] * b[2] - a[3] * b[3];
q[1] = a[0] * b[1] + a[1] * b[0] + a[2] * b[3] - a[3] * b[2];
q[2] = a[0] * b[2] - a[1] * b[3] + a[2] * b[0] + a[3] * b[1];
q[3] = a[0] * b[3] + a[1] * b[2] - a[2] * b[1] + a[3] * b[0];
return q;
}
// returns a quaternion from an axis angle representation
float [] quatAxisAngle(float [] axis, float angle) {
float [] q = new float[4];
float halfAngle = angle / 2.0;
float sinHalfAngle = sin(halfAngle);
q[0] = cos(halfAngle);
q[1] = -axis[0] * sinHalfAngle;
q[2] = -axis[1] * sinHalfAngle;
q[3] = -axis[2] * sinHalfAngle;
return q;
}
// return the quaternion conjugate of quat
float [] quatConjugate(float [] quat) {
float [] conj = new float[4];
conj[0] = quat[0];
conj[1] = -quat[1];
conj[2] = -quat[2];
conj[3] = -quat[3];
return conj;
}

This is just an image of what you can do with this program:

After a while using this library I realize that Neil was right; it’s very similar to Arduino.

Now it's time for another interesting library called FreeIMU_yaw_pitch_roll. Due to the parameters coded in the library I can play with it and modify it a bit.

This is the code and then the result:
*/
import processing.serial.*;
Serial myPort; // Create object from Serial class
float [] q = new float [4];
float [] Euler = new float [3]; // psi, theta, phi
int lf = 10; // 10 is '\n' in ASCII
byte[] inBuffer = new byte[22]; // this is the number of chars on each line from the Arduino (including /r/n)
PFont font;
final int VIEW_SIZE_X = 800, VIEW_SIZE_Y = 600;
int burst = 32, count = 0;
void myDelay(int time) {
try {
Thread.sleep(time);
} catch (InterruptedException e) { }
}
void setup()
{
size(VIEW_SIZE_X, VIEW_SIZE_Y, P3D);
myPort = new Serial(this, "/dev/tty.usbmodem641", 115200);
// The font must be located in the sketch's "data" directory to load successfully
font = loadFont("CourierNew36.vlw");
println("Waiting IMU..");
myPort.clear();
while (myPort.available() == 0) {
myPort.write("v");
myDelay(1000);
}
println(myPort.readStringUntil('\n'));
myPort.write("q" + char(burst));
myPort.bufferUntil('\n');
}
float decodeFloat(String inString) {
byte [] inData = new byte[4];
if(inString.length() == 8) {
inData[0] = (byte) unhex(inString.substring(0, 2));
inData[1] = (byte) unhex(inString.substring(2, 4));
inData[2] = (byte) unhex(inString.substring(4, 6));
inData[3] = (byte) unhex(inString.substring(6, 8));
}
int intbits = (inData[3] << 24) | ((inData[2] & 0xff) << 16) | ((inData[1] & 0xff) << 8) | (inData[0] & 0xff);
return Float.intBitsToFloat(intbits);
}
void serialEvent(Serial p) {
if(p.available() >= 18) {
String inputString = p.readStringUntil('\n');
//print(inputString);
if (inputString != null && inputString.length() > 0) {
String [] inputStringArr = split(inputString, ",");
if(inputStringArr.length >= 5) { // q1,q2,q3,q4,\r\n so we have 5 elements
q[0] = decodeFloat(inputStringArr[0]);
q[1] = decodeFloat(inputStringArr[1]);
q[2] = decodeFloat(inputStringArr[2]);
q[3] = decodeFloat(inputStringArr[3]);
}
}
count = count + 1;
if(burst == count) { // ask more data when burst completed
p.write("q" + char(burst));
count = 0;
}
}
}
void buildBoxShape() {
//box(60, 10, 40);
noStroke();
beginShape(QUADS);
//Z+ (to the drawing area)
fill(#00ff00);
vertex(-30, -5, 20);
vertex(30, -5, 20);
vertex(30, 5, 20);
vertex(-30, 5, 20);
//Z-
fill(#0000ff);
vertex(-30, -5, -20);
vertex(30, -5, -20);
vertex(30, 5, -20);
vertex(-30, 5, -20);
//X-
fill(#ff0000);
vertex(-30, -5, -20);
vertex(-30, -5, 20);
vertex(-30, 5, 20);
vertex(-30, 5, -20);
//X+
fill(#ffff00);
vertex(30, -5, -20);
vertex(30, -5, 20);
vertex(30, 5, 20);
vertex(30, 5, -20);
//Y-
fill(#ff00ff);
vertex(-30, -5, -20);
vertex(30, -5, -20);
vertex(30, -5, 20);
vertex(-30, -5, 20);
//Y+
fill(#00ffff);
vertex(-30, 5, -20);
vertex(30, 5, -20);
vertex(30, 5, 20);
vertex(-30, 5, 20);
endShape();
}
void drawcompass(float heading, int circlex, int circley, int circlediameter) {
noStroke();
ellipse(circlex, circley, circlediameter, circlediameter);
fill(#ff0000);
ellipse(circlex, circley, circlediameter/20, circlediameter/20);
stroke(#ff0000);
strokeWeight(4);
line(circlex, circley, circlex - circlediameter/2 * sin(-heading), circley - circlediameter/2 * cos(-heading));
noStroke();
fill(#ffffff);
textAlign(CENTER, BOTTOM);
text("N", circlex, circley - circlediameter/2 - 10);
textAlign(CENTER, TOP);
text("S", circlex, circley + circlediameter/2 + 10);
textAlign(RIGHT, CENTER);
text("W", circlex - circlediameter/2 - 10, circley);
textAlign(LEFT, CENTER);
text("E", circlex + circlediameter/2 + 10, circley);
}
void drawAngle(float angle, int circlex, int circley, int circlediameter, String title) {
angle = angle + PI/2;
noStroke();
ellipse(circlex, circley, circlediameter, circlediameter);
fill(#ff0000);
strokeWeight(4);
stroke(#ff0000);
line(circlex - circlediameter/2 * sin(angle), circley - circlediameter/2 * cos(angle), circlex + circlediameter/2 *
sin(angle), circley + circlediameter/2 * cos(angle));
noStroke();
fill(#ffffff);
textAlign(CENTER, BOTTOM);
text(title, circlex, circley - circlediameter/2 - 30);
}
void draw() {
quaternionToYawPitchRoll(q, Euler);
background(#000080);
fill(#ffffff);
textFont(font, 20);
//float temp_decoded = 35.0 + ((float) (temp + 13200)) / 280;
//text("temp:\n" + temp_decoded + " C", 350, 250);
textAlign(LEFT, TOP);
text("Q:\n" + q[0] + "\n" + q[1] + "\n" + q[2] + "\n" + q[3], 20, 10);
text("Euler Angles:\nYaw (psi) : " + degrees(Euler[0]) + "\nPitch (theta): " + degrees(Euler[1]) + "\nRoll (phi) : " +
degrees(Euler[2]), 200, 10);
drawcompass(Euler[0], VIEW_SIZE_X/2 - 250, VIEW_SIZE_Y/2, 200);
drawAngle(Euler[1], VIEW_SIZE_X/2, VIEW_SIZE_Y/2, 200, "Pitch:");
drawAngle(Euler[2], VIEW_SIZE_X/2 + 250, VIEW_SIZE_Y/2, 200, "Roll:");
}
void quaternionToYawPitchRoll(float [] q, float [] ypr) {
float gx, gy, gz; // estimated gravity direction
gx = 2 * (q[1]*q[3] - q[0]*q[2]);
gy = 2 * (q[0]*q[1] + q[2]*q[3]); vgz = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
ypr[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1);
ypr[1] = atan2(gx, sqrt(gy*gy + gz*gz));
ypr[2] = atan2(gy, sqrt(gx*gx + gz*gz));
}

What I learned:
I learned that coding is not a simple thing, feels like there is no end when you start coding, you just can see the infinite out there. There is no edge. A trully never ending story. Learning a programing language requires time and effort. I played with only Processing and feel like there is a huge world to learn yet. Despite of this, Processing coding is pretty understandable and you can make simple and useful programs like this two.