From c355c232e6c3ec427a269454ee35e42af9ce23af Mon Sep 17 00:00:00 2001
From: isakg22 <isakg22@ru.is>
Date: Thu, 17 Sep 2026 12:20:06 +0000
Subject: [PATCH] =?UTF-8?q?Fokka=C3=B0i=20upp=20p2?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 include/motor_driver.h               | 16 ++++++
 include/rotary_encoder_driver.h      |  6 +++
 src/main.cpp                         | 65 ++++++++++++++++++++---
 src/motor_driver.cpp                 | 49 ++++++++++++++++++
 src/part1_first_working_main.cpp     | 50 ------------------
 src/part1part2.cpp                   | 66 ------------------------
 src/part3part1.cpp                   | 69 -------------------------
 src/rotary_encoder_driver.cpp        | 77 +++++++++++++++++++++++++---
 src/rotary_encoder_driver_unused.cpp | 60 ++++++++++++++++++++++
 9 files changed, 259 insertions(+), 199 deletions(-)
 create mode 100644 include/motor_driver.h
 create mode 100644 src/motor_driver.cpp
 delete mode 100644 src/part1_first_working_main.cpp
 delete mode 100644 src/part1part2.cpp
 delete mode 100644 src/part3part1.cpp
 create mode 100644 src/rotary_encoder_driver_unused.cpp

diff --git a/include/motor_driver.h b/include/motor_driver.h
new file mode 100644
index 0000000..3527827
--- /dev/null
+++ b/include/motor_driver.h
@@ -0,0 +1,16 @@
+#ifndef MOTOR_DRIVER_H
+#define MOTOR_DRIVER_H
+
+class MotorDriver
+{
+public:
+    MotorDriver();
+
+    void begin();
+
+    void forward();
+    void reverse();
+    void stop();
+};
+
+#endif
\ No newline at end of file
diff --git a/include/rotary_encoder_driver.h b/include/rotary_encoder_driver.h
index 8355bb0..980607f 100644
--- a/include/rotary_encoder_driver.h
+++ b/include/rotary_encoder_driver.h
@@ -1,6 +1,8 @@
 #ifndef ROTARY_ENCODER_DRIVER_H
 #define ROTARY_ENCODER_DRIVER_H
 
+#include <stdint.h>
+
 #include "digital_in.h"
 #include "digital_out.h"
 #include "timer_usec.h"
@@ -9,6 +11,7 @@ class RotaryEncoderDriver {
     volatile uint8_t cycle;
     volatile int16_t count;
     volatile int16_t prev_count;
+    volatile bool speed_ready;
 
     Digital_out led;
 
@@ -28,5 +31,8 @@ public:
     float count_per_sec();
     float rpm();
 
+    bool new_speed_available();
+    void reset_speed_measurement();
+
 };
 #endif
diff --git a/src/main.cpp b/src/main.cpp
index 7d42d43..6536c11 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -3,23 +3,76 @@
 #include <Arduino.h>
 
 #include "rotary_encoder_driver.h"
+#include "motor_driver.h"
 
 RotaryEncoderDriver encoder;
+MotorDriver motor;
 
 int main() {
     init();
-    Serial.begin(9600);
+    Serial.begin(115200);
 
     cli();
     encoder.begin();
+    motor.begin();
+
+
+
+    
     sei();
 
+
+
+    // Make sure motor is stopped before starting test
+    motor.stop();
+
+    _delay_ms(2000);
+
+    Serial.println("time_ms,rpm");
+    Serial.println("0.0,0.0");
+
+    // Synchronize the RPM measurement window with the step input
+    encoder.reset_speed_measurement();
+
+    // Apply unit-step input
+    motor.forward();
+
+    uint16_t sample = 0;
+
     while (true) {
-        _delay_ms(50);
-        // Serial.print("Counts per second: ");
-        // Serial.println(encoder.count_per_sec());
-        Serial.print("RPM: ");
-        Serial.println(encoder.rpm());
+
+        if (encoder.new_speed_available())
+        {
+            sample++;
+
+            float time_ms = sample * 5.0f;
+            float speed = encoder.rpm();
+
+            Serial.print(time_ms, 1);
+            Serial.print(",");
+            Serial.println(speed, 2);
+
+
+            // Stop after about 8 seconds {2000 (<- input in sample >= xxxx) * 4 = 8 sec}
+            if (sample >= 400)
+            {
+                motor.stop();
+
+                while (true)
+                {
+
+                }
+            }
+        }
+        
+        //Serial.print("POSITION: ");
+        //Serial.print("RPM: ");
+        
+
+        //Serial.println(encoder.position());
+        //Serial.println(encoder.rpm());
+
+        //_delay_ms(50);
     }
     return 0;
 }
