Added ForceProfile
This commit is contained in:
dviid
parent
336b2b45ab
commit
9260d066ba
@ -0,0 +1,67 @@
|
||||
|
||||
#include <Motion.h>
|
||||
#include <Motor.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
@ -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];
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user