From 4d3a1c0bbcb879beb73b272dd7442fd9852723f9 Mon Sep 17 00:00:00 2001 From: nokkvinetti <82093782+nokkvinetti@users.noreply.github.com> Date: Thu, 8 Oct 2026 12:47:23 +0000 Subject: [PATCH] =?UTF-8?q?Setti=20up=20basic=20=C3=BEarf=20samt=20a=C3=B0?= =?UTF-8?q?=20laga=20svo=20virki?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../README.md | 38 ++ .../docs/diagrams/part1_state.wsd | 21 + .../docs/diagrams/part2_state.wsd | 30 ++ .../docs/part3_test_plan.md | 23 + .../include/README | 37 ++ .../include/context.h | 32 ++ .../include/control_timer.h | 17 + .../include/controller.h | 19 + .../include/digital_in.h | 22 + .../include/digital_out.h | 23 + .../include/encoder.h | 28 ++ .../include/initialization.h | 35 ++ .../include/motor_driver.h | 26 ++ .../include/operational.h | 52 +++ .../include/p_controller.h | 36 ++ .../include/pi_controller.h | 42 ++ .../include/pre_operational.h | 63 +++ .../include/speed_controller.h | 47 ++ .../include/speed_estimator.h | 37 ++ .../include/state.h | 45 ++ .../include/stopped.h | 51 ++ .../lib/README | 46 ++ .../platformio.ini | 17 + .../src/A4.1main.cpp | 98 ++++ .../src/context.cpp | 109 +++++ .../src/control_timer.cpp | 70 +++ .../src/digital_in.cpp | 26 ++ .../src/digital_out.cpp | 27 ++ .../src/encoder.cpp | 75 +++ .../src/initialization.cpp | 61 +++ .../src/motor_driver.cpp | 135 ++++++ .../src/operational.cpp | 143 ++++++ .../src/p_controller.cpp | 63 +++ .../src/pi_controller.cpp | 141 ++++++ .../src/pre_operational.cpp | 225 +++++++++ .../src/project3main.cpp | 427 +++++++++++++++++ .../src/speed_controller.cpp | 106 +++++ .../src/speed_estimator.cpp | 76 +++ .../src/stopped.cpp | 112 +++++ .../test/README | 11 + .../test/test_pi_controller/test_main.cpp | 438 ++++++++++++++++++ .../zephyr/CMakeLists.txt | 6 + .../zephyr/boards/nucleo_l432kc.overlay | 37 ++ .../zephyr/prj.conf | 2 + 44 files changed, 3175 insertions(+) create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/README.md create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/docs/diagrams/part1_state.wsd create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/docs/diagrams/part2_state.wsd create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/docs/part3_test_plan.md create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/README create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/context.h create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/control_timer.h create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/controller.h create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/digital_in.h create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/digital_out.h create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/encoder.h create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/initialization.h create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/motor_driver.h create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/operational.h create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/p_controller.h create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/pi_controller.h create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/pre_operational.h create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/speed_controller.h create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/speed_estimator.h create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/state.h create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/stopped.h create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/lib/README create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/platformio.ini create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/A4.1main.cpp create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/context.cpp create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/control_timer.cpp create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/digital_in.cpp create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/digital_out.cpp create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/encoder.cpp create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/initialization.cpp create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/motor_driver.cpp create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/operational.cpp create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/p_controller.cpp create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/pi_controller.cpp create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/pre_operational.cpp create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/project3main.cpp create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/speed_controller.cpp create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/speed_estimator.cpp create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/stopped.cpp create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/test/README create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/test/test_pi_controller/test_main.cpp create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/zephyr/CMakeLists.txt create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/zephyr/boards/nucleo_l432kc.overlay create mode 100644 Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/zephyr/prj.conf diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/README.md b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/README.md new file mode 100644 index 0000000..86d050a --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/README.md @@ -0,0 +1,38 @@ +.. Copyright 2019-present PlatformIO + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + http://www.apache.org/licenses/LICENSE-2.0 + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + +How to build PlatformIO based project +===================================== + +1. [Install PlatformIO Core](https://docs.platformio.org/page/core.html) +2. Download [development platform with examples](https://github.com/platformio/platform-ststm32/archive/develop.zip) +3. Extract ZIP archive +4. Run these commands: + +```shell +# Change directory to example +$ cd platform-ststm32/examples/zephyr-blink + +# Build project +$ pio run + +# Upload firmware +$ pio run --target upload + +# Build specific environment +$ pio run -e blackpill_f103c8 + +# Upload firmware for the specific environment +$ pio run -e blackpill_f103c8 --target upload + +# Clean build files +$ pio run --target clean +``` diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/docs/diagrams/part1_state.wsd b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/docs/diagrams/part1_state.wsd new file mode 100644 index 0000000..ad04a18 --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/docs/diagrams/part1_state.wsd @@ -0,0 +1,21 @@ +@startuml + +[*] --> Initialization + +state Initialization +state Operational + +Initialization : entry / initialize parameters +Initialization : entry / send Boot-up +Initialization : do / wait for command + +Operational : entry / enable motor control +Operational : entry / LED ON +Operational : do / update speed controller at 250 Hz +Operational : exit / stop motor control +Operational : exit / LED OFF + +Initialization --> Operational : set operational / 'o' +Operational --> Initialization : reset / 'r' + +@enduml \ No newline at end of file diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/docs/diagrams/part2_state.wsd b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/docs/diagrams/part2_state.wsd new file mode 100644 index 0000000..0a5400d --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/docs/diagrams/part2_state.wsd @@ -0,0 +1,30 @@ +@startuml + +[*] --> Initialization + +state Initialization +state Operational +state Stopped + +Initialization : entry / initialize parameters +Initialization : entry / send Boot-up +Initialization : do / wait for command + +Operational : entry / enable motor control +Operational : entry / LED ON +Operational : do / update speed controller at 250 Hz +Operational : exit / stop motor control +Operational : exit / LED OFF + +Stopped : entry / brake motor +Stopped : entry / disable normal controller activity +Stopped : do / LED blink at 2 Hz +Stopped : exit / LED OFF + +Initialization --> Operational : set operational / 'o' +Operational --> Initialization : reset / 'r' + +Operational --> Stopped : emergency stop / fault +Stopped --> Initialization : reset / 'r' + +@enduml \ No newline at end of file diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/docs/part3_test_plan.md b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/docs/part3_test_plan.md new file mode 100644 index 0000000..f3a4e3b --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/docs/part3_test_plan.md @@ -0,0 +1,23 @@ +# Part 3 - PI Controller TDD + +## Normal flows + +1. Zero error produces zero output. +2. Positive error produces positive control output. +3. Negative error produces negative control output. +4. Constant error accumulates integral action. +5. The PI controller operates through the common Controller interface. + +## Alternate flows + +1. Positive output is limited to +255. +2. Negative output is limited to -255. +3. Positive saturation does not cause integral windup. +4. Negative saturation does not cause integral windup. +5. The controller recovers correctly after saturation. +6. Reset clears the accumulated integral. + +The tests were written and executed before the PI control law was +implemented. The initial run produced the expected failing tests. +After implementing the PI control law and anti-windup behavior, +the same test suite passed all 11 tests. \ No newline at end of file diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/README b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/README new file mode 100644 index 0000000..49819c0 --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/README @@ -0,0 +1,37 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the convention is to give header files names that end with `.h'. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/context.h b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/context.h new file mode 100644 index 0000000..fb2fe73 --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/context.h @@ -0,0 +1,32 @@ +#ifndef CONTEXT_H +#define CONTEXT_H + +#include + +class Context +{ +public: + Context(State& initialState); + + void transition_to(State& newState); + + void do_work(); + + void reset(); + void set_operational(); + void set_pre_operational(); + void fault(); + + void select_p(); + void select_pi(); + + void set_kp(double value); + void set_ti(double value); + + const char* current_state_name() const; + +private: + State* state; +}; + +#endif \ No newline at end of file diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/control_timer.h b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/control_timer.h new file mode 100644 index 0000000..9b00f4d --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/control_timer.h @@ -0,0 +1,17 @@ +#ifndef CONTROL_TIMER_H +#define CONTROL_TIMER_H + +#include + +class Control_timer +{ +public: + void init(); + + // Returns true once for every 4 ms control tick. + bool take_tick(); + + uint32_t tick_count() const; +}; + +#endif \ No newline at end of file diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/controller.h b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/controller.h new file mode 100644 index 0000000..d71a59a --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/controller.h @@ -0,0 +1,19 @@ +#ifndef CONTROLLER_H +#define CONTROLLER_H + +class Controller +{ +public: + virtual ~Controller() = default; + + virtual double update( + double reference, + double actual + ) = 0; + + virtual void reset() = 0; + + virtual const char* name() const = 0; +}; + +#endif \ No newline at end of file diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/digital_in.h b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/digital_in.h new file mode 100644 index 0000000..2218aad --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/digital_in.h @@ -0,0 +1,22 @@ +#ifndef DIGITAL_IN_H +#define DIGITAL_IN_H + +#include + +class Digital_in +{ +public: + // pin is the PORTB bit number: + // PB0 = D8, PB1 = D9, ..., PB5 = D13 + Digital_in(uint8_t pin); + + void init(); + + bool is_hi() const; + bool is_lo() const; + +private: + uint8_t pinMask; +}; + +#endif \ No newline at end of file diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/digital_out.h b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/digital_out.h new file mode 100644 index 0000000..485541b --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/digital_out.h @@ -0,0 +1,23 @@ +#ifndef DIGITAL_OUT_H +#define DIGITAL_OUT_H + +#include + +class Digital_out +{ +public: + // pin is the PORTB bit number: + // PB0 = D8, ..., PB5 = D13 + Digital_out(uint8_t pin); + + void init(); + + void set_hi(); + void set_lo(); + void toggle(); + +private: + uint8_t pinMask; +}; + +#endif \ No newline at end of file diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/encoder.h b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/encoder.h new file mode 100644 index 0000000..84689a4 --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/encoder.h @@ -0,0 +1,28 @@ +#ifndef ENCODER_H +#define ENCODER_H + +#include +#include + +class Encoder +{ +public: + Encoder(uint8_t c1Pin, uint8_t c2Pin); + + void init(); + void update(); + + int32_t position() const; + +private: + Digital_in c1; + Digital_in c2; + + uint8_t c1Pin; + + bool previousC1; + + volatile int32_t count; +}; + +#endif \ No newline at end of file diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/initialization.h b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/initialization.h new file mode 100644 index 0000000..d3591de --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/initialization.h @@ -0,0 +1,35 @@ +#ifndef INITIALIZATION_H +#define INITIALIZATION_H + +#include +#include +#include + +class Initialization : public State +{ +public: + Initialization( + Speed_controller& speedController, + Digital_out& statusLED + ); + + const char* name() const override; + + void on_entry() override; + void on_do() override; + void on_exit() override; + + void set_pre_operational_state( + State& preOperationalState + ); + +private: + Speed_controller& speedController; + Digital_out& statusLED; + + State* preOperationalState; + + bool transitionPending; +}; + +#endif \ No newline at end of file diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/motor_driver.h b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/motor_driver.h new file mode 100644 index 0000000..9e2c8b4 --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/motor_driver.h @@ -0,0 +1,26 @@ +#ifndef MOTOR_DRIVER_H +#define MOTOR_DRIVER_H + +class Motor_driver +{ +public: + void init(); + + // Range: -255 ... +255 + void set_command(double command); + + void coast(); + + void brake(); + + double command() const; + +private: + double currentCommand; + + void enable_pwm_outputs(); + + unsigned char magnitude(double value) const; +}; + +#endif \ No newline at end of file diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/operational.h b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/operational.h new file mode 100644 index 0000000..15d5f5b --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/operational.h @@ -0,0 +1,52 @@ +#ifndef OPERATIONAL_H +#define OPERATIONAL_H + +#include + +#include +#include +#include + +class Operational : public State +{ +public: + Operational( + Speed_controller& speedController, + Digital_out& statusLED + ); + + const char* name() const override; + + void on_entry() override; + void on_do() override; + void on_exit() override; + + void reset() override; + void set_pre_operational() override; + void fault() override; + + void set_initialization_state( + State& initializationState + ); + + void set_pre_operational_state( + State& preOperationalState + ); + + void set_stopped_state( + State& stoppedState + ); + +private: + Speed_controller& speedController; + Digital_out& statusLED; + + State* initializationState; + State* preOperationalState; + State* stoppedState; + + uint8_t logCounter; + uint32_t sampleCounter; +}; + +#endif \ No newline at end of file diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/p_controller.h b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/p_controller.h new file mode 100644 index 0000000..c6d772e --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/p_controller.h @@ -0,0 +1,36 @@ +#ifndef P_CONTROLLER_H +#define P_CONTROLLER_H + +#include + +class P_controller : public Controller +{ +public: + explicit P_controller( + double kp + ); + + double update( + double reference, + double actual + ) override; + + void reset() override; + + const char* name() const override; + + void set_kp( + double value + ); + + double get_kp() const; + +private: + double kp; + + double limit( + double value + ) const; +}; + +#endif \ No newline at end of file diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/pi_controller.h b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/pi_controller.h new file mode 100644 index 0000000..ae3f860 --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/pi_controller.h @@ -0,0 +1,42 @@ +#ifndef PI_CONTROLLER_H +#define PI_CONTROLLER_H + +#include + +class PI_controller : public Controller +{ +public: + PI_controller( + double kp, + double ti, + double timestep + ); + + double update( + double reference, + double actual + ) override; + + void reset() override; + + const char* name() const override; + + void set_kp(double kp); + double get_kp() const; + + void set_ti(double ti); + double get_ti() const; + + double get_timestep() const; + +private: + double kp; + double ti; + double timestep; + + double integral; + + double limit(double value) const; +}; + +#endif \ No newline at end of file diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/pre_operational.h b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/pre_operational.h new file mode 100644 index 0000000..e38227b --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/pre_operational.h @@ -0,0 +1,63 @@ +#ifndef PRE_OPERATIONAL_H +#define PRE_OPERATIONAL_H + +#include + +#include +#include +#include + +#include +#include + +class PreOperational : public State +{ +public: + PreOperational( + Speed_controller& speedController, + Digital_out& statusLED, + P_controller& pController, + PI_controller& piController + ); + + const char* name() const override; + + void on_entry() override; + void on_do() override; + void on_exit() override; + + void reset() override; + void set_operational() override; + + void select_p() override; + void select_pi() override; + + void set_kp(double value) override; + void set_ti(double value) override; + + void set_initialization_state( + State& initializationState + ); + + void set_operational_state( + State& operationalState + ); + +private: + Speed_controller& speedController; + Digital_out& statusLED; + + P_controller& pController; + PI_controller& piController; + + State* initializationState; + State* operationalState; + + uint8_t ledCounter; + + bool piSelected; + + void print_configuration(); +}; + +#endif \ No newline at end of file diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/speed_controller.h b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/speed_controller.h new file mode 100644 index 0000000..0377e8c --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/speed_controller.h @@ -0,0 +1,47 @@ +#ifndef SPEED_CONTROLLER_H +#define SPEED_CONTROLLER_H + +#include +#include +#include +#include + +class Speed_controller +{ +public: + Speed_controller( + Encoder& encoder, + Speed_estimator& estimator, + Motor_driver& motor, + Controller& controller + ); + + void start(); + void stop(); + void brake(); + + void update(); + + void set_reference(double rpm); + double reference() const; + + void set_controller(Controller& controller); + + double actual_rpm() const; + double control_output() const; + +private: + Encoder& encoder; + Speed_estimator& estimator; + Motor_driver& motor; + + Controller* controller; + + bool running; + + double referenceRPM; + double actualRPM; + double controlOutput; +}; + +#endif \ No newline at end of file diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/speed_estimator.h b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/speed_estimator.h new file mode 100644 index 0000000..b11057e --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/speed_estimator.h @@ -0,0 +1,37 @@ +#ifndef SPEED_ESTIMATOR_H +#define SPEED_ESTIMATOR_H + +#include + +class Speed_estimator +{ +public: + Speed_estimator( + double countsPerRevolution, + double sampleTimeSeconds + ); + + void reset(int32_t initialPosition); + + double update(int32_t currentPosition); + + double rpm() const; + +private: + static const uint8_t WINDOW_SIZE = 10; + + double countsPerRevolution; + double sampleTime; + + int32_t previousPosition; + + int32_t countHistory[WINDOW_SIZE]; + int32_t rollingCounts; + + uint8_t historyIndex; + uint8_t samplesFilled; + + double currentRPM; +}; + +#endif \ No newline at end of file diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/state.h b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/state.h new file mode 100644 index 0000000..9049fb9 --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/state.h @@ -0,0 +1,45 @@ +#ifndef STATE_H +#define STATE_H + +class Context; + +class State +{ +protected: + Context* context_ = nullptr; + +public: + virtual ~State() = default; + + void set_context(Context* context) + { + context_ = context; + } + + virtual const char* name() const = 0; + + virtual void on_entry() = 0; + virtual void on_do() = 0; + virtual void on_exit() = 0; + + // State-machine events + virtual void reset() {} + virtual void set_operational() {} + virtual void set_pre_operational() {} + virtual void fault() {} + + // Pre-operational configuration events + virtual void select_p() {} + virtual void select_pi() {} + virtual void set_kp(double value) + { + (void)value; + } + + virtual void set_ti(double value) + { + (void)value; + } +}; + +#endif \ No newline at end of file diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/stopped.h b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/stopped.h new file mode 100644 index 0000000..f179cdd --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/include/stopped.h @@ -0,0 +1,51 @@ +#ifndef STOPPED_H +#define STOPPED_H + +#include + +#include +#include +#include + +class Stopped : public State +{ +public: + Stopped( + Speed_controller& speedController, + Digital_out& statusLED + ); + + const char* name() const override; + + void on_entry() override; + void on_do() override; + void on_exit() override; + + void reset() override; + void set_pre_operational() override; + void set_operational() override; + + void set_initialization_state( + State& initializationState + ); + + void set_pre_operational_state( + State& preOperationalState + ); + + void set_operational_state( + State& operationalState + ); + +private: + Speed_controller& speedController; + Digital_out& statusLED; + + State* initializationState; + State* preOperationalState; + State* operationalState; + + uint8_t ledCounter; +}; + +#endif \ No newline at end of file diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/lib/README b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/lib/README new file mode 100644 index 0000000..9379397 --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into the executable file. + +The source code of each library should be placed in a separate directory +("lib/your_library_name/[Code]"). + +For example, see the structure of the following example libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional. for custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +Example contents of `src/main.c` using Foo and Bar: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +The PlatformIO Library Dependency Finder will find automatically dependent +libraries by scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/platformio.ini b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/platformio.ini new file mode 100644 index 0000000..b0b3432 --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/platformio.ini @@ -0,0 +1,17 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter, extra scripting +; Upload options: custom port, speed and extra flags +; Library options: dependencies, extra library storages +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +[env:nucleo_l432kc] +platform = ststm32 +board = nucleo_l432kc +framework = zephyr + +test_framework = unity +build_flags = -I"C:\Users\nokkv\.platformio\packages\framework-zephyr\include\zephyr" +monitor_speed = 115200 \ No newline at end of file diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/A4.1main.cpp b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/A4.1main.cpp new file mode 100644 index 0000000..2004fc3 --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/A4.1main.cpp @@ -0,0 +1,98 @@ +#include +#include + +#include +#include +#include +#include + + +//#include /* GPIO_ACTIVE_LOW, GPIO_PULL_UP */ +//#include + + + +/* 1000 msec = 1 sec */ +#define SLEEP_TIME_MS 200 + +/* The devicetree node identifier for the "led0" alias. */ +#define LED0_NODE DT_ALIAS(led0) +// we add button +#define BUTTON_NODE DT_ALIAS(sw1) + + +/* + * A build error on this line means your board is unsupported. + * See the sample documentation for information on how to fix this. + */ +static const struct gpio_dt_spec led = GPIO_DT_SPEC_GET(LED0_NODE, gpios); +static const struct gpio_dt_spec button = GPIO_DT_SPEC_GET(BUTTON_NODE, gpios); + +// we add interrupt driven. +static struct gpio_callback button_cb_data; + +// Work item +static struct k_work button_work; + +bool led_state = true; + +//k work +static void button_work_handler(struct k_work *work) +{ + gpio_pin_toggle_dt(&led); + led_state = !led_state; + printf("LED state: %s\n", led_state ? "ON" : "OFF"); +} + +static void on_press(const struct device *dev, struct gpio_callback +*cb_data, uint32_t pins) {k_work_submit(&button_work);} + + + + + +int main(void) +{ + int ret; + + if (!gpio_is_ready_dt(&button)) { + return 0; + } + + if (!gpio_is_ready_dt(&led)) { + return 0; + } + + ret = gpio_pin_configure_dt(&led, GPIO_OUTPUT_ACTIVE); + if (ret < 0) { + return 0; + } + + ret = gpio_pin_configure_dt(&button, GPIO_INPUT); + if (ret < 0) { + return ret; + } + + // configure interrupt + + // button = D3, from the sw1 alias + ret = gpio_pin_interrupt_configure_dt(&button, GPIO_INT_EDGE_TO_ACTIVE); + if (ret < 0) { + return ret; + } + + //initialize work + k_work_init(&button_work, button_work_handler); + + // initialize callback + gpio_init_callback(&button_cb_data, on_press, BIT(button.pin)); + + // Register callback + gpio_add_callback(button.port, &button_cb_data); + + while (1) { + k_msleep(SLEEP_TIME_MS); + + } + return 0; +} \ No newline at end of file diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/context.cpp b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/context.cpp new file mode 100644 index 0000000..6de71ff --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/context.cpp @@ -0,0 +1,109 @@ +#include +#include + +Context::Context(State& initialState) + : state(nullptr) +{ + transition_to(initialState); +} + +void Context::transition_to(State& newState) +{ + if (state != nullptr) + { + state->on_exit(); + + Serial.print("Transition: "); + Serial.print(state->name()); + Serial.print(" -> "); + Serial.println(newState.name()); + } + + state = &newState; + + state->set_context(this); + + state->on_entry(); +} + +void Context::do_work() +{ + if (state != nullptr) + { + state->on_do(); + } +} + +void Context::reset() +{ + if (state != nullptr) + { + state->reset(); + } +} + +void Context::set_operational() +{ + if (state != nullptr) + { + state->set_operational(); + } +} + +void Context::set_pre_operational() +{ + if (state != nullptr) + { + state->set_pre_operational(); + } +} + +void Context::fault() +{ + if (state != nullptr) + { + state->fault(); + } +} + +void Context::select_p() +{ + if (state != nullptr) + { + state->select_p(); + } +} + +void Context::select_pi() +{ + if (state != nullptr) + { + state->select_pi(); + } +} + +void Context::set_kp(double value) +{ + if (state != nullptr) + { + state->set_kp(value); + } +} + +void Context::set_ti(double value) +{ + if (state != nullptr) + { + state->set_ti(value); + } +} + +const char* Context::current_state_name() const +{ + if (state == nullptr) + { + return "None"; + } + + return state->name(); +} \ No newline at end of file diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/control_timer.cpp b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/control_timer.cpp new file mode 100644 index 0000000..e3f7f62 --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/control_timer.cpp @@ -0,0 +1,70 @@ +#include + +#include +#include +#include + +static volatile uint8_t pendingTicks = 0; +static volatile uint32_t totalTicks = 0; + +ISR(TIMER1_COMPA_vect) +{ + totalTicks++; + + if (pendingTicks < 255) + { + pendingTicks++; + } +} + +void Control_timer::init() +{ + pendingTicks = 0; + totalTicks = 0; + + // Timer1 CTC mode, 250 Hz + // 16 MHz / 64 / (999 + 1) = 250 Hz + + TCCR1A = 0; + TCCR1B = 0; + TCNT1 = 0; + + OCR1A = 999; + + TCCR1B |= (1 << WGM12); + + // Prescaler = 64 + TCCR1B |= + (1 << CS11) | + (1 << CS10); + + TIMSK1 |= (1 << OCIE1A); +} + +bool Control_timer::take_tick() +{ + bool tickAvailable = false; + + ATOMIC_BLOCK(ATOMIC_RESTORESTATE) + { + if (pendingTicks > 0) + { + pendingTicks--; + tickAvailable = true; + } + } + + return tickAvailable; +} + +uint32_t Control_timer::tick_count() const +{ + uint32_t ticks; + + ATOMIC_BLOCK(ATOMIC_RESTORESTATE) + { + ticks = totalTicks; + } + + return ticks; +} \ No newline at end of file diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/digital_in.cpp b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/digital_in.cpp new file mode 100644 index 0000000..4ac93ce --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/digital_in.cpp @@ -0,0 +1,26 @@ +#include +#include + +Digital_in::Digital_in(uint8_t pin) +{ + pinMask = static_cast(1U << pin); +} + +void Digital_in::init() +{ + // Input + DDRB &= static_cast(~pinMask); + + // Internal pull-up + PORTB |= pinMask; +} + +bool Digital_in::is_hi() const +{ + return (PINB & pinMask) != 0; +} + +bool Digital_in::is_lo() const +{ + return (PINB & pinMask) == 0; +} \ No newline at end of file diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/digital_out.cpp b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/digital_out.cpp new file mode 100644 index 0000000..598ee0b --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/digital_out.cpp @@ -0,0 +1,27 @@ +#include +#include + +Digital_out::Digital_out(uint8_t pin) +{ + pinMask = static_cast(1U << pin); +} + +void Digital_out::init() +{ + DDRB |= pinMask; +} + +void Digital_out::set_hi() +{ + PORTB |= pinMask; +} + +void Digital_out::set_lo() +{ + PORTB &= static_cast(~pinMask); +} + +void Digital_out::toggle() +{ + PORTB ^= pinMask; +} \ No newline at end of file diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/encoder.cpp b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/encoder.cpp new file mode 100644 index 0000000..e497e54 --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/encoder.cpp @@ -0,0 +1,75 @@ +#include + +#include +#include + +static Encoder* activeEncoder = nullptr; + +ISR(PCINT0_vect) +{ + if (activeEncoder != nullptr) + { + activeEncoder->update(); + } +} + +Encoder::Encoder(uint8_t c1Pin, uint8_t c2Pin) + : c1(c1Pin), + c2(c2Pin), + c1Pin(c1Pin), + previousC1(false), + count(0) +{ +} + +void Encoder::init() +{ + c1.init(); + c2.init(); + + activeEncoder = this; + + previousC1 = c1.is_hi(); + count = 0; + + // Clear pending PORTB pin-change interrupt + PCIFR |= (1 << PCIF0); + + // Enable PORTB pin-change interrupts + PCICR |= (1 << PCIE0); + + // Enable interrupt on C1 + PCMSK0 |= (1 << c1Pin); +} + +void Encoder::update() +{ + bool currentC1 = c1.is_hi(); + + if (currentC1 != previousC1) + { + bool currentC2 = c2.is_hi(); + + if (currentC1 == currentC2) + { + count++; + } + else + { + count--; + } + + previousC1 = currentC1; + } +} + +int32_t Encoder::position() const +{ + uint8_t oldSREG = SREG; + + cli(); + int32_t currentCount = count; + SREG = oldSREG; + + return currentCount; +} \ No newline at end of file diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/initialization.cpp b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/initialization.cpp new file mode 100644 index 0000000..43499cc --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/initialization.cpp @@ -0,0 +1,61 @@ +#include + +#include +#include + +Initialization::Initialization( + Speed_controller& speedController, + Digital_out& statusLED +) + : speedController(speedController), + statusLED(statusLED), + preOperationalState(nullptr), + transitionPending(false) +{ +} + +const char* Initialization::name() const +{ + return "Initialization"; +} + +void Initialization::on_entry() +{ + speedController.stop(); + + statusLED.set_lo(); + + Serial.println(); + Serial.println("Initialization"); + Serial.println("Boot-up"); + + // CANopen-style autonomous transition + // into PreOperational. + transitionPending = true; +} + +void Initialization::on_do() +{ + if ( + transitionPending && + preOperationalState != nullptr + ) + { + transitionPending = false; + + context_->transition_to( + *preOperationalState + ); + } +} + +void Initialization::on_exit() +{ +} + +void Initialization::set_pre_operational_state( + State& state +) +{ + preOperationalState = &state; +} \ No newline at end of file diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/motor_driver.cpp b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/motor_driver.cpp new file mode 100644 index 0000000..6f247e6 --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/motor_driver.cpp @@ -0,0 +1,135 @@ +#include + +#include + +void Motor_driver::init() +{ + currentCommand = 0.0; + + // D11 / PB3 / OC2A -> AIN1 + DDRB |= (1 << PB3); + + // D3 / PD3 / OC2B -> AIN2 + DDRD |= (1 << PD3); + + PORTB &= ~(1 << PB3); + PORTD &= ~(1 << PD3); + + // Timer2 Fast PWM + TCCR2A = 0; + TCCR2B = 0; + TCNT2 = 0; + + OCR2A = 0; + OCR2B = 0; + + TCCR2A = + (1 << COM2A1) | + (1 << COM2B1) | + (1 << WGM21) | + (1 << WGM20); + + // Prescaler = 8 + // PWM frequency ~= 7.8 kHz + TCCR2B = + (1 << CS21); +} + +void Motor_driver::enable_pwm_outputs() +{ + // Non-inverting PWM on OC2A and OC2B + TCCR2A |= + (1 << COM2A1) | + (1 << COM2B1); + + TCCR2A &= + ~((1 << COM2A0) | + (1 << COM2B0)); +} + +unsigned char Motor_driver::magnitude( + double value +) const +{ + if (value < 0.0) + { + value = -value; + } + + if (value > 255.0) + { + value = 255.0; + } + + return static_cast(value); +} + +void Motor_driver::set_command(double command) +{ + if (command > 255.0) + { + command = 255.0; + } + + if (command < -255.0) + { + command = -255.0; + } + + currentCommand = command; + + enable_pwm_outputs(); + + unsigned char pwm = + magnitude(command); + + if (command > 0.0) + { + // Same direction that previously gave positive RPM: + // AIN1 = LOW + // AIN2 = PWM + OCR2A = 0; + OCR2B = pwm; + } + else if (command < 0.0) + { + OCR2A = pwm; + OCR2B = 0; + } + else + { + OCR2A = 0; + OCR2B = 0; + } +} + +void Motor_driver::coast() +{ + currentCommand = 0.0; + + enable_pwm_outputs(); + + OCR2A = 0; + OCR2B = 0; +} + +void Motor_driver::brake() +{ + currentCommand = 0.0; + + // Disconnect Timer2 PWM from the pins. + TCCR2A &= + ~((1 << COM2A1) | + (1 << COM2A0) | + (1 << COM2B1) | + (1 << COM2B0)); + + // DRV8833: both inputs HIGH gives braking. + PORTB |= (1 << PB3); + PORTD |= (1 << PD3); +} + +double Motor_driver::command() const +{ + return currentCommand; +} \ No newline at end of file diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/operational.cpp b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/operational.cpp new file mode 100644 index 0000000..a0848bf --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/operational.cpp @@ -0,0 +1,143 @@ +#include + +#include +#include + +Operational::Operational( + Speed_controller& speedController, + Digital_out& statusLED +) + : speedController(speedController), + statusLED(statusLED), + initializationState(nullptr), + preOperationalState(nullptr), + stoppedState(nullptr), + logCounter(0), + sampleCounter(0) +{ +} + +const char* Operational::name() const +{ + return "Operational"; +} + +void Operational::on_entry() +{ + Serial.println(); + Serial.println("Operational"); + + statusLED.set_hi(); + + logCounter = 0; + sampleCounter = 0; + + speedController.start(); + + Serial.println( + "DATA,time_ms,reference_rpm,actual_rpm,control" + ); +} + +void Operational::on_do() +{ + speedController.update(); + + sampleCounter++; + logCounter++; + + // 5 controller ticks × 4 ms = 20 ms + // => 50 Hz logging rate. + if (logCounter >= 5) + { + logCounter = 0; + + const uint32_t time_ms = + sampleCounter * 4UL; + + Serial.print("DATA,"); + Serial.print(time_ms); + Serial.print(","); + + Serial.print( + speedController.reference(), + 2 + ); + + Serial.print(","); + + Serial.print( + speedController.actual_rpm(), + 2 + ); + + Serial.print(","); + + Serial.println( + speedController.control_output(), + 2 + ); + } +} + +void Operational::on_exit() +{ + speedController.stop(); + + statusLED.set_lo(); +} + +void Operational::reset() +{ + if (initializationState != nullptr) + { + context_->transition_to( + *initializationState + ); + } +} + +void Operational::set_pre_operational() +{ + if (preOperationalState != nullptr) + { + context_->transition_to( + *preOperationalState + ); + } +} + +void Operational::fault() +{ + if (stoppedState != nullptr) + { + Serial.println( + "FAULT detected" + ); + + context_->transition_to( + *stoppedState + ); + } +} + +void Operational::set_initialization_state( + State& state +) +{ + initializationState = &state; +} + +void Operational::set_pre_operational_state( + State& state +) +{ + preOperationalState = &state; +} + +void Operational::set_stopped_state( + State& state +) +{ + stoppedState = &state; +} \ No newline at end of file diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/p_controller.cpp b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/p_controller.cpp new file mode 100644 index 0000000..98c1d63 --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/p_controller.cpp @@ -0,0 +1,63 @@ +#include + +P_controller::P_controller( + double kp +) + : kp(kp) +{ +} + +double P_controller::update( + double reference, + double actual +) +{ + const double error = + reference - actual; + + const double output = + kp * error; + + return limit(output); +} + +void P_controller::reset() +{ +} + +const char* P_controller::name() const +{ + return "P"; +} + +void P_controller::set_kp( + double value +) +{ + if (value > 0.0) + { + kp = value; + } +} + +double P_controller::get_kp() const +{ + return kp; +} + +double P_controller::limit( + double value +) const +{ + if (value > 255.0) + { + return 255.0; + } + + if (value < -255.0) + { + return -255.0; + } + + return value; +} \ No newline at end of file diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/pi_controller.cpp b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/pi_controller.cpp new file mode 100644 index 0000000..3fd9a2b --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/pi_controller.cpp @@ -0,0 +1,141 @@ +#include + +PI_controller::PI_controller( + double kp, + double ti, + double timestep +) + : kp(kp), + ti(ti), + timestep(timestep), + integral(0.0) +{ +} + +double PI_controller::update( + double reference, + double actual +) +{ + const double error = + reference - actual; + + // Candidate integral: + // E(k) = E(k-1) + e(k)T + const double candidateIntegral = + integral + error * timestep; + + // Candidate PI output: + // u = Kp * (e + E/Ti) + const double candidateOutput = + kp * + ( + error + + candidateIntegral / ti + ); + + // ----------------------------------------- + // Conditional integration / anti-windup + // ----------------------------------------- + + // Integrate normally if the candidate + // controller output is not saturated. + if ( + candidateOutput <= 255.0 && + candidateOutput >= -255.0 + ) + { + integral = + candidateIntegral; + } + + // If positively saturated, only integrate + // if the error is driving the output back down. + else if ( + candidateOutput > 255.0 && + error < 0.0 + ) + { + integral = + candidateIntegral; + } + + // If negatively saturated, only integrate + // if the error is driving the output back up. + else if ( + candidateOutput < -255.0 && + error > 0.0 + ) + { + integral = + candidateIntegral; + } + + const double output = + kp * + ( + error + + integral / ti + ); + + return limit(output); +} + +void PI_controller::reset() +{ + integral = 0.0; +} + +const char* PI_controller::name() const +{ + return "PI"; +} + +void PI_controller::set_kp( + double value +) +{ + kp = value; +} + +double PI_controller::get_kp() const +{ + return kp; +} + +void PI_controller::set_ti( + double value +) +{ + if (value > 0.0) + { + ti = value; + } +} + +double PI_controller::get_ti() const +{ + return ti; +} + +double PI_controller::get_timestep() const +{ + return timestep; +} + +double PI_controller::limit( + double value +) const +{ + if (value > 255.0) + { + return 255.0; + } + + if (value < -255.0) + { + return -255.0; + } + + return value; +} \ No newline at end of file diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/pre_operational.cpp b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/pre_operational.cpp new file mode 100644 index 0000000..fdf1f8e --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/pre_operational.cpp @@ -0,0 +1,225 @@ +#include + +#include +#include + +PreOperational::PreOperational( + Speed_controller& speedController, + Digital_out& statusLED, + P_controller& pController, + PI_controller& piController +) + : speedController(speedController), + statusLED(statusLED), + pController(pController), + piController(piController), + initializationState(nullptr), + operationalState(nullptr), + ledCounter(0), + piSelected(false) +{ +} + +const char* PreOperational::name() const +{ + return "PreOperational"; +} + +void PreOperational::on_entry() +{ + // Motor must not run while configuring. + speedController.stop(); + + statusLED.set_lo(); + + ledCounter = 0; + + Serial.println(); + Serial.println("PreOperational"); + Serial.println("Motor control disabled."); + + print_configuration(); + + Serial.println(); + Serial.println("Commands:"); + Serial.println("p = select P controller"); + Serial.println("i = select PI controller"); + Serial.println("k = set Kp"); + Serial.println("t = set Ti"); + Serial.println("o = set operational"); + Serial.println("r = reset"); +} + +void PreOperational::on_do() +{ + // State activity runs every 4 ms. + // + // 1 Hz LED: + // full period = 1000 ms + // toggle every 500 ms + // + // 500 ms / 4 ms = 125 ticks + + ledCounter++; + + if (ledCounter >= 125) + { + ledCounter = 0; + + statusLED.toggle(); + } +} + +void PreOperational::on_exit() +{ + statusLED.set_lo(); +} + +void PreOperational::reset() +{ + if (initializationState != nullptr) + { + context_->transition_to( + *initializationState + ); + } +} + +void PreOperational::set_operational() +{ + if (operationalState != nullptr) + { + context_->transition_to( + *operationalState + ); + } +} + +void PreOperational::select_p() +{ + piSelected = false; + + speedController.set_controller( + pController + ); + + Serial.println( + "Controller selected: P" + ); + + print_configuration(); +} + +void PreOperational::select_pi() +{ + piSelected = true; + + speedController.set_controller( + piController + ); + + Serial.println( + "Controller selected: PI" + ); + + print_configuration(); +} + +void PreOperational::set_kp( + double value +) +{ + if (value <= 0.0) + { + Serial.println( + "Invalid Kp." + ); + + return; + } + + if (piSelected) + { + piController.set_kp( + value + ); + } + else + { + pController.set_kp( + value + ); + } + + Serial.print("Kp set to: "); + Serial.println(value, 4); +} + +void PreOperational::set_ti( + double value +) +{ + if (value <= 0.0) + { + Serial.println( + "Invalid Ti." + ); + + return; + } + + piController.set_ti( + value + ); + + Serial.print("Ti set to: "); + Serial.print(value, 4); + Serial.println(" s"); +} + +void PreOperational::set_initialization_state( + State& state +) +{ + initializationState = &state; +} + +void PreOperational::set_operational_state( + State& state +) +{ + operationalState = &state; +} + +void PreOperational::print_configuration() +{ + Serial.print("Controller: "); + + if (piSelected) + { + Serial.println("PI"); + + Serial.print("Kp = "); + Serial.println( + piController.get_kp(), + 4 + ); + + Serial.print("Ti = "); + Serial.print( + piController.get_ti(), + 4 + ); + Serial.println(" s"); + } + else + { + Serial.println("P"); + + Serial.print("Kp = "); + Serial.println( + pController.get_kp(), + 4 + ); + } +} \ No newline at end of file diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/project3main.cpp b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/project3main.cpp new file mode 100644 index 0000000..135f741 --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/project3main.cpp @@ -0,0 +1,427 @@ +#include +#include + +#include +#include +#include // from A4.1 +#include // so i can print using serial monitor + +//#include removed since it was for arduino + +// sdo configuration +// rpdo1 real time + + +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +// -------------------------------------------------- +// Hardware / drivers +// -------------------------------------------------- + +// Encoder: +// PB0 = D8 +// PB1 = D9 +Encoder encoder(0, 1); + +// Emergency stop: +// PB2 = D10 +// Internal pull-up enabled. +// LOW = emergency stop pressed. +Digital_in emergencyStop(2); + +// Status LED: +// PB5 = D13 +Digital_out statusLED(5); + +Control_timer controlTimer; + +Motor_driver motorDriver; + +// Encoder resolution = 2100 counts/revolution +// Controller period = 4 ms = 250 Hz +Speed_estimator speedEstimator( + 2100.0, + 0.004 +); + +// -------------------------------------------------- +// Controllers +// -------------------------------------------------- + +P_controller pController( + 40.0 +); + +PI_controller piController( + 40.0, // Kp + 0.50, // Ti [s] + 0.004 // timestep [s] +); + +Speed_controller speedController( + encoder, + speedEstimator, + motorDriver, + pController +); + +// -------------------------------------------------- +// State machine states +// -------------------------------------------------- + +Initialization initialization( + speedController, + statusLED +); + +PreOperational preOperational( + speedController, + statusLED, + pController, + piController +); + +Operational operational( + speedController, + statusLED +); + +Stopped stopped( + speedController, + statusLED +); + +Context* stateMachine = nullptr; + +// -------------------------------------------------- +// Serial command buffer +// -------------------------------------------------- + +char commandBuffer[32]; +uint8_t commandIndex = 0; + +// -------------------------------------------------- +// Process one complete serial command +// -------------------------------------------------- + +void processCommand(const char* command) +{ + if (command[0] == '\0') + { + return; + } + + // -------------------------------------------------- + // Single-character commands + // -------------------------------------------------- + + if ( + command[0] == 'o' && + command[1] == '\0' + ) + { + stateMachine->set_operational(); + } + + else if ( + command[0] == 'c' && + command[1] == '\0' + ) + { + stateMachine->set_pre_operational(); + } + + else if ( + command[0] == 'r' && + command[1] == '\0' + ) + { + stateMachine->reset(); + } + + else if ( + command[0] == 'p' && + command[1] == '\0' + ) + { + stateMachine->select_p(); + } + + else if ( + command[0] == 'i' && + command[1] == '\0' + ) + { + stateMachine->select_pi(); + } + + // -------------------------------------------------- + // k = set Kp + // Example: + // k30.625 + // -------------------------------------------------- + + else if (command[0] == 'k') + { + char* endPointer; + + double value = + strtod( + command + 1, + &endPointer + ); + + if ( + endPointer == command + 1 || + *endPointer != '\0' + ) + { + Serial.println( + "Invalid Kp command." + ); + + Serial.println( + "Example: k30.625" + ); + + return; + } + + stateMachine->set_kp( + value + ); + } + + // -------------------------------------------------- + // t = set Ti + // Example: + // t0.4 + // -------------------------------------------------- + + else if (command[0] == 't') + { + char* endPointer; + + double value = + strtod( + command + 1, + &endPointer + ); + + if ( + endPointer == command + 1 || + *endPointer != '\0' + ) + { + Serial.println( + "Invalid Ti command." + ); + + Serial.println( + "Example: t0.4" + ); + + return; + } + + stateMachine->set_ti( + value + ); + } + + else + { + Serial.print( + "Unknown command: " + ); + + Serial.println( + command + ); + } +} + +// -------------------------------------------------- +// Non-blocking serial command reader +// -------------------------------------------------- + +void readSerialCommands() +{ + while (Serial.available() > 0) + { + char character = + Serial.read(); + + // Ignore carriage return + if (character == '\r') + { + continue; + } + + // Newline = end of command + if (character == '\n') + { + commandBuffer[ + commandIndex + ] = '\0'; + + processCommand( + commandBuffer + ); + + commandIndex = 0; + + continue; + } + + // Add character to buffer + if ( + commandIndex < + sizeof(commandBuffer) - 1 + ) + { + commandBuffer[ + commandIndex + ] = character; + + commandIndex++; + } + else + { + // Buffer overflow + commandIndex = 0; + + Serial.println( + "Command too long." + ); + } + } +} + +// -------------------------------------------------- +// Setup +// -------------------------------------------------- + +void setup() +{ + Serial.begin(115200); + + statusLED.init(); + + emergencyStop.init(); + + encoder.init(); + + motorDriver.init(); + + // Initial target speed + speedController.set_reference( + 50.0 + ); + + // -------------------------------------------------- + // Wire state transitions + // -------------------------------------------------- + + initialization.set_pre_operational_state( + preOperational + ); + + preOperational.set_initialization_state( + initialization + ); + + preOperational.set_operational_state( + operational + ); + + operational.set_initialization_state( + initialization + ); + + operational.set_pre_operational_state( + preOperational + ); + + operational.set_stopped_state( + stopped + ); + + stopped.set_initialization_state( + initialization + ); + + stopped.set_pre_operational_state( + preOperational + ); + + stopped.set_operational_state( + operational + ); + + // -------------------------------------------------- + // Create state machine + // -------------------------------------------------- + + static Context context( + initialization + ); + + stateMachine = &context; + + // Fixed 250 Hz timing + controlTimer.init(); + + Serial.println(); + Serial.println( + "Project 3 - Part 4" + ); +} + +// -------------------------------------------------- +// Main loop +// -------------------------------------------------- + +void loop() +{ + // -------------------------------------------------- + // Emergency stop + // -------------------------------------------------- + + if (emergencyStop.is_lo()) + { + stateMachine->fault(); + } + + // -------------------------------------------------- + // Serial interface + // -------------------------------------------------- + + readSerialCommands(); + + // -------------------------------------------------- + // Fixed 250 Hz state-machine activity + // -------------------------------------------------- + + if (controlTimer.take_tick()) + { + stateMachine->do_work(); + } +} \ No newline at end of file diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/speed_controller.cpp b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/speed_controller.cpp new file mode 100644 index 0000000..992055c --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/speed_controller.cpp @@ -0,0 +1,106 @@ +#include + +Speed_controller::Speed_controller( + Encoder& encoder, + Speed_estimator& estimator, + Motor_driver& motor, + Controller& controller +) + : encoder(encoder), + estimator(estimator), + motor(motor), + controller(&controller), + running(false), + referenceRPM(0.0), + actualRPM(0.0), + controlOutput(0.0) +{ +} + +void Speed_controller::start() +{ + estimator.reset( + encoder.position() + ); + + controller->reset(); + + actualRPM = 0.0; + controlOutput = 0.0; + + running = true; + + motor.coast(); +} + +void Speed_controller::stop() +{ + running = false; + + controlOutput = 0.0; + + motor.coast(); +} + +void Speed_controller::brake() +{ + running = false; + + controlOutput = 0.0; + + motor.brake(); +} + +void Speed_controller::update() +{ + if (!running) + { + return; + } + + actualRPM = + estimator.update( + encoder.position() + ); + + controlOutput = + controller->update( + referenceRPM, + actualRPM + ); + + motor.set_command( + controlOutput + ); +} + +void Speed_controller::set_reference( + double rpm +) +{ + referenceRPM = rpm; +} + +double Speed_controller::reference() const +{ + return referenceRPM; +} + +void Speed_controller::set_controller( + Controller& newController +) +{ + controller = &newController; + + controller->reset(); +} + +double Speed_controller::actual_rpm() const +{ + return actualRPM; +} + +double Speed_controller::control_output() const +{ + return controlOutput; +} \ No newline at end of file diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/speed_estimator.cpp b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/speed_estimator.cpp new file mode 100644 index 0000000..a61d6ba --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/speed_estimator.cpp @@ -0,0 +1,76 @@ +#include + +Speed_estimator::Speed_estimator( + double countsPerRevolution, + double sampleTimeSeconds +) + : countsPerRevolution(countsPerRevolution), + sampleTime(sampleTimeSeconds), + previousPosition(0), + rollingCounts(0), + historyIndex(0), + samplesFilled(0), + currentRPM(0.0) +{ + for (uint8_t i = 0; i < WINDOW_SIZE; i++) + { + countHistory[i] = 0; + } +} + +void Speed_estimator::reset(int32_t initialPosition) +{ + previousPosition = initialPosition; + + rollingCounts = 0; + historyIndex = 0; + samplesFilled = 0; + currentRPM = 0.0; + + for (uint8_t i = 0; i < WINDOW_SIZE; i++) + { + countHistory[i] = 0; + } +} + +double Speed_estimator::update(int32_t currentPosition) +{ + int32_t deltaCount = + currentPosition - previousPosition; + + previousPosition = currentPosition; + + rollingCounts -= + countHistory[historyIndex]; + + countHistory[historyIndex] = + deltaCount; + + rollingCounts += deltaCount; + + historyIndex++; + + if (historyIndex >= WINDOW_SIZE) + { + historyIndex = 0; + } + + if (samplesFilled < WINDOW_SIZE) + { + samplesFilled++; + } + + double windowTime = + samplesFilled * sampleTime; + + currentRPM = + (rollingCounts * 60.0) / + (countsPerRevolution * windowTime); + + return currentRPM; +} + +double Speed_estimator::rpm() const +{ + return currentRPM; +} \ No newline at end of file diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/stopped.cpp b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/stopped.cpp new file mode 100644 index 0000000..9a92b4e --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/src/stopped.cpp @@ -0,0 +1,112 @@ +#include + +#include +#include + +Stopped::Stopped( + Speed_controller& speedController, + Digital_out& statusLED +) + : speedController(speedController), + statusLED(statusLED), + initializationState(nullptr), + preOperationalState(nullptr), + operationalState(nullptr), + ledCounter(0) +{ +} + +const char* Stopped::name() const +{ + return "Stopped"; +} + +void Stopped::on_entry() +{ + Serial.println(); + Serial.println("Stopped"); + Serial.println( + "Emergency stop - motor brake active" + ); + + speedController.brake(); + + statusLED.set_hi(); + + ledCounter = 0; +} + +void Stopped::on_do() +{ + // State activity runs every 4 ms. + // + // 2 Hz LED: + // toggle every ~250 ms + // + // 250 / 4 = 62.5 ticks + + ledCounter++; + + if (ledCounter >= 63) + { + ledCounter = 0; + + statusLED.toggle(); + } +} + +void Stopped::on_exit() +{ + statusLED.set_lo(); +} + +void Stopped::reset() +{ + if (initializationState != nullptr) + { + context_->transition_to( + *initializationState + ); + } +} + +void Stopped::set_pre_operational() +{ + if (preOperationalState != nullptr) + { + context_->transition_to( + *preOperationalState + ); + } +} + +void Stopped::set_operational() +{ + if (operationalState != nullptr) + { + context_->transition_to( + *operationalState + ); + } +} + +void Stopped::set_initialization_state( + State& state +) +{ + initializationState = &state; +} + +void Stopped::set_pre_operational_state( + State& state +) +{ + preOperationalState = &state; +} + +void Stopped::set_operational_state( + State& state +) +{ + operationalState = &state; +} \ No newline at end of file diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/test/README b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/test/README new file mode 100644 index 0000000..9b1e87b --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/test/README @@ -0,0 +1,11 @@ + +This directory is intended for PlatformIO Test Runner and project tests. + +Unit Testing is a software testing method by which individual units of +source code, sets of one or more MCU program modules together with associated +control data, usage procedures, and operating procedures, are tested to +determine whether they are fit for use. Unit testing finds problems early +in the development cycle. + +More information about PlatformIO Unit Testing: +- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/test/test_pi_controller/test_main.cpp b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/test/test_pi_controller/test_main.cpp new file mode 100644 index 0000000..61f78b4 --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/test/test_pi_controller/test_main.cpp @@ -0,0 +1,438 @@ +#include + +#include +#include + +// -------------------------------------------------- +// Unity setup +// -------------------------------------------------- + +void setUp() +{ +} + +void tearDown() +{ +} + +// -------------------------------------------------- +// Normal-flow tests +// -------------------------------------------------- + +void test_zero_error_gives_zero_output() +{ + PI_controller controller( + 10.0, + 1.0, + 0.1 + ); + + double output = + controller.update( + 50.0, + 50.0 + ); + + TEST_ASSERT_DOUBLE_WITHIN( + 0.001, + 0.0, + output + ); +} + +void test_positive_error_gives_positive_output() +{ + PI_controller controller( + 10.0, + 1.0, + 0.1 + ); + + // error = 2 + // E = 2 * 0.1 = 0.2 + // u = 10 * (2 + 0.2 / 1) + // = 22 + + double output = + controller.update( + 52.0, + 50.0 + ); + + TEST_ASSERT_DOUBLE_WITHIN( + 0.001, + 22.0, + output + ); +} + +void test_negative_error_gives_negative_output() +{ + PI_controller controller( + 10.0, + 1.0, + 0.1 + ); + + // error = -2 + // E = -2 * 0.1 = -0.2 + // u = 10 * (-2 - 0.2) + // = -22 + + double output = + controller.update( + 48.0, + 50.0 + ); + + TEST_ASSERT_DOUBLE_WITHIN( + 0.001, + -22.0, + output + ); +} + +void test_integral_accumulates_over_time() +{ + PI_controller controller( + 10.0, + 1.0, + 0.1 + ); + + double first = + controller.update( + 51.0, + 50.0 + ); + + double second = + controller.update( + 51.0, + 50.0 + ); + + double third = + controller.update( + 51.0, + 50.0 + ); + + // After first update: + // E = 0.1 + // u = 10 * (1 + 0.1) = 11 + + TEST_ASSERT_DOUBLE_WITHIN( + 0.001, + 11.0, + first + ); + + // After second update: + // E = 0.2 + // u = 12 + + TEST_ASSERT_DOUBLE_WITHIN( + 0.001, + 12.0, + second + ); + + // After third update: + // E = 0.3 + // u = 13 + + TEST_ASSERT_DOUBLE_WITHIN( + 0.001, + 13.0, + third + ); +} + +// -------------------------------------------------- +// Saturation tests +// -------------------------------------------------- + +void test_positive_output_is_limited() +{ + PI_controller controller( + 100.0, + 1.0, + 0.1 + ); + + double output = + controller.update( + 60.0, + 50.0 + ); + + TEST_ASSERT_DOUBLE_WITHIN( + 0.001, + 255.0, + output + ); +} + +void test_negative_output_is_limited() +{ + PI_controller controller( + 100.0, + 1.0, + 0.1 + ); + + double output = + controller.update( + 40.0, + 50.0 + ); + + TEST_ASSERT_DOUBLE_WITHIN( + 0.001, + -255.0, + output + ); +} + +// -------------------------------------------------- +// Anti-windup tests +// -------------------------------------------------- + +void test_positive_saturation_does_not_wind_up() +{ + PI_controller controller( + 100.0, + 1.0, + 0.1 + ); + + // Repeatedly demand an unreachable positive output. + for (int i = 0; i < 20; i++) + { + controller.update( + 60.0, + 50.0 + ); + } + + // With anti-windup the large saturated error + // should not have accumulated. + // + // New error = +1 + // E = 1 * 0.1 = 0.1 + // u = 100 * (1 + 0.1) = 110 + + double output = + controller.update( + 51.0, + 50.0 + ); + + TEST_ASSERT_DOUBLE_WITHIN( + 0.001, + 110.0, + output + ); +} + +void test_negative_saturation_does_not_wind_up() +{ + PI_controller controller( + 100.0, + 1.0, + 0.1 + ); + + // Repeatedly demand an unreachable negative output. + for (int i = 0; i < 20; i++) + { + controller.update( + 40.0, + 50.0 + ); + } + + // New error = -1 + // E = -0.1 + // u = 100 * (-1 - 0.1) = -110 + + double output = + controller.update( + 49.0, + 50.0 + ); + + TEST_ASSERT_DOUBLE_WITHIN( + 0.001, + -110.0, + output + ); +} + +void test_controller_recovers_after_saturation() +{ + PI_controller controller( + 100.0, + 1.0, + 0.1 + ); + + // Cause positive saturation. + controller.update( + 60.0, + 50.0 + ); + + // Return to a small error. + // Anti-windup should allow immediate recovery. + double output = + controller.update( + 51.0, + 50.0 + ); + + TEST_ASSERT_DOUBLE_WITHIN( + 0.001, + 110.0, + output + ); +} + +// -------------------------------------------------- +// Reset test +// -------------------------------------------------- + +void test_reset_clears_integral() +{ + PI_controller controller( + 10.0, + 1.0, + 0.1 + ); + + // Build up integral contribution. + controller.update( + 51.0, + 50.0 + ); + + controller.update( + 51.0, + 50.0 + ); + + // Clear stored integral. + controller.reset(); + + // This should now behave exactly like + // the first +1 error sample again. + // + // E = 0.1 + // u = 10 * (1 + 0.1) = 11 + + double output = + controller.update( + 51.0, + 50.0 + ); + + TEST_ASSERT_DOUBLE_WITHIN( + 0.001, + 11.0, + output + ); +} + +// -------------------------------------------------- +// Polymorphism test +// -------------------------------------------------- + +void test_pi_controller_works_through_controller_interface() +{ + PI_controller pi( + 10.0, + 1.0, + 0.1 + ); + + // Access PI controller only through + // the common Controller interface. + Controller* controller = + π + + double output = + controller->update( + 52.0, + 50.0 + ); + + TEST_ASSERT_DOUBLE_WITHIN( + 0.001, + 22.0, + output + ); + + TEST_ASSERT_EQUAL_STRING( + "PI", + controller->name() + ); +} + +// -------------------------------------------------- +// Test runner +// -------------------------------------------------- + +int main(int argc, char** argv) +{ + (void)argc; + (void)argv; + + UNITY_BEGIN(); + + RUN_TEST( + test_zero_error_gives_zero_output + ); + + RUN_TEST( + test_positive_error_gives_positive_output + ); + + RUN_TEST( + test_negative_error_gives_negative_output + ); + + RUN_TEST( + test_integral_accumulates_over_time + ); + + RUN_TEST( + test_positive_output_is_limited + ); + + RUN_TEST( + test_negative_output_is_limited + ); + + RUN_TEST( + test_positive_saturation_does_not_wind_up + ); + + RUN_TEST( + test_negative_saturation_does_not_wind_up + ); + + RUN_TEST( + test_controller_recovers_after_saturation + ); + + RUN_TEST( + test_reset_clears_integral + ); + + RUN_TEST( + test_pi_controller_works_through_controller_interface + ); + + return UNITY_END(); +} \ No newline at end of file diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/zephyr/CMakeLists.txt b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/zephyr/CMakeLists.txt new file mode 100644 index 0000000..3b068ff --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/zephyr/CMakeLists.txt @@ -0,0 +1,6 @@ +cmake_minimum_required(VERSION 3.13.1) +include($ENV{ZEPHYR_BASE}/cmake/app/boilerplate.cmake NO_POLICY_SCOPE) +project(A4.1.1-GPIO-in-Zephyr-RTOS) + +FILE(GLOB app_sources ../src/*.c*) +target_sources(app PRIVATE ${app_sources}) diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/zephyr/boards/nucleo_l432kc.overlay b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/zephyr/boards/nucleo_l432kc.overlay new file mode 100644 index 0000000..54d75d8 --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/zephyr/boards/nucleo_l432kc.overlay @@ -0,0 +1,37 @@ +#include +#include + +/ { + aliases { + sw1 = &user_button; + pwm-led0 = &pwm_led_0; + }; + + buttons { + compatible = "gpio-keys"; + + user_button: button_0 { + gpios = <&gpiob 0 (GPIO_PULL_UP | GPIO_ACTIVE_LOW)>; + label = "External button"; + }; + }; + + pwmleds { + compatible = "pwm-leds"; + + pwm_led_0: pwm_led_0 { + pwms = <&pwm1 1 PWM_MSEC(20) PWM_POLARITY_NORMAL>; + }; + }; +}; + +&timers1 { + status = "okay"; + st,prescaler = <79>; + + pwm1: pwm { + status = "okay"; + pinctrl-0 = <&tim1_ch1_pa8>; + pinctrl-names = "default"; + }; +}; \ No newline at end of file diff --git a/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/zephyr/prj.conf b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/zephyr/prj.conf new file mode 100644 index 0000000..ba08c7e --- /dev/null +++ b/Project_4_Distributed_Control/Part_1_Porting_controller_to_Zephyr/zephyr/prj.conf @@ -0,0 +1,2 @@ +CONFIG_GPIO=y +CONFIG_PWM=y \ No newline at end of file