diff --git a/CMakeLists.txt b/CMakeLists.txt index 0e91c7441f83e25e0934fb95f11326ecaf0d4fc5..96e274d6234b7d5b086f3ab5aa730526e88d4eb5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -102,3 +102,10 @@ add_executable(lyra-gs-entry ) target_include_directories(lyra-gs-entry PRIVATE ${OBSW_INCLUDE_DIRS}) sbs_target(lyra-gs-entry stm32f767zi_lyra_gs) + +add_executable(rotating-entry + src/Groundstation/RotatingPlatform/rotating-entry.cpp + ${ROTATING_PLATFORM} +) +target_include_directories(rotating-entry PRIVATE ${OBSW_INCLUDE_DIRS}) +sbs_target(rotating-entry stm32f767zi_lyra_gs) diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake index 27364e888ef6b87ce7fafb589120f6ef41ea96e8..cfd4a35b5776be5c896f351feda53126fcdc87af 100644 --- a/cmake/dependencies.cmake +++ b/cmake/dependencies.cmake @@ -118,3 +118,10 @@ set (LYRA_GS src/Groundstation/Automated/PinHandler/PinHandler.cpp src/Groundstation/LyraGS/Ports/SerialLyraGS.cpp ) + +set (ROTATING_PLATFORM + src/Groundstation/RotatingPlatform/Actuators/Actuators.cpp + src/Groundstation/RotatingPlatform/PinHandler/PinHandler.cpp + src/Groundstation/Automated/Leds/Leds.cpp +) + diff --git a/src/Groundstation/RotatingPlatform/Actuators/Actuators.cpp b/src/Groundstation/RotatingPlatform/Actuators/Actuators.cpp new file mode 100644 index 0000000000000000000000000000000000000000..3872d379b7a2552a2a282c46f61e4b6cac586bba --- /dev/null +++ b/src/Groundstation/RotatingPlatform/Actuators/Actuators.cpp @@ -0,0 +1,246 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Authors: Nicolò Caruso + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include "Actuators.h" + +#include <interfaces-impl/hwmapping.h> + +#include "ActuatorsConfig.h" + +using namespace miosix; +using namespace Boardcore; + +namespace RotatingPlatform +{ + +using namespace Config; + +// TIM1_CH1 PA8 AF1 Stepper H step +// | +// TIM3_CH2 PC7 AF2 Stepper H count + +// TIM4_CH1 PD12 AF2 Stepper V step +// | +// TIM8_CH1 PC6 AF3 Stepper V count + +CountedPWM countedPwmX(StepperSettings::SERVO1_PULSE_TIM, + StepperSettings::SERVO1_PULSE_CH, + StepperSettings::SERVO1_PULSE_ITR, + StepperSettings::SERVO1_COUNT_TIM, + StepperSettings::SERVO1_COUNT_CH, + StepperSettings::SERVO1_COUNT_ITR); + +Actuators::Actuators() + : ActiveObject(STACK_MIN_FOR_SKYWARD, MAIN_PRIORITY), + stepperX(countedPwmX, stepper1::pulseTimer::getPin(), + stepper1::direction::getPin(), stepperXConfig.MAX_SPEED, + stepperXConfig.STEP_ANGLE, false, stepperXConfig.MICROSTEPPING, + Stepper::PinConfiguration::COMMON_CATHODE, + stepper1::enable::getPin()){}; + +Actuators::Actuators(StepperConfig config) + : ActiveObject(STACK_MIN_FOR_SKYWARD, MAIN_PRIORITY), + stepperX(countedPwmX, stepper1::pulseTimer::getPin(), + stepper1::direction::getPin(), config.MAX_SPEED, + config.STEP_ANGLE, false, config.MICROSTEPPING, + Stepper::PinConfiguration::COMMON_CATHODE, + stepper1::enable::getPin()), + configX(config) +{ +} + +void Actuators::run() +{ + { + miosix::Lock<FastMutex> lock(rotationMutex); + + float speed = speedX; + + // Acceleration/stable phase + if (stepperX.isEnabled() && isRotating) + { + if (speed < configX.MAX_SPEED) + speed += 0.1; + if (speed > configX.MAX_SPEED) + speed = configX.MAX_SPEED; + setSpeed(StepperList::STEPPER_X, speed); + } + // Deceleration phase + else + { + if (speedX > 0) + speed -= 0.1; + if (speed < 0) + speed = 0; + setSpeed(StepperList::STEPPER_X, speed); + } + } + Thread::sleep(100); +} + +void Actuators::arm() { stepperX.enable(); } + +void Actuators::disarm() { stepperX.disable(); } + +void Actuators::enableRotation() +{ + miosix::Lock<FastMutex> lock(rotationMutex); + isRotating = true; +} + +void Actuators::disableRotation() +{ + miosix::Lock<FastMutex> lock(rotationMutex); + isRotating = false; +} + +ActuationStatus Actuators::setSpeed(StepperList axis, float speed) +{ + const auto *config = getStepperConfig(axis); + auto *stepper = getStepper(axis); + auto actuationState = ActuationStatus::OK; + auto multiplier = getStepperMultiplier(axis); + + speed *= multiplier; + + if (speed > config->MAX_SPEED) + { + speed = config->MAX_SPEED; + actuationState = ActuationStatus::SPEED_LIMIT; + } + + stepper->setSpeed(speed); + + switch (axis) + { + case StepperList::STEPPER_X: + speedX = speed; + break; + default: + actuationState = ActuationStatus::NO_STEPPER; + break; + } + + return actuationState; +} + +void Actuators::zeroPosition() { stepperX.zeroPosition(); } + +int16_t Actuators::getCurrentPosition(StepperList axis) +{ + return getStepper(axis)->getCurrentPosition(); +} + +float Actuators::getCurrentDegPosition(StepperList axis) +{ + auto multiplier = getStepperMultiplier(axis); + auto *stepper = getStepper(axis); + + return stepper->getCurrentDegPosition() / multiplier; +} + +ActuationStatus Actuators::move(StepperList axis, int16_t steps) +{ + auto *stepper = getStepper(axis); + + ActuationStatus actuationState = + ActuationStatus::OK; //< In case the move command is not limited + + if (!stepper->isEnabled()) + return ActuationStatus::DISABLED; + + const auto *config = getStepperConfig(axis); + float position = getCurrentPosition(axis); + + if (configX.HAS_ANGLE_LIMITS) + { + int16_t maxSteps = + config->MAX_ANGLE * config->MICROSTEPPING / config->STEP_ANGLE; + int16_t minSteps = + config->MIN_ANGLE * config->MICROSTEPPING / config->STEP_ANGLE; + + // POSITION_LIMIT POSITION IN ACCEPTABLE RANGE + if (position + steps > maxSteps) + { + steps = config->MAX_ANGLE - position; + actuationState = ActuationStatus::POSITION_LIMIT; + } + else if (position + steps < minSteps) + { + steps = config->MIN_ANGLE - position; + } + } + stepper->move(steps); + logStepperData(axis, stepper->getState(steps * config->STEP_ANGLE / + config->MICROSTEPPING)); + return actuationState; +} + +ActuationStatus Actuators::moveDeg(StepperList axis, float degrees) +{ + auto *stepper = getStepper(axis); + + ActuationStatus actuationState = + ActuationStatus::OK; //< In case the move command is not limited + + const auto *config = getStepperConfig(axis); + auto multiplier = getStepperMultiplier(axis); + float positionDeg = getCurrentDegPosition(axis); + + if (configX.HAS_ANGLE_LIMITS) + { + // POSITION_LIMIT POSITION IN ACCEPTABLE RANGE + if (positionDeg + degrees > config->MAX_ANGLE) + { + degrees = config->MAX_ANGLE - positionDeg; + actuationState = ActuationStatus::POSITION_LIMIT; + } + else if (positionDeg + degrees < config->MIN_ANGLE) + { + degrees = config->MIN_ANGLE - positionDeg; + } + } + + // Moving stepper of the angle 'degrees' + stepper->moveDeg(degrees * multiplier); + logStepperData(axis, stepper->getState(degrees)); + return actuationState; +} + +ActuationStatus Actuators::setPositionDeg(StepperList axis, float positionDeg) +{ + return moveDeg(axis, positionDeg - getCurrentDegPosition(axis)); +} + +void Actuators::setMultipliers(StepperList axis, float multiplier) +{ + switch (axis) + { + case StepperList::STEPPER_X: + multiplierX = multiplier; + break; + default: + assert(false && "Non existent stepper"); + break; + } +} +} // namespace RotatingPlatform diff --git a/src/Groundstation/RotatingPlatform/Actuators/Actuators.h b/src/Groundstation/RotatingPlatform/Actuators/Actuators.h new file mode 100644 index 0000000000000000000000000000000000000000..5a1b1e03ab59902fdfe1ba2b78f3c7927ffbb962 --- /dev/null +++ b/src/Groundstation/RotatingPlatform/Actuators/Actuators.h @@ -0,0 +1,224 @@ +/* Copyright (c) 2023-2024 Skyward Experimental Rocketry + * Authors: Emilio Corigliano, Nicolò Caruso + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ +#pragma once + +#include <ActiveObject.h> +#include <common/MavlinkLyra.h> +#include <logger/Logger.h> +#include <utils/DependencyManager/DependencyManager.h> + +#include "ActuatorsConfig.h" +#include "ActuatorsData.h" +#include "actuators/stepper/StepperPWM.h" + +// TIM1_CH1 PA8 AF1 Stepper H step +// | +// TIM3_CH2 PC7 AF2 Stepper H count + +namespace RotatingPlatform +{ + +/** + * @brief Error handling enum for the stepper movement + */ +enum class ActuationStatus : uint8_t +{ + OK, ///< `0` + POSITION_LIMIT, ///< `1` The STEPPER_X reached its actuation limits + SPEED_LIMIT, ///< `2` Stepper reached its velocity limits + NOT_TEST, ///< `3` Such movement is allowed only in test + NO_STEPPER, ///< `4` The specified stepper does not exist + DISABLED, ///< `5` +}; + +/** + * @brief Actuators handles the stepper X and exposes functions to drive it. + * Also, it updates its status by checking the speed and linearly increase and + * decrease it. This is done to avoid issues due to inertia and for safety + * reasons. + */ +class Actuators : public Boardcore::Injectable, public Boardcore::ActiveObject +{ +public: + /** + * @brief Construct a new Actuators object using default stepper + * configurations + * + */ + Actuators(); + + /** + * @brief Construct a new Actuators object using a given configuration for + * the stepper + * + * @param config The stepper configuration, used to override the default one + */ + Actuators(StepperConfig config); + + /** + * @brief Enables the actuators + */ + void arm(); + + /** + * @brief Disables the actuators + */ + void disarm(); + + /** + * @brief Enables the rotational movement + * + */ + void enableRotation(); + + /** + * @brief Disables the rotational movement + * + */ + void disableRotation(); + + ActuationStatus setSpeed(StepperList axis, float speed); + ActuationStatus move(StepperList axis, int16_t steps); + ActuationStatus moveDeg(StepperList axis, float degrees); + ActuationStatus setPosition(StepperList axis, int16_t steps); + ActuationStatus setPositionDeg(StepperList axis, float degrees); + + void setMultipliers(StepperList axis, float multiplier); + bool areMultipliersSet(); + + void zeroPosition(); + + int16_t getCurrentPosition(StepperList axis); + float getCurrentDegPosition(StepperList axis); + + /** + * @brief Getter for the last actuation of the wanted stepper. + * @param axis The stepper from which we want this information. + * @returns The last delta angle the chosen stepper is performing [deg]. + */ + float getDeltaAngleDeg(StepperList axis) const + { + switch (axis) + { + case StepperList::STEPPER_X: + return deltaX; + default: + assert(false && "Non existent stepper"); + return 0; + } + } + + /** + * @brief Getter for the speed of the wanted stepper. + * @param axis The stepper from which we want this information. + * @returns The speed of the chosen stepper [rps]. + */ + float getSpeed(StepperList axis) const + { + switch (axis) + { + case StepperList::STEPPER_X: + return speedX; + default: + assert(false && "Non existent stepper"); + return 0; + } + } + +protected: + /** + * @brief Run function for actuators in rotational platform case. It + * accelerate and then spin to the wanted rotational speed + * @note This function is used to linearly increase and decrease the speed + * of the steppers. + */ + void run() override; + +private: + Boardcore::StepperPWM* getStepper(StepperList stepper) + { + switch (stepper) + { + case StepperList::STEPPER_X: + return &stepperX; + default: + assert(false && "Non existent stepper"); + return nullptr; + } + }; + + const StepperConfig* getStepperConfig(StepperList stepper) const + { + switch (stepper) + { + case StepperList::STEPPER_X: + return &RotatingPlatform::Config::stepperXConfig; + + default: + assert(false && "Non existent stepperConfig"); + return nullptr; + } + }; + + float getStepperMultiplier(StepperList stepper) const + { + switch (stepper) + { + case StepperList::STEPPER_X: + return multiplierX; + default: + assert(false && "Non existent stepperConfig"); + return 0; + } + }; + + void logStepperData(StepperList stepper, Boardcore::StepperData data) + { + switch (stepper) + { + case StepperList::STEPPER_X: + Boardcore::Logger::getInstance().log( + static_cast<StepperXData>(data)); + break; + default: + assert(false && "Non existent stepper"); + break; + } + } + + Boardcore::StepperPWM stepperX; + + // Flags indicating if the multipliers have been set + float multiplierX = 2.2f; // Multiplier for the stepper X + float multiplierY = 2.2f; // Multiplier for the stepper Y + + float deltaX = 0.0f; // Delta angle to perform [deg] + float deltaY = 0.0f; // Delta angle to perform [deg] + float speedX = 0; // Speed of the stepper [rps] + + StepperConfig configX = Config::stepperXConfig; + + // Variables to enable the rotation + bool isRotating = false; + miosix::FastMutex rotationMutex; +}; +} // namespace RotatingPlatform diff --git a/src/Groundstation/RotatingPlatform/Actuators/ActuatorsConfig.h b/src/Groundstation/RotatingPlatform/Actuators/ActuatorsConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..f076a8f01a4e9418d19552d80d4355fb659e79f4 --- /dev/null +++ b/src/Groundstation/RotatingPlatform/Actuators/ActuatorsConfig.h @@ -0,0 +1,66 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: Nicolò Caruso + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once +#include <drivers/timer/PWM.h> +#include <drivers/timer/TimerUtils.h> + +#include "ActuatorsData.h" + +namespace RotatingPlatform +{ +namespace Config +{ + +static const RotatingPlatform::StepperConfig stepperXConfig{ + .MICROSTEPPING = 4, + .STEP_ANGLE = 1.8, + .HAS_ANGLE_LIMITS = false, + .MIN_ANGLE = 0, + .MAX_ANGLE = 0, + .MAX_SPEED = 0.1, //<[RPS] +}; + +namespace StepperSettings +{ +// TIM1_CH1 PA8 AF1 Stepper H step +// | +// TIM3_CH2 PC7 AF2 Stepper H count + +static TIM_TypeDef* const SERVO1_PULSE_TIM = TIM1; +static TIM_TypeDef* const SERVO1_COUNT_TIM = TIM3; +static TIM_TypeDef* const SERVO2_PULSE_TIM = TIM4; +static TIM_TypeDef* const SERVO2_COUNT_TIM = TIM8; + +constexpr Boardcore::TimerUtils::Channel SERVO1_PULSE_CH = + Boardcore::TimerUtils::Channel::CHANNEL_1; +constexpr Boardcore::TimerUtils::Channel SERVO1_COUNT_CH = + Boardcore::TimerUtils::Channel::CHANNEL_2; + +constexpr Boardcore::TimerUtils::TriggerSource SERVO1_PULSE_ITR = + Boardcore::TimerUtils::TriggerSource::ITR2; +constexpr Boardcore::TimerUtils::TriggerSource SERVO1_COUNT_ITR = + Boardcore::TimerUtils::TriggerSource::ITR0; + +} // namespace StepperSettings +} // namespace Config +} // namespace RotatingPlatform diff --git a/src/Groundstation/RotatingPlatform/Actuators/ActuatorsData.h b/src/Groundstation/RotatingPlatform/Actuators/ActuatorsData.h new file mode 100644 index 0000000000000000000000000000000000000000..0ed512f3453471c7029a9e9c19eb18f816b3bab2 --- /dev/null +++ b/src/Groundstation/RotatingPlatform/Actuators/ActuatorsData.h @@ -0,0 +1,48 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ +#pragma once +#include <actuators/stepper/StepperData.h> + +namespace RotatingPlatform +{ + +struct StepperConfig +{ + int MICROSTEPPING; + float STEP_ANGLE; + bool HAS_ANGLE_LIMITS; + float MIN_ANGLE; + float MAX_ANGLE; + float MAX_SPEED; +}; + +struct StepperXData : Boardcore::StepperData +{ + explicit StepperXData() : Boardcore::StepperData() {} + + explicit StepperXData(const Boardcore::StepperData& data) + : Boardcore::StepperData(data) + { + } +}; + +} // namespace RotatingPlatform diff --git a/src/Groundstation/RotatingPlatform/PinHandler/PinHandler.cpp b/src/Groundstation/RotatingPlatform/PinHandler/PinHandler.cpp new file mode 100644 index 0000000000000000000000000000000000000000..6e513e1f1786dfb3c25683d262853d1348da2339 --- /dev/null +++ b/src/Groundstation/RotatingPlatform/PinHandler/PinHandler.cpp @@ -0,0 +1,130 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: Nicolò Caruso + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <Groundstation/RotatingPlatform/PinHandler/PinHandler.h> +#include <common/Events.h> +#include <common/Topics.h> +#include <drivers/timer/TimestampTimer.h> +#include <events/EventBroker.h> +#include <interfaces-impl/hwmapping.h> + +#include <functional> + +using namespace Boardcore; +using namespace std; +using namespace std::placeholders; +using namespace Common; + +namespace RotatingPlatform +{ +PinHandler::PinHandler() : scheduler(), pin_observer(scheduler) +{ + pin_observer.registerPinCallback( + miosix::commBox::switchArm::getPin(), + bind(&PinHandler::onArmTransition, this, _1), + PinHandlerConfig::ARM_SWITCH_THRESHOLD); + + pin_observer.registerPinCallback( + miosix::commBox::switchActive::getPin(), + bind(&PinHandler::onActiveTransition, this, _1), + PinHandlerConfig::ACTIVE_SWITCH_THRESHOLD); +} + +bool PinHandler::start() { return scheduler.start(); } + +bool PinHandler::isStarted() { return scheduler.isRunning(); } + +void PinHandler::onArmTransition(PinTransition transition) +{ + if (transition == Boardcore::PinTransition::RISING_EDGE) + { + getModule<Antennas::Leds>()->setOn(Antennas::LedColor::YELLOW); + getModule<Actuators>()->arm(); + + // Log the event + PinData data = getPinData(PinList::ARM_SWITCH); + PinChangeData log{TimestampTimer::getTimestamp(), PinList::ARM_SWITCH, + data.changesCount}; + Logger::getInstance().log(log); + } + else + { + getModule<Antennas::Leds>()->setFastBlink(Antennas::LedColor::YELLOW); + getModule<Actuators>()->disarm(); + + // Log the event + PinData data = getPinData(PinList::ARM_SWITCH); + PinChangeData log{TimestampTimer::getTimestamp(), PinList::ARM_SWITCH, + data.changesCount}; + Logger::getInstance().log(log); + } +} + +void PinHandler::onActiveTransition(PinTransition transition) +{ + if (transition == Boardcore::PinTransition::RISING_EDGE) + { + getModule<Antennas::Leds>()->setOn(Antennas::LedColor::BLUE); + getModule<Actuators>()->enableRotation(); + + // Log the event + PinData data = getPinData(PinList::ACTIVE_SWITCH); + PinChangeData log{TimestampTimer::getTimestamp(), + PinList::ACTIVE_SWITCH, data.changesCount}; + Logger::getInstance().log(log); + } + + else + { + getModule<Antennas::Leds>()->setFastBlink(Antennas::LedColor::BLUE); + getModule<Actuators>()->disableRotation(); + + // Log the event + PinData data = getPinData(PinList::ACTIVE_SWITCH); + PinChangeData log{TimestampTimer::getTimestamp(), + PinList::ACTIVE_SWITCH, data.changesCount}; + Logger::getInstance().log(log); + } +} + +PinData PinHandler::getPinData(PinList pin) +{ + switch (pin) + { + case PinList::ARM_SWITCH: + { + return pin_observer.getPinData( + miosix::commBox::switchArm::getPin()); + } + case PinList::ACTIVE_SWITCH: + { + return pin_observer.getPinData( + miosix::commBox::switchActive::getPin()); + } + default: + { + LOG_ERR(logger, "Requested non valid pin"); + return PinData{}; + } + } +} +} // namespace RotatingPlatform \ No newline at end of file diff --git a/src/Groundstation/RotatingPlatform/PinHandler/PinHandler.h b/src/Groundstation/RotatingPlatform/PinHandler/PinHandler.h new file mode 100644 index 0000000000000000000000000000000000000000..cb818b57c204206e68977b0169b887c3dfe34cc6 --- /dev/null +++ b/src/Groundstation/RotatingPlatform/PinHandler/PinHandler.h @@ -0,0 +1,106 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: Nicolò Caruso + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <Groundstation/Automated/PinHandler/PinData.h> +#include <Groundstation/Automated/Leds/Leds.h> +#include <Groundstation/RotatingPlatform/Actuators/Actuators.h> +#include <diagnostic/PrintLogger.h> +#include <scheduler/TaskScheduler.h> +#include <utils/DependencyManager/DependencyManager.h> +#include <utils/PinObserver/PinObserver.h> + +namespace RotatingPlatform +{ + +namespace PinHandlerConfig +{ +constexpr unsigned int ARM_SWITCH_THRESHOLD = 1; +constexpr unsigned int ACTIVE_SWITCH_THRESHOLD = 1; +} // namespace PinHandlerConfig + +struct PinChangeData +{ + uint64_t timestamp = 0; + uint8_t pinID = 0; + uint32_t changesCount = 0; + + PinChangeData(uint64_t timestamp, uint8_t pinID, uint32_t changesCount) + : timestamp(timestamp), pinID(pinID), changesCount(changesCount) + { + } + + PinChangeData() : PinChangeData{0, 0, 0} {} + + static std::string header() { return "timestamp,pinID,changesCount\n"; } + + void print(std::ostream& os) const + { + os << timestamp << "," << (int)pinID << "," << (int)changesCount + << "\n"; + } +}; + +class PinHandler : public Boardcore::InjectableWithDeps<Actuators, Antennas::Leds> +{ +public: + enum PinList : uint8_t + { + ARM_SWITCH, + ACTIVE_SWITCH + }; + + PinHandler(); + + /** + * @brief Starts the PinObserver module thread + */ + bool start(); + + /** + * @brief Checks if the module has started correctly + */ + bool isStarted(); + + /** + * @brief Called when the arm switch has been flipped + */ + void onArmTransition(Boardcore::PinTransition transition); + + /** + * @brief Called when the active switch has been flipped + */ + void onActiveTransition(Boardcore::PinTransition transition); + + /** + * @brief Returns the status of the requested pin + */ + Boardcore::PinData getPinData(PinList pin); + +private: + Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("PinHandler"); + + Boardcore::TaskScheduler scheduler; + Boardcore::PinObserver pin_observer; +}; +} // namespace RotatingPlatform \ No newline at end of file diff --git a/src/Groundstation/RotatingPlatform/rotating-entry.cpp b/src/Groundstation/RotatingPlatform/rotating-entry.cpp new file mode 100644 index 0000000000000000000000000000000000000000..5610fd0bbb6067d3cb806d3ea5235ec5cfd08975 --- /dev/null +++ b/src/Groundstation/RotatingPlatform/rotating-entry.cpp @@ -0,0 +1,137 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Authors: Nicolò Caruso + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <Groundstation/Automated/Leds/Leds.h> +#include <Groundstation/LyraGS/Buses.h> +#include <Groundstation/RotatingPlatform/Actuators/Actuators.h> +#include <Groundstation/RotatingPlatform/Actuators/ActuatorsConfig.h> +#include <Groundstation/RotatingPlatform/PinHandler/PinHandler.h> +#include <common/Events.h> +#include <drivers/timer/PWM.h> +#include <drivers/timer/TimerUtils.h> +#include <interfaces-impl/hwmapping.h> + +#include <functional> + +using namespace Boardcore; +using namespace std; +using namespace std::placeholders; +using namespace Common; + +using namespace Antennas; +using namespace LyraGS; +using namespace Boardcore; +using namespace miosix; +using namespace RotatingPlatform; + +// Flags indicating if the multipliers have been set +static constexpr float MULTIPLIER_X = 2.2f; // Multiplier for the stepper X +static constexpr uint8_t ARM_SWITCH_THRESHOLD = 1; +static constexpr uint8_t FOLLOW_SWITCH_THRESHOLD = 1; + +void idleLoop() +{ + while (1) + { + Thread::wait(); + } +} + +/** + * @brief Blinking RED led at 5Hz + */ +void errorLoop() +{ + while (1) + { + led3On(); //< Turn on RED led (CU) + Thread::sleep(100); + led3Off(); + Thread::sleep(100); + } +} + +/** + * @brief Entrypoint for the rotational platform. To use it, is needed the + * command box of ARP. The arm switch arms the actuators while the follow one + * activates the rotation with constant speed (except for a linearly accelerated + * start and decelerated end) + * + * @note The rotation is imposed by a configuration parameter given by + * constructor or, in this case, by the ActuatorsData stepper config. + * + * @attention the LED color code is the following: `RED` always `fast blink`; + * `YELLOW` `fast blink`: disarmed, `Y-on`: armed; `BLUE` `fast blink`: not + * rotating, `B-on`: rotating + */ +int main() +{ + DependencyManager manager; + PrintLogger logger = Logging::getLogger("rotation_test"); + TaskScheduler* scheduler_high = new TaskScheduler(); + TaskScheduler* scheduler_low = new TaskScheduler(0); + + Buses* buses = new Buses(); + RotatingPlatform::Actuators* actuators = new RotatingPlatform::Actuators(); + Antennas::Leds* leds = new Antennas::Leds(scheduler_low); + + bool ok = true; + + ok &= manager.insert(buses); + ok &= manager.insert(leds); + ok &= manager.insert(actuators); + + if (!ok) + errorLoop(); + + if (!manager.inject()) + { + std::cout << "[error] Failed to inject the dependencies!" << std::endl; + errorLoop(); + } + + if (!Logger::getInstance().start()) + { + std::cout << "ERROR: Failed to start Logger" << std::endl; + ok = false; + } + + if (!scheduler_high->start()) + { + std::cout << "[error] Failed to start scheduler_high!" << std::endl; + ok = false; + } + + if (!scheduler_low->start()) + { + std::cout << "[error] Failed to start scheduler_low!" << std::endl; + ok = false; + } + + // Fast blink to make the operator aware that this is not an ARP + // entrypoint (lyra-gs-entry) + leds->setFastBlink(LedColor::RED); + leds->setFastBlink(LedColor::YELLOW); + leds->setFastBlink(LedColor::BLUE); + + idleLoop(); +} \ No newline at end of file