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

Added Motion lib 101

Browse Source
This commit is contained in:
dviid 2012-11-26 15:22:39 +01:00
parent 2e44dcdf03
commit d991358765
7 changed files with 1712 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
hardware/datasheets/doc2505.pdf Normal file
View File

Binary file not shown.

1460
software/apps/Motion/Graph2.maxpat Normal file
View File

File diff suppressed because it is too large Load Diff

28
software/apps/Motion/Graph2/Graph2.ino Normal file
View File

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

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

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

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

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

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

@ -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
//Correctthese modes are preferred for motor control applications p.130
TCCR1A = 0;
TCCR1B |= (1 << WGM13);
if(!reg_init){
//direction pins are outputs
DDRD |= (1 << PD7);
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) {
DDRB |= (1 << PB1);
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;
if(_m == MOTORA) OCR1A = value;
else if(_m == MOTORB) OCR1B = value;
_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()

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

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