diff --git a/src/boards/Parafoil/Configs/WESConfig.h b/src/boards/Parafoil/Configs/WESConfig.h index 586462d587157da24665076246e7829792e2e77f..5fe0b701224305661b0490fc3113b46c53c996b4 100644 --- a/src/boards/Parafoil/Configs/WESConfig.h +++ b/src/boards/Parafoil/Configs/WESConfig.h @@ -33,6 +33,8 @@ namespace WESConfig constexpr uint32_t WES_CALIBRATION_TIMEOUT = 5 * 1000; // time needed for the first loop [ms] +constexpr uint32_t WES_ROTATION_PERIOD = 10 * 1000 * 1000; // [us] + constexpr int WES_CALIBRATION_SAMPLE_NUMBER = 20; // number to sample to take in the first loop constexpr uint32_t WES_CALIBRATION_UPDATE_PERIOD = diff --git a/src/boards/Parafoil/Configs/WingConfig.h b/src/boards/Parafoil/Configs/WingConfig.h index 06d0d393267cc7acae789bf4dd88ea9bff6bc5e6..1ea29f7ea76e24bd4e70a672ad04f7d7469e6db4 100644 --- a/src/boards/Parafoil/Configs/WingConfig.h +++ b/src/boards/Parafoil/Configs/WingConfig.h @@ -39,6 +39,8 @@ constexpr int SELECTED_ALGORITHM = 0; constexpr int SELECTED_ALGORITHM = 1; #elif SEQUENCE constexpr int SELECTED_ALGORITHM = 2; +#elif ROTATION +constexpr int SELECTED_ALGORITHM = 3; #else constexpr int SELECTED_ALGORITHM = 0; #endif diff --git a/src/boards/Parafoil/StateMachines/WingController/WingController.cpp b/src/boards/Parafoil/StateMachines/WingController/WingController.cpp index 46a119d4cef2303175298933568d730923929da7..f0542e62af41233747567654b41f4e583ba168e7 100644 --- a/src/boards/Parafoil/StateMachines/WingController/WingController.cpp +++ b/src/boards/Parafoil/StateMachines/WingController/WingController.cpp @@ -23,6 +23,7 @@ #include <Parafoil/Actuators/Actuators.h> #include <Parafoil/BoardScheduler.h> +#include <Parafoil/Configs/ActuatorsConfigs.h> #include <Parafoil/Configs/WESConfig.h> #include <Parafoil/Configs/WingConfig.h> #include <Parafoil/StateMachines/FlightModeManager/FlightModeManager.h> @@ -41,6 +42,7 @@ using namespace Boardcore; using namespace Parafoil::WingConfig; using namespace Parafoil::WESConfig; +using namespace Parafoil::ActuatorsConfigs; using namespace Common; using namespace miosix; @@ -288,6 +290,39 @@ bool WingController::addAlgorithms() // Add the algorithm to the vector algorithms.push_back(algorithm); + // Algorithm 3 (rotation) + algorithm = new WingAlgorithm(PARAFOIL_LEFT_SERVO, PARAFOIL_RIGHT_SERVO); + step.timestamp = 0; + step.servo1Angle = LEFT_SERVO_ROTATION / 2; + step.servo2Angle = 0; + algorithm->addStep(step); + step.timestamp += WES_ROTATION_PERIOD; // us + step.servo1Angle = 0; + step.servo2Angle = RIGHT_SERVO_ROTATION / 2; + algorithm->addStep(step); + step.timestamp += WES_ROTATION_PERIOD; // us + step.servo1Angle = 0; + step.servo2Angle = 0; + algorithm->addStep(step); + step.timestamp += WES_ROTATION_PERIOD; // us + step.servo1Angle = LEFT_SERVO_ROTATION; + step.servo2Angle = RIGHT_SERVO_ROTATION; + algorithm->addStep(step); + step.timestamp += WES_ROTATION_PERIOD; // us + step.servo1Angle = 0; + step.servo2Angle = RIGHT_SERVO_ROTATION; + algorithm->addStep(step); + step.timestamp += WES_ROTATION_PERIOD; // us + step.servo1Angle = 0; + step.servo2Angle = 0; + algorithm->addStep(step); + step.timestamp += WES_ROTATION_PERIOD; // us + step.servo1Angle = 0; + step.servo2Angle = 0; + algorithm->addStep(step); + result &= algorithm->init(); + algorithms.push_back(algorithm); + selectAlgorithm(SELECTED_ALGORITHM); return result;