Setti inn allar skrár frá Project 3 og bætti við overlay í samræmi við controlloerinn og pin out.

2026-10-08 13:11:39 +00:00
parent 9194d71f10
commit 53091f68df
29 changed files with 1645 additions and 0 deletions
@@ -0,0 +1,161 @@
+#include "pre_operational_state.h"
+#include "motor_controller.h"
+
+#include <Arduino.h>
+#include <stdlib.h>
+
+PreOperationalState::PreOperationalState(P_controller& p_controller, P_controller& pi_controller)
+    : p_controller(p_controller),
+      pi_controller(pi_controller),
+      pi_selected(true),
+      previous_time(0),
+      led_state(false),
+      //waiting_for_kp(false),
+      //waiting_for_ti(false),
+      input_mode(NONE),
+      input_index(0)
+{
+}
+
+void PreOperationalState::enter()
+{
+    Serial.println("State: Pre-operational");
+
+    context->stop_motor();
+
+    context->led_off();
+
+    led_state = false;
+
+    previous_time = millis();
+}
+
+void PreOperationalState::update()
+{
+    unsigned long current_time = millis();
+
+    if (current_time - previous_time >= 500)
+    {
+        previous_time = current_time;
+
+        led_state = !led_state;
+
+        if (led_state)
+        {
+            context->led_on();
+        }
+        else
+        {
+            context->led_off();
+        }
+    }
+}
+
+void PreOperationalState::command(char c)
+{
+    // We are currently entering a Kp or Ti value
+    if (input_mode != NONE)
+    {
+        // Enter pressed
+        if (c == '\n' || c == '\r')
+        {
+            if (input_index > 0)
+            {
+                // End the string
+                input_buffer[input_index] = '\0';
+
+                // Convert text to a number
+                double value = atof(input_buffer);
+
+                if (input_mode == ENTER_KP)
+                {
+                    context->set_Kp(value);
+
+                    Serial.print("Kp set to: ");
+                    Serial.println(value);
+                }
+                else if (input_mode == ENTER_TI)
+                {
+                    context->set_Ti(value);
+
+                    Serial.print("Ti set to: ");
+                    Serial.println(value);
+                }
+            }
+
+            // Finished entering a value
+            input_index = 0;
+            input_mode = NONE;
+
+            return;
+        }
+
+        // Store number characters
+        if ((c >= '0' && c <= '9') ||
+            c == '.' ||
+            c == '-')
+        {
+            if (input_index < sizeof(input_buffer) - 1)
+            {
+                input_buffer[input_index] = c;
+                input_index++;
+            }
+        }
+
+        return;
+    }
+
+
+    // Normal state commands
+    if (c == 'o' || c == 'O')
+    {
+        Serial.println("Command: set operational");
+
+        context->to_operational();
+    }
+
+    else if (c == 'r' || c == 'R')
+    {
+        Serial.println("Command: reset");
+
+        context->to_initialization();
+    }
+
+    else if (c == 'k' || c == 'K')
+    {
+        input_mode = ENTER_KP;
+        input_index = 0;
+
+        Serial.println("Enter Kp and press Enter:");
+    }
+
+    else if (c == 't' || c == 'T')
+    {
+        input_mode = ENTER_TI;
+        input_index = 0;
+
+        Serial.println("Enter Ti and press Enter:");
+    }
+
+    // Select controller algorithm, the controller is passed to the motor controller
+    else if (c == 'c' || c == 'C')
+    {
+        pi_selected = !pi_selected;
+
+        if (pi_selected)
+        {
+            context->set_controller(pi_controller);
+            Serial.println("Controller: PI");
+        }
+        else
+        {
+            context->set_controller(p_controller);
+            Serial.println("Controller: P");
+        }
+    }
+}
+
+void PreOperationalState::exit()
+{
+    context->led_off();
+}