From 62b7761e9ab419cd77738bae8e413d06d0492b3f Mon Sep 17 00:00:00 2001 From: dviid Date: Mon, 28 Jan 2013 12:05:23 +0100 Subject: [PATCH] Added Oscilloscope code in apps --- .../apps/Motion/Oscilloscope/Oscilloscope.pde | 111 ++++++++++++ .../apps/Motion/Oscilloscope/Protocol.pde | 24 +++ .../apps/Motion/Oscilloscope/ScopePlot.pde | 161 ++++++++++++++++++ .../OscilloscopeArduino.ino | 82 +++++++++ 4 files changed, 378 insertions(+) create mode 100644 software/apps/Motion/Oscilloscope/Oscilloscope.pde create mode 100644 software/apps/Motion/Oscilloscope/Protocol.pde create mode 100644 software/apps/Motion/Oscilloscope/ScopePlot.pde create mode 100644 software/apps/Motion/OscilloscopeArduino/OscilloscopeArduino.ino diff --git a/software/apps/Motion/Oscilloscope/Oscilloscope.pde b/software/apps/Motion/Oscilloscope/Oscilloscope.pde new file mode 100644 index 0000000..fb8d236 --- /dev/null +++ b/software/apps/Motion/Oscilloscope/Oscilloscope.pde @@ -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()); + } +} + + diff --git a/software/apps/Motion/Oscilloscope/Protocol.pde b/software/apps/Motion/Oscilloscope/Protocol.pde new file mode 100644 index 0000000..cb3a307 --- /dev/null +++ b/software/apps/Motion/Oscilloscope/Protocol.pde @@ -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]); + } + } +} diff --git a/software/apps/Motion/Oscilloscope/ScopePlot.pde b/software/apps/Motion/Oscilloscope/ScopePlot.pde new file mode 100644 index 0000000..263b737 --- /dev/null +++ b/software/apps/Motion/Oscilloscope/ScopePlot.pde @@ -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; + */ + } +} diff --git a/software/apps/Motion/OscilloscopeArduino/OscilloscopeArduino.ino b/software/apps/Motion/OscilloscopeArduino/OscilloscopeArduino.ino new file mode 100644 index 0000000..9ee5f3a --- /dev/null +++ b/software/apps/Motion/OscilloscopeArduino/OscilloscopeArduino.ino @@ -0,0 +1,82 @@ +#include +#include +#include + +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); +}