From 9260d066baa7adf2468af60e38869110e4a11255 Mon Sep 17 00:00:00 2001 From: dviid Date: Sat, 9 Feb 2013 21:18:22 +0100 Subject: [PATCH] Added ForceProfile --- .../ForceProfileArduino.ino | 67 ++++++++ .../ForceProfilePlot.pde | 144 ++++++++++++++++++ .../ForceProfileProcessing.pde | 128 ++++++++++++++++ .../ForceProfileProcessing/TerrainPlot.pde | 95 ++++++++++++ 4 files changed, 434 insertions(+) create mode 100644 software/apps/Motor/ForceProfile/ForceProfileArduino/ForceProfileArduino.ino create mode 100644 software/apps/Motor/ForceProfile/ForceProfileProcessing/ForceProfilePlot.pde create mode 100644 software/apps/Motor/ForceProfile/ForceProfileProcessing/ForceProfileProcessing.pde create mode 100644 software/apps/Motor/ForceProfile/ForceProfileProcessing/TerrainPlot.pde diff --git a/software/apps/Motor/ForceProfile/ForceProfileArduino/ForceProfileArduino.ino b/software/apps/Motor/ForceProfile/ForceProfileArduino/ForceProfileArduino.ino new file mode 100644 index 0000000..d9d01a2 --- /dev/null +++ b/software/apps/Motor/ForceProfile/ForceProfileArduino/ForceProfileArduino.ino @@ -0,0 +1,67 @@ + +#include +#include + +#define NEW '!' +#define OK '*' +#define PRINT '&' + +int table[800]; +int len = 800; + +char buf[32] = ""; +int cnt = 0; + +void setup() { + Serial.begin(9600); + MotorA.init(); + MotionA.init(INPUTA0); +} + +void loop() { + MotionA.update_position(); + int i = (int) (((float) MotionA.Xin / 1023.0) * len); + int f = table[i]; + MotorA.torque(f); + + // TODO - send position back to interface + /* + cnt++; + if(cnt == 1000) { + sprintf(buf, "%d %d %d", MotionA.Xin, i, f); + Serial.println(buf); + cnt = 0; + } + */ +} + + +void serialEvent() { + + if(Serial.available()) { + char in = (char)Serial.read(); + if(in == NEW) { + Serial.print(OK); + len = serial_read(); + for(int i = 0; i < len; i++) { + table[i] = serial_read(); + } + } else if(in == PRINT) { + Serial.println(len); + for(int i = 0; i < len; i++) { + Serial.print(table[i]); + Serial.print(';'); + } + Serial.print('#'); + } + } +} + + +int serial_read() { + while(Serial.available() < 2); + int l = Serial.read(); + int h = Serial.read(); + Serial.write(OK); + return (h * 256) + l; +} diff --git a/software/apps/Motor/ForceProfile/ForceProfileProcessing/ForceProfilePlot.pde b/software/apps/Motor/ForceProfile/ForceProfileProcessing/ForceProfilePlot.pde new file mode 100644 index 0000000..dcf3a60 --- /dev/null +++ b/software/apps/Motor/ForceProfile/ForceProfileProcessing/ForceProfilePlot.pde @@ -0,0 +1,144 @@ +class ForceProfilePlot { + + int d = 4, s = 4; + float steps = 4; + int max, min; + int w, h, by; + int px, py; + + int res = 800; + + int[] forces; + int[] forces_pixelvalues; + + int mx, my; + + String title; + + TerrainPlot tr; + + + ForceProfilePlot(String t, int w, int h, int x, int y, int nbr_ctrpoints, boolean terrain) { + this.title = t; + this.w = w + d; + this.h = h + d; + this.px = x - d; + this.py = y + d; + this.max = 512; + this.min = -512; + noFill(); + textSize(12); + this.s = 800 / nbr_ctrpoints; + this.steps = (float)(w + 2*d) / (nbr_ctrpoints - 1); + this.by = h / 2; + forces = new int[nbr_ctrpoints]; // zeros??? + forces_pixelvalues = new int[nbr_ctrpoints]; + if (terrain) + tr = new TerrainPlot("Terrain", w, h, x, y + h + 2 * d, nbr_ctrpoints, steps); + } + + void draw() { + + pushMatrix(); + + translate(px, py); + + stroke(255, 255, 255); + line( -d, h, w + d + d, h); + line( 0, -d, 0, h + d); + line( -d, 0, w + d + d, 0); + line( w + d, h + d, w + d, -d); + + fill(255, 255, 255); + textSize(10); + text(title, d, -d+2); + noFill(); + line(-d, by, w + d, by); + text(Integer.toString(this.max), w + d + d + d, 0); + text(Integer.toString(this.min), w + d + d + d, h + d); + + float x = 0; + for (int i = 0; i < forces_pixelvalues.length; i++) { + + stroke(127, 34, 255); + line(x, by, x, by - forces_pixelvalues[i]); + + stroke(255, 34, 127); + ellipse(x, by - forces_pixelvalues[i], 5, 5); + + if (i != 0) { + stroke(255, 255, 255); + line(x, by - forces_pixelvalues[i], x - steps, by - forces_pixelvalues[i - 1]); + } + + if(steps > 20) + text(Integer.toString(forces[i]), x, by - forces_pixelvalues[i] + (forces[i] < 0 ? 7 : -7)); + + x += steps; + } + + + if (mx > 0) { + int e = round((float) mx / steps); + stroke(255, 34, 127); + line(e * steps, by - forces_pixelvalues[e], mx, my); + } + + + popMatrix(); + + if (tr != null) tr.draw(); + } + + void forces_from_pixels() { + for (int i = 0; i < forces_pixelvalues.length; i++) { + forces[i] = (int)(((by - forces_pixelvalues[i]) / h * 2) * 512); + } + } + + void pixels_from_forces() { + for (int i = 0; i < forces.length; i++) { + forces_pixelvalues[i] = (int)(((float) forces[i] / (float)this.max) * h / 2); + } + } + + + void drag(int mousex, int mousey) { + if (hit(mousex, mousey)) + position_point(mousex, mousey); + } + + void click(int mousex, int mousey) { + if (hit(mousex, mousey)) + position_point(mousex, mousey); + } + + void release() { + mx = -1; + my = -1; + } + + void position_point(int mousex, int mousey) { + int i = round((float) (mousex - px - d) / steps); + forces_pixelvalues[i] = h / 2 - (mousey - py - d); + forces[i] = (int)((((float)forces_pixelvalues[i]) / h * 2) * 512); + + my = mousey - py - d; + mx = mousex - px - d; + + if (tr != null) { + tr.data(forces_pixelvalues); + /* + if (i > 0) + tr.data(i - 1, forces_pixelvalues[i - 1], i, forces_pixelvalues[i]); + if (i < forces_pixelvalues.length - 1) + tr.data(i, forces_pixelvalues[i], i + 1, forces_pixelvalues[i + 1]); + */ + } + } + + boolean hit(int mousex, int mousey) { + return (mousex < px + w) && (mousex > px) && (mousey > py) && (mousey < py + h); + } +} + diff --git a/software/apps/Motor/ForceProfile/ForceProfileProcessing/ForceProfileProcessing.pde b/software/apps/Motor/ForceProfile/ForceProfileProcessing/ForceProfileProcessing.pde new file mode 100644 index 0000000..e4b6da4 --- /dev/null +++ b/software/apps/Motor/ForceProfile/ForceProfileProcessing/ForceProfileProcessing.pde @@ -0,0 +1,128 @@ +import controlP5.*; +import processing.serial.*; + +static char NEW = '!'; +static char OK = '*'; +static char PRINT = '&'; + +ForceProfilePlot fpp; +Button send; +Button print; + +//Serial port +Serial p; + +void setup() { + + int b = 15; + //int h = (displayHeight / 3); (work in certain resolution) + //int w = displayWidth / 2; (work in certain resolution) + int w = 720; + int h = 300; + + println("w: " + w + " h: " + h); + + fpp = new ForceProfilePlot("Force Profile - Perception Terrain", w, h, b, b, 80, true); + + ControlP5 cp5 = new ControlP5(this); + + send = cp5.addButton("send"); + send.setPosition(w - 20, 2*h + 2*b + 7); + send.setSize(35, 15); + send.setColorForeground(color(127,34,255)); + send.setColorBackground(color(50,50,50)); + + print = cp5.addButton("print"); + print.setPosition(w - 140, 2*h + 2*b + 7); + print.setSize(35, 15); + print.setColorForeground(color(127,34,255)); + print.setColorBackground(color(50,50,50)); + + size(w + 60, 2*h + 60); + + p = new Serial(this, Serial.list()[0], 9600); + +} + +void draw() { + background(0); + fpp.draw(); + + if(p.available() > 0) { + String in = p.readStringUntil('\n'); + if(in != null) { + println(in); + String[] t = splitTokens(in); + } + } + +} + +void mouseDragged() { + fpp.drag(mouseX, mouseY); +} + +void mouseClicked() +{ + fpp.click(mouseX, mouseY); +} + +void mouseReleased() { + fpp.release(); +} + +public void send() { + + send.setOff(); + + p.write(NEW); + println("1"); + while(p.available() <= 0); + println("2"); + char in = p.readChar(); + println("3"); + if(in == OK) { + println("GO!"); + int s = 800 / fpp.forces.length; + int len = s * (fpp.forces.length - 1); + serial_send(len); + float dx = 1023.0f / (fpp.forces.length - 1); + float ddx = dx / s; + for(int i = 0; i < fpp.forces.length-1; i++) { + float k = fpp.forces[i]; + float m = (fpp.forces[i+1] - k) / dx; + for(int j = 0; j < s; j++) { + int d = (int)(k + m*j); + serial_send(d); + } + } + } + + send.setOn(); + +} + +public void print() { + println("print"); + print.setOff(); + p.write(PRINT); + while(p.available() <= 0); // block + while(p.available() > 0) { + String s = p.readStringUntil('#'); + if(s != null) + println(s); + } + send.setOn(); +} + +public boolean serial_send(int i) { + p.clear(); + char msb = (char) (i / 256); + char lsb = (char) (i & 0xff); + p.write(lsb); + p.write(msb); + while(p.available() <= 0); + char in = p.readChar(); + return (in == OK); +} + diff --git a/software/apps/Motor/ForceProfile/ForceProfileProcessing/TerrainPlot.pde b/software/apps/Motor/ForceProfile/ForceProfileProcessing/TerrainPlot.pde new file mode 100644 index 0000000..5a92d29 --- /dev/null +++ b/software/apps/Motor/ForceProfile/ForceProfileProcessing/TerrainPlot.pde @@ -0,0 +1,95 @@ +class TerrainPlot { + + int d = 4; + float max, min; + int w, h; + int px, py; + + float steps_per_index; // this should not exceed step_index; + float s, step_index; + + String title; + float pixelvalues[]; + + TerrainPlot(String t, int w, int h, int x, int y, int nbr_index, float step_index) { + this.title = t; + this.w = w + d; + this.h = h + d; + this.px = x - d; + this.py = y + d; + noFill(); + textSize(12); + this.step_index = step_index; + this.steps_per_index = (step_index / 3); + this.s = 3; + /* + + if(step_index > steps_per_index) { + this.s = ((float) step_index / steps_per_index); + } else { + this.s = step_index; + this.steps_per_index = 1; + } + */ + pixelvalues = new float[(int)((nbr_index - 1) * steps_per_index) + 1]; + } + + void draw() { + pushMatrix(); + + pushMatrix(); + + translate(px, py); + + stroke(255, 255, 255); + line( -d, h, w + d + d, h); + line( 0, -d, 0, h + d); + line( -d, 0, w + d + d, 0); + line( w + d, h + d, w + d, -d); + + fill(255, 255, 255); + textSize(10); + //text(title, d, -d+2); + + popMatrix(); + + translate(px, py + h); + + for(int i = 0; i < pixelvalues.length; i++) { + float y = pixelvalues[i]; + if(abs(min) > h) { + y = (y / abs(min)) * h; + } + line(i * s, 0, i * s, y); + } + + popMatrix(); + } + + void data(int[] forces_points) { + + float FF = 0;//forces[0]; // forces interpolated + float top = 0; // terrain value (integral of forces) + float inc; // interpolated slope ( F[i+1] - F[i] ) / s + + max = 0; min = 0; + for(int i = 0; i < forces_points.length - 1; i++) { + inc = 0.005 * (forces_points[i+1] - forces_points[i]) ; + for(int j = 0; j < steps_per_index; j++) { + FF += inc; + top = top + FF; + pixelvalues[(int)(i * steps_per_index) + j] = top; + if(top > max) max = top; + } + } + + for(int i = 0; i < pixelvalues.length; i++) { + pixelvalues[i] = (int)(pixelvalues[i] - max); + if(pixelvalues[i] < min) { + min = pixelvalues[i]; + } + } + + } + +}