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

Updates: Motion + Motor

Browse Source 
Integration with unsyncable branch
This commit is contained in:
dviid 2013-01-28 11:58:58 +01:00
parent b0bce01fac
commit 4a2f82f907
5 changed files with 23 additions and 241 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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

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

@ -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)
float X, V, F;
float m, k, d;
int Xin;
long tick;
MOTION _m;
INPUT _s;
bool _i; // is it inititated
SENSOR_INPUT _s;
bool _i;
force_callback _fcb; // NOT IMPLEMENTED
PHY _fcb_phy; // NOT IMPLEMENTED
force_callback _fcb; // not used in current version
};
extern Motion MotionA;

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

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

0
software/lib/MMM/Motor.cpp Normal file → Executable file
View File

0
software/lib/MMM/Motor.h Normal file → Executable file
View File