From 39650094bde6c28757e9e158b5b5791c768dc3ee Mon Sep 17 00:00:00 2001 From: mattmcw Date: Fri, 23 Feb 2024 00:31:14 +0100 Subject: [PATCH] Code is in test mode for getting approximations of the motor speed with no load, under single strand of film load and with 2 films. Should be possible to create a control loop to maintain a roughly-accurate speed. --- ino/contact_printer/ContactPrinter.cpp | 28 +++++--- ino/contact_printer/ContactPrinter.h | 14 ++-- ino/contact_printer/DriveMotor.cpp | 94 +++++++++++++++++++++----- ino/contact_printer/DriveMotor.h | 30 ++++++-- 4 files changed, 127 insertions(+), 39 deletions(-) diff --git a/ino/contact_printer/ContactPrinter.cpp b/ino/contact_printer/ContactPrinter.cpp index 4881d51..1bd775f 100644 --- a/ino/contact_printer/ContactPrinter.cpp +++ b/ino/contact_printer/ContactPrinter.cpp @@ -1,8 +1,7 @@ #include "ContactPrinter.h" ContactPrinter::ContactPrinter () { - SetSpeedDrive(drive_speed); - SetSpeedTakeup(takeup_speed); + } void ContactPrinter::Setup () { @@ -26,9 +25,8 @@ void ContactPrinter::Setup () { digitalWrite(takeup_pin_dir_a, LOW); digitalWrite(takeup_pin_dir_b, LOW); - SetDirectionTakeup(true); - SetSpeedTakeup(1.0); - SetSpeedDrive(1.0); + SetupTakeup(); + SetupDrive(); start_time = millis(); } @@ -48,6 +46,10 @@ void ContactPrinter::Stop () { running = false; } +void ContactPrinter::SetDirectionTakeup(bool dir) { + takeup_dir = dir; +} + void ContactPrinter::SetSpeedTakeup(float speed) { takeup_speed = speed; takeup_pwm_duty_cycle = floor(speed * pwm_maximum); @@ -74,12 +76,17 @@ void ContactPrinter::StopTakeup() { ledcWrite(takeup_pwm_channel, 0); } -void ContactPrinter::SetSpeedDrive(float speed) { - drive_motor.SetSpeed(speed); +void ContactPrinter::SetupTakeup () { + SetDirectionTakeup(true); + SetSpeedTakeup(0.9); } -void ContactPrinter::SetDirectionTakeup(bool dir) { - takeup_dir = dir; +void ContactPrinter::SetupDrive() { + //drive_motor.SetSpeed(speed); + //drive_motor.SetPWM(247); + drive_motor.SetLoad(load); + drive_motor.SetFPS(18.0); + } //linear @@ -128,6 +135,9 @@ void ContactPrinter::Loop () { ButtonLoop(); if (running) { drive_motor.Loop(); + if (drive_motor.GetFrames() >= 1000) { + Stop(); + } } } else if (timer >= start_time + 100) { initialized = true; diff --git a/ino/contact_printer/ContactPrinter.h b/ino/contact_printer/ContactPrinter.h index d679e40..869ac45 100644 --- a/ino/contact_printer/ContactPrinter.h +++ b/ino/contact_printer/ContactPrinter.h @@ -38,23 +38,22 @@ class ContactPrinter { volatile long start_time = 0; volatile long run_time = 0; - volatile float drive_speed = 1.0; //calculated rpm - volatile float takeup_speed = 1.0; //estimated rpm + volatile float drive_speed = 1.0; //CHANGE + volatile float takeup_speed = 1.0; //percentage of max PWM volatile uint16_t takeup_pwm_duty_cycle = 0; volatile uint16_t takeup_ramp_steps = 0; //# of steps volatile uint16_t takeup_ramp_step = 0; //length of step (ms) volatile boolean takeup_ramp_dir = true; //true = up, false = down - volatile uint16_t takeup_ramp_time = 500; //default ramp time (ms) volatile long takeup_ramp_start = 0; //time to start ramping volatile long takeup_ramp_current_step = 0; volatile long takeup_ramp_next_step_start = 0; - volatile boolean takeup_ramping = false; - volatile bool takeup_dir = true; + volatile uint8_t load = 2; //0 = no load, 1 = single thread, 2 = dual thread + volatile bool takeup_dir = true; volatile bool initialized = false; volatile bool running = false; @@ -67,8 +66,9 @@ class ContactPrinter { void Loop(); void Start(); void Stop(); - void SetSpeedTakeup(float speed); - void SetSpeedDrive(float speed); + void SetSpeedTakeup(float speed); //percent + void SetupDrive(); + void SetupTakeup(); void SetDirectionTakeup(bool dir); void StartTakeup(); diff --git a/ino/contact_printer/DriveMotor.cpp b/ino/contact_printer/DriveMotor.cpp index 004878a..f51947c 100644 --- a/ino/contact_printer/DriveMotor.cpp +++ b/ino/contact_printer/DriveMotor.cpp @@ -49,25 +49,11 @@ 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); + Report(); +} - 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::SetLoad(uint8_t loadInt) { + load = loadInt; } void DriveMotor::SetSpeed(float speed) { @@ -78,6 +64,20 @@ void DriveMotor::SetSpeed(float speed) { Serial.println(pwm_maximum); } +void DriveMotor::SetPWM(uint32_t pwm) { + pwm_duty_cycle = pwm; + Serial.print("Set drive motor PWM = "); + Serial.print(pwm_duty_cycle); + Serial.print(" / "); + Serial.println(pwm_maximum); +} + +void DriveMotor::SetFPS(float fps) { + target_fps = fps; + Serial.print("FPS = "); + Serial.println(fps); + SetPWM(EstimatePWMFromFPS(fps)); +} int32_t DriveMotor::pulses = 0; @@ -125,6 +125,31 @@ void DriveMotor::Loop () { } } + +void DriveMotor::Report () { + 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); +} + +/* Helper methods */ + float DriveMotor::CalculateFPS (long time_length, uint32_t frames) { return 1000.0 / ((float) time_length / (float) frames); } @@ -132,3 +157,36 @@ float DriveMotor::CalculateFPS (long time_length, uint32_t frames) { float DriveMotor::CalculateRPM (long time_length, uint32_t rotations) { return 60000.0 / ((float) time_length / (float) rotations); } + +int32_t DriveMotor::GetFrames () { + return frames; +} + +int32_t DriveMotor::GetRotations () { + return rotations; +} + +float DriveMotor::FloatMap (float x, float in_min, float in_max, float out_min, float out_max) { + return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; +} + +uint16_t DriveMotor::EstimatePWMFromFPS (float fps) { + float min, max; + float max_pwm = pwm_range[0]; + float min_pwm = pwm_range[1]; + switch (load) { + case 0 : + max = load_none[0]; + min = load_none[1]; + break; + case 1 : + max = load_one[0]; + min = load_one[1]; + break; + case 2 : + max = load_two[0]; + min = load_two[1]; + break; + } + return (uint16_t) floor(FloatMap(fps, min, max, (float) min_pwm, (float) max_pwm)); +} diff --git a/ino/contact_printer/DriveMotor.h b/ino/contact_printer/DriveMotor.h index b1d4534..a8ad2ce 100644 --- a/ino/contact_printer/DriveMotor.h +++ b/ino/contact_printer/DriveMotor.h @@ -16,8 +16,8 @@ class DriveMotor { const uint32_t pwm_frequency = 5000; const uint8_t pwm_channel = 0; - const uint8_t pwm_resolution = 10; - const uint16_t pwm_maximum = 1024; //8 = 255, 10 = 1024, 16 = 65535 + const uint8_t pwm_resolution = 8; + const uint16_t pwm_maximum = 255; //8 = 255, 10 = 1024, 16 = 65535 const uint8_t ppr = 11; const float ratio = 187.0 / 3.0; @@ -25,7 +25,15 @@ class DriveMotor { const uint8_t frames_per_rotation = 18; const float pulses_per_frame = (float) pulses_per_rotation / (float) frames_per_rotation; - volatile uint32_t pwm_duty_cycle = 0; + //pwm ranges for mapping to estimated fps + const uint16_t pwm_range[2] = { 255, 210 }; + const float load_none[2] = { 25.4, 16.75 }; + const float load_one [2] = { 24.8, 16.5 }; + const float load_two [2] = { 21.9, 13.4 }; + + volatile uint8_t load = 2; + + volatile uint16_t pwm_duty_cycle = 0; static int32_t pulses; @@ -54,6 +62,15 @@ class DriveMotor { volatile float target_fps = 0.0; volatile float target_rpm = 0.0; + volatile float test[1000]; + + + float CalculateFPS (long time_length, uint32_t frames); + float CalculateRPM (long time_length, uint32_t rotations); + float FloatMap(float x, float in_min, float in_max, float out_min, float out_max); + uint16_t EstimatePWMFromFPS(float); + void Report(); + public: DriveMotor(); @@ -61,10 +78,13 @@ class DriveMotor { void Loop(); void Start(); void Stop(); + void SetLoad(uint8_t loadInt); void SetSpeed(float speed); + void SetPWM(uint32_t pwm); + void SetFPS(float fps); - float CalculateFPS (long time_length, uint32_t frames); - float CalculateRPM (long time_length, uint32_t rotations); + int32_t GetFrames(); + int32_t GetRotations(); protected: