diff --git a/Core/Inc/VCU.hpp b/Core/Inc/VCU.hpp index 5d1433c..ce91471 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; @@ -25,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(){ @@ -50,28 +41,87 @@ 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{ + public: + static VCU_CLASS* vcu; + + Data data; + Actuators actuators; + EnvironmentalSensors environmental_sensors; + 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, outgoing_orders, encoder) + {} + + void init(){ + STLIB::start(); + actuators.brakes.init(); + tcp_handler.init(); + udp_handler.init(); + } + + static void read_brakes_sensors(){ + vcu->actuators.brakes.read(); + } + + static void read_environmental_sensors(){ + vcu->environmental_sensors.read(); + } + + static void send_to_backend(){ + 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.environmental_packet); + vcu->udp_handler.send_to_backend(vcu->packets.states_packet); + } + + static void update_state_machine(){ + vcu->state_machine_handler.general_state_machine.check_transitions(); + } }; void set_regulator_pressure(){ - VCU_CLASS::vcu->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(){ - 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(); } } VCU::VCU_CLASS* VCU::VCU_CLASS::vcu = nullptr; +VCU::VCU_CLASS* VCU::VCU_CLASS::vcu = nullptr; 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..db693b7 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; @@ -11,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); @@ -27,7 +45,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 +54,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 +65,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..3ba9e26 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; @@ -157,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), @@ -189,18 +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(){ @@ -228,6 +235,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 3c5cb49..7bdb539 100644 --- a/Core/Inc/VCU_Communications/VCU_TCP/IncomingOrders.hpp +++ b/Core/Inc/VCU_Communications/VCU_TCP/IncomingOrders.hpp @@ -1,20 +1,48 @@ +/* + * VCU_IncomingOrders.hpp + * + * Created on: Jun 1, 2023 + * Author: stefancostea + */ + #pragma once + #include "Packets/Order.hpp" #include "VCU.hpp" + 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, + traction_data = 233, + traction_end_data = 234, + }; + + void hardware_reset(){ HAL_NVIC_SystemReset(); } void set_regulator_pressure(); - void brake(); - void unbrake(); - void disable_emergency_tape(); - void enable_emergency_tape(); template class IncomingOrders; @@ -30,9 +58,33 @@ 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{ + 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((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 a89887a..e151b1f 100644 --- a/Core/Inc/VCU_Communications/VCU_TCP/OutgoingOrders.hpp +++ b/Core/Inc/VCU_Communications/VCU_TCP/OutgoingOrders.hpp @@ -1,2 +1,59 @@ +/* + * VCU_IncomingOrders.hpp + * + * Created on: Jun 1, 2023 + * Author: stefancostea + */ + #pragma once + #include "VCU.hpp" + +namespace VCU{ + enum class LevitationOrdes: uint16_t{ + TAKE_OFF = 300, + LANDING = 301, + }; + + enum class TractionOrders: uint16_t{ + MOVE = 612, + BRAKE = 613, + TURN_OFF = 614, + }; + + 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<4, float, DIRECTION> move; + StackOrder<0> brake; + StackOrder<0> turn_off; + + StackOrder<0> close_contactors; + 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, &direction), + 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 a69466b..56df9de 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,13 +20,76 @@ 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{ + public: + ServerSocket BACKEND_CONNECTION; + 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_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); + } + + bool check_connections(){ + return + BACKEND_CONNECTION.is_connected() + && + OBCCU_CONNECTION.is_connected() + && + BMSL_CONNECTION.is_connected() + && + LCU_MASTER_CONNECTION.is_connected() + && + 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 6ad62c3..d3d2a8b 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,10 +19,53 @@ 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) {} }; + + template<> + class Packets{ + public: + 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<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; + + //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((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 07de35f..c9dedde 100644 --- a/Core/Inc/VCU_Communications/VCU_UDP/VCU_UDP.hpp +++ b/Core/Inc/VCU_Communications/VCU_UDP/VCU_UDP.hpp @@ -1,7 +1,16 @@ +/* + * VCU_UDP.hpp + * + * Created on: Jun 1, 2023 + * Author: stefancostea & Pablo + */ + #pragma once + #include "DatagramSocket.hpp" #include "VCU.hpp" #include "VCU_Utilities/VCU_Types.hpp" +#include "Packets.hpp" namespace VCU{ template class UDP; @@ -11,12 +20,66 @@ 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; + 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(); + + 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(); + + + } + + 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..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; @@ -22,19 +21,33 @@ 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 + 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; @@ -57,5 +70,48 @@ namespace VCU{ double tapes_direction = 0.0f; 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; + + ContactorState contactors_state = ContactorState::Open; + + float engine_speed; + float engine_acceleration; + + //Demostrations + 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 + }; + + 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(); + } + }; } diff --git a/Core/Inc/VCU_Mode/VCU_Mode.hpp b/Core/Inc/VCU_Mode/VCU_Mode.hpp index d5d1bf4..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_Protections/VCU_Protections.hpp b/Core/Inc/VCU_Protections/VCU_Protections.hpp new file mode 100644 index 0000000..55cff39 --- /dev/null +++ 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_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_ContactorsStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_ContactorsStateMachine.hpp new file mode 100644 index 0000000..78dee08 --- /dev/null +++ b/Core/Inc/VCU_StateMachine/VCU_ContactorsStateMachine.hpp @@ -0,0 +1,122 @@ +#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 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, [&](){ + 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 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, [&](){ + 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.add_state(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 new file mode 100644 index 0000000..1de2641 --- /dev/null +++ b/Core/Inc/VCU_StateMachine/VCU_DynamicLevStateMachine.hpp @@ -0,0 +1,168 @@ +#pragma once + +#include "VCU_Utilities/VCU_Includes.hpp" + +namespace VCU{ + template class DynamicLevStateMachine; + + 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; + 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, + 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), + close_contactors_state_machine(data, tcp_handler, outgoing_orders), + open_contactors_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, 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; + }); + + 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::STICK_DOWN; + }); + + state_machine.add_transition(OpenContactors, Idle, [&](){ + return open_contactors_state_machine.ended; + }); + + } + + 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([&](){ + 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){ + ErrorHandler("Kenos has not been able to stabilize in time, maximum timeout: %dms", levitation_timeout); + } + }); + }, TakeOff); + + state_machine.add_enter_action([&](){ + tcp_handler.send_to_lcu(outgoing_orders.landing_order); + + 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([&](){ + 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); + } + + void register_timed_actions(){} + + 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); + 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(); + + 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..5f6099f --- /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::FaultSpecificStateMachine::healthcheck_and_unload_requested = false; +} diff --git a/Core/Inc/VCU_StateMachine/VCU_HealtCheckLoadStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_HealtCheckLoadStateMachine.hpp new file mode 100644 index 0000000..a095049 --- /dev/null +++ b/Core/Inc/VCU_StateMachine/VCU_HealtCheckLoadStateMachine.hpp @@ -0,0 +1,154 @@ +#pragma once + +#include "VCU_Utilities/VCU_Includes.hpp" + +namespace VCU{ + template class LoadStateMachine; + + template<> + class LoadStateMachine{ + public: + 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; + + bool ended = false; + + static bool crawling_requested; + + enum UnloadStates{ + Idle, + Pushing, + CloseContactors, + Crawling, + Braking, + OpenContactors, + }; + + 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) + { + init(); + } + + static void enter_crawling(){ + crawling_requested = true; + } + + void add_transitions(){ + state_machine.add_transition(Idle, Pushing, [&](){ + if(not ended){ + return true; + } + return false; + }); + + state_machine.add_transition(Pushing, CloseContactors, [&](){ + //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; + } + return false; + }); + + 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, OpenContactors, [&](){ + return data.engine_speed <= Data::min_speed; + }); + + state_machine.add_transition(OpenContactors, Idle, [&](){ + return open_contactors_state_machine.ended; + }); + } + + void add_on_enter_actions(){ + + state_machine.add_enter_action([&](){ + actuators.brakes.disable_emergency_brakes(); + actuators.brakes.not_brake(); + }, Pushing); + + state_machine.add_enter_action([&](){ + outgoing_orders.speed = Data::crawling_speed; + outgoing_orders.direction = DIRECTION::FORWARD; + tcp_handler.send_to_pcu(outgoing_orders.move); + }, Crawling); + + state_machine.add_enter_action([&](){ + tcp_handler.send_to_pcu(outgoing_orders.brake); + actuators.brakes.enable_emergency_brakes(); + }, Braking); + + state_machine.add_enter_action([&](){ + tcp_handler.send_to_pcu(outgoing_orders.turn_off); + 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&Load procedure"); + } + }, 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(){ + state_machine.add_low_precision_cyclic_action([&](){ + if(data.tapes_direction != FORWARD){ + ErrorHandler("The vehicle is moving backwards when it should be moving forward during load procedure"); + } + }, (ms)1, Crawling); + + } + + void init(){ + state_machine = {Idle}; + state_machine.add_state(CloseContactors); + state_machine.add_state(Pushing); + 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(); + add_transitions(); + register_timed_actions(); + add_transitions(); + + data.load_state = &state_machine.current_state; + } + + }; + + bool LoadStateMachine::crawling_requested = false; +} diff --git a/Core/Inc/VCU_StateMachine/VCU_HealtCheckUnloadStateMachine.hpp b/Core/Inc/VCU_StateMachine/VCU_HealtCheckUnloadStateMachine.hpp new file mode 100644 index 0000000..2dee330 --- /dev/null +++ b/Core/Inc/VCU_StateMachine/VCU_HealtCheckUnloadStateMachine.hpp @@ -0,0 +1,167 @@ +#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; + Actuators& actuators; + TCP& tcp_handler; + OutgoingOrders& outgoing_orders; + EncoderSensor& encoder; + + CloseContactorsStateMachine close_contactors_state_machine; + OpenContactorsStateMachine open_contactors_state_machine; + + StateMachine state_machine; + + bool ended = false; + + enum LoadStates{ + Idle, + CloseContactors, + Returning, + Crawling, + Braking, + OpenContactors, + }; + + 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) + { + init(); + } + + void add_transitions(){ + //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, [&](){ + 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, [&](){ + //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, [&](){ + //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.engine_speed <= Data::min_speed; + }); + + 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.disable_emergency_brakes(); + + 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([&](){ + 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([&](){ + 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"); + } + }, 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(){ + 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 != 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 != BACKWARD){ + 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(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(); + add_transitions(); + 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 new file mode 100644 index 0000000..e5942dd --- /dev/null +++ b/Core/Inc/VCU_StateMachine/VCU_PointTravelStateMachine.hpp @@ -0,0 +1,152 @@ +#pragma once + +#include "VCU_Utilities/VCU_Includes.hpp" + +namespace VCU{ + template class PointTravelStateMachine; + + template<> + class PointTravelStateMachine{ + + public: + Data& data; + Actuators actuators; + TCP& tcp_handler; + OutgoingOrders& outgoing_orders; + + StateMachine state_machine; + + enum TravelStates{ + Idle, + Calculating, + MovingForward, + MovingBackward, + Braking, + End + }; + + bool ended = false; + + point_t initial_point; + point_t* calculated_point = nullptr; + DIRECTION calculated_direction = DIRECTION::FORWARD; + bool calculating = true; + + 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; + }); + + state_machine.add_transition(Calculating, MovingForward, [&](){ + return not calculating && calculated_direction == DIRECTION::FORWARD; + }); + + 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; + } + + 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 <= 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([&](){ + 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; + outgoing_orders.direction = calculated_direction; + 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; + actuators.brakes.brake(); + }, End); + } + + void add_on_exit_actions(){ + + } + + void register_timed_actions(){ + + + } + + 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(); + add_transitions(); + register_timed_actions(); + add_transitions(); + } + + }; +} 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 new file mode 100644 index 0000000..bf63c53 --- /dev/null +++ b/Core/Inc/VCU_StateMachine/VCU_SpecificStateMachine.hpp @@ -0,0 +1,186 @@ +#pragma once + +#include "VCU_Utilities/VCU_Includes.hpp" + +namespace VCU{ + template class SpecificStateMachine; + + template<> + class SpecificStateMachine{ + public: + Data& data; + Actuators& actuators; + TCP& tcp_handler; + OutgoingOrders& outgoing_orders; + EncoderSensor& encoder; + + LoadStateMachine health_load_state_machine; + UnloadStateMachine health_unload_state_machine; + TractionStateMachine traction_state_machine; + StaticLevStateMachine static_lev_state_machine; + DynamicLevStateMachine dynamic_lev_state_machine; + + StackStateOrder<0> start_crawling; + StackStateOrder<0> take_off; + StackStateOrder<0> landing; + + StateMachine state_machine; + + 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, + Unload, + Load, + DynamicLev, + StaticLev, + Traction, + + }; + + 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), + 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) + + { + init(); + } + + 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; + }); + + 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([&](){ + 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(){ + 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(){} + + 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); + + 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(); + + data.specific_state = &state_machine.current_state; + } + }; + + 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 new file mode 100644 index 0000000..a6a9a57 --- /dev/null +++ b/Core/Inc/VCU_StateMachine/VCU_StateMachine.hpp @@ -0,0 +1,234 @@ +#pragma once + +#include "VCU_Utilities/VCU_Includes.hpp" + +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) + { + init(); + } + + 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; + Actuators& actuators; + TCP& tcp_handler; + OutgoingOrders& outgoing_orders; + EncoderSensor& encoder; + SpecificStateMachine specific_state_machine; + FaultSpecificStateMachine fault_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; + + StackStateOrder<0> fault_healthcheck_and_unload; + + bool tcp_timeout = false; + + static constexpr uint16_t max_tcp_connection_timeout = 65000; //ms //TODO: arreglar + + enum States{ + INITIAL, + OPERATIONAL, + FAULT + }; + + 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), + + 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(){ + general_state_machine.add_transition(INITIAL, OPERATIONAL, [&](){ + 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"); + 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); + //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; + } + }); + + general_state_machine.add_enter_action([&](){ + actuators.led_fault.turn_on(); + actuators.brakes.brake(); + actuators.brakes.enable_emergency_brakes(); + }, FAULT); + + general_state_machine.add_enter_action([&](){ + actuators.led_operational.turn_on(); + actuators.brakes.enable_emergency_brakes(); + }, OPERATIONAL); + } + + void add_on_exit_actions(){ + general_state_machine.add_exit_action([&](){ + actuators.led_fault.turn_off(); + actuators.brakes.not_brake(); + }, FAULT); + general_state_machine.add_exit_action([&](){ + actuators.led_operational.turn_off(); + }, OPERATIONAL); + general_state_machine.add_exit_action([&](){ + actuators.led_operational.turn_off(); + }, INITIAL); + } + + 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); + } + + 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); + + 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 new file mode 100644 index 0000000..e24c4a5 --- /dev/null +++ b/Core/Inc/VCU_StateMachine/VCU_StaticLevStateMachine.hpp @@ -0,0 +1,136 @@ +#pragma once + +#include "VCU_Utilities/VCU_Includes.hpp" + +namespace VCU{ + template class StaticLevStateMachine; + + template<> + class StaticLevStateMachine{ + public: + 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; + + bool ended = false; + + static bool start_levitation_requested; + static bool stop_levitation_requested; + + enum StaticLevStates{ + Idle, + CloseContactors, + LevOff, + LevOn, + OpenContactors, + }; + + 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) + { + init(); + } + + static void start_levitation(){ + start_levitation_requested = true; + } + + static void stop_levitation(){ + stop_levitation_requested = true; + } + + 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){ + 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, Idle, [&](){ + return open_contactors_state_machine.ended; + }); + } + + 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); + }, LevOn); + + state_machine.add_enter_action([&](){ + tcp_handler.send_to_lcu(outgoing_orders.landing_order); + }, 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); + } + + void register_timed_actions(){} + + void init(){ + state_machine = {Idle}; + state_machine.add_state(CloseContactors); + state_machine.add_state(LevOff); + state_machine.add_state(LevOn); + state_machine.add_state(OpenContactors); + + add_on_enter_actions(); + add_on_exit_actions(); + add_transitions(); + register_timed_actions(); + add_transitions(); + + data.static_lev = &state_machine.current_state; + } + }; + + 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 new file mode 100644 index 0000000..a7eb820 --- /dev/null +++ b/Core/Inc/VCU_StateMachine/VCU_TractionStateMachine.hpp @@ -0,0 +1,135 @@ +#pragma once + +#include "VCU_Utilities/VCU_Includes.hpp" + +namespace VCU{ + template class TractionStateMachine; + + template<> + 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; + 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 TractionStates{ + Idle, + ReceivingData, + CloseContactors, + PointTravel, + OpenContactors, + }; + + 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), + 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, 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; + }); + + 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([&](){ + 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([&](){ + actuators.brakes.brake(); + actuators.brakes.enable_emergency_brakes(); + }, OpenContactors); + + } + + 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); + } + + void register_timed_actions(){} + + 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(); + add_on_exit_actions(); + 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 eb91047..cc68040 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,21 @@ 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); + 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 new file mode 100644 index 0000000..157f16b --- /dev/null +++ b/Core/Inc/VCU_Utilities/VCU_Includes.hpp @@ -0,0 +1,42 @@ +#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_Protections/VCU_Protections.hpp" + +#include "VCU_Sensors/VCU_EnviromentalSensors.hpp" +#include "VCU_Sensors/VCU_RegulatorSensor.hpp" +#include "VCU_Sensors/VCU_Reed.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" +#include "VCU_Communications/VCU_TCP/IncomingOrders.hpp" +#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" +#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" + + + + + + + + diff --git a/Core/Inc/VCU_Utilities/VCU_Types.hpp b/Core/Inc/VCU_Utilities/VCU_Types.hpp index 398a5d9..ab7af5b 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,47 @@ namespace VCU{ RETRACTED, }; + enum DIRECTION{ + FORWARD = 0, + BACKWARD = 1, + }; + + enum ContactorState{ + Open, + Close, + }; + + enum LevitaionState{ + IDLE, + TAKING_OFF, + STABLE, + STICK_UP, + STICK_DOWN, + 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 3ccbcea..7a63db9 100644 --- a/Core/Src/main.cpp +++ b/Core/Src/main.cpp @@ -6,12 +6,159 @@ #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); +} + +//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){ +// state_machine_test(); + +// +// 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(); } 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 @@ - +