Merge remote-tracking branch 'origin/main' into Controls

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();

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;

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();
 

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;

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;