diff --git a/software/lib/MMM/Motion.cpp.old b/software/lib/MMM/Motion.cpp.old
deleted file mode 100644
index e2eeaef..0000000
--- a/software/lib/MMM/Motion.cpp.old
+++ /dev/null
@@ -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 .
-
- +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- + author: dviid
- + contact: dviid@labs.ciid.dk
- */
-
-#include "Motion.h"
-
-#include
-#include
-#include
-
-#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;
-
- }
- */
-
-}
-
diff --git a/software/lib/MMM/Motion.h b/software/lib/MMM/Motion.h
index e73a43c..70b84fe 100644
--- a/software/lib/MMM/Motion.h
+++ b/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;
diff --git a/software/lib/MMM/Motion.h.old b/software/lib/MMM/Motion.h.old
deleted file mode 100644
index 05119f4..0000000
--- a/software/lib/MMM/Motion.h.old
+++ /dev/null
@@ -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 .
-
- +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- + author: dviid
- + contact: dviid@labs.ciid.dk
- */
-
-#include
-
-#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;
\ No newline at end of file
diff --git a/software/lib/MMM/Motor.cpp b/software/lib/MMM/Motor.cpp
old mode 100644
new mode 100755
diff --git a/software/lib/MMM/Motor.h b/software/lib/MMM/Motor.h
old mode 100644
new mode 100755