diff --git a/main/spincoat-plater-firmware.c b/main/spincoat-plater-firmware.c
index 5a6dd07..8c71e90 100644
--- a/main/spincoat-plater-firmware.c
+++ b/main/spincoat-plater-firmware.c
@@ -19,19 +19,56 @@
 
 static const char *TAG = "spincoat-plater-firmware";
 
-void v_echo_task_func(void *pvParameters) {
-    TickType_t frequency = 5000 / portTICK_PERIOD_MS; // 5000 ms
+rmt_encoder_handle_t dshot_encoder = NULL;
+rmt_channel_handle_t esc_chan = NULL;
+
+rmt_transmit_config_t tx_config = {
+    .loop_count = 0,
+};
+
+dshot_esc_throttle_t throttle = {
+    .throttle = 0,
+    .telemetry_req = false, // telemetry is not supported in this example
+};
+
+/**
+ * Sends a telemetry packet at a set, constant interval
+ */
+void v_telemetry_packet_func(void *pvParameters) {
+    TickType_t frequency = 1000 / portTICK_PERIOD_MS;
     TickType_t last_wake_time = xTaskGetTickCount();
     while(1) {
-        printf("Hello from vEchoTaskFunction()\n");
+        throttle.telemetry_req = true;
         vTaskDelayUntil(&last_wake_time, frequency);
     }
 }
 
-void init_rmt_esc_tx(void)
-{
+/**
+ * Sends zero throttle to arm ESC for control. Stop/delete this task once the ESC has armed.
+ */
+void v_initialize_esc_throttle_func(void *pvParameters) {
+    while(1) {
+        ESP_ERROR_CHECK(rmt_transmit(esc_chan, dshot_encoder, &throttle, sizeof(throttle), &tx_config));
+    }
+}
+
+/**
+ * Starts task *v_initialize_esc_throttle_func()* for a few seconds and then destroys it.
+ * This function takes care of the arming stage of ESC control.
+ */
+void initialize_esc_throttle(void) {
+    TaskHandle_t v_init_throttle_handle = NULL;
+    xTaskCreate(&v_initialize_esc_throttle_func, "v_init_throttle_func", 2048, NULL, 5, &v_init_throttle_handle);
+    vTaskDelay(pdMS_TO_TICKS(5000));
+    vTaskDelete(v_init_throttle_handle);
+    vTaskDelay(pdMS_TO_TICKS(1000));
+}
+
+/**
+ * Initialize the RMT system in preparation for sending DSHOT packets to the connected ESC.
+ */
+void init_rmt_esc_tx(void) {
     ESP_LOGI(TAG, "Create RMT TX channel");
-    rmt_channel_handle_t esc_chan = NULL;
     rmt_tx_channel_config_t tx_chan_config = {
         .clk_src = RMT_CLK_SRC_DEFAULT, // select a clock that can provide needed resolution
         .gpio_num = DSHOT_ESC_GPIO_NUM,
@@ -42,7 +79,6 @@ void init_rmt_esc_tx(void)
     ESP_ERROR_CHECK(rmt_new_tx_channel(&tx_chan_config, &esc_chan));
 
     ESP_LOGI(TAG, "Install Dshot ESC encoder");
-    rmt_encoder_handle_t dshot_encoder = NULL;
     dshot_esc_encoder_config_t encoder_config = {
         .resolution = DSHOT_ESC_RESOLUTION_HZ,
         .baud_rate = 300000, // DSHOT300 protocol
@@ -53,32 +89,30 @@ void init_rmt_esc_tx(void)
     ESP_LOGI(TAG, "Enable RMT TX channel");
     ESP_ERROR_CHECK(rmt_enable(esc_chan));
 
-    rmt_transmit_config_t tx_config = {
-        .loop_count = -1, // infinite loop
-    };
-    dshot_esc_throttle_t throttle = {
-        .throttle = 0,
-        .telemetry_req = false, // telemetry is not supported in this example
-    };
-
     ESP_LOGI(TAG, "Start ESC by sending zero throttle for a while...");
+    initialize_esc_throttle();
+}
+
+/**
+ * Sends a DSHOT packet via the RMT. Make sure the RMT channel has been initialized
+ * by calling *init_rmt_esc_tx()*
+ */
+void send_dshot_packet(void) {
     ESP_ERROR_CHECK(rmt_transmit(esc_chan, dshot_encoder, &throttle, sizeof(throttle), &tx_config));
-    vTaskDelay(pdMS_TO_TICKS(5000));
-
-    ESP_LOGI(TAG, "Increase throttle, no telemetry");
-    // Commented out esc driving example, working on other stuff for now
-//    for (uint16_t thro = 100; thro < 1000; thro += 10) {
-//        throttle.throttle = thro;
-//        ESP_ERROR_CHECK(rmt_transmit(esc_chan, dshot_encoder, &throttle, sizeof(throttle), &tx_config));
-//        // the previous loop transfer is till undergoing, we need to stop it and restart,
-//        // so that the new throttle can be updated on the output
-//        ESP_ERROR_CHECK(rmt_disable(esc_chan));
-//        ESP_ERROR_CHECK(rmt_enable(esc_chan));
-//        vTaskDelay(pdMS_TO_TICKS(1000));
-//    }
-
+    
+    if(throttle.telemetry_req == true) {
+        throttle.telemetry_req = false;
+        printf("Set telemetry to false.\n");
+    }
 }
 
 void app_main(void) {
-    xTaskCreate(&v_echo_task_func, "v_echo_task_func", 2048, NULL, 5, NULL);
+    init_rmt_esc_tx();
+    
+    throttle.throttle = 100;
+    xTaskCreate(&v_telemetry_packet_func, "v_telemetry_packet_func", 2048, NULL, 5, NULL);
+    
+    while(1) {
+        send_dshot_packet();
+    }
 }