diff --git a/software/apps/Motion/Motion0/Motion0.ino b/software/apps/Motion/Motion0/Motion0.ino index 8677ff3..3d19118 100644 --- a/software/apps/Motion/Motion0/Motion0.ino +++ b/software/apps/Motion/Motion0/Motion0.ino @@ -1,4 +1,5 @@ #include +#include /* @@ -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); + */ } diff --git a/software/apps/Motion/Pendulum/Pendulum.ino b/software/apps/Motion/Pendulum/Pendulum.ino new file mode 100644 index 0000000..5a3a26c --- /dev/null +++ b/software/apps/Motion/Pendulum/Pendulum.ino @@ -0,0 +1,63 @@ +// "Pendulum" - spring-mass oscillator +#include +#include + +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); + } + } +} diff --git a/software/apps/Motion/PendulumMM_2/PendulumMM_2.ino b/software/apps/Motion/PendulumMM_2/PendulumMM_2.ino deleted file mode 100644 index b4f24ad..0000000 --- a/software/apps/Motion/PendulumMM_2/PendulumMM_2.ino +++ /dev/null @@ -1,80 +0,0 @@ -// "Pendulum" - spring-mass oscillator -// - -#include -#include -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); - } - } -} diff --git a/software/apps/Motor/Motor.ino b/software/apps/Motor/Motor.ino index 31cedc0..162f225 100644 --- a/software/apps/Motor/Motor.ino +++ b/software/apps/Motor/Motor.ino @@ -1,27 +1,16 @@ -//#include - #include 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); } diff --git a/software/lib/MMM/Motion.cpp b/software/lib/MMM/Motion.cpp index 7baccdb..2954df7 100644 --- a/software/lib/MMM/Motion.cpp +++ b/software/lib/MMM/Motion.cpp @@ -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; } + */ } diff --git a/software/lib/MMM/Motion.h b/software/lib/MMM/Motion.h index b066034..36a88b3 100644 --- a/software/lib/MMM/Motion.h +++ b/software/lib/MMM/Motion.h @@ -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; diff --git a/software/lib/MMM/Motor.cpp b/software/lib/MMM/Motor.cpp index 4399009..dce2ea5 100644 --- a/software/lib/MMM/Motor.cpp +++ b/software/lib/MMM/Motor.cpp @@ -47,17 +47,18 @@ void MMotor::init() if(!reg_init){ //direction pins are outputs - DDRD |= (1 << PD7); - DDRB |= (1 << PB0); + DDRD |= (1 << PD7); + 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; + ICR1 = 512; + reg_init = true; }