diff --git a/src/boards/Payload/Configs/WingConfig.h b/src/boards/Payload/Configs/WingConfig.h index 33b3f342d8ed49556d691d14350e8b24264cde9d..8824118432eafa8af0d58db2fcd558d88c8aad15 100644 --- a/src/boards/Payload/Configs/WingConfig.h +++ b/src/boards/Payload/Configs/WingConfig.h @@ -44,8 +44,7 @@ enum class AlgorithmId : size_t { EARLY_MANEUVER = 0, CLOSED_LOOP, - SEQUENCE, ///< A predefined sequence of maneuvers - ROTATION, ///< A sequence of maneuvers to rotate the wing + ROTATION, ///< A predefined sequence of rotation maneuvers LAST, ///< Used to count the number of algorithms }; diff --git a/src/boards/Payload/StateMachines/WingController/WingController.cpp b/src/boards/Payload/StateMachines/WingController/WingController.cpp index edb38402249d2bba47f14ede6c47f15c171376c3..9c3396ca3399072fdeef7784a2df307cab7589d7 100644 --- a/src/boards/Payload/StateMachines/WingController/WingController.cpp +++ b/src/boards/Payload/StateMachines/WingController/WingController.cpp @@ -390,7 +390,6 @@ bool WingController::selectAlgorithm(uint8_t index) { case static_cast<uint8_t>(AlgorithmId::EARLY_MANEUVER): case static_cast<uint8_t>(AlgorithmId::CLOSED_LOOP): - case static_cast<uint8_t>(AlgorithmId::SEQUENCE): case static_cast<uint8_t>(AlgorithmId::ROTATION): { selectedAlgorithm = static_cast<AlgorithmId>(index); @@ -434,32 +433,7 @@ void WingController::loadAlgorithms() PI::KP, PI::KI, PARAFOIL_LEFT_SERVO, PARAFOIL_RIGHT_SERVO, clGuidance); - // Sequence - { - auto algorithm = std::make_unique<WingAlgorithm>(PARAFOIL_LEFT_SERVO, - PARAFOIL_RIGHT_SERVO); - WingAlgorithmData step; - - step.timestamp = 0; - step.servo1Angle = 0; - step.servo2Angle = 120; - algorithm->addStep(step); - - step.timestamp += microseconds{STRAIGHT_FLIGHT_TIMEOUT}.count(); - step.servo1Angle = 0; - step.servo2Angle = 0; - algorithm->addStep(step); - - step.timestamp += microseconds{STRAIGHT_FLIGHT_TIMEOUT}.count(); - step.servo1Angle = 0; - step.servo2Angle = 0; - algorithm->addStep(step); - - algorithms[static_cast<size_t>(AlgorithmId::SEQUENCE)] = - std::move(algorithm); - } - - // Rotation + // Rotation Sequence { auto algorithm = std::make_unique<WingAlgorithm>(PARAFOIL_LEFT_SERVO, PARAFOIL_RIGHT_SERVO); @@ -481,21 +455,16 @@ void WingController::loadAlgorithms() algorithm->addStep(step); step.timestamp += microseconds{ROTATION_PERIOD}.count(); - step.servo1Angle = LeftServo::ROTATION; - step.servo2Angle = RightServo::ROTATION; - algorithm->addStep(step); - - step.timestamp += microseconds{ROTATION_PERIOD}.count(); - step.servo1Angle = 0; - step.servo2Angle = RightServo::ROTATION; + step.servo1Angle = LeftServo::ROTATION / 2; + step.servo2Angle = 0; algorithm->addStep(step); step.timestamp += microseconds{ROTATION_PERIOD}.count(); step.servo1Angle = 0; - step.servo2Angle = 0; + step.servo2Angle = RightServo::ROTATION / 2; algorithm->addStep(step); - step.timestamp += microseconds{ROTATION_PERIOD}.count(); + step.timestamp += microseconds{2min}.count(); step.servo1Angle = 0; step.servo2Angle = 0; algorithm->addStep(step);