diff --git a/src/motor_driver.cpp b/src/motor_driver.cpp
new file mode 100644
index 0000000..bb62543
--- /dev/null
+++ b/src/motor_driver.cpp
@@ -0,0 +1,49 @@
+#include "motor_driver.h"
+
+#include <avr/io.h>
+
+
+MotorDriver::MotorDriver()
+{
+}
+
+
+void MotorDriver::begin()
+{
+    // Arduino D3  = PD3 = AIN1
+    // Arduino D11 = PB3 = AIN2
+
+    DDRD |= (1 << DDD3);
+    DDRB |= (1 << DDB3);
+
+    stop();
+}
+
+
+void MotorDriver::forward()
+{
+    // AIN1 / D3  = HIGH
+    // AIN2 / D11 = LOW
+
+    PORTD |= (1 << PORTD3);
+    PORTB &= ~(1 << PORTB3);
+}
+
+
+void MotorDriver::reverse()
+{
+    // AIN1 / D3  = LOW
+    // AIN2 / D11 = HIGH
+
+    PORTD &= ~(1 << PORTD3);
+    PORTB |= (1 << PORTB3);
+}
+
+
+void MotorDriver::stop()
+{
+    // Both LOW
+
+    PORTD &= ~(1 << PORTD3);
+    PORTB &= ~(1 << PORTB3);
+}
\ No newline at end of file
diff --git a/src/part1_first_working_main.cpp b/src/part1_first_working_main.cpp
deleted file mode 100644
index f62fd70..0000000
--- a/src/part1_first_working_main.cpp
+++ /dev/null
@@ -1,50 +0,0 @@
-/* #include <avr/io.h>
-#include <util/delay.h>
-#include <Arduino.h>
-
-#include "digital_in.h"
-#include "digital_out.h"
-
-
-int main() {
-    uint8_t cycle{0};
-    int8_t counter{0};
-    double ratio;
-
-    bool last_lo = false;
-
-    Digital_in encoderA{4};
-    Digital_in encoderB{5};
-    Digital_out led{5};
-
-    encoderA.init();
-    encoderB.init();
-    led.init();
-
-    Serial.begin(9600);
-
-    while (true) {
-        _delay_us(100);
-        led.set_lo();
-
-        bool curr_hi = encoderA.is_hi();
-
-        if (last_lo && curr_hi) {
-            counter += encoderB.is_lo() ? -1 : 1;
-            led.set_hi();
-        }
-        ++cycle;
-
-        if (cycle == 255) {
-            ratio = counter / (0.0255 * 700);
-            counter = 0;
-            cycle = 0;
-            Serial.println(ratio);
-        }
-        
-        last_lo = !curr_hi;
-    }
-
-
-    return 0;
-} */
diff --git a/src/part1part2.cpp b/src/part1part2.cpp
deleted file mode 100644
index 47fa231..0000000
--- a/src/part1part2.cpp
+++ /dev/null
@@ -1,66 +0,0 @@
-/* #include <avr/io.h>
-#include <util/delay.h>
-#include <Arduino.h>
-
-#include "digital_in.h"
-#include "digital_out.h"
-
-class RotaryEncoderDriver {
-    int count{0};
-    bool last_lo{false};
-
-    Digital_out led{5};
-
-    Digital_in eA;
-    Digital_in eB;
-
-public:
-    RotaryEncoderDriver(unsigned short pin_a = 4, unsigned short pin_b = 5) : eA(pin_a), eB(pin_b) {
-        led.init();
-        eA.init();
-        eB.init();
-        last_lo = eA.is_lo();
-    }
-
-    int position() {
-        bool curr_hi = eA.is_hi();
-
-        if (last_lo && curr_hi) {
-            count += eB.is_hi() ? 1 : -1;
-            led.set_hi();
-        } else {
-            led.set_lo();
-        }
-
-        last_lo = !curr_hi;
-        return count;
-    } 
-
-};
-
-int main() {
-    uint8_t cycle{0};
-
-    RotaryEncoderDriver encoder;
-    Serial.begin(9600);
-
-    int prev_pos = encoder.position();
-    while (true) {
-        _delay_us(100);
-        int curr_pos = encoder.position();
-
-        if (cycle++ == 255) {
-            int delta = curr_pos - prev_pos;
-            float rpm = delta * 60.0f / (700.0f * 0.0256f);
-
-            Serial.print("Count: ");
-            Serial.print(delta);
-            Serial.print("\tRPM: ");
-            Serial.println(rpm);
-
-            prev_pos = curr_pos;
-        }
-    }
-    return 0;
-}
- */
\ No newline at end of file
diff --git a/src/part3part1.cpp b/src/part3part1.cpp
deleted file mode 100644
index 2419209..0000000
--- a/src/part3part1.cpp
+++ /dev/null
@@ -1,69 +0,0 @@
-/* #include <avr/io.h>
-#include <util/delay.h>
-#include <Arduino.h>
-
-#include "digital_in.h"
-#include "digital_out.h"
-
-class RotaryEncoderDriver {
-    volatile int count{0};
-
-    Digital_out led{5};
-
-    Digital_in eA;
-    Digital_in eB;
-
-public:
-    RotaryEncoderDriver(unsigned short pin_a = 4, unsigned short pin_b = 5) : eA(pin_a), eB(pin_b) {
-        led.init();
-        eA.init();
-        eB.init();
-
-        EICRA |= (1<<ISC01) | (1<<ISC00);
-        EIMSK |= (1<<INT0);
-    }
-
-    void update() {
-        count += eB.is_hi() ? 1 : -1;
-        led.set_hi();
-    }
-
-    int position() {
-        led.set_lo();
-        return count;
-    } 
-
-};
-
-RotaryEncoderDriver encoder{2,5};
-
-int main() {
-    uint8_t cycle{0};
-
-    Serial.begin(9600);
-
-    int prev_pos = encoder.position();
-    sei();
-    while (true) {
-        _delay_us(100);
-        int curr_pos = encoder.position();
-
-        if (cycle++ == 255) {
-            int delta = curr_pos - prev_pos;
-            float rpm = delta * 60.0f / (700.0f * 0.0256f);
-
-            Serial.print("Count: ");
-            Serial.print(delta);
-            Serial.print("\tRPM: ");
-            Serial.println(rpm);
-
-            prev_pos = curr_pos;
-        }
-    }
-    return 0;
-}
-
-ISR(INT0_vect) {
-    encoder.update();
-}
- */
\ No newline at end of file
diff --git a/src/rotary_encoder_driver.cpp b/src/rotary_encoder_driver.cpp
index 6dd2767..e71c99c 100644
--- a/src/rotary_encoder_driver.cpp
+++ b/src/rotary_encoder_driver.cpp
@@ -7,10 +7,12 @@ RotaryEncoderDriver::RotaryEncoderDriver()
     : cycle(0)
     , count(0)
     , prev_count(0)
