From f129ad29d0924d9328e88c42f5cefb2ba7ee5775 Mon Sep 17 00:00:00 2001
From: Angelo Prete <angelo.prete@skywarder.eu>
Date: Tue, 16 Apr 2024 22:33:50 +0200
Subject: [PATCH] [Parafoil] Added rotation wing algorithm in WingController
---
src/boards/Parafoil/Configs/WESConfig.h | 2 ++
src/boards/Parafoil/Configs/WingConfig.h | 2 ++
.../WingController/WingController.cpp | 35 +++++++++++++++++++
3 files changed, 39 insertions(+)
diff --git a/src/boards/Parafoil/Configs/WESConfig.h b/src/boards/Parafoil/Configs/WESConfig.h
index 586462d58..5fe0b7012 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 06d0d3932..1ea29f7ea 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 46a119d4c..f0542e62a 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;
--
GitLab