From 6d021d5cc75b081541041e0657d47228d8f2cd6d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <niccolo.caruso@skywarder.eu>
Date: Tue, 10 Dec 2024 17:09:06 +0100
Subject: [PATCH] [ARP-RP] Fixed the entrypoint and actuator
<estions
---
.../RotatingPlatform/Actuators/Actuators.cpp | 54 +++++++++++--------
.../RotatingPlatform/Actuators/Actuators.h | 9 ++++
.../PinHandler/PinHandler.cpp | 4 ++
.../RotatingPlatform/rotating-entry.cpp | 26 +++++++++
4 files changed, 72 insertions(+), 21 deletions(-)
diff --git a/src/Groundstation/RotatingPlatform/Actuators/Actuators.cpp b/src/Groundstation/RotatingPlatform/Actuators/Actuators.cpp
index 3872d379b..6210a7718 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 5a1b1e03a..9102e8a50 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 6e513e1f1..27c5de06d 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 5610fd0bb..71af04c40 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
--
GitLab