From 0ee6879f99eb6a15a226a0e585c4d6769a720e81 Mon Sep 17 00:00:00 2001 From: helgie22 Date: Fri, 16 Oct 2026 15:12:07 +0000 Subject: [PATCH] =?UTF-8?q?undirb=C3=BAningur=20falla=20fyrir=20can=20sams?= =?UTF-8?q?kipti=20og=20pr=C3=B3fun=20me=C3=B0=20lyklabor=C3=B0i?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- docs/diagrams/src/P4-architecture.wsd | 80 ++++++++++++++++++++++++++ docs/diagrams/src/P4-class-diagram.wsd | 29 ++++++++++ docs/diagrams/src/motor_state.wsd | 50 ---------------- include/initialization_state.h | 4 +- include/motor_controller.h | 54 ++++++++++++----- include/node_application.h | 51 ++++++++++++++++ include/node_types.h | 18 ++++++ include/operational_state.h | 4 +- include/p_controller.h | 8 ++- include/pi_controller.h | 5 +- include/pre_operational_state.h | 18 +++--- include/state.h | 6 +- include/stopped_state.h | 4 +- platformio.ini | 18 +++--- src/CAN_FRAME_TEST.c | 4 +- src/app.cpp | 12 +++- src/motor_controller.cpp | 65 ++++++++++++++++++--- src/operational_state.cpp | 31 +++++++++- src/p_controller.cpp | 9 ++- src/pi_controller.cpp | 20 ++++++- src/pre_operational_state.cpp | 53 +++++++++++------ 21 files changed, 426 insertions(+), 117 deletions(-) create mode 100644 docs/diagrams/src/P4-architecture.wsd create mode 100644 docs/diagrams/src/P4-class-diagram.wsd delete mode 100644 docs/diagrams/src/motor_state.wsd create mode 100644 include/node_application.h create mode 100644 include/node_types.h diff --git a/docs/diagrams/src/P4-architecture.wsd b/docs/diagrams/src/P4-architecture.wsd new file mode 100644 index 0000000..87e6f01 --- /dev/null +++ b/docs/diagrams/src/P4-architecture.wsd @@ -0,0 +1,80 @@ +@startuml P4-architecture +' Project 4, Part 2.2: Layers of the reduced CANopen node (Node ID 1). +' Arrows point from the user to the thing it uses. + +' skinparam packageStyle rectangle +skinparam classAttributeIconSize 0 +hide empty members + +package "Application layer (uses OSI 7)" { + class MotorController { + + to_operational() + + to_pre_operational() + + to_stopped() + } + abstract class State + class SetpointGenerator + class PI_controller +} + +package "CANopen layer (OSI 5-7)" { + class CanopenNode { + + on_frame(frame) + + poll(now_ms) + } + class SdoServer + class ObjectDictionary + interface NodeApplication { + + on_nmt_command(cmd) + + set_target_velocity(rpm) + + get_velocity() : rpm + + get_duty() : 0.1 % + } + interface CanBus { + + send(frame) + } +} + +package "Platform layer (Zephyr)" { + class ZephyrCanBus + class MotorDriver + class RotaryEncoderDriver +} + +package "Hardware (OSI 1-2)" { + class "CAN controller + transceiver" as CAN +} + +MotorController -> State +MotorController --> SetpointGenerator +MotorController --> PI_controller +MotorController ..|> NodeApplication +MotorController --> MotorDriver +MotorController --> RotaryEncoderDriver + +CanopenNode --> SdoServer +SdoServer --> ObjectDictionary +CanopenNode --> ObjectDictionary +CanopenNode --> NodeApplication +CanopenNode --> CanBus + +ZephyrCanBus ..|> CanBus +ZephyrCanBus --> CAN + +note top of CanopenNode + NMT (0x000), boot-up (0x701), + RPDO1 (0x201), TPDO1 (0x181) +end note + +note left of SdoServer + 0x601 / 0x581, expedited only + abort codes 0x06020000, 0x08000022 +end note + +note right of ObjectDictionary + 0x1000, 0x2000-0x2010, 0x606C, + 0x6083, 0x6084, 0x60FF + access rules, little-endian packing +end note + +@enduml diff --git a/docs/diagrams/src/P4-class-diagram.wsd b/docs/diagrams/src/P4-class-diagram.wsd new file mode 100644 index 0000000..7169e94 --- /dev/null +++ b/docs/diagrams/src/P4-class-diagram.wsd @@ -0,0 +1,29 @@ +@startuml +' skinparam linetype ortho +' skinparam nodesep 100 +skinparam ranksep 80 + +skinparam state { + BackgroundColor LightBlue +} + +' hide empty members + +Application <|-- CANopen + +class Application { + {field} state machine + {field} controllers + {field} setpoint generator + {method} hello world +} + +CANopen : Zephyr + +Class01 <-- Class02 +Class03 *-- Class04 +Class05 o-- Class06 +Class07 .. Class08 +Class09 -- Class10 + +@enduml diff --git a/docs/diagrams/src/motor_state.wsd b/docs/diagrams/src/motor_state.wsd deleted file mode 100644 index 6f04fa7..0000000 --- a/docs/diagrams/src/motor_state.wsd +++ /dev/null @@ -1,50 +0,0 @@ -@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/initialization_state.h b/include/initialization_state.h index 03cf56d..50abb10 100644 --- a/include/initialization_state.h +++ b/include/initialization_state.h @@ -19,9 +19,11 @@ public: void update() override; - void command(char c) override; + // void command(char c) override; void exit() override; + + NodeState id() const override { return NodeState::INITIALIZATION; } }; #endif diff --git a/include/motor_controller.h b/include/motor_controller.h index 63cbb24..70d390e 100644 --- a/include/motor_controller.h +++ b/include/motor_controller.h @@ -7,14 +7,15 @@ #include "motor_driver.h" #include "p_controller.h" #include "digital_out.h" +#include "node_types.h" +#include "node_application.h" class State; - -class MotorController +class MotorController : public NodeApplication { private: - static constexpr double DEFAULT_REFERENCE_RPM = 25.0; + static constexpr double DEFAULT_TARGET_RPM = 0.0; static constexpr double PWM_PER_PERCENT = 255.0 / 100.0; static constexpr uint8_t LED_PIN = Digital_out::STATUS_LED; @@ -27,22 +28,32 @@ private: RotaryEncoderDriver &encoder; MotorDriver &motor; - P_controller *controller; + + P_controller &p_controller; + P_controller &pi_controller; + P_controller *controller; // Bendir á p_controller eða pi_controller Digital_out led; - double reference_rpm; + double target_rpm; // Það sem beðið er um (RPDO1) + double reference_rpm; // Það sem stýringin fær + + uint32_t profile_acceleration; // RPM/s + uint32_t profile_deceleration; // Síðasta sýni, notað fyrir CSV-útprentun double last_actual_rpm; double last_pwm; + + public: MotorController( RotaryEncoderDriver &encoder, MotorDriver &motor, - P_controller &controller, + P_controller &p_controller, + P_controller &pi_controller, State &initialization, State &pre_operational, State &operational, @@ -59,13 +70,15 @@ public: void transition_to(State &new_state); - void to_initialization(); + void to_initialization() override; - void to_pre_operational(); + void to_pre_operational() override; - void to_operational(); + void to_operational() override; - void to_stopped(); + void to_stopped() override; + + NodeState get_state() const override; bool fault_active(); @@ -73,21 +86,34 @@ public: // Skilar true þegar nýtt sýni var keyrt í gegnum stýringuna bool run_speed_controller(); + void set_target_rpm(double rpm) override { target_rpm = rpm; } + double get_target_rpm() const override { return target_rpm; } double get_reference_rpm() const { return reference_rpm; } - double get_actual_rpm() const { return last_actual_rpm; } + double get_actual_rpm() const override { return last_actual_rpm; } + double get_pwm() const { return last_pwm; } + double get_duty_percent() const override { return last_pwm / PWM_PER_PERCENT; } + + void set_profile_acceleration(uint32_t value) override {profile_acceleration = value; } + uint32_t get_profile_acceleration() const override { return profile_acceleration; } + void set_profile_deceleration(uint32_t value) override {profile_deceleration = value; } + uint32_t get_profile_deceleration() const override { return profile_deceleration; } void stop_motor(); void reset_speed_measurement(); void reset_controller(); + void reset_parameters() override; - void set_Kp(double value); + void set_Kp(double value) override; + double get_Kp() const override { return controller->get_Kp(); } - void set_Ti(double value); + bool set_Ti(double value) override; + double get_Ti() const override { return controller->get_Ti(); } - void set_controller(P_controller &new_controller); + void select_controller(ControllerType type) override; + ControllerType get_controller_type() const override; void led_on(); diff --git a/include/node_application.h b/include/node_application.h new file mode 100644 index 0000000..6b76cf2 --- /dev/null +++ b/include/node_application.h @@ -0,0 +1,51 @@ +#ifndef NODE_APPLICATION_H +#define NODE_APPLICATION_H + +#include +#include "node_types.h" + +class NodeApplication +{ +public: + virtual ~NodeApplication() {} + + // NMT + virtual void to_initialization() = 0; + virtual void to_pre_operational() = 0; + virtual void to_operational() = 0; + virtual void to_stopped() = 0; + virtual void reset_parameters() = 0; + virtual NodeState get_state() const = 0; + + // 0x2000 + virtual void select_controller(ControllerType type) = 0; + virtual ControllerType get_controller_type() const = 0; + + // 0x2001 + virtual void set_Kp(double value) = 0; + virtual double get_Kp() const = 0; + + // 0x2002 + virtual bool set_Ti(double value) = 0; + virtual double get_Ti() const = 0; + + // 0x2010 + virtual double get_duty_percent() const = 0; + + // 0x606C + virtual double get_actual_rpm() const = 0; + + // 0x6083 + virtual void set_profile_acceleration(uint32_t value) = 0; + virtual uint32_t get_profile_acceleration() const = 0; + // 0x6084 + virtual void set_profile_deceleration(uint32_t value) = 0; + virtual uint32_t get_profile_deceleration() const = 0; + + // 0x60FF (RPDO1) + virtual void set_target_rpm(double value) = 0; + virtual double get_target_rpm() const = 0; + +}; + +#endif \ No newline at end of file diff --git a/include/node_types.h b/include/node_types.h new file mode 100644 index 0000000..50f06b2 --- /dev/null +++ b/include/node_types.h @@ -0,0 +1,18 @@ +#ifndef NODE_TYPES_H +#define NODE_TYPES_H + +enum class ControllerType +{ + P, + PI +}; + +enum class NodeState +{ + INITIALIZATION, + PRE_OPERATIONAL, + OPERATIONAL, + STOPPED +}; + +#endif \ No newline at end of file diff --git a/include/operational_state.h b/include/operational_state.h index e0867ef..2274e55 100644 --- a/include/operational_state.h +++ b/include/operational_state.h @@ -21,9 +21,11 @@ public: void update() override; - void command(char c) override; + // void command(char c) override; void exit() override; + + NodeState id() const override { return NodeState::OPERATIONAL; } }; #endif diff --git a/include/p_controller.h b/include/p_controller.h index b94f1ae..32f9163 100644 --- a/include/p_controller.h +++ b/include/p_controller.h @@ -5,21 +5,27 @@ class P_controller { protected: double Kp; + double default_Kp; // Gildið úr constructornum, notað af restore_defaults() public: P_controller(double Kp); virtual ~P_controller() {} virtual double update(double ref, double actual); virtual void reset() {} + virtual void restore_defaults(); // + virtual double get_Kp() const { return Kp; } + virtual double get_Ti() const { return 0.0; } virtual void set_Kp(double value) { Kp = value; } - virtual void set_Ti(double value) + virtual bool set_Ti(double value) { // P controller does not use Ti + (void)value; + return true; } }; diff --git a/include/pi_controller.h b/include/pi_controller.h index bec8645..4e8c2ab 100644 --- a/include/pi_controller.h +++ b/include/pi_controller.h @@ -10,14 +10,17 @@ class PI_controller : public P_controller static constexpr double U_MIN = -100.0; // % double Ti; + double default_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 restore_defaults() override; - void set_Ti(double value) override; + bool set_Ti(double value) override; + double get_Ti() const override { return Ti; } }; #endif diff --git a/include/pre_operational_state.h b/include/pre_operational_state.h index 12b014b..3d9015a 100644 --- a/include/pre_operational_state.h +++ b/include/pre_operational_state.h @@ -4,14 +4,14 @@ #include "state.h" #include "stdint.h" -class P_controller; +// class P_controller; class PreOperationalState : public State { private: - P_controller &p_controller; - P_controller &pi_controller; - bool pi_selected; + // P_controller &p_controller; + // P_controller &pi_controller; + // bool pi_selected; int64_t previous_time; bool led_state; @@ -21,7 +21,8 @@ private: { NONE, ENTER_KP, - ENTER_TI + ENTER_TI, + ENTER_RPM }; InputMode input_mode; @@ -30,12 +31,15 @@ private: uint8_t input_index; public: - PreOperationalState(P_controller& p_controller, P_controller& pi_controller); + // PreOperationalState(P_controller& p_controller, P_controller& pi_controller); + PreOperationalState(); void enter() override; void update() override; - void command(char c) override; + // void command(char c) override; void exit() override; + + NodeState id() const override { return NodeState::PRE_OPERATIONAL; } }; #endif diff --git a/include/state.h b/include/state.h index f6a010f..1544bf3 100644 --- a/include/state.h +++ b/include/state.h @@ -1,6 +1,8 @@ #ifndef STATE_H #define STATE_H +#include "node_types.h" + class MotorController; class State @@ -22,9 +24,11 @@ public: virtual void update() = 0; // run motor controller - virtual void command(char c) = 0; // Listen for reset + // virtual void command(char c) = 0; // Listen for reset virtual void exit() = 0; // Stop motor + + virtual NodeState id() const = 0; // }; #endif diff --git a/include/stopped_state.h b/include/stopped_state.h index ad06f9d..e9b0635 100644 --- a/include/stopped_state.h +++ b/include/stopped_state.h @@ -18,9 +18,11 @@ public: void update() override; - void command(char c) override; + // void command(char c) override; void exit() override; + + NodeState id() const override { return NodeState::STOPPED; } }; #endif diff --git a/platformio.ini b/platformio.ini index de6a0ed..e96be14 100644 --- a/platformio.ini +++ b/platformio.ini @@ -24,18 +24,18 @@ upload_flags = ; Part 2.1: CAN bring-up test (src/can_send_test.c) instead of the motor app. ; Build/upload: pio run -e can_test -t upload -[env:can_test] -extends = env:nucleo_l432kc -build_flags = -D CAN_SEND_TEST=1 +; [env:can_test] +; extends = env:nucleo_l432kc +; build_flags = -D CAN_SEND_TEST=1 ; L4.4 example (src/CAN_FRAME_TEST.c) instead of the motor app. ; Build/upload: pio run -e can_frame_test -t upload -[env:can_frame_test] -extends = env:nucleo_l432kc -build_flags = -D CAN_FRAME_TEST=1 +; [env:can_frame_test] +; extends = env:nucleo_l432kc +; build_flags = -D CAN_FRAME_TEST=1 ; CAN reception with a filter (src/can_rx_test.c) instead of the motor app. ; Build/upload: pio run -e can_rx_test -t upload -[env:can_rx_test] -extends = env:nucleo_l432kc -build_flags = -D CAN_RX_TEST=1 +; [env:can_rx_test] +; extends = env:nucleo_l432kc +; build_flags = -D CAN_RX_TEST=1 diff --git a/src/CAN_FRAME_TEST.c b/src/CAN_FRAME_TEST.c index bca3a33..80b949c 100644 --- a/src/CAN_FRAME_TEST.c +++ b/src/CAN_FRAME_TEST.c @@ -32,8 +32,8 @@ void can_frame_test_main(void) struct can_frame frame = { .flags = 0, - .id = 0x123, - .dlc = 8, + .id = 0x456, + .dlc = 4, .data = {1, 2, 3, 4, 5, 6, 7, 8}, }; diff --git a/src/app.cpp b/src/app.cpp index 6be897e..1e3b3c6 100644 --- a/src/app.cpp +++ b/src/app.cpp @@ -34,7 +34,7 @@ static PI_controller pi_controller(Kp, Ti, T); static InitializationState initialization_state; // Pre-operational velur á milli stýringanna með 'c' -static PreOperationalState pre_operational_state(p_controller, pi_controller); +static PreOperationalState pre_operational_state; static OperationalState operational_state; static StoppedState stopped_state; @@ -42,6 +42,7 @@ static StoppedState stopped_state; static MotorController motor_controller( encoder, motor, + p_controller, pi_controller, // Sjálfgefin stýring initialization_state, pre_operational_state, @@ -107,6 +108,15 @@ extern "C" void motor_application_main(void) motor_controller.update(); + static NodeState last_state = NodeState::INITIALIZATION; + + const NodeState now = motor_controller.get_state(); + if (now != last_state) + { + printk("get_state: %d -> %d\n", (int)last_state, (int)now); + last_state = now; + } + // Sleep until the encoder has a new speed sample (every 4 ms), so // the controller runs right after sampling. The timeout only // matters if the timer stops; the thread is idle while it waits. diff --git a/src/motor_controller.cpp b/src/motor_controller.cpp index 3482e54..7c0aab2 100644 --- a/src/motor_controller.cpp +++ b/src/motor_controller.cpp @@ -4,7 +4,8 @@ MotorController::MotorController( RotaryEncoderDriver &encoder, MotorDriver &motor, - P_controller &controller, + P_controller &p_controller, + P_controller &pi_controller, State &initialization, State &pre_operational, State &operational, @@ -17,9 +18,14 @@ MotorController::MotorController( stopped_state(stopped), encoder(encoder), motor(motor), - controller(&controller), + p_controller(p_controller), + pi_controller(pi_controller), + controller(&pi_controller), // PI er sjálfgefin stýring led(LED_PIN), - reference_rpm(DEFAULT_REFERENCE_RPM), + target_rpm(DEFAULT_TARGET_RPM), + reference_rpm(DEFAULT_TARGET_RPM), + profile_acceleration(0), + profile_deceleration(0), last_actual_rpm(0.0), last_pwm(0.0) { @@ -92,6 +98,8 @@ bool MotorController::run_speed_controller() return false; } + reference_rpm = target_rpm; + const double actual = encoder.rpm(); const double pwm_percent = @@ -135,28 +143,71 @@ void MotorController::reset_controller() controller->reset(); } +void MotorController::reset_parameters() +{ + p_controller.restore_defaults(); + pi_controller.restore_defaults(); + + select_controller(ControllerType::PI); + set_target_rpm(DEFAULT_TARGET_RPM); + + set_profile_acceleration(0); + set_profile_deceleration(0); + +} + void MotorController::set_Kp(double value) { controller->set_Kp(value); } -void MotorController::set_Ti(double value) +bool MotorController::set_Ti(double value) { - controller->set_Ti(value); + return controller->set_Ti(value); } -void MotorController::set_controller(P_controller& new_controller) +void MotorController::select_controller(ControllerType type) { - controller = &new_controller; + if (type == ControllerType::PI) + { + controller = &pi_controller; + } + else + { + controller = &p_controller; + } controller->reset(); } +ControllerType MotorController::get_controller_type() const +{ + if (controller == &pi_controller) + { + return ControllerType::PI; + } + else + { + return ControllerType::P; + } +} + void MotorController::to_stopped() { transition_to(stopped_state); } +NodeState MotorController::get_state() const +{ + if (current_state == nullptr) + { + return NodeState::INITIALIZATION; + } + else{ + return current_state->id(); + } +} + bool MotorController::fault_active() { return motor.fault_active(); diff --git a/src/operational_state.cpp b/src/operational_state.cpp index c3e8923..9ee5422 100644 --- a/src/operational_state.cpp +++ b/src/operational_state.cpp @@ -13,6 +13,10 @@ void OperationalState::enter() sample_count = 0; + // Tímabundið til þess að prófa undirbúning CAN communication + printk("Kp=%.4f Ti=%.4f Target=%.0f RPM\n", + context->get_Kp(), context->get_Ti(), context->get_target_rpm()); + #if LOG_CSV printk("time_ms,ref_rpm,actual_rpm,pwm\n"); #endif @@ -50,7 +54,7 @@ void OperationalState::command(char c) { if (c == 'r' || c == 'R') { - printk("Command: reset\n"); + printk("Command: reset communication\n"); // Heldur Kp og Ti gildum context->to_initialization(); } @@ -61,6 +65,31 @@ void OperationalState::command(char c) context->to_pre_operational(); } + + else if (c == '+') // Bætti þessu við til þess að prófa að rampa rpm í operational state + { + context->set_target_rpm(context->get_target_rpm() + 10); + printk("Target: %.0f RPM\n", context->get_target_rpm()); + } + else if (c == '-') + { + context->set_target_rpm(context->get_target_rpm() - 10); + printk("Target: %.0f RPM\n", context->get_target_rpm()); + } + + // Tímabundið til þess að testa reset parameters + else if (c == 'x' || c == 'X') + { + printk("Command: reset node\n"); + context->reset_parameters(); + context->to_initialization(); + } + + else if (c == '?') + { + printk("Actual=%.0f RPM Duty=%.1f\n", + context->get_actual_rpm(), context->get_duty_percent()); + } } diff --git a/src/p_controller.cpp b/src/p_controller.cpp index fc82027..de861cb 100644 --- a/src/p_controller.cpp +++ b/src/p_controller.cpp @@ -1,7 +1,8 @@ #include "p_controller.h" P_controller::P_controller(double Kp) - : Kp(Kp) + : Kp(Kp), + default_Kp(Kp) { } @@ -11,3 +12,9 @@ double P_controller::update(double ref, double actual) return Kp * error; } + +void P_controller::restore_defaults() +{ + Kp = default_Kp; + reset(); +} diff --git a/src/pi_controller.cpp b/src/pi_controller.cpp index eaf1f6b..d6d9d99 100644 --- a/src/pi_controller.cpp +++ b/src/pi_controller.cpp @@ -1,6 +1,6 @@ #include "pi_controller.h" -PI_controller::PI_controller(double Kp, double Ti, double T) : P_controller(Kp), Ti(Ti), T(T), integral(0.0) +PI_controller::PI_controller(double Kp, double Ti, double T) : P_controller(Kp), Ti(Ti), default_Ti(Ti), T(T), integral(0.0) { } @@ -30,7 +30,21 @@ void PI_controller::reset() integral = 0.0; } -void PI_controller::set_Ti(double value) +bool PI_controller::set_Ti(double value) { - Ti = value; + if (value > 0.0) + { + Ti = value; + return true; + } + else + { + return false; + } +} + +void PI_controller::restore_defaults() +{ + P_controller::restore_defaults(); + Ti = default_Ti; } diff --git a/src/pre_operational_state.cpp b/src/pre_operational_state.cpp index c1c7cd2..c18a134 100644 --- a/src/pre_operational_state.cpp +++ b/src/pre_operational_state.cpp @@ -1,17 +1,18 @@ #include "pre_operational_state.h" #include "motor_controller.h" -#include "p_controller.h" +// #include "p_controller.h" #include #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), +// PreOperationalState::PreOperationalState(P_controller &p_controller, P_controller &pi_controller) +PreOperationalState::PreOperationalState() + // : p_controller(p_controller), + // pi_controller(pi_controller), + // pi_selected(true), + : previous_time(0), led_state(false), input_mode(NONE), input_index(0) @@ -23,7 +24,8 @@ void PreOperationalState::enter() printk("State: Pre-operational\n"); // The selection survives a reset, so report it (used by capture_serial.py). - printk("Controller: %s\n", pi_selected ? "PI" : "P"); + printk("Controller: %s\n", (context->get_controller_type() == ControllerType::PI) ? "PI" : "P"); + // Prófaði þetta, veit ekki hvort þetta skili bool context->stop_motor(); @@ -79,9 +81,20 @@ void PreOperationalState::command(char c) } else if (input_mode == ENTER_TI) { - context->set_Ti(value); + if (context->set_Ti(value)) + { + printk("Ti set to: %.4f\n", value); + } + else + { + printk("Ti rejected, must be > 0"); + } + } + else if (input_mode == ENTER_RPM) + { + context->set_target_rpm(value); - printk("Ti set to: %.4f\n", value); + printk("Target RPM set to: %.0f\n", value); } } @@ -138,20 +151,28 @@ void PreOperationalState::command(char c) printk("Enter Ti and press Enter:\n"); } + else if (c == 'v' || c == 'V') + { + input_mode = ENTER_RPM; + input_index = 0; + + printk("Enter RPM and press Enter:\n"); + } + // Select controller algorithm, the controller is passed to the motor controller else if (c == 'c' || c == 'C') { - pi_selected = !pi_selected; - - if (pi_selected) + if (context->get_controller_type() == ControllerType::P) { - context->set_controller(pi_controller); - printk("Controller: PI\n"); + context->select_controller(ControllerType::PI); + printk("Controller: PI, Kp=%.4f, Ti=%.4f\n", + context->get_Kp(), context->get_Ti()); } else { - context->set_controller(p_controller); - printk("Controller: P\n"); + context->select_controller(ControllerType::P); + printk("Controller: P, Kp=%.4f\n", + context->get_Kp()); } } }