From 82538819e6b76849cc43cb521d4fbbf643d3348a Mon Sep 17 00:00:00 2001
From: BlakeFreer <freerb@mcmaster.ca>
Date: Tue, 2 Apr 2024 22:35:17 -0400
Subject: [PATCH 1/3] updates TMS to run in a 100 ms RTOS task

---
 firmware/projects/TMS/inc/app.h               | 39 ++++++---
 firmware/projects/TMS/main.cc                 | 34 ++++++--
 .../TMS/platforms/stm32f767/CMakeLists.txt    |  9 +-
 .../TMS/platforms/stm32f767/bindings.cc       |  9 +-
 .../platforms/stm32f767/cubemx/CMakeLists.txt |  4 +
 .../stm32f767/cubemx/board_config.ioc         | 86 +++++++++++--------
 .../projects/TMS/platforms/stm32f767/os.cc    | 25 ++++++
 .../TMS/platforms/windows/bindings.cc         |  4 +-
 8 files changed, 148 insertions(+), 62 deletions(-)
 create mode 100644 firmware/projects/TMS/platforms/stm32f767/os.cc

diff --git a/firmware/projects/TMS/inc/app.h b/firmware/projects/TMS/inc/app.h
index 3d38c2802..41b7bff39 100644
--- a/firmware/projects/TMS/inc/app.h
+++ b/firmware/projects/TMS/inc/app.h
@@ -12,6 +12,7 @@
 #include "shared/periph/gpio.h"
 #include "shared/periph/pwm.h"
 #include "shared/util/mappers/clamper.h"
+#include "shared/util/mappers/linear_map.h"
 #include "shared/util/mappers/mapper.h"
 #include "shared/util/moving_average.h"
 
