Im trying to create a tuned mass damper using a dc motor, hall effect sensor and ultrasonic sensor. My code almost works but freezes up once in a while and also the motor doesn't always go down. I'm super new to Arduino so my code could probably use a lot of improvement. Any help would be appreciated!
// read RPM
#define E1 5 // Enable Pin for motor
#define I1 3 // Control pin 1 for motor 1
#define I2 4 // Control pin 2 for motor 1
#define trigPin 6 //sets trigpin ultrasonic
#define echoPin 7 //sets echopoin for ultrasonic
float Steps2Take, wn, ActualL, CalcL, MeasureL, rotation, duration, squaredwn;
float DeltaL = 0;
float g = 386.4;
volatile int rpmcount = 0
int rpm = 0;
unsigned long lastmillis = 0;
void setup() {
Serial.begin(9600);
pinMode(E1, OUTPUT); //sets motor pin to output
pinMode(I1, OUTPUT); //sets motor control pin output
pinMode(I2, OUTPUT); //sets motor contril pin 2 output
pinMode(trigPin, OUTPUT); //ultrasonic sensor output
pinMode(echoPin, INPUT); //ultrasonic sensor input
attachInterrupt(0, rpm_fan, FALLING); //interrupt cero (0) is on pin two(2).
}
void loop() {
//read frequency from hall effect sensor
if (millis() - lastmillis >= 5000){ /Uptade every one second, this will be equal to reading frecuency (Hz)./
detachInterrupt(0); //Disable interrupt when calculating
rpm = rpmcount * 60 / 5;
wn = rpmcount * 2 *3.14 / 5;
Serial.print("RPM =\t"); //print the word "RPM" and tab.
Serial.print(rpm); // print the rpm value.
Serial.print("\t Hz=\t"); //print the word "Hz".
Serial.println(rpmcount/5); /print revolutions per second or Hz. And print new line or enter./
Serial.print("rad/s =\t"); //print the word "rad/s" and tab.
Serial.print(wn); // print the wn value.
rpmcount = 0; // Restart the RPM counter
lastmillis = millis(); // Uptade lasmillis
squaredwn = wn * wn; //swared natural frequency
CalcL = g / squaredwn+1.5; //calculates needed length
if (CalcL > 10) {
CalcL = 10;
}
if (CalcL < 1) {
CalcL = 1;
}
//set up and measure of ultrasonics sensor
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
MeasureL = ((duration / 2) / 29.1) / 2.54; // conversts to inches
ActualL = 12 - MeasureL; //total height minus measured value
//finds difference between measured and calculated length
DeltaL = ActualL - CalcL;
// Serial.println(DeltaL);
if (wn > 1 ) {
while ( DeltaL > .5 ) {
DeltaL=0;
//set up and measure of ultrasonics sensor
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
MeasureL = ((duration / 2) / 29.1) / 2.54; // conversts to inches
ActualL = 12 - MeasureL; //total height minus measured value
//finds difference between measured and calculated length
DeltaL = ActualL - CalcL;
Serial.print("\t +=\t");
Serial.println(DeltaL);
// turns on motor
analogWrite(E1, 160); // Run in FULL speed up
digitalWrite(I1, HIGH);
digitalWrite(I2, LOW);
}
while ( DeltaL < -.5 ) {
DeltaL=0;
//set up and measure of ultrasonics sensor
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
MeasureL = ((duration / 2) / 29.1) / 2.54; // conversts to inches
ActualL = 12 - MeasureL; //total height minus measured value
//finds difference between measured and calculated length
DeltaL = ActualL - CalcL;
Serial.print("\t -=\t");
Serial.println(DeltaL);
// turns on motor
analogWrite(E1, 160); //turn on motor speed full down
digitalWrite(I1, LOW);
digitalWrite(I2, HIGH);
}
//turn motor off
analogWrite(E1, 0);
Serial.println("done");
}
attachInterrupt(0, rpm_fan, FALLING); //enable interrupt
}
}
// this code will be executed every time the interrupt 0 (pin2) gets low.
void rpm_fan() {
rpmcount++;
}