Changes to Motion, Motor and Music Library

Motion library still needs to be fixed. Don't use yet.

software/apps/Motion/Center/Center.ino

@@ -4,7 +4,8 @@
 //reverse the motor leads
 //or for a quick fix in the code: change if(f < 0) to if (f > 0)
 
-#include <Motor.h>
+#include "Motor.h"
+#include "Music.h"
 
 int pos;    // position from analogRead
 int force;  // computed from pos and k
@@ -13,8 +14,8 @@ int duty;   // pwm duty for Timer1 (range 0 - 1023) 10-bit resolution
  
 void setup() 
 {
-  
   MotorA.init();
+  Music.init();  
   
 }
 
@@ -23,14 +24,16 @@ void loop()
 
   pos = analogRead(A0);
   
+  Music.setFrequency(pos);
+  
   force = k * (512 - pos);
-  duty = abs(force);
+  //duty = abs(force);
-  duty = min(512, duty);
+  //duty = min(512, duty);
   
-  MotorA.torque(duty);
+  MotorA.torque(force);
   
-  if(force < 0) MotorA.direction(FORWARD);
+  //if(force < 0) MotorA.direction(FORWARD);
-  else MotorA.direction(BACKWARD);
+  //else MotorA.direction(BACKWARD);
     
 }
   

software/lib/MMM/Motion.cpp

@@ -32,7 +32,7 @@
 bool motion_reg_init = false;
 
 Motion MotionA(MOTIONA);
-Motion MotionB(MOTIONA);
+Motion MotionB(MOTIONB);
 
 uint8_t lb;
 uint8_t hb;
