From fc8f35306497394916ed1eecbce6eead857b93b8 Mon Sep 17 00:00:00 2001
From: Bill Verplank <billver@bill-verplanks-macbook-2.local>
Date: Thu, 24 Jan 2013 16:52:11 +0100
Subject: [PATCH] Bill's Protocol Pendulum Plot

Added force-out to pendulum
---
 .../ProtocolBVmotor/ProtocolBVmotor.ino       | 69 +++++++++++++++++++
 1 file changed, 69 insertions(+)
 create mode 100644 software/apps/Motion/ProtocolBVmotor/ProtocolBVmotor.ino

diff --git a/software/apps/Motion/ProtocolBVmotor/ProtocolBVmotor.ino b/software/apps/Motion/ProtocolBVmotor/ProtocolBVmotor.ino
new file mode 100644
index 0000000..49e9354
--- /dev/null
+++ b/software/apps/Motion/ProtocolBVmotor/ProtocolBVmotor.ino
@@ -0,0 +1,69 @@
+#include <Motion.h>
+#include <Motor.h>
+
+char buf[16] = "";
+
+char b = 'x';
+String  inputString = "";
+boolean stringComplete = false;
+
+float k, m, d;
+int duty;
+
+void setup() {
+  MotionA.init(INPUTA0);
+
+  MotionA.k = 0.2f;   // spring
+  MotionA.m = 1.0f;   // mass
+  MotionA.d = 0.02f;  // damping
+
+  Serial.begin(9600);  
+}
+
+void loop() {
+  //send force out
+  if (MotionA.F > 0) MotorA.direction(FORWARD);
+  else MotorA.direction(BACKWARD);
+  duty = abs(MotionA.F);
+  duty = min(512,duty);
+  MotorA.torque(duty);
+  
+  //send data to be plotted by Processing "plotter"
+  sprintf(buf, "%d %d %d %d", (int)MotionA.F, (int)MotionA.A, (int)MotionA.V, (int)MotionA.X);
+  Serial.println(buf);  
+  
+  delay(10);  
+  
+  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);  
+}