diff --git a/src/boards/Parafoil/Wing/AutomaticWingAlgorithm.cpp b/src/boards/Parafoil/Wing/AutomaticWingAlgorithm.cpp index e56bd236e788bd60de821f058a2be529f81ef34a..a8cdb069fdc62bbf4c83e604d35354672bbf9f64 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;