Updates: Motion + Motor

Integration with unsyncable branch

software/lib/MMM/Motion.cpp.old

@@ -1,126 +0,0 @@
-/* 
- Motion.cpp - Motion library
- Copyright (c) 2012 Copenhagen Institute of Interaction Design. 
- All right reserved.
- 
- This library is free software: you can redistribute it and/or modify
- it under the terms of the GNU Lesser Public License as published by
- the Free Software Foundation, either version 3 of the License, or
- (at your option) any later version.
- 
- This library is distributed in the hope that it will be useful,
- but WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- GNU Lesser Public License for more details.
- 
- You should have received a copy of the GNU Lesser Public License
- along with Foobar.  If not, see <http://www.gnu.org/licenses/>.
- 
- +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- + author: dviid
- + contact: dviid@labs.ciid.dk 
- */
-
-#include "Motion.h"
-
-#include <avr/io.h>
-#include <avr/interrupt.h>
-#include <stdlib.h>
-
-#define PB0 PORTB0
-
-bool motion_reg_init = false;
-
-Motion MotionA(MOTIONA);
-Motion MotionB(MOTIONB);
-
-uint8_t lb;
-uint8_t hb;
- 
-float   T = N * 0.004f;             // 4ms (see TCNT1)
-int     xin, dx;
-
-float MAX_POS = 1023;
-float MAX_VEL = MAX_POS / T;
-float MAX_ACC = MAX_VEL / T;
-
-
-Motion::Motion(MOTION m){
-    _m = m;
-    _i = false;
-    _fcb = NULL;
-};
-
-void Motion::init(INPUT sensor)
-{        
-    if(!motion_reg_init){
-        TCNT1 = 1000;               //4 ms (TCNT1)
-        TIMSK1 = (1 << TOIE1);
-        motion_reg_init = true;    
-    }
-    _i = true;
-    _s = sensor;
-    
-    // initial values
-    d = 0;
-    k = 1;
-    m = 1;
-}
-
-void Motion::set_force_callback(force_callback fcb, PHY physics) {
-    _fcb = fcb;
-    _fcb_phy = physics;
-}
-
-
-// clocked at 4ms period
-ISR(TIMER1_OVF_vect) {
-    TCNT1 = 1000;     
-    
-    if(MotionA._i) {
-        ADMUX = MotionA._s & 0x07;
-        ADCSRA |= (1 << ADSC);
-        while (ADCSRA & (1 << ADSC));
-        lb = ADCL;
-        hb = ADCH;
-        
-        xin = (hb << 8) | lb;
-        MotionA.Xin =  xin;
-        
-        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);
-        while (ADCSRA & (1 << ADSC));
-        lb = ADCL;
-        hb = ADCH;
-        x = (hb << 8) | lb;
-	 
-        MotionB._xv[MotionB._ix] = x;
-        MotionB._ix++;
-        MotionB._ix %= N;    
-        
-        
-        xx = x - MotionB._x;
-        if(abs(xx) < 2) {            
-            v = 0;
-        } else {
-            v = xx / T;
-        }
-        
-        MotionB._a = (v - MotionB._v) / T;        
-        MotionB._v = v;
-        MotionB._x = x;       
-        
-    }
-     */
-    
-}
-

software/lib/MMM/Motion.h

@@ -32,21 +32,15 @@ enum MOTION {
     MOTIONB = 1
 };
 
