Setti inn allar skrár frá Project 3 og bætti við overlay í samræmi við controlloerinn og pin out.

src/digital_in.cpp

@@ -0,0 +1,14 @@
+#include <avr/io.h>
+#include "digital_in.h"
+
+Digital_in::Digital_in(uint8_t pin) : pin_mask(1<<pin) {}
+
+void Digital_in::init() {
+    DDRD  &= ~pin_mask;
+    PORTD |=  pin_mask;
+}
+
+bool Digital_in::is_hi() { return PIND & pin_mask; }
+bool Digital_in::is_lo() { return !is_hi(); }
+
+

src/digital_out.cpp

@@ -0,0 +1,9 @@
+#include "digital_out.h"
+#include <avr/io.h>
+
+Digital_out::Digital_out(uint8_t pin) : pin_mask(1<<pin) {}
+
+void Digital_out::init()   { DDRB  |=  pin_mask; }
+void Digital_out::toggle() { PORTB ^=  pin_mask; }
+void Digital_out::set_hi() { PORTB |=  pin_mask; }
+void Digital_out::set_lo() { PORTB &= ~pin_mask; }

src/initialization_state.cpp

@@ -0,0 +1,41 @@
+#include "initialization_state.h"
+#include "motor_controller.h"
+
+#include <Arduino.h>
+
+
+void InitializationState::enter()
+{
+    Serial.println();
+    Serial.println("State: Initialization");
+
+    context->led_off();
+
+    context->stop_motor();
+
+    context->reset_speed_measurement();
+
+    enter_time_ms = millis();
+}
+
+
+void InitializationState::update()
+{
+    // Non-blocking bið svo aðallykkjan haldi áfram að keyra
+    if (millis() - enter_time_ms >= WAIT_MS)
+    {
+        context->to_pre_operational();
+        // context->to_operational();
+    }
+}
+
+
+void InitializationState::command(char c)
+{
+}
+
+
+void InitializationState::exit()
+{
+    Serial.println("Boot-up");
+}

src/main.cpp

@@ -0,0 +1,84 @@
+// main.cpp er sleppt i test-build (pio test) svo setup()/loop() rekist ekki a prof
+#ifndef PIO_UNIT_TESTING
+
+#include <Arduino.h>
+#include <avr/interrupt.h>
+
+#include "rotary_encoder_driver.h"
+#include "motor_driver.h"
+#include "pi_controller.h"
+
+#include "motor_controller.h"
+
+#include "initialization_state.h"
+
+#include "pre_operational_state.h"
+
+#include "operational_state.h"
+#include "stopped_state.h"
+
+
+RotaryEncoderDriver encoder;
+
+MotorDriver motor;
+
+const double Kp_P = 10.0;     // P-controller from Project 2
+
+const double Kp = 10.125;     // Ziegler-Nichols PI
+const double Ti = 0.0334;
+const double T = 0.004;
+
+P_controller p_controller(Kp_P);
+PI_controller pi_controller(Kp, Ti, T);
+
+
+InitializationState initialization_state;
+// Pre-operational velur á milli stýringanna með 'c'
+PreOperationalState pre_operational_state(p_controller, pi_controller);
+OperationalState operational_state;
+StoppedState stopped_state;
+
+// Stöðurnar verða að vera skilgreindar á undan motor_controller
+MotorController motor_controller(
+    encoder,
+    motor,
+    pi_controller,      // Sjálfgefin stýring
+    initialization_state,
+    pre_operational_state,
+    operational_state,
+    stopped_state
+);
+
+
+int main()
+{
+    init();
+    Serial.begin(115200);
+    cli();
+
+    encoder.begin();
+    motor.begin();
+
+    sei();
+
+    motor_controller.begin();
+
+    while (true)
+    {
+        if (Serial.available() > 0)
+        {
+            char c = Serial.read();
+
+            motor_controller.command(c);   
+
+        }
+
+
+        motor_controller.update();
+    }
+
+
+    return 0;
+}
+
+#endif 

src/motor_controller.cpp

