Kom inn eftir uppfærlsu á VSc

motor_driver.cpp
motor_main.cpp

.gitignore

@@ -1 +1,6 @@
 .pio
+venv
+*.py
+*.csv
+scripts
+*.typ

.vscode/c_cpp_properties.json

@@ -8,29 +8,29 @@
         {
             "name": "PlatformIO",
             "includePath": [
-                "C:/Users/helgi/H26/ESP/Projects/ESP_Project2_H26/include",
-                "C:/Users/helgi/H26/ESP/Projects/ESP_Project2_H26/src",
-                "C:/Users/helgi/.platformio/packages/framework-arduino-avr/cores/arduino",
-                "C:/Users/helgi/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs",
-                "C:/Users/helgi/.platformio/packages/framework-arduino-avr/libraries/EEPROM/src",
-                "C:/Users/helgi/.platformio/packages/framework-arduino-avr/libraries/HID/src",
-                "C:/Users/helgi/.platformio/packages/framework-arduino-avr/libraries/SPI/src",
-                "C:/Users/helgi/.platformio/packages/framework-arduino-avr/libraries/SoftwareSerial/src",
-                "C:/Users/helgi/.platformio/packages/framework-arduino-avr/libraries/Wire/src",
+                "C:/Users/isakt/Embedded/ESP_Project2_H26/include",
+                "C:/Users/isakt/Embedded/ESP_Project2_H26/src",
+                "C:/Users/isakt/.platformio/packages/framework-arduino-avr/cores/arduino",
+                "C:/Users/isakt/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs",
+                "C:/Users/isakt/.platformio/packages/framework-arduino-avr/libraries/EEPROM/src",
+                "C:/Users/isakt/.platformio/packages/framework-arduino-avr/libraries/HID/src",
+                "C:/Users/isakt/.platformio/packages/framework-arduino-avr/libraries/SPI/src",
+                "C:/Users/isakt/.platformio/packages/framework-arduino-avr/libraries/SoftwareSerial/src",
+                "C:/Users/isakt/.platformio/packages/framework-arduino-avr/libraries/Wire/src",
                 ""
             ],
             "browse": {
                 "limitSymbolsToIncludedHeaders": true,
                 "path": [
-                    "C:/Users/helgi/H26/ESP/Projects/ESP_Project2_H26/include",
-                    "C:/Users/helgi/H26/ESP/Projects/ESP_Project2_H26/src",
-                    "C:/Users/helgi/.platformio/packages/framework-arduino-avr/cores/arduino",
-                    "C:/Users/helgi/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs",
-                    "C:/Users/helgi/.platformio/packages/framework-arduino-avr/libraries/EEPROM/src",
-                    "C:/Users/helgi/.platformio/packages/framework-arduino-avr/libraries/HID/src",
-                    "C:/Users/helgi/.platformio/packages/framework-arduino-avr/libraries/SPI/src",
-                    "C:/Users/helgi/.platformio/packages/framework-arduino-avr/libraries/SoftwareSerial/src",
-                    "C:/Users/helgi/.platformio/packages/framework-arduino-avr/libraries/Wire/src",
+                    "C:/Users/isakt/Embedded/ESP_Project2_H26/include",
+                    "C:/Users/isakt/Embedded/ESP_Project2_H26/src",
+                    "C:/Users/isakt/.platformio/packages/framework-arduino-avr/cores/arduino",
+                    "C:/Users/isakt/.platformio/packages/framework-arduino-avr/variants/eightanaloginputs",
+                    "C:/Users/isakt/.platformio/packages/framework-arduino-avr/libraries/EEPROM/src",
+                    "C:/Users/isakt/.platformio/packages/framework-arduino-avr/libraries/HID/src",
+                    "C:/Users/isakt/.platformio/packages/framework-arduino-avr/libraries/SPI/src",
+                    "C:/Users/isakt/.platformio/packages/framework-arduino-avr/libraries/SoftwareSerial/src",
+                    "C:/Users/isakt/.platformio/packages/framework-arduino-avr/libraries/Wire/src",
                     ""
                 ]
             },
@@ -45,7 +45,7 @@
             ],
             "cStandard": "gnu11",
             "cppStandard": "gnu++11",
