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

New motion lib

Browse Source
This commit is contained in:
dviid 2013-01-23 16:55:40 +01:00
parent 0a0e857cea
commit afd4b230b2
7 changed files with 104 additions and 135 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
software/apps/Motion/Motion0/Motion0.ino
View File

@ -1,4 +1,5 @@
#include <Motion.h>
#include <Motor.h>
/*
@ -41,15 +42,30 @@ float getAcceleration();
void setup() {
MotionA.init(INPUTA0);
MotorA.init();
Serial.begin(9600);
MotionA.k = 0.2f;
MotionA.m = 0.3f;
MotionA.d = 0.02f;
}
void loop() {
if(MotionA.F < 0) MotorA.direction(FORWARD);
else MotorA.direction(BACKWARD);
float t = abs(MotionA.F);
MotorA.torque(t);
/*
Serial.print("position: "); Serial.println(MotionA.getPosition());
Serial.print("velocity: "); Serial.println(MotionA.getVelocity());
Serial.print("accel: "); Serial.println(MotionA.getAcceleration());
Serial.println("-------");
delay(100);
*/
}

63
software/apps/Motion/Pendulum/Pendulum.ino Normal file
View File

@ -0,0 +1,63 @@
// "Pendulum" - spring-mass oscillator
#include <Motion.h>
#include <Motor.h>
byte incomingByte;
void setup()
{
// init MotionA & MotorA
MotionA.init(INPUTA0);
MotorA.init();
// provide MotionA with initial physics constants
MotionA.k = 0.2f; // spring
MotionA.m = 0.3f; // mass
MotionA.d = 0.02f; // damping
Serial.begin(9600);
}
void loop(){
if(MotionA.F < 0) MotorA.direction(FORWARD);
else MotorA.direction(BACKWARD);
float t = abs(MotionA.F);
//if(t > 512) t = 512;
//MotorA.torque(t);
if (Serial.available() > 0) {
incomingByte = Serial.read();
if(incomingByte == '1'){
MotionA.m = MotionA.m * 1.1;
Serial.print("m: ");
Serial.println(MotionA.m);
}
else if(incomingByte == '!'){
MotionA.m = MotionA.m / 1.1;
Serial.print("m: ");
Serial.println(MotionA.m);
}
else if(incomingByte == '2'){
MotionA.k = MotionA.k * 1.1;
Serial.print("k: ");
Serial.println(MotionA.k);
}
else if(incomingByte == '@'){
MotionA.k = MotionA.k / 1.1;
Serial.print("k: ");
Serial.println(MotionA.k);
}
else if(incomingByte == '3'){
MotionA.d = MotionA.d * 1.1;
Serial.print("d: ");
Serial.println(MotionA.d);
}
else if(incomingByte == '#'){
MotionA.d = MotionA.d / 1.1;
Serial.print("d: ");
Serial.println(MotionA.d);
}
}
}

80
software/apps/Motion/PendulumMM_2/PendulumMM_2.ino
View File