@@ -0,0 +1,191 @@
+#include "motor_controller.h"
+#include "state.h"
+
+#include <Arduino.h>
+
+
+MotorController::MotorController(
+    RotaryEncoderDriver& encoder,
+    MotorDriver& motor,
+    P_controller& controller,
+    State& initialization,
+    State& pre_operational,
+    State& operational,
+    State& stopped
+)
+    : current_state(nullptr),
+      initialization_state(initialization),
+      pre_operational_state(pre_operational),
+      operational_state(operational),
+      stopped_state(stopped),
+      encoder(encoder),
+      motor(motor),
+      controller(&controller),
+      led(LED_PIN),
+      reference_rpm(DEFAULT_REFERENCE_RPM),
+      last_actual_rpm(0.0),
+      last_pwm(0.0)
+{
+    initialization_state.set_context(this);
+    pre_operational_state.set_context(this);
+    operational_state.set_context(this);
+    stopped_state.set_context(this);
+}
+
+
+// void MotorController::set_states(
+//     State* initialization,
+//     State* operational,
+//     State* stopped
+// )
+
+    // initialization_state = initialization;
+    // operational_state = operational;
+    // stopped_state = stopped;
+
+
+
+void MotorController::begin()
+{
+    led.init();
+    led.set_lo();
+
+    transition_to(initialization_state);
+}
+
+
+void MotorController::transition_to(State& new_state)
+{
+    if (current_state != nullptr)
+    {
+        current_state->exit();
+    }
+
+    current_state = &new_state;
+
+    current_state->enter();
+}
+
+
+void MotorController::update()
+{
+    if (current_state != nullptr)
+    {
+        current_state->update();
+    }
+}
+
+
+void MotorController::command(char c)
+{
+    if (current_state != nullptr)
+    {
+        current_state->command(c);
+    }
+}
+
+
+void MotorController::to_initialization()
+{
+    transition_to(initialization_state);
+}
+
+void MotorController::to_pre_operational()
+{
+    transition_to(pre_operational_state);
+}
+
+
+void MotorController::to_operational()
+{
+    transition_to(operational_state);
+}
+
+
+bool MotorController::run_speed_controller()
+{
+    if (!encoder.new_speed_available())
+    {
+        return false;
+    }
+
+    double actual = encoder.rpm();
+
+    double pwm_percent =
+        controller->update(reference_rpm, actual);
+
+    double pwm =
+        pwm_percent * PWM_PER_PERCENT;
+
+    // Klippt hér svo last_pwm sé nákvæmlega það sem fer á mótorinn
+    if (pwm > 255.0)
+    {
+        pwm = 255.0;
+    }
+    else if (pwm < -255.0)
+    {
+        pwm = -255.0;
+    }
+
+    motor.set(pwm);
+
+    last_actual_rpm = actual;
+    last_pwm = pwm;
+
+    return true;
+}
+
+
+void MotorController::stop_motor()
+{
+    motor.stop();
+}
+
+
+void MotorController::reset_speed_measurement()
+{
+    encoder.reset_speed_measurement();
+}
+
+void MotorController::reset_controller()
+{
+    controller->reset();
+}
+
+void MotorController::set_Kp(double value)
+{
+    controller->set_Kp(value);
+}
+
+void MotorController::set_Ti(double value)
+{
+    controller->set_Ti(value);
+}
+
+void MotorController::set_controller(P_controller& new_controller)
+{
+    controller = &new_controller;
+
+    controller->reset();
+}
+
+void MotorController::to_stopped()
+{
+    transition_to(stopped_state);
+}
+
+bool MotorController::fault_active()
+{
+    return motor.fault_active();
+}
+
+void MotorController::led_on()
+{
+   led.set_hi();
+}
+
+
+void MotorController::led_off()
+{
+   led.set_lo();
+}

src/motor_driver.cpp

