New motion lib

2013-01-23 16:55:40 +01:00
parent 0a0e857cea
commit afd4b230b2
7 changed files with 104 additions and 135 deletions
@@ -1,4 +1,5 @@
 #include <Motion.h>
+#include <Motor.h>

 /*

@@ -41,15 +42,30 @@ float getAcceleration();

 void setup() {
  MotionA.init(INPUTA0);
+  MotorA.init();
  Serial.begin(9600);
+  
+  MotionA.k = 0.2f;
+  MotionA.m = 0.3f;
+  MotionA.d = 0.02f;
+  
 }

 void loop() {
+  
+  if(MotionA.F < 0) MotorA.direction(FORWARD);
+  else MotorA.direction(BACKWARD);
+  
+  float t = abs(MotionA.F);
+  MotorA.torque(t);
+  
+  /*
  Serial.print("position: "); Serial.println(MotionA.getPosition());
  Serial.print("velocity: "); Serial.println(MotionA.getVelocity());
  Serial.print("accel: "); Serial.println(MotionA.getAcceleration());
  Serial.println("-------");
  delay(100);
+  */
 }


@@ -0,0 +1,63 @@
+// "Pendulum" - spring-mass oscillator
+#include <Motion.h>
+#include <Motor.h>
+
+byte incomingByte;
+
+void setup() 
+{
+  // init MotionA & MotorA
+  MotionA.init(INPUTA0);
+  MotorA.init();
+
+  // provide MotionA with initial physics constants
+  MotionA.k = 0.2f;   // spring
+  MotionA.m = 0.3f;   // mass
+  MotionA.d = 0.02f;  // damping
+  
+  Serial.begin(9600);
+}
+
+void loop(){
+  
+  if(MotionA.F < 0) MotorA.direction(FORWARD);
+  else MotorA.direction(BACKWARD);
+  
+  float t = abs(MotionA.F);
+  //if(t > 512) t = 512;
+  //MotorA.torque(t);
+
+  if (Serial.available() > 0) {
+    incomingByte = Serial.read();
+    if(incomingByte == '1'){
+      MotionA.m = MotionA.m * 1.1;
+      Serial.print("m: ");
+      Serial.println(MotionA.m);
+    }
+    else if(incomingByte == '!'){      
+      MotionA.m = MotionA.m / 1.1;
+      Serial.print("m: ");
+      Serial.println(MotionA.m);
+    }    
+    else if(incomingByte == '2'){
+     MotionA.k = MotionA.k * 1.1;
+     Serial.print("k: ");
+     Serial.println(MotionA.k);
+    }
+    else if(incomingByte == '@'){
+     MotionA.k = MotionA.k / 1.1;
+     Serial.print("k: ");
+     Serial.println(MotionA.k);
+    }    
+    else if(incomingByte == '3'){
+      MotionA.d = MotionA.d * 1.1;
+      Serial.print("d: ");
+      Serial.println(MotionA.d);
+    }
+    else if(incomingByte == '#'){
+      MotionA.d = MotionA.d / 1.1;
+      Serial.print("d: ");
+      Serial.println(MotionA.d);
+    }    
+  }
+}
@@ -1,80 +0,0 @@
-// "Pendulum" - spring-mass oscillator
-//
-
-#include <TimerOne.h>
-#include <Motor.h>
-float tf = 0.002; //time constant was .002
-float kf = 0.2;  // spring constant was .15
-float xf,vf,ff; // floating point versions of pendulum x and v and force
-int x, f;  //int versions of position (from a/d) and force (to Pwm)
-int duty; // abs(f)
-byte c=0;  //counts up until 0 then prints
-
-float m = 0.3;
-float k = 0.01;
-
-float damp = 0.09;
-
-byte incomingByte;
-
-
-
-void setup() 
-{
-  //Timer1.initialize(64); //64 microsecond period
-  MotorA.torque(100); //initializes Timer1, pinModes
-  Serial.begin(9600);
-}
-
-void loop(){
-  x = analogRead(0); // position [0-1024]
-  ff = kf*(x - xf); // spring
-  f = max(ff,-1024);
-  f = min(f,1024);
- // if(f>0)digitalWrite(DIRA,HIGH);
- // else digitalWrite(DIRA,LOW);
-  duty = abs(f);
- // Timer1.pwm(9,duty);
-  MotorA.torque(duty);
-  if(f>0)MotorA.direction(FORWARD);
-  else MotorA.direction(BACKWARD);
-  
-  // update (integrate) floating versions of xf and vf
-  //integrate twice tf is deltaT/mass;
-  vf += ff*tf;
-  xf += vf*tf;
-  
-  if(c++==0) // when c gets to 255 it next == 0 and send data
-  {
-    Serial.print(x);
-    Serial.print(" ");
-    Serial.print(xf);
-    Serial.print(" ");
-    Serial.println(ff);
-  }
-  
-if (c++ == 0)Serial.println(x); //
-  if (Serial.available() > 0) {
-    //Serial.print(v);
-    //Serial.print(" ");
-    //Serial.println(x);
-    incomingByte = Serial.read();
-    //Serial.write(incomingByte);
-    if(incomingByte == '\''){
-      m = m * 1.1;
-      Serial.println(m);
-    }
-    if(incomingByte == ';'){
-     m = m / 1.1;
-     Serial.println(m);
-    }
-    if(incomingByte == '.'){
-      k = k / 1.1;
-      Serial.println(k);
-    }
-    if(incomingByte == '/'){
-      k = k * 1.1;
-      Serial.println(k);
-    }
-  }
-}