+    , speed_ready(false)
     , led(5)
     , eA(2)
     , eB(5) {}
 
+
 void RotaryEncoderDriver::begin() {
     driver = this;
 
@@ -21,7 +23,7 @@ void RotaryEncoderDriver::begin() {
     EICRA |= (1<<ISC01) | (1<<ISC00);
     EIMSK |= (1<<INT0);
 
-    timer.init(100);
+    timer.init(5000);
 }
 
 void RotaryEncoderDriver::update() {
@@ -30,27 +32,85 @@ void RotaryEncoderDriver::update() {
 }
 
 int RotaryEncoderDriver::position() {
+
+    uint8_t oldSREG = SREG;
+
+    cli();
+
+    int16_t local_count = count;
+
+    SREG = oldSREG;
+
     led.set_lo();
-    return count;
+
+    return local_count;
 } 
 
 void RotaryEncoderDriver::update_delta() {
-    if (++cycle == 0) {
+    
         delta = count - prev_count;
+
         prev_count = count;
-    }
+
+        speed_ready = true;
+    
     led.set_lo();
 }
 
 float RotaryEncoderDriver::count_per_sec() {
-    return delta / (256.0f * 0.0001f);
-} 
 
-float RotaryEncoderDriver::rpm() {
-    return (count_per_sec() * 60) / 1050.0;
+    uint8_t oldSREG = SREG;
+
+    cli();
+
+    int16_t local_delta = delta;
+
+    SREG = oldSREG;
+
+    return local_delta / 0.005f;
 }
 
 
+float RotaryEncoderDriver::rpm()
+{
+    const float COUNTS_PER_REV = 1050.0f;
+
+    return count_per_sec() * 60.0f / COUNTS_PER_REV;
+}
+
+bool RotaryEncoderDriver::new_speed_available()
+{
+    uint8_t oldSREG = SREG;
+
+    cli();
+
+    bool ready = speed_ready;
+
+    if (ready)
+    {
+        speed_ready = false;
+    }
+
+    SREG = oldSREG;
+
+    return ready;
+}
+
+
+void RotaryEncoderDriver::reset_speed_measurement()
+{
+    uint8_t oldSREG = SREG;
+
+    cli();
+
+    cycle = 0;
+    prev_count = count;
+    delta = 0;
+    speed_ready = false;
+
+    SREG = oldSREG;
+}
+
 ISR(INT0_vect) {
     driver->update();
 }
@@ -58,3 +118,4 @@ ISR(TIMER1_COMPA_vect) {
     driver->update_delta();    
 }
 
+
diff --git a/src/rotary_encoder_driver_unused.cpp b/src/rotary_encoder_driver_unused.cpp
new file mode 100644
index 0000000..6dd2767
--- /dev/null
+++ b/src/rotary_encoder_driver_unused.cpp
@@ -0,0 +1,60 @@
+#include "rotary_encoder_driver.h"
+#include <avr/interrupt.h>
+
+RotaryEncoderDriver* driver = nullptr;
+
+RotaryEncoderDriver::RotaryEncoderDriver()
+    : cycle(0)
+    , count(0)
+    , prev_count(0)
+    , led(5)
+    , eA(2)
+    , eB(5) {}
+
+void RotaryEncoderDriver::begin() {
+    driver = this;
+
+    led.init();
+    eA.init();
+    eB.init();
+
+    EICRA |= (1<<ISC01) | (1<<ISC00);
+    EIMSK |= (1<<INT0);
+
+    timer.init(100);
+}
+
+void RotaryEncoderDriver::update() {
+    count += eB.is_hi() ? 1 : -1;
+    led.set_hi();
+}
+
+int RotaryEncoderDriver::position() {
+    led.set_lo();
+    return count;
+} 
+
+void RotaryEncoderDriver::update_delta() {
+    if (++cycle == 0) {
+        delta = count - prev_count;
+        prev_count = count;
+    }
+    led.set_lo();
+}
+
+float RotaryEncoderDriver::count_per_sec() {
+    return delta / (256.0f * 0.0001f);
+} 
+
+float RotaryEncoderDriver::rpm() {
+    return (count_per_sec() * 60) / 1050.0;
+}
+
+
+ISR(INT0_vect) {
+    driver->update();
+}
+ISR(TIMER1_COMPA_vect) {
+    driver->update_delta();    
+}
+