From de1365ea7bec01a8c997b31b51b2fcae7586b210 Mon Sep 17 00:00:00 2001
From: Federico Mandelli <federicomandelli@skywarder.eu>
Date: Tue, 12 Mar 2024 22:20:31 +0100
Subject: [PATCH] [AutomaticWing] Better defined convection for the PI output
---
.../Parafoil/Wing/AutomaticWingAlgorithm.cpp | 49 +++++++------------
1 file changed, 17 insertions(+), 32 deletions(-)
diff --git a/src/boards/Parafoil/Wing/AutomaticWingAlgorithm.cpp b/src/boards/Parafoil/Wing/AutomaticWingAlgorithm.cpp
index e56bd236e..a8cdb069f 100644
--- a/src/boards/Parafoil/Wing/AutomaticWingAlgorithm.cpp
+++ b/src/boards/Parafoil/Wing/AutomaticWingAlgorithm.cpp
@@ -65,17 +65,25 @@ void AutomaticWingAlgorithm::step()
modules.get<WindEstimation>()->getWindEstimationScheme());
// Actuate the result
- if (result >= 0 && result <= 1)
- {
- // Activate the servo1 and reset servo2
- modules.get<Actuators>()->setServo(servo1, result);
- modules.get<Actuators>()->setServo(servo2, 0);
- }
- else if (result < 0 && result <= -1)
+ // To see how to interpret the PI output
+ // https://www.geogebra.org/calculator/xrhwarpz
+ // to system is
+ /* N ^
+ |
+ |
+ ----> E
+ */
+ if (result > 0)
{
// Activate the servo2 and reset servo1
modules.get<Actuators>()->setServo(servo1, 0);
- modules.get<Actuators>()->setServo(servo2, result * -1);
+ modules.get<Actuators>()->setServo(servo2, result);
+ }
+ else
+ {
+ // Activate the servo1 and reset servo2
+ modules.get<Actuators>()->setServo(servo1, result * -1);
+ modules.get<Actuators>()->setServo(servo2, 0);
}
// Log the servo positions
@@ -116,29 +124,6 @@ float AutomaticWingAlgorithm::algorithmStep(NASState state, Vector2f windNED)
// All angle are computed as angle from the north direction
- // if (relativeVelocity[0] == 0 && relativeVelocity[1] == 0)
- // {
- // // If we are not moving velocityAngle is 0
- // velocityAngle = 0;
- // }
- // else if (relativeVelocity[0] == 0)
- // {
- // // If we are not moving in the N S axis we interpret velocity[1]
- // velocityAngle = (relativeVelocity[1] > 0 ? 1 : -1) * Constants::PI /
- // 2;
- // }
- // else if (relativeVelocity[1] == 0)
- // {
- // // If we are not moving in the E O axis we interpret velocity[0]
- // velocityAngle = (relativeVelocity[0] > 0 ? 0 : 1) * Constants::PI;
- // }
- // else
- // {
- // // angle in radiants of the velocity
- // velocityAngle = atan2(relativeVelocity[1], relativeVelocity[0]);
- // }
-
- // TODO test if this is safe
velocityAngle = atan2(relativeVelocity[1], relativeVelocity[0]);
// Compute the angle difference
@@ -149,7 +134,7 @@ float AutomaticWingAlgorithm::algorithmStep(NASState state, Vector2f windNED)
result = controller->update(error);
// Convert the result from radians back to degrees
- result = result * (180.f / Constants::PI);
+ // result = result * (180.f / Constants::PI);
// Flip the servo orientation
// result *= -1;
--
GitLab