@@ -0,0 +1,130 @@
+#include "motor_driver.h"
+
+#include <avr/io.h>
+
+
+// Í Fast PWM gefur OCR = 0 samt einn stuttan púls í hverri lotu.
+// Til að pinni sé alveg LOW er hann aftengdur frá timernum (COM-bitar)
+// og keyrður LOW gegnum PORT-registerið.
+
+static void ain1_off()   // D3 = PD3 = OC2B
+{
+    TCCR2A &= ~(1 << COM2B1);
+    PORTD  &= ~(1 << PORTD3);
+    OCR2B = 0;
+}
+
+static void ain2_off()   // D11 = PB3 = OC2A
+{
+    TCCR2A &= ~(1 << COM2A1);
+    PORTB  &= ~(1 << PORTB3);
+    OCR2A = 0;
+}
+
+static void ain1_pwm(uint8_t duty)
+{
+    OCR2B = duty;
+    TCCR2A |= (1 << COM2B1);
+}
+
+static void ain2_pwm(uint8_t duty)
+{
+    OCR2A = duty;
+    TCCR2A |= (1 << COM2A1);
+}
+
+
+MotorDriver::MotorDriver()
+{
+}
+
+
+void MotorDriver::begin()
+{
+    // Arduino D3  = PD3 = AIN1
+    // Arduino D11 = PB3 = AIN2
+    // Arduino D4  = PD4 = FLT 
+
+    DDRD |= (1 << DDD3);
+    DDRB |= (1 << DDB3);
+
+    DDRD &= ~(1 << DDD4); 
+
+    PORTD |= (1 << PORTD4); // Enable pull-up resistor on D4 for part 2
+
+
+    // Timer2 Fast PWM
+    TCCR2A = 0;
+    TCCR2B = 0;
+
+    // Fast PWM mode
+    TCCR2A |= (1 << WGM20);
+    TCCR2A |= (1 << WGM21);
+
+    // Timer2 prescaler = 32
+    TCCR2B |= (1 << CS21);
+    TCCR2B |= (1 << CS20);
+
+    // PWM-úttök eru tengd í set() eftir þörfum
+    stop();
+}
+
+
+void MotorDriver::forward()
+{
+    set(255.0);
+}
+
+
+void MotorDriver::reverse()
+{
+    set(-255.0);
+}
+
+
+void MotorDriver::stop()
+{
+    // Both LOW
+    ain1_off();
+    ain2_off();
+}
+
+void MotorDriver::set(double pwm)
+{
+    // Limit PWM command
+    if (pwm > 255.0)
+    {
+        pwm = 255.0;
+    }
+
+    if (pwm < -255.0)
+    {
+        pwm = -255.0;
+    }
+
+
+    uint8_t duty = (uint8_t)(pwm > 0 ? pwm : -pwm);
+
+    if (duty == 0)
+    {
+        stop();
+    }
+    else if (pwm > 0)
+    {
+        // Forward
+        ain2_off();
+        ain1_pwm(duty);
+    }
+    else
+    {
+        // Reverse
+        ain1_off();
+        ain2_pwm(duty);
+    }
+}
+
+
+bool MotorDriver::fault_active()
+{
+    return !(PIND & (1 << PIND4));
+}

src/operational_state.cpp

@@ -0,0 +1,83 @@
+#include "operational_state.h"
+#include "motor_controller.h"
+
+#include <Arduino.h>
+
+
+void OperationalState::enter()
+{
+    Serial.println("State: Operational");
+
+    context->reset_controller();
+
+    context->led_on();
+
+    sample_count = 0;
+
+#if LOG_CSV
+    Serial.println("time_ms,ref_rpm,actual_rpm,pwm");
+#endif
+}
+
+// Part 1 
+
+//void OperationalState::update()
+//{
+//    context->run_speed_controller();
+//}
+
+// Part 2
+
+void OperationalState::update()
+{
+    if (context->fault_active())
+    {
+        Serial.println("Fault detected");
+
+        context->to_stopped();
+
+        return;
+    }
+
+    if (context->run_speed_controller())
+    {
+        sample_count++;
+
+#if LOG_CSV
+        // 4 ms á milli sýna, tíminn byrjar á 0 við hverja innkomu í Operational
+        Serial.print(sample_count * 4UL);
+        Serial.print(',');
+        Serial.print(context->get_reference_rpm(), 1);
+        Serial.print(',');
+        Serial.print(context->get_actual_rpm(), 2);
+        Serial.print(',');
+        Serial.println(context->get_pwm(), 1);
+#endif
+    }
+}
+
+
+void OperationalState::command(char c)
+{
+    if (c == 'r' || c == 'R')
+    {
+        Serial.println("Command: reset");
+
+        context->to_initialization();
+    }
+
+    else if (c == 'p' || c == 'P')
+    {
+        Serial.println("Command: set pre-operational");
+
+        context->to_pre_operational();
+    }
+}
+
+
+void OperationalState::exit()
+{
+    context->stop_motor();
+
+    context->led_off();
+}

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/pi_controller.cpp

