diff --git a/src/boards/Groundstation/Automated/Actuators/Actuators.cpp b/src/boards/Groundstation/Automated/Actuators/Actuators.cpp index 3899485f7c8f0801d273f0d2b6170c06094d1fa5..2b69c619843b5ff7f9e6d0bca12ea1129ccd98ec 100644 --- a/src/boards/Groundstation/Automated/Actuators/Actuators.cpp +++ b/src/boards/Groundstation/Automated/Actuators/Actuators.cpp @@ -1,5 +1,5 @@ -/* Copyright (c) 2023 Skyward Experimental Rocketry - * Author: Emilio Corigliano +/* Copyright (c) 2023-2024 Skyward Experimental Rocketry + * Author: 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 @@ -151,7 +151,7 @@ float Actuators::getCurrentDegPosition(StepperList axis) return 0; } -void Actuators::move(StepperList axis, int16_t steps) +ErrorMovement Actuators::move(StepperList axis, int16_t steps) { float microstepping = 1.0; float step_angle = 1.8; @@ -170,10 +170,11 @@ void Actuators::move(StepperList axis, int16_t steps) assert(false && "Non existent stepper"); break; } - moveDeg(axis, static_cast<float>(steps) * step_angle / microstepping); + return moveDeg(axis, + static_cast<float>(steps) * step_angle / microstepping); } -void Actuators::moveDeg(StepperList axis, float degrees) +ErrorMovement Actuators::moveDeg(StepperList axis, float degrees) { if (emergencyStop) { @@ -192,17 +193,24 @@ void Actuators::moveDeg(StepperList axis, float degrees) break; } - return; + return ErrorMovement::EMERGENCY_STOP; } + ErrorMovement actuationState = + ErrorMovement::OK; //< In case the move command is not limited float positionDeg = getCurrentDegPosition(axis); switch (axis) { case StepperList::STEPPER_X: + + if (!stepperX.isEnabled()) + return ErrorMovement::DISABLED; + // LIMIT POSITION IN ACCEPTABLE RANGE if (positionDeg + degrees > Config::MAX_ANGLE_HORIZONTAL) { - degrees = Config::MAX_ANGLE_HORIZONTAL - positionDeg; + degrees = Config::MAX_ANGLE_HORIZONTAL - positionDeg; + actuationState = ErrorMovement::LIMIT; } else if (positionDeg + degrees < Config::MIN_ANGLE_HORIZONTAL) { @@ -215,10 +223,15 @@ void Actuators::moveDeg(StepperList axis, float degrees) deltaX = degrees; break; case StepperList::STEPPER_Y: + + if (!stepperY.isEnabled()) + return ErrorMovement::DISABLED; + // LIMIT POSITION IN ACCEPTABLE RANGE if (positionDeg + degrees > Config::MAX_ANGLE_VERTICAL) { - degrees = Config::MAX_ANGLE_VERTICAL - positionDeg; + degrees = Config::MAX_ANGLE_VERTICAL - positionDeg; + actuationState = ErrorMovement::LIMIT; } else if (positionDeg + degrees < Config::MIN_ANGLE_VERTICAL) { @@ -232,11 +245,13 @@ void Actuators::moveDeg(StepperList axis, float degrees) break; default: assert(false && "Non existent stepper"); + actuationState = ErrorMovement::NO_STEPPER; break; } + return actuationState; } -void Actuators::setPosition(StepperList axis, int16_t steps) +ErrorMovement Actuators::setPosition(StepperList axis, int16_t steps) { float microstepping = 1.0; @@ -244,25 +259,30 @@ void Actuators::setPosition(StepperList axis, int16_t steps) switch (axis) { case StepperList::STEPPER_X: + if (!stepperX.isEnabled()) + return ErrorMovement::DISABLED; microstepping = static_cast<float>(Config::HORIZONTAL_MICROSTEPPING); step_angle = Config::HORIZONTAL_STEP_ANGLE; break; case StepperList::STEPPER_Y: + if (!stepperY.isEnabled()) + return ErrorMovement::DISABLED; microstepping = static_cast<float>(Config::VERTICAL_MICROSTEPPING); step_angle = Config::VERTICAL_STEP_ANGLE; break; default: assert(false && "Non existent stepper"); + return ErrorMovement::NO_STEPPER; break; } - setPositionDeg(axis, - static_cast<float>(steps) * step_angle / microstepping); + return setPositionDeg( + axis, static_cast<float>(steps) * step_angle / microstepping); } -void Actuators::setPositionDeg(StepperList axis, float positionDeg) +ErrorMovement Actuators::setPositionDeg(StepperList axis, float positionDeg) { - moveDeg(axis, positionDeg - getCurrentDegPosition(axis)); + return moveDeg(axis, positionDeg - getCurrentDegPosition(axis)); } void Actuators::IRQemergencyStop() diff --git a/src/boards/Groundstation/Automated/Actuators/Actuators.h b/src/boards/Groundstation/Automated/Actuators/Actuators.h index 84243dac0c5b368bd076ba191c9244b71f00056e..c78b9e7ab3614c0a0671280c8d62c56e282a7f21 100644 --- a/src/boards/Groundstation/Automated/Actuators/Actuators.h +++ b/src/boards/Groundstation/Automated/Actuators/Actuators.h @@ -1,5 +1,5 @@ -/* Copyright (c) 2023 Skyward Experimental Rocketry - * Author: Emilio Corigliano +/* Copyright (c) 2023-2024 Skyward Experimental Rocketry + * Author: 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 @@ -39,6 +39,20 @@ namespace Antennas { + +/** + * @brief Error handling enum for the stepper movement + */ +enum class ErrorMovement : uint8_t +{ + OK, ///< `0` + LIMIT, ///< `1` The STEPPER_X reached its actuation limits + NOT_TEST, ///< `2` Such movement is allowed only in test + NO_STEPPER, ///< `3` The specified stepper does not exist + DISABLED, ///< `4` + EMERGENCY_STOP, ///< `5` +}; + class Actuators : public Boardcore::Module { public: @@ -54,10 +68,10 @@ public: void setSpeed(StepperList axis, float speed); - void move(StepperList axis, int16_t steps); - void moveDeg(StepperList axis, float degrees); - void setPosition(StepperList axis, int16_t steps); - void setPositionDeg(StepperList axis, float degrees); + ErrorMovement move(StepperList axis, int16_t steps); + ErrorMovement moveDeg(StepperList axis, float degrees); + ErrorMovement setPosition(StepperList axis, int16_t steps); + ErrorMovement setPositionDeg(StepperList axis, float degrees); void zeroPosition(); diff --git a/src/boards/Groundstation/Automated/Follower/Follower.cpp b/src/boards/Groundstation/Automated/Follower/Follower.cpp index cfc0c96d22f2cd0cff389e5b9e2eaeb98762749a..0a5276059b8837df921f4f07e2eea5c7334733c3 100644 --- a/src/boards/Groundstation/Automated/Follower/Follower.cpp +++ b/src/boards/Groundstation/Automated/Follower/Follower.cpp @@ -1,5 +1,5 @@ /* Copyright (c) 2024 Skyward Experimental Rocketry - * Author: Emilio Corigliano, Niccolò Betto, Federico Lolli + * Author: Emilio Corigliano, Niccolò Betto, Federico Lolli, 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 @@ -152,10 +152,24 @@ void Follower::step() StepperList::STEPPER_Y, verticalSpeed); // Actuating steppers - ModuleManager::getInstance().get<Actuators>()->moveDeg( - StepperList::STEPPER_X, stepperAngles.yaw); - ModuleManager::getInstance().get<Actuators>()->moveDeg( + ErrorMovement actuation = + ModuleManager::getInstance().get<Actuators>()->moveDeg( + StepperList::STEPPER_X, stepperAngles.yaw); + + if (actuation != ErrorMovement::OK) + LOG_ERR(logger, + "Step antenna - STEPPER_X could not move or reached move " + "limit. Error: ", + actuation, "\n"); + + actuation = ModuleManager::getInstance().get<Actuators>()->moveDeg( StepperList::STEPPER_Y, stepperAngles.pitch); + + if (actuation != ErrorMovement::OK) + LOG_ERR(logger, + "Step antenna - STEPPER_Y could not move or reached move " + "limit. Error: ", + actuation, "\n"); } AntennaAngles Follower::rocketPositionToAntennaAngles( diff --git a/src/boards/Groundstation/Automated/Hub.cpp b/src/boards/Groundstation/Automated/Hub.cpp index d1b3d9845b82ac537d09bf514bd22d968b062e65..caa0dd88a2ccda4bce3278bec1ac85865aa3e223 100644 --- a/src/boards/Groundstation/Automated/Hub.cpp +++ b/src/boards/Groundstation/Automated/Hub.cpp @@ -1,5 +1,5 @@ /* Copyright (c) 2024 Skyward Experimental Rocketry - * Author: Davide Mor, Federico Lolli + * Author: Davide Mor, Federico Lolli, 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 @@ -94,8 +94,12 @@ void Hub::dispatchOutgoingMsg(const mavlink_message_t& msg) float angle = mavlink_msg_set_stepper_angle_tc_get_angle(&msg); // The stepper is moved of 'angle' degrees - modules.get<SMController>()->moveStepperDeg(stepperId, angle); - sendAck(msg); + ErrorMovement moved = + modules.get<SMController>()->moveStepperDeg(stepperId, angle); + if (moved == ErrorMovement::OK) + sendAck(msg); + else + sendNack(msg); break; } case MAVLINK_MSG_ID_SET_STEPPER_STEPS_TC: @@ -106,8 +110,12 @@ void Hub::dispatchOutgoingMsg(const mavlink_message_t& msg) int16_t steps = mavlink_msg_set_stepper_steps_tc_get_steps(&msg); // The stepper is moved of 'steps' steps - modules.get<SMController>()->moveStepperSteps(stepperId, steps); - sendAck(msg); + ErrorMovement moved = + modules.get<SMController>()->moveStepperSteps(stepperId, steps); + if (moved == ErrorMovement::OK) + sendAck(msg); + else + sendNack(msg); break; } case MAVLINK_MSG_ID_SET_ROCKET_COORDINATES_ARP_TC: diff --git a/src/boards/Groundstation/Automated/SMController/SMController.cpp b/src/boards/Groundstation/Automated/SMController/SMController.cpp index 58676053ac4fee294d5eeb8cbccb2977fc580add..b6204506871cfa0ce1e46ff0e2edc75ff36a2a10 100644 --- a/src/boards/Groundstation/Automated/SMController/SMController.cpp +++ b/src/boards/Groundstation/Automated/SMController/SMController.cpp @@ -1,5 +1,5 @@ /* Copyright (c) 2024 Skyward Experimental Rocketry - * Author: Federico Lolli + * Author: Federico Lolli, 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 @@ -112,30 +112,34 @@ void SMController::setInitialRocketCoordinates( } } -void SMController::moveStepperDeg(StepperList stepperId, float angle) +ErrorMovement SMController::moveStepperDeg(StepperList stepperId, float angle) { if (!testState(&SMController::state_test) && !testState(&SMController::state_test_nf)) { LOG_ERR(logger, "Stepper can only be manually moved in the TEST state"); + return ErrorMovement::NOT_TEST; } else { - ModuleManager::getInstance().get<Actuators>()->moveDeg(stepperId, - angle); + return ModuleManager::getInstance().get<Actuators>()->moveDeg(stepperId, + angle); } } -void SMController::moveStepperSteps(StepperList stepperId, int16_t steps) +ErrorMovement SMController::moveStepperSteps(StepperList stepperId, + int16_t steps) { if (!testState(&SMController::state_test) && !testState(&SMController::state_test_nf)) { LOG_ERR(logger, "Stepper can only be manually moved in the TEST state"); + return ErrorMovement::NOT_TEST; } else { - ModuleManager::getInstance().get<Actuators>()->move(stepperId, steps); + return ModuleManager::getInstance().get<Actuators>()->move(stepperId, + steps); } } diff --git a/src/boards/Groundstation/Automated/SMController/SMController.h b/src/boards/Groundstation/Automated/SMController/SMController.h index 5eb7dcd692d4a199cbd72e6d8e8a7a695fd66ce1..2b751da75b52a6771e9457683b291896e40855eb 100644 --- a/src/boards/Groundstation/Automated/SMController/SMController.h +++ b/src/boards/Groundstation/Automated/SMController/SMController.h @@ -1,5 +1,5 @@ /* Copyright (c) 2024 Skyward Experimental Rocketry - * Author: Federico Lolli + * Author: Federico Lolli, 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 @@ -91,12 +91,12 @@ public: /** * @brief move the stepper `stepperId` of `angle` degrees */ - void moveStepperDeg(StepperList stepperId, float angle); + ErrorMovement moveStepperDeg(StepperList stepperId, float angle); /** * @brief move the stepper `stepperId` of `steps` steps */ - void moveStepperSteps(StepperList stepperId, int16_t steps); + ErrorMovement moveStepperSteps(StepperList stepperId, int16_t steps); /** * @brief Getter for follower target angles