From 48ae7f74a3c3ef6a2278a620738793845fa85109 Mon Sep 17 00:00:00 2001 From: Pablo Date: Mon, 12 Jun 2023 22:08:02 +0200 Subject: [PATCH 01/10] health load and unload ready --- Core/Inc/VCU.hpp | 54 +++++--- .../VCU_TCP/IncomingOrders.hpp | 30 ++++- .../VCU_Communications/VCU_TCP/VCU_TCP.hpp | 13 ++ .../VCU_Communications/VCU_UDP/Packets.hpp | 16 +++ .../VCU_Communications/VCU_UDP/VCU_UDP.hpp | 15 +++ Core/Inc/VCU_Data/VCU_Data.hpp | 1 + Core/Inc/VCU_Mode/VCU_Mode.hpp | 2 +- .../VCU_DynamicLevStateMachine.hpp | 53 ++++++++ .../VCU_HealtCheckLoadStateMachine.hpp | 97 +++++++++++++++ .../VCU_HealtCheckUnloadStateMachine.hpp | 112 +++++++++++++++++ .../VCU_SpecificStateMachine.hpp | 68 ++++++++++ .../Inc/VCU_StateMachine/VCU_StateMachine.hpp | 116 ++++++++++++++++++ .../VCU_StaticLevStateMachine.hpp | 46 +++++++ .../VCU_TractionStateMachine.hpp | 48 ++++++++ Core/Inc/VCU_Utilities/VCU_Includes.hpp | 39 ++++++ 15 files changed, 689 insertions(+), 21 deletions(-) create mode 100644 Core/Inc/VCU_StateMachine/VCU_DynamicLevStateMachine.hpp create mode 100644 Core/Inc/VCU_StateMachine/VCU_HealtCheckLoadStateMachine.hpp create mode 100644 Core/Inc/VCU_StateMachine/VCU_HealtCheckUnloadStateMachine.hpp create mode 100644 Core/Inc/VCU_StateMachine/VCU_SpecificStateMachine.hpp create mode 100644 Core/Inc/VCU_StateMachine/VCU_StateMachine.hpp create mode 100644 Core/Inc/VCU_StateMachine/VCU_StaticLevStateMachine.hpp create mode 100644 Core/Inc/VCU_StateMachine/VCU_TractionStateMachine.hpp create mode 100644 Core/Inc/VCU_Utilities/VCU_Includes.hpp diff --git a/Core/Inc/VCU.hpp b/Core/Inc/VCU.hpp index 5d1433c..c696ad9 100644 --- a/Core/Inc/VCU.hpp +++ b/Core/Inc/VCU.hpp @@ -1,21 +1,6 @@ #pragma once -#include -#include -#include "VCU_Pinout/Pinout.hpp" -#include "VCU_Mode/VCU_Mode.hpp" -#include "VCU_Data/VCU_Data.hpp" -#include "VCU_Sensors/VCU_EnviromentalSensors.hpp" -#include "VCU_Sensors/VCU_RegulatorSensor.hpp" -#include "VCU_Sensors/VCU_Reed.hpp" -#include "VCU_Actuators/VCU_LedsActuator.hpp" -#include "VCU_Actuators/VCU_RegulatorActuator.hpp" -#include "VCU_Actuators/VCU_ValveActuator.hpp" -#include "VCU_Brakes/VCU_Brakes.hpp" -#include "VCU_Utilities/VCU_Types.hpp" -#include "VCU_Communications/VCU_TCP/IncomingOrders.hpp" -#include "VCU_Communications/VCU_UDP/Packets.hpp" - +#include "VCU_Utilities/VCU_Includes.hpp" namespace VCU{ template class VCU_CLASS; @@ -52,6 +37,43 @@ namespace VCU{ } }; + template<> class VCU_CLASS{ + public: + static VCU_CLASS* vcu; + + Data data; + Brakes brakes; + TCP tcp_handler; + UDP udp_handler; + IncomingOrders incoming_orders; + Packets packets; + EncoderSensor encoder; + GeneralStateMachine state_machine_handler; + + VCU_CLASS():data(), brakes(data), tcp_handler(), udp_handler(), incoming_orders(data), packets(data), + encoder(Pinout::TAPE1, Pinout::TAPE2, &data.tapes_position, &data.tapes_direction, &data.tapes_speed, &data.tapes_acceleration) + ,state_machine_handler(data, brakes, tcp_handler, encoder) + {} + + void init(){ + STLIB::start(); + brakes.init(); + udp_handler.init(); + tcp_handler.init(); + } + + static void read_brakes_sensors(){ + vcu->brakes.read(); + } + + static void send_to_backend(){ + vcu->udp_handler.BACKEND_CONNECTION.send(vcu->packets.regulator_packet); + vcu->udp_handler.BACKEND_CONNECTION.send(vcu->packets.pressure_packets); + vcu->udp_handler.BACKEND_CONNECTION.send(vcu->packets.bottle_temperature_packet); + vcu->udp_handler.BACKEND_CONNECTION.send(vcu->packets.reed_packet); + } + }; + void set_regulator_pressure(){ VCU_CLASS::vcu->brakes.set_regulator_pressure(VCU_CLASS::vcu->incoming_orders.new_pressure); } diff --git a/Core/Inc/VCU_Communications/VCU_TCP/IncomingOrders.hpp b/Core/Inc/VCU_Communications/VCU_TCP/IncomingOrders.hpp index 3c5cb49..c396f69 100644 --- a/Core/Inc/VCU_Communications/VCU_TCP/IncomingOrders.hpp +++ b/Core/Inc/VCU_Communications/VCU_TCP/IncomingOrders.hpp @@ -30,9 +30,31 @@ namespace VCU{ public: float new_pressure = 0; - IncomingOrders(Data& data) : hardware_reset_order(209,hardware_reset), - set_regulator_pressure_order(210, set_regulator_pressure, &new_pressure), - brake_order(215, brake), unbrake_order(216, unbrake), - disable_emergency_tape_order(217,disable_emergency_tape), enable_emergency_tape_order(218,enable_emergency_tape){} + IncomingOrders(Data& data) : + hardware_reset_order(209,hardware_reset), + set_regulator_pressure_order(210, set_regulator_pressure, &new_pressure), + brake_order(215, brake), unbrake_order(216, unbrake), + disable_emergency_tape_order(217,disable_emergency_tape), enable_emergency_tape_order(218,enable_emergency_tape) + {} + }; + + template<> + class IncomingOrders{ + //TODO: Hacer todas las ordenes para el vehicle y hacer las states + StackOrder<0> hardware_reset_order; + StackOrder<4,float> set_regulator_pressure_order; + StackOrder<0> brake_order; + StackOrder<0> unbrake_order; + StackOrder<0> disable_emergency_tape_order; + StackOrder<0> enable_emergency_tape_order; + public: + float new_pressure = 0; + + IncomingOrders(Data& data) : + hardware_reset_order(209,hardware_reset), + set_regulator_pressure_order(210, set_regulator_pressure, &new_pressure), + brake_order(215, brake), unbrake_order(216, unbrake), + disable_emergency_tape_order(217,disable_emergency_tape), enable_emergency_tape_order(218,enable_emergency_tape) + {} }; } diff --git a/Core/Inc/VCU_Communications/VCU_TCP/VCU_TCP.hpp b/Core/Inc/VCU_Communications/VCU_TCP/VCU_TCP.hpp index a69466b..fb9265c 100644 --- a/Core/Inc/VCU_Communications/VCU_TCP/VCU_TCP.hpp +++ b/Core/Inc/VCU_Communications/VCU_TCP/VCU_TCP.hpp @@ -21,4 +21,17 @@ namespace VCU{ } }; + + template<> + class TCP{ + public: + ServerSocket BACKEND_CONNECTION; + TCP() {} + void init(){ + BACKEND_CONNECTION = ServerSocket(VCU_IP, SERVER_PORT); + } + void send_to_master(Order& order){ + BACKEND_CONNECTION.send_order(order); + } + }; } diff --git a/Core/Inc/VCU_Communications/VCU_UDP/Packets.hpp b/Core/Inc/VCU_Communications/VCU_UDP/Packets.hpp index 6ad62c3..7f09fb4 100644 --- a/Core/Inc/VCU_Communications/VCU_UDP/Packets.hpp +++ b/Core/Inc/VCU_Communications/VCU_UDP/Packets.hpp @@ -17,4 +17,20 @@ namespace VCU{ pressure_packets(214, &data.high_pressure1, &data.low_pressure1, &data.low_pressure2) {} }; + + template<> + class Packets{ + public: + //TODO: Hay que revisar todos los paquetes + StackPacket<9,VALVE_STATE, float, float> regulator_packet; + StackPacket<5,REED_STATE, REED_STATE, REED_STATE, REED_STATE, bool> reed_packet; + StackPacket<16,double,double> bottle_temperature_packet; + StackPacket<12,float,float,float> pressure_packets; + Packets(Data& data) : + regulator_packet(211, &data.valve_state, &data.regulator_reference_pressure, &data.regulator_real_pressure), + reed_packet(212, &data.reed1, &data.reed2, &data.reed3, &data.reed4, &data.reeds_ok), + bottle_temperature_packet(213, &data.bottle_temperature1, &data.bottle_temperature2), + pressure_packets(214, &data.high_pressure1, &data.low_pressure1, &data.low_pressure2) + {} + }; } diff --git a/Core/Inc/VCU_Communications/VCU_UDP/VCU_UDP.hpp b/Core/Inc/VCU_Communications/VCU_UDP/VCU_UDP.hpp index 07de35f..d216bc2 100644 --- a/Core/Inc/VCU_Communications/VCU_UDP/VCU_UDP.hpp +++ b/Core/Inc/VCU_Communications/VCU_UDP/VCU_UDP.hpp @@ -19,4 +19,19 @@ namespace VCU{ BACKEND_CONNECTION.send(packet); } }; + + template<> + class UDP { + public: + DatagramSocket BACKEND_CONNECTION; + UDP() {} + void init(){ + //TODO: Conectarse a todas las placas + BACKEND_CONNECTION = DatagramSocket(VCU_IP, UDP_PORT, BACKEND_IP, UDP_PORT); + BACKEND_CONNECTION.reconnect(); + } + void send_to_backend(Packet& packet){ + BACKEND_CONNECTION.send(packet); + } + }; } diff --git a/Core/Inc/VCU_Data/VCU_Data.hpp b/Core/Inc/VCU_Data/VCU_Data.hpp index 2d1cd37..ad6668e 100644 --- a/Core/Inc/VCU_Data/VCU_Data.hpp +++ b/Core/Inc/VCU_Data/VCU_Data.hpp @@ -35,6 +35,7 @@ namespace VCU{ PinState emergeny_tape_enable = PinState::OFF; PinState emergency_tape = PinState::OFF; + float emergency_distance = 6000; // 6000mm float high_pressure1 = 0.0f; float low_pressure1 = 0.0f; diff --git a/Core/Inc/VCU_Mode/VCU_Mode.hpp b/Core/Inc/VCU_Mode/VCU_Mode.hpp index d5d1bf4..d79414b 100644 --- a/Core/Inc/VCU_Mode/VCU_Mode.hpp +++ b/Core/Inc/VCU_Mode/VCU_Mode.hpp @@ -5,5 +5,5 @@ namespace VCU{ BRAKE_VALIDATION, POSITION_VALIDATION, VEHICLE, -}; + }; } diff --git a/Core/Inc/VCU_StateMachine/VCU_DynamicLevStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_DynamicLevStateMachine.hpp new file mode 100644 index 0000000..56f002b --- /dev/null +++ b/Core/Inc/VCU_StateMachine/VCU_DynamicLevStateMachine.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include "VCU_Utilities/VCU_Includes.hpp" + +namespace VCU{ + template class DynamicLevStateMachine; + + template<> + class DynamicLevStateMachine{ + public: + Data& data; + Brakes& brakes; + TCP& tcp_handler; + EncoderSensor& encoder; + StateMachine state_machine; + + enum DynamicLevStates{ + Idle, + TakeOff, + Accelerating, + Brake, + Landing, + }; + + DynamicLevStateMachine(Data& data, Brakes&brakes, TCP& tcp, EncoderSensor& encoder) : + data(data),brakes(brakes),tcp_handler(tcp), encoder(encoder) + {} + + void add_transitions(){} + + void add_on_enter_actions(){} + + void add_on_exit_actions(){} + + void register_timed_actions(){} + + void init(){ + state_machine = {Idle}; + state_machine.add_state(TakeOff); + state_machine.add_state(Accelerating); + state_machine.add_state(Brake); + state_machine.add_state(Landing); + + + add_on_enter_actions(); + add_on_exit_actions(); + add_transitions(); + register_timed_actions(); + add_transitions(); + } + + }; +} diff --git a/Core/Inc/VCU_StateMachine/VCU_HealtCheckLoadStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_HealtCheckLoadStateMachine.hpp new file mode 100644 index 0000000..b7e5fde --- /dev/null +++ b/Core/Inc/VCU_StateMachine/VCU_HealtCheckLoadStateMachine.hpp @@ -0,0 +1,97 @@ +#pragma once + +#include "VCU_Utilities/VCU_Includes.hpp" + +namespace VCU{ + template class LoadStateMachine; + + template<> + class LoadStateMachine{ + public: + Data& data; + Brakes& brakes; + TCP& tcp_handler; + EncoderSensor& encoder; + StateMachine state_machine; + + enum UnloadStates{ + Idle, + Pushing, + Accelerating, + Braking, + }; + + LoadStateMachine(Data& data, Brakes&brakes, TCP& tcp, EncoderSensor& encoder) : + data(data),brakes(brakes),tcp_handler(tcp), encoder(encoder) + {} + + void add_transitions(){ + //TODO: IDLE -> Pushing con una state orden + + //TODO: Pushing -> Accelerating con una state orden + + state_machine.add_transition(Accelerating, Braking, [&](){ + return data.tapes_position > data.emergency_distance; + }); + + state_machine.add_transition(Braking, Idle, [&](){ + return data.tapes_acceleration == 0; //TODO: Muy importante hacer esto con la IMU + }); + } + + void add_on_enter_actions(){ + state_machine.add_enter_action([&](){ + brakes.enable_emergency_brakes(); + }, Idle); + + state_machine.add_enter_action([&](){ + brakes.disable_emergency_brakes(); + brakes.not_brake(); + }, Pushing); + + state_machine.add_enter_action([&](){ + encoder.reset(); + //TODO: set engine parameters + //TODO: send start engine order + }, Accelerating); + + state_machine.add_enter_action([&](){ + //TODO: send engine brake order + }, Braking); + } + + void add_on_exit_actions(){ + state_machine.add_exit_action([&](){ + if(data.emergency_tape == PinState::ON){ + ErrorHandler("The vehicle is still in emergency zone after Health&Load procedure"); + }else{ + brakes.brake(); //Siguiendo el FDD kenos debe quedar frenado con pneumatica hasta empezar una demostración + } + }, Braking); + + } + + void register_timed_actions(){ + state_machine.add_low_precision_cyclic_action([&](){ + if(data.tapes_direction < 1){ //!TODO: MUY IMPORTANTE comprobar que 1 es hacia adelante!!!!!!!!! + ErrorHandler("The vehicle is moving backwards when it should be moving forward during load procedure"); + } + }, (ms)1, Accelerating); + + } + + void init(){ + state_machine = {Idle}; + state_machine.add_state(Pushing); + state_machine.add_state(Accelerating); + state_machine.add_state(Braking); + + add_on_enter_actions(); + add_on_exit_actions(); + add_transitions(); + register_timed_actions(); + add_transitions(); + } + + }; +} diff --git a/Core/Inc/VCU_StateMachine/VCU_HealtCheckUnloadStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_HealtCheckUnloadStateMachine.hpp new file mode 100644 index 0000000..39065fe --- /dev/null +++ b/Core/Inc/VCU_StateMachine/VCU_HealtCheckUnloadStateMachine.hpp @@ -0,0 +1,112 @@ +#pragma once + +#include "VCU_Utilities/VCU_Includes.hpp" + +namespace VCU{ + + template class UnloadStateMachine; + + template<> + class UnloadStateMachine{ + static constexpr float distance_offset = 250; //250mm + + public: + Data& data; + Brakes& brakes; + TCP& tcp_handler; + EncoderSensor& encoder; + StateMachine state_machine; + + enum LoadStates{ + Idle, + Returning, + Crawling, + Braking, + + }; + + UnloadStateMachine(Data& data, Brakes&brakes, TCP& tcp, EncoderSensor& encoder) : + data(data),brakes(brakes),tcp_handler(tcp), encoder(encoder) + {} + + void add_transitions(){ + //TODO: IDLE -> Returning con una state orden + //Quitar los frenos debe hacerse manualmente con una orden una vez finalizado el procedure + //porque segun el fdd el pod debe estar frenado hasta el momento que se va a sacar + + state_machine.add_transition(Returning, Crawling, [&](){ + return data.emergency_tape == PinState::ON; //Si hemos llegado a la zona de emergencia pasamos a fase crawling + }); + + state_machine.add_transition(Crawling, Braking, [&](){ + return data.emergency_tape == PinState::OFF; //Si hemos llegado al final de la zona de emergencia frenamos + }); + + state_machine.add_transition(Braking, Idle, [&](){ + return data.tapes_acceleration == 0; //TODO: Muy importante hacer esto con la IMU + }); + } + + void add_on_enter_actions(){ + state_machine.add_enter_action([&](){ + brakes.enable_emergency_brakes(); + brakes.brake(); + }, Idle); + + state_machine.add_enter_action([&](){ + brakes.not_brake(); + brakes.disable_emergency_brakes(); + //TODO: set engine parameters + //TODO: send start engine order + }, Returning); + + state_machine.add_enter_action([&](){ + //Esto es por si queremos cambiar los parametros (como ir mas lento) + //TODO: set engine parameters + }, Crawling); + + state_machine.add_enter_action([&](){ + //TODO: send engine stop order + brakes.brake(); + }, Braking); + + } + + void add_on_exit_actions(){} + + void register_timed_actions(){ + state_machine.add_low_precision_cyclic_action([&](){ + if(data.tapes_speed != 0.0){ + ErrorHandler("The vehicle is moving when it shouldn't, in the IDLE state of the Health&Unload procedure"); + } + }, (ms)1, Crawling); + + state_machine.add_low_precision_cyclic_action([&](){ + if(data.tapes_direction > 0){ //!TODO: MUY IMPORTANTE comprobar que01 es hacia atras!!!!!!!!! + ErrorHandler("The vehicle is moving forward when it should be moving backwards during Health&Unload procedure"); + } + }, (ms)1, Returning); + + state_machine.add_low_precision_cyclic_action([&](){ + if(data.tapes_direction > 0){ //!TODO: MUY IMPORTANTE comprobar que01 es hacia atras!!!!!!!!! + ErrorHandler("The vehicle is moving forward when it should be moving backwards during Health&Unload procedure"); + } + }, (ms)1, Crawling); + } + + void init(){ + state_machine = {Idle}; + state_machine.add_state(Returning); + state_machine.add_state(Crawling); + state_machine.add_state(Braking); + + add_on_enter_actions(); + add_on_exit_actions(); + add_transitions(); + register_timed_actions(); + add_transitions(); + + } + + }; +} diff --git a/Core/Inc/VCU_StateMachine/VCU_SpecificStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_SpecificStateMachine.hpp new file mode 100644 index 0000000..b6cf064 --- /dev/null +++ b/Core/Inc/VCU_StateMachine/VCU_SpecificStateMachine.hpp @@ -0,0 +1,68 @@ +#pragma once + +#include "VCU_Utilities/VCU_Includes.hpp" + +namespace VCU{ + template class SpecificStateMachine; + + template<> + class SpecificStateMachine{ + public: + Data& data; + Brakes& brakes; + TCP& tcp_handler; + EncoderSensor& encoder; + LoadStateMachine health_load_state_machine; + UnloadStateMachine health_unload_state_machine; + TractionLevStateMachine traction_state_machine; + StaticLevStateMachine static_lev_state_machine; + DynamicLevStateMachine dynamic_lev_state_machine; + StateMachine state_machine; + //TODO: Añadir todas las statemachines + + enum SpecificStates{ + Idle, + Unload, + Load, + DynamicLev, + StaticLev, + Traction, + }; + + SpecificStateMachine(Data& data, Brakes&brakes, TCP& tcp, EncoderSensor& encoder) : + data(data),brakes(brakes),tcp_handler(tcp), encoder(encoder), + health_load_state_machine(data, brakes, tcp, encoder), + health_unload_state_machine(data, brakes, tcp, encoder), + traction_state_machine(data, brakes, tcp, encoder), + static_lev_state_machine(data, brakes, tcp, encoder), + dynamic_lev_state_machine(data, brakes, tcp, encoder) + {} + + void add_transitions(){} + + void add_on_enter_actions(){} + + void add_on_exit_actions(){} + + void register_timed_actions(){} + + void init(){ + state_machine = {Idle}; + state_machine.add_state(Unload); + state_machine.add_state(Load); + state_machine.add_state(DynamicLev); + state_machine.add_state(StaticLev); + state_machine.add_state(Traction); + + add_on_enter_actions(); + add_on_exit_actions(); + add_transitions(); + register_timed_actions(); + add_transitions(); + + //TODO: Añadir todas las state machines + //state_machine.add_state_machine(.state_machine, OPERATIONAL); + } + + }; +} diff --git a/Core/Inc/VCU_StateMachine/VCU_StateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_StateMachine.hpp new file mode 100644 index 0000000..17ab0b5 --- /dev/null +++ b/Core/Inc/VCU_StateMachine/VCU_StateMachine.hpp @@ -0,0 +1,116 @@ +#pragma once + +#include "VCU_Utilities/VCU_Includes.hpp" + +namespace VCU{ + template class GeneralStateMachine; + + template<> + class GeneralStateMachine{ + public: + Data& data; + Brakes& brakes; + TCP& tcp_handler; + EncoderSensor& encoder; + SpecificStateMachine specific_state_machine; + StateMachine general_state_machine; + + bool tcp_timeout = false; + + static constexpr uint16_t max_tcp_connection_timeout = 30000; //ms + + enum States{ + INITIAL, + OPERATIONAL, + FAULT + }; + + GeneralStateMachine(Data& data, Brakes&brakes, TCP& tcp, EncoderSensor& encoder) : + data(data),brakes(brakes),tcp_handler(tcp), encoder(encoder), specific_state_machine(data, brakes, tcp, encoder) + {} + + void add_transitions(){ + general_state_machine.add_transition(INITIAL, OPERATIONAL, [&](){ + return tcp_handler.BACKEND_CONNECTION.state == ServerSocket::ServerState::ACCEPTED ; + }); + general_state_machine.add_transition(INITIAL, FAULT, [&](){ + if(tcp_timeout){ + ErrorHandler("TCP connections could not be established in time and timed out"); + } + return tcp_timeout; + }); + general_state_machine.add_transition(OPERATIONAL, FAULT, [&](){ + if(tcp_handler.BACKEND_CONNECTION.state != ServerSocket::ServerState::ACCEPTED){ + ErrorHandler("TCP connections fell"); + return true; + } + return false; + }); + } + + void add_on_enter_actions(){ + general_state_machine.add_enter_action([&](){ + Time::set_timeout(max_tcp_connection_timeout, [&](){ + if(not (tcp_handler.BACKEND_CONNECTION.state == ServerSocket::ServerState::ACCEPTED)){ + tcp_timeout = true; + } + }); + }, INITIAL); + + Time::set_timeout(max_tcp_connection_timeout, [&](){ + if(not (tcp_handler.BACKEND_CONNECTION.state == ServerSocket::ServerState::ACCEPTED)){ + tcp_timeout = true; + } + }); + + general_state_machine.add_enter_action([&](){ + //TODO: encender led fault + brakes.brake(); + brakes.enable_emergency_brakes(); + + }, FAULT); + + general_state_machine.add_enter_action([&](){ + //TODO: encender led operational + brakes.enable_emergency_brakes(); + }, OPERATIONAL); + } + + void add_on_exit_actions(){ + general_state_machine.add_exit_action([&](){ + //TODO: apagar led fault + brakes.not_brake(); + }, FAULT); + general_state_machine.add_exit_action([&](){ + //TODO: apagar led operational + }, OPERATIONAL); + general_state_machine.add_exit_action([&](){ + //TODO: apagar led operational + }, INITIAL); + } + + void register_timed_actions(){ + //TODO: general_state_machine.add_low_precision_cyclic_action([&](){actuators.led_operational.toggle();}, 150ms, INITIAL); + general_state_machine.add_low_precision_cyclic_action(ProtectionManager::check_protections, (ms)1, OPERATIONAL); + } + + void init(){ + general_state_machine = {INITIAL}; + general_state_machine.add_state(OPERATIONAL); + general_state_machine.add_state(FAULT); + + ProtectionManager::link_state_machine(general_state_machine, FAULT); + ProtectionManager::set_id(Boards::ID::VCU); + + add_on_enter_actions(); + add_on_exit_actions(); + add_transitions(); + register_timed_actions(); + add_transitions(); + + general_state_machine.add_state_machine(specific_state_machine.state_machine, OPERATIONAL); + } + }; +} + + diff --git a/Core/Inc/VCU_StateMachine/VCU_StaticLevStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_StaticLevStateMachine.hpp new file mode 100644 index 0000000..70759c9 --- /dev/null +++ b/Core/Inc/VCU_StateMachine/VCU_StaticLevStateMachine.hpp @@ -0,0 +1,46 @@ +#pragma once + +#include "VCU_Utilities/VCU_Includes.hpp" + +namespace VCU{ + template class StaticLevStateMachine; + + template<> + class StaticLevStateMachine{ + public: + Data& data; + Brakes& brakes; + TCP& tcp_handler; + EncoderSensor& encoder; + StateMachine state_machine; + + enum DynamicLevStates{ + LevOff, + LevOn, + }; + + StaticLevStateMachine(Data& data, Brakes&brakes, TCP& tcp, EncoderSensor& encoder) : + data(data),brakes(brakes),tcp_handler(tcp), encoder(encoder) + {} + + void add_transitions(){} + + void add_on_enter_actions(){} + + void add_on_exit_actions(){} + + void register_timed_actions(){} + + void init(){ + state_machine = {LevOff}; + state_machine.add_state(LevOn); + + add_on_enter_actions(); + add_on_exit_actions(); + add_transitions(); + register_timed_actions(); + add_transitions(); + } + + }; +} diff --git a/Core/Inc/VCU_StateMachine/VCU_TractionStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_TractionStateMachine.hpp new file mode 100644 index 0000000..ac2b9c1 --- /dev/null +++ b/Core/Inc/VCU_StateMachine/VCU_TractionStateMachine.hpp @@ -0,0 +1,48 @@ +#pragma once + +#include "VCU_Utilities/VCU_Includes.hpp" + +namespace VCU{ + template class TractionLevStateMachine; + + template<> + class TractionLevStateMachine{ + public: + Data& data; + Brakes& brakes; + TCP& tcp_handler; + EncoderSensor& encoder; + StateMachine state_machine; + + enum DynamicLevStates{ + Idle, + Accelerating, + Brake, + }; + + TractionLevStateMachine(Data& data, Brakes&brakes, TCP& tcp, EncoderSensor& encoder) : + data(data),brakes(brakes),tcp_handler(tcp), encoder(encoder) + {} + + void add_transitions(){} + + void add_on_enter_actions(){} + + void add_on_exit_actions(){} + + void register_timed_actions(){} + + void init(){ + state_machine = {Idle}; + state_machine.add_state(Accelerating); + state_machine.add_state(Brake); + + add_on_enter_actions(); + add_on_exit_actions(); + add_transitions(); + register_timed_actions(); + add_transitions(); + } + + }; +} diff --git a/Core/Inc/VCU_Utilities/VCU_Includes.hpp b/Core/Inc/VCU_Utilities/VCU_Includes.hpp new file mode 100644 index 0000000..3f8c14f --- /dev/null +++ b/Core/Inc/VCU_Utilities/VCU_Includes.hpp @@ -0,0 +1,39 @@ +#pragma once + +#include "ST-LIB.hpp" + +#include "VCU_Utilities/VCU_Types.hpp" +#include "VCU_Data/VCU_Data.hpp" +#include "VCU_Mode/VCU_Mode.hpp" +#include "VCU_Pinout/Pinout.hpp" + +#include "VCU_Sensors/VCU_EnviromentalSensors.hpp" +#include "VCU_Sensors/VCU_RegulatorSensor.hpp" +#include "VCU_Sensors/VCU_Reed.hpp" +#include "VCU_Actuators/VCU_LedsActuator.hpp" +#include "VCU_Actuators/VCU_RegulatorActuator.hpp" +#include "VCU_Actuators/VCU_ValveActuator.hpp" +#include "VCU_Brakes/VCU_Brakes.hpp" + +#include "VCU_Communications/VCU_TCP/VCU_TCP.hpp" +#include "VCU_Communications/VCU_UDP/VCU_UDP.hpp" +#include "VCU_Communications/VCU_TCP/OutgoingOrders.hpp" +#include "VCU_Communications/VCU_TCP/IncomingOrders.hpp" +#include "VCU_Communications/VCU_UDP/Packets.hpp" + +#include "VCU_StateMachine/VCU_HealtCheckLoadStateMachine.hpp" +#include "VCU_StateMachine/VCU_HealtCheckUnloadStateMachine.hpp" +#include "VCU_StateMachine/VCU_DynamicLevStateMachine.hpp" +#include "VCU_StateMachine/VCU_StaticLevStateMachine.hpp" +#include "VCU_StateMachine/VCU_TractionStateMachine.hpp" +#include "VCU_StateMachine/VCU_SpecificStateMachine.hpp" +#include "VCU_StateMachine/VCU_StateMachine.hpp" + + + + + + + + + From 8b2eab7ed8f1afc99931286f10c4da98d3b647b9 Mon Sep 17 00:00:00 2001 From: Pablo Date: Mon, 12 Jun 2023 22:44:53 +0200 Subject: [PATCH 02/10] added static lev --- Core/Inc/VCU_Data/VCU_Data.hpp | 1 - .../VCU_HealtCheckLoadStateMachine.hpp | 7 +++++-- .../VCU_StaticLevStateMachine.hpp | 17 +++++++++++++++-- 3 files changed, 20 insertions(+), 5 deletions(-) diff --git a/Core/Inc/VCU_Data/VCU_Data.hpp b/Core/Inc/VCU_Data/VCU_Data.hpp index ad6668e..2d1cd37 100644 --- a/Core/Inc/VCU_Data/VCU_Data.hpp +++ b/Core/Inc/VCU_Data/VCU_Data.hpp @@ -35,7 +35,6 @@ namespace VCU{ PinState emergeny_tape_enable = PinState::OFF; PinState emergency_tape = PinState::OFF; - float emergency_distance = 6000; // 6000mm float high_pressure1 = 0.0f; float low_pressure1 = 0.0f; diff --git a/Core/Inc/VCU_StateMachine/VCU_HealtCheckLoadStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_HealtCheckLoadStateMachine.hpp index b7e5fde..bfa1b8d 100644 --- a/Core/Inc/VCU_StateMachine/VCU_HealtCheckLoadStateMachine.hpp +++ b/Core/Inc/VCU_StateMachine/VCU_HealtCheckLoadStateMachine.hpp @@ -31,7 +31,7 @@ namespace VCU{ //TODO: Pushing -> Accelerating con una state orden state_machine.add_transition(Accelerating, Braking, [&](){ - return data.tapes_position > data.emergency_distance; + return data.emergency_tape == PinState::OFF; }); state_machine.add_transition(Braking, Idle, [&](){ @@ -42,6 +42,8 @@ namespace VCU{ void add_on_enter_actions(){ state_machine.add_enter_action([&](){ brakes.enable_emergency_brakes(); + brakes.brake(); + }, Idle); state_machine.add_enter_action([&](){ @@ -50,12 +52,13 @@ namespace VCU{ }, Pushing); state_machine.add_enter_action([&](){ - encoder.reset(); //TODO: set engine parameters //TODO: send start engine order }, Accelerating); state_machine.add_enter_action([&](){ + //Quizas hace falta darle un offset (avanzar un poco más) para evitar que entre + //en fault si se mueve ligeremante hacia atras y ha quedado demasido cerca de la emergency tape. //TODO: send engine brake order }, Braking); } diff --git a/Core/Inc/VCU_StateMachine/VCU_StaticLevStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_StaticLevStateMachine.hpp index 70759c9..d1466c0 100644 --- a/Core/Inc/VCU_StateMachine/VCU_StaticLevStateMachine.hpp +++ b/Core/Inc/VCU_StateMachine/VCU_StaticLevStateMachine.hpp @@ -23,9 +23,22 @@ namespace VCU{ data(data),brakes(brakes),tcp_handler(tcp), encoder(encoder) {} - void add_transitions(){} + void add_transitions(){ + //TODO: Las transiciones son enteramente con state orders - void add_on_enter_actions(){} + } + + void add_on_enter_actions(){ + state_machine.add_enter_action([&](){ + //TODO: set lev parameters + //TODO: send lev order + }, LevOn); + + state_machine.add_enter_action([&](){ + //TODO: send stop lev order + }, LevOff); + + } void add_on_exit_actions(){} From cb8d9bb7c6fe28ef142fe28632d2cd451a9e1132 Mon Sep 17 00:00:00 2001 From: Pablo Date: Mon, 12 Jun 2023 22:45:55 +0200 Subject: [PATCH 03/10] ficed static lev --- Core/Inc/VCU_StateMachine/VCU_StaticLevStateMachine.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Core/Inc/VCU_StateMachine/VCU_StaticLevStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_StaticLevStateMachine.hpp index d1466c0..d5381d2 100644 --- a/Core/Inc/VCU_StateMachine/VCU_StaticLevStateMachine.hpp +++ b/Core/Inc/VCU_StateMachine/VCU_StaticLevStateMachine.hpp @@ -30,12 +30,14 @@ namespace VCU{ void add_on_enter_actions(){ state_machine.add_enter_action([&](){ + brakes.not_brake(); //TODO: set lev parameters //TODO: send lev order }, LevOn); state_machine.add_enter_action([&](){ //TODO: send stop lev order + brakes.brake(); }, LevOff); } From 8a1d2d1cbace70cf347b938f0f2b3ce9db64b4b9 Mon Sep 17 00:00:00 2001 From: Pablo Date: Wed, 21 Jun 2023 22:05:59 +0200 Subject: [PATCH 04/10] mega checkpoint --- Core/Inc/VCU.hpp | 49 +++++-- Core/Inc/VCU_Actuators/VCU_Actuators.hpp | 41 ++++++ Core/Inc/VCU_Actuators/VCU_LedsActuator.hpp | 32 ++++- .../VCU_Actuators/VCU_RegulatorActuator.hpp | 8 ++ Core/Inc/VCU_Actuators/VCU_ValveActuator.hpp | 8 ++ Core/Inc/VCU_Brakes/VCU_Brakes.hpp | 20 +-- .../VCU_Communications/VCU_Communications.hpp | 15 ++ .../VCU_TCP/IncomingOrders.hpp | 18 ++- .../VCU_TCP/OutgoingOrders.hpp | 24 ++++ .../VCU_Communications/VCU_TCP/VCU_TCP.hpp | 58 +++++++- .../VCU_Communications/VCU_UDP/Packets.hpp | 19 ++- .../VCU_Communications/VCU_UDP/VCU_UDP.hpp | 70 ++++++++-- Core/Inc/VCU_Data/VCU_Data.hpp | 17 +++ Core/Inc/VCU_Mode/VCU_Mode.hpp | 9 ++ Core/Inc/VCU_Protections/VCU_Protections.hpp | 0 Core/Inc/VCU_Sensors/VCU_Reed.hpp | 3 +- .../VCU_DynamicLevStateMachine.hpp | 8 +- .../VCU_HealtCheckLoadStateMachine.hpp | 59 ++++++-- .../VCU_HealtCheckUnloadStateMachine.hpp | 58 ++++++-- .../VCU_SpecificStateMachine.hpp | 132 ++++++++++++++++-- .../Inc/VCU_StateMachine/VCU_StateMachine.hpp | 116 +++++++++++++-- .../VCU_StaticLevStateMachine.hpp | 13 +- .../VCU_TractionStateMachine.hpp | 12 +- Core/Inc/VCU_Time/VCU_Time.hpp | 15 ++ Core/Inc/VCU_Utilities/VCU_Includes.hpp | 5 +- Core/Inc/VCU_Utilities/VCU_Types.hpp | 13 +- Core/Src/main.cpp | 94 ++++++++++++- 27 files changed, 775 insertions(+), 141 deletions(-) create mode 100644 Core/Inc/VCU_Actuators/VCU_Actuators.hpp create mode 100644 Core/Inc/VCU_Communications/VCU_Communications.hpp create mode 100644 Core/Inc/VCU_Protections/VCU_Protections.hpp diff --git a/Core/Inc/VCU.hpp b/Core/Inc/VCU.hpp index c696ad9..67f6ab3 100644 --- a/Core/Inc/VCU.hpp +++ b/Core/Inc/VCU.hpp @@ -10,23 +10,29 @@ namespace VCU{ static VCU_CLASS* vcu; Data data; - Brakes brakes; + Actuators actuators; TCP tcp_handler; UDP udp_handler; IncomingOrders incoming_orders; Packets packets; + GeneralStateMachine general_state_machine; - VCU_CLASS():data(), brakes(data), tcp_handler(), udp_handler(), incoming_orders(data), packets(data){} + VCU_CLASS(): + data(), actuators(data), tcp_handler(), udp_handler(), incoming_orders(data), packets(data), + general_state_machine(data, actuators, tcp_handler) + {} void init(){ STLIB::start(); - brakes.init(); + actuators.brakes.init(); udp_handler.init(); tcp_handler.init(); + general_state_machine.init(); + data.add_protections(); } static void read_brakes_sensors(){ - vcu->brakes.read(); + vcu->actuators.brakes.read(); } static void send_to_backend(){ @@ -35,6 +41,10 @@ namespace VCU{ vcu->udp_handler.BACKEND_CONNECTION.send(vcu->packets.bottle_temperature_packet); vcu->udp_handler.BACKEND_CONNECTION.send(vcu->packets.reed_packet); } + + static void update_state_machine(){ + vcu->general_state_machine.general_state_machine.check_transitions(); + } }; template<> class VCU_CLASS{ @@ -42,7 +52,8 @@ namespace VCU{ static VCU_CLASS* vcu; Data data; - Brakes brakes; + Actuators actuators; + EnvironmentalSensors environmental_sensors; TCP tcp_handler; UDP udp_handler; IncomingOrders incoming_orders; @@ -50,20 +61,24 @@ namespace VCU{ EncoderSensor encoder; GeneralStateMachine state_machine_handler; - VCU_CLASS():data(), brakes(data), tcp_handler(), udp_handler(), incoming_orders(data), packets(data), + VCU_CLASS():data(), actuators(data), environmental_sensors(data), tcp_handler(), udp_handler(), incoming_orders(data), packets(data), encoder(Pinout::TAPE1, Pinout::TAPE2, &data.tapes_position, &data.tapes_direction, &data.tapes_speed, &data.tapes_acceleration) - ,state_machine_handler(data, brakes, tcp_handler, encoder) + ,state_machine_handler(data, actuators, tcp_handler, encoder) {} void init(){ STLIB::start(); - brakes.init(); + actuators.brakes.init(); udp_handler.init(); tcp_handler.init(); } static void read_brakes_sensors(){ - vcu->brakes.read(); + vcu->actuators.brakes.read(); + } + + static void read_environmental_sensors(){ + vcu->environmental_sensors.read(); } static void send_to_backend(){ @@ -71,27 +86,33 @@ namespace VCU{ vcu->udp_handler.BACKEND_CONNECTION.send(vcu->packets.pressure_packets); vcu->udp_handler.BACKEND_CONNECTION.send(vcu->packets.bottle_temperature_packet); vcu->udp_handler.BACKEND_CONNECTION.send(vcu->packets.reed_packet); + vcu->udp_handler.BACKEND_CONNECTION.send(vcu->packets.environmental_packet); + } + + static void update_state_machine(){ + vcu->state_machine_handler.general_state_machine.check_transitions(); } }; + //Esto hay que modificar Incoming orders para que pueda estar dentro de la clase VCU, aqui fuera no tiene sentido void set_regulator_pressure(){ - VCU_CLASS::vcu->brakes.set_regulator_pressure(VCU_CLASS::vcu->incoming_orders.new_pressure); + VCU_CLASS::vcu->actuators.brakes.set_regulator_pressure(VCU_CLASS::vcu->incoming_orders.new_pressure); } void brake(){ - VCU_CLASS::vcu->brakes.brake(); + VCU_CLASS::vcu->actuators.brakes.brake(); } void unbrake(){ - VCU_CLASS::vcu->brakes.not_brake(); + VCU_CLASS::vcu->actuators.brakes.not_brake(); } void disable_emergency_tape(){ - VCU_CLASS::vcu->brakes.disable_emergency_brakes(); + VCU_CLASS::vcu->actuators.brakes.disable_emergency_brakes(); } void enable_emergency_tape(){ - VCU_CLASS::vcu->brakes.enable_emergency_brakes(); + VCU_CLASS::vcu->actuators.brakes.enable_emergency_brakes(); } } diff --git a/Core/Inc/VCU_Actuators/VCU_Actuators.hpp b/Core/Inc/VCU_Actuators/VCU_Actuators.hpp new file mode 100644 index 0000000..7ed24e2 --- /dev/null +++ b/Core/Inc/VCU_Actuators/VCU_Actuators.hpp @@ -0,0 +1,41 @@ +/* + * VCU_Actuators.hpp + * + * Created on: Jun 14, 2023 + * Author: stefancostea & Pablo + */ + +#pragma once + +#include "VCU_Brakes/VCU_Brakes.hpp" +#include "VCU_LedsActuator.hpp" + +namespace VCU{ + template class Actuators; + + template<> + class Actuators{ + public: + Brakes brakes; + DigitalOutput led_sleep, led_flash, led_fault, led_operational, led_can; + Actuators(Data& data) : + brakes(data), + led_sleep(Pinout::SLEEP_LED), led_flash(Pinout::FLASH_LED), led_fault(Pinout::FAULT_LED), + led_operational(Pinout::OPERATIONAL_LED), led_can(Pinout::CAN_LED) + {} + }; + + template<> + class Actuators{ + public: + Brakes brakes; + LEDSActuator vehicle_leds; + DigitalOutput led_sleep, led_flash, led_fault, led_operational, led_can; + Actuators(Data& data) : + brakes(data), + vehicle_leds(Pinout::LEDR, Pinout::LEDG, Pinout::LEDB), + led_sleep(Pinout::SLEEP_LED), led_flash(Pinout::FLASH_LED), led_fault(Pinout::FAULT_LED), + led_operational(Pinout::OPERATIONAL_LED), led_can(Pinout::CAN_LED) + {} + }; +} diff --git a/Core/Inc/VCU_Actuators/VCU_LedsActuator.hpp b/Core/Inc/VCU_Actuators/VCU_LedsActuator.hpp index ba805ea..c1f3876 100644 --- a/Core/Inc/VCU_Actuators/VCU_LedsActuator.hpp +++ b/Core/Inc/VCU_Actuators/VCU_LedsActuator.hpp @@ -1,8 +1,22 @@ +/* + * VCU_LedsActuator.hpp + * + * Created on: Jun 1, 2023 + * Author: Pablo & stefancostea + */ + #pragma once #include "ST-LIB.hpp" namespace VCU{ + class RGBColor{ + public: + uint8_t red, green, blue; + RGBColor(uint8_t red, uint8_t green, uint8_t blue) : red(red), green(green), blue(blue){} + static RGBColor RED, BLUE, GREEN, MAGENTA, PURPLE, ORANGE, TURQUOISE, PISTACCIO_GREEN, WHITE; + }; + class LEDSActuator{ private: PWM red; @@ -27,7 +41,7 @@ namespace VCU{ void set_color(uint8_t red, uint8_t green, uint8_t blue){ if (red > 255 || green > 255 || blue > 255) { - ErrorHandler("Invalid color"); + ErrorHandler("Invalid color RGB code: %d , %d , %d", red, green, blue); return; } @@ -36,6 +50,10 @@ namespace VCU{ this->blue.set_duty_cycle(blue / 255.0f * 100.0f); } + void set_color(RGBColor& color){ + set_color(color.red,color.green, color.blue); + } + void turn_off(){ this->red.set_duty_cycle(0.0f); this->green.set_duty_cycle(0.0f); @@ -43,4 +61,14 @@ namespace VCU{ } }; -} \ No newline at end of file + RGBColor RGBColor::RED(255, 0,0); + RGBColor RGBColor::BLUE(0,0,255); + RGBColor RGBColor::GREEN(0,255,0); + RGBColor RGBColor::MAGENTA(255,0,255); + RGBColor RGBColor::PURPLE(160,32,240); + RGBColor RGBColor::ORANGE(255,165,0); + RGBColor RGBColor::TURQUOISE(93,193,185); + RGBColor RGBColor::PISTACCIO_GREEN(147,197,114); + RGBColor RGBColor::WHITE(255,255,255); + +} diff --git a/Core/Inc/VCU_Actuators/VCU_RegulatorActuator.hpp b/Core/Inc/VCU_Actuators/VCU_RegulatorActuator.hpp index e4e9986..bda36b9 100644 --- a/Core/Inc/VCU_Actuators/VCU_RegulatorActuator.hpp +++ b/Core/Inc/VCU_Actuators/VCU_RegulatorActuator.hpp @@ -1,3 +1,11 @@ +/* + * VCU_RegulatorActuator.hpp + * + * Created on: Jun 1, 2023 + * Author: Pablo + */ + + #pragma once #include "ST-LIB.hpp" diff --git a/Core/Inc/VCU_Actuators/VCU_ValveActuator.hpp b/Core/Inc/VCU_Actuators/VCU_ValveActuator.hpp index d316cae..63ba0db 100644 --- a/Core/Inc/VCU_Actuators/VCU_ValveActuator.hpp +++ b/Core/Inc/VCU_Actuators/VCU_ValveActuator.hpp @@ -1,3 +1,11 @@ +/* + * VCU_ValveActuator.hpp + * + * Created on: Jun 1, 2023 + * Author: Pablo + */ + + #pragma once #include "ST-LIB.hpp" diff --git a/Core/Inc/VCU_Brakes/VCU_Brakes.hpp b/Core/Inc/VCU_Brakes/VCU_Brakes.hpp index fae68d2..b235b02 100644 --- a/Core/Inc/VCU_Brakes/VCU_Brakes.hpp +++ b/Core/Inc/VCU_Brakes/VCU_Brakes.hpp @@ -1,3 +1,10 @@ +/* + * VCU_Brakes.hpp + * + * Created on: Jun 1, 2023 + * Author: Pablo + */ + #pragma once #include "ST-LIB.hpp" @@ -56,9 +63,7 @@ namespace VCU{ high_pressure_sensor(Pinout::HIGH_PRESSURE, high_pressure_sensor_slope, high_pressure_sensor_offset, &data.high_pressure1), low_pressure_sensor1(Pinout::LOW_PRESSURE1, low_pressure_sensors_slope, low_pressure_sensors_offset, &data.low_pressure1), low_pressure_sensor2(Pinout::LOW_PRESSURE2, low_pressure_sensors_slope, low_pressure_sensors_offset, &data.low_pressure2) - { - - } + {} void read(){ @@ -90,8 +95,6 @@ namespace VCU{ emergency_tape_enable.turn_off(); } - - void check_reeds(){ reed.read(); } @@ -116,12 +119,11 @@ namespace VCU{ class Brakes{ constexpr static uint16_t ntc_lookup_table_size = 256; - constexpr static float high_pressure_sensor_slope = 0.006681691; - constexpr static float high_pressure_sensor_offset = -43.75; + constexpr static float high_pressure_sensor_slope = 113.46153*1.20822977; + constexpr static float high_pressure_sensor_offset = (-516.25/11.8)*1.20822977; constexpr static float low_pressure_sensors_slope = 0.000190905; constexpr static float low_pressure_sensors_offset = -1.25; - constexpr static float operating_pressure = 8.0f; @@ -189,7 +191,6 @@ namespace VCU{ void brake(){ valve_actuator.close(); - Time::set_timeout(1, [&](){ check_reeds(); }); @@ -228,6 +229,5 @@ namespace VCU{ regulator_actuator.set_pressure(operating_pressure); read(); } - }; } diff --git a/Core/Inc/VCU_Communications/VCU_Communications.hpp b/Core/Inc/VCU_Communications/VCU_Communications.hpp new file mode 100644 index 0000000..a2c9421 --- /dev/null +++ b/Core/Inc/VCU_Communications/VCU_Communications.hpp @@ -0,0 +1,15 @@ +#pragma once + +namespace VCU{ + const IPV4 VCU_IP = {"192.168.1.3"}; + const IPV4 LCU_MASTER_IP = {"192.168.1.4"}; + const IPV4 LCU_SLAVE_IP = {"192.168.1.5"}; + const IPV4 PCU_IP = {"192.168.1.6"}; + const IPV4 BLCU_IP = {"192.168.1.7"}; + const IPV4 BMSL_IP = {"192.168.1.8"}; + const IPV4 OBCCU_IP = {"192.168.1.9"}; + const IPV4 BACKEND_IP = {"192.168.0.9"}; + constexpr uint16_t SERVER_PORT = 50500; + constexpr uint16_t CLIENT_PORT = 50501; + constexpr uint16_t UDP_PORT = 50400; +} diff --git a/Core/Inc/VCU_Communications/VCU_TCP/IncomingOrders.hpp b/Core/Inc/VCU_Communications/VCU_TCP/IncomingOrders.hpp index c396f69..0be8ebd 100644 --- a/Core/Inc/VCU_Communications/VCU_TCP/IncomingOrders.hpp +++ b/Core/Inc/VCU_Communications/VCU_TCP/IncomingOrders.hpp @@ -1,6 +1,15 @@ +/* + * VCU_IncomingOrders.hpp + * + * Created on: Jun 1, 2023 + * Author: stefancostea + */ + #pragma once + #include "Packets/Order.hpp" #include "VCU.hpp" + namespace VCU{ void hardware_reset(){ @@ -8,13 +17,9 @@ namespace VCU{ } void set_regulator_pressure(); - void brake(); - void unbrake(); - void disable_emergency_tape(); - void enable_emergency_tape(); template class IncomingOrders; @@ -47,14 +52,15 @@ namespace VCU{ StackOrder<0> unbrake_order; StackOrder<0> disable_emergency_tape_order; StackOrder<0> enable_emergency_tape_order; + public: float new_pressure = 0; IncomingOrders(Data& data) : - hardware_reset_order(209,hardware_reset), + hardware_reset_order(209, hardware_reset), set_regulator_pressure_order(210, set_regulator_pressure, &new_pressure), brake_order(215, brake), unbrake_order(216, unbrake), - disable_emergency_tape_order(217,disable_emergency_tape), enable_emergency_tape_order(218,enable_emergency_tape) + disable_emergency_tape_order(217, disable_emergency_tape), enable_emergency_tape_order(218,enable_emergency_tape) {} }; } diff --git a/Core/Inc/VCU_Communications/VCU_TCP/OutgoingOrders.hpp b/Core/Inc/VCU_Communications/VCU_TCP/OutgoingOrders.hpp index a89887a..5d2d165 100644 --- a/Core/Inc/VCU_Communications/VCU_TCP/OutgoingOrders.hpp +++ b/Core/Inc/VCU_Communications/VCU_TCP/OutgoingOrders.hpp @@ -1,2 +1,26 @@ +/* + * VCU_IncomingOrders.hpp + * + * Created on: Jun 1, 2023 + * Author: stefancostea + */ + #pragma once + #include "VCU.hpp" + +namespace LCU{ + enum LevitationOrdes{ + START_LEVITATING, + STOP_LEVITATING, + LANDING, + }; + + enum TractionOrders{ + + }; + + enum BatteryOrders{ + + }; +} diff --git a/Core/Inc/VCU_Communications/VCU_TCP/VCU_TCP.hpp b/Core/Inc/VCU_Communications/VCU_TCP/VCU_TCP.hpp index fb9265c..9f52b15 100644 --- a/Core/Inc/VCU_Communications/VCU_TCP/VCU_TCP.hpp +++ b/Core/Inc/VCU_Communications/VCU_TCP/VCU_TCP.hpp @@ -1,4 +1,12 @@ +/* + * VCU_TCP.hpp + * + * Created on: Jun 1, 2023 + * Author: stefancostea + */ + #pragma once + #include "VCU_Pinout/Pinout.hpp" #include "VCU_Utilities/VCU_Types.hpp" #include "VCU_Mode/VCU_Mode.hpp" @@ -12,26 +20,62 @@ namespace VCU{ class TCP{ public: ServerSocket BACKEND_CONNECTION; + TCP() {} + void init(){ BACKEND_CONNECTION = ServerSocket(VCU_IP, SERVER_PORT); } - void send_to_master(Order& order){ + + void send_to_backend(Order& order){ BACKEND_CONNECTION.send_order(order); } }; - template<> - class TCP{ + class TCP{ public: ServerSocket BACKEND_CONNECTION; - TCP() {} + Socket OBCCU_CONNECTION; + Socket BMSL_CONNECTION; + Socket PCU_CONNECTION; + Socket LCU_MASTER_CONNECTION; + + TCP(){} + void init(){ BACKEND_CONNECTION = ServerSocket(VCU_IP, SERVER_PORT); + + OBCCU_CONNECTION = Socket(VCU_IP, CLIENT_PORT, OBCCU_IP, SERVER_PORT); + OBCCU_CONNECTION.reconnect(); + + BMSL_CONNECTION = Socket(VCU_IP, CLIENT_PORT, BMSL_IP, SERVER_PORT); + BMSL_CONNECTION.reconnect(); + + PCU_CONNECTION = Socket(VCU_IP, CLIENT_PORT, PCU_IP, SERVER_PORT); + PCU_CONNECTION.reconnect(); + + LCU_MASTER_CONNECTION = Socket(VCU_IP, CLIENT_PORT, LCU_MASTER_IP, SERVER_PORT); } - void send_to_master(Order& order){ - BACKEND_CONNECTION.send_order(order); - } + + void send_to_backend(Order& order){ + BACKEND_CONNECTION.send_order(order); + } + + void send_to_obccu(Order& order){ + OBCCU_CONNECTION.send_order(order); + } + + void send_to_bsml(Order& order){ + BMSL_CONNECTION.send_order(order); + } + + void send_to_lcu(Order& order){ + LCU_MASTER_CONNECTION.send_order(order); + } + + void send_to_pcu(Order& order){ + PCU_CONNECTION.send_order(order); + } }; } diff --git a/Core/Inc/VCU_Communications/VCU_UDP/Packets.hpp b/Core/Inc/VCU_Communications/VCU_UDP/Packets.hpp index 7f09fb4..0d02002 100644 --- a/Core/Inc/VCU_Communications/VCU_UDP/Packets.hpp +++ b/Core/Inc/VCU_Communications/VCU_UDP/Packets.hpp @@ -1,5 +1,13 @@ +/* + * VCU_Packets.hpp + * + * Created on: Jun 1, 2023 + * Author: Pablo & stefancostea + */ + #pragma once #include "VCU.hpp" + namespace VCU{ template class Packets; @@ -11,8 +19,10 @@ namespace VCU{ StackPacket<1,REED_STATE> reed_packet; StackPacket<16,double,double> bottle_temperature_packet; StackPacket<12,float,float,float> pressure_packets; - Packets(Data& data) : regulator_packet(211, &data.valve_state, &data.regulator_reference_pressure, &data.regulator_real_pressure), - reed_packet(212,&data.reed), + + Packets(Data& data) : + regulator_packet(211, &data.valve_state, &data.regulator_reference_pressure, &data.regulator_real_pressure), + reed_packet(212, &data.reed), bottle_temperature_packet(213, &data.bottle_temperature1, &data.bottle_temperature2), pressure_packets(214, &data.high_pressure1, &data.low_pressure1, &data.low_pressure2) {} @@ -26,11 +36,14 @@ namespace VCU{ StackPacket<5,REED_STATE, REED_STATE, REED_STATE, REED_STATE, bool> reed_packet; StackPacket<16,double,double> bottle_temperature_packet; StackPacket<12,float,float,float> pressure_packets; + StackPacket<8, float,float> environmental_packet; + Packets(Data& data) : regulator_packet(211, &data.valve_state, &data.regulator_reference_pressure, &data.regulator_real_pressure), reed_packet(212, &data.reed1, &data.reed2, &data.reed3, &data.reed4, &data.reeds_ok), bottle_temperature_packet(213, &data.bottle_temperature1, &data.bottle_temperature2), - pressure_packets(214, &data.high_pressure1, &data.low_pressure1, &data.low_pressure2) + pressure_packets(214, &data.high_pressure1, &data.low_pressure1, &data.low_pressure2), + environmental_packet(215, &data.enviremont_pressure, &data.enviroment_temperature) {} }; } diff --git a/Core/Inc/VCU_Communications/VCU_UDP/VCU_UDP.hpp b/Core/Inc/VCU_Communications/VCU_UDP/VCU_UDP.hpp index d216bc2..4854a63 100644 --- a/Core/Inc/VCU_Communications/VCU_UDP/VCU_UDP.hpp +++ b/Core/Inc/VCU_Communications/VCU_UDP/VCU_UDP.hpp @@ -1,4 +1,12 @@ +/* + * VCU_UDP.hpp + * + * Created on: Jun 1, 2023 + * Author: stefancostea + */ + #pragma once + #include "DatagramSocket.hpp" #include "VCU.hpp" #include "VCU_Utilities/VCU_Types.hpp" @@ -11,27 +19,63 @@ namespace VCU{ public: DatagramSocket BACKEND_CONNECTION; UDP() {} + void init(){ BACKEND_CONNECTION = DatagramSocket(VCU_IP, UDP_PORT, BACKEND_IP, UDP_PORT); BACKEND_CONNECTION.reconnect(); } + void send_to_backend(Packet& packet){ BACKEND_CONNECTION.send(packet); } }; template<> - class UDP { - public: - DatagramSocket BACKEND_CONNECTION; - UDP() {} - void init(){ - //TODO: Conectarse a todas las placas - BACKEND_CONNECTION = DatagramSocket(VCU_IP, UDP_PORT, BACKEND_IP, UDP_PORT); - BACKEND_CONNECTION.reconnect(); - } - void send_to_backend(Packet& packet){ - BACKEND_CONNECTION.send(packet); - } - }; + class UDP { + public: + DatagramSocket BACKEND_CONNECTION; + DatagramSocket OBCCU_CONNECTION; + DatagramSocket BMSL_CONNECTION; + DatagramSocket PCU_CONNECTION; + DatagramSocket LCU_MASTER_CONNECTION; + + UDP() {} + + void init(){ + BACKEND_CONNECTION = DatagramSocket(VCU_IP, UDP_PORT, BACKEND_IP, UDP_PORT); + BACKEND_CONNECTION.reconnect(); + + OBCCU_CONNECTION = DatagramSocket(VCU_IP, UDP_PORT, OBCCU_IP, UDP_PORT); + OBCCU_CONNECTION.reconnect(); + + BMSL_CONNECTION = DatagramSocket(VCU_IP, UDP_PORT, BMSL_IP, UDP_PORT); + BMSL_CONNECTION.reconnect(); + + PCU_CONNECTION = DatagramSocket(VCU_IP, UDP_PORT, PCU_IP, UDP_PORT); + PCU_CONNECTION.reconnect(); + + LCU_MASTER_CONNECTION = DatagramSocket(VCU_IP, UDP_PORT, LCU_MASTER_IP, UDP_PORT); + LCU_MASTER_CONNECTION.reconnect(); + } + + void send_to_backend(Packet& packet){ + BACKEND_CONNECTION.send(packet); + } + + void send_to_obccu(Packet& packet){ + OBCCU_CONNECTION.send(packet); + } + + void send_to_bmsl(Packet& packet){ + BMSL_CONNECTION.send(packet); + } + + void send_to_pcu(Packet& packet){ + PCU_CONNECTION.send(packet); + } + + void send_to_lcu(Packet& packet){ + LCU_MASTER_CONNECTION.send(packet); + } + }; } diff --git a/Core/Inc/VCU_Data/VCU_Data.hpp b/Core/Inc/VCU_Data/VCU_Data.hpp index 2d1cd37..80f4db1 100644 --- a/Core/Inc/VCU_Data/VCU_Data.hpp +++ b/Core/Inc/VCU_Data/VCU_Data.hpp @@ -22,14 +22,22 @@ namespace VCU{ double bottle_temperature1 = 0.0f; double bottle_temperature2 = 0.0f; + bool emergency_tape = false; + REED_STATE reed = REED_STATE::RETRACTED; VALVE_STATE valve_state; + + void add_protections(){ + add_protection(&emergency_tape, Boundary(true)); + add_protection((void*)nullptr, Boundary()); + } }; template<> class Data{ public: + //BOARD Data float regulator_real_pressure = 0.0f; float regulator_reference_pressure = 0.0f; @@ -57,5 +65,14 @@ namespace VCU{ double tapes_direction = 0.0f; double tapes_speed = 0.0f; double tapes_acceleration = 0.0f; + + //VEHICLE Data + LevitaionState levitation_state = IDLE; + + void add_protections(){ + add_protection(&high_pressure1, Boundary(300)); + add_protection(&reeds_ok, Boundary(true)); + add_protection(&emergency_tape, Boundary(PinState::ON)); + } }; } diff --git a/Core/Inc/VCU_Mode/VCU_Mode.hpp b/Core/Inc/VCU_Mode/VCU_Mode.hpp index d79414b..c0598e6 100644 --- a/Core/Inc/VCU_Mode/VCU_Mode.hpp +++ b/Core/Inc/VCU_Mode/VCU_Mode.hpp @@ -6,4 +6,13 @@ namespace VCU{ POSITION_VALIDATION, VEHICLE, }; + + enum LevitaionState{ + IDLE, + TAKING_OFF, + STABLE, + STICK_UP, + STICK_DOWN, + LANDING, + }; } diff --git a/Core/Inc/VCU_Protections/VCU_Protections.hpp b/Core/Inc/VCU_Protections/VCU_Protections.hpp new file mode 100644 index 0000000..e69de29 diff --git a/Core/Inc/VCU_Sensors/VCU_Reed.hpp b/Core/Inc/VCU_Sensors/VCU_Reed.hpp index 27eb51a..e3a89c6 100644 --- a/Core/Inc/VCU_Sensors/VCU_Reed.hpp +++ b/Core/Inc/VCU_Sensors/VCU_Reed.hpp @@ -15,6 +15,5 @@ namespace VCU{ void read(){ reed.read(); } - }; -} \ No newline at end of file +} diff --git a/Core/Inc/VCU_StateMachine/VCU_DynamicLevStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_DynamicLevStateMachine.hpp index 56f002b..7a095b9 100644 --- a/Core/Inc/VCU_StateMachine/VCU_DynamicLevStateMachine.hpp +++ b/Core/Inc/VCU_StateMachine/VCU_DynamicLevStateMachine.hpp @@ -9,11 +9,13 @@ namespace VCU{ class DynamicLevStateMachine{ public: Data& data; - Brakes& brakes; + Actuators& actuators; TCP& tcp_handler; EncoderSensor& encoder; StateMachine state_machine; + bool ended = false; + enum DynamicLevStates{ Idle, TakeOff, @@ -22,8 +24,8 @@ namespace VCU{ Landing, }; - DynamicLevStateMachine(Data& data, Brakes&brakes, TCP& tcp, EncoderSensor& encoder) : - data(data),brakes(brakes),tcp_handler(tcp), encoder(encoder) + DynamicLevStateMachine(Data& data, Actuators& actuators, TCP& tcp, EncoderSensor& encoder) : + data(data), actuators(actuators), tcp_handler(tcp), encoder(encoder) {} void add_transitions(){} diff --git a/Core/Inc/VCU_StateMachine/VCU_HealtCheckLoadStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_HealtCheckLoadStateMachine.hpp index bfa1b8d..e041fa7 100644 --- a/Core/Inc/VCU_StateMachine/VCU_HealtCheckLoadStateMachine.hpp +++ b/Core/Inc/VCU_StateMachine/VCU_HealtCheckLoadStateMachine.hpp @@ -9,11 +9,19 @@ namespace VCU{ class LoadStateMachine{ public: Data& data; - Brakes& brakes; + Actuators& actuators; TCP& tcp_handler; EncoderSensor& encoder; StateMachine state_machine; + StackStateOrder<0> start_pushing; + StackStateOrder<0> start_crawling; + + bool ended = false; + + static bool pushing_requested; + static bool crawling_requested; + enum UnloadStates{ Idle, Pushing, @@ -21,14 +29,37 @@ namespace VCU{ Braking, }; - LoadStateMachine(Data& data, Brakes&brakes, TCP& tcp, EncoderSensor& encoder) : - data(data),brakes(brakes),tcp_handler(tcp), encoder(encoder) + LoadStateMachine(Data& data, Actuators& actuators, TCP& tcp, EncoderSensor& encoder) : + data(data), actuators(actuators), tcp_handler(tcp), encoder(encoder), + start_pushing(230, enter_pushing, state_machine, Idle), + start_crawling(231, enter_crawling, state_machine, Pushing) {} + static void enter_pushing(){ + pushing_requested = true; + } + + static void enter_crawling(){ + crawling_requested = true; + } + void add_transitions(){ - //TODO: IDLE -> Pushing con una state orden + state_machine.add_transition(Idle, Pushing, [&](){ + if(pushing_requested && not ended){ + pushing_requested = false; + return true; + } + return false; + }); - //TODO: Pushing -> Accelerating con una state orden + state_machine.add_transition(Pushing, Accelerating, [&](){ + //Solo se puede emepezar el crawling si se esta en zona de emergencia + if(crawling_requested && data.emergency_tape == PinState::ON){ + crawling_requested = false; + return true; + } + return false; + }); state_machine.add_transition(Accelerating, Braking, [&](){ return data.emergency_tape == PinState::OFF; @@ -41,14 +72,13 @@ namespace VCU{ void add_on_enter_actions(){ state_machine.add_enter_action([&](){ - brakes.enable_emergency_brakes(); - brakes.brake(); - + actuators.brakes.enable_emergency_brakes(); + actuators.brakes.brake(); }, Idle); state_machine.add_enter_action([&](){ - brakes.disable_emergency_brakes(); - brakes.not_brake(); + actuators.brakes.disable_emergency_brakes(); + actuators.brakes.not_brake(); }, Pushing); state_machine.add_enter_action([&](){ @@ -68,7 +98,9 @@ namespace VCU{ if(data.emergency_tape == PinState::ON){ ErrorHandler("The vehicle is still in emergency zone after Health&Load procedure"); }else{ - brakes.brake(); //Siguiendo el FDD kenos debe quedar frenado con pneumatica hasta empezar una demostración + ended = true; + actuators.brakes.brake(); + //Siguiendo el FDD kenos debe quedar frenado con pneumatica hasta empezar una demostración } }, Braking); @@ -76,7 +108,7 @@ namespace VCU{ void register_timed_actions(){ state_machine.add_low_precision_cyclic_action([&](){ - if(data.tapes_direction < 1){ //!TODO: MUY IMPORTANTE comprobar que 1 es hacia adelante!!!!!!!!! + if(data.tapes_direction != FORWARD){ ErrorHandler("The vehicle is moving backwards when it should be moving forward during load procedure"); } }, (ms)1, Accelerating); @@ -97,4 +129,7 @@ namespace VCU{ } }; + + bool LoadStateMachine::crawling_requested = false; + bool LoadStateMachine::pushing_requested = false; } diff --git a/Core/Inc/VCU_StateMachine/VCU_HealtCheckUnloadStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_HealtCheckUnloadStateMachine.hpp index 39065fe..c879ebb 100644 --- a/Core/Inc/VCU_StateMachine/VCU_HealtCheckUnloadStateMachine.hpp +++ b/Core/Inc/VCU_StateMachine/VCU_HealtCheckUnloadStateMachine.hpp @@ -12,11 +12,17 @@ namespace VCU{ public: Data& data; - Brakes& brakes; + Actuators& actuators; TCP& tcp_handler; EncoderSensor& encoder; StateMachine state_machine; + StackStateOrder<0> return_order; + + bool ended = false; + + static bool return_requested; + enum LoadStates{ Idle, Returning, @@ -25,15 +31,27 @@ namespace VCU{ }; - UnloadStateMachine(Data& data, Brakes&brakes, TCP& tcp, EncoderSensor& encoder) : - data(data),brakes(brakes),tcp_handler(tcp), encoder(encoder) + UnloadStateMachine(Data& data, Actuators& actuators, TCP& tcp, EncoderSensor& encoder) : + data(data), actuators(actuators), tcp_handler(tcp), encoder(encoder), + return_order(232, enter_returning, state_machine, Idle) {} + static void enter_returning(){ + return_requested = true; + } + void add_transitions(){ - //TODO: IDLE -> Returning con una state orden //Quitar los frenos debe hacerse manualmente con una orden una vez finalizado el procedure //porque segun el fdd el pod debe estar frenado hasta el momento que se va a sacar + state_machine.add_transition(Idle, Returning, [&](){ + if(return_requested && not ended){ + return_requested = false; + return true; + } + return false; + }); + state_machine.add_transition(Returning, Crawling, [&](){ return data.emergency_tape == PinState::ON; //Si hemos llegado a la zona de emergencia pasamos a fase crawling }); @@ -43,19 +61,19 @@ namespace VCU{ }); state_machine.add_transition(Braking, Idle, [&](){ - return data.tapes_acceleration == 0; //TODO: Muy importante hacer esto con la IMU + return data.tapes_acceleration == 0; //TODO: Muy importante hacer esto con la IMU del motor }); } void add_on_enter_actions(){ state_machine.add_enter_action([&](){ - brakes.enable_emergency_brakes(); - brakes.brake(); + actuators.brakes.enable_emergency_brakes(); + actuators.brakes.brake(); }, Idle); state_machine.add_enter_action([&](){ - brakes.not_brake(); - brakes.disable_emergency_brakes(); + actuators.brakes.not_brake(); + actuators.brakes.disable_emergency_brakes(); //TODO: set engine parameters //TODO: send start engine order }, Returning); @@ -67,12 +85,23 @@ namespace VCU{ state_machine.add_enter_action([&](){ //TODO: send engine stop order - brakes.brake(); }, Braking); } - void add_on_exit_actions(){} + void add_on_exit_actions(){ + + + state_machine.add_exit_action([&](){ + if(data.emergency_tape == PinState::ON){ + ErrorHandler("The vehicle is still in emergency zone after Health&Unload procedure"); + }else{ + ended = true; + actuators.brakes.brake(); + } + }, Braking); + + } void register_timed_actions(){ state_machine.add_low_precision_cyclic_action([&](){ @@ -82,13 +111,13 @@ namespace VCU{ }, (ms)1, Crawling); state_machine.add_low_precision_cyclic_action([&](){ - if(data.tapes_direction > 0){ //!TODO: MUY IMPORTANTE comprobar que01 es hacia atras!!!!!!!!! + if(data.tapes_direction != BACKWARD){ ErrorHandler("The vehicle is moving forward when it should be moving backwards during Health&Unload procedure"); } }, (ms)1, Returning); state_machine.add_low_precision_cyclic_action([&](){ - if(data.tapes_direction > 0){ //!TODO: MUY IMPORTANTE comprobar que01 es hacia atras!!!!!!!!! + if(data.tapes_direction != BACKWARD){ ErrorHandler("The vehicle is moving forward when it should be moving backwards during Health&Unload procedure"); } }, (ms)1, Crawling); @@ -107,6 +136,7 @@ namespace VCU{ add_transitions(); } - }; + + bool UnloadStateMachine::return_requested = false; } diff --git a/Core/Inc/VCU_StateMachine/VCU_SpecificStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_SpecificStateMachine.hpp index b6cf064..d5ba1c0 100644 --- a/Core/Inc/VCU_StateMachine/VCU_SpecificStateMachine.hpp +++ b/Core/Inc/VCU_StateMachine/VCU_SpecificStateMachine.hpp @@ -9,16 +9,31 @@ namespace VCU{ class SpecificStateMachine{ public: Data& data; - Brakes& brakes; + Actuators& actuators; TCP& tcp_handler; EncoderSensor& encoder; + LoadStateMachine health_load_state_machine; UnloadStateMachine health_unload_state_machine; TractionLevStateMachine traction_state_machine; StaticLevStateMachine static_lev_state_machine; DynamicLevStateMachine dynamic_lev_state_machine; + + StackStateOrder<0> healthcheck_and_load; + StackStateOrder<0> healthcheck_and_unload; + StackStateOrder<0> start_static_lev; + StackStateOrder<0, vector> start_dynamic_lev; + StackStateOrder<0, vector> start_traction; + StateMachine state_machine; - //TODO: Añadir todas las statemachines + + vector traction_points; + + static bool healthcheck_and_load_requested; + static bool healthcheck_and_unload_requested; + static bool start_static_lev_requested; + static bool start_dynamic_lev_requested; + static bool start_traction_requested; enum SpecificStates{ Idle, @@ -29,18 +44,97 @@ namespace VCU{ Traction, }; - SpecificStateMachine(Data& data, Brakes&brakes, TCP& tcp, EncoderSensor& encoder) : - data(data),brakes(brakes),tcp_handler(tcp), encoder(encoder), - health_load_state_machine(data, brakes, tcp, encoder), - health_unload_state_machine(data, brakes, tcp, encoder), - traction_state_machine(data, brakes, tcp, encoder), - static_lev_state_machine(data, brakes, tcp, encoder), - dynamic_lev_state_machine(data, brakes, tcp, encoder) + SpecificStateMachine(Data& data, Actuators& actuators, TCP& tcp, EncoderSensor& encoder) : + data(data),actuators(actuators),tcp_handler(tcp), encoder(encoder), + health_load_state_machine(data, actuators, tcp, encoder), + health_unload_state_machine(data, actuators, tcp, encoder), + traction_state_machine(data, actuators, tcp, encoder), + static_lev_state_machine(data, actuators, tcp, encoder), + dynamic_lev_state_machine(data, actuators, tcp, encoder), + healthcheck_and_load(220, enter_health_and_load, state_machine, Idle), + healthcheck_and_unload(221, enter_health_and_unload, state_machine, Idle), + start_static_lev(222, enter_static_lev, state_machine, Idle), + start_dynamic_lev(223, enter_dynamic_lev, state_machine, Idle, &traction_points), + start_traction(224, enter_traction, state_machine, Idle, &traction_points) {} - void add_transitions(){} + static void enter_health_and_load(){ + healthcheck_and_load_requested = true; + } + + static void enter_health_and_unload(){ + healthcheck_and_unload_requested = true; + } + + static void enter_dynamic_lev(){ + start_static_lev_requested = true; + } + + static void enter_static_lev(){ + start_dynamic_lev_requested = true; + } + + static void enter_traction(){ + start_traction_requested = true; + } + + void add_transitions(){ + state_machine.add_transition(Idle, Load, [&](){ + return healthcheck_and_load_requested; + }); + + state_machine.add_transition(Idle, Unload, [&](){ + return healthcheck_and_unload_requested; + }); - void add_on_enter_actions(){} + state_machine.add_transition(Idle, DynamicLev, [&](){ + return start_static_lev_requested; + }); + + state_machine.add_transition(Idle, StaticLev, [&](){ + return start_dynamic_lev_requested; + }); + + state_machine.add_transition(Idle, Traction, [&](){ + return start_traction_requested; + }); + + state_machine.add_transition(Load, Idle, [&](){ + return health_load_state_machine.ended; + }); + + state_machine.add_transition(Unload, Idle, [&](){ + return health_unload_state_machine.ended; + }); + + state_machine.add_transition(DynamicLev, Idle, [&](){ + return dynamic_lev_state_machine.ended; + }); + + state_machine.add_transition(StaticLev, Idle, [&](){ + return static_lev_state_machine.ended; + }); + + state_machine.add_transition(Traction, Idle, [&](){ + return traction_state_machine.ended; + }); + } + + void add_on_enter_actions(){ + state_machine.add_enter_action([&](){ + health_load_state_machine.ended = false; + health_unload_state_machine.ended = false; + dynamic_lev_state_machine.ended = false; + static_lev_state_machine.ended = false; + traction_state_machine.ended = false; + + healthcheck_and_load_requested = false; + healthcheck_and_unload_requested = false; + start_static_lev_requested = false; + start_dynamic_lev_requested = false; + start_traction_requested = false; + }, Idle); + } void add_on_exit_actions(){} @@ -54,15 +148,23 @@ namespace VCU{ state_machine.add_state(StaticLev); state_machine.add_state(Traction); + state_machine.add_state_machine(health_unload_state_machine.state_machine, Unload); + state_machine.add_state_machine(health_load_state_machine.state_machine, Load); + state_machine.add_state_machine(traction_state_machine.state_machine, Traction); + state_machine.add_state_machine(static_lev_state_machine.state_machine, StaticLev); + state_machine.add_state_machine(dynamic_lev_state_machine.state_machine, DynamicLev); + add_on_enter_actions(); add_on_exit_actions(); add_transitions(); register_timed_actions(); add_transitions(); - - //TODO: Añadir todas las state machines - //state_machine.add_state_machine(.state_machine, OPERATIONAL); } - }; + + bool VCU::SpecificStateMachine::healthcheck_and_load_requested = false; + bool VCU::SpecificStateMachine::healthcheck_and_unload_requested = false; + bool VCU::SpecificStateMachine::start_static_lev_requested = false; + bool VCU::SpecificStateMachine::start_dynamic_lev_requested = false; + bool VCU::SpecificStateMachine::start_traction_requested = false; } diff --git a/Core/Inc/VCU_StateMachine/VCU_StateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_StateMachine.hpp index 17ab0b5..b6f474f 100644 --- a/Core/Inc/VCU_StateMachine/VCU_StateMachine.hpp +++ b/Core/Inc/VCU_StateMachine/VCU_StateMachine.hpp @@ -5,11 +5,99 @@ namespace VCU{ template class GeneralStateMachine; + template<> class GeneralStateMachine{ + public: + Actuators& actuators; + TCP& tcp_handler; + StateMachine general_state_machine; + bool tcp_timeout = false; + + static constexpr uint16_t max_tcp_connection_timeout = 25000; //ms + + GeneralStateMachine(Data& data, Actuators& actuators , TCP& tcp_handler) : actuators(actuators), tcp_handler(tcp_handler) + {} + + enum States{ + INITIAL, + OPERATIONAL, + FAULT + }; + + + void init(){ + general_state_machine = {INITIAL}; + general_state_machine.add_state(OPERATIONAL); + general_state_machine.add_state(FAULT); + ProtectionManager::link_state_machine(general_state_machine, FAULT); + ProtectionManager::set_id(Boards::ID::VCU); + add_on_enter_actions(); + add_on_exit_actions(); + add_transitions(); + register_timed_actions(); + } + + void add_on_enter_actions(){ + general_state_machine.add_enter_action([&](){ + Time::set_timeout(max_tcp_connection_timeout, [&](){ + if(not (tcp_handler.BACKEND_CONNECTION.state == ServerSocket::ServerState::ACCEPTED)){ + tcp_timeout = true; + } + }); + }, INITIAL); + + Time::set_timeout(max_tcp_connection_timeout, [&](){ + if(not (tcp_handler.BACKEND_CONNECTION.state == ServerSocket::ServerState::ACCEPTED)){ + tcp_timeout = true; + } + }); + general_state_machine.add_enter_action([&](){ + actuators.led_fault.turn_on(); + actuators.brakes.brake(); + }, FAULT); + + general_state_machine.add_enter_action([&](){ + actuators.led_operational.turn_on(); + }, OPERATIONAL); + } + + void add_on_exit_actions(){ + general_state_machine.add_exit_action([&](){ + actuators.led_fault.turn_off(); + }, FAULT); + general_state_machine.add_exit_action([&](){ + actuators.led_operational.turn_off(); + }, OPERATIONAL); + } + + void add_transitions(){ + general_state_machine.add_transition(INITIAL, OPERATIONAL, [&](){ + return tcp_handler.BACKEND_CONNECTION.state == ServerSocket::ServerState::ACCEPTED; + }); + general_state_machine.add_transition(INITIAL, FAULT, [&](){ + if(tcp_timeout) ErrorHandler("TCP connections could not be established in time and timed out"); + return tcp_timeout; + }); + general_state_machine.add_transition(OPERATIONAL, FAULT, [&](){ + if(tcp_handler.BACKEND_CONNECTION.state != ServerSocket::ServerState::ACCEPTED){ + ErrorHandler("TCP connections fell"); + return true; + } + return false; + }); + } + + void register_timed_actions(){ + general_state_machine.add_low_precision_cyclic_action([&](){actuators.led_operational.toggle();}, (ms)150, INITIAL); + general_state_machine.add_low_precision_cyclic_action(ProtectionManager::check_protections, (ms)1, OPERATIONAL); + } + + }; + template<> class GeneralStateMachine{ public: Data& data; - Brakes& brakes; + Actuators& actuators; TCP& tcp_handler; EncoderSensor& encoder; SpecificStateMachine specific_state_machine; @@ -25,11 +113,12 @@ namespace VCU{ FAULT }; - GeneralStateMachine(Data& data, Brakes&brakes, TCP& tcp, EncoderSensor& encoder) : - data(data),brakes(brakes),tcp_handler(tcp), encoder(encoder), specific_state_machine(data, brakes, tcp, encoder) + GeneralStateMachine(Data& data, Actuators& actuators, TCP& tcp, EncoderSensor& encoder) : + data(data), actuators(actuators), tcp_handler(tcp), encoder(encoder), specific_state_machine(data, actuators, tcp, encoder) {} void add_transitions(){ + //todo: COMPROBAR que estan todos conectados antes de pasar a operational general_state_machine.add_transition(INITIAL, OPERATIONAL, [&](){ return tcp_handler.BACKEND_CONNECTION.state == ServerSocket::ServerState::ACCEPTED ; }); @@ -64,33 +153,32 @@ namespace VCU{ }); general_state_machine.add_enter_action([&](){ - //TODO: encender led fault - brakes.brake(); - brakes.enable_emergency_brakes(); - + actuators.led_fault.turn_on(); + actuators.brakes.brake(); + actuators.brakes.enable_emergency_brakes(); }, FAULT); general_state_machine.add_enter_action([&](){ - //TODO: encender led operational - brakes.enable_emergency_brakes(); + actuators.led_operational.turn_on(); + actuators.brakes.enable_emergency_brakes(); }, OPERATIONAL); } void add_on_exit_actions(){ general_state_machine.add_exit_action([&](){ - //TODO: apagar led fault - brakes.not_brake(); + actuators.led_fault.turn_off(); + actuators.brakes.not_brake(); }, FAULT); general_state_machine.add_exit_action([&](){ - //TODO: apagar led operational + actuators.led_operational.turn_on(); }, OPERATIONAL); general_state_machine.add_exit_action([&](){ - //TODO: apagar led operational + actuators.led_operational.turn_off(); }, INITIAL); } void register_timed_actions(){ - //TODO: general_state_machine.add_low_precision_cyclic_action([&](){actuators.led_operational.toggle();}, 150ms, INITIAL); + general_state_machine.add_low_precision_cyclic_action([&](){actuators.led_operational.toggle();}, (ms)150, INITIAL); general_state_machine.add_low_precision_cyclic_action(ProtectionManager::check_protections, (ms)1, OPERATIONAL); } diff --git a/Core/Inc/VCU_StateMachine/VCU_StaticLevStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_StaticLevStateMachine.hpp index d5381d2..db8ce78 100644 --- a/Core/Inc/VCU_StateMachine/VCU_StaticLevStateMachine.hpp +++ b/Core/Inc/VCU_StateMachine/VCU_StaticLevStateMachine.hpp @@ -9,18 +9,20 @@ namespace VCU{ class StaticLevStateMachine{ public: Data& data; - Brakes& brakes; + Actuators& actuators; TCP& tcp_handler; EncoderSensor& encoder; StateMachine state_machine; + bool ended = false; + enum DynamicLevStates{ LevOff, LevOn, }; - StaticLevStateMachine(Data& data, Brakes&brakes, TCP& tcp, EncoderSensor& encoder) : - data(data),brakes(brakes),tcp_handler(tcp), encoder(encoder) + StaticLevStateMachine(Data& data, Actuators& actuators, TCP& tcp, EncoderSensor& encoder) : + data(data), actuators(actuators), tcp_handler(tcp), encoder(encoder) {} void add_transitions(){ @@ -30,14 +32,15 @@ namespace VCU{ void add_on_enter_actions(){ state_machine.add_enter_action([&](){ - brakes.not_brake(); + actuators.brakes.not_brake(); + //TODO: set lev parameters //TODO: send lev order }, LevOn); state_machine.add_enter_action([&](){ //TODO: send stop lev order - brakes.brake(); + actuators.brakes.brake(); }, LevOff); } diff --git a/Core/Inc/VCU_StateMachine/VCU_TractionStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_TractionStateMachine.hpp index ac2b9c1..f48af53 100644 --- a/Core/Inc/VCU_StateMachine/VCU_TractionStateMachine.hpp +++ b/Core/Inc/VCU_StateMachine/VCU_TractionStateMachine.hpp @@ -9,26 +9,30 @@ namespace VCU{ class TractionLevStateMachine{ public: Data& data; - Brakes& brakes; + Actuators& actuators; TCP& tcp_handler; EncoderSensor& encoder; StateMachine state_machine; + bool ended = false; + enum DynamicLevStates{ Idle, Accelerating, Brake, }; - TractionLevStateMachine(Data& data, Brakes&brakes, TCP& tcp, EncoderSensor& encoder) : - data(data),brakes(brakes),tcp_handler(tcp), encoder(encoder) + TractionLevStateMachine(Data& data, Actuators&actuators, TCP& tcp, EncoderSensor& encoder) : + data(data),actuators(actuators),tcp_handler(tcp), encoder(encoder) {} void add_transitions(){} void add_on_enter_actions(){} - void add_on_exit_actions(){} + void add_on_exit_actions(){ + + } void register_timed_actions(){} diff --git a/Core/Inc/VCU_Time/VCU_Time.hpp b/Core/Inc/VCU_Time/VCU_Time.hpp index eb91047..db6fd56 100644 --- a/Core/Inc/VCU_Time/VCU_Time.hpp +++ b/Core/Inc/VCU_Time/VCU_Time.hpp @@ -9,6 +9,7 @@ namespace VCU{ template class CyclicActions; + template<> class CyclicActions{ public: Brakes& brakes; @@ -17,6 +18,20 @@ namespace VCU{ static void register_cyclic_actions(){ Time::register_low_precision_alarm(1, VCU::VCU_CLASS::read_brakes_sensors); Time::register_low_precision_alarm(16, VCU::VCU_CLASS::send_to_backend); + Time::register_low_precision_alarm(1, VCU::VCU_CLASS::update_state_machine); + } + }; + + template<> class CyclicActions{ + public: + Brakes& brakes; + CyclicActions(Brakes& brakes) : brakes(brakes){} + + static void register_cyclic_actions(){ + Time::register_low_precision_alarm(1, VCU::VCU_CLASS::read_brakes_sensors); + Time::register_low_precision_alarm(100, VCU::VCU_CLASS::read_environmental_sensors); + Time::register_low_precision_alarm(16, VCU::VCU_CLASS::send_to_backend); + Time::register_low_precision_alarm(1, VCU::VCU_CLASS::update_state_machine); } }; } diff --git a/Core/Inc/VCU_Utilities/VCU_Includes.hpp b/Core/Inc/VCU_Utilities/VCU_Includes.hpp index 3f8c14f..50a39e3 100644 --- a/Core/Inc/VCU_Utilities/VCU_Includes.hpp +++ b/Core/Inc/VCU_Utilities/VCU_Includes.hpp @@ -10,11 +10,10 @@ #include "VCU_Sensors/VCU_EnviromentalSensors.hpp" #include "VCU_Sensors/VCU_RegulatorSensor.hpp" #include "VCU_Sensors/VCU_Reed.hpp" -#include "VCU_Actuators/VCU_LedsActuator.hpp" -#include "VCU_Actuators/VCU_RegulatorActuator.hpp" -#include "VCU_Actuators/VCU_ValveActuator.hpp" +#include "VCU_Actuators/VCU_Actuators.hpp" #include "VCU_Brakes/VCU_Brakes.hpp" +#include "VCU_Communications/VCU_Communications.hpp" #include "VCU_Communications/VCU_TCP/VCU_TCP.hpp" #include "VCU_Communications/VCU_UDP/VCU_UDP.hpp" #include "VCU_Communications/VCU_TCP/OutgoingOrders.hpp" diff --git a/Core/Inc/VCU_Utilities/VCU_Types.hpp b/Core/Inc/VCU_Utilities/VCU_Types.hpp index 398a5d9..5b4689a 100644 --- a/Core/Inc/VCU_Utilities/VCU_Types.hpp +++ b/Core/Inc/VCU_Utilities/VCU_Types.hpp @@ -2,14 +2,6 @@ namespace VCU{ - const IPV4 VCU_IP = {"192.168.1.3"}; - const IPV4 LCU_SLAVE_IP = {"192.168.1.5"}; - const IPV4 LCU_MASTER_IP = {"192.168.1.4"}; - const IPV4 BACKEND_IP = {"192.168.0.9"}; - constexpr uint16_t SERVER_PORT = 50500; - constexpr uint16_t CLIENT_PORT = 50501; - constexpr uint16_t UDP_PORT = 50400; - enum VALVE_STATE{ CLOSED, OPEN, @@ -20,5 +12,10 @@ namespace VCU{ RETRACTED, }; + enum DIRECTION{ //TODO: Comprobar que esto es cierto + BACKWARD = 0, + FORWARD = 1, + }; + } diff --git a/Core/Src/main.cpp b/Core/Src/main.cpp index 3ccbcea..65e56d7 100644 --- a/Core/Src/main.cpp +++ b/Core/Src/main.cpp @@ -6,12 +6,94 @@ #include "VCU.hpp" #include "VCU_Time/VCU_Time.hpp" -int main(void) -{ - VCU::VCU_CLASS vcu; - VCU::VCU_CLASS::vcu = &vcu; - vcu.init(); - VCU::CyclicActions::register_cyclic_actions(); +StateMachine* global; + +enum est{ + ini, + op, + fa, +}; + +void sys_res(){ + HAL_NVIC_SystemReset(); +} + +void a_start(){ + global->force_change_state(op); +} + +void a_fa_func(){ + global->force_change_state(fa); +} + +void a_op_func(){ + global->force_change_state(op); +} + +int main(void){ +//{ +// VCU::VCU_CLASS vcu; +// VCU::VCU_CLASS::vcu = &vcu; +// vcu.init(); +// VCU::CyclicActions::register_cyclic_actions(); + + est current_state = ini; + STLIB::start(); + ServerSocket BACKEND_CONNECTION(IPV4("192.168.1.3"), 50500); + DatagramSocket udp(IPV4("192.168.1.3"), 50400, IPV4("192.168.0.9"),50400); + + StateOrder::set_socket(BACKEND_CONNECTION); + + + StateMachine machine = {ini}; + machine.add_state(op); + machine.add_state(fa); + + global = &machine; + + float valor = 1.0f; + + StackPacket<1, est> data_pack ( + 99, + ¤t_state + ); + + StackOrder<0> start{ + 98, + a_start + }; + + StackOrder<0> reset{ + 97, + sys_res + }; + + StackStateOrder<4, float> a_fa( + 101, + a_fa_func, + machine, + op, + &valor + ); + + StackStateOrder<4, float> a_op( + 102, + a_op_func, + machine, + fa, + &valor + ); + + Time::register_low_precision_alarm(100, [&](){ + machine.check_transitions(); + }); + + Time::register_low_precision_alarm(10, [&](){ + current_state = (est)machine.current_state; + //udp.send(data_pack); + }); + + while(1){ STLIB::update(); } From 0d0fe5f3ac7c7fb789e213ba1f41d2b5929d30ec Mon Sep 17 00:00:00 2001 From: Pablo Date: Wed, 21 Jun 2023 23:20:24 +0200 Subject: [PATCH 05/10] added check all tcp connections --- .../VCU_Communications/VCU_TCP/VCU_TCP.hpp | 14 ++++++++++++ .../Inc/VCU_StateMachine/VCU_StateMachine.hpp | 22 +++++++++---------- 2 files changed, 25 insertions(+), 11 deletions(-) diff --git a/Core/Inc/VCU_Communications/VCU_TCP/VCU_TCP.hpp b/Core/Inc/VCU_Communications/VCU_TCP/VCU_TCP.hpp index 9f52b15..c6d954d 100644 --- a/Core/Inc/VCU_Communications/VCU_TCP/VCU_TCP.hpp +++ b/Core/Inc/VCU_Communications/VCU_TCP/VCU_TCP.hpp @@ -77,5 +77,19 @@ namespace VCU{ void send_to_pcu(Order& order){ PCU_CONNECTION.send_order(order); } + + bool check_connections(){ + return + BACKEND_CONNECTION.state == ServerSocket::ServerState::ACCEPTED + && + OBCCU_CONNECTION.state == ServerSocket::ServerState::ACCEPTED + && + BMSL_CONNECTION.state == ServerSocket::ServerState::ACCEPTED + && + LCU_MASTER_CONNECTION.state == ServerSocket::ServerState::ACCEPTED + && + PCU_CONNECTION.state == ServerSocket::ServerState::ACCEPTED + ; + } }; } diff --git a/Core/Inc/VCU_StateMachine/VCU_StateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_StateMachine.hpp index b6f474f..51c96a1 100644 --- a/Core/Inc/VCU_StateMachine/VCU_StateMachine.hpp +++ b/Core/Inc/VCU_StateMachine/VCU_StateMachine.hpp @@ -120,7 +120,7 @@ namespace VCU{ void add_transitions(){ //todo: COMPROBAR que estan todos conectados antes de pasar a operational general_state_machine.add_transition(INITIAL, OPERATIONAL, [&](){ - return tcp_handler.BACKEND_CONNECTION.state == ServerSocket::ServerState::ACCEPTED ; + return tcp_handler.check_connections(); }); general_state_machine.add_transition(INITIAL, FAULT, [&](){ if(tcp_timeout){ @@ -129,7 +129,7 @@ namespace VCU{ return tcp_timeout; }); general_state_machine.add_transition(OPERATIONAL, FAULT, [&](){ - if(tcp_handler.BACKEND_CONNECTION.state != ServerSocket::ServerState::ACCEPTED){ + if(not tcp_handler.check_connections() ){ ErrorHandler("TCP connections fell"); return true; } @@ -138,16 +138,16 @@ namespace VCU{ } void add_on_enter_actions(){ - general_state_machine.add_enter_action([&](){ - Time::set_timeout(max_tcp_connection_timeout, [&](){ - if(not (tcp_handler.BACKEND_CONNECTION.state == ServerSocket::ServerState::ACCEPTED)){ - tcp_timeout = true; - } - }); - }, INITIAL); - +// general_state_machine.add_enter_action([&](){ +// Time::set_timeout(max_tcp_connection_timeout, [&](){ +// if(not (tcp_handler.BACKEND_CONNECTION.state == ServerSocket::ServerState::ACCEPTED)){ +// tcp_timeout = true; +// } +// }); +// }, INITIAL); + //Replicado porque el estado incial no ejecuta las enter actions Time::set_timeout(max_tcp_connection_timeout, [&](){ - if(not (tcp_handler.BACKEND_CONNECTION.state == ServerSocket::ServerState::ACCEPTED)){ + if(not (tcp_handler.check_connections())){ tcp_timeout = true; } }); From 4d6ecbf56f178b7689f50500ca8792fbf2f71f2c Mon Sep 17 00:00:00 2001 From: Pablo Date: Thu, 22 Jun 2023 02:18:38 +0200 Subject: [PATCH 06/10] checkpoint, only traction and dynamic demostration --- Core/Inc/VCU.hpp | 3 +- .../VCU_TCP/IncomingOrders.hpp | 13 ++ .../VCU_TCP/OutgoingOrders.hpp | 35 ++++-- .../VCU_Communications/VCU_UDP/Packets.hpp | 3 + Core/Inc/VCU_Data/VCU_Data.hpp | 2 + Core/Inc/VCU_Mode/VCU_Mode.hpp | 9 -- .../VCU_ContactorsStateMachine.hpp | 117 ++++++++++++++++++ .../VCU_DynamicLevStateMachine.hpp | 10 +- .../VCU_HealtCheckLoadStateMachine.hpp | 67 ++++++---- .../VCU_HealtCheckUnloadStateMachine.hpp | 62 ++++++---- .../VCU_SpecificStateMachine.hpp | 28 +++-- .../Inc/VCU_StateMachine/VCU_StateMachine.hpp | 5 +- .../VCU_StaticLevStateMachine.hpp | 73 +++++++++-- .../VCU_TractionStateMachine.hpp | 9 +- Core/Inc/VCU_Utilities/VCU_Includes.hpp | 1 + Core/Inc/VCU_Utilities/VCU_Types.hpp | 14 +++ 16 files changed, 355 insertions(+), 96 deletions(-) create mode 100644 Core/Inc/VCU_StateMachine/VCU_ContactorsStateMachine.hpp diff --git a/Core/Inc/VCU.hpp b/Core/Inc/VCU.hpp index 67f6ab3..06cb162 100644 --- a/Core/Inc/VCU.hpp +++ b/Core/Inc/VCU.hpp @@ -57,13 +57,14 @@ namespace VCU{ TCP tcp_handler; UDP udp_handler; IncomingOrders incoming_orders; + OutgoingOrders outgoing_orders; Packets packets; EncoderSensor encoder; GeneralStateMachine state_machine_handler; VCU_CLASS():data(), actuators(data), environmental_sensors(data), tcp_handler(), udp_handler(), incoming_orders(data), packets(data), encoder(Pinout::TAPE1, Pinout::TAPE2, &data.tapes_position, &data.tapes_direction, &data.tapes_speed, &data.tapes_acceleration) - ,state_machine_handler(data, actuators, tcp_handler, encoder) + ,state_machine_handler(data, actuators, tcp_handler, outgoing_orders, encoder) {} void init(){ diff --git a/Core/Inc/VCU_Communications/VCU_TCP/IncomingOrders.hpp b/Core/Inc/VCU_Communications/VCU_TCP/IncomingOrders.hpp index 0be8ebd..f57f78c 100644 --- a/Core/Inc/VCU_Communications/VCU_TCP/IncomingOrders.hpp +++ b/Core/Inc/VCU_Communications/VCU_TCP/IncomingOrders.hpp @@ -12,6 +12,19 @@ namespace VCU{ + enum class IncomingOrdersIDs: uint16_t{ + heakthcheck_and_load = 220, + healthcheck_and_unload = 221, + start_static_lev_demostration = 222, + start_dynamic_lev_demostration = 223, + start_traction_demostration = 224, + stop_demostration = 225, + take_off = 226, + landing = 227, + start_crawling = 231, + }; + + void hardware_reset(){ HAL_NVIC_SystemReset(); } diff --git a/Core/Inc/VCU_Communications/VCU_TCP/OutgoingOrders.hpp b/Core/Inc/VCU_Communications/VCU_TCP/OutgoingOrders.hpp index 5d2d165..cc1feb1 100644 --- a/Core/Inc/VCU_Communications/VCU_TCP/OutgoingOrders.hpp +++ b/Core/Inc/VCU_Communications/VCU_TCP/OutgoingOrders.hpp @@ -9,18 +9,39 @@ #include "VCU.hpp" -namespace LCU{ - enum LevitationOrdes{ - START_LEVITATING, - STOP_LEVITATING, - LANDING, +namespace VCU{ + enum class LevitationOrdes: uint16_t{ + TAKE_OFF = 300, + LANDING = 301, }; - enum TractionOrders{ + enum class TractionOrders: uint16_t{ }; - enum BatteryOrders{ + enum class BatteryOrders: uint16_t{ + CLOSE_CONTACTORS = 903, + OPEN_CONTACTORS = 902, + }; + + template class OutgoingOrders; + + template<> + class OutgoingOrders{ + public: + StackOrder<0> take_off_order; + StackOrder<0> landing_order; + + StackOrder<0> close_contactors; + StackOrder<0> open_contactors; + + OutgoingOrders() : + take_off_order((uint16_t)LevitationOrdes::TAKE_OFF), + landing_order((uint16_t)LevitationOrdes::LANDING), + close_contactors((uint16_t)BatteryOrders::CLOSE_CONTACTORS), + open_contactors((uint16_t)BatteryOrders::OPEN_CONTACTORS) + {} }; + } diff --git a/Core/Inc/VCU_Communications/VCU_UDP/Packets.hpp b/Core/Inc/VCU_Communications/VCU_UDP/Packets.hpp index 0d02002..cd36d7f 100644 --- a/Core/Inc/VCU_Communications/VCU_UDP/Packets.hpp +++ b/Core/Inc/VCU_Communications/VCU_UDP/Packets.hpp @@ -32,12 +32,15 @@ namespace VCU{ class Packets{ public: //TODO: Hay que revisar todos los paquetes + //Outgoing packets StackPacket<9,VALVE_STATE, float, float> regulator_packet; StackPacket<5,REED_STATE, REED_STATE, REED_STATE, REED_STATE, bool> reed_packet; StackPacket<16,double,double> bottle_temperature_packet; StackPacket<12,float,float,float> pressure_packets; StackPacket<8, float,float> environmental_packet; + //Incoming packets + Packets(Data& data) : regulator_packet(211, &data.valve_state, &data.regulator_reference_pressure, &data.regulator_real_pressure), reed_packet(212, &data.reed1, &data.reed2, &data.reed3, &data.reed4, &data.reeds_ok), diff --git a/Core/Inc/VCU_Data/VCU_Data.hpp b/Core/Inc/VCU_Data/VCU_Data.hpp index 80f4db1..3a57f1c 100644 --- a/Core/Inc/VCU_Data/VCU_Data.hpp +++ b/Core/Inc/VCU_Data/VCU_Data.hpp @@ -69,6 +69,8 @@ namespace VCU{ //VEHICLE Data LevitaionState levitation_state = IDLE; + ContactorState contactors_state = ContactorState::Open; + void add_protections(){ add_protection(&high_pressure1, Boundary(300)); add_protection(&reeds_ok, Boundary(true)); diff --git a/Core/Inc/VCU_Mode/VCU_Mode.hpp b/Core/Inc/VCU_Mode/VCU_Mode.hpp index c0598e6..d79414b 100644 --- a/Core/Inc/VCU_Mode/VCU_Mode.hpp +++ b/Core/Inc/VCU_Mode/VCU_Mode.hpp @@ -6,13 +6,4 @@ namespace VCU{ POSITION_VALIDATION, VEHICLE, }; - - enum LevitaionState{ - IDLE, - TAKING_OFF, - STABLE, - STICK_UP, - STICK_DOWN, - LANDING, - }; } diff --git a/Core/Inc/VCU_StateMachine/VCU_ContactorsStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_ContactorsStateMachine.hpp new file mode 100644 index 0000000..434e823 --- /dev/null +++ b/Core/Inc/VCU_StateMachine/VCU_ContactorsStateMachine.hpp @@ -0,0 +1,117 @@ +#pragma once + +#include "VCU_Utilities/VCU_Includes.hpp" + +namespace VCU{ + template class CloseContactorsStateMachine; + + template<> + class CloseContactorsStateMachine{ + public: + Data& data; + TCP& tcp_handler; + OutgoingOrders& outgoing_orders; + StateMachine state_machine; + + bool ended = false; + + enum DynamicLevStates{ + RequestClose, + Closed, + }; + + CloseContactorsStateMachine(Data& data, TCP& tcp, OutgoingOrders outgoing_orders) : + data(data), tcp_handler(tcp), outgoing_orders(outgoing_orders) + {} + + void add_transitions(){ + state_machine.add_transition(RequestClose, Closed, [&](){ + if (data.contactors_state == ContactorState::Close) { + ended = true; + return true; + } + + return false; + }); + + } + + void add_on_enter_actions(){ + state_machine.add_enter_action([&](){ + tcp_handler.send_to_obccu(outgoing_orders.close_contactors); + }, RequestClose); + + } + + void add_on_exit_actions(){} + + void register_timed_actions(){} + + void init(){ + state_machine = {RequestClose}; + state_machine.add_state(Closed); + + add_on_enter_actions(); + add_on_exit_actions(); + add_transitions(); + register_timed_actions(); + add_transitions(); + } + + }; + + template class OpenContactorsStateMachine; + + template<> + class OpenContactorsStateMachine{ + public: + Data& data; + TCP& tcp_handler; + OutgoingOrders& outgoing_orders; + StateMachine state_machine; + + bool ended = false; + + enum DynamicLevStates{ + RequestOpen, + Opened, + }; + + OpenContactorsStateMachine(Data& data, TCP& tcp, OutgoingOrders outgoing_orders) : + data(data), tcp_handler(tcp), outgoing_orders(outgoing_orders) + {} + + void add_transitions(){ + state_machine.add_transition(RequestOpen, Opened, [&](){ + if (data.contactors_state == ContactorState::Open) { + ended = true; + return true; + } + + return false; + }); + } + + void add_on_enter_actions(){ + state_machine.add_enter_action([&](){ + tcp_handler.send_to_obccu(outgoing_orders.open_contactors); + }, RequestOpen); + } + + void add_on_exit_actions(){} + + void register_timed_actions(){} + + void init(){ + state_machine = {RequestOpen}; + state_machine.add_state(Opened); + + add_on_enter_actions(); + add_on_exit_actions(); + add_transitions(); + register_timed_actions(); + add_transitions(); + } + + }; +} diff --git a/Core/Inc/VCU_StateMachine/VCU_DynamicLevStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_DynamicLevStateMachine.hpp index 7a095b9..6d3dc96 100644 --- a/Core/Inc/VCU_StateMachine/VCU_DynamicLevStateMachine.hpp +++ b/Core/Inc/VCU_StateMachine/VCU_DynamicLevStateMachine.hpp @@ -11,6 +11,7 @@ namespace VCU{ Data& data; Actuators& actuators; TCP& tcp_handler; + OutgoingOrders& outgoing_orders; EncoderSensor& encoder; StateMachine state_machine; @@ -18,14 +19,16 @@ namespace VCU{ enum DynamicLevStates{ Idle, + CloseContactors, TakeOff, Accelerating, Brake, Landing, + OpenContactors, }; - DynamicLevStateMachine(Data& data, Actuators& actuators, TCP& tcp, EncoderSensor& encoder) : - data(data), actuators(actuators), tcp_handler(tcp), encoder(encoder) + DynamicLevStateMachine(Data& data, Actuators& actuators, TCP& tcp, OutgoingOrders& outgoing_orders, EncoderSensor& encoder) : + data(data), actuators(actuators), tcp_handler(tcp), outgoing_orders(outgoing_orders), encoder(encoder) {} void add_transitions(){} @@ -38,11 +41,12 @@ namespace VCU{ void init(){ state_machine = {Idle}; + state_machine.add_state(CloseContactors); state_machine.add_state(TakeOff); state_machine.add_state(Accelerating); state_machine.add_state(Brake); state_machine.add_state(Landing); - + state_machine.add_state(OpenContactors); add_on_enter_actions(); add_on_exit_actions(); diff --git a/Core/Inc/VCU_StateMachine/VCU_HealtCheckLoadStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_HealtCheckLoadStateMachine.hpp index e041fa7..54c4a98 100644 --- a/Core/Inc/VCU_StateMachine/VCU_HealtCheckLoadStateMachine.hpp +++ b/Core/Inc/VCU_StateMachine/VCU_HealtCheckLoadStateMachine.hpp @@ -11,48 +11,49 @@ namespace VCU{ Data& data; Actuators& actuators; TCP& tcp_handler; + OutgoingOrders& outgoing_orders; EncoderSensor& encoder; + + CloseContactorsStateMachine close_contactors_state_machine; + OpenContactorsStateMachine open_contactors_state_machine; + StateMachine state_machine; - StackStateOrder<0> start_pushing; StackStateOrder<0> start_crawling; bool ended = false; - static bool pushing_requested; static bool crawling_requested; enum UnloadStates{ Idle, Pushing, - Accelerating, + CloseContactors, + Crawling, Braking, + OpenContactors, }; - LoadStateMachine(Data& data, Actuators& actuators, TCP& tcp, EncoderSensor& encoder) : - data(data), actuators(actuators), tcp_handler(tcp), encoder(encoder), - start_pushing(230, enter_pushing, state_machine, Idle), - start_crawling(231, enter_crawling, state_machine, Pushing) + LoadStateMachine(Data& data, Actuators& actuators, TCP& tcp, OutgoingOrders& outgoing_orders, EncoderSensor& encoder) : + data(data), actuators(actuators), tcp_handler(tcp), outgoing_orders(outgoing_orders), encoder(encoder), + close_contactors_state_machine(data, tcp_handler, outgoing_orders), + open_contactors_state_machine(data, tcp_handler, outgoing_orders), + start_crawling((uint16_t)IncomingOrdersIDs::start_crawling, enter_crawling, state_machine, Pushing) {} - static void enter_pushing(){ - pushing_requested = true; - } - static void enter_crawling(){ crawling_requested = true; } void add_transitions(){ state_machine.add_transition(Idle, Pushing, [&](){ - if(pushing_requested && not ended){ - pushing_requested = false; + if(not ended){ return true; } return false; }); - state_machine.add_transition(Pushing, Accelerating, [&](){ + state_machine.add_transition(Pushing, CloseContactors, [&](){ //Solo se puede emepezar el crawling si se esta en zona de emergencia if(crawling_requested && data.emergency_tape == PinState::ON){ crawling_requested = false; @@ -61,20 +62,24 @@ namespace VCU{ return false; }); - state_machine.add_transition(Accelerating, Braking, [&](){ + state_machine.add_transition(CloseContactors, Crawling, [&](){ + return close_contactors_state_machine.ended; + }); + + state_machine.add_transition(Crawling, Braking, [&](){ return data.emergency_tape == PinState::OFF; }); - state_machine.add_transition(Braking, Idle, [&](){ + state_machine.add_transition(Braking, OpenContactors, [&](){ return data.tapes_acceleration == 0; //TODO: Muy importante hacer esto con la IMU }); + + state_machine.add_transition(OpenContactors, Idle, [&](){ + return open_contactors_state_machine.ended; + }); } void add_on_enter_actions(){ - state_machine.add_enter_action([&](){ - actuators.brakes.enable_emergency_brakes(); - actuators.brakes.brake(); - }, Idle); state_machine.add_enter_action([&](){ actuators.brakes.disable_emergency_brakes(); @@ -84,13 +89,18 @@ namespace VCU{ state_machine.add_enter_action([&](){ //TODO: set engine parameters //TODO: send start engine order - }, Accelerating); + }, Crawling); state_machine.add_enter_action([&](){ //Quizas hace falta darle un offset (avanzar un poco más) para evitar que entre //en fault si se mueve ligeremante hacia atras y ha quedado demasido cerca de la emergency tape. //TODO: send engine brake order }, Braking); + + state_machine.add_enter_action([&](){ + actuators.brakes.enable_emergency_brakes(); + actuators.brakes.brake(); + }, OpenContactors); } void add_on_exit_actions(){ @@ -98,12 +108,15 @@ namespace VCU{ if(data.emergency_tape == PinState::ON){ ErrorHandler("The vehicle is still in emergency zone after Health&Load procedure"); }else{ - ended = true; actuators.brakes.brake(); //Siguiendo el FDD kenos debe quedar frenado con pneumatica hasta empezar una demostración } }, Braking); + state_machine.add_exit_action([&](){ + ended = true; + }, OpenContactors); + } void register_timed_actions(){ @@ -111,15 +124,20 @@ namespace VCU{ if(data.tapes_direction != FORWARD){ ErrorHandler("The vehicle is moving backwards when it should be moving forward during load procedure"); } - }, (ms)1, Accelerating); + }, (ms)1, Crawling); } void init(){ state_machine = {Idle}; + state_machine.add_state(CloseContactors); state_machine.add_state(Pushing); - state_machine.add_state(Accelerating); + state_machine.add_state(Crawling); state_machine.add_state(Braking); + state_machine.add_state(OpenContactors); + + state_machine.add_state_machine(close_contactors_state_machine.state_machine, CloseContactors); + state_machine.add_state_machine(open_contactors_state_machine.state_machine, OpenContactors); add_on_enter_actions(); add_on_exit_actions(); @@ -131,5 +149,4 @@ namespace VCU{ }; bool LoadStateMachine::crawling_requested = false; - bool LoadStateMachine::pushing_requested = false; } diff --git a/Core/Inc/VCU_StateMachine/VCU_HealtCheckUnloadStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_HealtCheckUnloadStateMachine.hpp index c879ebb..aca5b27 100644 --- a/Core/Inc/VCU_StateMachine/VCU_HealtCheckUnloadStateMachine.hpp +++ b/Core/Inc/VCU_StateMachine/VCU_HealtCheckUnloadStateMachine.hpp @@ -14,44 +14,47 @@ namespace VCU{ Data& data; Actuators& actuators; TCP& tcp_handler; + OutgoingOrders& outgoing_orders; EncoderSensor& encoder; - StateMachine state_machine; - StackStateOrder<0> return_order; + CloseContactorsStateMachine close_contactors_state_machine; + OpenContactorsStateMachine open_contactors_state_machine; - bool ended = false; + StateMachine state_machine; - static bool return_requested; + bool ended = false; enum LoadStates{ Idle, + CloseContactors, Returning, Crawling, Braking, - + OpenContactors, }; - UnloadStateMachine(Data& data, Actuators& actuators, TCP& tcp, EncoderSensor& encoder) : - data(data), actuators(actuators), tcp_handler(tcp), encoder(encoder), - return_order(232, enter_returning, state_machine, Idle) + UnloadStateMachine(Data& data, Actuators& actuators, TCP& tcp, OutgoingOrders& outgoing_orders, EncoderSensor& encoder) : + data(data), actuators(actuators), tcp_handler(tcp), outgoing_orders(outgoing_orders), encoder(encoder), + close_contactors_state_machine(data, tcp_handler, outgoing_orders), + open_contactors_state_machine(data, tcp_handler, outgoing_orders) {} - static void enter_returning(){ - return_requested = true; - } - void add_transitions(){ //Quitar los frenos debe hacerse manualmente con una orden una vez finalizado el procedure //porque segun el fdd el pod debe estar frenado hasta el momento que se va a sacar - state_machine.add_transition(Idle, Returning, [&](){ - if(return_requested && not ended){ - return_requested = false; + state_machine.add_transition(Idle, CloseContactors, [&](){ + if(not ended){ return true; } + return false; }); + state_machine.add_transition(CloseContactors, Returning, [&](){ + return close_contactors_state_machine.ended; + }); + state_machine.add_transition(Returning, Crawling, [&](){ return data.emergency_tape == PinState::ON; //Si hemos llegado a la zona de emergencia pasamos a fase crawling }); @@ -60,16 +63,16 @@ namespace VCU{ return data.emergency_tape == PinState::OFF; //Si hemos llegado al final de la zona de emergencia frenamos }); - state_machine.add_transition(Braking, Idle, [&](){ + state_machine.add_transition(Braking, OpenContactors, [&](){ return data.tapes_acceleration == 0; //TODO: Muy importante hacer esto con la IMU del motor }); + + state_machine.add_transition(OpenContactors, Idle, [&](){ + return open_contactors_state_machine.ended; + }); } void add_on_enter_actions(){ - state_machine.add_enter_action([&](){ - actuators.brakes.enable_emergency_brakes(); - actuators.brakes.brake(); - }, Idle); state_machine.add_enter_action([&](){ actuators.brakes.not_brake(); @@ -87,20 +90,26 @@ namespace VCU{ //TODO: send engine stop order }, Braking); + state_machine.add_enter_action([&](){ + actuators.brakes.enable_emergency_brakes(); + actuators.brakes.brake(); + }, OpenContactors); + } void add_on_exit_actions(){ - - state_machine.add_exit_action([&](){ if(data.emergency_tape == PinState::ON){ ErrorHandler("The vehicle is still in emergency zone after Health&Unload procedure"); }else{ - ended = true; actuators.brakes.brake(); } }, Braking); + state_machine.add_exit_action([&](){ + ended = true; + }, OpenContactors); + } void register_timed_actions(){ @@ -125,9 +134,14 @@ namespace VCU{ void init(){ state_machine = {Idle}; + state_machine.add_state(CloseContactors); state_machine.add_state(Returning); state_machine.add_state(Crawling); state_machine.add_state(Braking); + state_machine.add_state(OpenContactors); + + state_machine.add_state_machine(close_contactors_state_machine.state_machine, CloseContactors); + state_machine.add_state_machine(open_contactors_state_machine.state_machine, OpenContactors); add_on_enter_actions(); add_on_exit_actions(); @@ -137,6 +151,4 @@ namespace VCU{ } }; - - bool UnloadStateMachine::return_requested = false; } diff --git a/Core/Inc/VCU_StateMachine/VCU_SpecificStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_SpecificStateMachine.hpp index d5ba1c0..c733c67 100644 --- a/Core/Inc/VCU_StateMachine/VCU_SpecificStateMachine.hpp +++ b/Core/Inc/VCU_StateMachine/VCU_SpecificStateMachine.hpp @@ -11,6 +11,7 @@ namespace VCU{ Data& data; Actuators& actuators; TCP& tcp_handler; + OutgoingOrders& outgoing_orders; EncoderSensor& encoder; LoadStateMachine health_load_state_machine; @@ -42,20 +43,23 @@ namespace VCU{ DynamicLev, StaticLev, Traction, + }; - SpecificStateMachine(Data& data, Actuators& actuators, TCP& tcp, EncoderSensor& encoder) : - data(data),actuators(actuators),tcp_handler(tcp), encoder(encoder), - health_load_state_machine(data, actuators, tcp, encoder), - health_unload_state_machine(data, actuators, tcp, encoder), - traction_state_machine(data, actuators, tcp, encoder), - static_lev_state_machine(data, actuators, tcp, encoder), - dynamic_lev_state_machine(data, actuators, tcp, encoder), - healthcheck_and_load(220, enter_health_and_load, state_machine, Idle), - healthcheck_and_unload(221, enter_health_and_unload, state_machine, Idle), - start_static_lev(222, enter_static_lev, state_machine, Idle), - start_dynamic_lev(223, enter_dynamic_lev, state_machine, Idle, &traction_points), - start_traction(224, enter_traction, state_machine, Idle, &traction_points) + SpecificStateMachine(Data& data, Actuators& actuators, TCP& tcp, OutgoingOrders& outgoing_orders, EncoderSensor& encoder) : + data(data),actuators(actuators),tcp_handler(tcp), outgoing_orders(outgoing_orders), encoder(encoder), + + health_load_state_machine(data, actuators, tcp, outgoing_orders, encoder), + health_unload_state_machine(data, actuators, tcp, outgoing_orders, encoder), + traction_state_machine(data, actuators, tcp, outgoing_orders, encoder), + static_lev_state_machine(data, actuators, tcp, outgoing_orders, encoder), + dynamic_lev_state_machine(data, actuators, tcp, outgoing_orders, encoder), + + healthcheck_and_load((uint16_t)IncomingOrdersIDs::heakthcheck_and_load, enter_health_and_load, state_machine, Idle), + healthcheck_and_unload((uint16_t)IncomingOrdersIDs::healthcheck_and_unload, enter_health_and_unload, state_machine, Idle), + start_static_lev((uint16_t)IncomingOrdersIDs::start_static_lev_demostration, enter_static_lev, state_machine, Idle), + start_dynamic_lev((uint16_t)IncomingOrdersIDs::start_dynamic_lev_demostration, enter_dynamic_lev, state_machine, Idle, &traction_points), + start_traction((uint16_t)IncomingOrdersIDs::start_traction_demostration, enter_traction, state_machine, Idle, &traction_points) {} static void enter_health_and_load(){ diff --git a/Core/Inc/VCU_StateMachine/VCU_StateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_StateMachine.hpp index 51c96a1..761287b 100644 --- a/Core/Inc/VCU_StateMachine/VCU_StateMachine.hpp +++ b/Core/Inc/VCU_StateMachine/VCU_StateMachine.hpp @@ -99,6 +99,7 @@ namespace VCU{ Data& data; Actuators& actuators; TCP& tcp_handler; + OutgoingOrders& outgoing_orders; EncoderSensor& encoder; SpecificStateMachine specific_state_machine; StateMachine general_state_machine; @@ -113,8 +114,8 @@ namespace VCU{ FAULT }; - GeneralStateMachine(Data& data, Actuators& actuators, TCP& tcp, EncoderSensor& encoder) : - data(data), actuators(actuators), tcp_handler(tcp), encoder(encoder), specific_state_machine(data, actuators, tcp, encoder) + GeneralStateMachine(Data& data, Actuators& actuators, TCP& tcp, OutgoingOrders& outgoing_orders, EncoderSensor& encoder) : + data(data), actuators(actuators), tcp_handler(tcp), outgoing_orders(outgoing_orders), encoder(encoder), specific_state_machine(data, actuators, tcp, outgoing_orders, encoder) {} void add_transitions(){ diff --git a/Core/Inc/VCU_StateMachine/VCU_StaticLevStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_StaticLevStateMachine.hpp index db8ce78..3b2dee2 100644 --- a/Core/Inc/VCU_StateMachine/VCU_StaticLevStateMachine.hpp +++ b/Core/Inc/VCU_StateMachine/VCU_StaticLevStateMachine.hpp @@ -11,47 +11,97 @@ namespace VCU{ Data& data; Actuators& actuators; TCP& tcp_handler; + OutgoingOrders& outgoing_orders; EncoderSensor& encoder; + + CloseContactorsStateMachine close_contactors_state_machine; + OpenContactorsStateMachine open_contactors_state_machine; + StateMachine state_machine; + StackStateOrder<0> take_off; + StackStateOrder<0> landing; + bool ended = false; + static bool start_levitation_requested; + static bool stop_levitation_requested; + enum DynamicLevStates{ + CloseContactors, LevOff, LevOn, + OpenContactors, }; - StaticLevStateMachine(Data& data, Actuators& actuators, TCP& tcp, EncoderSensor& encoder) : - data(data), actuators(actuators), tcp_handler(tcp), encoder(encoder) + StaticLevStateMachine(Data& data, Actuators& actuators, TCP& tcp, OutgoingOrders& outgoing_orders, EncoderSensor& encoder) : + data(data), actuators(actuators), tcp_handler(tcp), outgoing_orders(outgoing_orders), encoder(encoder), + close_contactors_state_machine(data, tcp_handler, outgoing_orders), + open_contactors_state_machine(data, tcp_handler, outgoing_orders), + take_off((uint16_t)IncomingOrdersIDs::take_off, start_levitation, state_machine, LevOff), + landing((uint16_t)IncomingOrdersIDs::landing, stop_levitation, state_machine, LevOn) {} - void add_transitions(){ - //TODO: Las transiciones son enteramente con state orders + static void start_levitation(){ + start_levitation_requested = true; + } + static void stop_levitation(){ + stop_levitation_requested = true; + } + + void add_transitions(){ + state_machine.add_transition(CloseContactors, LevOff, [&](){ + return close_contactors_state_machine.ended; + }); + + state_machine.add_transition(LevOff, LevOn, [&](){ + if(start_levitation_requested && not ended){ + start_levitation_requested = false; + return true; + } + return false; + }); + + state_machine.add_transition(LevOn, OpenContactors, [&](){ + if(stop_levitation_requested){ + stop_levitation_requested = false; + return true; + } + return false; + }); + + state_machine.add_transition(OpenContactors, LevOff, [&](){ + return open_contactors_state_machine.ended; + }); } void add_on_enter_actions(){ state_machine.add_enter_action([&](){ actuators.brakes.not_brake(); - - //TODO: set lev parameters - //TODO: send lev order + tcp_handler.send_to_lcu(outgoing_orders.take_off_order); }, LevOn); state_machine.add_enter_action([&](){ - //TODO: send stop lev order + tcp_handler.send_to_lcu(outgoing_orders.landing_order); actuators.brakes.brake(); }, LevOff); } - void add_on_exit_actions(){} + void add_on_exit_actions(){ + state_machine.add_exit_action([&](){ + ended = true; + }, OpenContactors); + } void register_timed_actions(){} void init(){ - state_machine = {LevOff}; + state_machine = {CloseContactors}; + state_machine.add_state(LevOff); state_machine.add_state(LevOn); + state_machine.add_state(OpenContactors); add_on_enter_actions(); add_on_exit_actions(); @@ -61,4 +111,7 @@ namespace VCU{ } }; + + bool StaticLevStateMachine::start_levitation_requested = false; + bool StaticLevStateMachine::stop_levitation_requested = false; } diff --git a/Core/Inc/VCU_StateMachine/VCU_TractionStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_TractionStateMachine.hpp index f48af53..51aed8e 100644 --- a/Core/Inc/VCU_StateMachine/VCU_TractionStateMachine.hpp +++ b/Core/Inc/VCU_StateMachine/VCU_TractionStateMachine.hpp @@ -11,6 +11,7 @@ namespace VCU{ Data& data; Actuators& actuators; TCP& tcp_handler; + OutgoingOrders& outgoing_orders; EncoderSensor& encoder; StateMachine state_machine; @@ -18,12 +19,14 @@ namespace VCU{ enum DynamicLevStates{ Idle, + CloseContactors, Accelerating, Brake, + OpenContactors, }; - TractionLevStateMachine(Data& data, Actuators&actuators, TCP& tcp, EncoderSensor& encoder) : - data(data),actuators(actuators),tcp_handler(tcp), encoder(encoder) + TractionLevStateMachine(Data& data, Actuators&actuators, TCP& tcp, OutgoingOrders& outgoing_orders, EncoderSensor& encoder) : + data(data),actuators(actuators),tcp_handler(tcp), outgoing_orders(outgoing_orders), encoder(encoder) {} void add_transitions(){} @@ -38,8 +41,10 @@ namespace VCU{ void init(){ state_machine = {Idle}; + state_machine.add_state(CloseContactors); state_machine.add_state(Accelerating); state_machine.add_state(Brake); + state_machine.add_state(OpenContactors); add_on_enter_actions(); add_on_exit_actions(); diff --git a/Core/Inc/VCU_Utilities/VCU_Includes.hpp b/Core/Inc/VCU_Utilities/VCU_Includes.hpp index 50a39e3..2787b1e 100644 --- a/Core/Inc/VCU_Utilities/VCU_Includes.hpp +++ b/Core/Inc/VCU_Utilities/VCU_Includes.hpp @@ -20,6 +20,7 @@ #include "VCU_Communications/VCU_TCP/IncomingOrders.hpp" #include "VCU_Communications/VCU_UDP/Packets.hpp" +#include "VCU_StateMachine/VCU_ContactorsStateMachine.hpp" #include "VCU_StateMachine/VCU_HealtCheckLoadStateMachine.hpp" #include "VCU_StateMachine/VCU_HealtCheckUnloadStateMachine.hpp" #include "VCU_StateMachine/VCU_DynamicLevStateMachine.hpp" diff --git a/Core/Inc/VCU_Utilities/VCU_Types.hpp b/Core/Inc/VCU_Utilities/VCU_Types.hpp index 5b4689a..6a96d08 100644 --- a/Core/Inc/VCU_Utilities/VCU_Types.hpp +++ b/Core/Inc/VCU_Utilities/VCU_Types.hpp @@ -17,5 +17,19 @@ namespace VCU{ FORWARD = 1, }; + enum ContactorState{ + Open, + Close, + }; + + enum LevitaionState{ + IDLE, + TAKING_OFF, + STABLE, + STICK_UP, + STICK_DOWN, + LANDING, + }; + } From 1911cf2983da0b7f3adc68df8c68a8b70e1b22e1 Mon Sep 17 00:00:00 2001 From: Pablo Date: Thu, 22 Jun 2023 20:47:32 +0200 Subject: [PATCH 07/10] queda pointtravel , hay que revisar y testear --- Core/Inc/VCU_Data/VCU_Data.hpp | 10 ++ .../VCU_DynamicLevStateMachine.hpp | 91 +++++++++++++++-- .../VCU_PointTravelStateMachine.hpp | 99 +++++++++++++++++++ .../VCU_SpecificStateMachine.hpp | 8 +- .../VCU_StaticLevStateMachine.hpp | 1 + .../VCU_TractionStateMachine.hpp | 67 ++++++++++--- Core/Inc/VCU_Utilities/VCU_Includes.hpp | 1 + 7 files changed, 251 insertions(+), 26 deletions(-) create mode 100644 Core/Inc/VCU_StateMachine/VCU_PointTravelStateMachine.hpp diff --git a/Core/Inc/VCU_Data/VCU_Data.hpp b/Core/Inc/VCU_Data/VCU_Data.hpp index 3a57f1c..3559c15 100644 --- a/Core/Inc/VCU_Data/VCU_Data.hpp +++ b/Core/Inc/VCU_Data/VCU_Data.hpp @@ -71,6 +71,16 @@ namespace VCU{ ContactorState contactors_state = ContactorState::Open; + float engine_speed; + + //Demostrations + float target_speed; + vector traction_points; + + uint32_t brake_distance_lookup_table[35] = { + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 + }; + void add_protections(){ add_protection(&high_pressure1, Boundary(300)); add_protection(&reeds_ok, Boundary(true)); diff --git a/Core/Inc/VCU_StateMachine/VCU_DynamicLevStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_DynamicLevStateMachine.hpp index 6d3dc96..65f6097 100644 --- a/Core/Inc/VCU_StateMachine/VCU_DynamicLevStateMachine.hpp +++ b/Core/Inc/VCU_StateMachine/VCU_DynamicLevStateMachine.hpp @@ -7,12 +7,19 @@ namespace VCU{ template<> class DynamicLevStateMachine{ + static constexpr uint32_t levitation_timeout = 6000;//ms + static constexpr uint32_t landing_timeout = 3000;//ms public: Data& data; Actuators& actuators; TCP& tcp_handler; OutgoingOrders& outgoing_orders; EncoderSensor& encoder; + + CloseContactorsStateMachine close_contactors_state_machine; + OpenContactorsStateMachine open_contactors_state_machine; + PointTravelStateMachine point_travel_state_machine; + StateMachine state_machine; bool ended = false; @@ -21,21 +28,85 @@ namespace VCU{ Idle, CloseContactors, TakeOff, - Accelerating, - Brake, + PointTravel, Landing, OpenContactors, }; DynamicLevStateMachine(Data& data, Actuators& actuators, TCP& tcp, OutgoingOrders& outgoing_orders, EncoderSensor& encoder) : - data(data), actuators(actuators), tcp_handler(tcp), outgoing_orders(outgoing_orders), encoder(encoder) + data(data), actuators(actuators), tcp_handler(tcp), outgoing_orders(outgoing_orders), encoder(encoder), + close_contactors_state_machine(data, tcp_handler, outgoing_orders), + open_contactors_state_machine(data, tcp_handler, outgoing_orders), + point_travel_state_machine(data, tcp_handler, outgoing_orders) {} - void add_transitions(){} + void add_transitions(){ + state_machine.add_transition(Idle, CloseContactors, [&](){ + if(not ended){ + return true; + } + return false; + }); + + state_machine.add_transition(CloseContactors, TakeOff, [&](){ + return close_contactors_state_machine.ended; + }); + + state_machine.add_transition(TakeOff, PointTravel, [&](){ + return data.levitation_state == LevitaionState::STABLE; + }); + + state_machine.add_transition(PointTravel, Landing, [&](){ + return point_travel_state_machine.ended; + }); + + state_machine.add_transition(Landing, OpenContactors, [&](){ + return data.levitation_state == LevitaionState::IDLE; + }); + + state_machine.add_transition(OpenContactors, Idle, [&](){ + return open_contactors_state_machine.ended; + }); + + } + + void add_on_enter_actions(){ + state_machine.add_enter_action([&](){ + actuators.brakes.not_brake(); + actuators.brakes.enable_emergency_brakes(); + }, CloseContactors); + + state_machine.add_enter_action([&](){ + //TODO: Enviar orden de levitar + + Time::set_timeout(levitation_timeout, [&](){ + if(state_machine.current_state == TakeOff && data.levitation_state != STABLE){ + ErrorHandler("Kenos has not been able to stabilize in time, maximum timeout: %dms", levitation_timeout); + } + }); + }, TakeOff); - void add_on_enter_actions(){} + state_machine.add_enter_action([&](){ + //TODO: Enviar orden de landing - void add_on_exit_actions(){} + Time::set_timeout(landing_timeout, [&](){ + if(state_machine.current_state == Landing && data.levitation_state != IDLE){ + ErrorHandler("Kenos failed to land on time, maximum timeout: %dms", landing_timeout); + } + }); + }, Landing); + + state_machine.add_enter_action([&](){ + actuators.brakes.brake(); + actuators.brakes.enable_emergency_brakes(); + }, OpenContactors); + } + + void add_on_exit_actions(){ + state_machine.add_exit_action([&](){ + ended = true; + }, OpenContactors); + } void register_timed_actions(){} @@ -43,17 +114,19 @@ namespace VCU{ state_machine = {Idle}; state_machine.add_state(CloseContactors); state_machine.add_state(TakeOff); - state_machine.add_state(Accelerating); - state_machine.add_state(Brake); + state_machine.add_state(PointTravel); state_machine.add_state(Landing); state_machine.add_state(OpenContactors); + state_machine.add_state_machine(close_contactors_state_machine.state_machine, CloseContactors); + state_machine.add_state_machine(open_contactors_state_machine.state_machine, OpenContactors); + state_machine.add_state_machine(point_travel_state_machine.state_machine, PointTravel); + add_on_enter_actions(); add_on_exit_actions(); add_transitions(); register_timed_actions(); add_transitions(); } - }; } diff --git a/Core/Inc/VCU_StateMachine/VCU_PointTravelStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_PointTravelStateMachine.hpp new file mode 100644 index 0000000..7272c42 --- /dev/null +++ b/Core/Inc/VCU_StateMachine/VCU_PointTravelStateMachine.hpp @@ -0,0 +1,99 @@ +#pragma once + +#include "VCU_Utilities/VCU_Includes.hpp" + +namespace VCU{ + template class PointTravelStateMachine; + + template<> + class PointTravelStateMachine{ + static constexpr uint32_t initial_aux_state = 0; + + public: + Data& data; + TCP& tcp_handler; + OutgoingOrders& outgoing_orders; + + StateMachine state_machine; + + enum TravelStates{ + ReceivingData, + Calculating, + MovingForward, + MovingBackward, + Braking, + End, + }; + + bool ended = false; + + uint32_t aux_last_state; + + PointTravelStateMachine(Data& data, TCP& tcp, OutgoingOrders& outgoing_orders) : + data(data),tcp_handler(tcp), outgoing_orders(outgoing_orders) + {} + + void add_transitions(){ + + uint32_t next_state; + for(uint32_t current_state = initial_aux_state; current_state < data.traction_points.size() + initial_aux_state; current_state++){ + + if (current_state + 1 > aux_last_state) { + next_state = (uint32_t)End; + }else{ + next_state = ++current_state; + } + + state_machine.add_transition(current_state, next_state, [&](){ + uint32_t speed = std::ceil(std::max(data.target_speed, data.engine_speed)); + uint32_t brake_distance = data.brake_distance_lookup_table[speed]; + uint32_t objective_position = data.traction_points.at(current_state - initial_aux_state); + if (data.tapes_position > objective_position) { + return data.tapes_position <= (objective_position + brake_distance); + }else{ + return data.tapes_position >= (objective_position - brake_distance); + } + }); + } + } + + void add_on_enter_actions(){ + for(uint32_t current_state = initial_aux_state; current_state < data.traction_points.size() + initial_aux_state; current_state++){ + + state_machine.add_enter_action([&](){ + uint32_t objective_position = data.traction_points.at(current_state - initial_aux_state); + if (data.tapes_position > objective_position) { + //TODO: send order to MOVE BACKWARDS + }else{ + //TODO: send order to MOVE FORWARDS + } + }, current_state); + } + + state_machine.add_enter_action([&](){ + ended = true; + }, End); + } + + void add_on_exit_actions(){} + + void register_timed_actions(){} + + void init(){ + + state_machine = initial_aux_state; + + for(uint32_t i = initial_aux_state + 1; i < (data.traction_points.size() + initial_aux_state); i++){ + state_machine.add_state(i); + aux_last_state = i; + } + + add_on_enter_actions(); + add_on_exit_actions(); + add_transitions(); + register_timed_actions(); + add_transitions(); + } + + }; +} diff --git a/Core/Inc/VCU_StateMachine/VCU_SpecificStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_SpecificStateMachine.hpp index c733c67..e064451 100644 --- a/Core/Inc/VCU_StateMachine/VCU_SpecificStateMachine.hpp +++ b/Core/Inc/VCU_StateMachine/VCU_SpecificStateMachine.hpp @@ -16,7 +16,7 @@ namespace VCU{ LoadStateMachine health_load_state_machine; UnloadStateMachine health_unload_state_machine; - TractionLevStateMachine traction_state_machine; + TractionStateMachine traction_state_machine; StaticLevStateMachine static_lev_state_machine; DynamicLevStateMachine dynamic_lev_state_machine; @@ -28,8 +28,6 @@ namespace VCU{ StateMachine state_machine; - vector traction_points; - static bool healthcheck_and_load_requested; static bool healthcheck_and_unload_requested; static bool start_static_lev_requested; @@ -58,8 +56,8 @@ namespace VCU{ healthcheck_and_load((uint16_t)IncomingOrdersIDs::heakthcheck_and_load, enter_health_and_load, state_machine, Idle), healthcheck_and_unload((uint16_t)IncomingOrdersIDs::healthcheck_and_unload, enter_health_and_unload, state_machine, Idle), start_static_lev((uint16_t)IncomingOrdersIDs::start_static_lev_demostration, enter_static_lev, state_machine, Idle), - start_dynamic_lev((uint16_t)IncomingOrdersIDs::start_dynamic_lev_demostration, enter_dynamic_lev, state_machine, Idle, &traction_points), - start_traction((uint16_t)IncomingOrdersIDs::start_traction_demostration, enter_traction, state_machine, Idle, &traction_points) + start_dynamic_lev((uint16_t)IncomingOrdersIDs::start_dynamic_lev_demostration, enter_dynamic_lev, state_machine, Idle, &data.traction_points), + start_traction((uint16_t)IncomingOrdersIDs::start_traction_demostration, enter_traction, state_machine, Idle, &data.traction_points) {} static void enter_health_and_load(){ diff --git a/Core/Inc/VCU_StateMachine/VCU_StaticLevStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_StaticLevStateMachine.hpp index 3b2dee2..9c27c31 100644 --- a/Core/Inc/VCU_StateMachine/VCU_StaticLevStateMachine.hpp +++ b/Core/Inc/VCU_StateMachine/VCU_StaticLevStateMachine.hpp @@ -77,6 +77,7 @@ namespace VCU{ } void add_on_enter_actions(){ + state_machine.add_enter_action([&](){ actuators.brakes.not_brake(); tcp_handler.send_to_lcu(outgoing_orders.take_off_order); diff --git a/Core/Inc/VCU_StateMachine/VCU_TractionStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_TractionStateMachine.hpp index 51aed8e..4a93bbd 100644 --- a/Core/Inc/VCU_StateMachine/VCU_TractionStateMachine.hpp +++ b/Core/Inc/VCU_StateMachine/VCU_TractionStateMachine.hpp @@ -3,16 +3,21 @@ #include "VCU_Utilities/VCU_Includes.hpp" namespace VCU{ - template class TractionLevStateMachine; + template class TractionStateMachine; template<> - class TractionLevStateMachine{ + class TractionStateMachine{ public: Data& data; Actuators& actuators; TCP& tcp_handler; OutgoingOrders& outgoing_orders; EncoderSensor& encoder; + + CloseContactorsStateMachine close_contactors_state_machine; + OpenContactorsStateMachine open_contactors_state_machine; + PointTravelStateMachine point_travel_state_machine; + StateMachine state_machine; bool ended = false; @@ -20,21 +25,57 @@ namespace VCU{ enum DynamicLevStates{ Idle, CloseContactors, - Accelerating, - Brake, + PointTravel, OpenContactors, }; - TractionLevStateMachine(Data& data, Actuators&actuators, TCP& tcp, OutgoingOrders& outgoing_orders, EncoderSensor& encoder) : - data(data),actuators(actuators),tcp_handler(tcp), outgoing_orders(outgoing_orders), encoder(encoder) + uint32_t aux_last_state; + + TractionStateMachine(Data& data, Actuators&actuators, TCP& tcp, OutgoingOrders& outgoing_orders, EncoderSensor& encoder) : + data(data),actuators(actuators),tcp_handler(tcp), outgoing_orders(outgoing_orders), encoder(encoder), + close_contactors_state_machine(data, tcp_handler, outgoing_orders), + open_contactors_state_machine(data, tcp_handler, outgoing_orders), + point_travel_state_machine(data, tcp_handler, outgoing_orders) {} - void add_transitions(){} + void add_transitions(){ + state_machine.add_transition(Idle, CloseContactors, [&](){ + if(not ended){ + return true; + } + return false; + }); - void add_on_enter_actions(){} + state_machine.add_transition(CloseContactors, PointTravel, [&](){ + return close_contactors_state_machine.ended; + }); - void add_on_exit_actions(){ + state_machine.add_transition(PointTravel, OpenContactors, [&](){ + return point_travel_state_machine.ended; + }); + state_machine.add_transition(OpenContactors, Idle, [&](){ + return open_contactors_state_machine.ended; + }); + } + + void add_on_enter_actions(){ + state_machine.add_enter_action([&](){ + actuators.brakes.not_brake(); + actuators.brakes.enable_emergency_brakes(); + }, CloseContactors); + + state_machine.add_enter_action([&](){ + actuators.brakes.brake(); + actuators.brakes.enable_emergency_brakes(); + }, OpenContactors); + + } + + void add_on_exit_actions(){ + state_machine.add_exit_action([&](){ + ended = true; + }, OpenContactors); } void register_timed_actions(){} @@ -42,16 +83,18 @@ namespace VCU{ void init(){ state_machine = {Idle}; state_machine.add_state(CloseContactors); - state_machine.add_state(Accelerating); - state_machine.add_state(Brake); + state_machine.add_state(PointTravel); state_machine.add_state(OpenContactors); + state_machine.add_state_machine(close_contactors_state_machine.state_machine, CloseContactors); + state_machine.add_state_machine(open_contactors_state_machine.state_machine, OpenContactors); + state_machine.add_state_machine(point_travel_state_machine.state_machine, PointTravel); + add_on_enter_actions(); add_on_exit_actions(); add_transitions(); register_timed_actions(); add_transitions(); } - }; } diff --git a/Core/Inc/VCU_Utilities/VCU_Includes.hpp b/Core/Inc/VCU_Utilities/VCU_Includes.hpp index 2787b1e..4a98be9 100644 --- a/Core/Inc/VCU_Utilities/VCU_Includes.hpp +++ b/Core/Inc/VCU_Utilities/VCU_Includes.hpp @@ -21,6 +21,7 @@ #include "VCU_Communications/VCU_UDP/Packets.hpp" #include "VCU_StateMachine/VCU_ContactorsStateMachine.hpp" +#include "VCU_StateMachine/VCU_PointTravelStateMachine.hpp" #include "VCU_StateMachine/VCU_HealtCheckLoadStateMachine.hpp" #include "VCU_StateMachine/VCU_HealtCheckUnloadStateMachine.hpp" #include "VCU_StateMachine/VCU_DynamicLevStateMachine.hpp" From d4281db50aca4d5fcf091519d20fd6ce5eda372d Mon Sep 17 00:00:00 2001 From: Pablo Date: Thu, 29 Jun 2023 10:50:26 +0200 Subject: [PATCH 08/10] mucho trabjo por hacer, checkpoint --- Core/Inc/VCU.hpp | 13 +- Core/Inc/VCU_Actuators/VCU_LedsActuator.hpp | 4 + .../VCU_TCP/IncomingOrders.hpp | 2 + .../VCU_TCP/OutgoingOrders.hpp | 13 +- .../VCU_Communications/VCU_TCP/VCU_TCP.hpp | 16 +- .../VCU_Communications/VCU_UDP/Packets.hpp | 1 + .../VCU_Communications/VCU_UDP/VCU_UDP.hpp | 6 +- Core/Inc/VCU_Data/VCU_Data.hpp | 25 ++- Core/Inc/VCU_Mode/VCU_Mode.hpp | 1 + .../VCU_ContactorsStateMachine.hpp | 15 +- .../VCU_DynamicLevStateMachine.hpp | 44 +++++- .../VCU_HealtCheckLoadStateMachine.hpp | 28 ++-- .../VCU_HealtCheckUnloadStateMachine.hpp | 4 +- .../VCU_PointTravelStateMachine.hpp | 145 ++++++++++++------ .../VCU_ReceivingDataStateMachine.hpp | 88 +++++++++++ .../VCU_SpecificStateMachine.hpp | 48 +++--- .../Inc/VCU_StateMachine/VCU_StateMachine.hpp | 30 +++- .../VCU_StaticLevStateMachine.hpp | 28 ++-- .../VCU_TractionStateMachine.hpp | 33 +++- Core/Inc/VCU_Utilities/VCU_Includes.hpp | 1 + Core/Inc/VCU_Utilities/VCU_Types.hpp | 23 +++ Core/Src/Runes/Runes.hpp | 13 +- Core/Src/main.cpp | 75 ++++++++- .../Third_Party/LwIP/src/include/lwip/opt.h | 4 +- VCU Debug.launch | 2 +- 25 files changed, 520 insertions(+), 142 deletions(-) create mode 100644 Core/Inc/VCU_StateMachine/VCU_ReceivingDataStateMachine.hpp diff --git a/Core/Inc/VCU.hpp b/Core/Inc/VCU.hpp index 06cb162..da69e06 100644 --- a/Core/Inc/VCU.hpp +++ b/Core/Inc/VCU.hpp @@ -70,8 +70,8 @@ namespace VCU{ void init(){ STLIB::start(); actuators.brakes.init(); - udp_handler.init(); tcp_handler.init(); + udp_handler.init(); } static void read_brakes_sensors(){ @@ -83,11 +83,11 @@ namespace VCU{ } static void send_to_backend(){ - vcu->udp_handler.BACKEND_CONNECTION.send(vcu->packets.regulator_packet); - vcu->udp_handler.BACKEND_CONNECTION.send(vcu->packets.pressure_packets); - vcu->udp_handler.BACKEND_CONNECTION.send(vcu->packets.bottle_temperature_packet); - vcu->udp_handler.BACKEND_CONNECTION.send(vcu->packets.reed_packet); - vcu->udp_handler.BACKEND_CONNECTION.send(vcu->packets.environmental_packet); + vcu->udp_handler.send_to_backend(vcu->packets.regulator_packet); + vcu->udp_handler.send_to_backend(vcu->packets.pressure_packets); + vcu->udp_handler.send_to_backend(vcu->packets.bottle_temperature_packet); + vcu->udp_handler.send_to_backend(vcu->packets.reed_packet); + vcu->udp_handler.send_to_backend(vcu->packets.environmental_packet); } static void update_state_machine(){ @@ -119,3 +119,4 @@ namespace VCU{ } VCU::VCU_CLASS* VCU::VCU_CLASS::vcu = nullptr; +VCU::VCU_CLASS* VCU::VCU_CLASS::vcu = nullptr; diff --git a/Core/Inc/VCU_Actuators/VCU_LedsActuator.hpp b/Core/Inc/VCU_Actuators/VCU_LedsActuator.hpp index c1f3876..db693b7 100644 --- a/Core/Inc/VCU_Actuators/VCU_LedsActuator.hpp +++ b/Core/Inc/VCU_Actuators/VCU_LedsActuator.hpp @@ -25,6 +25,10 @@ namespace VCU{ public: LEDSActuator(Pin& red_pin, Pin& green_pin, Pin& blue_pin): red(red_pin), green(green_pin), blue(blue_pin){ + + } + + void leds_init(){ red.set_frequency(1000); green.set_frequency(1000); blue.set_frequency(1000); diff --git a/Core/Inc/VCU_Communications/VCU_TCP/IncomingOrders.hpp b/Core/Inc/VCU_Communications/VCU_TCP/IncomingOrders.hpp index f57f78c..da454c9 100644 --- a/Core/Inc/VCU_Communications/VCU_TCP/IncomingOrders.hpp +++ b/Core/Inc/VCU_Communications/VCU_TCP/IncomingOrders.hpp @@ -22,6 +22,8 @@ namespace VCU{ take_off = 226, landing = 227, start_crawling = 231, + traction_data = 233, + traction_end_data = 234, }; diff --git a/Core/Inc/VCU_Communications/VCU_TCP/OutgoingOrders.hpp b/Core/Inc/VCU_Communications/VCU_TCP/OutgoingOrders.hpp index cc1feb1..2a8af87 100644 --- a/Core/Inc/VCU_Communications/VCU_TCP/OutgoingOrders.hpp +++ b/Core/Inc/VCU_Communications/VCU_TCP/OutgoingOrders.hpp @@ -16,7 +16,9 @@ namespace VCU{ }; enum class TractionOrders: uint16_t{ - + MOVE = 612, + BRAKE = 613, + TURN_OFF = 614, }; enum class BatteryOrders: uint16_t{ @@ -33,12 +35,21 @@ namespace VCU{ StackOrder<0> take_off_order; StackOrder<0> landing_order; + StackOrder<4, float> move; + StackOrder<0> brake; + StackOrder<0> turn_off; + StackOrder<0> close_contactors; StackOrder<0> open_contactors; + float speed = 0.0f; + OutgoingOrders() : take_off_order((uint16_t)LevitationOrdes::TAKE_OFF), landing_order((uint16_t)LevitationOrdes::LANDING), + move((uint16_t)TractionOrders::MOVE, &speed), + brake((uint16_t)TractionOrders::BRAKE), + turn_off((uint16_t)TractionOrders::TURN_OFF), close_contactors((uint16_t)BatteryOrders::CLOSE_CONTACTORS), open_contactors((uint16_t)BatteryOrders::OPEN_CONTACTORS) {} diff --git a/Core/Inc/VCU_Communications/VCU_TCP/VCU_TCP.hpp b/Core/Inc/VCU_Communications/VCU_TCP/VCU_TCP.hpp index c6d954d..56df9de 100644 --- a/Core/Inc/VCU_Communications/VCU_TCP/VCU_TCP.hpp +++ b/Core/Inc/VCU_Communications/VCU_TCP/VCU_TCP.hpp @@ -47,13 +47,13 @@ namespace VCU{ BACKEND_CONNECTION = ServerSocket(VCU_IP, SERVER_PORT); OBCCU_CONNECTION = Socket(VCU_IP, CLIENT_PORT, OBCCU_IP, SERVER_PORT); - OBCCU_CONNECTION.reconnect(); +// OBCCU_CONNECTION.reconnect(); BMSL_CONNECTION = Socket(VCU_IP, CLIENT_PORT, BMSL_IP, SERVER_PORT); - BMSL_CONNECTION.reconnect(); +// BMSL_CONNECTION.reconnect(); PCU_CONNECTION = Socket(VCU_IP, CLIENT_PORT, PCU_IP, SERVER_PORT); - PCU_CONNECTION.reconnect(); +// PCU_CONNECTION.reconnect(); LCU_MASTER_CONNECTION = Socket(VCU_IP, CLIENT_PORT, LCU_MASTER_IP, SERVER_PORT); } @@ -80,15 +80,15 @@ namespace VCU{ bool check_connections(){ return - BACKEND_CONNECTION.state == ServerSocket::ServerState::ACCEPTED + BACKEND_CONNECTION.is_connected() && - OBCCU_CONNECTION.state == ServerSocket::ServerState::ACCEPTED + OBCCU_CONNECTION.is_connected() && - BMSL_CONNECTION.state == ServerSocket::ServerState::ACCEPTED + BMSL_CONNECTION.is_connected() && - LCU_MASTER_CONNECTION.state == ServerSocket::ServerState::ACCEPTED + LCU_MASTER_CONNECTION.is_connected() && - PCU_CONNECTION.state == ServerSocket::ServerState::ACCEPTED + PCU_CONNECTION.is_connected() ; } }; diff --git a/Core/Inc/VCU_Communications/VCU_UDP/Packets.hpp b/Core/Inc/VCU_Communications/VCU_UDP/Packets.hpp index cd36d7f..2a7e393 100644 --- a/Core/Inc/VCU_Communications/VCU_UDP/Packets.hpp +++ b/Core/Inc/VCU_Communications/VCU_UDP/Packets.hpp @@ -41,6 +41,7 @@ namespace VCU{ //Incoming packets + Packets(Data& data) : regulator_packet(211, &data.valve_state, &data.regulator_reference_pressure, &data.regulator_real_pressure), reed_packet(212, &data.reed1, &data.reed2, &data.reed3, &data.reed4, &data.reeds_ok), diff --git a/Core/Inc/VCU_Communications/VCU_UDP/VCU_UDP.hpp b/Core/Inc/VCU_Communications/VCU_UDP/VCU_UDP.hpp index 4854a63..46ab91a 100644 --- a/Core/Inc/VCU_Communications/VCU_UDP/VCU_UDP.hpp +++ b/Core/Inc/VCU_Communications/VCU_UDP/VCU_UDP.hpp @@ -51,11 +51,13 @@ namespace VCU{ BMSL_CONNECTION = DatagramSocket(VCU_IP, UDP_PORT, BMSL_IP, UDP_PORT); BMSL_CONNECTION.reconnect(); + LCU_MASTER_CONNECTION = DatagramSocket(VCU_IP, UDP_PORT, LCU_MASTER_IP, UDP_PORT); + LCU_MASTER_CONNECTION.reconnect(); + PCU_CONNECTION = DatagramSocket(VCU_IP, UDP_PORT, PCU_IP, UDP_PORT); PCU_CONNECTION.reconnect(); - LCU_MASTER_CONNECTION = DatagramSocket(VCU_IP, UDP_PORT, LCU_MASTER_IP, UDP_PORT); - LCU_MASTER_CONNECTION.reconnect(); + } void send_to_backend(Packet& packet){ diff --git a/Core/Inc/VCU_Data/VCU_Data.hpp b/Core/Inc/VCU_Data/VCU_Data.hpp index 3559c15..a581774 100644 --- a/Core/Inc/VCU_Data/VCU_Data.hpp +++ b/Core/Inc/VCU_Data/VCU_Data.hpp @@ -72,16 +72,35 @@ namespace VCU{ ContactorState contactors_state = ContactorState::Open; float engine_speed; + float engine_acceleration; //Demostrations - float target_speed; - vector traction_points; + vector traction_points; uint32_t brake_distance_lookup_table[35] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; - void add_protections(){ + uint32_t change_direction_distance_lookup_table[35] = { + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 + }; + + + bool get_next_point(point_t* p){ + if (traction_points.empty()) { + return false; + } + + *p = *traction_points.begin(); + traction_points.erase(traction_points.begin()); + return true; + } + + void clean_traction_points(){ + traction_points.clear(); + } + + void add_protections(){//TODO: Mover esto a un archivo separado y completar add_protection(&high_pressure1, Boundary(300)); add_protection(&reeds_ok, Boundary(true)); add_protection(&emergency_tape, Boundary(PinState::ON)); diff --git a/Core/Inc/VCU_Mode/VCU_Mode.hpp b/Core/Inc/VCU_Mode/VCU_Mode.hpp index d79414b..d15c569 100644 --- a/Core/Inc/VCU_Mode/VCU_Mode.hpp +++ b/Core/Inc/VCU_Mode/VCU_Mode.hpp @@ -5,5 +5,6 @@ namespace VCU{ BRAKE_VALIDATION, POSITION_VALIDATION, VEHICLE, + SHUTUP_TESTING, }; } diff --git a/Core/Inc/VCU_StateMachine/VCU_ContactorsStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_ContactorsStateMachine.hpp index 434e823..78dee08 100644 --- a/Core/Inc/VCU_StateMachine/VCU_ContactorsStateMachine.hpp +++ b/Core/Inc/VCU_StateMachine/VCU_ContactorsStateMachine.hpp @@ -15,14 +15,16 @@ namespace VCU{ bool ended = false; - enum DynamicLevStates{ + enum CloseContactorStates{ RequestClose, Closed, }; CloseContactorsStateMachine(Data& data, TCP& tcp, OutgoingOrders outgoing_orders) : data(data), tcp_handler(tcp), outgoing_orders(outgoing_orders) - {} + { + init(); + } void add_transitions(){ state_machine.add_transition(RequestClose, Closed, [&](){ @@ -72,14 +74,16 @@ namespace VCU{ bool ended = false; - enum DynamicLevStates{ + enum OpenContactorsStates{ RequestOpen, Opened, }; OpenContactorsStateMachine(Data& data, TCP& tcp, OutgoingOrders outgoing_orders) : data(data), tcp_handler(tcp), outgoing_orders(outgoing_orders) - {} + { + init(); + } void add_transitions(){ state_machine.add_transition(RequestOpen, Opened, [&](){ @@ -103,9 +107,10 @@ namespace VCU{ void register_timed_actions(){} void init(){ - state_machine = {RequestOpen}; + state_machine.add_state(RequestOpen); state_machine.add_state(Opened); + add_on_enter_actions(); add_on_exit_actions(); add_transitions(); diff --git a/Core/Inc/VCU_StateMachine/VCU_DynamicLevStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_DynamicLevStateMachine.hpp index 65f6097..36736ab 100644 --- a/Core/Inc/VCU_StateMachine/VCU_DynamicLevStateMachine.hpp +++ b/Core/Inc/VCU_StateMachine/VCU_DynamicLevStateMachine.hpp @@ -18,14 +18,19 @@ namespace VCU{ CloseContactorsStateMachine close_contactors_state_machine; OpenContactorsStateMachine open_contactors_state_machine; + ReceivingDataStateMachine receiving_data_state_machine; PointTravelStateMachine point_travel_state_machine; + StackStateOrder<8, uint32_t, float> traction_data; + StackStateOrder<0> traction_end_data; + StateMachine state_machine; bool ended = false; enum DynamicLevStates{ Idle, + ReceivingData, CloseContactors, TakeOff, PointTravel, @@ -37,17 +42,27 @@ namespace VCU{ data(data), actuators(actuators), tcp_handler(tcp), outgoing_orders(outgoing_orders), encoder(encoder), close_contactors_state_machine(data, tcp_handler, outgoing_orders), open_contactors_state_machine(data, tcp_handler, outgoing_orders), - point_travel_state_machine(data, tcp_handler, outgoing_orders) - {} + receiving_data_state_machine(data, tcp_handler), + point_travel_state_machine(data, actuators, tcp_handler, outgoing_orders), + + traction_data((uint16_t)IncomingOrdersIDs::traction_data, ReceivingDataStateMachine::add_traction_point, receiving_data_state_machine.state_machine, ReceivingDataStateMachine::ReceivingDataStates::ReceivingData, &ReceivingDataStateMachine::received_data_distance, &ReceivingDataStateMachine::received_data_speed), + traction_end_data((uint16_t)IncomingOrdersIDs::traction_end_data, ReceivingDataStateMachine::last_data_packet, receiving_data_state_machine.state_machine, ReceivingDataStateMachine::ReceivingDataStates::ReceivingData) + { + init(); + } void add_transitions(){ - state_machine.add_transition(Idle, CloseContactors, [&](){ + state_machine.add_transition(Idle, ReceivingData, [&](){ if(not ended){ return true; } return false; }); + state_machine.add_transition(ReceivingData, CloseContactors, [&](){ + return receiving_data_state_machine.ended; + }); + state_machine.add_transition(CloseContactors, TakeOff, [&](){ return close_contactors_state_machine.ended; }); @@ -71,13 +86,18 @@ namespace VCU{ } void add_on_enter_actions(){ + state_machine.add_enter_action([&](){ + data.clean_traction_points(); + ReceivingDataStateMachine::clean_traction_points(); + }, Idle); + state_machine.add_enter_action([&](){ actuators.brakes.not_brake(); actuators.brakes.enable_emergency_brakes(); }, CloseContactors); state_machine.add_enter_action([&](){ - //TODO: Enviar orden de levitar + tcp_handler.send_to_lcu(outgoing_orders.take_off_order); Time::set_timeout(levitation_timeout, [&](){ if(state_machine.current_state == TakeOff && data.levitation_state != STABLE){ @@ -87,7 +107,7 @@ namespace VCU{ }, TakeOff); state_machine.add_enter_action([&](){ - //TODO: Enviar orden de landing + tcp_handler.send_to_lcu(outgoing_orders.landing_order); Time::set_timeout(landing_timeout, [&](){ if(state_machine.current_state == Landing && data.levitation_state != IDLE){ @@ -104,6 +124,19 @@ namespace VCU{ void add_on_exit_actions(){ state_machine.add_exit_action([&](){ + close_contactors_state_machine.ended = false; + }, CloseContactors); + + state_machine.add_exit_action([&](){ + receiving_data_state_machine.ended = false; + }, ReceivingData); + + state_machine.add_exit_action([&](){ + point_travel_state_machine.ended = false; + }, PointTravel); + + state_machine.add_exit_action([&](){ + open_contactors_state_machine.ended = false; ended = true; }, OpenContactors); } @@ -112,6 +145,7 @@ namespace VCU{ void init(){ state_machine = {Idle}; + state_machine.add_state(ReceivingData); state_machine.add_state(CloseContactors); state_machine.add_state(TakeOff); state_machine.add_state(PointTravel); diff --git a/Core/Inc/VCU_StateMachine/VCU_HealtCheckLoadStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_HealtCheckLoadStateMachine.hpp index 54c4a98..ac7724f 100644 --- a/Core/Inc/VCU_StateMachine/VCU_HealtCheckLoadStateMachine.hpp +++ b/Core/Inc/VCU_StateMachine/VCU_HealtCheckLoadStateMachine.hpp @@ -7,6 +7,8 @@ namespace VCU{ template<> class LoadStateMachine{ + static constexpr float crawling_speed = 3.0f; + public: Data& data; Actuators& actuators; @@ -19,8 +21,6 @@ namespace VCU{ StateMachine state_machine; - StackStateOrder<0> start_crawling; - bool ended = false; static bool crawling_requested; @@ -37,9 +37,10 @@ namespace VCU{ LoadStateMachine(Data& data, Actuators& actuators, TCP& tcp, OutgoingOrders& outgoing_orders, EncoderSensor& encoder) : data(data), actuators(actuators), tcp_handler(tcp), outgoing_orders(outgoing_orders), encoder(encoder), close_contactors_state_machine(data, tcp_handler, outgoing_orders), - open_contactors_state_machine(data, tcp_handler, outgoing_orders), - start_crawling((uint16_t)IncomingOrdersIDs::start_crawling, enter_crawling, state_machine, Pushing) - {} + open_contactors_state_machine(data, tcp_handler, outgoing_orders) + { + init(); + } static void enter_crawling(){ crawling_requested = true; @@ -87,18 +88,17 @@ namespace VCU{ }, Pushing); state_machine.add_enter_action([&](){ - //TODO: set engine parameters - //TODO: send start engine order + outgoing_orders.speed = crawling_speed; + tcp_handler.send_to_pcu(outgoing_orders.move); }, Crawling); state_machine.add_enter_action([&](){ - //Quizas hace falta darle un offset (avanzar un poco más) para evitar que entre - //en fault si se mueve ligeremante hacia atras y ha quedado demasido cerca de la emergency tape. - //TODO: send engine brake order + tcp_handler.send_to_pcu(outgoing_orders.brake); + actuators.brakes.enable_emergency_brakes(); }, Braking); state_machine.add_enter_action([&](){ - actuators.brakes.enable_emergency_brakes(); + tcp_handler.send_to_pcu(outgoing_orders.turn_off); actuators.brakes.brake(); }, OpenContactors); } @@ -114,9 +114,13 @@ namespace VCU{ }, Braking); state_machine.add_exit_action([&](){ + close_contactors_state_machine.ended = false; + }, CloseContactors); + + state_machine.add_exit_action([&](){ + open_contactors_state_machine.ended = false; ended = true; }, OpenContactors); - } void register_timed_actions(){ diff --git a/Core/Inc/VCU_StateMachine/VCU_HealtCheckUnloadStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_HealtCheckUnloadStateMachine.hpp index aca5b27..1e35f3e 100644 --- a/Core/Inc/VCU_StateMachine/VCU_HealtCheckUnloadStateMachine.hpp +++ b/Core/Inc/VCU_StateMachine/VCU_HealtCheckUnloadStateMachine.hpp @@ -37,7 +37,9 @@ namespace VCU{ data(data), actuators(actuators), tcp_handler(tcp), outgoing_orders(outgoing_orders), encoder(encoder), close_contactors_state_machine(data, tcp_handler, outgoing_orders), open_contactors_state_machine(data, tcp_handler, outgoing_orders) - {} + { + init(); + } void add_transitions(){ //Quitar los frenos debe hacerse manualmente con una orden una vez finalizado el procedure diff --git a/Core/Inc/VCU_StateMachine/VCU_PointTravelStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_PointTravelStateMachine.hpp index 7272c42..8001707 100644 --- a/Core/Inc/VCU_StateMachine/VCU_PointTravelStateMachine.hpp +++ b/Core/Inc/VCU_StateMachine/VCU_PointTravelStateMachine.hpp @@ -7,86 +7,143 @@ namespace VCU{ template<> class PointTravelStateMachine{ - static constexpr uint32_t initial_aux_state = 0; + static constexpr float min_speed = 1.0f; public: Data& data; + Actuators actuators; TCP& tcp_handler; OutgoingOrders& outgoing_orders; StateMachine state_machine; enum TravelStates{ - ReceivingData, + Idle, Calculating, MovingForward, MovingBackward, Braking, - End, + End }; bool ended = false; - uint32_t aux_last_state; + point_t initial_point; + point_t* calculated_point = nullptr; + DIRECTION calculated_direction = DIRECTION::FORWARD; + bool calculating = true; - PointTravelStateMachine(Data& data, TCP& tcp, OutgoingOrders& outgoing_orders) : - data(data),tcp_handler(tcp), outgoing_orders(outgoing_orders) - {} + PointTravelStateMachine(Data& data, Actuators actuators, TCP& tcp, OutgoingOrders& outgoing_orders) : + data(data), actuators(actuators), tcp_handler(tcp), outgoing_orders(outgoing_orders) + { + init(); + } void add_transitions(){ + state_machine.add_transition(Idle, Calculating, [&](){ + return not ended; + }); - uint32_t next_state; - for(uint32_t current_state = initial_aux_state; current_state < data.traction_points.size() + initial_aux_state; current_state++){ + state_machine.add_transition(Calculating, MovingForward, [&](){ + return not calculating && calculated_direction == DIRECTION::FORWARD; + }); - if (current_state + 1 > aux_last_state) { - next_state = (uint32_t)End; - }else{ - next_state = ++current_state; + state_machine.add_transition(Calculating, MovingBackward, [&](){ + return not calculating && calculated_direction == DIRECTION::BACKWARD; + }); + + state_machine.add_transition(MovingForward, Calculating, [&](){ + if (data.tapes_position >= calculated_point->position - data.change_direction_distance_lookup_table[(uint32_t)ceil(data.engine_speed)]) { + return true; } - state_machine.add_transition(current_state, next_state, [&](){ - uint32_t speed = std::ceil(std::max(data.target_speed, data.engine_speed)); - uint32_t brake_distance = data.brake_distance_lookup_table[speed]; - uint32_t objective_position = data.traction_points.at(current_state - initial_aux_state); - if (data.tapes_position > objective_position) { - return data.tapes_position <= (objective_position + brake_distance); - }else{ - return data.tapes_position >= (objective_position - brake_distance); - } - }); - } + return false; + }); + + state_machine.add_transition(MovingBackward, Calculating, [&](){ + if (data.tapes_position <= calculated_point->position + data.change_direction_distance_lookup_table[(uint32_t)ceil(data.engine_speed)]) { + return true; + } + + return false; + }); + + state_machine.add_transition(Braking, End, [&](){ + return data.engine_speed <= min_speed; + }); } void add_on_enter_actions(){ - for(uint32_t current_state = initial_aux_state; current_state < data.traction_points.size() + initial_aux_state; current_state++){ - - state_machine.add_enter_action([&](){ - uint32_t objective_position = data.traction_points.at(current_state - initial_aux_state); - if (data.tapes_position > objective_position) { - //TODO: send order to MOVE BACKWARDS - }else{ - //TODO: send order to MOVE FORWARDS - } - }, current_state); - } + state_machine.add_enter_action([&](){ + initial_point = point_t(data.tapes_position, 0.0f); + }, Idle); + + state_machine.add_enter_action([&](){ + calculating = true; + if(not data.get_next_point(calculated_point)){ + + state_machine.force_change_state(Braking); + return; + } + + if (calculated_point->position > data.tapes_position ) { + calculated_direction = DIRECTION::FORWARD; + }else{ + calculated_direction = DIRECTION::BACKWARD; + } + + calculating = false; + + }, Calculating); state_machine.add_enter_action([&](){ + calculating = false; + + outgoing_orders.speed = calculated_point->speed; + tcp_handler.send_to_pcu(outgoing_orders.move); + + }, MovingForward); + + state_machine.add_enter_action([&](){ + calculating = false; + + outgoing_orders.speed = calculated_point->speed; + tcp_handler.send_to_pcu(outgoing_orders.move); + + }, MovingBackward); + + state_machine.add_enter_action([&](){ + tcp_handler.send_to_pcu(outgoing_orders.brake); + }, Braking); + + state_machine.add_enter_action([&](){ + tcp_handler.send_to_pcu(outgoing_orders.turn_off); ended = true; }, End); } - void add_on_exit_actions(){} - - void register_timed_actions(){} + void add_on_exit_actions(){ + state_machine.add_exit_action([&](){ + actuators.brakes.not_brake(); + }, Braking); + } - void init(){ + void register_timed_actions(){ + state_machine.add_low_precision_cyclic_action([&](){ + if (data.tapes_position <= initial_point.position && data.engine_speed != 0.0f) { + actuators.brakes.brake(); + } + }, (ms)1, Braking); - state_machine = initial_aux_state; + } - for(uint32_t i = initial_aux_state + 1; i < (data.traction_points.size() + initial_aux_state); i++){ - state_machine.add_state(i); - aux_last_state = i; - } + void init(){ + state_machine = {Idle}; + state_machine.add_state(Calculating); + state_machine.add_state(MovingForward); + state_machine.add_state(MovingBackward); + state_machine.add_state(Braking); + state_machine.add_state(End); add_on_enter_actions(); add_on_exit_actions(); diff --git a/Core/Inc/VCU_StateMachine/VCU_ReceivingDataStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_ReceivingDataStateMachine.hpp new file mode 100644 index 0000000..8d32a41 --- /dev/null +++ b/Core/Inc/VCU_StateMachine/VCU_ReceivingDataStateMachine.hpp @@ -0,0 +1,88 @@ +#pragma once + +#include "VCU_Utilities/VCU_Includes.hpp" + +namespace VCU{ + template class ReceivingDataStateMachine; + + template<> + class ReceivingDataStateMachine{ + public: + Data& data; + TCP& tcp_handler; + StateMachine state_machine; + + bool ended = false; + + static vector traction_points; + static bool last_data_packet_requested; + static uint32_t received_data_distance; + static float received_data_speed; + + enum ReceivingDataStates{ + Idle, + ReceivingData, + End, + }; + + static void add_traction_point(){ + traction_points.push_back(point_t(received_data_distance, received_data_speed)); + } + + static void clean_traction_points(){ + traction_points.clear(); + } + + static void last_data_packet(){ + last_data_packet_requested = true; + } + + ReceivingDataStateMachine(Data& data, TCP& tcp) : + data(data), tcp_handler(tcp) + { + init(); + } + + void add_transitions(){ + state_machine.add_transition(Idle, ReceivingData, [&](){ + return not ended; + }); + + state_machine.add_transition(ReceivingData, End, [&](){ + return last_data_packet_requested; + }); + + } + + void add_on_enter_actions(){ + state_machine.add_enter_action([&](){ + data.traction_points = traction_points; + last_data_packet_requested = false; + }, End); + + } + + void add_on_exit_actions(){} + + void register_timed_actions(){ + } + + void init(){ + state_machine = {Idle}; + state_machine.add_state(ReceivingData); + state_machine.add_state(End); + + add_on_enter_actions(); + add_on_exit_actions(); + add_transitions(); + register_timed_actions(); + add_transitions(); + } + + }; + + vector ReceivingDataStateMachine::traction_points = {}; + bool ReceivingDataStateMachine::last_data_packet_requested = false; +} + + diff --git a/Core/Inc/VCU_StateMachine/VCU_SpecificStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_SpecificStateMachine.hpp index e064451..bdaf216 100644 --- a/Core/Inc/VCU_StateMachine/VCU_SpecificStateMachine.hpp +++ b/Core/Inc/VCU_StateMachine/VCU_SpecificStateMachine.hpp @@ -20,11 +20,9 @@ namespace VCU{ StaticLevStateMachine static_lev_state_machine; DynamicLevStateMachine dynamic_lev_state_machine; - StackStateOrder<0> healthcheck_and_load; - StackStateOrder<0> healthcheck_and_unload; - StackStateOrder<0> start_static_lev; - StackStateOrder<0, vector> start_dynamic_lev; - StackStateOrder<0, vector> start_traction; + StackStateOrder<0> start_crawling; + StackStateOrder<0> take_off; + StackStateOrder<0> landing; StateMachine state_machine; @@ -52,13 +50,13 @@ namespace VCU{ traction_state_machine(data, actuators, tcp, outgoing_orders, encoder), static_lev_state_machine(data, actuators, tcp, outgoing_orders, encoder), dynamic_lev_state_machine(data, actuators, tcp, outgoing_orders, encoder), + start_crawling((uint16_t)IncomingOrdersIDs::start_crawling, LoadStateMachine::enter_crawling, health_load_state_machine.state_machine, LoadStateMachine::UnloadStates::Pushing), + take_off((uint16_t)IncomingOrdersIDs::take_off, StaticLevStateMachine::start_levitation, static_lev_state_machine.state_machine, StaticLevStateMachine::StaticLevStates::LevOff), + landing((uint16_t)IncomingOrdersIDs::landing, StaticLevStateMachine::stop_levitation, static_lev_state_machine.state_machine, StaticLevStateMachine::StaticLevStates::LevOn) - healthcheck_and_load((uint16_t)IncomingOrdersIDs::heakthcheck_and_load, enter_health_and_load, state_machine, Idle), - healthcheck_and_unload((uint16_t)IncomingOrdersIDs::healthcheck_and_unload, enter_health_and_unload, state_machine, Idle), - start_static_lev((uint16_t)IncomingOrdersIDs::start_static_lev_demostration, enter_static_lev, state_machine, Idle), - start_dynamic_lev((uint16_t)IncomingOrdersIDs::start_dynamic_lev_demostration, enter_dynamic_lev, state_machine, Idle, &data.traction_points), - start_traction((uint16_t)IncomingOrdersIDs::start_traction_demostration, enter_traction, state_machine, Idle, &data.traction_points) - {} + { + init(); + } static void enter_health_and_load(){ healthcheck_and_load_requested = true; @@ -124,12 +122,6 @@ namespace VCU{ void add_on_enter_actions(){ state_machine.add_enter_action([&](){ - health_load_state_machine.ended = false; - health_unload_state_machine.ended = false; - dynamic_lev_state_machine.ended = false; - static_lev_state_machine.ended = false; - traction_state_machine.ended = false; - healthcheck_and_load_requested = false; healthcheck_and_unload_requested = false; start_static_lev_requested = false; @@ -138,7 +130,27 @@ namespace VCU{ }, Idle); } - void add_on_exit_actions(){} + void add_on_exit_actions(){ + state_machine.add_exit_action([&](){ + health_unload_state_machine.ended = false; + }, Unload); + + state_machine.add_exit_action([&](){ + health_load_state_machine.ended = false; + }, Load); + + state_machine.add_exit_action([&](){ + traction_state_machine.ended = false; + }, Traction); + + state_machine.add_exit_action([&](){ + static_lev_state_machine.ended = false; + }, StaticLev); + + state_machine.add_exit_action([&](){ + dynamic_lev_state_machine.ended = false; + }, DynamicLev); + } void register_timed_actions(){} diff --git a/Core/Inc/VCU_StateMachine/VCU_StateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_StateMachine.hpp index 761287b..b7548fd 100644 --- a/Core/Inc/VCU_StateMachine/VCU_StateMachine.hpp +++ b/Core/Inc/VCU_StateMachine/VCU_StateMachine.hpp @@ -15,7 +15,9 @@ namespace VCU{ static constexpr uint16_t max_tcp_connection_timeout = 25000; //ms GeneralStateMachine(Data& data, Actuators& actuators , TCP& tcp_handler) : actuators(actuators), tcp_handler(tcp_handler) - {} + { + init(); + } enum States{ INITIAL, @@ -104,9 +106,15 @@ namespace VCU{ SpecificStateMachine specific_state_machine; StateMachine general_state_machine; + StackStateOrder<0> healthcheck_and_load; + StackStateOrder<0> healthcheck_and_unload; + StackStateOrder<0> start_static_lev; + StackStateOrder<0> start_dynamic_lev; + StackStateOrder<0> start_traction; + bool tcp_timeout = false; - static constexpr uint16_t max_tcp_connection_timeout = 30000; //ms + static constexpr uint16_t max_tcp_connection_timeout = 65000; //ms //TODO: arreglar enum States{ INITIAL, @@ -115,13 +123,23 @@ namespace VCU{ }; GeneralStateMachine(Data& data, Actuators& actuators, TCP& tcp, OutgoingOrders& outgoing_orders, EncoderSensor& encoder) : - data(data), actuators(actuators), tcp_handler(tcp), outgoing_orders(outgoing_orders), encoder(encoder), specific_state_machine(data, actuators, tcp, outgoing_orders, encoder) - {} + data(data), actuators(actuators), tcp_handler(tcp), outgoing_orders(outgoing_orders), encoder(encoder), + specific_state_machine(data, actuators, tcp, outgoing_orders, encoder), + + healthcheck_and_load((uint16_t)IncomingOrdersIDs::heakthcheck_and_load, SpecificStateMachine::enter_health_and_load, specific_state_machine.state_machine, SpecificStateMachine::SpecificStates::Idle), + healthcheck_and_unload((uint16_t)IncomingOrdersIDs::healthcheck_and_unload, SpecificStateMachine::enter_health_and_unload, specific_state_machine.state_machine, SpecificStateMachine::SpecificStates::Idle), + start_static_lev((uint16_t)IncomingOrdersIDs::start_static_lev_demostration, SpecificStateMachine::enter_static_lev, specific_state_machine.state_machine, SpecificStateMachine::SpecificStates::Idle), + start_dynamic_lev((uint16_t)IncomingOrdersIDs::start_dynamic_lev_demostration, SpecificStateMachine::enter_dynamic_lev, specific_state_machine.state_machine, SpecificStateMachine::SpecificStates::Idle), + start_traction((uint16_t)IncomingOrdersIDs::start_traction_demostration, SpecificStateMachine::enter_traction, specific_state_machine.state_machine, SpecificStateMachine::SpecificStates::Idle) + { + init(); + } void add_transitions(){ //todo: COMPROBAR que estan todos conectados antes de pasar a operational general_state_machine.add_transition(INITIAL, OPERATIONAL, [&](){ - return tcp_handler.check_connections(); + bool res = tcp_handler.check_connections(); + return res; }); general_state_machine.add_transition(INITIAL, FAULT, [&](){ if(tcp_timeout){ @@ -171,7 +189,7 @@ namespace VCU{ actuators.brakes.not_brake(); }, FAULT); general_state_machine.add_exit_action([&](){ - actuators.led_operational.turn_on(); + actuators.led_operational.turn_off(); }, OPERATIONAL); general_state_machine.add_exit_action([&](){ actuators.led_operational.turn_off(); diff --git a/Core/Inc/VCU_StateMachine/VCU_StaticLevStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_StaticLevStateMachine.hpp index 9c27c31..c3f3f3b 100644 --- a/Core/Inc/VCU_StateMachine/VCU_StaticLevStateMachine.hpp +++ b/Core/Inc/VCU_StateMachine/VCU_StaticLevStateMachine.hpp @@ -19,15 +19,13 @@ namespace VCU{ StateMachine state_machine; - StackStateOrder<0> take_off; - StackStateOrder<0> landing; - bool ended = false; static bool start_levitation_requested; static bool stop_levitation_requested; - enum DynamicLevStates{ + enum StaticLevStates{ + Idle, CloseContactors, LevOff, LevOn, @@ -37,10 +35,10 @@ namespace VCU{ StaticLevStateMachine(Data& data, Actuators& actuators, TCP& tcp, OutgoingOrders& outgoing_orders, EncoderSensor& encoder) : data(data), actuators(actuators), tcp_handler(tcp), outgoing_orders(outgoing_orders), encoder(encoder), close_contactors_state_machine(data, tcp_handler, outgoing_orders), - open_contactors_state_machine(data, tcp_handler, outgoing_orders), - take_off((uint16_t)IncomingOrdersIDs::take_off, start_levitation, state_machine, LevOff), - landing((uint16_t)IncomingOrdersIDs::landing, stop_levitation, state_machine, LevOn) - {} + open_contactors_state_machine(data, tcp_handler, outgoing_orders) + { + init(); + } static void start_levitation(){ start_levitation_requested = true; @@ -51,12 +49,19 @@ namespace VCU{ } void add_transitions(){ + state_machine.add_transition(Idle, CloseContactors, [&](){ + if(not ended){ + return true; + } + return false; + }); + state_machine.add_transition(CloseContactors, LevOff, [&](){ return close_contactors_state_machine.ended; }); state_machine.add_transition(LevOff, LevOn, [&](){ - if(start_levitation_requested && not ended){ + if(start_levitation_requested){ start_levitation_requested = false; return true; } @@ -71,7 +76,7 @@ namespace VCU{ return false; }); - state_machine.add_transition(OpenContactors, LevOff, [&](){ + state_machine.add_transition(OpenContactors, Idle, [&](){ return open_contactors_state_machine.ended; }); } @@ -99,7 +104,8 @@ namespace VCU{ void register_timed_actions(){} void init(){ - state_machine = {CloseContactors}; + state_machine = {Idle}; + state_machine.add_state(CloseContactors); state_machine.add_state(LevOff); state_machine.add_state(LevOn); state_machine.add_state(OpenContactors); diff --git a/Core/Inc/VCU_StateMachine/VCU_TractionStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_TractionStateMachine.hpp index 4a93bbd..cc3db1f 100644 --- a/Core/Inc/VCU_StateMachine/VCU_TractionStateMachine.hpp +++ b/Core/Inc/VCU_StateMachine/VCU_TractionStateMachine.hpp @@ -16,36 +16,49 @@ namespace VCU{ CloseContactorsStateMachine close_contactors_state_machine; OpenContactorsStateMachine open_contactors_state_machine; + ReceivingDataStateMachine receiving_data_state_machine; PointTravelStateMachine point_travel_state_machine; + StackStateOrder<8, uint32_t, float> traction_data; + StackStateOrder<0> traction_end_data; + StateMachine state_machine; bool ended = false; - enum DynamicLevStates{ + enum TractionStates{ Idle, + ReceivingData, CloseContactors, PointTravel, OpenContactors, }; - uint32_t aux_last_state; - TractionStateMachine(Data& data, Actuators&actuators, TCP& tcp, OutgoingOrders& outgoing_orders, EncoderSensor& encoder) : data(data),actuators(actuators),tcp_handler(tcp), outgoing_orders(outgoing_orders), encoder(encoder), close_contactors_state_machine(data, tcp_handler, outgoing_orders), open_contactors_state_machine(data, tcp_handler, outgoing_orders), - point_travel_state_machine(data, tcp_handler, outgoing_orders) - {} + receiving_data_state_machine(data, tcp_handler), + point_travel_state_machine(data, actuators, tcp_handler, outgoing_orders), + + traction_data((uint16_t)IncomingOrdersIDs::traction_data, ReceivingDataStateMachine::add_traction_point, receiving_data_state_machine.state_machine, ReceivingDataStateMachine::ReceivingDataStates::ReceivingData, &ReceivingDataStateMachine::received_data_distance, &ReceivingDataStateMachine::received_data_speed), + traction_end_data((uint16_t)IncomingOrdersIDs::traction_end_data, ReceivingDataStateMachine::last_data_packet, receiving_data_state_machine.state_machine, ReceivingDataStateMachine::ReceivingDataStates::ReceivingData) + { + init(); + } void add_transitions(){ - state_machine.add_transition(Idle, CloseContactors, [&](){ + state_machine.add_transition(Idle, ReceivingData, [&](){ if(not ended){ return true; } return false; }); + state_machine.add_transition(ReceivingData, CloseContactors, [&](){ + return receiving_data_state_machine.ended; + }); + state_machine.add_transition(CloseContactors, PointTravel, [&](){ return close_contactors_state_machine.ended; }); @@ -60,6 +73,11 @@ namespace VCU{ } void add_on_enter_actions(){ + state_machine.add_enter_action([&](){ + data.clean_traction_points(); + ReceivingDataStateMachine::clean_traction_points(); + }, Idle); + state_machine.add_enter_action([&](){ actuators.brakes.not_brake(); actuators.brakes.enable_emergency_brakes(); @@ -73,6 +91,7 @@ namespace VCU{ } void add_on_exit_actions(){ + state_machine.add_exit_action([&](){ ended = true; }, OpenContactors); @@ -82,12 +101,14 @@ namespace VCU{ void init(){ state_machine = {Idle}; + state_machine.add_state(ReceivingData); state_machine.add_state(CloseContactors); state_machine.add_state(PointTravel); state_machine.add_state(OpenContactors); state_machine.add_state_machine(close_contactors_state_machine.state_machine, CloseContactors); state_machine.add_state_machine(open_contactors_state_machine.state_machine, OpenContactors); + state_machine.add_state_machine(receiving_data_state_machine.state_machine, ReceivingData); state_machine.add_state_machine(point_travel_state_machine.state_machine, PointTravel); add_on_enter_actions(); diff --git a/Core/Inc/VCU_Utilities/VCU_Includes.hpp b/Core/Inc/VCU_Utilities/VCU_Includes.hpp index 4a98be9..4c4c9ed 100644 --- a/Core/Inc/VCU_Utilities/VCU_Includes.hpp +++ b/Core/Inc/VCU_Utilities/VCU_Includes.hpp @@ -21,6 +21,7 @@ #include "VCU_Communications/VCU_UDP/Packets.hpp" #include "VCU_StateMachine/VCU_ContactorsStateMachine.hpp" +#include "VCU_StateMachine/VCU_ReceivingDataStateMachine.hpp" #include "VCU_StateMachine/VCU_PointTravelStateMachine.hpp" #include "VCU_StateMachine/VCU_HealtCheckLoadStateMachine.hpp" #include "VCU_StateMachine/VCU_HealtCheckUnloadStateMachine.hpp" diff --git a/Core/Inc/VCU_Utilities/VCU_Types.hpp b/Core/Inc/VCU_Utilities/VCU_Types.hpp index 6a96d08..4878982 100644 --- a/Core/Inc/VCU_Utilities/VCU_Types.hpp +++ b/Core/Inc/VCU_Utilities/VCU_Types.hpp @@ -31,5 +31,28 @@ namespace VCU{ LANDING, }; + struct point_t{ + uint32_t position; + float speed; + + point_t(){ + position = 0; + speed = 0.0f; + } + + point_t(uint32_t position, float speed){ + this->position = position; + this->speed = speed; + } + + point_t& operator=(const point_t& other){ + this->position = other.position; + this->speed = other.speed; + + return *this; + } + }; + + } diff --git a/Core/Src/Runes/Runes.hpp b/Core/Src/Runes/Runes.hpp index ec60000..0a9d2bb 100644 --- a/Core/Src/Runes/Runes.hpp +++ b/Core/Src/Runes/Runes.hpp @@ -6,7 +6,6 @@ DMA_HandleTypeDef hdma_adc2; DMA_HandleTypeDef hdma_adc3; DMA_HandleTypeDef hdma_i2c2_rx; DMA_HandleTypeDef hdma_i2c2_tx; -I2C_HandleTypeDef hi2c2; ADC_HandleTypeDef hadc1; ADC_HandleTypeDef hadc2; ADC_HandleTypeDef hadc3; @@ -158,6 +157,9 @@ vector> TimerPeripheral::timers = { PWMmap TimerPeripheral::available_pwm = { {PB9, {timer17, {TIM_CHANNEL_1, NORMAL}}}, + {PF1, {timer23, {TIM_CHANNEL_2, NORMAL}}}, + {PF2, {timer23, {TIM_CHANNEL_3, NORMAL}}}, + {PF3, {timer23, {TIM_CHANNEL_4, NORMAL}}} }; DualPWMmap TimerPeripheral::available_dual_pwms = { @@ -270,13 +272,12 @@ map ExternalInterrupt::instances = { ***********************************************/ #ifdef HAL_I2C_MODULE_ENABLED -extern I2C_HandleTypeDef hi2c2; -I2C::Instance I2C::instance2 = { .SCL = PF1, .SDA = PB11, .hi2c = &hi2c2, .instance = I2C2, .RX_DMA = DMA::Stream::DMA1Stream3, .TX_DMA = DMA::Stream::DMA1Stream4}; -I2C::Peripheral I2C::i2c2 = I2C::Peripheral::peripheral2; +//extern I2C_HandleTypeDef hi2c2; + unordered_map I2C::available_i2cs = { - {I2C::i2c2, &I2C::instance2} + }; unordered_map I2C::available_speed_frequencies = { - {100, 0x60404E72} + }; #endif diff --git a/Core/Src/main.cpp b/Core/Src/main.cpp index 65e56d7..b2b3418 100644 --- a/Core/Src/main.cpp +++ b/Core/Src/main.cpp @@ -30,12 +30,77 @@ void a_op_func(){ global->force_change_state(op); } +//void state_machine_test(){ +// DigitalOutput flash( VCU::Pinout::FLASH_LED); +// DigitalOutput fault( VCU::Pinout::FAULT_LED); +// +// STLIB::start(); +// +// flash.turn_off(); +// +// StateMachine m2 = 0; +// m2.add_state(1); +// +// m2.add_enter_action([&](){ +// flash.turn_on(); +// },0); +// +// m2.add_enter_action([&](){ +// fault.turn_on(); +// },1); +// +// m2.add_transition(0, 1, [&](){ +// return true; +// }); +// +// StateMachine m1 = 0; +// m1.add_state(1); +// +// m1.add_state_machine(m2, 1); +// +// m1.add_transition(0, 1, [&](){ +// return true; +// }); +// +// m1.check_transitions(); +// while(1){ +// STLIB::update(); +// __NOP(); +// } +// +//} + +//void emulate_pcu(){ +// STLIB::start(); +// +// ServerSocket socket(VCU::PCU_IP, VCU::SERVER_PORT); +// +// +// while(1){ +// STLIB::update(); +// } +//} +// +//void emulate_bmsl(){ +// STLIB::start(); +// +// ServerSocket socket(VCU::BMSL_IP, VCU::SERVER_PORT); +// +// +// while(1){ +// STLIB::update(); +// } +//} + + int main(void){ -//{ -// VCU::VCU_CLASS vcu; -// VCU::VCU_CLASS::vcu = &vcu; +// state_machine_test(); + + +// VCU::VCU_CLASS vcu; +// VCU::VCU_CLASS::vcu = &vcu; // vcu.init(); -// VCU::CyclicActions::register_cyclic_actions(); +// VCU::CyclicActions::register_cyclic_actions(); est current_state = ini; STLIB::start(); @@ -90,7 +155,7 @@ int main(void){ Time::register_low_precision_alarm(10, [&](){ current_state = (est)machine.current_state; - //udp.send(data_pack); + udp.send(data_pack); }); diff --git a/Middlewares/Third_Party/LwIP/src/include/lwip/opt.h b/Middlewares/Third_Party/LwIP/src/include/lwip/opt.h index bb6c33d..b193948 100644 --- a/Middlewares/Third_Party/LwIP/src/include/lwip/opt.h +++ b/Middlewares/Third_Party/LwIP/src/include/lwip/opt.h @@ -416,7 +416,7 @@ * (requires the LWIP_RAW option) */ #if !defined MEMP_NUM_RAW_PCB || defined __DOXYGEN__ -#define MEMP_NUM_RAW_PCB 4 +#define MEMP_NUM_RAW_PCB 10 #endif /** @@ -425,7 +425,7 @@ * (requires the LWIP_UDP option) */ #if !defined MEMP_NUM_UDP_PCB || defined __DOXYGEN__ -#define MEMP_NUM_UDP_PCB 4 +#define MEMP_NUM_UDP_PCB 5 #endif /** diff --git a/VCU Debug.launch b/VCU Debug.launch index edb1791..8095026 100644 --- a/VCU Debug.launch +++ b/VCU Debug.launch @@ -35,7 +35,7 @@ - + From 334e0a52b6c47bbb89724256a10504e4ada58931 Mon Sep 17 00:00:00 2001 From: Pablo Date: Thu, 29 Jun 2023 16:27:48 +0200 Subject: [PATCH 09/10] checkpoint, toda la lista TODO hecha --- Core/Inc/VCU.hpp | 11 ++- Core/Inc/VCU_Brakes/VCU_Brakes.hpp | 22 +++-- .../VCU_TCP/IncomingOrders.hpp | 19 +++-- .../VCU_TCP/OutgoingOrders.hpp | 5 +- .../VCU_Communications/VCU_UDP/Packets.hpp | 38 ++++++--- .../VCU_Communications/VCU_UDP/VCU_UDP.hpp | 4 +- Core/Inc/VCU_Data/VCU_Data.hpp | 22 +++-- Core/Inc/VCU_Protections/VCU_Protections.hpp | 30 +++++++ .../VCU_DynamicLevStateMachine.hpp | 4 +- .../VCU_FaultSpecificStateMachine.hpp | 82 +++++++++++++++++++ .../VCU_HealtCheckLoadStateMachine.hpp | 14 ++-- .../VCU_HealtCheckUnloadStateMachine.hpp | 35 +++++--- .../VCU_PointTravelStateMachine.hpp | 16 ++-- .../VCU_SpecificStateMachine.hpp | 2 + .../Inc/VCU_StateMachine/VCU_StateMachine.hpp | 21 +++-- .../VCU_StaticLevStateMachine.hpp | 16 +++- .../VCU_TractionStateMachine.hpp | 14 ++++ Core/Inc/VCU_Time/VCU_Time.hpp | 1 + Core/Inc/VCU_Utilities/VCU_Includes.hpp | 3 +- Core/Inc/VCU_Utilities/VCU_Types.hpp | 6 +- 20 files changed, 287 insertions(+), 78 deletions(-) create mode 100644 Core/Inc/VCU_StateMachine/VCU_FaultSpecificStateMachine.hpp diff --git a/Core/Inc/VCU.hpp b/Core/Inc/VCU.hpp index da69e06..ce91471 100644 --- a/Core/Inc/VCU.hpp +++ b/Core/Inc/VCU.hpp @@ -86,8 +86,8 @@ namespace VCU{ vcu->udp_handler.send_to_backend(vcu->packets.regulator_packet); vcu->udp_handler.send_to_backend(vcu->packets.pressure_packets); vcu->udp_handler.send_to_backend(vcu->packets.bottle_temperature_packet); - vcu->udp_handler.send_to_backend(vcu->packets.reed_packet); vcu->udp_handler.send_to_backend(vcu->packets.environmental_packet); + vcu->udp_handler.send_to_backend(vcu->packets.states_packet); } static void update_state_machine(){ @@ -95,9 +95,14 @@ namespace VCU{ } }; - //Esto hay que modificar Incoming orders para que pueda estar dentro de la clase VCU, aqui fuera no tiene sentido void set_regulator_pressure(){ - VCU_CLASS::vcu->actuators.brakes.set_regulator_pressure(VCU_CLASS::vcu->incoming_orders.new_pressure); + float n_pressure = VCU_CLASS::vcu->incoming_orders.new_pressure; + if( n_pressure < 0 || n_pressure > 10 ){ + ProtectionManager::warn("The new value for the regulator is out of range!"); + return; + } + + VCU_CLASS::vcu->actuators.brakes.set_regulator_pressure(n_pressure); } void brake(){ diff --git a/Core/Inc/VCU_Brakes/VCU_Brakes.hpp b/Core/Inc/VCU_Brakes/VCU_Brakes.hpp index b235b02..3ba9e26 100644 --- a/Core/Inc/VCU_Brakes/VCU_Brakes.hpp +++ b/Core/Inc/VCU_Brakes/VCU_Brakes.hpp @@ -159,7 +159,13 @@ namespace VCU{ regulator_actuator(Pinout::REGULATOR_OUT, data.regulator_reference_pressure), regulator_sensor(Pinout::REGULATOR_IN, data.regulator_real_pressure), - emergency_tape(Pinout::EMERGENCY_TAPE, [&](){emergency_tape.read();}, data.emergency_tape), + emergency_tape(Pinout::EMERGENCY_TAPE, [&](){ + emergency_tape.read(); + //INOF: emergency tape enable a nivel bajo + if (data.emergeny_tape_enable == PinState::OFF) { + data.emergency_braking = true; + } + }, data.emergency_tape), emergency_tape_enable(Pinout::EMERGENCY_TAPE_ENABLE), reed1(Pinout::REED1, &data.reed1), @@ -191,17 +197,17 @@ namespace VCU{ void brake(){ valve_actuator.close(); - Time::set_timeout(1, [&](){ - check_reeds(); - }); +// Time::set_timeout(1, [&](){ +// check_reeds(); +// }); } void not_brake(){ valve_actuator.open(); - - Time::set_timeout(1, [&](){ - check_reeds(); - }); + data.emergency_braking = false; +// Time::set_timeout(1, [&](){ +// check_reeds(); +// }); } void disable_emergency_brakes(){ diff --git a/Core/Inc/VCU_Communications/VCU_TCP/IncomingOrders.hpp b/Core/Inc/VCU_Communications/VCU_TCP/IncomingOrders.hpp index da454c9..7bdb539 100644 --- a/Core/Inc/VCU_Communications/VCU_TCP/IncomingOrders.hpp +++ b/Core/Inc/VCU_Communications/VCU_TCP/IncomingOrders.hpp @@ -13,12 +13,20 @@ namespace VCU{ enum class IncomingOrdersIDs: uint16_t{ + hardware_reset_order = 209, + set_regulator_pressure_order = 210, + brake_order = 216, + unbrake_order = 217, + disable_emergency_tape_order = 218, + enable_emergency_tape_order = 219, + heakthcheck_and_load = 220, healthcheck_and_unload = 221, start_static_lev_demostration = 222, start_dynamic_lev_demostration = 223, start_traction_demostration = 224, stop_demostration = 225, + take_off = 226, landing = 227, start_crawling = 231, @@ -60,7 +68,6 @@ namespace VCU{ template<> class IncomingOrders{ - //TODO: Hacer todas las ordenes para el vehicle y hacer las states StackOrder<0> hardware_reset_order; StackOrder<4,float> set_regulator_pressure_order; StackOrder<0> brake_order; @@ -72,10 +79,12 @@ namespace VCU{ float new_pressure = 0; IncomingOrders(Data& data) : - hardware_reset_order(209, hardware_reset), - set_regulator_pressure_order(210, set_regulator_pressure, &new_pressure), - brake_order(215, brake), unbrake_order(216, unbrake), - disable_emergency_tape_order(217, disable_emergency_tape), enable_emergency_tape_order(218,enable_emergency_tape) + hardware_reset_order((uint16_t)IncomingOrdersIDs::hardware_reset_order, hardware_reset), + set_regulator_pressure_order((uint16_t)IncomingOrdersIDs::set_regulator_pressure_order, set_regulator_pressure, &new_pressure), + brake_order((uint16_t)IncomingOrdersIDs::brake_order, brake), + unbrake_order((uint16_t)IncomingOrdersIDs::unbrake_order, unbrake), + disable_emergency_tape_order((uint16_t)IncomingOrdersIDs::disable_emergency_tape_order, disable_emergency_tape), + enable_emergency_tape_order((uint16_t)IncomingOrdersIDs::enable_emergency_tape_order,enable_emergency_tape) {} }; } diff --git a/Core/Inc/VCU_Communications/VCU_TCP/OutgoingOrders.hpp b/Core/Inc/VCU_Communications/VCU_TCP/OutgoingOrders.hpp index 2a8af87..e151b1f 100644 --- a/Core/Inc/VCU_Communications/VCU_TCP/OutgoingOrders.hpp +++ b/Core/Inc/VCU_Communications/VCU_TCP/OutgoingOrders.hpp @@ -35,7 +35,7 @@ namespace VCU{ StackOrder<0> take_off_order; StackOrder<0> landing_order; - StackOrder<4, float> move; + StackOrder<4, float, DIRECTION> move; StackOrder<0> brake; StackOrder<0> turn_off; @@ -43,11 +43,12 @@ namespace VCU{ StackOrder<0> open_contactors; float speed = 0.0f; + DIRECTION direction = DIRECTION::FORWARD; OutgoingOrders() : take_off_order((uint16_t)LevitationOrdes::TAKE_OFF), landing_order((uint16_t)LevitationOrdes::LANDING), - move((uint16_t)TractionOrders::MOVE, &speed), + move((uint16_t)TractionOrders::MOVE, &speed, &direction), brake((uint16_t)TractionOrders::BRAKE), turn_off((uint16_t)TractionOrders::TURN_OFF), close_contactors((uint16_t)BatteryOrders::CLOSE_CONTACTORS), diff --git a/Core/Inc/VCU_Communications/VCU_UDP/Packets.hpp b/Core/Inc/VCU_Communications/VCU_UDP/Packets.hpp index 2a7e393..d3d2a8b 100644 --- a/Core/Inc/VCU_Communications/VCU_UDP/Packets.hpp +++ b/Core/Inc/VCU_Communications/VCU_UDP/Packets.hpp @@ -31,23 +31,41 @@ namespace VCU{ template<> class Packets{ public: - //TODO: Hay que revisar todos los paquetes - //Outgoing packets + enum class PacketsIDs: uint16_t{ + vcu_regulator_packet = 211, + vcu_bottle_temperature_packet = 213, + vcu_pressure_packet = 214, + vcu_environmental_packet = 215, + states_packet = 232, + + lcu_master_state_machine_packet = 306, + + accelerations = 606, + }; + + uint8_t trash_u8 = 0; + float trash_f = 0.0f; + + //INFO: Outgoing packets StackPacket<9,VALVE_STATE, float, float> regulator_packet; - StackPacket<5,REED_STATE, REED_STATE, REED_STATE, REED_STATE, bool> reed_packet; StackPacket<16,double,double> bottle_temperature_packet; StackPacket<12,float,float,float> pressure_packets; StackPacket<8, float,float> environmental_packet; + StackPacket<7, uint8_t, uint8_t, uint8_t, uint8_t, uint8_t, uint8_t, uint8_t> states_packet; - //Incoming packets - + //INFO: Incoming packets0 + StackPacket<4, uint8_t, uint8_t, LevitaionState, uint8_t> lcu_states_packet; + //TODO: Un packete con el contactors state + StackPacket<20, float, float, float, float, float> accelerations; Packets(Data& data) : - regulator_packet(211, &data.valve_state, &data.regulator_reference_pressure, &data.regulator_real_pressure), - reed_packet(212, &data.reed1, &data.reed2, &data.reed3, &data.reed4, &data.reeds_ok), - bottle_temperature_packet(213, &data.bottle_temperature1, &data.bottle_temperature2), - pressure_packets(214, &data.high_pressure1, &data.low_pressure1, &data.low_pressure2), - environmental_packet(215, &data.enviremont_pressure, &data.enviroment_temperature) + regulator_packet((uint16_t)PacketsIDs::vcu_regulator_packet, &data.valve_state, &data.regulator_reference_pressure, &data.regulator_real_pressure), + bottle_temperature_packet((uint16_t)PacketsIDs::vcu_bottle_temperature_packet, &data.bottle_temperature1, &data.bottle_temperature2), + pressure_packets((uint16_t)PacketsIDs::vcu_pressure_packet, &data.high_pressure1, &data.low_pressure1, &data.low_pressure2), + environmental_packet((uint16_t)PacketsIDs::vcu_environmental_packet, &data.enviremont_pressure, &data.enviroment_temperature), + states_packet((uint16_t)PacketsIDs::states_packet, data.general_state, data.specific_state, data.load_state, data.unload_state, data.traction_state, data.dynamic_lev, data.specific_state), + lcu_states_packet((uint16_t)PacketsIDs::lcu_master_state_machine_packet, &trash_u8, &trash_u8, &data.levitation_state, &trash_u8), + accelerations((uint16_t)PacketsIDs::accelerations, &trash_f, &trash_f, &trash_f, &data.engine_acceleration, &data.engine_speed) {} }; } diff --git a/Core/Inc/VCU_Communications/VCU_UDP/VCU_UDP.hpp b/Core/Inc/VCU_Communications/VCU_UDP/VCU_UDP.hpp index 46ab91a..c9dedde 100644 --- a/Core/Inc/VCU_Communications/VCU_UDP/VCU_UDP.hpp +++ b/Core/Inc/VCU_Communications/VCU_UDP/VCU_UDP.hpp @@ -2,7 +2,7 @@ * VCU_UDP.hpp * * Created on: Jun 1, 2023 - * Author: stefancostea + * Author: stefancostea & Pablo */ #pragma once @@ -10,6 +10,7 @@ #include "DatagramSocket.hpp" #include "VCU.hpp" #include "VCU_Utilities/VCU_Types.hpp" +#include "Packets.hpp" namespace VCU{ template class UDP; @@ -79,5 +80,6 @@ namespace VCU{ void send_to_lcu(Packet& packet){ LCU_MASTER_CONNECTION.send(packet); } + }; } diff --git a/Core/Inc/VCU_Data/VCU_Data.hpp b/Core/Inc/VCU_Data/VCU_Data.hpp index a581774..51332c0 100644 --- a/Core/Inc/VCU_Data/VCU_Data.hpp +++ b/Core/Inc/VCU_Data/VCU_Data.hpp @@ -1,10 +1,9 @@ #pragma once #include "ST-LIB.hpp" -#include "VCU_Utilities/VCU_Types.hpp" +#include "VCU_Utilities/VCU_Includes.hpp" #include "VCU_Mode/VCU_Mode.hpp" - namespace VCU{ template class Data; @@ -38,11 +37,17 @@ namespace VCU{ class Data{ public: //BOARD Data + static constexpr float returning_speed = 5.0f; + static constexpr float crawling_speed = 3.0f; + static constexpr float min_speed = 0.0f; + static constexpr uint16_t brakes_time = 250; + float regulator_real_pressure = 0.0f; float regulator_reference_pressure = 0.0f; PinState emergeny_tape_enable = PinState::OFF; PinState emergency_tape = PinState::OFF; + bool emergency_braking = false; float high_pressure1 = 0.0f; float low_pressure1 = 0.0f; @@ -66,6 +71,14 @@ namespace VCU{ double tapes_speed = 0.0f; double tapes_acceleration = 0.0f; + uint8_t* general_state; + uint8_t* specific_state; + uint8_t* load_state; + uint8_t* unload_state; + uint8_t* traction_state; + uint8_t* dynamic_lev; + uint8_t* static_lev; + //VEHICLE Data LevitaionState levitation_state = IDLE; @@ -100,10 +113,5 @@ namespace VCU{ traction_points.clear(); } - void add_protections(){//TODO: Mover esto a un archivo separado y completar - add_protection(&high_pressure1, Boundary(300)); - add_protection(&reeds_ok, Boundary(true)); - add_protection(&emergency_tape, Boundary(PinState::ON)); - } }; } diff --git a/Core/Inc/VCU_Protections/VCU_Protections.hpp b/Core/Inc/VCU_Protections/VCU_Protections.hpp index e69de29..6f37b7f 100644 --- a/Core/Inc/VCU_Protections/VCU_Protections.hpp +++ b/Core/Inc/VCU_Protections/VCU_Protections.hpp @@ -0,0 +1,30 @@ +#pragma once + +#include "VCU_Utilities/VCU_Includes.hpp" + +namespace VCU{ + template class Protections; + + template<> + class Protections{ + Data& data; + + Protections(Data data): data(data){ + add_protection(&data.high_pressure1, Boundary(100, 300)); + add_protection(&data.low_pressure1, Boundary(0, 10)); + add_protection(&data.low_pressure2, Boundary(0, 10)); + + add_protection(&data.regulator_real_pressure, Boundary(0, 10)); + + add_protection(&data.bottle_temperature1, Boundary(0, 50)); + add_protection(&data.bottle_temperature2, Boundary(0, 50)); + + add_protection(&data.enviremont_pressure, Boundary(0, 1.2f)); + add_protection(&data.enviroment_temperature, Boundary(0, 50)); + +// add_protection(&data.reeds_ok, Boundary(true)); + add_protection(&data.emergency_braking, Boundary(true)); + } + }; + +} diff --git a/Core/Inc/VCU_StateMachine/VCU_DynamicLevStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_DynamicLevStateMachine.hpp index 36736ab..1de2641 100644 --- a/Core/Inc/VCU_StateMachine/VCU_DynamicLevStateMachine.hpp +++ b/Core/Inc/VCU_StateMachine/VCU_DynamicLevStateMachine.hpp @@ -76,7 +76,7 @@ namespace VCU{ }); state_machine.add_transition(Landing, OpenContactors, [&](){ - return data.levitation_state == LevitaionState::IDLE; + return data.levitation_state == LevitaionState::STICK_DOWN; }); state_machine.add_transition(OpenContactors, Idle, [&](){ @@ -161,6 +161,8 @@ namespace VCU{ add_transitions(); register_timed_actions(); add_transitions(); + + data.dynamic_lev = &state_machine.current_state; } }; } diff --git a/Core/Inc/VCU_StateMachine/VCU_FaultSpecificStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_FaultSpecificStateMachine.hpp new file mode 100644 index 0000000..cf450a7 --- /dev/null +++ b/Core/Inc/VCU_StateMachine/VCU_FaultSpecificStateMachine.hpp @@ -0,0 +1,82 @@ +#pragma once + +#include "VCU_Utilities/VCU_Includes.hpp" + +namespace VCU{ + template class FaultSpecificStateMachine; + + template<> + class FaultSpecificStateMachine{ + public: + Data& data; + Actuators& actuators; + TCP& tcp_handler; + OutgoingOrders& outgoing_orders; + EncoderSensor& encoder; + + UnloadStateMachine health_unload_state_machine; + + + StateMachine state_machine; + + static bool healthcheck_and_unload_requested; + + enum SpecificStates{ + Idle, + Unload, + }; + + FaultSpecificStateMachine(Data& data, Actuators& actuators, TCP& tcp, OutgoingOrders& outgoing_orders, EncoderSensor& encoder) : + data(data),actuators(actuators),tcp_handler(tcp), outgoing_orders(outgoing_orders), encoder(encoder), + health_unload_state_machine(data, actuators, tcp, outgoing_orders, encoder) + + { + init(); + } + + static void enter_health_and_unload(){ + healthcheck_and_unload_requested = true; + } + + void add_transitions(){ + state_machine.add_transition(Idle, Unload, [&](){ + return healthcheck_and_unload_requested; + }); + + state_machine.add_transition(Unload, Idle, [&](){ + return health_unload_state_machine.ended; + }); + } + + void add_on_enter_actions(){ + state_machine.add_enter_action([&](){ + healthcheck_and_unload_requested = false; + }, Idle); + } + + void add_on_exit_actions(){ + state_machine.add_exit_action([&](){ + health_unload_state_machine.ended = false; + }, Unload); + } + + void register_timed_actions(){} + + void init(){ + state_machine = {Idle}; + state_machine.add_state(Unload); + + state_machine.add_state_machine(health_unload_state_machine.state_machine, Unload); + + add_on_enter_actions(); + add_on_exit_actions(); + add_transitions(); + register_timed_actions(); + add_transitions(); + + data.specific_state = &state_machine.current_state; + } + }; + + bool VCU::SpecificStateMachine::healthcheck_and_unload_requested = false; +} diff --git a/Core/Inc/VCU_StateMachine/VCU_HealtCheckLoadStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_HealtCheckLoadStateMachine.hpp index ac7724f..a095049 100644 --- a/Core/Inc/VCU_StateMachine/VCU_HealtCheckLoadStateMachine.hpp +++ b/Core/Inc/VCU_StateMachine/VCU_HealtCheckLoadStateMachine.hpp @@ -7,8 +7,6 @@ namespace VCU{ template<> class LoadStateMachine{ - static constexpr float crawling_speed = 3.0f; - public: Data& data; Actuators& actuators; @@ -55,7 +53,7 @@ namespace VCU{ }); state_machine.add_transition(Pushing, CloseContactors, [&](){ - //Solo se puede emepezar el crawling si se esta en zona de emergencia + //INFO: Solo se puede emepezar el crawling si se esta en zona de emergencia if(crawling_requested && data.emergency_tape == PinState::ON){ crawling_requested = false; return true; @@ -72,7 +70,7 @@ namespace VCU{ }); state_machine.add_transition(Braking, OpenContactors, [&](){ - return data.tapes_acceleration == 0; //TODO: Muy importante hacer esto con la IMU + return data.engine_speed <= Data::min_speed; }); state_machine.add_transition(OpenContactors, Idle, [&](){ @@ -88,7 +86,8 @@ namespace VCU{ }, Pushing); state_machine.add_enter_action([&](){ - outgoing_orders.speed = crawling_speed; + outgoing_orders.speed = Data::crawling_speed; + outgoing_orders.direction = DIRECTION::FORWARD; tcp_handler.send_to_pcu(outgoing_orders.move); }, Crawling); @@ -107,9 +106,6 @@ namespace VCU{ state_machine.add_exit_action([&](){ if(data.emergency_tape == PinState::ON){ ErrorHandler("The vehicle is still in emergency zone after Health&Load procedure"); - }else{ - actuators.brakes.brake(); - //Siguiendo el FDD kenos debe quedar frenado con pneumatica hasta empezar una demostración } }, Braking); @@ -148,6 +144,8 @@ namespace VCU{ add_transitions(); register_timed_actions(); add_transitions(); + + data.load_state = &state_machine.current_state; } }; diff --git a/Core/Inc/VCU_StateMachine/VCU_HealtCheckUnloadStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_HealtCheckUnloadStateMachine.hpp index 1e35f3e..2dee330 100644 --- a/Core/Inc/VCU_StateMachine/VCU_HealtCheckUnloadStateMachine.hpp +++ b/Core/Inc/VCU_StateMachine/VCU_HealtCheckUnloadStateMachine.hpp @@ -42,7 +42,7 @@ namespace VCU{ } void add_transitions(){ - //Quitar los frenos debe hacerse manualmente con una orden una vez finalizado el procedure + //INFO: Quitar los frenos debe hacerse manualmente con una orden una vez finalizado el procedure //porque segun el fdd el pod debe estar frenado hasta el momento que se va a sacar state_machine.add_transition(Idle, CloseContactors, [&](){ @@ -58,15 +58,17 @@ namespace VCU{ }); state_machine.add_transition(Returning, Crawling, [&](){ - return data.emergency_tape == PinState::ON; //Si hemos llegado a la zona de emergencia pasamos a fase crawling + //INFO: Si hemos llegado a la zona de emergencia pasamos a fase crawling + return data.emergency_tape == PinState::ON; }); state_machine.add_transition(Crawling, Braking, [&](){ - return data.emergency_tape == PinState::OFF; //Si hemos llegado al final de la zona de emergencia frenamos + //INFO: Si hemos llegado al final de la zona de emergencia frenamos + return data.emergency_tape == PinState::OFF; }); state_machine.add_transition(Braking, OpenContactors, [&](){ - return data.tapes_acceleration == 0; //TODO: Muy importante hacer esto con la IMU del motor + return data.engine_speed <= Data::min_speed; }); state_machine.add_transition(OpenContactors, Idle, [&](){ @@ -79,36 +81,44 @@ namespace VCU{ state_machine.add_enter_action([&](){ actuators.brakes.not_brake(); actuators.brakes.disable_emergency_brakes(); - //TODO: set engine parameters - //TODO: send start engine order + + Time::set_timeout(Data::brakes_time, [&](){ + outgoing_orders.speed = Data::returning_speed; + tcp_handler.send_to_pcu(outgoing_orders.move); + }); }, Returning); state_machine.add_enter_action([&](){ - //Esto es por si queremos cambiar los parametros (como ir mas lento) - //TODO: set engine parameters + outgoing_orders.speed = Data::crawling_speed; + outgoing_orders.direction = VCU::DIRECTION::BACKWARD; + tcp_handler.send_to_pcu(outgoing_orders.move); }, Crawling); state_machine.add_enter_action([&](){ - //TODO: send engine stop order + tcp_handler.send_to_pcu(outgoing_orders.brake); }, Braking); state_machine.add_enter_action([&](){ actuators.brakes.enable_emergency_brakes(); actuators.brakes.brake(); }, OpenContactors); - } void add_on_exit_actions(){ state_machine.add_exit_action([&](){ + tcp_handler.send_to_pcu(outgoing_orders.turn_off); + if(data.emergency_tape == PinState::ON){ ErrorHandler("The vehicle is still in emergency zone after Health&Unload procedure"); - }else{ - actuators.brakes.brake(); } }, Braking); state_machine.add_exit_action([&](){ + close_contactors_state_machine.ended = false; + }, CloseContactors); + + state_machine.add_exit_action([&](){ + open_contactors_state_machine.ended = false; ended = true; }, OpenContactors); @@ -151,6 +161,7 @@ namespace VCU{ register_timed_actions(); add_transitions(); + data.unload_state = &state_machine.current_state; } }; } diff --git a/Core/Inc/VCU_StateMachine/VCU_PointTravelStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_PointTravelStateMachine.hpp index 8001707..e5942dd 100644 --- a/Core/Inc/VCU_StateMachine/VCU_PointTravelStateMachine.hpp +++ b/Core/Inc/VCU_StateMachine/VCU_PointTravelStateMachine.hpp @@ -7,7 +7,6 @@ namespace VCU{ template<> class PointTravelStateMachine{ - static constexpr float min_speed = 1.0f; public: Data& data; @@ -69,13 +68,14 @@ namespace VCU{ }); state_machine.add_transition(Braking, End, [&](){ - return data.engine_speed <= min_speed; + return data.engine_speed <= Data::min_speed or data.tapes_position <= initial_point.position; }); } void add_on_enter_actions(){ state_machine.add_enter_action([&](){ initial_point = point_t(data.tapes_position, 0.0f); + data.traction_points.push_back(initial_point); }, Idle); state_machine.add_enter_action([&](){ @@ -100,6 +100,7 @@ namespace VCU{ calculating = false; outgoing_orders.speed = calculated_point->speed; + outgoing_orders.direction = calculated_direction; tcp_handler.send_to_pcu(outgoing_orders.move); }, MovingForward); @@ -119,21 +120,16 @@ namespace VCU{ state_machine.add_enter_action([&](){ tcp_handler.send_to_pcu(outgoing_orders.turn_off); ended = true; + actuators.brakes.brake(); }, End); } void add_on_exit_actions(){ - state_machine.add_exit_action([&](){ - actuators.brakes.not_brake(); - }, Braking); + } void register_timed_actions(){ - state_machine.add_low_precision_cyclic_action([&](){ - if (data.tapes_position <= initial_point.position && data.engine_speed != 0.0f) { - actuators.brakes.brake(); - } - }, (ms)1, Braking); + } diff --git a/Core/Inc/VCU_StateMachine/VCU_SpecificStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_SpecificStateMachine.hpp index bdaf216..bf63c53 100644 --- a/Core/Inc/VCU_StateMachine/VCU_SpecificStateMachine.hpp +++ b/Core/Inc/VCU_StateMachine/VCU_SpecificStateMachine.hpp @@ -173,6 +173,8 @@ namespace VCU{ add_transitions(); register_timed_actions(); add_transitions(); + + data.specific_state = &state_machine.current_state; } }; diff --git a/Core/Inc/VCU_StateMachine/VCU_StateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_StateMachine.hpp index b7548fd..a6a9a57 100644 --- a/Core/Inc/VCU_StateMachine/VCU_StateMachine.hpp +++ b/Core/Inc/VCU_StateMachine/VCU_StateMachine.hpp @@ -52,6 +52,7 @@ namespace VCU{ tcp_timeout = true; } }); + general_state_machine.add_enter_action([&](){ actuators.led_fault.turn_on(); actuators.brakes.brake(); @@ -66,6 +67,7 @@ namespace VCU{ general_state_machine.add_exit_action([&](){ actuators.led_fault.turn_off(); }, FAULT); + general_state_machine.add_exit_action([&](){ actuators.led_operational.turn_off(); }, OPERATIONAL); @@ -104,6 +106,7 @@ namespace VCU{ OutgoingOrders& outgoing_orders; EncoderSensor& encoder; SpecificStateMachine specific_state_machine; + FaultSpecificStateMachine fault_specific_state_machine; StateMachine general_state_machine; StackStateOrder<0> healthcheck_and_load; @@ -112,6 +115,8 @@ namespace VCU{ StackStateOrder<0> start_dynamic_lev; StackStateOrder<0> start_traction; + StackStateOrder<0> fault_healthcheck_and_unload; + bool tcp_timeout = false; static constexpr uint16_t max_tcp_connection_timeout = 65000; //ms //TODO: arreglar @@ -125,28 +130,32 @@ namespace VCU{ GeneralStateMachine(Data& data, Actuators& actuators, TCP& tcp, OutgoingOrders& outgoing_orders, EncoderSensor& encoder) : data(data), actuators(actuators), tcp_handler(tcp), outgoing_orders(outgoing_orders), encoder(encoder), specific_state_machine(data, actuators, tcp, outgoing_orders, encoder), + fault_specific_state_machine(data, actuators, tcp, outgoing_orders, encoder), healthcheck_and_load((uint16_t)IncomingOrdersIDs::heakthcheck_and_load, SpecificStateMachine::enter_health_and_load, specific_state_machine.state_machine, SpecificStateMachine::SpecificStates::Idle), healthcheck_and_unload((uint16_t)IncomingOrdersIDs::healthcheck_and_unload, SpecificStateMachine::enter_health_and_unload, specific_state_machine.state_machine, SpecificStateMachine::SpecificStates::Idle), start_static_lev((uint16_t)IncomingOrdersIDs::start_static_lev_demostration, SpecificStateMachine::enter_static_lev, specific_state_machine.state_machine, SpecificStateMachine::SpecificStates::Idle), start_dynamic_lev((uint16_t)IncomingOrdersIDs::start_dynamic_lev_demostration, SpecificStateMachine::enter_dynamic_lev, specific_state_machine.state_machine, SpecificStateMachine::SpecificStates::Idle), - start_traction((uint16_t)IncomingOrdersIDs::start_traction_demostration, SpecificStateMachine::enter_traction, specific_state_machine.state_machine, SpecificStateMachine::SpecificStates::Idle) + start_traction((uint16_t)IncomingOrdersIDs::start_traction_demostration, SpecificStateMachine::enter_traction, specific_state_machine.state_machine, SpecificStateMachine::SpecificStates::Idle), + + fault_healthcheck_and_unload((uint16_t)IncomingOrdersIDs::healthcheck_and_unload, FaultSpecificStateMachine::enter_health_and_unload, specific_state_machine.state_machine, FaultSpecificStateMachine::SpecificStates::Idle) + { init(); } void add_transitions(){ - //todo: COMPROBAR que estan todos conectados antes de pasar a operational general_state_machine.add_transition(INITIAL, OPERATIONAL, [&](){ - bool res = tcp_handler.check_connections(); - return res; + return tcp_handler.check_connections(); }); + general_state_machine.add_transition(INITIAL, FAULT, [&](){ if(tcp_timeout){ ErrorHandler("TCP connections could not be established in time and timed out"); } return tcp_timeout; }); + general_state_machine.add_transition(OPERATIONAL, FAULT, [&](){ if(not tcp_handler.check_connections() ){ ErrorHandler("TCP connections fell"); @@ -164,7 +173,7 @@ namespace VCU{ // } // }); // }, INITIAL); - //Replicado porque el estado incial no ejecuta las enter actions + //INFO: Replicado porque el estado incial no ejecuta las enter actions Time::set_timeout(max_tcp_connection_timeout, [&](){ if(not (tcp_handler.check_connections())){ tcp_timeout = true; @@ -216,6 +225,8 @@ namespace VCU{ add_transitions(); general_state_machine.add_state_machine(specific_state_machine.state_machine, OPERATIONAL); + + data.general_state = &general_state_machine.current_state; } }; } diff --git a/Core/Inc/VCU_StateMachine/VCU_StaticLevStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_StaticLevStateMachine.hpp index c3f3f3b..e24c4a5 100644 --- a/Core/Inc/VCU_StateMachine/VCU_StaticLevStateMachine.hpp +++ b/Core/Inc/VCU_StateMachine/VCU_StaticLevStateMachine.hpp @@ -83,6 +83,10 @@ namespace VCU{ void add_on_enter_actions(){ + state_machine.add_enter_action([&](){ + actuators.brakes.not_brake(); + }, CloseContactors); + state_machine.add_enter_action([&](){ actuators.brakes.not_brake(); tcp_handler.send_to_lcu(outgoing_orders.take_off_order); @@ -90,13 +94,20 @@ namespace VCU{ state_machine.add_enter_action([&](){ tcp_handler.send_to_lcu(outgoing_orders.landing_order); - actuators.brakes.brake(); }, LevOff); + state_machine.add_enter_action([&](){ + actuators.brakes.brake(); + }, OpenContactors); } void add_on_exit_actions(){ state_machine.add_exit_action([&](){ + close_contactors_state_machine.ended = false; + }, CloseContactors); + + state_machine.add_exit_action([&](){ + open_contactors_state_machine.ended = false; ended = true; }, OpenContactors); } @@ -115,8 +126,9 @@ namespace VCU{ add_transitions(); register_timed_actions(); add_transitions(); - } + data.static_lev = &state_machine.current_state; + } }; bool StaticLevStateMachine::start_levitation_requested = false; diff --git a/Core/Inc/VCU_StateMachine/VCU_TractionStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_TractionStateMachine.hpp index cc3db1f..a7eb820 100644 --- a/Core/Inc/VCU_StateMachine/VCU_TractionStateMachine.hpp +++ b/Core/Inc/VCU_StateMachine/VCU_TractionStateMachine.hpp @@ -91,8 +91,20 @@ namespace VCU{ } void add_on_exit_actions(){ + state_machine.add_exit_action([&](){ + receiving_data_state_machine.ended = false; + }, ReceivingData); + + state_machine.add_exit_action([&](){ + close_contactors_state_machine.ended = false; + }, CloseContactors); state_machine.add_exit_action([&](){ + point_travel_state_machine.ended = false; + }, PointTravel); + + state_machine.add_exit_action([&](){ + open_contactors_state_machine.ended = false; ended = true; }, OpenContactors); } @@ -116,6 +128,8 @@ namespace VCU{ add_transitions(); register_timed_actions(); add_transitions(); + + data.traction_state = &state_machine.current_state; } }; } diff --git a/Core/Inc/VCU_Time/VCU_Time.hpp b/Core/Inc/VCU_Time/VCU_Time.hpp index db6fd56..cc68040 100644 --- a/Core/Inc/VCU_Time/VCU_Time.hpp +++ b/Core/Inc/VCU_Time/VCU_Time.hpp @@ -32,6 +32,7 @@ namespace VCU{ Time::register_low_precision_alarm(100, VCU::VCU_CLASS::read_environmental_sensors); Time::register_low_precision_alarm(16, VCU::VCU_CLASS::send_to_backend); Time::register_low_precision_alarm(1, VCU::VCU_CLASS::update_state_machine); + Time::register_low_precision_alarm(1, STLIB::update); } }; } diff --git a/Core/Inc/VCU_Utilities/VCU_Includes.hpp b/Core/Inc/VCU_Utilities/VCU_Includes.hpp index 4c4c9ed..157f16b 100644 --- a/Core/Inc/VCU_Utilities/VCU_Includes.hpp +++ b/Core/Inc/VCU_Utilities/VCU_Includes.hpp @@ -6,6 +6,7 @@ #include "VCU_Data/VCU_Data.hpp" #include "VCU_Mode/VCU_Mode.hpp" #include "VCU_Pinout/Pinout.hpp" +#include "VCU_Protections/VCU_Protections.hpp" #include "VCU_Sensors/VCU_EnviromentalSensors.hpp" #include "VCU_Sensors/VCU_RegulatorSensor.hpp" @@ -28,6 +29,7 @@ #include "VCU_StateMachine/VCU_DynamicLevStateMachine.hpp" #include "VCU_StateMachine/VCU_StaticLevStateMachine.hpp" #include "VCU_StateMachine/VCU_TractionStateMachine.hpp" +#include "VCU_StateMachine/VCU_FaultSpecificStateMachine.hpp" #include "VCU_StateMachine/VCU_SpecificStateMachine.hpp" #include "VCU_StateMachine/VCU_StateMachine.hpp" @@ -38,4 +40,3 @@ - diff --git a/Core/Inc/VCU_Utilities/VCU_Types.hpp b/Core/Inc/VCU_Utilities/VCU_Types.hpp index 4878982..ab7af5b 100644 --- a/Core/Inc/VCU_Utilities/VCU_Types.hpp +++ b/Core/Inc/VCU_Utilities/VCU_Types.hpp @@ -12,9 +12,9 @@ namespace VCU{ RETRACTED, }; - enum DIRECTION{ //TODO: Comprobar que esto es cierto - BACKWARD = 0, - FORWARD = 1, + enum DIRECTION{ + FORWARD = 0, + BACKWARD = 1, }; enum ContactorState{ From 56ebbb5929ab9e8f70289bfcc66922920cdb20fa Mon Sep 17 00:00:00 2001 From: Pablo Date: Fri, 30 Jun 2023 19:13:51 +0200 Subject: [PATCH 10/10] added some protections --- Core/Inc/VCU_Protections/VCU_Protections.hpp | 4 ++-- Core/Inc/VCU_StateMachine/VCU_FaultSpecificStateMachine.hpp | 2 +- Core/Src/main.cpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/Core/Inc/VCU_Protections/VCU_Protections.hpp b/Core/Inc/VCU_Protections/VCU_Protections.hpp index 6f37b7f..55cff39 100644 --- a/Core/Inc/VCU_Protections/VCU_Protections.hpp +++ b/Core/Inc/VCU_Protections/VCU_Protections.hpp @@ -16,8 +16,8 @@ namespace VCU{ add_protection(&data.regulator_real_pressure, Boundary(0, 10)); - add_protection(&data.bottle_temperature1, Boundary(0, 50)); - add_protection(&data.bottle_temperature2, Boundary(0, 50)); + add_protection(&data.bottle_temperature1, Boundary(0, 50)); + add_protection(&data.bottle_temperature2, Boundary(0, 50)); add_protection(&data.enviremont_pressure, Boundary(0, 1.2f)); add_protection(&data.enviroment_temperature, Boundary(0, 50)); diff --git a/Core/Inc/VCU_StateMachine/VCU_FaultSpecificStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_FaultSpecificStateMachine.hpp index cf450a7..5f6099f 100644 --- a/Core/Inc/VCU_StateMachine/VCU_FaultSpecificStateMachine.hpp +++ b/Core/Inc/VCU_StateMachine/VCU_FaultSpecificStateMachine.hpp @@ -78,5 +78,5 @@ namespace VCU{ } }; - bool VCU::SpecificStateMachine::healthcheck_and_unload_requested = false; + bool VCU::FaultSpecificStateMachine::healthcheck_and_unload_requested = false; } diff --git a/Core/Src/main.cpp b/Core/Src/main.cpp index b2b3418..7a63db9 100644 --- a/Core/Src/main.cpp +++ b/Core/Src/main.cpp @@ -96,7 +96,7 @@ void a_op_func(){ int main(void){ // state_machine_test(); - +// // VCU::VCU_CLASS vcu; // VCU::VCU_CLASS::vcu = &vcu; // vcu.init();