@@ -0,0 +1,37 @@
+#include "pi_controller.h"
+
+PI_controller::PI_controller(double Kp, double Ti, double T) : P_controller(Kp), Ti(Ti), T(T), integral(0.0)
+{
+}
+
+double PI_controller::update(double ref, double actual)
+{
+    double e = ref - actual;
+    double new_integral = integral + e * T;
+    double u = Kp * (e + new_integral / Ti);
+
+    if (u > U_MAX)
+    {
+        u = U_MAX;
+    }
+    else if (u < U_MIN)
+    {
+        u = U_MIN;
+    }
+    else
+    {
+        integral = new_integral;
+    }
+    return u;
+    // return 0.0;
+}
+
+void PI_controller::reset()
+{
+    integral = 0.0;
+}
+
+void PI_controller::set_Ti(double value)
+{
+    Ti = value;
+}

src/pre_operational_state.cpp

@@ -0,0 +1,161 @@
+#include "pre_operational_state.h"
+#include "motor_controller.h"
+
+#include <Arduino.h>
+#include <stdlib.h>
+
+PreOperationalState::PreOperationalState(P_controller& p_controller, P_controller& pi_controller)
+    : p_controller(p_controller),
+      pi_controller(pi_controller),
+      pi_selected(true),
+      previous_time(0),
+      led_state(false),
+      //waiting_for_kp(false),
+      //waiting_for_ti(false),
+      input_mode(NONE),
+      input_index(0)
+{
+}
+
+void PreOperationalState::enter()
+{
+    Serial.println("State: Pre-operational");
+
+    context->stop_motor();
+
+    context->led_off();
+
+    led_state = false;
+
+    previous_time = millis();
+}
+
+void PreOperationalState::update()
+{
+    unsigned long current_time = millis();
+
+    if (current_time - previous_time >= 500)
+    {
+        previous_time = current_time;
+
+        led_state = !led_state;
+
+        if (led_state)
+        {
+            context->led_on();
+        }
+        else
+        {
+            context->led_off();
+        }
+    }
+}
+
+void PreOperationalState::command(char c)
+{
+    // We are currently entering a Kp or Ti value
+    if (input_mode != NONE)
+    {
+        // Enter pressed
+        if (c == '\n' || c == '\r')
+        {
+            if (input_index > 0)
+            {
+                // End the string
+                input_buffer[input_index] = '\0';
+
+                // Convert text to a number
+                double value = atof(input_buffer);
+
+                if (input_mode == ENTER_KP)
+                {
+                    context->set_Kp(value);
+
+                    Serial.print("Kp set to: ");
+                    Serial.println(value);
+                }
+                else if (input_mode == ENTER_TI)
+                {
+                    context->set_Ti(value);
+
+                    Serial.print("Ti set to: ");
+                    Serial.println(value);
+                }
+            }
+
+            // Finished entering a value
+            input_index = 0;
+            input_mode = NONE;
+
+            return;
+        }
+
+        // Store number characters
+        if ((c >= '0' && c <= '9') ||
+            c == '.' ||
+            c == '-')
+        {
+            if (input_index < sizeof(input_buffer) - 1)
+            {
+                input_buffer[input_index] = c;
+                input_index++;
+            }
+        }
+
+        return;
+    }
+
+
+    // Normal state commands
+    if (c == 'o' || c == 'O')
+    {
+        Serial.println("Command: set operational");
+
+        context->to_operational();
+    }
+
+    else if (c == 'r' || c == 'R')
+    {
+        Serial.println("Command: reset");
+
+        context->to_initialization();
+    }
+
+    else if (c == 'k' || c == 'K')
+    {
+        input_mode = ENTER_KP;
+        input_index = 0;
+
+        Serial.println("Enter Kp and press Enter:");
+    }
+
+    else if (c == 't' || c == 'T')
+    {
+        input_mode = ENTER_TI;
+        input_index = 0;
+
+        Serial.println("Enter Ti and press Enter:");
+    }
+
+    // Select controller algorithm, the controller is passed to the motor controller
+    else if (c == 'c' || c == 'C')
+    {
+        pi_selected = !pi_selected;
+
+        if (pi_selected)
+        {
+            context->set_controller(pi_controller);
+            Serial.println("Controller: PI");
+        }
+        else
+        {
+            context->set_controller(p_controller);
+            Serial.println("Controller: P");
+        }
+    }
+}
+
+void PreOperationalState::exit()
+{
+    context->led_off();
+}

