From cd4fdfc9073277c90c40bc2404955c96a129b1de Mon Sep 17 00:00:00 2001
From: Bill Verplank <billver@bill-verplanks-macbook-2.local>
Date: Thu, 24 Jan 2013 12:48:04 +0100
Subject: [PATCH] Bills Pendulum Works!

---
 .../PendulumBillsLatest.ino                   | 70 +++++++++++++++++++
 1 file changed, 70 insertions(+)
 create mode 100644 software/apps/Motion/PendulumBillsLatest/PendulumBillsLatest.ino

diff --git a/software/apps/Motion/PendulumBillsLatest/PendulumBillsLatest.ino b/software/apps/Motion/PendulumBillsLatest/PendulumBillsLatest.ino
new file mode 100644
index 0000000..0dafa11
--- /dev/null
+++ b/software/apps/Motion/PendulumBillsLatest/PendulumBillsLatest.ino
@@ -0,0 +1,70 @@
+// "Pendulum" - spring-mass oscillator
+//
+
+//#include <TimerOne.h>
+#include <Motor.h>
+float dt = .005; //time per cycle
+float m = 4.0; //time constant was .002
+
+float k = 1.0;  // spring constant was .15
+float xf,vf,ff; // floating point versions of pendulum x and v and force
+int x; // input from analogRead();
+int f; // integer version of force
+int duty; // abs(f)
+byte c=0;  //counts up until 0 then prints
+
+byte incomingByte;
+
+
+void setup() 
+{
+  //Timer1.initialize(64); //64 microsecond period
+  MotorA.init(); //initializes Timer1, pinModes
+  Serial.begin(9600);
+  //initialize position (xf), velocity (vf), force (ff)
+  x = analogRead(A0) - 512; //middle of 0-1-23 range
+  xf = x;
+  vf = 0.0;
+  ff = 0.0;
+  
+}
+
+void loop(){
+  x = analogRead(0) - 512; // position [0-1024]
+  ff = k*(x - xf); // spring
+  vf = vf + (ff/m)*dt; // integrate F/m to get velocity
+  xf = xf + vf*dt; // integrate velocity to get position
+  
+  f = min(ff,512); //Motor has max torque of 512
+  duty = abs(f);
+  
+  
+  MotorA.torque(duty);
+  if(ff>0)MotorA.direction(FORWARD);
+  else MotorA.direction(BACKWARD);
+  
+if (c++ == 0){
+  Serial.println(xf);
+}
+/*
+if (Serial.available() > 0) {
+    incomingByte = Serial.read();
+    if(incomingByte == '\''){
+      tf = tf * 1.1;
+      Serial.println(tf);
+    }
+    if(incomingByte == ';'){
+     tf = tf / 1.1;
+     Serial.println(tf);
+    }
+    if(incomingByte == '.'){
+      kf = kf / 1.1;
+      Serial.println(tf);
+    }
+    if(incomingByte == '/'){
+      kf = kf * 1.1;
+      Serial.println(tf);
+    }
+  }
+  */
+}