@@ -92,7 +93,8 @@ class FanContoller {
     void Update(float temperature) {
         // convert pwm = 100 - power since the fan runs on inverse logic
         // ex. pwm=20% => fan is running at 80%
-        float desired_pwm = 100.0f - temp_to_power_.Evaluate(temperature);
+        float desired_pwm =
+            power_to_pwm_.Evaluate(temp_to_power_.Evaluate(temperature));
         float current_pwm = pwm_.GetDutyCycle();
         float delta_pwm = desired_pwm - current_pwm;
 
@@ -102,36 +104,51 @@ class FanContoller {
         pwm_.SetDutyCycle(current_pwm + pwm_step);
     }
 
-    void StartPWM(float initial_duty_cycle) {
+    void Start(float initial_power) {
         pwm_.Start();
-        pwm_.SetDutyCycle(initial_duty_cycle);
+        pwm_.SetDutyCycle(power_to_pwm_.Evaluate(initial_power));
     }
 
 private:
     shared::periph::PWMOutput& pwm_;
 
-    /// @brief Mapping from temperature [degC] to fan PWM
+    /// @brief Mapping from temperature [degC] to fan power
     shared::util::Mapper<float>& temp_to_power_;
 
+    /// @brief Fan pwm signal is inverted (high duty = low power)
+    const shared::util::LinearMap<float> power_to_pwm_{-1.0f, 100.0f};
+
     /// @brief Largest allowable PWM per Update() call.
-    /// @todo Express pwm_step_size in pwm/second and use Update() frequency to
-    /// determine it.
+    /// @todo Express pwm_step_size in pwm/second and use Update() frequency
+    /// to determine it.
     float pwm_step_size_;
 };
 
 class DebugIndicator {
-private:
-    shared::periph::DigitalOutput& digital_output_;
-
 public:
     DebugIndicator(shared::periph::DigitalOutput& digital_output)
         : digital_output_(digital_output) {}
 
     void Set() {
-        digital_output_.SetHigh();
+        state_ = true;
+        UpdateDO();
     }
 
     void Reset() {
-        digital_output_.SetLow();
+        state_ = false;
+        UpdateDO();
+    }
+
+    void Toggle() {
+        state_ = !state_;
+        UpdateDO();
+    }
+
+private:
+    shared::periph::DigitalOutput& digital_output_;
+    bool state_ = false;
+
+    inline void UpdateDO() {
+        digital_output_.Set(state_);
     }
 };
\ No newline at end of file
diff --git a/firmware/projects/TMS/main.cc b/firmware/projects/TMS/main.cc
index 908f52261..0fe24751d 100644
--- a/firmware/projects/TMS/main.cc
+++ b/firmware/projects/TMS/main.cc
@@ -21,13 +21,23 @@ extern shared::periph::ADCInput& temp_sensor_adc_6;
 
 extern shared::periph::PWMOutput& fan_controller_pwm;
 
-extern shared::periph::DigitalOutput& debug_do_green;
+extern shared::periph::DigitalOutput& debug_do_blue;
 extern shared::periph::DigitalOutput& debug_do_red;
 
 extern void Initialize();
 extern void Log(std::string);
 }  // namespace bindings
 
+namespace os {
+extern void Tick(uint32_t ticks);
+extern void InitializeKernel();
+extern void StartKernel();
+}  // namespace os
+
+extern "C" {
+void UpdateTask(void* argument);
+}
+
 // clang-format off
 const float temp_lut_data[][2] = {
     {2475, 120},
@@ -87,7 +97,7 @@ shared::util::LookupTable<fan_lut_length> fan_temp_lut{fan_lut_data};
 ***************************************************************/
 FanContoller fan_controller{bindings::fan_controller_pwm, fan_temp_lut, 2.0f};
 
-DebugIndicator debug_green{bindings::debug_do_green};
+DebugIndicator debug_blue{bindings::debug_do_blue};
 DebugIndicator debug_red{bindings::debug_do_red};
 
 TempSensor temp_sensors[] = {
@@ -105,7 +115,8 @@ TempSensorManager<kSensorCount> ts_manager{temp_sensors};
 /***************************************************************
     Program Logic
 ***************************************************************/
-void UpdateTask() {
+
+void Update() {
     static float temperature_buffer[kSensorCount];
 
     ts_manager.Update();
@@ -122,18 +133,25 @@ void UpdateTask() {
 
     /// TODO: Pack & send CAN message
 
-    /// TODO: Needs PWM_Sweep_Nonblocking
     fan_controller.Update(temp_avg);
 }
 
+void UpdateTask(void* argument) {
+    fan_controller.Start(0);
+    while (true) {
+        Update();
+        debug_blue.Toggle();  // toggling indicates the loop is running
+        os::Tick(100);
+    }
+}
+
 int main(void) {
     bindings::Initialize();
+    os::InitializeKernel();
 
-    fan_controller.StartPWM(0);
+    os::StartKernel();
 
-    while (true) {
-        UpdateTask();
-    }
+    while (true) continue;
 
     return 0;
 }
\ No newline at end of file
diff --git a/firmware/projects/TMS/platforms/stm32f767/CMakeLists.txt b/firmware/projects/TMS/platforms/stm32f767/CMakeLists.txt
index e7e7c1978..cc537ca7a 100644
--- a/firmware/projects/TMS/platforms/stm32f767/CMakeLists.txt
+++ b/firmware/projects/TMS/platforms/stm32f767/CMakeLists.txt
@@ -1,12 +1,19 @@
 # Blake Freer
 # January 8, 2024
+add_library(os)
 
 target_sources(bindings
-PUBLIC
+	PUBLIC
 	bindings.cc
 )
 
+target_sources(os
+	PUBLIC
+	os.cc
+)
+
 add_subdirectory(cubemx)
 
 # must be public so that the 'main' executable can see its headers when including bindings.h
 target_link_libraries(bindings PUBLIC driver)
+target_link_libraries(os PUBLIC driver)
diff --git a/firmware/projects/TMS/platforms/stm32f767/bindings.cc b/firmware/projects/TMS/platforms/stm32f767/bindings.cc
index f3b5eeb70..c141d8329 100644
--- a/firmware/projects/TMS/platforms/stm32f767/bindings.cc
+++ b/firmware/projects/TMS/platforms/stm32f767/bindings.cc
@@ -34,9 +34,10 @@ periph::ADCInput temp_sensor_adc_5{&hadc1, SENS_5_UC_IN_CHANNEL};
 periph::ADCInput temp_sensor_adc_6{&hadc1, SENS_6_UC_IN_CHANNEL};
 
 periph::PWMOutput fan_controller_pwm{&htim4, TIM_CHANNEL_1};
-periph::DigitalOutput debug_do_green{DEBUG_LED_GREEN_GPIO_Port,
-                                     DEBUG_LED_GREEN_Pin};
-periph::DigitalOutput debug_do_red{DEBUG_LED_RED_GPIO_Port, DEBUG_LED_RED_Pin};
+periph::DigitalOutput debug_do_blue{NUCLEO_BLUE_LED_GPIO_Port,
+                                    NUCLEO_BLUE_LED_Pin};
+periph::DigitalOutput debug_do_red{NUCLEO_RED_LED_GPIO_Port,
+                                   NUCLEO_RED_LED_Pin};
 
 }  // namespace mcal
 
@@ -49,7 +50,7 @@ const shared::periph::ADCInput& temp_sensor_adc_5 = mcal::temp_sensor_adc_5;
 const shared::periph::ADCInput& temp_sensor_adc_6 = mcal::temp_sensor_adc_6;
 
 const shared::periph::PWMOutput& fan_controller_pwm = mcal::fan_controller_pwm;
-const shared::periph::DigitalOutput& debug_do_green = mcal::debug_do_green;
+const shared::periph::DigitalOutput& debug_do_blue = mcal::debug_do_blue;
 const shared::periph::DigitalOutput& debug_do_red = mcal::debug_do_red;
 
 void Initialize() {
diff --git a/firmware/projects/TMS/platforms/stm32f767/cubemx/CMakeLists.txt b/firmware/projects/TMS/platforms/stm32f767/cubemx/CMakeLists.txt
index 5bab7c61c..f3ebc8796 100644
--- a/firmware/projects/TMS/platforms/stm32f767/cubemx/CMakeLists.txt
+++ b/firmware/projects/TMS/platforms/stm32f767/cubemx/CMakeLists.txt
@@ -29,6 +29,10 @@ PUBLIC
 	Drivers/STM32F7xx_HAL_Driver/Inc
 	Drivers/STM32F7xx_HAL_Driver/Inc/Legacy
 	Inc
+	Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2
+	Middlewares/Third_Party/FreeRTOS/Source/include
+	Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM7/r0p1
+	Middlewares/Third_Party/FreeRTOS/Source/portable/MemMang
 )
 
 # The rest of the file specifies parameters required to link the object files.
diff --git a/firmware/projects/TMS/platforms/stm32f767/cubemx/board_config.ioc b/firmware/projects/TMS/platforms/stm32f767/cubemx/board_config.ioc
index 257586091..6cb9ce032 100644
--- a/firmware/projects/TMS/platforms/stm32f767/cubemx/board_config.ioc
+++ b/firmware/projects/TMS/platforms/stm32f767/cubemx/board_config.ioc
@@ -8,6 +8,9 @@ ADC1.master=1
 CAD.formats=
 CAD.pinconfig=
 CAD.provider=
+FREERTOS.FootprintOK=true
+FREERTOS.IPParameters=Tasks01,FootprintOK
+FREERTOS.Tasks01=Update,24,128,UpdateTask,As weak,NULL,Static,UpdateBuffer,UpdateControlBlock
 File.Version=6
 GPIO.groupedBy=Group By Peripherals
 KeepUserPlacement=false
@@ -15,49 +18,61 @@ Mcu.CPN=STM32F767ZIT6
 Mcu.Family=STM32F7
 Mcu.IP0=ADC1
 Mcu.IP1=CORTEX_M7
-Mcu.IP2=NVIC
-Mcu.IP3=RCC
-Mcu.IP4=SYS
-Mcu.IP5=TIM4
-Mcu.IPNb=6
+Mcu.IP2=FREERTOS
+Mcu.IP3=NVIC
+Mcu.IP4=RCC
+Mcu.IP5=SYS
+Mcu.IP6=TIM4
+Mcu.IPNb=7
 Mcu.Name=STM32F767ZITx
 Mcu.Package=LQFP144
-Mcu.Pin0=PE2
-Mcu.Pin1=PC0
-Mcu.Pin10=VP_TIM4_VS_ClockSourceINT
-Mcu.Pin2=PC1
-Mcu.Pin3=PC2
-Mcu.Pin4=PC3
-Mcu.Pin5=PA3
-Mcu.Pin6=PC4
-Mcu.Pin7=PC5
-Mcu.Pin8=PB6
-Mcu.Pin9=VP_SYS_VS_Systick
-Mcu.PinsNb=11
+Mcu.Pin0=PC0
+Mcu.Pin1=PC1
+Mcu.Pin10=VP_SYS_VS_tim3
+Mcu.Pin11=VP_TIM4_VS_ClockSourceINT
+Mcu.Pin2=PC2
+Mcu.Pin3=PC3
+Mcu.Pin4=PC4
+Mcu.Pin5=PC5
+Mcu.Pin6=PB14
+Mcu.Pin7=PB6
+Mcu.Pin8=PB7
+Mcu.Pin9=VP_FREERTOS_VS_CMSIS_V2
+Mcu.PinsNb=12
 Mcu.ThirdPartyNb=0
 Mcu.UserConstants=SENS_2_UC_IN_CHANNEL,ADC_CHANNEL_11;SENS_6_UC_IN_CHANNEL,ADC_CHANNEL_15;SENS_3_UC_IN_CHANNEL,ADC_CHANNEL_12;SENS_1_UC_IN_CHANNEL,ADC_CHANNEL_10;SENS_4_UC_IN_CHANNEL,ADC_CHANNEL_13;SENS_5_UC_IN_CHANNEL,ADC_CHANNEL_14
 Mcu.UserName=STM32F767ZITx
 MxCube.Version=6.8.1
 MxDb.Version=DB.6.0.81
-NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:true\:false\:false
-NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:true\:false\:false
+NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:false\:false
+NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:false\:false
 NVIC.ForceEnableDMAVector=true
-NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:true\:false\:false
-NVIC.MemoryManagement_IRQn=true\:0\:0\:false\:false\:true\:true\:false\:false
-NVIC.NonMaskableInt_IRQn=true\:0\:0\:false\:false\:true\:true\:false\:false
-NVIC.PendSV_IRQn=true\:0\:0\:false\:false\:true\:true\:false\:false
+NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:false\:false
+NVIC.MemoryManagement_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:false\:false
+NVIC.NonMaskableInt_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:false\:false
+NVIC.PendSV_IRQn=true\:15\:0\:false\:false\:false\:true\:true\:false\:false
 NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4
-NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:true\:true\:false\:false
-NVIC.SysTick_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:false
-NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:true\:false\:false
-PA3.GPIOParameters=GPIO_Label
-PA3.GPIO_Label=DEBUG_LED_GREEN
-PA3.Locked=true
-PA3.Signal=GPIO_Output
+NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:false\:false\:true\:false\:false
+NVIC.SavedPendsvIrqHandlerGenerated=true
+NVIC.SavedSvcallIrqHandlerGenerated=true
+NVIC.SavedSystickIrqHandlerGenerated=true
+NVIC.SysTick_IRQn=true\:15\:0\:false\:false\:false\:true\:true\:true\:false
+NVIC.TIM3_IRQn=true\:15\:0\:false\:false\:true\:false\:false\:true\:true
+NVIC.TimeBase=TIM3_IRQn
+NVIC.TimeBaseIP=TIM3
+NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:false\:false
+PB14.GPIOParameters=GPIO_Label
+PB14.GPIO_Label=NUCLEO_RED_LED
+PB14.Locked=true
+PB14.Signal=GPIO_Output
 PB6.GPIOParameters=GPIO_Label
 PB6.GPIO_Label=FAN_PWM
 PB6.Locked=true
 PB6.Signal=S_TIM4_CH1
+PB7.GPIOParameters=GPIO_Label
+PB7.GPIO_Label=NUCLEO_BLUE_LED
+PB7.Locked=true
+PB7.Signal=GPIO_Output
 PC0.GPIOParameters=GPIO_Label
 PC0.GPIO_Label=SENS_1_UC_IN
 PC0.Locked=true
@@ -82,10 +97,6 @@ PC5.GPIOParameters=GPIO_Label
 PC5.GPIO_Label=SENS_6_UC_IN
 PC5.Locked=true
 PC5.Signal=ADCx_IN15
-PE2.GPIOParameters=GPIO_Label
-PE2.GPIO_Label=DEBUG_LED_RED
-PE2.Locked=true
-PE2.Signal=GPIO_Output
 PinOutPanel.RotationAngle=0
 ProjectManager.AskForMigrate=true
 ProjectManager.BackupPrevious=false
@@ -219,9 +230,12 @@ SH.S_TIM4_CH1.0=TIM4_CH1,PWM Generation1 CH1
 SH.S_TIM4_CH1.ConfNb=1
 TIM4.Channel-PWM\ Generation1\ CH1=TIM_CHANNEL_1
 TIM4.IPParameters=Channel-PWM Generation1 CH1
-VP_SYS_VS_Systick.Mode=SysTick
-VP_SYS_VS_Systick.Signal=SYS_VS_Systick
+VP_FREERTOS_VS_CMSIS_V2.Mode=CMSIS_V2
+VP_FREERTOS_VS_CMSIS_V2.Signal=FREERTOS_VS_CMSIS_V2
+VP_SYS_VS_tim3.Mode=TIM3
+VP_SYS_VS_tim3.Signal=SYS_VS_tim3
 VP_TIM4_VS_ClockSourceINT.Mode=Internal
 VP_TIM4_VS_ClockSourceINT.Signal=TIM4_VS_ClockSourceINT
 board=NUCLEO-F767ZI
 boardIOC=true
+rtos.0.ip=FREERTOS
diff --git a/firmware/projects/TMS/platforms/stm32f767/os.cc b/firmware/projects/TMS/platforms/stm32f767/os.cc
new file mode 100644
index 000000000..28c351e91
--- /dev/null
+++ b/firmware/projects/TMS/platforms/stm32f767/os.cc
@@ -0,0 +1,25 @@
+/// @author Blake Freer
+/// @date 2024-04-02
+
+#include "cmsis_os2.h"
+#include "mcal/stm32f767/os/tick.h"
+
+extern "C" {
+/**
+ * This requires extern since it is not declared in a header, only defined
+ * in cubemx/../freertos.c
+ */
+void MX_FREERTOS_Init();
+}
+
+namespace os {
+void InitializeKernel() {
+    osKernelInitialize();
+    MX_FREERTOS_Init();
+}
+
+void StartKernel() {
+    osKernelStart();
+}
+
+}  // namespace os
\ No newline at end of file
diff --git a/firmware/projects/TMS/platforms/windows/bindings.cc b/firmware/projects/TMS/platforms/windows/bindings.cc
index 2a3ad8d40..38c0ba992 100644
--- a/firmware/projects/TMS/platforms/windows/bindings.cc
+++ b/firmware/projects/TMS/platforms/windows/bindings.cc
@@ -19,7 +19,7 @@ periph::ADCInput temp_sensor_adc_4{"Temperature Sensor 4"};
 periph::ADCInput temp_sensor_adc_5{"Temperature Sensor 5"};
 periph::ADCInput temp_sensor_adc_6{"Temperature Sensor 6"};
 periph::PWMOutput fan_controller_pwm{"Fan Controller"};
-periph::DigitalOutput debug_do_green{"Debug: Green"};
+periph::DigitalOutput debug_do_blue{"Debug: Blue"};
 periph::DigitalOutput debug_do_red{"Debug: Red"};
 }  // namespace mcal
 
@@ -32,7 +32,7 @@ const shared::periph::ADCInput& temp_sensor_adc_4 = mcal::temp_sensor_adc_4;
 const shared::periph::ADCInput& temp_sensor_adc_5 = mcal::temp_sensor_adc_5;
 const shared::periph::ADCInput& temp_sensor_adc_6 = mcal::temp_sensor_adc_6;
 const shared::periph::PWMOutput& fan_controller_pwm = mcal::fan_controller_pwm;
-const shared::periph::DigitalOutput& debug_do_green = mcal::debug_do_green;
+const shared::periph::DigitalOutput& debug_do_blue = mcal::debug_do_blue;
 const shared::periph::DigitalOutput& debug_do_red = mcal::debug_do_red;
 
 void Initialize() {

From 15723cdd864fe1dc64d004cf128726a243986971 Mon Sep 17 00:00:00 2001
From: BlakeFreer <freerb@mcmaster.ca>
Date: Tue, 2 Apr 2024 22:37:18 -0400
Subject: [PATCH 2/3] reorders Fan functions

---
 firmware/projects/TMS/inc/app.h | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/firmware/projects/TMS/inc/app.h b/firmware/projects/TMS/inc/app.h
index 41b7bff39..3be70b28a 100644
--- a/firmware/projects/TMS/inc/app.h
+++ b/firmware/projects/TMS/inc/app.h
@@ -90,6 +90,11 @@ class FanContoller {
           temp_to_power_(temp_to_power),
           pwm_step_size_(pwm_step_size) {}
 
+    void Start(float initial_power) {
+        pwm_.Start();
+        pwm_.SetDutyCycle(power_to_pwm_.Evaluate(initial_power));
+    }
+
     void Update(float temperature) {
         // convert pwm = 100 - power since the fan runs on inverse logic
         // ex. pwm=20% => fan is running at 80%
@@ -104,11 +109,6 @@ class FanContoller {
         pwm_.SetDutyCycle(current_pwm + pwm_step);
     }
 
-    void Start(float initial_power) {
-        pwm_.Start();
-        pwm_.SetDutyCycle(power_to_pwm_.Evaluate(initial_power));
-    }
-
 private:
     shared::periph::PWMOutput& pwm_;
 

From 2396514b7cb49f6b895f825e7c580dda195cb67a Mon Sep 17 00:00:00 2001
From: BlakeFreer <freerb@mcmaster.ca>
Date: Sun, 7 Apr 2024 10:39:11 -0400
Subject: [PATCH 3/3] uses tick until, few cleanups

---
 firmware/projects/TMS/main.cc | 14 +++++++++-----
 1 file changed, 9 insertions(+), 5 deletions(-)

diff --git a/firmware/projects/TMS/main.cc b/firmware/projects/TMS/main.cc
index 0fe24751d..2eddcd3bc 100644
--- a/firmware/projects/TMS/main.cc
+++ b/firmware/projects/TMS/main.cc
@@ -5,6 +5,7 @@
 #include <string>
 
 #include "app.h"
+#include "shared/os/tick.h"
 #include "shared/periph/adc.h"
 #include "shared/periph/gpio.h"
 #include "shared/periph/pwm.h"
@@ -38,8 +39,8 @@ extern "C" {
 void UpdateTask(void* argument);
 }
 
-// clang-format off
 const float temp_lut_data[][2] = {
+    // clang-format off
     {2475, 120},
 	{2480, 115},
 	{2485, 110},
@@ -73,16 +74,16 @@ const float temp_lut_data[][2] = {
 	{3056, -30},
 	{3066, -35},
     {3077, -40},
+    // clang-format on
 };
-// clang-format on
 
-// clang-format off
 const float fan_lut_data[][2] = {
+    // clang-format off
 	{-1,    0},
 	{ 0,   30},
 	{50,  100}
+    // clang-format on
 };
-// clang-format on
 
 constexpr int temp_lut_length =
     (sizeof(temp_lut_data)) / (sizeof(temp_lut_data[0]));
@@ -137,11 +138,14 @@ void Update() {
 }
 
 void UpdateTask(void* argument) {
+    const static uint32_t kTaskPeriodMs = 100;
+
     fan_controller.Start(0);
     while (true) {
+        uint32_t start_time_ms = os::GetTickCount();
         Update();
         debug_blue.Toggle();  // toggling indicates the loop is running
-        os::Tick(100);
+        os::TickUntil(start_time_ms + kTaskPeriodMs);
     }
 }