Added Oscilloscope code in apps
This commit is contained in:
dviid
parent
4a2f82f907
commit
62b7761e9a
111
software/apps/Motion/Oscilloscope/Oscilloscope.pde
Normal file
111
software/apps/Motion/Oscilloscope/Oscilloscope.pde
Normal file
@ -0,0 +1,111 @@
|
||||
import controlP5.*;
|
||||
import processing.serial.*;
|
||||
|
||||
//Plots
|
||||
ScopePlot plotA;
|
||||
ScopePlot plotB;
|
||||
ScopePlotDouble plotC;
|
||||
|
||||
//Communication with MMM boards
|
||||
Protocol proto;
|
||||
|
||||
//Serial port
|
||||
Serial p;
|
||||
|
||||
//Controlers
|
||||
ControlP5 cp5;
|
||||
SliderCallback scb;
|
||||
|
||||
// (k)
|
||||
float kmin = 0.0f;
|
||||
float kmax = 7.0f;
|
||||
float k = 0.2f;
|
||||
Slider k_slider;
|
||||
|
||||
// (m)
|
||||
float mmin = 0.0f;
|
||||
float mmax = 7.0f;
|
||||
float m = 1.0f;
|
||||
Slider m_slider;
|
||||
|
||||
// (d)
|
||||
float dmin = 0.0f;
|
||||
float dmax = 7.0f;
|
||||
float d = 0.02f;
|
||||
Slider d_slider;
|
||||
|
||||
void setup() {
|
||||
|
||||
int b = 15;
|
||||
int h = (displayHeight / 4) - (3*b);
|
||||
int w = displayWidth / 2;
|
||||
|
||||
plotA = new ScopePlot("Force", w, h, b, b + b, 512, true);
|
||||
plotB = new ScopePlot("Velocity", w, h, b, h + 4*b, 512, true);
|
||||
plotC = new ScopePlotDouble("Position Estimate", w, h, b, 2*h + 6*b, 512, true);
|
||||
|
||||
p = new Serial(this, Serial.list()[0], 9600);
|
||||
proto = new Protocol(p);
|
||||
|
||||
// Controls
|
||||
cp5 = new ControlP5(this);
|
||||
scb = new SliderCallback();
|
||||
|
||||
int sw = w / 3;
|
||||
|
||||
k_slider = cp5.addSlider("k", kmin, kmax, b, b-5, sw - b, b-5);
|
||||
k_slider.setColorForeground(color(127,34,255));
|
||||
k_slider.setColorBackground(color(50,50,50));
|
||||
k_slider.addListener(scb);
|
||||
|
||||
m_slider = cp5.addSlider("m", mmin, mmax, 2*b + sw, b-5, sw - b, b-5);
|
||||
m_slider.setColorForeground(color(127,34,255));
|
||||
m_slider.setColorBackground(color(50,50,50));
|
||||
m_slider.addListener(scb);
|
||||
|
||||
d_slider = cp5.addSlider("d", dmin, dmax, 3*b + 2*sw, b-5, sw - b, b-5);
|
||||
d_slider.setColorForeground(color(127,34,255));
|
||||
d_slider.setColorBackground(color(50,50,50));
|
||||
d_slider.addListener(scb);
|
||||
|
||||
size(w + 60, displayHeight - (h + (3*b)));
|
||||
}
|
||||
|
||||
void draw() {
|
||||
background(0);
|
||||
plotA.draw();
|
||||
plotB.draw();
|
||||
plotC.draw();
|
||||
}
|
||||
|
||||
/*
|
||||
void mouseDragged()
|
||||
{
|
||||
plotA.mdata(mouseX, mouseY);
|
||||
plotB.mdata(mouseX, mouseY);
|
||||
plotC.mdata(mouseX, mouseY);
|
||||
}
|
||||
|
||||
void mouseClicked()
|
||||
{
|
||||
plotA.mdata(mouseX, mouseY);
|
||||
plotB.mdata(mouseX, mouseY);
|
||||
plotC.mdata(mouseX, mouseY);
|
||||
}
|
||||
*/
|
||||
|
||||
void serialEvent (Serial myPort) {
|
||||
proto.processData();
|
||||
plotA.data(proto.F);
|
||||
plotB.data(proto.V);
|
||||
plotC.data(proto.X - 512, proto.Xin - 512);
|
||||
}
|
||||
|
||||
class SliderCallback implements ControlListener {
|
||||
public void controlEvent(ControlEvent ev) {
|
||||
p.write(new String(ev.controller().name()+ev.controller().value()+";"));
|
||||
//println(ev.controller().name() + ": " + ev.controller().value());
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
24
software/apps/Motion/Oscilloscope/Protocol.pde
Normal file
24
software/apps/Motion/Oscilloscope/Protocol.pde
Normal file
@ -0,0 +1,24 @@
|
||||
import processing.serial.*;
|
||||
|
||||
class Protocol {
|
||||
Serial port; // The serial port
|
||||
Protocol() {;}
|
||||
|
||||
int F, V, X, Xin;
|
||||
|
||||
Protocol(Serial p) {
|
||||
port = p;
|
||||
port.bufferUntil('\n');
|
||||
}
|
||||
|
||||
void processData() {
|
||||
String in = port.readStringUntil('\n');
|
||||
if(in != null){
|
||||
String[] t = splitTokens(in);
|
||||
F = int(t[0]);
|
||||
V = int(t[1]);
|
||||
X = int(t[2]);
|
||||
Xin = int(t[3]);
|
||||
}
|
||||
}
|
||||
}
|
||||
161
software/apps/Motion/Oscilloscope/ScopePlot.pde
Normal file
161
software/apps/Motion/Oscilloscope/ScopePlot.pde
Normal file
@ -0,0 +1,161 @@
|
||||
class ScopePlot {
|
||||
|
||||
int d = 4, s = 4;
|
||||
int indx, max, min;
|
||||
int w, h;
|
||||
int px, py;
|
||||
int by;
|
||||
int[] values;
|
||||
int[] pixelvalues;
|
||||
String title;
|
||||
|
||||
ScopePlot(String t, int w, int h, int x, int y, int maxval, boolean mid){
|
||||
this.title = t;
|
||||
this.w = w + d;
|
||||
this.h = h + d;
|
||||
this.indx = s;
|
||||
this.px = x - d;
|
||||
this.py = y + d;
|
||||
this.max = maxval;
|
||||
this.min = -maxval;
|
||||
noFill();
|
||||
textSize(12);
|
||||
values = new int[w / s];
|
||||
pixelvalues = new int[w / s];
|
||||
if(mid) by = h / 2;
|
||||
else by = this.h;
|
||||
}
|
||||
|
||||
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();
|
||||
if(by != h) {
|
||||
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);
|
||||
} else {
|
||||
text(Integer.toString(this.max), w + d + d + d, 0);
|
||||
text(Integer.toString(values.length), w + d + d + d, h + d + d + d);
|
||||
}
|
||||
|
||||
int x = s;
|
||||
for(int i = 0; i < pixelvalues.length; i++) {
|
||||
stroke(127,34,255);
|
||||
line(x, by, x, by - pixelvalues[i]);
|
||||
stroke(255,34,127);
|
||||
if(i == indx - 1) {
|
||||
stroke(255);
|
||||
text(Integer.toString(values[i]), w + d + d + d, by - pixelvalues[i]);
|
||||
}
|
||||
ellipse(x, by - pixelvalues[i], 3, 3);
|
||||
x += s;
|
||||
}
|
||||
|
||||
|
||||
popMatrix();
|
||||
|
||||
}
|
||||
|
||||
void data(int d) {
|
||||
values[indx] = d;
|
||||
float i = (float) d / (float)this.max;
|
||||
if(by != h) pixelvalues[indx++] = (int)(i * h / 2);
|
||||
else pixelvalues[indx++] = (int)(i * h);
|
||||
indx %= (w / s) - 1;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
class ScopePlotDouble extends ScopePlot {
|
||||
|
||||
int[] values_second;
|
||||
int[] pixelvalues_second;
|
||||
|
||||
ScopePlotDouble(String t, int w, int h, int x, int y, int maxval, boolean mid){
|
||||
super(t, w, h, x, y, maxval, mid);
|
||||
values_second = new int[values.length];
|
||||
pixelvalues_second = new int[pixelvalues.length];
|
||||
}
|
||||
|
||||
void data(int d0, int d1) {
|
||||
values_second[indx] = d1;
|
||||
float i = (float) d1 / (float)this.max;
|
||||
if(by != h) pixelvalues_second[indx] = (int)(i * h / 2);
|
||||
else pixelvalues_second[indx] = (int)(i * h);
|
||||
super.data(d0);
|
||||
}
|
||||
|
||||
void draw() {
|
||||
|
||||
pushMatrix();
|
||||
|
||||
translate(px, py);
|
||||
|
||||
int x = s;
|
||||
for(int i = 0; i < pixelvalues_second.length; i++) {
|
||||
stroke(255,34,127);
|
||||
line(x, by, x, by - pixelvalues_second[i]);
|
||||
stroke(127,34,255);
|
||||
if(i == indx - 1) {
|
||||
stroke(255);
|
||||
text(Integer.toString(values_second[i]), w + d + d + d, by - pixelvalues_second[i]);
|
||||
}
|
||||
ellipse(x, by - pixelvalues_second[i], 3, 3);
|
||||
x += s;
|
||||
}
|
||||
|
||||
popMatrix();
|
||||
|
||||
super.draw();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
class ScopePlotInteract extends ScopePlot {
|
||||
|
||||
int[] cpoints;
|
||||
int[] cpointsvalues;
|
||||
|
||||
int mx, my;
|
||||
|
||||
ScopePlotInteract(String t, int w, int h, int x, int y, int maxval, boolean mid) {
|
||||
super(t, w, h, x, y, maxval, mid);
|
||||
}
|
||||
|
||||
void draw() {
|
||||
mx = 0;
|
||||
super.draw();
|
||||
}
|
||||
|
||||
void mdata(int mousex, int mousey) {
|
||||
if(mousex < px + w && mousex > px && mousey > py && mousey < py + h) {
|
||||
mx = (mousex - px) / s;
|
||||
my = mousey;
|
||||
if(mx < pixelvalues.length)
|
||||
pixelvalues[mx] = (by + py) - my;
|
||||
// compute real values here
|
||||
}
|
||||
/*
|
||||
else if((mousex > px + w && mousex < px + w + d) || (mousex < px && mousex > px - d) ||
|
||||
(mousey < py && mousey > py - d) || (mousey > py + h && mousey < py + h + d)) {
|
||||
px += (mousex - mx);
|
||||
py += (my - mousey);
|
||||
}
|
||||
mx = mousex;
|
||||
my = mousey;
|
||||
*/
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,82 @@
|
||||
#include <Motion.h>
|
||||
#include <Motor.h>
|
||||
#include <Music.h>
|
||||
|
||||
char buf[16] = "";
|
||||
|
||||
char b = 'x';
|
||||
String inputString = "";
|
||||
boolean stringComplete = false;
|
||||
|
||||
float k, m, d;
|
||||
|
||||
int cnt = 0;
|
||||
|
||||
void setup() {
|
||||
|
||||
// MOTOR
|
||||
MotorA.init();
|
||||
|
||||
// MUSIC
|
||||
Music.init();
|
||||
|
||||
// MOTION
|
||||
MotionA.init(INPUTA0);
|
||||
MotionA.k = 5.2f; // spring
|
||||
MotionA.m = 1.0f; // mass
|
||||
MotionA.d = 8.02f; // damping
|
||||
|
||||
// Serial
|
||||
Serial.begin(9600);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
|
||||
MotionA.update_mass_spring_damper();
|
||||
|
||||
MotorA.torque(MotionA.F);
|
||||
|
||||
int f = map(abs(MotionA.F), 0, 512, 64, 76);
|
||||
Music.noteOn(f);
|
||||
|
||||
cnt++;
|
||||
|
||||
if(cnt == 10) {
|
||||
sprintf(buf, "%d %d %d %d", (int)MotionA.F, (int)MotionA.V, (int)MotionA.X, (int)MotionA.Xin);
|
||||
Serial.println(buf);
|
||||
cnt = 0;
|
||||
}
|
||||
|
||||
if(stringComplete) {
|
||||
if(b == 'k') {
|
||||
MotionA.k = convertToFloat(inputString);
|
||||
} else if(b == 'm') {
|
||||
MotionA.m = convertToFloat(inputString);
|
||||
} else if(b == 'd') {
|
||||
MotionA.d = convertToFloat(inputString);
|
||||
}
|
||||
b = 'x';
|
||||
stringComplete = false;
|
||||
inputString = "";
|
||||
}
|
||||
}
|
||||
|
||||
void serialEvent() {
|
||||
while (Serial.available()) {
|
||||
char inChar = (char)Serial.read();
|
||||
if(inChar == 'k' || inChar == 'm' || inChar == 'd') {
|
||||
b = inChar;
|
||||
} else {
|
||||
if (inChar == ';') {
|
||||
stringComplete = true;
|
||||
} else
|
||||
inputString += inChar;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
float convertToFloat(String str) {
|
||||
char buf[16] = "";
|
||||
str.toCharArray(buf, str.length() + 1);
|
||||
return atof(buf);
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user