Bill - Pendulum and PendulumDamped

2013-01-24 16:06:22 +01:00
parent cd4fdfc907
commit f6fcb48859
2 changed files with 88 additions and 5 deletions
@@ -0,0 +1,85 @@
+// Damped Spring-Mass Oscillator
+// BV 24Jan13
+
+#include <Motor.h>
+int Fout, Xin, duty;
+
+// mass sring damper model
+float m = 4.0;
+float k = 1.0;
+float b = 0.0;
+float dT = .005;
+float X, V, F; // model parameters (2nd-order oscil)
+
+byte c=0; //counts up until 0 then prints
+
+byte incomingByte;
+
+void setup(){
+  Serial.begin(9600);
+  MotorA.init(); // initializes Timer1, pinModes
+  Xin = analogRead(A0) - 512; //middle of 0-1023 range
+  // initialize model variables
+  X = Xin;
+  V = 0.0;
+  F = 0.0;
+}
+
+void loop(){
+  Xin = analogRead(A0) - 512;
+  F = k*(Xin - X) - b*V; //spring and damper
+  V += (F/m) * dT;
+  X += V * dT;
+  
+  duty = abs(Fout);
+  duty = min(duty,512);
+  
+  MotorA.torque(duty);
+  if(Fout>0)MotorA.direction(FORWARD);
+  else MotorA.direction(BACKWARD);
+  
+if (c++ == 0){
+  Serial.println(X);
+}
+/*
+if (Serial.available() > 0) {
+    incomingByte = Serial.read();
+    if(incomingByte == '}''){
+      dT = dT * 1.1;
+      Serial.println(dT);
+    }
+    if(incomingByte == '\'){ shift \
+     dT = dT / 1.1;
+     Serial.println(dT);
+    }
+    if(incomingByte == '|'){
+      k = k / 1.1;
+      Serial.println(k);
+    }
+    if(incomingByte == '}'){ //shift ]
+      k = k * 1.1;
+      Serial.println(k);
+    }
+    if(incomingByte == '['){ 
+      b = b / 1.1;
+      Serial.println(b);
+    }
+    if(incomingByte == '{'){ //shift [
+      b = b * 1.1;
+      Serial.println(b);
+    }
+   if(incomingByte == '['){ 
+      m = m / 1.1;
+      Serial.println(m);
+    }
+    if(incomingBmyte == '{'){ //shift [
+      m = m * 1.1;
+      Serial.println(m);
+    }
+    }
+  }
+  */
+}
+  
+  
+