From 7c8be86fb8edf7c1dd03bfe46dd2ba4feca180a1 Mon Sep 17 00:00:00 2001 From: mattmcw Date: Wed, 21 Feb 2024 22:48:08 +0100 Subject: [PATCH] Motor speed is now calculated (flawed). Tests can be run on various PWM speeds, resolutions and with and without loads to determine the ability for the printer run at maximum speeds. --- VERSION.txt | 2 +- ino/contact_printer/ContactPrinter.cpp | 8 ++- ino/contact_printer/ContactPrinter.h | 4 +- ino/contact_printer/DriveMotor.cpp | 92 ++++++++++++++++++++----- ino/contact_printer/DriveMotor.h | 46 +++++++++---- ino/contact_printer/contact_printer.ino | 2 +- scripts/version.sh | 2 +- 7 files changed, 117 insertions(+), 39 deletions(-) diff --git a/VERSION.txt b/VERSION.txt index 0c62199..ee1372d 100644 --- a/VERSION.txt +++ b/VERSION.txt @@ -1 +1 @@ -0.2.1 +0.2.2 diff --git a/ino/contact_printer/ContactPrinter.cpp b/ino/contact_printer/ContactPrinter.cpp index 0dae16c..4881d51 100644 --- a/ino/contact_printer/ContactPrinter.cpp +++ b/ino/contact_printer/ContactPrinter.cpp @@ -27,8 +27,8 @@ void ContactPrinter::Setup () { digitalWrite(takeup_pin_dir_b, LOW); SetDirectionTakeup(true); - SetSpeedTakeup(0.9); - SetSpeedDrive(0.8); + SetSpeedTakeup(1.0); + SetSpeedDrive(1.0); start_time = millis(); } @@ -52,7 +52,9 @@ void ContactPrinter::SetSpeedTakeup(float speed) { takeup_speed = speed; takeup_pwm_duty_cycle = floor(speed * pwm_maximum); Serial.print("Set takeup motors PWM = "); - Serial.println(takeup_pwm_duty_cycle); + Serial.print(takeup_pwm_duty_cycle); + Serial.print(" / "); + Serial.println(pwm_maximum); } void ContactPrinter::StartTakeup () { diff --git a/ino/contact_printer/ContactPrinter.h b/ino/contact_printer/ContactPrinter.h index 20d654f..d679e40 100644 --- a/ino/contact_printer/ContactPrinter.h +++ b/ino/contact_printer/ContactPrinter.h @@ -27,7 +27,7 @@ class ContactPrinter { const uint32_t pwm_frequency = 5000; const uint8_t takeup_pwm_channel = 1; const uint8_t pwm_resolution = 8; - const uint16_t pwm_maximum = 255; //8 = 255, 16 = 65535 + const uint16_t pwm_maximum = 255; //8 = 255, 10 = 1024, 16 = 65535 /* BUTTONS */ const uint16_t button_delay = 500; @@ -82,8 +82,6 @@ class ContactPrinter { bool IsRunning (); - float CalculateFPS (long timeLength, uint32_t frames); - float CalculateRPM (long rotationLength); }; #endif diff --git a/ino/contact_printer/DriveMotor.cpp b/ino/contact_printer/DriveMotor.cpp index b30fe9e..004878a 100644 --- a/ino/contact_printer/DriveMotor.cpp +++ b/ino/contact_printer/DriveMotor.cpp @@ -24,10 +24,22 @@ void DriveMotor::Setup () { digitalWrite(forward_pin, LOW); digitalWrite(backward_pin, LOW); - //attachInterrupt(digitalPinToInterrupt(encoder_b_pin), ReadEncoder, RISING); + attachInterrupt(digitalPinToInterrupt(encoder_b_pin), ReadEncoder, RISING); } void DriveMotor::Start() { + pulses = 0; + fps = 0.0; + fps_max = -1.0; + fps_min = 100000.0; + fps_avg = -1.0; + rpm = 0.0; + rpm_max = -1.0; + rpm_min = 100000.0; + rpm_avg = -1.0; + start_frame = frames; + start_rotation = rotations; + start_time = millis(); ledcWrite(pwm_channel, pwm_duty_cycle); digitalWrite(forward_pin, HIGH); digitalWrite(backward_pin, LOW); @@ -37,38 +49,86 @@ void DriveMotor::Stop() {; digitalWrite(forward_pin, LOW); digitalWrite(backward_pin, LOW); ledcWrite(pwm_channel, 0); + fps = CalculateFPS(timer - start_time, frames - start_frame); + rpm = CalculateRPM(timer - start_time, rotations - start_rotation); + Serial.print("RPM "); + Serial.println(rpm); + Serial.print("RPM avg: "); + Serial.println(rpm_avg); + Serial.print("RPM min: "); + Serial.println(rpm_min); + Serial.print("RPM max: "); + Serial.println(rpm_max); + + Serial.print("FPS "); + Serial.println(fps); + Serial.print("FPS avg: "); + Serial.println(fps_avg); + Serial.print("FPS min: "); + Serial.println(fps_min); + Serial.print("FPS max: "); + Serial.println(fps_max); } void DriveMotor::SetSpeed(float speed) { pwm_duty_cycle = floor(pwm_maximum * speed); Serial.print("Set drive motor PWM = "); - Serial.println(pwm_duty_cycle); + Serial.print(pwm_duty_cycle); + Serial.print(" / "); + Serial.println(pwm_maximum); } -int64_t DriveMotor::posi = 0; -/* +int32_t DriveMotor::pulses = 0; + void DriveMotor::ReadEncoder () { - int b = digitalRead(DriveMotor::encoder_b_pin); + int b = digitalRead(encoder_b_pin); if (b > 0) { - posi++; + pulses++; } else { - posi--; + pulses--; } -}*/ +} void DriveMotor::Loop () { - int64_t pos; + timer = millis(); //monitor speed - /*ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { - pos = posi; - }*/ + frames = (int32_t) floor((float) pulses / pulses_per_frame); + if (frames != last_frame) { + last_frame = frames; + fps = CalculateFPS(timer - start_time, frames - start_frame); + if (fps < fps_min) { fps_min = fps; } + if (fps > fps_max) { fps_max = fps; } + if (fps_avg < 0.0) { + fps_avg = fps; + } else { + fps_avg = (fps_avg + fps) / 2.0; + } + Serial.print("Frame: "); + Serial.println(frames); + } + rotations = (int32_t) floor((float) pulses / (float) pulses_per_rotation); + if (rotations != last_rotation) { + last_rotation = rotations; + //correction + frames = rotations * frames_per_rotation; + rpm = CalculateRPM(timer - start_time, rotations - start_rotation); + if (rpm < rpm_min) { rpm_min = rpm; } + if (rpm > rpm_max) { rpm_max = rpm; } + if (rpm_avg < 0.0) { + rpm_avg = rpm; + } else { + rpm_avg = (rpm_avg + rpm) / 2.0; + } + Serial.print("Rotation: "); + Serial.println(rotations); + } } -float DriveMotor::CalculateFPS (long timeLength, uint32_t frames) { - return 1000.0 / ((float) timeLength / (float) frames); +float DriveMotor::CalculateFPS (long time_length, uint32_t frames) { + return 1000.0 / ((float) time_length / (float) frames); } -float DriveMotor::CalculateRPM (long rotationLength) { - return 60000.0 / (float) (rotationLength); +float DriveMotor::CalculateRPM (long time_length, uint32_t rotations) { + return 60000.0 / ((float) time_length / (float) rotations); } diff --git a/ino/contact_printer/DriveMotor.h b/ino/contact_printer/DriveMotor.h index 599070d..b1d4534 100644 --- a/ino/contact_printer/DriveMotor.h +++ b/ino/contact_printer/DriveMotor.h @@ -8,29 +8,47 @@ class DriveMotor { private: //defaults are for EPS32 dev board - const uint8_t enable_pin = 26; - const uint8_t forward_pin = 27; //Clockwise - const uint8_t backward_pin = 14; //Counter-clockwise - const uint8_t encoder_a_pin = 33; - const uint8_t encoder_b_pin = 25; + static const uint8_t enable_pin = 26; + static const uint8_t forward_pin = 27; //Clockwise + static const uint8_t backward_pin = 14; //Counter-clockwise + static const uint8_t encoder_a_pin = 33; + static const uint8_t encoder_b_pin = 25; const uint32_t pwm_frequency = 5000; const uint8_t pwm_channel = 0; - const uint8_t pwm_resolution = 8; - const uint16_t pwm_maximum = 255; //8 = 255, 16 = 65535 + const uint8_t pwm_resolution = 10; + const uint16_t pwm_maximum = 1024; //8 = 255, 10 = 1024, 16 = 65535 const uint8_t ppr = 11; const float ratio = 187.0 / 3.0; - const uint32_t maxPulses = (int) round((float) ppr * ratio); - const uint8_t framesPerRotation = 18; + const uint32_t pulses_per_rotation = (int) round((float) ppr * ratio); + const uint8_t frames_per_rotation = 18; + const float pulses_per_frame = (float) pulses_per_rotation / (float) frames_per_rotation; - volatile uint8_t pwm_duty_cycle = 0; + volatile uint32_t pwm_duty_cycle = 0; - static int64_t posi; + static int32_t pulses; - //measured + //state + volatile long timer = 0; + volatile long start_time = 0; + volatile int32_t start_rotation = 0; + volatile int32_t start_frame = 0; + + //measure volatile float rpm = 0.0; + volatile float rpm_max = -1.0; + volatile float rpm_min = 100000.0; + volatile float rpm_avg = -1.0; + volatile int32_t rotations = 0; + volatile int32_t last_rotation = 0; + volatile float fps = 0.0; + volatile float fps_max = -1.0; + volatile float fps_min = 100000.0; + volatile float fps_avg = -1.0; + volatile int32_t frames = 0; + volatile int32_t last_frame = 0; //target volatile float target_fps = 0.0; @@ -45,8 +63,8 @@ class DriveMotor { void Stop(); void SetSpeed(float speed); - float CalculateFPS (long timeLength, uint32_t frames); - float CalculateRPM (long rotationLength); + float CalculateFPS (long time_length, uint32_t frames); + float CalculateRPM (long time_length, uint32_t rotations); protected: diff --git a/ino/contact_printer/contact_printer.ino b/ino/contact_printer/contact_printer.ino index a8247eb..dbbc122 100644 --- a/ino/contact_printer/contact_printer.ino +++ b/ino/contact_printer/contact_printer.ino @@ -1,6 +1,6 @@ #include "ContactPrinter.h"; - #define VERSION "0" + #define VERSION "0.2.2" /** * diff --git a/scripts/version.sh b/scripts/version.sh index 84846f4..6eb79b5 100644 --- a/scripts/version.sh +++ b/scripts/version.sh @@ -22,7 +22,7 @@ fi V="${VERSION[0]}.${VERSION[1]}.${VERSION[2]}" #echo "{ \"version\" : \"$V\", \"bin\" : \"/bin/contact_printer.bin\", \"date\" : $DATESTR }" > ./ota.json -VERSION_UPDATE=`sed "s/.*define VERSION.*/ #define VERSION \"$VERSION\"/" "${SOURCE_FILE}"` +VERSION_UPDATE=`sed "s/.*define VERSION.*/ #define VERSION \"${V}\"/" "${SOURCE_FILE}"` echo "${VERSION_UPDATE}" > "${SOURCE_FILE}" echo $V > "${VERSION_FILE}"