Added Motion lib 101

software/apps/Motion/Graph2/Graph2.ino

@@ -0,0 +1,28 @@
+int x, v, t;  //position velocity time
+#define n 10
+int xtab[n]; //table of x
+int i; // index for table
+
+void setup()
+{
+  // start serial port at 9600 bps:
+  Serial.begin(9600);
+}
+
+void loop()
+{
+  // if we get a valid byte, read analog ins:
+  if (Serial.available() > 0) {
+    x = analogRead(A0)/4; //just for this test
+    xtab[i] = x; //put it in array of x
+    i = i + 1; //index to last x (9 times ago)
+    if(i>9)i=0; //increment index
+    v = x - xtab[i]; //difference x vs old x
+    t = (t+1)%256;  
+    // send sensor values:
+    Serial.write(x); // sends one byte [0-255]
+    Serial.write(v+128);
+    Serial.write(t);              
+  }
+}
+

software/lib/MMM/Motion.cpp

@@ -0,0 +1,134 @@
+/* 
+ 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(MOTIONA);
+
+uint8_t lb;
+uint8_t hb;
+int     x, xx; 
+float   T = N * 0.004f; // 4ms
+float   v;
+
+
+Motion::Motion(MOTION m){
+    _m = m;
+    _i = false;
+};
+
+void Motion::init(INPUT sensor)
+{        
+    if(!motion_reg_init){
+        TCNT1 = 1000; //4 ms 
+        TIMSK1 = (1 << TOIE1);
+        motion_reg_init = true;    
+    }
+    _i = true;
+    _s = sensor;
+}
+
+void Motion::updatePhysics() 
+{
+    
+}
+
+int Motion::getPosition() {
+    return _x;
+}
+
+float Motion::getVelocity() {
+    return _v;
+}
+
+float Motion::getAcceleration() {
+    return _a;
+}
+
+
+// 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;
+        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;
+        }
+        
+        MotionA._a = (v - MotionA._v) / T;        
+        MotionA._v = v;
+        MotionA._x = x;       
+        
+    }
+    
+    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

@@ -0,0 +1,73 @@
+/* 
+ 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
+
+enum MOTION {
+    MOTIONA = 0,
+    MOTIONB = 1
+};
+
+enum INPUT {
+    INPUTA0 = 0x0000,
+    INPUTA1 = 0x0001,
+    INPUTA2 = 0x0010,
+    INPUTA3 = 0x0011,
+    INPUTA4 = 0x0100,
+    INPUTA5 = 0x0101,
+    INPUTA6 = 0x0110,
+    INPUTA7 = 0x0111    
+};
+
+
+class Motion {
+    
+public:
+    Motion(MOTION m);
+    
+    void init(INPUT sensor);
+    
+    int getPosition();
+    float getVelocity();
+    float getAcceleration();
+    
+    void updatePhysics();
+    
+    
+    // raw position vector
+    int _xv[N];
+    int _ix;    
+    int _x;    
+    float _v;
+    float _a;
+        
+    MOTION _m;
+    INPUT  _s;
+    bool   _i;
+    
+};
+
+extern Motion MotionA;
+extern Motion MotionB;

software/lib/MMM/Motor.cpp

@@ -39,70 +39,36 @@ MMotor MotorB(MOTORB); // this is motor B
 MMotor::MMotor(MOTOR m)
 {
     _m = m;
-    init();
 }
 
 void MMotor::init()
 {    
-    if(!reg_init){
         
-        //PWM, Phase and Frequency
+    if(!reg_init){
-        //Correctthese modes are preferred for motor control applications p.130
-        TCCR1A = 0;
-        TCCR1B |= (1 << WGM13);
         
         //direction pins are outputs
         DDRD |= (1 << PD7);
-        DDRB |= (1 << PB0);
+        DDRB |= (1 << PB0);        
         
+        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;                                                  
         reg_init = true;
     }
     
-    //set period for minimal noise            
-    _period(64);
-    
 }
 
-void MMotor::_set_period_bits()
-{
-    // clear prescaler reg
-    TCCR1B &= ~((1 << CS10) | (1 << CS11) | (1 << CS12));
-    
-    if(_p < RESOLUTION)                  TCCR1B |= (1 << CS10);                  // pre-s 0
-    else if((_p >>= 3) < RESOLUTION)     TCCR1B |= (1 << CS11);                  // pre-s 8
-    else if((_p >>= 3) < RESOLUTION)     TCCR1B |= (1 << CS11) | (1 << CS10);    // pre-s 64
-    else if((_p >>= 2) < RESOLUTION)     TCCR1B |= (1 << CS12);                  // pre-s 256
-    else if((_p >>= 2) < RESOLUTION)     TCCR1B |= (1 << CS12) | (1 << CS10);    // pre-s 1024
-    else _p = RESOLUTION - 1,            TCCR1B |= (1 << CS12) | (1 << CS10);    // pre-s 1024
-    
-    ICR1 = _p;
-    
-}
-
-void MMotor::_period(long ms)
-{
-    _p = (F_CPU * ms) / 2000000;
-    _set_period_bits();
-}
 
 void MMotor::torque(int value) 
-{
+{            
-    
+    if(_m == MOTORA) OCR1A = value;
-    if(_m == MOTORA) {
+    else if(_m == MOTORB) OCR1B = value;        
-        DDRB |= (1 << PB1);
+    _t = value;
-        TCCR1A |= (1 << COM1A1);
-    } else if(_m == MOTORB) {
-        DDRB |= (1 << PB2);
-        TCCR1A |= (1 << COM1B1);        
-    }
-    
-    unsigned long duty = _p * value;
-    duty >>= 10;
-    if(_m == MOTORA) OCR1A = duty;
-    else if(_m == MOTORB) OCR1B = duty;    
-    _t = value;    
-    
-    start();
 }
 
 int MMotor::torque()
@@ -126,7 +92,7 @@ void MMotor::stop()
 
 void MMotor::start()
 {
-    _set_period_bits();
+    TCCR1B = (1 << WGM13) | (1 << CS10);
 }
 
 void MMotor::restart()

software/lib/MMM/Motor.h

@@ -21,8 +21,6 @@
  + contact: dviid@labs.ciid.dk 
  */
 
-#include <avr/io.h>
-
 enum MOTOR{
     MOTORA = 0,
     MOTORB = 1
@@ -35,7 +33,7 @@ enum DIRECTION {
 
 class MMotor {    
 
-public:
+public:       
     
     MMotor(MOTOR m);
     
@@ -51,10 +49,7 @@ public:
     void start();
     void restart();
     
-private:
+protected:
-    
-    void _period(long ms);
-    void _set_period_bits();
     
     MOTOR       _m;
     DIRECTION   _d;