Update Motion library and added an example how-to in apps folder

software/lib/MMM/Motion.cpp

@@ -37,19 +37,24 @@ Motion MotionB(MOTIONA);
 uint8_t lb;
 uint8_t hb;
 int     x, xx; 
-float   T = N * 0.004f; // 4ms
+float   T = N * 0.004f;             // 4ms (see TCNT1)
 float   v;
 
+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 = 1000;               //4 ms (TCNT1)
         TIMSK1 = (1 << TOIE1);
         motion_reg_init = true;    
     }
@@ -57,11 +62,6 @@ void Motion::init(INPUT sensor)
     _s = sensor;
 }
 
-void Motion::updatePhysics() 
-{
-    
-}
-
 int Motion::getPosition() {
     return _x;
 }
@@ -74,6 +74,11 @@ float Motion::getAcceleration() {
     return _a;
 }
 
+void Motion::set_force_callback(force_callback fcb, PHY physics) {
+    _fcb = fcb;
+    _fcb_phy = physics;
+}
+
 
 // clocked at 4ms period
 ISR(TIMER1_OVF_vect) {

software/lib/MMM/Motion.h

@@ -25,6 +25,8 @@
 
 #define N 10
 
+typedef float (*force_callback)(float position);
+
 enum MOTION {
     MOTIONA = 0,
     MOTIONB = 1
@@ -41,6 +43,12 @@ enum INPUT {
     INPUTA7 = 0x0111    
 };
 
+enum PHY {
+    POSITION = 0,
+    VELOCITY = 1,
+    ACCELERATION = 2
+};
+
 
 class Motion {
     
@@ -53,12 +61,13 @@ public:
     float getVelocity();
     float getAcceleration();
     
-    void updatePhysics();
+    void set_force_callback(force_callback fcb, PHY physics);
     
     
     // raw position vector
     int _xv[N];
     int _ix;    
+    
     int _x;    
     float _v;
     float _a;
@@ -67,6 +76,8 @@ public:
     INPUT  _s;
     bool   _i;
     
+    force_callback _fcb;
+    PHY            _fcb_phy;
 };
 
 extern Motion MotionA;