-            "compilerPath": "C:/Users/helgi/.platformio/packages/toolchain-atmelavr/bin/avr-gcc.exe",
+            "compilerPath": "C:/Users/isakt/.platformio/packages/toolchain-atmelavr/bin/avr-gcc.exe",
             "compilerArgs": [
                 "-mmcu=atmega328p",
                 ""

.vscode/launch.json

@@ -12,9 +12,9 @@
             "type": "platformio-debug",
             "request": "launch",
             "name": "PIO Debug",
-            "executable": "C:/Users/helgi/H26/ESP/Projects/ESP_Project2_H26/.pio/build/nanoatmega328new/firmware.elf",
+            "executable": "C:/Users/isakt/Embedded/ESP_Project2_H26/.pio/build/nanoatmega328new/firmware.elf",
             "projectEnvName": "nanoatmega328new",
-            "toolchainBinDir": "C:/Users/helgi/.platformio/packages/toolchain-atmelavr/bin",
+            "toolchainBinDir": "C:/Users/isakt/.platformio/packages/toolchain-atmelavr/bin",
             "internalConsoleOptions": "openOnSessionStart",
             "preLaunchTask": {
                 "type": "PlatformIO",
@@ -25,18 +25,18 @@
             "type": "platformio-debug",
             "request": "launch",
             "name": "PIO Debug (skip Pre-Debug)",
-            "executable": "C:/Users/helgi/H26/ESP/Projects/ESP_Project2_H26/.pio/build/nanoatmega328new/firmware.elf",
+            "executable": "C:/Users/isakt/Embedded/ESP_Project2_H26/.pio/build/nanoatmega328new/firmware.elf",
             "projectEnvName": "nanoatmega328new",
-            "toolchainBinDir": "C:/Users/helgi/.platformio/packages/toolchain-atmelavr/bin",
+            "toolchainBinDir": "C:/Users/isakt/.platformio/packages/toolchain-atmelavr/bin",
             "internalConsoleOptions": "openOnSessionStart"
         },
         {
             "type": "platformio-debug",
             "request": "launch",
             "name": "PIO Debug (without uploading)",
-            "executable": "C:/Users/helgi/H26/ESP/Projects/ESP_Project2_H26/.pio/build/nanoatmega328new/firmware.elf",
+            "executable": "C:/Users/isakt/Embedded/ESP_Project2_H26/.pio/build/nanoatmega328new/firmware.elf",
             "projectEnvName": "nanoatmega328new",
-            "toolchainBinDir": "C:/Users/helgi/.platformio/packages/toolchain-atmelavr/bin",
+            "toolchainBinDir": "C:/Users/isakt/.platformio/packages/toolchain-atmelavr/bin",
             "internalConsoleOptions": "openOnSessionStart",
             "loadMode": "manual"
         }

include/motor_driver.h

@@ -0,0 +1,18 @@
+#ifndef MOTOR_DRIVER_H
+#define MOTOR_DRIVER_H
+
+class MotorDriver
+{
+public:
+    MotorDriver();
+
+    void begin();
+
+    void forward();
+    void reverse();
+    void stop();
+
+    void set(double pwm);
+};
+
+#endif

include/p_controller.h

@@ -0,0 +1,15 @@
+#ifndef P_CONTROLLER_H
+#define P_CONTROLLER_H
+
+class P_controller
+{
+private:
+    double Kp;
+
+public:
+    P_controller(double Kp);
+
+    double update(double ref, double actual);
+};
+
+#endif

include/rotary_encoder_driver.h

@@ -1,14 +1,22 @@
 #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"
 
 class RotaryEncoderDriver {
+    static const uint8_t HIST_LEN = 4;   // 8 x 5 ms = 40 ms gluggi fyrir rpm-utreikning
+
     volatile uint8_t cycle;
     volatile int16_t count;
     volatile int16_t prev_count;
+    volatile bool speed_ready;
+
+    volatile int16_t history[HIST_LEN];
+    volatile uint8_t hist_idx;
 
     Digital_out led;
 
@@ -28,5 +36,8 @@ public:
     float count_per_sec();
     float rpm();
 
+    bool new_speed_available();
+    void reset_speed_measurement();
+
 };
 #endif

platformio.ini

@@ -12,3 +12,5 @@
 platform = atmelavr
 board = nanoatmega328new
 framework = arduino
+
+monitor_speed = 115200

src/main.cpp

@@ -3,23 +3,85 @@
 #include <Arduino.h>
 
 #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(9600);
+    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) {
-        _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 * 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;
 }

src/motor_driver.cpp

