New motion lib

software/apps/Motion/Motion0/Motion0.ino

@@ -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);
+  */
 }
 
 

software/apps/Motion/Pendulum/Pendulum.ino

@@ -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);
+    }    
+  }
+}

software/apps/Motion/PendulumMM_2/PendulumMM_2.ino

@@ -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);
-    }
-  }
-}

software/apps/Motor/Motor.ino

@@ -1,27 +1,16 @@
-//#include <TimerOne.h>
-
 #include <Motor.h>
 
 void setup() 
 {
-
-  MotorA.torque(500);  
-  
+  MotorA.init();
+  MotorA.torque(255); 
 }
 
 void loop() 
 {
-  static int j = 1;
-  static long cnt = 0;
-  
-  if(cnt == 50000) {
-    j = -1;
     MotorA.direction(FORWARD);
-  } else if(cnt == 0) {
-    j = 1;
+    delay(1000);
     MotorA.direction(BACKWARD);
-  }
-  cnt += j;  
-  
+    delay(1000);  
 }
   

software/lib/MMM/Motion.cpp

@@ -36,9 +36,9 @@ Motion MotionB(MOTIONA);
 
 uint8_t lb;
 uint8_t hb;
-int     x, xx; 
+ 
 float   T = N * 0.004f;             // 4ms (see TCNT1)
-float   v;
+int     xin, dx;
 
 float MAX_POS = 1023;
 float MAX_VEL = MAX_POS / T;
@@ -60,18 +60,11 @@ void Motion::init(INPUT sensor)
     }
     _i = true;
     _s = sensor;
-}
-
-int Motion::getPosition() {
-    return _x;
-}
-
-float Motion::getVelocity() {
-    return _v;
-}
-
-float Motion::getAcceleration() {
-    return _a;
+    
+    // initial values
+    d = 0;
+    k = 1;
+    m = 1;
 }
 
 void Motion::set_force_callback(force_callback fcb, PHY physics) {
@@ -84,32 +77,23 @@ void Motion::set_force_callback(force_callback fcb, PHY physics) {
 ISR(TIMER1_OVF_vect) {
     TCNT1 = 1000;     
     
-    
-    
     if(MotionA._i) {
         ADMUX = MotionA._s & 0x07;
         ADCSRA |= (1 << ADSC);
         while (ADCSRA & (1 << ADSC));
         lb = ADCL;
         hb = ADCH;
-        x = (hb << 8) | lb;
-        MotionA._xv[MotionA._ix] = x;
-        MotionA._ix++;
-        MotionA._ix %= N;        
         
-        xx = x - MotionA._x;
-        if(abs(xx) < 2) {            
-            v = 0.0f;
-        } else {
-            v = xx / T;
-        }
+        xin = (hb << 8) | lb;
         
-        MotionA._a = (v - MotionA._v) / T;        
-        MotionA._v = v;
-        MotionA._x = x;       
+        MotionA.F = MotionA.k * (xin - MotionA.X) - (MotionA.d * MotionA.V);
+        MotionA.A = MotionA.F / MotionA.m;
+        MotionA.V += MotionA.A * T;
+        MotionA.X += MotionA.V * T;
         
     }
     
+    /*
     if(MotionB._i) {
         ADMUX = MotionB._s & 0x07;
         ADCSRA |= (1 << ADSC);
@@ -134,6 +118,7 @@ ISR(TIMER1_OVF_vect) {
         MotionB._x = x;       
         
     }
+     */
     
 }
 

software/lib/MMM/Motion.h

@@ -57,10 +57,6 @@ public:
     
     void init(INPUT sensor);
     
-    int getPosition();
-    float getVelocity();
-    float getAcceleration();
-    
     void set_force_callback(force_callback fcb, PHY physics);
     
     
@@ -68,9 +64,8 @@ public:
     int _xv[N];
     int _ix;    
     
-    int _x;    
-    float _v;
-    float _a;
+    float X, V, A, F;    
+    float m, k, d;
         
     MOTION _m;
     INPUT  _s;

software/lib/MMM/Motor.cpp

@@ -47,17 +47,18 @@ void MMotor::init()
     if(!reg_init){
         
         //direction pins are outputs
-        DDRD |= (1 << PD7);
-        DDRB |= (1 << PB0);        
+        DDRD |= (1 << PD7);        
+        DDRB = (1 << PB0) | (1 << PB1) | (1 << PB2);
         
-        DDRB |= (1 << PB1) | (1 << PB2);
         TCCR1A = (1 << COM1A1) | (1 << COM1B1);        
         
         // clear the bits
         TCCR1B &= ~((1 << CS10) | (1 << CS11) | (1 << CS12));
         TCCR1B = (1 << WGM13) | (1 << CS10);
         
-        ICR1 = 512;                                                  
+        //ICR1 = 512;    
+        ICR1 = 512;    
+        
         reg_init = true;
     }