src/rotary_encoder_driver.cpp

@@ -0,0 +1,132 @@
+#include "rotary_encoder_driver.h"
+#include <avr/interrupt.h>
+
+static RotaryEncoderDriver* driver = nullptr;
+
+RotaryEncoderDriver::RotaryEncoderDriver()
+    : count(0)
+    , delta(0)
+    , speed_ready(false)
+    , hist_idx(0)
+    , eA(2)
+    , eB(5)
+{
+    for (uint8_t i = 0; i < HIST_LEN; i++)
+    {
+        history[i] = 0;
+    }
+}
+
+
+void RotaryEncoderDriver::begin()
+{
+    driver = this;
+
+    eA.init();
+    eB.init();
+
+    EICRA |= (1<<ISC01) | (1<<ISC00);
+    EIMSK |= (1<<INT0);
+
+    timer.init(SAMPLE_PERIOD_US);
+}
+
+void RotaryEncoderDriver::update()
+{
+    count += eB.is_hi() ? 1 : -1;
+}
+
+int32_t RotaryEncoderDriver::position()
+{
+
+    uint8_t oldSREG = SREG;
+
+    cli();
+
+    int32_t local_count = count;
+
+    SREG = oldSREG;
+
+    return local_count;
+}
+
+void RotaryEncoderDriver::update_delta()
+{
+
+    int32_t oldest = history[hist_idx];
+
+    history[hist_idx] = count;
+
+    hist_idx = (hist_idx + 1) % HIST_LEN;
+
+    delta = (int16_t)(count - oldest);   // breyting yfir HIST_LEN * Ts (16 ms) glugga
+
+    speed_ready = true;
+}
+
+float RotaryEncoderDriver::count_per_sec()
+{
+
+    uint8_t oldSREG = SREG;
+
+    cli();
+
+    int16_t local_delta = delta;
+
+    SREG = oldSREG;
+
+    return local_delta / (HIST_LEN * SAMPLE_PERIOD_US * 1e-6f);
+}
+
+
+float RotaryEncoderDriver::rpm()
+{
+    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();
+
+    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();
+}
+ISR(TIMER1_COMPA_vect)
+{
+    driver->update_delta();
+}

src/stopped_state.cpp

@@ -0,0 +1,81 @@
+#include "stopped_state.h"
+#include "motor_controller.h"
+
+#include <Arduino.h>
+
+
+StoppedState::StoppedState()
+    : previous_time(0),
+      led_state(false)
+{
+}
+
+
+void StoppedState::enter()
+{
+    Serial.println("State: Stopped");
+
+    context->stop_motor();
+
+    led_state = false;
+
+    context->led_off();
+
+    previous_time = millis();
+}
+
+// Controlling the blinking of the LED @ 2Hz
+
+void StoppedState::update()
+{
+    unsigned long current_time = millis();
+
+    if (current_time - previous_time >= 250)
+    {
+        previous_time = current_time;
+
+        led_state = !led_state;
+
+        if (led_state)
+        {
+            context->led_on();
+        }
+        else
+        {
+            context->led_off();
+        }
+    }
+}
+
+
+// Commands for Stopped [ r = reset and o for operational]
+
+void StoppedState::command(char c)
+{
+    if (c == 'r' || c == 'R')
+    {
+        Serial.println("Command: reset");
+
+        context->to_initialization();
+    }
+
+    else if (c == 'o' || c == 'O')
+    {
+        Serial.println("Command: set operational");
+
+        context->to_operational();
+    }
+
+    else if (c == 'p' || c == 'P')
+    {
+        Serial.println("Command: set pre-operational");
+
+        context->to_pre_operational();
+    }
+}
+
+
+void StoppedState::exit()
+{
+    context->led_off();
+}

src/timer_usec.cpp

@@ -0,0 +1,14 @@
+#include "timer_usec.h"
+
+Timer_usec::Timer_usec() = default;
+
+void Timer_usec::init(uint16_t period_us) { // for custom duty cycle duration in ms in part 1
+    TCCR1A = 0;
+    TCCR1B = 0;
+    TCNT1 = 0;
+
+    OCR1A = period_us * 2 - 1;
+    TCCR1B |= (1<<WGM12);
+    TIMSK1 |= (1<<OCIE1A);
+    TCCR1B |= (1<<CS11);
+}