From f0556802f5ea80f2e5e163e495e547b66ea471a8 Mon Sep 17 00:00:00 2001
From: dviid <dviid@ciid.dk>
Date: Wed, 30 Jan 2013 18:58:19 +0100
Subject: [PATCH] Fixed Motion

---
 software/lib/MMM/Motion.cpp | 117 +++++++-----------------------------
 software/lib/MMM/Motion.h   |   2 -
 2 files changed, 21 insertions(+), 98 deletions(-)

diff --git a/software/lib/MMM/Motion.cpp b/software/lib/MMM/Motion.cpp
index 4e56ea9..f10dd6f 100644
--- a/software/lib/MMM/Motion.cpp
+++ b/software/lib/MMM/Motion.cpp
@@ -23,121 +23,46 @@
 
 #include "Motion.h"
 
+#include "Arduino.h"
+
 #include <avr/io.h>
 #include <avr/interrupt.h>
 #include <stdlib.h>
 
-#define PB0 PORTB0
-
-bool motion_reg_init = false;
+//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;
-
+int MAX_POS = 1023;
 
 Motion::Motion(MOTION m){
     _m = m;
     _i = false;
     _fcb = NULL;
-};
+}
 
 void Motion::init(SENSOR_INPUT sensor)
 {        
-    if(!motion_reg_init){
-        //TCNT1 = 500;               //4 ms (TCNT1)
-        TIMSK1 = (1 << TOIE1);
-        motion_reg_init = true;    
-    }
     _i = true;
-    _s = sensor;
+    _s = sensor;    
     
-    // initial values
-    d = 0;
-    k = 1;
-    m = 1;
+    tick = millis();
 }
 
-/*
-void Motion::set_force_callback(force_callback fcb, PHY physics) {
-    _fcb = fcb;
-    _fcb_phy = physics;
-}
-*/
-
-/*
-float Motion::calculateFAVX(int x_in)
-{
-	F = k * (x_in - X) - (d * V);
-	//MotionA.A = MotionA.F / MotionA.m;
-	V += (F / m) * T;
-	X += V * T;
-	return F;
-}
-*/
-
-int Motion::getX()
-{
-	return Xin - 512;
-}
-
-// clocked at 4ms period
-ISR(TIMER1_OVF_vect) {
-    //TCNT1 = 500;     
+void Motion::update_mass_spring_damper() {        
     
-    if(MotionA._i) {
-        ADMUX = MotionA._s & 0x07; //setting the lower four bits of ADMUX to INPUT bits and upper four bits to 0000.
-        ADCSRA |= (1 << ADSC);
-        while (ADCSRA & (1 << ADSC));
-        lb = ADCL;
-        hb = ADCH;
-        
-        xin = (hb << 8) | lb;
-        MotionA.Xin =  xin;
-                
-    }
- 
+    // todo: filtering
+    
+    long t = millis();
+    float dt = (float)(t - tick) / 100.0f;
+    
+    int xin = analogRead(_s); // may take some time
+    
+    Xin = xin;
+    F = k * (xin - X) - (d * V);
+    V += (F / m) * dt;
+    X += V * dt;       
+    
+    tick = 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;       
-        
-    }
-     */
-    
diff --git a/software/lib/MMM/Motion.h b/software/lib/MMM/Motion.h
index c4917f2..70b84fe 100644
--- a/software/lib/MMM/Motion.h
+++ b/software/lib/MMM/Motion.h
@@ -52,8 +52,6 @@ public:
     void init(SENSOR_INPUT sensor);
     
     void update_mass_spring_damper();
-	
-	int getX();
         
     // raw position vector
     int _xv[N];