@ -1,80 +0,0 @@
// "Pendulum" - spring-mass oscillator
//
#include <TimerOne.h>
#include <Motor.h>
float tf = 0.002; //time constant was .002
float kf = 0.2; // spring constant was .15
float xf,vf,ff; // floating point versions of pendulum x and v and force
int x, f; //int versions of position (from a/d) and force (to Pwm)
int duty; // abs(f)
byte c=0; //counts up until 0 then prints
float m = 0.3;
float k = 0.01;
float damp = 0.09;
byte incomingByte;
void setup()
{
//Timer1.initialize(64); //64 microsecond period
MotorA.torque(100); //initializes Timer1, pinModes
Serial.begin(9600);
}
void loop(){
x = analogRead(0); // position [0-1024]
ff = kf*(x - xf); // spring
f = max(ff,-1024);
f = min(f,1024);
// if(f>0)digitalWrite(DIRA,HIGH);
// else digitalWrite(DIRA,LOW);
duty = abs(f);
// Timer1.pwm(9,duty);
MotorA.torque(duty);
if(f>0)MotorA.direction(FORWARD);
else MotorA.direction(BACKWARD);
// update (integrate) floating versions of xf and vf
//integrate twice tf is deltaT/mass;
vf += ff*tf;
xf += vf*tf;
if(c++==0) // when c gets to 255 it next == 0 and send data
{
Serial.print(x);
Serial.print(" ");
Serial.print(xf);
Serial.print(" ");
Serial.println(ff);
}
if (c++ == 0)Serial.println(x); //
if (Serial.available() > 0) {
//Serial.print(v);
//Serial.print(" ");
//Serial.println(x);
incomingByte = Serial.read();
//Serial.write(incomingByte);
if(incomingByte == '\''){
m = m * 1.1;
Serial.println(m);
}
if(incomingByte == ';'){
m = m / 1.1;
Serial.println(m);
}
if(incomingByte == '.'){
k = k / 1.1;
Serial.println(k);
}
if(incomingByte == '/'){
k = k * 1.1;
Serial.println(k);
}
}
}

19
software/apps/Motor/Motor.ino
View File

@ -1,27 +1,16 @@
//#include <TimerOne.h>
#include <Motor.h>
void setup()
{
MotorA.torque(500);
MotorA.init();
MotorA.torque(255);
}
void loop()
{
static int j = 1;
static long cnt = 0;
if(cnt == 50000) {
j = -1;
MotorA.direction(FORWARD);
} else if(cnt == 0) {
j = 1;
delay(1000);
MotorA.direction(BACKWARD);
}
cnt += j;
delay(1000);
}

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

@ -36,9 +36,9 @@ Motion MotionB(MOTIONA);
uint8_t lb;
uint8_t hb;
int x, xx;
float T = N * 0.004f; // 4ms (see TCNT1)
float v;
int xin, dx;
float MAX_POS = 1023;
float MAX_VEL = MAX_POS / T;
@ -60,18 +60,11 @@ void Motion::init(INPUT sensor)
}
_i = true;
_s = sensor;
}
int Motion::getPosition() {
return _x;
}
float Motion::getVelocity() {
return _v;
}
float Motion::getAcceleration() {
return _a;
// initial values
d = 0;
k = 1;
m = 1;
}
void Motion::set_force_callback(force_callback fcb, PHY physics) {
@ -84,32 +77,23 @@ void Motion::set_force_callback(force_callback fcb, PHY physics) {
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;
}
xin = (hb << 8) | lb;
MotionA._a = (v - MotionA._v) / T;
MotionA._v = v;
MotionA._x = x;
MotionA.F = MotionA.k * (xin - MotionA.X) - (MotionA.d * MotionA.V);
MotionA.A = MotionA.F / MotionA.m;
MotionA.V += MotionA.A * T;
MotionA.X += MotionA.V * T;
}
/*
if(MotionB._i) {
ADMUX = MotionB._s & 0x07;
ADCSRA |= (1 << ADSC);
@ -134,6 +118,7 @@ ISR(TIMER1_OVF_vect) {
MotionB._x = x;
}
*/
}

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

@ -57,10 +57,6 @@ public:
void init(INPUT sensor);
int getPosition();
float getVelocity();
float getAcceleration();
void set_force_callback(force_callback fcb, PHY physics);
@ -68,9 +64,8 @@ public:
int _xv[N];
int _ix;
int _x;
float _v;
float _a;
float X, V, A, F;
float m, k, d;
MOTION _m;
INPUT _s;

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

@ -48,16 +48,17 @@ void MMotor::init()
//direction pins are outputs
DDRD |= (1 << PD7);
DDRB |= (1 << PB0);
DDRB = (1 << PB0) | (1 << PB1) | (1 << PB2);
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;
ICR1 = 512;
reg_init = true;
}