diff --git a/src/Control Tasks/MotorControlTask.cpp b/src/Control Tasks/MotorControlTask.cpp index 1b8713a..d3f3001 100644 --- a/src/Control Tasks/MotorControlTask.cpp +++ b/src/Control Tasks/MotorControlTask.cpp @@ -11,6 +11,17 @@ void MotorControlTask::begin() { } void MotorControlTask::execute() { + if (sfr::flight::initial_hold && (millis() > constants::flight::initial_hold_time)) { + sfr::flight::initial_hold = false; + sfr::flight::initial_spin = true; + esc.write(map(1180, 1000, 2000, 0, 180)); + } + + if (sfr::flight::initial_spin && (millis() > constants::flight::initial_spin_time)) { + sfr::flight::initial_spin = false; + set_white(); + } + if (sfr::motor::spinning_up) { vlogln("asking it to spin up: "); spinup(); diff --git a/src/constants.hpp b/src/constants.hpp index 88326e5..45cc9f7 100644 --- a/src/constants.hpp +++ b/src/constants.hpp @@ -75,6 +75,10 @@ inline void set_purple() { } namespace constants { + namespace flight { + constexpr unsigned long initial_hold_time = 5000; + constexpr unsigned long initial_spin_time = 10000; // TODO: This is kinda hacky but I'm lazy rn + } // namespace flight namespace ir { constexpr unsigned long arm_timeout = 2000; constexpr unsigned long deploy_led_timeout = 3000; diff --git a/src/main.cpp b/src/main.cpp index 905555d..b8139d4 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -14,7 +14,7 @@ MotorControlTask motor_control_task; SDControlTask sd_control_task; void set_automated() { - sfr::test::automated = true; + sfr::flight::automated = true; set_purple(); } @@ -28,7 +28,7 @@ void setup() { pinMode(BUTTON_PIN, INPUT); attachInterrupt(BUTTON_PIN, set_automated, FALLING); - set_white(); + set_blue(); imu_monitor.begin(); ir_control_task.begin(); @@ -40,7 +40,9 @@ void loop() { vlogln(F("-------------------- START LOOP --------------------")); imu_monitor.execute(); - ir_control_task.execute(); + if (!sfr::flight::automated) { + ir_control_task.execute(); + } motor_control_task.execute(); sd_control_task.execute(); diff --git a/src/sfr.cpp b/src/sfr.cpp index b2dae6d..e58e668 100644 --- a/src/sfr.cpp +++ b/src/sfr.cpp @@ -1,9 +1,11 @@ #include "sfr.hpp" namespace sfr { - namespace test { + namespace flight { + bool initial_hold = true; + bool initial_spin = false; volatile bool automated = false; - } // namespace test + } // namespace flight namespace ir { bool is_armed = false; unsigned long armed_start = 0; diff --git a/src/sfr.hpp b/src/sfr.hpp index 030da19..33e1633 100644 --- a/src/sfr.hpp +++ b/src/sfr.hpp @@ -2,9 +2,11 @@ #define SFR_HPP namespace sfr { - namespace test { + namespace flight { + extern bool initial_hold; + extern bool initial_spin; extern volatile bool automated; - } // namespace test + } // namespace flight namespace ir { extern bool is_armed; extern unsigned long armed_start;