diff --git a/software/apps/Motion/Center/Center.ino b/software/apps/Motion/Center/Center.ino index 3e53fbf..791dc2e 100644 --- a/software/apps/Motion/Center/Center.ino +++ b/software/apps/Motion/Center/Center.ino @@ -14,7 +14,7 @@ int duty; // pwm duty for Timer1 (range 0 - 1023) 10-bit resolution void setup() { - MotorA.torque(100); //is this necessary? + MotorA.init(); } @@ -25,7 +25,7 @@ void loop() force = k * (512 - pos); duty = abs(force); - duty = min(1023, duty); + duty = min(512, duty); MotorA.torque(duty); diff --git a/software/apps/Motion/CenterA/CenterA.ino b/software/apps/Motion/CenterA/CenterA.ino index 73896ed..6370077 100644 --- a/software/apps/Motion/CenterA/CenterA.ino +++ b/software/apps/Motion/CenterA/CenterA.ino @@ -8,20 +8,18 @@ int pos; // position from analogRead int force; // computed from pos and k -int k = 1; // spring constant +int k = 10; // spring constant int duty; // pwm duty for Timer1 (range 0 - 1023) 10-bit resolution void setup() { - - MotorA.torque(100); //is this necessary? - + MotorA.init(); } void loop() { - pos = analogRead(A3); + pos = analogRead(A0); force = k * (512 - pos); duty = abs(force); diff --git a/software/apps/Motion/Pendulum/Pendulum.ino b/software/apps/Motion/Pendulum/Pendulum.ino new file mode 100644 index 0000000..5506c7e --- /dev/null +++ b/software/apps/Motion/Pendulum/Pendulum.ino @@ -0,0 +1,62 @@ +// "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); + 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/Motor/A_pos_force_test/A_pos_force_test.ino b/software/apps/Motor/A_pos_force_test/A_pos_force_test.ino new file mode 100644 index 0000000..cb96863 --- /dev/null +++ b/software/apps/Motor/A_pos_force_test/A_pos_force_test.ino @@ -0,0 +1,24 @@ +//position and force reporting +//sends A0 (MotorA), A1 (ForceA) +//sends A3 (MotorB), A4 (ForceB) +// to Serial Monitor + +// should FSR set-up be in MMM lib? +// Bill Verplank 24Jan13 + +void setup() +{ + Serial.begin(9600); + //set up for FSR A1 to D4 + pinMode(A1,INPUT); + pinMode(4,OUTPUT); + digitalWrite(4,LOW); + digitalWrite(A1,HIGH); //internal pull-up +} + +void loop() +{ + Serial.print(analogRead(A0)); //positionA + Serial.print(" "); + Serial.println(analogRead(A1)); //forceA +} diff --git a/software/apps/Motor/Motor.ino b/software/apps/Motor/Motor.ino index 31cedc0..3f362fc 100644 --- a/software/apps/Motor/Motor.ino +++ b/software/apps/Motor/Motor.ino @@ -1,27 +1,22 @@ //#include +//BV 24Jan13 #include void setup() { - - MotorA.torque(500); + MotorA.init(); + MotorA.torque(500); //512 is max torque } void loop() { - static int j = 1; - static long cnt = 0; - - if(cnt == 50000) { - j = -1; - MotorA.direction(FORWARD); - } else if(cnt == 0) { - j = 1; - MotorA.direction(BACKWARD); - } - cnt += j; + + MotorA.direction(FORWARD); + delay(1000); + MotorA.direction(BACKWARD); + delay(1000); } diff --git a/software/apps/Motor/motortest/motortest.ino b/software/apps/Motor/motortest/motortest.ino index e315554..4fc4dc1 100644 --- a/software/apps/Motor/motortest/motortest.ino +++ b/software/apps/Motor/motortest/motortest.ino @@ -7,6 +7,7 @@ int led = 13; void setup() { + MotorA.init(); MotorA.torque(511); //what is max torque? pinMode(13, OUTPUT); } @@ -14,7 +15,9 @@ void setup() void loop() { MotorA.direction(FORWARD); + digitalWrite(led,HIGH); delay(500); MotorA.direction(BACKWARD); + digitalWrite(led,LOW); delay(500); } diff --git a/software/apps/Motor/postest/postest.ino b/software/apps/Motor/postest/postest.ino index c260e55..2b85c93 100644 --- a/software/apps/Motor/postest/postest.ino +++ b/software/apps/Motor/postest/postest.ino @@ -1,14 +1,21 @@ -//position -//sends A0 (MotorA), A3 (MotorB) to Serial Monitor +//position and force reporting +//sends A0 (MotorA), A1 (ForceA) +//sends A3 (MotorB), A4 (ForceB) +// to Serial Monitor void setup() { Serial.begin(9600); + //set up for FSR A1 to D4 + pinMode(A1,INPUT); + pinMode(4,OUTPUT); + digitalWrite(4,LOW); + digitalWrite(A1,HIGH); //internal pull-up } void loop() { - Serial.print(analogRead(A3)); + Serial.print(analogRead(A0)); //positionA Serial.print(" "); - Serial.println(analogRead(A0)); + Serial.println(analogRead(A1)); //forceA } diff --git a/software/lib/MMM/Motion.cpp b/software/lib/MMM/Motion.cpp index 7baccdb..ed0c547 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,24 @@ 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.Xin = xin; - 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.F / MotionA.m) * T; + MotionA.X += MotionA.V * T; } + /* if(MotionB._i) { ADMUX = MotionB._s & 0x07; ADCSRA |= (1 << ADSC); @@ -134,6 +119,7 @@ ISR(TIMER1_OVF_vect) { MotionB._x = x; } + */ } diff --git a/software/lib/MMM/Motion.h b/software/lib/MMM/Motion.h index b066034..d25da4c 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,9 @@ public: int _xv[N]; int _ix; - int _x; - float _v; - float _a; + float X, V, A, F; // from model + float m, k, d; + int Xin; // from the ADC channel specified in Motion constructor (e.g INPUTA0) 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; }