86 lines
1.5 KiB
Arduino
86 lines
1.5 KiB
Arduino
|
// Damped Spring-Mass Oscillator
|
||
|
|
// BV 24Jan13
|
||
|
|
|
||
|
|
#include <Motor.h>
|
||
|
|
int Fout, Xin, duty;
|
||
|
|
|
||
|
|
// mass sring damper model
|
||
|
|
float m = 4.0;
|
||
|
|
float k = 1.0;
|
||
|
|
float b = 0.0;
|
||
|
|
float dT = .005;
|
||
|
|
float X, V, F; // model parameters (2nd-order oscil)
|
||
|
|
|
||
|
|
byte c=0; //counts up until 0 then prints
|
||
|
|
|
||
|
|
byte incomingByte;
|
||
|
|
|
||
|
|
void setup(){
|
||
|
|
Serial.begin(9600);
|
||
|
|
MotorA.init(); // initializes Timer1, pinModes
|
||
|
|
Xin = analogRead(A0) - 512; //middle of 0-1023 range
|
||
|
|
// initialize model variables
|
||
|
|
X = Xin;
|
||
|
|
V = 0.0;
|
||
|
|
F = 0.0;
|
||
|
|
}
|
||
|
|
|
||
|
|
void loop(){
|
||
|
|
Xin = analogRead(A0) - 512;
|
||
|
|
F = k*(Xin - X) - b*V; //spring and damper
|
||
|
|
V += (F/m) * dT;
|
||
|
|
X += V * dT;
|
||
|
|
|
||
|
|
duty = abs(Fout);
|
||
|
|
duty = min(duty,512);
|
||
|
|
|
||
|
|
MotorA.torque(duty);
|
||
|
|
if(Fout>0)MotorA.direction(FORWARD);
|
||
|
|
else MotorA.direction(BACKWARD);
|
||
|
|
|
||
|
|
if (c++ == 0){
|
||
|
|
Serial.println(X);
|
||
|
|
}
|
||
|
|
/*
|
||
|
|
if (Serial.available() > 0) {
|
||
|
|
incomingByte = Serial.read();
|
||
|
|
if(incomingByte == '}''){
|
||
|
|
dT = dT * 1.1;
|
||
|
|
Serial.println(dT);
|
||
|
|
}
|
||
|
|
if(incomingByte == '\'){ shift \
|
||
|
|
dT = dT / 1.1;
|
||
|
|
Serial.println(dT);
|
||
|
|
}
|
||
|
|
if(incomingByte == '|'){
|
||
|
|
k = k / 1.1;
|
||
|
|
Serial.println(k);
|
||
|
|
}
|
||
|
|
if(incomingByte == '}'){ //shift ]
|
||
|
|
k = k * 1.1;
|
||
|
|
Serial.println(k);
|
||
|
|
}
|
||
|
|
if(incomingByte == '['){
|
||
|
|
b = b / 1.1;
|
||
|
|
Serial.println(b);
|
||
|
|
}
|
||
|
|
if(incomingByte == '{'){ //shift [
|
||
|
|
b = b * 1.1;
|
||
|
|
Serial.println(b);
|
||
|
|
}
|
||
|
|
if(incomingByte == '['){
|
||
|
|
m = m / 1.1;
|
||
|
|
Serial.println(m);
|
||
|
|
}
|
||
|
|
if(incomingBmyte == '{'){ //shift [
|
||
|
|
m = m * 1.1;
|
||
|
|
Serial.println(m);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
*/
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|