From bb3d64a04646ff0b1ab2f9c743c619fd9fb61434 Mon Sep 17 00:00:00 2001
From: BOTAlex <zhentao2004@gmail.com>
Date: Sat, 25 Jan 2025 03:34:00 +0100
Subject: [PATCH] Now enters a button protocol when clicked

---
 src/InputController.h | 25 +++++++++++++++++++++++++
 1 file changed, 25 insertions(+)

diff --git a/src/InputController.h b/src/InputController.h
index 50678e9..619f18b 100644
--- a/src/InputController.h
+++ b/src/InputController.h
@@ -7,12 +7,37 @@
 #define JOYSTICK_PIN 63
 #define JOY_CRITICAL_POINT 25
 
+#define BTN_PIN 19
+
+TaskHandle_t inputTask;
+
+void ClawProtocol(void *params) {
+    Serial.println("Started button protocol");
+
+    StepperController::SetMotorEnabled('y', true);
+    StepperController::SetMovementEnabled('y', true);
+    StepperController::SetMoveQueue('y', 3000);
+
+    vTaskResume(inputTask);
+    Serial.println("Button protocol done");
+    vTaskDelete(NULL);
+}
+
 void InputControlTask(void *params) {
+    inputTask = xTaskGetCurrentTaskHandle();
+    pinMode(BTN_PIN, INPUT_PULLUP); // Setup button pin
+
   while (true) {
     int joystickValueRaw = analogRead(JOYSTICK_PIN);
     int joystickValue = map(joystickValueRaw, 0, 1023, 0, 100);
     bool leftMovement = (joystickValue > (100 - JOY_CRITICAL_POINT));
     bool rightMovement = (joystickValue < JOY_CRITICAL_POINT);
+
+    bool isButtonPress = digitalRead(BTN_PIN) == LOW;
+    if (isButtonPress){
+        xTaskCreate(ClawProtocol, "Button protocol", 500, (void*)&inputTask, 100, NULL);
+        vTaskSuspend(inputTask);
+    }
     
     if (leftMovement) {
         StepperController::SetMotorEnabled('x', true);