diff --git a/src/Parafoil/Actuators/Actuators.cpp b/src/Parafoil/Actuators/Actuators.cpp new file mode 100644 index 0000000000000000000000000000000000000000..7b25f46ecf543af56057ec9e326a0dbc859e6b14 --- /dev/null +++ b/src/Parafoil/Actuators/Actuators.cpp @@ -0,0 +1,186 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: Davide Basso + * + * 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 <Parafoil/BoardScheduler.h> +#include <Parafoil/Configs/ActuatorsConfig.h> +#include <interfaces-impl/hwmapping.h> + +using namespace std::chrono; +using namespace miosix; +using namespace Boardcore; +namespace config = Parafoil::Config::Actuators; + +namespace Parafoil +{ + +Actuators::Actuators() +{ + leftServo.servo = std::make_unique<Servo>( + config::LeftServo::TIMER, config::LeftServo::PWM_CH, + config::LeftServo::MIN_PULSE.count(), + config::LeftServo::MAX_PULSE.count()); + leftServo.fullRangeAngle = config::LeftServo::ROTATION.value(); + + rightServo.servo = std::make_unique<Servo>( + config::RightServo::TIMER, config::RightServo::PWM_CH, + config::RightServo::MIN_PULSE.count(), + config::RightServo::MAX_PULSE.count()); + rightServo.fullRangeAngle = config::RightServo::ROTATION.value(); +} + +bool Actuators::start() +{ + using namespace std::chrono; + + auto& scheduler = getModule<BoardScheduler>()->actuators(); + + leftServo.servo->enable(); + rightServo.servo->enable(); + + leftServo.servo->setPosition(0); + rightServo.servo->setPosition(0); + + started = true; + return true; +} + +bool Actuators::isStarted() { return started; } + +bool Actuators::setServoPosition(ServosList servoId, float position) +{ + auto actuator = getServoActuator(servoId); + if (!actuator) + return false; + + miosix::Lock<miosix::FastMutex> lock(actuator->mutex); + + actuator->servo->setPosition(position); + + return true; +} + +bool Actuators::setServoAngle(ServosList servoId, float angle) +{ + auto actuator = getServoActuator(servoId); + if (!actuator) + return false; + + miosix::Lock<miosix::FastMutex> lock(actuator->mutex); + + actuator->servo->setPosition(angle / actuator->fullRangeAngle); + + return true; +} + +float Actuators::getServoPosition(ServosList servoId) +{ + auto actuator = getServoActuator(servoId); + if (!actuator) + return -1.f; + + miosix::Lock<miosix::FastMutex> lock(actuator->mutex); + + return actuator->servo->getPosition(); +} + +float Actuators::getServoAngle(ServosList servoId) +{ + auto actuator = getServoActuator(servoId); + if (!actuator) + return -1.f; + + miosix::Lock<miosix::FastMutex> lock(actuator->mutex); + + return actuator->servo->getPosition() * actuator->fullRangeAngle; +} + +bool Actuators::wiggleServo(ServosList servoId) +{ + auto actuator = getServoActuator(servoId); + if (!actuator) + return false; + + miosix::Lock<miosix::FastMutex> lock(actuator->mutex); + + actuator->servo->setPosition(1.0f); + Thread::sleep(1000); + actuator->servo->setPosition(0.0f); + + return true; +} + +bool Actuators::enableServo(ServosList servoId) +{ + auto actuator = getServoActuator(servoId); + if (!actuator) + return false; + + miosix::Lock<miosix::FastMutex> lock(actuator->mutex); + + actuator->servo->enable(); + + return true; +} + +bool Actuators::disableServo(ServosList servoId) +{ + auto actuator = getServoActuator(servoId); + if (!actuator) + return false; + + miosix::Lock<miosix::FastMutex> lock(actuator->mutex); + + actuator->servo->disable(); + + return true; +} + +Actuators::ServoActuator* Actuators::getServoActuator(ServosList servoId) +{ + switch (servoId) + { + case PARAFOIL_LEFT_SERVO: + assert(leftServo.servo); + return &leftServo; + case PARAFOIL_RIGHT_SERVO: + assert(rightServo.servo); + return &rightServo; + default: + return nullptr; + } +} + +void Actuators::startTwirl() +{ + leftServo.servo->setPosition(config::SERVO_TWIRL_RADIUS); + rightServo.servo->setPosition(0); +} + +void Actuators::stopTwirl() +{ + leftServo.servo->setPosition(0); + rightServo.servo->setPosition(0); +} + +} // namespace Parafoil diff --git a/src/Parafoil/Actuators/Actuators.h b/src/Parafoil/Actuators/Actuators.h new file mode 100644 index 0000000000000000000000000000000000000000..524a42edef2ebbfd84539a18f5d16139619cc982 --- /dev/null +++ b/src/Parafoil/Actuators/Actuators.h @@ -0,0 +1,138 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Authors: Federico Lolli, Angelo Prete, Niccolò Betto, Davide Basso + * + * 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/Servo/Servo.h> +#include <common/MavlinkOrion.h> +#include <utils/DependencyManager/DependencyManager.h> + +#include <utils/ModuleManager/ModuleManager.hpp> + +namespace Parafoil +{ +class BoardScheduler; + +class Actuators : public Boardcore::InjectableWithDeps<BoardScheduler> +{ +public: + /** + * @brief A small struct to hold a servo along with per-servo data. + */ + struct ServoActuator + { + std::unique_ptr<Boardcore::Servo> servo; + float fullRangeAngle; ///< The full range of the servo [degrees] + miosix::FastMutex mutex; + }; + + Actuators(); + + [[nodiscard]] bool start(); + + bool isStarted(); + + /** + * @brief Moves the specified servo to the given position. + * + * @param servoId Servo to move. + * @param percentage Angle to set [0-1]. + * @return True if the the angle was set. + */ + bool setServoPosition(ServosList servoId, float percentage); + + /** + * @brief Moves the specified servo to the given position. + * + * @param servoId Servo to move. + * @param angle Angle to set [degree]. + * @return True if the the angle was set. + */ + bool setServoAngle(ServosList servoId, float angle); + + /** + * @brief Wiggles the servo for few seconds. + * + * @param servoId Servo to move. + * @return true + * @return false + */ + bool wiggleServo(ServosList servoId); + + /** + * @brief Enables the specified servo. + * + * @param servoId Servo to enable. + * @return True if the servo was enabled. + * @return False if the servoId is invalid. + */ + bool enableServo(ServosList servoId); + + /** + * @brief Disables the specified servo. + * + * @param servoId Servo to disable. + * @return True if the servo was disabled. + * @return False if the servoId is invalid. + */ + bool disableServo(ServosList servoId); + + /** + * @brief Returns the current position of the specified servo. + * + * @param servoId Servo to read. + * @return float current Servo position in range [0-1], (-1) if the servoId + * is invalid. + */ + float getServoPosition(ServosList servoId); + + /** + * @brief Returns the current angle of the specified servo. + * + * @param servoId Servo to read. + * @return float current Servo angle in range [0-180], (-1) if the servoId + * is invalid. + */ + float getServoAngle(ServosList servoId); + + /** + * @brief Starts twirl (one servo is set to 0 and the other one is not). + */ + void startTwirl(); + + /** + * @brief Stops twirling (both servos are set to 0). + */ + void stopTwirl(); + +private: + ServoActuator* getServoActuator(ServosList servoId); + + ServoActuator leftServo; + ServoActuator rightServo; + + std::atomic<bool> started{false}; + + Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("Actuators"); +}; + +} // namespace Parafoil diff --git a/src/Parafoil/BoardScheduler.h b/src/Parafoil/BoardScheduler.h index cd331b343cb1718e8df0ba101324af21c60e2f67..75a8d06c7a57af2d3cccded0f97c2adf9eae997c 100644 --- a/src/Parafoil/BoardScheduler.h +++ b/src/Parafoil/BoardScheduler.h @@ -55,6 +55,7 @@ public: Boardcore::TaskScheduler& nasController() { return critical; } Boardcore::TaskScheduler& sensors() { return high; } Boardcore::TaskScheduler& pinHandler() { return high; } + Boardcore::TaskScheduler& actuators() { return low; } static Priority::PriorityLevel flightModeManagerPriority() { diff --git a/src/Parafoil/Configs/ActuatorsConfig.h b/src/Parafoil/Configs/ActuatorsConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..915a2d757802064cdf08e1293ff7002a0b214f47 --- /dev/null +++ b/src/Parafoil/Configs/ActuatorsConfig.h @@ -0,0 +1,68 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: Davide Basso + * + * 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/TimerUtils.h> +#include <units/Angle.h> + +#include <chrono> + +namespace Parafoil +{ +namespace Config +{ +namespace Actuators +{ + +/* linter off */ using namespace std::chrono_literals; +/* linter off */ using namespace Boardcore::Units::Angle; + +namespace LeftServo +{ +static TIM_TypeDef* const TIMER = TIM4; +constexpr Boardcore::TimerUtils::Channel PWM_CH = + Boardcore::TimerUtils::Channel::CHANNEL_1; + +constexpr auto ROTATION = 180_deg; +constexpr auto MIN_PULSE = 500us; +constexpr auto MAX_PULSE = 2460us; +} // namespace LeftServo + +namespace RightServo +{ +static TIM_TypeDef* const TIMER = TIM8; +constexpr Boardcore::TimerUtils::Channel PWM_CH = + Boardcore::TimerUtils::Channel::CHANNEL_2; + +constexpr auto ROTATION = 180_deg; +constexpr auto MIN_PULSE = 2460us; +constexpr auto MAX_PULSE = 500us; +} // namespace RightServo + +constexpr auto SERVO_TWIRL_RADIUS = 0.5f; // [%] + +// Status configs + +} // namespace Actuators +} // namespace Config +} // namespace Parafoil diff --git a/src/Parafoil/Configs/SensorsConfig.h b/src/Parafoil/Configs/SensorsConfig.h index d5906dd378db620a635978c4aa7a38ff0b72a2a6..3147326619b4fd54a39ed6346670f5c5661301ca 100644 --- a/src/Parafoil/Configs/SensorsConfig.h +++ b/src/Parafoil/Configs/SensorsConfig.h @@ -126,4 +126,4 @@ constexpr auto CALIBRATION_PATH = "/sd/bmx160_magnetometer_calibration.csv"; } // namespace Config -} // namespace Parafoil \ No newline at end of file +} // namespace Parafoil diff --git a/src/Parafoil/parafoil-entry.cpp b/src/Parafoil/parafoil-entry.cpp index 0b5451b3f21aa99ced8bdbfd34373ff5ecf9679b..da56cf30367c899643e4c84942705789616ac10d 100644 --- a/src/Parafoil/parafoil-entry.cpp +++ b/src/Parafoil/parafoil-entry.cpp @@ -20,11 +20,13 @@ * THE SOFTWARE. */ +#include <Parafoil/Actuators/Actuators.h> #include <Parafoil/BoardScheduler.h> #include <Parafoil/Buses.h> #include <Parafoil/PinHandler/PinHandler.h> #include <Parafoil/Sensors/Sensors.h> #include <Parafoil/StateMachines/FlightModeManager/FlightModeManager.h> +#include <Parafoil/StateMachines/NASController/NASController.h> #include <common/Events.h> #include <common/Topics.h> #include <diagnostic/PrintLogger.h> @@ -98,6 +100,10 @@ int main() auto flightModeManager = new FlightModeManager(); initResult &= depman.insert(flightModeManager); + // Attitude estimation + auto nasController = new NASController(); + initResult &= depman.insert(nasController); + // Sensors auto sensors = new Sensors(); initResult &= depman.insert(sensors); @@ -106,7 +112,11 @@ int main() // TODO: Radio - // TODO: Flight algorithms + // TODO: Wing Algorithm + + // Actuators + auto actuators = new Actuators(); + initResult &= depman.insert(actuators); START_SINGLETON(Logger) { @@ -116,15 +126,13 @@ int main() } START_MODULE(flightModeManager); - // START_MODULE(pinHandler); + START_MODULE(pinHandler); // START_MODULE(radio) { miosix::led2On(); } - // START_MODULE(canHandler) { miosix::led3On(); } - // START_MODULE(nasController); - // START_MODULE(altitudeTrigger); + START_MODULE(nasController); // START_MODULE(wingController); - // START_MODULE(actuators); + START_MODULE(actuators); - // START_MODULE(scheduler); + START_MODULE(scheduler); START_MODULE(sensors); @@ -133,13 +141,11 @@ int main() EventBroker::getInstance().post(FMM_INIT_OK, TOPIC_FMM); // Turn on the initialization led on the CU miosix::ledOn(); - // TODO: actuators->setStatusOk(); std::cout << "Payload initialization Ok!" << std::endl; } else { EventBroker::getInstance().post(FMM_INIT_ERROR, TOPIC_FMM); - // TODO: actuators->setStatusError(); std::cerr << "*** Payload initialization error ***" << std::endl; }