diff --git a/software/apps/Motion/Center/Center.ino b/software/apps/Motion/Center/Center.ino index 791dc2e..e50fc80 100644 --- a/software/apps/Motion/Center/Center.ino +++ b/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 +#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 = min(512, duty); + //duty = abs(force); + //duty = min(512, duty); - MotorA.torque(duty); + MotorA.torque(force); - if(force < 0) MotorA.direction(FORWARD); - else MotorA.direction(BACKWARD); + //if(force < 0) MotorA.direction(FORWARD); + //else MotorA.direction(BACKWARD); } diff --git a/software/lib/MMM/Motion.cpp b/software/lib/MMM/Motion.cpp index ed0c547..4c93857 100644 --- a/software/lib/MMM/Motion.cpp +++ b/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; @@ -86,14 +102,16 @@ 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) { } */ -} - diff --git a/software/lib/MMM/Motion.cpp.old b/software/lib/MMM/Motion.cpp.old new file mode 100644 index 0000000..e2eeaef --- /dev/null +++ b/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 . + + +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ + + 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 d25da4c..e73a43c 100644 --- a/software/lib/MMM/Motion.h +++ b/software/lib/MMM/Motion.h @@ -33,14 +33,14 @@ enum MOTION { }; enum INPUT { - INPUTA0 = 0x0000, - INPUTA1 = 0x0001, - INPUTA2 = 0x0010, - INPUTA3 = 0x0011, - INPUTA4 = 0x0100, - INPUTA5 = 0x0101, - INPUTA6 = 0x0110, - INPUTA7 = 0x0111 + 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 { @@ -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; - PHY _fcb_phy; + force_callback _fcb; // NOT IMPLEMENTED + PHY _fcb_phy; // NOT IMPLEMENTED }; extern Motion MotionA; diff --git a/software/lib/MMM/Motion.h.old b/software/lib/MMM/Motion.h.old new file mode 100644 index 0000000..05119f4 --- /dev/null +++ b/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 . + + +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ + + 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 index dce2ea5..59c7ac9 100644 --- a/software/lib/MMM/Motor.cpp +++ b/software/lib/MMM/Motor.cpp @@ -23,6 +23,8 @@ #include "Motor.h" #include +#include +#include #define RESOLUTION 65536 @@ -45,30 +47,48 @@ 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(); } } void MMotor::torque(int value) -{ - if(_m == MOTORA) OCR1A = value; - else if(_m == MOTORB) OCR1B = value; +{ + if(value < 0) { + _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; } diff --git a/software/lib/MMM/Motor.h b/software/lib/MMM/Motor.h index 1d26962..835fab9 100644 --- a/software/lib/MMM/Motor.h +++ b/software/lib/MMM/Motor.h @@ -53,8 +53,7 @@ protected: MOTOR _m; DIRECTION _d; - int _t; - long _p; + int _t; // torque }; diff --git a/software/lib/MMM/Music.cpp b/software/lib/MMM/Music.cpp index e75debd..edb0b02 100644 --- a/software/lib/MMM/Music.cpp +++ b/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); + + }