From 00c251b26d4c2f4a82dde5c212c2ae0ad7657a54 Mon Sep 17 00:00:00 2001 From: isakg22 Date: Thu, 24 Sep 2026 11:20:26 +0000 Subject: [PATCH] =?UTF-8?q?N=C3=BDtt=20branch=20og=20k=C3=B3=C3=B0ar/files?= =?UTF-8?q?=20fyrir=20part=201?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- include/initialization_state.h | 0 include/motor_controller.h | 0 include/operational_state.h | 0 include/rotary_encoder_driver.h | 13 +++-- include/state.h | 32 ++++++++++++ src/initialization_state.cpp | 0 src/main.cpp | 91 ++++++--------------------------- src/main_P2.cpp | 88 +++++++++++++++++++++++++++++++ src/motor_controller.cpp | 0 src/operational_state.cpp | 0 src/rotary_encoder_driver.cpp | 41 ++++++++++----- 11 files changed, 173 insertions(+), 92 deletions(-) create mode 100644 include/initialization_state.h create mode 100644 include/motor_controller.h create mode 100644 include/operational_state.h create mode 100644 include/state.h create mode 100644 src/initialization_state.cpp create mode 100644 src/main_P2.cpp create mode 100644 src/motor_controller.cpp create mode 100644 src/operational_state.cpp diff --git a/include/initialization_state.h b/include/initialization_state.h new file mode 100644 index 0000000..e69de29 diff --git a/include/motor_controller.h b/include/motor_controller.h new file mode 100644 index 0000000..e69de29 diff --git a/include/operational_state.h b/include/operational_state.h new file mode 100644 index 0000000..e69de29 diff --git a/include/rotary_encoder_driver.h b/include/rotary_encoder_driver.h index 2b51f95..d097eba 100644 --- a/include/rotary_encoder_driver.h +++ b/include/rotary_encoder_driver.h @@ -4,11 +4,11 @@ #include #include "digital_in.h" -#include "digital_out.h" +//#include "digital_out.h" #include "timer_usec.h" class RotaryEncoderDriver { - static const uint8_t HIST_LEN = 4; // 8 x 5 ms = 40 ms gluggi fyrir rpm-utreikning + static const uint8_t HIST_LEN = 4; // 4 x 4 ms = 16 ms gluggi fyrir rpm-utreikning volatile uint8_t cycle; volatile int16_t count; @@ -18,7 +18,7 @@ class RotaryEncoderDriver { volatile int16_t history[HIST_LEN]; volatile uint8_t hist_idx; - Digital_out led; + //Digital_out led; Digital_in eA; Digital_in eB; @@ -29,14 +29,21 @@ public: volatile int16_t delta; RotaryEncoderDriver(); + void begin(); + void update(); + int position(); + void update_delta(); + float count_per_sec(); + float rpm(); bool new_speed_available(); + void reset_speed_measurement(); }; diff --git a/include/state.h b/include/state.h new file mode 100644 index 0000000..4f58648 --- /dev/null +++ b/include/state.h @@ -0,0 +1,32 @@ +#ifndef STATE_H +#define STATE_H + +class MotorController; + +class State +{ +protected: + MotorController* context; + +public: + virtual ~State() + { + + } + + void set_context(MotorController* context) + { + this->context = context; + } + + virtual void enter() = 0; // LED ON + + virtual void update() = 0; // run motor controller + + virtual void command(char c) = 0; // Listen for reset + + virtual void exit() = 0; // Stop motor + +}; + +#endif \ No newline at end of file diff --git a/src/initialization_state.cpp b/src/initialization_state.cpp new file mode 100644 index 0000000..e69de29 diff --git a/src/main.cpp b/src/main.cpp index 7b09aef..de8e322 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,87 +1,28 @@ -#include -#include -#include - -#include "rotary_encoder_driver.h" -#include "motor_driver.h" -#include "p_controller.h" - -RotaryEncoderDriver encoder; -MotorDriver motor; - -P_controller controller(12.5); - -const double REF_RPM1 = 30.0; -const double REF_RPM2 = 50.0; - -const uint16_t STEP1_AT_SAMPLE = 50; -const uint16_t STEP2_AT_SAMPLE = 500; - -const uint16_t STOP_AT_SAMPLE = 1000; - -int main() { +int main() +{ init(); + Serial.begin(115200); cli(); + encoder.begin(); motor.begin(); + sei(); - // Mótor stopp og smá bið svo kerfið sé í kyrrstöðu áður en mælingin hefst - motor.stop(); - _delay_ms(2000); + system.begin(); - Serial.println("time_ms,ref_rpm,actual_rpm,pwm_requested,pwm_actual"); - Serial.println("0.0,0.0,0.0,0.0,0.0"); - - encoder.reset_speed_measurement(); - - uint16_t sample = 0; - double ref = 0.0; - - while (true) { - if (encoder.new_speed_available()) { - sample++; - - float time_ms = sample * 4.0f; - - // Step-inntak: viðmiðið hoppar úr 0 í REF_RPM á STEP_AT_SAMPLE - if (sample == STEP1_AT_SAMPLE) { - ref = REF_RPM1; - } - - if (sample == STEP2_AT_SAMPLE) { - ref = REF_RPM2; - } - - double actual = encoder.rpm(); - - double pwm_percent = controller.update(ref, actual); - double pwm_requested = pwm_percent * 2.55; - - double pwm_actual = pwm_requested; - if (pwm_actual > 255.0) pwm_actual = 255.0; - if (pwm_actual < -255.0) pwm_actual = -255.0; - - motor.set(pwm_actual); - - Serial.print(time_ms, 1); - Serial.print(","); - Serial.print(ref, 1); - Serial.print(","); - Serial.print(actual, 2); - Serial.print(","); - Serial.print(pwm_requested, 1); - Serial.print(","); - Serial.println(pwm_actual, 1); - - if (sample >= STOP_AT_SAMPLE) { - motor.stop(); - while (true) { - } - } + while (true) + { + if (Serial.available()) + { + char c = Serial.read(); + system.command(c); } + + system.update(); } + return 0; -} +} \ No newline at end of file diff --git a/src/main_P2.cpp b/src/main_P2.cpp new file mode 100644 index 0000000..dec3819 --- /dev/null +++ b/src/main_P2.cpp @@ -0,0 +1,88 @@ +/* #include +#include +#include + +#include "rotary_encoder_driver.h" +#include "motor_driver.h" +#include "p_controller.h" + +RotaryEncoderDriver encoder; +MotorDriver motor; + +P_controller controller(12.5); + +const double REF_RPM1 = 30.0; +const double REF_RPM2 = 50.0; + +const uint16_t STEP1_AT_SAMPLE = 50; +const uint16_t STEP2_AT_SAMPLE = 500; + +const uint16_t STOP_AT_SAMPLE = 1000; + +int main() { + init(); + Serial.begin(115200); + + cli(); + encoder.begin(); + motor.begin(); + sei(); + + // Mótor stopp og smá bið svo kerfið sé í kyrrstöðu áður en mælingin hefst + motor.stop(); + _delay_ms(2000); + + Serial.println("time_ms,ref_rpm,actual_rpm,pwm_requested,pwm_actual"); + Serial.println("0.0,0.0,0.0,0.0,0.0"); + + encoder.reset_speed_measurement(); + + uint16_t sample = 0; + double ref = 0.0; + + while (true) { + if (encoder.new_speed_available()) { + sample++; + + float time_ms = sample * 4.0f; + + // Step-inntak: viðmiðið hoppar úr 0 í REF_RPM á STEP_AT_SAMPLE + if (sample == STEP1_AT_SAMPLE) { + ref = REF_RPM1; + } + + if (sample == STEP2_AT_SAMPLE) { + ref = REF_RPM2; + } + + double actual = encoder.rpm(); + + double pwm_percent = controller.update(ref, actual); + double pwm_requested = pwm_percent * 2.55; + + double pwm_actual = pwm_requested; + if (pwm_actual > 255.0) pwm_actual = 255.0; + if (pwm_actual < -255.0) pwm_actual = -255.0; + + motor.set(pwm_actual); + + Serial.print(time_ms, 1); + Serial.print(","); + Serial.print(ref, 1); + Serial.print(","); + Serial.print(actual, 2); + Serial.print(","); + Serial.print(pwm_requested, 1); + Serial.print(","); + Serial.println(pwm_actual, 1); + + if (sample >= STOP_AT_SAMPLE) { + motor.stop(); + while (true) { + } + } + } + } + return 0; +} + */ \ No newline at end of file diff --git a/src/motor_controller.cpp b/src/motor_controller.cpp new file mode 100644 index 0000000..e69de29 diff --git a/src/operational_state.cpp b/src/operational_state.cpp new file mode 100644 index 0000000..e69de29 diff --git a/src/rotary_encoder_driver.cpp b/src/rotary_encoder_driver.cpp index b247747..8d7668a 100644 --- a/src/rotary_encoder_driver.cpp +++ b/src/rotary_encoder_driver.cpp @@ -12,17 +12,20 @@ RotaryEncoderDriver::RotaryEncoderDriver() , led(5) , eA(2) , eB(5) + , delta(0) { - for (uint8_t i = 0; i < HIST_LEN; i++) { + for (uint8_t i = 0; i < HIST_LEN; i++) + { history[i] = 0; } } -void RotaryEncoderDriver::begin() { +void RotaryEncoderDriver::begin() +{ driver = this; - led.init(); + //led.init(); eA.init(); eB.init(); @@ -32,12 +35,14 @@ void RotaryEncoderDriver::begin() { timer.init(4000); } -void RotaryEncoderDriver::update() { +void RotaryEncoderDriver::update() +{ count += eB.is_hi() ? 1 : -1; - led.set_hi(); + //led.set_hi(); } -int RotaryEncoderDriver::position() { +int RotaryEncoderDriver::position() +{ uint8_t oldSREG = SREG; @@ -47,27 +52,31 @@ int RotaryEncoderDriver::position() { SREG = oldSREG; - led.set_lo(); + //led.set_lo(); return local_count; } -void RotaryEncoderDriver::update_delta() { +void RotaryEncoderDriver::update_delta() +{ int16_t oldest = history[hist_idx]; + history[hist_idx] = count; + hist_idx = (hist_idx + 1) % HIST_LEN; - delta = count - oldest; // breyting yfir HIST_LEN * Ts (40 ms) glugga + delta = count - oldest; // breyting yfir HIST_LEN * Ts (16 ms) glugga prev_count = count; speed_ready = true; - led.set_lo(); + //led.set_lo(); } -float RotaryEncoderDriver::count_per_sec() { +float RotaryEncoderDriver::count_per_sec() +{ uint8_t oldSREG = SREG; @@ -118,18 +127,22 @@ void RotaryEncoderDriver::reset_speed_measurement() delta = 0; speed_ready = false; - for (uint8_t i = 0; i < HIST_LEN; i++) { + for (uint8_t i = 0; i < HIST_LEN; i++) + { history[i] = count; } + hist_idx = 0; SREG = oldSREG; } -ISR(INT0_vect) { +ISR(INT0_vect) +{ driver->update(); } -ISR(TIMER1_COMPA_vect) { +ISR(TIMER1_COMPA_vect) +{ driver->update_delta(); }