diff --git a/src/Groundstation/RotatingPlatform/Actuators/Actuators.cpp b/src/Groundstation/RotatingPlatform/Actuators/Actuators.cpp index 3872d379b7a2552a2a282c46f61e4b6cac586bba..6210a7718b6c2939e2c924ca3a5de8b0b456105d 100644 --- a/src/Groundstation/RotatingPlatform/Actuators/Actuators.cpp +++ b/src/Groundstation/RotatingPlatform/Actuators/Actuators.cpp @@ -70,31 +70,43 @@ Actuators::Actuators(StepperConfig config) void Actuators::run() { + while (true) { - 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); + miosix::Lock<FastMutex> lock(rotationMutex); + + float stepSpeed = ACCELERATION * (TIME_WAIT_MS / 1000); + + // Acceleration/stable phase + if (stepperX.isEnabled() && isRotating) + { + if (speed < configX.MAX_SPEED && speed < MAX_MAX_SPEED) + speed += stepSpeed; + if (speed > configX.MAX_SPEED) + speed = configX.MAX_SPEED; + if (speed > MAX_MAX_SPEED) + speed = MAX_MAX_SPEED; + setSpeed(StepperList::STEPPER_X, speed); + std::cout << "Accelerate speed" << speed << std::endl; + } + // Deceleration phase + else if (stepperX.isEnabled()) + { + speed = speed - stepSpeed; + if (speed <= 0.0001) + speed = 0.0; + setSpeed(StepperList::STEPPER_X, speed); + std::cout << "Decelerate speed" << speed << std::endl; + } + else + { + speed = 0.0; + setSpeed(StepperList::STEPPER_X, 0.0); + } } + moveDeg(StepperList::STEPPER_X, 20); + Thread::sleep(TIME_WAIT_MS); } - Thread::sleep(100); } void Actuators::arm() { stepperX.enable(); } diff --git a/src/Groundstation/RotatingPlatform/Actuators/Actuators.h b/src/Groundstation/RotatingPlatform/Actuators/Actuators.h index 5a1b1e03ab59902fdfe1ba2b78f3c7927ffbb962..9102e8a50bd320d4d98171a66e57a69d61071e7e 100644 --- a/src/Groundstation/RotatingPlatform/Actuators/Actuators.h +++ b/src/Groundstation/RotatingPlatform/Actuators/Actuators.h @@ -23,6 +23,7 @@ #include <ActiveObject.h> #include <common/MavlinkLyra.h> +#include <diagnostic/PrintLogger.h> #include <logger/Logger.h> #include <utils/DependencyManager/DependencyManager.h> @@ -36,6 +37,10 @@ namespace RotatingPlatform { +static constexpr float MAX_MAX_SPEED = + 0.03; // Structurally not go more than this +static constexpr float ACCELERATION = 0.01; //[RPS^2] +static constexpr float TIME_WAIT_MS = 100; //[MS] /** * @brief Error handling enum for the stepper movement @@ -220,5 +225,9 @@ private: // Variables to enable the rotation bool isRotating = false; miosix::FastMutex rotationMutex; + + Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("Actuator"); + + float speed = 0; }; } // namespace RotatingPlatform diff --git a/src/Groundstation/RotatingPlatform/PinHandler/PinHandler.cpp b/src/Groundstation/RotatingPlatform/PinHandler/PinHandler.cpp index 6e513e1f1786dfb3c25683d262853d1348da2339..27c5de06d2388f162e7599a6f312b010f831ec86 100644 --- a/src/Groundstation/RotatingPlatform/PinHandler/PinHandler.cpp +++ b/src/Groundstation/RotatingPlatform/PinHandler/PinHandler.cpp @@ -57,6 +57,7 @@ void PinHandler::onArmTransition(PinTransition transition) { if (transition == Boardcore::PinTransition::RISING_EDGE) { + LOG_DEBUG(logger, "ARM!\n"); getModule<Antennas::Leds>()->setOn(Antennas::LedColor::YELLOW); getModule<Actuators>()->arm(); @@ -68,6 +69,7 @@ void PinHandler::onArmTransition(PinTransition transition) } else { + LOG_DEBUG(logger, "DISARM!\n"); getModule<Antennas::Leds>()->setFastBlink(Antennas::LedColor::YELLOW); getModule<Actuators>()->disarm(); @@ -83,6 +85,7 @@ void PinHandler::onActiveTransition(PinTransition transition) { if (transition == Boardcore::PinTransition::RISING_EDGE) { + LOG_DEBUG(logger, "SPIN TO WIN!\n"); getModule<Antennas::Leds>()->setOn(Antennas::LedColor::BLUE); getModule<Actuators>()->enableRotation(); @@ -95,6 +98,7 @@ void PinHandler::onActiveTransition(PinTransition transition) else { + LOG_DEBUG(logger, "NO SPIN!\n"); getModule<Antennas::Leds>()->setFastBlink(Antennas::LedColor::BLUE); getModule<Actuators>()->disableRotation(); diff --git a/src/Groundstation/RotatingPlatform/rotating-entry.cpp b/src/Groundstation/RotatingPlatform/rotating-entry.cpp index 5610fd0bbb6067d3cb806d3ea5235ec5cfd08975..71af04c40f0927adb91b37f40457159f46a837e4 100644 --- a/src/Groundstation/RotatingPlatform/rotating-entry.cpp +++ b/src/Groundstation/RotatingPlatform/rotating-entry.cpp @@ -93,12 +93,16 @@ int main() Buses* buses = new Buses(); RotatingPlatform::Actuators* actuators = new RotatingPlatform::Actuators(); Antennas::Leds* leds = new Antennas::Leds(scheduler_low); + PinHandler* pinHandler = new PinHandler(); bool ok = true; + std::cout << "[error] Hello!" << std::endl; + ok &= manager.insert(buses); ok &= manager.insert(leds); ok &= manager.insert(actuators); + ok &= manager.insert(pinHandler); if (!ok) errorLoop(); @@ -127,11 +131,33 @@ int main() ok = false; } + if (!leds->start()) + { + std::cout << "[error] Failed to start leds!" << 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); + if (actuators->start()) + { + LOG_INFO(logger, "[info] Actuators started!\n"); + } + else + std::cout << "[error] Failed to start actuators!" << std::endl; + + if (pinHandler->start()) + { + LOG_INFO(logger, "[info] Actuators started!\n"); + } + else + std::cout << "[error] Failed to start actuators!" << std::endl; + + std::cout << "[info] Ok!" << std::endl; + idleLoop(); } \ No newline at end of file