From 53091f68dfc2ec68f396e9ea9eff1ca1610b5b48 Mon Sep 17 00:00:00 2001 From: isakg22 Date: Thu, 8 Oct 2026 13:11:39 +0000 Subject: [PATCH] =?UTF-8?q?Setti=20inn=20allar=20skr=C3=A1r=20fr=C3=A1=20P?= =?UTF-8?q?roject=203=20og=20b=C3=A6tti=20vi=C3=B0=20overlay=20=C3=AD=20sa?= =?UTF-8?q?mr=C3=A6mi=20vi=C3=B0=20controlloerinn=20og=20pin=20out.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- docs/diagrams/src/motor_state.wsd | 50 +++++++ include/digital_in.h | 14 ++ include/digital_out.h | 15 ++ include/initialization_state.h | 24 ++++ include/motor_controller.h | 95 +++++++++++++ include/motor_driver.h | 22 +++ include/operational_state.h | 29 ++++ include/p_controller.h | 26 ++++ include/pi_controller.h | 23 ++++ include/pre_operational_state.h | 43 ++++++ include/rotary_encoder_driver.h | 49 +++++++ include/state.h | 32 +++++ include/stopped_state.h | 25 ++++ include/timer_usec.h | 9 ++ src/digital_in.cpp | 14 ++ src/digital_out.cpp | 9 ++ src/initialization_state.cpp | 41 ++++++ src/main.cpp | 84 +++++++++++ src/motor_controller.cpp | 191 ++++++++++++++++++++++++++ src/motor_driver.cpp | 130 ++++++++++++++++++ src/operational_state.cpp | 83 +++++++++++ src/p_controller.cpp | 13 ++ src/pi_controller.cpp | 37 +++++ src/pre_operational_state.cpp | 161 ++++++++++++++++++++++ src/rotary_encoder_driver.cpp | 132 ++++++++++++++++++ src/stopped_state.cpp | 81 +++++++++++ src/timer_usec.cpp | 14 ++ test/test_pi_controller/test_main.cpp | 150 ++++++++++++++++++++ zephyr/boards/nucleo_l432kc.overlay | 49 +++++++ 29 files changed, 1645 insertions(+) create mode 100644 docs/diagrams/src/motor_state.wsd create mode 100644 include/digital_in.h create mode 100644 include/digital_out.h create mode 100644 include/initialization_state.h create mode 100644 include/motor_controller.h create mode 100644 include/motor_driver.h create mode 100644 include/operational_state.h create mode 100644 include/p_controller.h create mode 100644 include/pi_controller.h create mode 100644 include/pre_operational_state.h create mode 100644 include/rotary_encoder_driver.h create mode 100644 include/state.h create mode 100644 include/stopped_state.h create mode 100644 include/timer_usec.h create mode 100644 src/digital_in.cpp create mode 100644 src/digital_out.cpp create mode 100644 src/initialization_state.cpp create mode 100644 src/main.cpp create mode 100644 src/motor_controller.cpp create mode 100644 src/motor_driver.cpp create mode 100644 src/operational_state.cpp create mode 100644 src/p_controller.cpp create mode 100644 src/pi_controller.cpp create mode 100644 src/pre_operational_state.cpp create mode 100644 src/rotary_encoder_driver.cpp create mode 100644 src/stopped_state.cpp create mode 100644 src/timer_usec.cpp create mode 100644 test/test_pi_controller/test_main.cpp create mode 100644 zephyr/boards/nucleo_l432kc.overlay diff --git a/docs/diagrams/src/motor_state.wsd b/docs/diagrams/src/motor_state.wsd new file mode 100644 index 0000000..6f04fa7 --- /dev/null +++ b/docs/diagrams/src/motor_state.wsd @@ -0,0 +1,50 @@ +@startuml +' skinparam linetype ortho +' skinparam nodesep 100 +skinparam ranksep 80 + +skinparam state { + BackgroundColor LightBlue +} + +[*] --> Initialization + +state Initialization + Initialization : Entry / Stop motor + Initialization : Entry / Initialize internal parameters + Initialization : Entry / Reset speed measurement + Initialization : Do / Wait 2 seconds + Initialization : Exit / Send Boot-up message + +state PreOperational + PreOperational : Entry / Stop motor + PreOperational : Do / Blink LED at 1 Hz + PreOperational : Do / Accept Kp and Ti configuration + PreOperational : Do / Select P or PI controller [c] + PreOperational : Exit / LED off + +state Operational + Operational : Entry / LED on + Operational : Do / Run speed controller + Operational : Exit / Stop motor and LED off + +state Stopped + Stopped : Entry / Stop motor + Stopped : Do / Blink LED at 2 Hz + Stopped : Exit / LED off + +Initialization -r-> PreOperational : initialization\n complete +' Initialization -d-> Operational : initialization\n complete + +PreOperational -d-> Operational : set operational [o] +PreOperational -l--> Initialization : reset [r] + +Operational -u-> PreOperational : set pre-operational [p] +Operational -> Initialization : reset [r] +Operational --> Stopped : fault detected + +Stopped -l-> Initialization : reset [r] +Stopped -> Operational : set operational [o] +Stopped -u-> PreOperational : set pre-operational [p] + +@enduml diff --git a/include/digital_in.h b/include/digital_in.h new file mode 100644 index 0000000..0f62f9c --- /dev/null +++ b/include/digital_in.h @@ -0,0 +1,14 @@ +#ifndef DIGITAL_IN_H +#define DIGITAL_IN_H +#include + +struct Digital_in { + uint8_t pin_mask; + + Digital_in(uint8_t pin); + + void init(); + bool is_hi(); + bool is_lo(); +}; +#endif diff --git a/include/digital_out.h b/include/digital_out.h new file mode 100644 index 0000000..7ed8229 --- /dev/null +++ b/include/digital_out.h @@ -0,0 +1,15 @@ +#ifndef DIGITAL_OUT_H +#define DIGITAL_OUT_H +#include + +struct Digital_out { + uint8_t pin_mask; + + Digital_out(uint8_t pin); + + void init(); + void toggle(); + void set_hi(); + void set_lo(); +}; +#endif diff --git a/include/initialization_state.h b/include/initialization_state.h new file mode 100644 index 0000000..01cc28c --- /dev/null +++ b/include/initialization_state.h @@ -0,0 +1,24 @@ +#ifndef INITIALIZATION_STATE_H +#define INITIALIZATION_STATE_H + +#include "state.h" + + +class InitializationState : public State +{ +private: + static const unsigned long WAIT_MS = 2000; + + unsigned long enter_time_ms = 0; + +public: + void enter() override; + + void update() override; + + void command(char c) override; + + void exit() override; +}; + +#endif \ No newline at end of file diff --git a/include/motor_controller.h b/include/motor_controller.h new file mode 100644 index 0000000..f81ec22 --- /dev/null +++ b/include/motor_controller.h @@ -0,0 +1,95 @@ +#ifndef MOTOR_CONTROLLER_H +#define MOTOR_CONTROLLER_H + +#include "rotary_encoder_driver.h" +#include "motor_driver.h" +#include "p_controller.h" +#include "digital_out.h" + +class State; + + +class MotorController +{ +private: + static constexpr double DEFAULT_REFERENCE_RPM = 40.0; + static constexpr double PWM_PER_PERCENT = 255.0 / 100.0; + static const uint8_t LED_PIN = 5; // PB5 = D13 = LED_BUILTIN + + State* current_state; + + State& initialization_state; + State& pre_operational_state; + State& operational_state; + State& stopped_state; + + RotaryEncoderDriver& encoder; + MotorDriver& motor; + P_controller* controller; + + Digital_out led; + + double reference_rpm; + + // Síðasta sýni, notað fyrir CSV-útprentun + double last_actual_rpm; + double last_pwm; + + +public: + MotorController( + RotaryEncoderDriver& encoder, + MotorDriver& motor, + P_controller& controller, + State& initialization, + State& pre_operational, + State& operational, + State& stopped + ); + + + void begin(); + + void update(); + + void command(char c); + + void transition_to(State& new_state); + + + void to_initialization(); + + void to_pre_operational(); + + void to_operational(); + + void to_stopped(); + + bool fault_active(); + + + // Skilar true þegar nýtt sýni var keyrt í gegnum stýringuna + bool run_speed_controller(); + + double get_reference_rpm() const { return reference_rpm; } + double get_actual_rpm() const { return last_actual_rpm; } + double get_pwm() const { return last_pwm; } + + void stop_motor(); + + void reset_speed_measurement(); + + void reset_controller(); + + void set_Kp(double value); + + void set_Ti(double value); + + void set_controller(P_controller& new_controller); + + void led_on(); + + void led_off(); +}; + +#endif diff --git a/include/motor_driver.h b/include/motor_driver.h new file mode 100644 index 0000000..9b4e54b --- /dev/null +++ b/include/motor_driver.h @@ -0,0 +1,22 @@ +#ifndef MOTOR_DRIVER_H +#define MOTOR_DRIVER_H + +class MotorDriver +{ +public: + MotorDriver(); + + void begin(); + + void forward(); + + void reverse(); + + void stop(); + + void set(double pwm); + + bool fault_active(); // Fault detection for part 2 +}; + +#endif \ No newline at end of file diff --git a/include/operational_state.h b/include/operational_state.h new file mode 100644 index 0000000..f213ce9 --- /dev/null +++ b/include/operational_state.h @@ -0,0 +1,29 @@ +#ifndef OPERATIONAL_STATE_H +#define OPERATIONAL_STATE_H + +#include "state.h" +#include + +// Sett á 1 til að prenta CSV (time_ms,ref_rpm,actual_rpm,pwm) í Operational, +// t.d. fyrir step og load response. Einnig hægt með build_flags = -D LOG_CSV=1 +#ifndef LOG_CSV +#define LOG_CSV 0 +#endif + + +class OperationalState : public State +{ +private: + uint32_t sample_count = 0; + +public: + void enter() override; + + void update() override; + + void command(char c) override; + + void exit() override; +}; + +#endif \ No newline at end of file diff --git a/include/p_controller.h b/include/p_controller.h new file mode 100644 index 0000000..b94f1ae --- /dev/null +++ b/include/p_controller.h @@ -0,0 +1,26 @@ +#ifndef P_CONTROLLER_H +#define P_CONTROLLER_H + +class P_controller +{ +protected: + double Kp; + +public: + P_controller(double Kp); + virtual ~P_controller() {} + virtual double update(double ref, double actual); + virtual void reset() {} + + virtual void set_Kp(double value) + { + Kp = value; + } + + virtual void set_Ti(double value) + { + // P controller does not use Ti + } +}; + +#endif diff --git a/include/pi_controller.h b/include/pi_controller.h new file mode 100644 index 0000000..bec8645 --- /dev/null +++ b/include/pi_controller.h @@ -0,0 +1,23 @@ +#ifndef PI_CONTROLLER_H +#define PI_CONTROLLER_H + +#include "p_controller.h" + +class PI_controller : public P_controller +{ + private: + static constexpr double U_MAX = 100.0; // % + static constexpr double U_MIN = -100.0; // % + + double Ti; + double T; + double integral; + public: + PI_controller(double Kp, double Ti, double T); + double update(double ref, double actual) override; + void reset() override; + + void set_Ti(double value) override; +}; + +#endif diff --git a/include/pre_operational_state.h b/include/pre_operational_state.h new file mode 100644 index 0000000..7ad5cc6 --- /dev/null +++ b/include/pre_operational_state.h @@ -0,0 +1,43 @@ +#ifndef PRE_OPERATIONAL_STATE_H +#define PRE_OPERATIONAL_STATE_H + +#include "state.h" +#include "stdint.h" + +class P_controller; + +class PreOperationalState : public State +{ +private: + P_controller& p_controller; + P_controller& pi_controller; + bool pi_selected; + + unsigned long previous_time; + bool led_state; + + //bool waiting_for_kp; + //bool waiting_for_ti; + + enum InputMode + { + NONE, + ENTER_KP, + ENTER_TI + }; + + InputMode input_mode; + + char input_buffer[16]; + uint8_t input_index; + +public: + PreOperationalState(P_controller& p_controller, P_controller& pi_controller); + + void enter() override; + void update() override; + void command(char c) override; + void exit() override; +}; + +#endif \ No newline at end of file diff --git a/include/rotary_encoder_driver.h b/include/rotary_encoder_driver.h new file mode 100644 index 0000000..ef1ed44 --- /dev/null +++ b/include/rotary_encoder_driver.h @@ -0,0 +1,49 @@ +#ifndef ROTARY_ENCODER_DRIVER_H +#define ROTARY_ENCODER_DRIVER_H + +#include + +#include "digital_in.h" +#include "timer_usec.h" + +class RotaryEncoderDriver { +public: + static constexpr uint16_t SAMPLE_PERIOD_US = 4000; // Ts = 4 ms + static constexpr float COUNTS_PER_REV = 1050.0f; + +private: + static const uint8_t HIST_LEN = 4; // 4 x Ts = 16 ms gluggi fyrir rpm-utreikning + + volatile int32_t count; + volatile int16_t delta; + volatile bool speed_ready; + + volatile int32_t history[HIST_LEN]; + volatile uint8_t hist_idx; + + Digital_in eA; + Digital_in eB; + + Timer_usec timer; + +public: + RotaryEncoderDriver(); + + void begin(); + + void update(); + + int32_t position(); + + void update_delta(); + + float count_per_sec(); + + float rpm(); + + bool new_speed_available(); + + void reset_speed_measurement(); + +}; +#endif diff --git a/include/state.h b/include/state.h new file mode 100644 index 0000000..2707215 --- /dev/null +++ b/include/state.h @@ -0,0 +1,32 @@ +#ifndef STATE_H +#define STATE_H + +class MotorController; + +class State +{ +protected: + MotorController* context = nullptr; + +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/include/stopped_state.h b/include/stopped_state.h new file mode 100644 index 0000000..b42c686 --- /dev/null +++ b/include/stopped_state.h @@ -0,0 +1,25 @@ +#ifndef STOPPED_STATE_H +#define STOPPED_STATE_H + +#include "state.h" + +class StoppedState : public State +{ +private: + unsigned long previous_time; // Used for blinking LED + bool led_state; // Ditto + +public: + StoppedState(); + + void enter() override; + + void update() override; + + void command(char c) override; + + void exit() override; + +}; + +#endif diff --git a/include/timer_usec.h b/include/timer_usec.h new file mode 100644 index 0000000..660e9b0 --- /dev/null +++ b/include/timer_usec.h @@ -0,0 +1,9 @@ +#ifndef TIMER_USEC_H +#define TIMER_USEC_H +#include + +struct Timer_usec { + Timer_usec(); + void init(uint16_t period_us); +}; +#endif diff --git a/src/digital_in.cpp b/src/digital_in.cpp new file mode 100644 index 0000000..5735eaf --- /dev/null +++ b/src/digital_in.cpp @@ -0,0 +1,14 @@ +#include +#include "digital_in.h" + +Digital_in::Digital_in(uint8_t pin) : pin_mask(1< + +Digital_out::Digital_out(uint8_t pin) : pin_mask(1< + + +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"); +} diff --git a/src/main.cpp b/src/main.cpp new file mode 100644 index 0000000..4dc9246 --- /dev/null +++ b/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 +#include + +#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 diff --git a/src/motor_controller.cpp b/src/motor_controller.cpp new file mode 100644 index 0000000..9d31ea1 --- /dev/null +++ b/src/motor_controller.cpp @@ -0,0 +1,191 @@ +#include "motor_controller.h" +#include "state.h" + +#include + + +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(); +} diff --git a/src/motor_driver.cpp b/src/motor_driver.cpp new file mode 100644 index 0000000..6b841a7 --- /dev/null +++ b/src/motor_driver.cpp @@ -0,0 +1,130 @@ +#include "motor_driver.h" + +#include + + +// Í 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)); +} diff --git a/src/operational_state.cpp b/src/operational_state.cpp new file mode 100644 index 0000000..59c6673 --- /dev/null +++ b/src/operational_state.cpp @@ -0,0 +1,83 @@ +#include "operational_state.h" +#include "motor_controller.h" + +#include + + +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(); +} \ No newline at end of file diff --git a/src/p_controller.cpp b/src/p_controller.cpp new file mode 100644 index 0000000..616207b --- /dev/null +++ b/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; +} \ No newline at end of file diff --git a/src/pi_controller.cpp b/src/pi_controller.cpp new file mode 100644 index 0000000..c2e980f --- /dev/null +++ b/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; +} diff --git a/src/pre_operational_state.cpp b/src/pre_operational_state.cpp new file mode 100644 index 0000000..3bb4342 --- /dev/null +++ b/src/pre_operational_state.cpp @@ -0,0 +1,161 @@ +#include "pre_operational_state.h" +#include "motor_controller.h" + +#include +#include + +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(); +} \ No newline at end of file diff --git a/src/rotary_encoder_driver.cpp b/src/rotary_encoder_driver.cpp new file mode 100644 index 0000000..2c75a4b --- /dev/null +++ b/src/rotary_encoder_driver.cpp @@ -0,0 +1,132 @@ +#include "rotary_encoder_driver.h" +#include + +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<update(); +} +ISR(TIMER1_COMPA_vect) +{ + driver->update_delta(); +} diff --git a/src/stopped_state.cpp b/src/stopped_state.cpp new file mode 100644 index 0000000..421d3bd --- /dev/null +++ b/src/stopped_state.cpp @@ -0,0 +1,81 @@ +#include "stopped_state.h" +#include "motor_controller.h" + +#include + + +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(); +} \ No newline at end of file diff --git a/src/timer_usec.cpp b/src/timer_usec.cpp new file mode 100644 index 0000000..762359f --- /dev/null +++ b/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< + +// #ifdef ARDUINO +#include +// #endif + +#include "p_controller.h" +#include "pi_controller.h" + +static const double KP = 2.0; +static const double TI = 1.0; +static const double T = 0.1; +static const double TOL = 1e-4; // double er 32-bita float a AVR + +// void setUp(void) {} +// void tearDown(void) {} + +// Normal flow: engin villa -> ekkert úttak +void test_zero_error_gives_zero_output(void) +{ + PI_controller c(KP, TI, T); + TEST_ASSERT_FLOAT_WITHIN(TOL, 0.0, c.update(10.0, 10.0)); +} + +// Normal flow: fyrsta skref u = Kp * (e + e*T/Ti) +void test_first_step(void) +{ + PI_controller c(KP, TI, T); + double e = 5.0; + TEST_ASSERT_FLOAT_WITHIN(TOL, KP * (e + e * T / TI), c.update(e, 0.0)); +} + +// Normal flow: stöðug villa safnast upp, u_n = Kp * (e + n*e*T/Ti) +void test_integral_accumulates(void) +{ + PI_controller c(KP, TI, T); + double e = 5.0; + double u = 0.0; + for (int n = 1; n <= 5; n++) + { + u = c.update(e, 0.0); + TEST_ASSERT_FLOAT_WITHIN(TOL, KP * (e + n * e * T / TI), u); + } +} + +// Normal flow: þegar villan er orðin 0 heldur integralið úttakinu uppi +void test_output_held_when_error_zero(void) +{ + PI_controller c(KP, TI, T); + double e = 5.0; + for (int n = 0; n < 5; n++) + { + c.update(e, 0.0); + } + double E = 5 * e * T; + TEST_ASSERT_FLOAT_WITHIN(TOL, KP * E / TI, c.update(0.0, 0.0)); +} + +// Alt flow: villa langt utan getu -> mettun í +100 % +void test_saturates_high(void) +{ + PI_controller c(KP, TI, T); + TEST_ASSERT_FLOAT_WITHIN(TOL, 100.0, c.update(10000.0, 0.0)); +} + +// Alt flow: mettun í -100 % +void test_saturates_low(void) +{ + PI_controller c(KP, TI, T); + TEST_ASSERT_FLOAT_WITHIN(TOL, -100.0, c.update(-10000.0, 0.0)); +} + +// Alt flow: anti-windup. Eftir langa mettun á lítil neikvæð villa +// strax að gefa úttak undir mettun (integralið óx ekki) +void test_no_integral_windup(void) +{ + PI_controller c(KP, TI, T); + for (int n = 0; n < 1000; n++) + { + c.update(10000.0, 0.0); + } + double u = c.update(0.0, 1.0); // e = -1 + TEST_ASSERT_TRUE(u < 0.0); +} + +// Alt flow: reset (stopped -> operational) hreinsar integralið +void test_reset_clears_integral(void) +{ + PI_controller c(KP, TI, T); + for (int n = 0; n < 5; n++) + { + c.update(5.0, 0.0); + } + c.reset(); + TEST_ASSERT_FLOAT_WITHIN(TOL, 0.0, c.update(0.0, 0.0)); +} + +// Polymorphism: kall í gegnum P_controller& gefur PI hegðun +void test_polymorphic_update(void) +{ + PI_controller pi(KP, TI, T); + P_controller& c = pi; + double e = 5.0; + TEST_ASSERT_FLOAT_WITHIN(TOL, KP * (e + e * T / TI), c.update(e, 0.0)); +} + +// P_controller hegðar sér óbreytt +void test_p_controller_unchanged(void) +{ + P_controller c(KP); + TEST_ASSERT_FLOAT_WITHIN(TOL, KP * 5.0, c.update(5.0, 0.0)); +} + +void setup() +{ + // init(); + delay(2000); + UNITY_BEGIN(); + RUN_TEST(test_zero_error_gives_zero_output); + RUN_TEST(test_first_step); + RUN_TEST(test_integral_accumulates); + RUN_TEST(test_output_held_when_error_zero); + RUN_TEST(test_saturates_high); + RUN_TEST(test_saturates_low); + RUN_TEST(test_no_integral_windup); + RUN_TEST(test_reset_clears_integral); + RUN_TEST(test_polymorphic_update); + RUN_TEST(test_p_controller_unchanged); + // return UNITY_END(); + UNITY_END(); +} + +void loop() {} + +// #ifdef ARDUINO +// void setup() +// { +// delay(2000); // bida eftir serial tengingu eftir reset +// run_tests(); +// } + +// void loop() +// { +// } +// #else +// int main(int argc, char** argv) +// { +// return run_tests(); +// } +// #endif diff --git a/zephyr/boards/nucleo_l432kc.overlay b/zephyr/boards/nucleo_l432kc.overlay new file mode 100644 index 0000000..0e90f33 --- /dev/null +++ b/zephyr/boards/nucleo_l432kc.overlay @@ -0,0 +1,49 @@ +#include +#include + +/ { + aliases { + encoder-c1 = &encoder_c1; + encoder-c2 = &encoder_c2; + + motor-bin1-pwm = &motor_bin1_pwm; + motor-bin2 = &motor_bin2; + motor-flt = &motor_flt; + motor-slp = &motor_slp; + }; + + encoder_c1: encoder_c1 { + gpios = <&gpiob 7 GPIO_ACTIVE_HIGH>; /* D4, PB7 */ + }; + + encoder_c2: encoder_c2 { + gpios = <&gpiob 6 GPIO_ACTIVE_HIGH>; /* D5, PB6 */ + }; + + motor_bin1_pwm: motor_bin1_pwm { + pwms = <&pwm1 1 PWM_USEC(50) 0>; /* D9, PA8; 20 kHz */ + }; + + motor_bin2: motor_bin2 { + gpios = <&gpiob 1 GPIO_ACTIVE_HIGH>; /* D6, PB1 */ + }; + + motor_flt: motor_flt { + gpios = <&gpioa 0 (GPIO_ACTIVE_LOW | GPIO_PULL_UP)>; /* A0, PA0 */ + }; + + motor_slp: motor_slp { + gpios = <&gpioa 1 GPIO_ACTIVE_HIGH>; /* A1, PA1 */ + }; +}; + +&timers1 { + status = "okay"; + st,prescaler = <79>; + + pwm1: pwm { + status = "okay"; + pinctrl-0 = <&tim1_ch1_pa8>; + pinctrl-names = "default"; + }; +}; \ No newline at end of file