-enum INPUT {
-    INPUTA0 = 0x00, // bits 0000
-    INPUTA1 = 0x01, // bits 0001
-    INPUTA2 = 0x02, // bits 0010
-    INPUTA3 = 0x03, // bits 0011
-    INPUTA4 = 0x04, // bits 0100
-    INPUTA5 = 0x05, // bits 0101
-    INPUTA6 = 0x06, // bits 0110
-    INPUTA7 = 0x07	// bits 0111
-};
-
-enum PHY {
-    POSITION = 0,
-    VELOCITY = 1,
-    ACCELERATION = 2
+enum SENSOR_INPUT {
+    INPUTA0 = 0x0000,
+    INPUTA1 = 0x0001,
+    INPUTA2 = 0x0010,
+    INPUTA3 = 0x0011,
+    INPUTA4 = 0x0100,
+    INPUTA5 = 0x0101,
+    INPUTA6 = 0x0110,
+    INPUTA7 = 0x0111    
 };
 
 
@@ -55,30 +49,25 @@ class Motion {
 public:
     Motion(MOTION m);
     
-    void init(INPUT sensor);
-    
-    void set_force_callback(force_callback fcb, PHY physics);  // NOT IMPLEMENTED
-	
-	int getX();
-	
-	float calculateFAVX(int);
-    
+    void init(SENSOR_INPUT sensor);
     
+    void update_mass_spring_damper();
+        
     // raw position vector
-    int _xv[N];	// NOT IMPLEMENTED
+    int _xv[N];
     int _ix;    
     
-    float X, V, A, F;    // from model
-    float m, k, d;		// constants from mass-spring-damper model
-    int Xin;            // from the ADC channel specified in Motion constructor (e.g INPUTA0)
-
-
-    MOTION _m;
-    INPUT  _s;
-    bool   _i;	// is it inititated
+    float X, V, F;    
+    float m, k, d;
+    int Xin;
     
-    force_callback _fcb;	// NOT IMPLEMENTED
-    PHY            _fcb_phy; // NOT IMPLEMENTED
+    long tick;
+        
+    MOTION _m;
+    SENSOR_INPUT  _s;
+    bool   _i;
+    
+    force_callback _fcb; // not used in current version
 };
 
 extern Motion MotionA;

software/lib/MMM/Motion.h.old

@@ -1,81 +0,0 @@
-/* 
- Motion.h - Motion library
- Copyright (c) 2012 Copenhagen Institute of Interaction Design. 
- All right reserved.
- 
- This library is free software: you can redistribute it and/or modify
- it under the terms of the GNU Lesser Public License as published by
- the Free Software Foundation, either version 3 of the License, or
- (at your option) any later version.
- 
- This library is distributed in the hope that it will be useful,
- but WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- GNU Lesser Public License for more details.
- 
- You should have received a copy of the GNU Lesser Public License
- along with Foobar.  If not, see <http://www.gnu.org/licenses/>.
- 
- +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- + author: dviid
- + contact: dviid@labs.ciid.dk 
- */
-
-#include <avr/io.h>
-
-#define N 10
-
-typedef float (*force_callback)(float position);
-
-enum MOTION {
-    MOTIONA = 0,
-    MOTIONB = 1
-};
-
-enum INPUT {
-    INPUTA0 = 0x0000,
-    INPUTA1 = 0x0001,
-    INPUTA2 = 0x0010,
-    INPUTA3 = 0x0011,
-    INPUTA4 = 0x0100,
-    INPUTA5 = 0x0101,
-    INPUTA6 = 0x0110,
-    INPUTA7 = 0x0111    
-};
-
-enum PHY {
-    POSITION = 0,
-    VELOCITY = 1,
-    ACCELERATION = 2
-};
-
-
-class Motion {
-    
-public:
-    Motion(MOTION m);
-    
-    void init(INPUT sensor);
-    
-    void set_force_callback(force_callback fcb, PHY physics);  // NOT IMPLEMENTED
-    
-    
-    // raw position vector
-    int _xv[N];	// NOT IMPLEMENTED
-    int _ix;    
-    
-    float X, V, A, F;    // from model
-    float m, k, d;		// constants from mass-spring-damper model
-    int Xin;            // from the ADC channel specified in Motion constructor (e.g INPUTA0)
-
-private:
-    MOTION _m;
-    INPUT  _s;
-    bool   _i;	// is it inititated
-    
-    force_callback _fcb;	// NOT IMPLEMENTED
-    PHY            _fcb_phy; // NOT IMPLEMENTED
-};
-
-extern Motion MotionA;
-extern Motion MotionB;