@@ -0,0 +1,110 @@
+#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);
+
+    // Timer2 Fast PWM
+    TCCR2A = 0;
+    TCCR2B = 0;
+
+    // Fast PWM mode
+    TCCR2A |= (1 << WGM20);
+    TCCR2A |= (1 << WGM21);
+
+    // Non-inverting PWM on OC2A and OC2B
+    TCCR2A |= (1 << COM2A1);
+    TCCR2A |= (1 << COM2B1);
+
+
+    // Timer2 prescaler = 32
+    TCCR2B |= (1 << CS21);
+    TCCR2B |= (1 << CS20);
+
+    OCR2A = 0;
+    OCR2B = 0;
+
+    //stop();
+}
+
+
+void MotorDriver::forward()
+{
+    // AIN1 / D3  = HIGH
+    // AIN2 / D11 = LOW
+
+    //PORTD |= (1 << PORTD3);
+    //PORTB &= ~(1 << PORTB3);
+
+    OCR2A = 0;
+    OCR2B = 255;
+}
+
+
+void MotorDriver::reverse()
+{
+    // AIN1 / D3  = LOW
+    // AIN2 / D11 = HIGH
+
+    //PORTD &= ~(1 << PORTD3);
+    //PORTB |= (1 << PORTB3);
+
+    OCR2B = 0;
+    OCR2A = 255;
+}
+
+
+void MotorDriver::stop()
+{
+    // Both LOW
+
+    //PORTD &= ~(1 << PORTD3);
+    //PORTB &= ~(1 << PORTB3);
+
+    OCR2A = 0;
+    OCR2B = 0;
+}
+
+void MotorDriver::set(double pwm)
+{
+    // Limit PWM command
+    if (pwm > 255.0)
+    {
+        pwm = 255.0;
+    }
+
+    if (pwm < -255.0)
+    {
+        pwm = -255.0;
+    }
+
+
+    if (pwm > 0)
+    {
+        // Forward
+        OCR2A = 0;
+        OCR2B = (uint8_t)pwm;
+    }
+    else if (pwm < 0)
+    {
+        // Reverse
+        OCR2B = 0;
+        OCR2A = (uint8_t)(-pwm);
+    }
+    else
+    {
+        stop();
+    }
+}

src/p_controller.cpp

@@ -0,0 +1,13 @@
+#include "p_controller.h"
+
+P_controller::P_controller(double Kp)
+    : Kp(Kp)
+{
+}
+
+double P_controller::update(double ref, double actual)
+{
+    double error = ref - actual;
+
+    return Kp * error;
+}

src/part1_first_working_main.cpp

@@ -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;
-} */

src/part1part2.cpp

@@ -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;
-}
- */

src/part3part1.cpp

@@ -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();
-}
- */

src/rotary_encoder_driver.cpp

@@ -7,9 +7,17 @@ RotaryEncoderDriver::RotaryEncoderDriver()
     : cycle(0)
     , count(0)
     , prev_count(0)
+    , speed_ready(false)
+    , hist_idx(0)
     , led(5)
     , eA(2)
-    , eB(5) {}
+    , eB(5)
+{
+    for (uint8_t i = 0; i < HIST_LEN; i++) {
+        history[i] = 0;
+    }
+}
+
 
 void RotaryEncoderDriver::begin() {
     driver = this;
@@ -21,7 +29,7 @@ void RotaryEncoderDriver::begin() {
     EICRA |= (1<<ISC01) | (1<<ISC00);
     EIMSK |= (1<<INT0);
 
-    timer.init(100);
+    timer.init(4000);
 }
 
 void RotaryEncoderDriver::update() {
@@ -30,26 +38,93 @@ 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;
+
+    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
+
     prev_count = count;
-    }
+
+    speed_ready = true;
+
     led.set_lo();
 }
 
 float RotaryEncoderDriver::count_per_sec() {
-    return delta / (256.0f * 0.0001f);
+
+    uint8_t oldSREG = SREG;
+
+    cli();
+
+    int16_t local_delta = delta;
+
+    SREG = oldSREG;
+
+    return local_delta / (HIST_LEN * 0.004f);
 }
 
-float RotaryEncoderDriver::rpm() {
-    return (count_per_sec() * 60) / 1050.0;
+
+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;
+
+    for (uint8_t i = 0; i < HIST_LEN; i++) {
+        history[i] = count;
+    }
+    hist_idx = 0;
+
+    SREG = oldSREG;
+}
 
 ISR(INT0_vect) {
     driver->update();
@@ -58,3 +133,4 @@ ISR(TIMER1_COMPA_vect) {
     driver->update_delta();    
 }
 
+

src/rotary_encoder_driver_unused.cpp

@@ -0,0 +1,61 @@
+/* #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();    
+}
+
+ */

src/temporary_main.cpp

@@ -0,0 +1,53 @@
+/* Óvirkt núna, "alvöru" main() er í main.cpp. Þessi skrá var notuð í undirbúning/prófun
+
+#include <Arduino.h>
+
+#include "p_controller.h"
+
+P_controller controller(2.0);
+
+int main()
+{
+    init();
+
+    Serial.begin(115200);
+
+    double result = controller.update(100.0, 70.0);
+
+    Serial.print("PWM = ");
+    Serial.println(result);
+
+    while (true)
+    {
+    }
+
+    return 0;
+}
+
+
+#include <Arduino.h>
+#include <util/delay.h>
+
+#include "motor_driver.h"
+
+MotorDriver motor;
+
+int main()
+{
+    init();
+
+    motor.begin();
+
+    while (true)
+    {
+        motor.set(60);
+        _delay_ms(3000);
+
+        motor.set(120);
+        _delay_ms(3000);
+
+        motor.set(240);
+        _delay_ms(3000);
+    }
+}
+*/