gauthiier/M-M-M
1
0
Fork 0
Code Issues 11 Pull Requests Releases Wiki Activity

Changes to Motion, Motor and Music Library

Browse Source 
Motion library still needs to be fixed. Don't use yet.
This commit is contained in:
Jakob Bak 2013-01-24 21:14:05 +01:00
parent fc8f353064
commit 9f2f760a47
8 changed files with 304 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
software/apps/Motion/Center/Center.ino
View File

@ -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 = 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);
}

41
software/lib/MMM/Motion.cpp
View File

@ -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) {
}
*/
}

126
software/lib/MMM/Motion.cpp.old Normal file
View File

@ -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;
}
*/
}

35
software/lib/MMM/Motion.h
View File

@ -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;

81
software/lib/MMM/Motion.h.old Normal file
View File

@ -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;

34
software/lib/MMM/Motor.cpp
View File

@ -23,6 +23,8 @@
#include "Motor.h"
#include <avr/io.h>
#include <stdlib.h>
#include <avr/interrupt.h>
#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;
}

3
software/lib/MMM/Motor.h
View File

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

14
software/lib/MMM/Music.cpp
View File

@ -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);
}