diff --git a/src/boards/Parafoil/Configs/WingConfig.h b/src/boards/Parafoil/Configs/WingConfig.h index 0aa8d462db34576dd2bb8d43a0db6400d9080b36..3aba0fcf013ef402870d0d11d892312c53d8ecb5 100644 --- a/src/boards/Parafoil/Configs/WingConfig.h +++ b/src/boards/Parafoil/Configs/WingConfig.h @@ -22,6 +22,8 @@ #pragma once +#include <utils/Constants.h> + #include <ostream> namespace Parafoil @@ -45,16 +47,16 @@ constexpr int SELECTED_ALGORITHM = 0; constexpr float DEFAULT_TARGET_LAT = 45.565835; constexpr float DEFAULT_TARGET_LON = 12.577307; #else // Milan -constexpr float DEFAULT_TARGET_LAT = 45.501148; -constexpr float DEFAULT_TARGET_LON = 9.156301; +constexpr float DEFAULT_TARGET_LAT = 45.565835; +constexpr float DEFAULT_TARGET_LON = 12.577307; #endif constexpr int WING_STRAIGHT_FLIGHT_TIMEOUT = 15 * 1000; // [ms] constexpr int WING_UPDATE_PERIOD = 1000; // [ms] -constexpr float PI_CONTROLLER_SATURATION_MAX_LIMIT = 1; -constexpr float PI_CONTROLLER_SATURATION_MIN_LIMIT = -1; +constexpr float PI_CONTROLLER_SATURATION_MAX_LIMIT = Boardcore::Constants::PI; +constexpr float PI_CONTROLLER_SATURATION_MIN_LIMIT = -Boardcore::Constants::PI; constexpr int GUIDANCE_CONFIDENCE = 15; constexpr int GUIDANCE_M1_ALTITUDE_THRESHOLD = 250; //[m] @@ -62,8 +64,8 @@ constexpr int GUIDANCE_M2_ALTITUDE_THRESHOLD = 150; //[m] constexpr int GUIDANCE_TARGET_ALTITUDE_THRESHOLD = 50; //[m] // TODO check this parameter preflight -constexpr float KP = 0.0075; -constexpr float KI = 0.0007; +constexpr float KP = 1.3537; +constexpr float KI = 0.0111; constexpr float ALTITUDE_TRIGGER_DEPLOYMENT_ALTITUDE = 300; // [meters] constexpr int ALTITUDE_TRIGGER_CONFIDENCE = 10; // [number of sample] diff --git a/src/boards/Parafoil/Wing/AutomaticWingAlgorithm.cpp b/src/boards/Parafoil/Wing/AutomaticWingAlgorithm.cpp index a8cdb069fdc62bbf4c83e604d35354672bbf9f64..963b91d1dbdf65ed5c1e9081587f2b83595b2f6d 100644 --- a/src/boards/Parafoil/Wing/AutomaticWingAlgorithm.cpp +++ b/src/boards/Parafoil/Wing/AutomaticWingAlgorithm.cpp @@ -76,14 +76,14 @@ void AutomaticWingAlgorithm::step() if (result > 0) { // Activate the servo2 and reset servo1 - modules.get<Actuators>()->setServo(servo1, 0); - modules.get<Actuators>()->setServo(servo2, result); + modules.get<Actuators>()->setServoAngle(servo1, 0); + modules.get<Actuators>()->setServoAngle(servo2, result); } else { // Activate the servo1 and reset servo2 - modules.get<Actuators>()->setServo(servo1, result * -1); - modules.get<Actuators>()->setServo(servo2, 0); + modules.get<Actuators>()->setServoAngle(servo1, result * -1); + modules.get<Actuators>()->setServoAngle(servo2, 0); } // Log the servo positions @@ -134,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;