Thursday Commit

including David's lib from yesterday

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,24 @@ 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.Xin =  xin;
         
-        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.F / MotionA.m) * T;
+        MotionA.X += MotionA.V * T;
         
     }
     
+    /*
     if(MotionB._i) {
         ADMUX = MotionB._s & 0x07;
         ADCSRA |= (1 << ADSC);
@@ -134,6 +119,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,9 @@ public:
     int _xv[N];
     int _ix;    
     
-    int _x;    
-    float _v;
-    float _a;
+    float X, V, A, F;    // from model
+    float m, k, d;    
+    int Xin;            // from the ADC channel specified in Motion constructor (e.g INPUTA0)
         
     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;
     }