@@ -54,7 +54,7 @@ Motion::Motion(MOTION m){
 void Motion::init(INPUT sensor)
 {        
     if(!motion_reg_init){
-        TCNT1 = 1000;               //4 ms (TCNT1)
+        //TCNT1 = 500;               //4 ms (TCNT1)
         TIMSK1 = (1 << TOIE1);
         motion_reg_init = true;    
     }
@@ -73,12 +73,28 @@ void Motion::set_force_callback(force_callback 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 = 1000;     
+    //TCNT1 = 500;     
     
     if(MotionA._i) {
-        ADMUX = MotionA._s & 0x07;
+        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;
@@ -87,13 +103,15 @@ ISR(TIMER1_OVF_vect) {
         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;
@@ -102,6 +120,7 @@ ISR(TIMER1_OVF_vect) {
         lb = ADCL;
         hb = ADCH;
         x = (hb << 8) | lb;
+	 
         MotionB._xv[MotionB._ix] = x;
         MotionB._ix++;
         MotionB._ix %= N;    
@@ -121,5 +140,3 @@ ISR(TIMER1_OVF_vect) {
     }
      */
     
-}
-

software/lib/MMM/Motion.cpp.old

@@ -0,0 +1,126 @@
+/* 
+ 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

@@ -33,14 +33,14 @@ enum MOTION {
 };
 
 enum INPUT {
-    INPUTA0 = 0x0000,
+    INPUTA0 = 0x00, // bits 0000
-    INPUTA1 = 0x0001,
+    INPUTA1 = 0x01, // bits 0001
-    INPUTA2 = 0x0010,
+    INPUTA2 = 0x02, // bits 0010
-    INPUTA3 = 0x0011,
+    INPUTA3 = 0x03, // bits 0011
-    INPUTA4 = 0x0100,
+    INPUTA4 = 0x04, // bits 0100
-    INPUTA5 = 0x0101,
+    INPUTA5 = 0x05, // bits 0101
-    INPUTA6 = 0x0110,
+    INPUTA6 = 0x06, // bits 0110
-    INPUTA7 = 0x0111    
+    INPUTA7 = 0x07	// bits 0111
 };
 
 enum PHY {
@@ -57,23 +57,28 @@ public:
     
     void init(INPUT sensor);
     
-    void set_force_callback(force_callback fcb, PHY physics);
+    void set_force_callback(force_callback fcb, PHY physics);  // NOT IMPLEMENTED
+	
+	int getX();
+	
+	float calculateFAVX(int);
     
     
     // raw position vector
-    int _xv[N];
+    int _xv[N];	// NOT IMPLEMENTED
     int _ix;    
     
     float X, V, A, F;    // from model
-    float m, k, d;    
+    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;
+    bool   _i;	// is it inititated
     
-    force_callback _fcb;
+    force_callback _fcb;	// NOT IMPLEMENTED
-    PHY            _fcb_phy;
+    PHY            _fcb_phy; // NOT IMPLEMENTED
 };
 
 extern Motion MotionA;

software/lib/MMM/Motion.h.old

@@ -0,0 +1,81 @@
+/* 
+ 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;

software/lib/MMM/Motor.cpp

@@ -23,6 +23,8 @@
 
 #include "Motor.h"
 #include <avr/io.h>
+#include <stdlib.h>
+#include <avr/interrupt.h>
 
 #define RESOLUTION 65536
 
@@ -46,20 +48,24 @@ void MMotor::init()
         
     if(!reg_init){
 
+        //cli();
         //direction pins are outputs
         DDRD |= (1 << PD7);        
-        DDRB = (1 << PB0) | (1 << PB1) | (1 << PB2);
+        DDRB |= (1 << PB0) | (1 << PB1) | (1 << PB2);
         
         TCCR1A = (1 << COM1A1) | (1 << COM1B1);        
+        //TCCR1A = (1 << COM1A1) | (1 << COM1B1) | (1 << WGM10);        
         
         // clear the bits
-        TCCR1B &= ~((1 << CS10) | (1 << CS11) | (1 << CS12));
+        //TCCR1B &= ~((1 << CS10) | (1 << CS11) | (1 << CS12));
+		
         TCCR1B = (1 << WGM13) | (1 << CS10);
         
-        //ICR1 = 512;    
         ICR1 = 512;    
+        //OCR1A = 512;    
         
         reg_init = true;
+		//sei();
     }
     
 }
@@ -67,8 +73,22 @@ void MMotor::init()
 
 void MMotor::torque(int value) 
 {	
-    if(_m == MOTORA) OCR1A = value;
+	if(value < 0) {
-    else if(_m == MOTORB) OCR1B = value;        
+		_d = BACKWARD;
+	} else {
+		_d = FORWARD;
+	}
+	value = abs(value);
+	if(value > 512) value = 512;
+	//else value = abs(512);
+    if(_m == MOTORA) {
+		OCR1A = value;
+		direction(_d);
+	}
+    else if(_m == MOTORB) {
+		OCR1B = value;
+		direction(_d);
+	}
     _t = value;
 }
 

software/lib/MMM/Motor.h

@@ -53,8 +53,7 @@ protected:
     
     MOTOR       _m;
     DIRECTION   _d;
-    int         _t;  
+    int         _t;	// torque
-    long        _p;
     
 };
 

software/lib/MMM/Music.cpp

@@ -66,7 +66,7 @@ ISR(TIMER2_COMPA_vect) { // timer 2 is audio interrupt timer
 void MMusic::init()
 {
 	// clear interrupts. to make sure the interrupt timer doesn't start until we've set it up.
-	cli();
+	//cli();
 	
 	// set up syntheziser
 	// this is the timer 2 audio rate timer, fires an interrupt at 15625 Hz sampling rate
@@ -77,9 +77,9 @@ void MMusic::init()
 	
 	// OUTPUTS
 	// sck + mosi + ss
-	DDRB = (1 << DDB2) | (1 << DDB3) | (1 << DDB5);
+	DDRB |= (1 << DDB2) | (1 << DDB3) | (1 << DDB5);
 	// dac_cs output
-	DDRD = (1 << DDD6);
+	DDRD |= (1 << DDD6) | (1 << DDB3);
 		
 	// set up SPI port
 	SPCR = 0x50;
@@ -116,7 +116,7 @@ void MMusic::init()
 	setRelease(64);
 	setVelSustain(0);
 	
-	sei(); // global interrupt enable 
+	//sei(); // global interrupt enable 
 	
 	Serial.println("MUSIC INITIALIZED!");
 }
@@ -522,6 +522,8 @@ void MMusic::setVelPeak(uint8_t vel)
 
 void inline MMusic::synthInterrupt8bit()
 {
+	PORTD &= ~(1<<3);
+	
 	// Frame sync low for SPI (making it low here so that we can measure lenght of interrupt with scope)
 	PORTD &= ~(1<<6);
 
@@ -728,6 +730,10 @@ void MMusic::synthInterrupt12bitSine()
 	// Frame sync high
 	PORTD |= (1<<6);
 	
+	// Frame sync high
+	PORTD |= (1<<3);
+
+
 }