From f817f6642ab1012882feb7a75f706567019da6d0 Mon Sep 17 00:00:00 2001
From: Cameron Goddard <camerongoddard@me.com>
Date: Mon, 2 Dec 2024 17:46:14 -0500
Subject: [PATCH] add initial hold and spin

---
 src/Control Tasks/MotorControlTask.cpp | 11 +++++++++++
 src/constants.hpp                      |  4 ++++
 src/main.cpp                           |  8 +++++---
 src/sfr.cpp                            |  6 ++++--
 src/sfr.hpp                            |  6 ++++--
 5 files changed, 28 insertions(+), 7 deletions(-)

diff --git a/src/Control Tasks/MotorControlTask.cpp b/src/Control Tasks/MotorControlTask.cpp
index 42a0417..38619bf 100644
--- a/src/Control Tasks/MotorControlTask.cpp	
+++ b/src/Control Tasks/MotorControlTask.cpp	
@@ -10,6 +10,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) {
         spinup();
     }
diff --git a/src/constants.hpp b/src/constants.hpp
index 32f82de..670d2b9 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 cf87cfb..c68de49 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 2797ae8..81c1c2d 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 c577b24..0e7b407 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;