From bc0d325cd53c4d036427a03797e85185bd83113f Mon Sep 17 00:00:00 2001
From: Devoalda <linuxes_mojoworld@aleeas.com>
Date: Tue, 3 Oct 2023 04:54:45 +0800
Subject: [PATCH] Lab 6

---
 lab_6/CMakeLists.txt | 47 ++++++++++++++++++++++++++
 lab_6/lab_6.c        | 79 ++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 126 insertions(+)
 create mode 100644 lab_6/CMakeLists.txt
 create mode 100644 lab_6/lab_6.c

diff --git a/lab_6/CMakeLists.txt b/lab_6/CMakeLists.txt
new file mode 100644
index 0000000..61cbe77
--- /dev/null
+++ b/lab_6/CMakeLists.txt
@@ -0,0 +1,47 @@
+# Generated Cmake Pico project file
+
+cmake_minimum_required(VERSION 3.13)
+
+set(CMAKE_C_STANDARD 11)
+set(CMAKE_CXX_STANDARD 17)
+
+set(PICO_BOARD pico_w CACHE STRING "Board type")
+
+# Pull in Raspberry Pi Pico SDK (must be before project)
+include(pico_sdk_import.cmake)
+
+if (PICO_SDK_VERSION_STRING VERSION_LESS "1.4.0")
+  message(FATAL_ERROR "Raspberry Pi Pico SDK version 1.4.0 (or later) required. Your version is ${PICO_SDK_VERSION_STRING}")
+endif()
+
+project(lab_6 C CXX ASM)
+
+# Initialise the Raspberry Pi Pico SDK
+pico_sdk_init()
+
+# Add executable. Default name is the project name, version 0.1
+
+add_executable(lab_6 lab_6.c )
+
+pico_set_program_name(lab_6 "lab_6")
+pico_set_program_version(lab_6 "0.1")
+
+pico_enable_stdio_uart(lab_6 1)
+pico_enable_stdio_usb(lab_6 1)
+
+# Add the standard library to the build
+target_link_libraries(lab_6
+        pico_stdlib)
+
+# Add the standard include files to the build
+target_include_directories(lab_6 PRIVATE
+  ${CMAKE_CURRENT_LIST_DIR}
+  ${CMAKE_CURRENT_LIST_DIR}/.. # for our common lwipopts or any other standard includes, if required
+)
+
+# Add any user requested libraries
+target_link_libraries(lab_6
+        )
+
+pico_add_extra_outputs(lab_6)
+
diff --git a/lab_6/lab_6.c b/lab_6/lab_6.c
new file mode 100644
index 0000000..22a2293
--- /dev/null
+++ b/lab_6/lab_6.c
@@ -0,0 +1,79 @@
+#include <stdio.h>
+#include "pico/stdlib.h"
+//#include <stdlib.h>
+//#include <math.h>
+
+const float Kp = 1.0f;  // SE
+const float Ki = 0.1f;  // SE
+const float Kd = 0.01f; // SE
+
+// Function to compute the control signal
+float
+compute_pid(float setpoint,
+            float current_value,
+            float *integral,
+            float *prev_error
+            ) {
+
+//    float error = *current_value - setpoint; // LE
+    float error = setpoint - current_value;
+
+    *integral += error;
+
+    float derivative = error - *prev_error;
+
+    // Extra multiplication by 0.1 (LE)
+//    float control_signal = Kp * error + Ki * (*integral) + Kd * derivative * 0.1;
+    float control_signal = Kp * error + Ki * (*integral) + Kd * derivative;
+
+    *prev_error = error; // LE
+
+    return control_signal;
+}
+
+int
+main() {
+    float setpoint = 100.0f;     // Desired position
+    float current_value = 0.0f;  // Current position
+    float integral = 0.0f;       // Integral term
+    float prev_error = 0.0f;     // Previous error term
+
+    float time_step = 0.1f;
+    int num_iterations = 100;
+
+    // Missing init (SE)
+    stdio_init_all();
+
+    // Simulate the control loop
+    for (int i = 0; i < num_iterations; i++) {
+        // Syntax error, prev_error pass by reference missing & (SE)
+//        float control_signal = compute_pid(setpoint, current_value, &integral, prev_error);
+        float control_signal = compute_pid(
+                setpoint,
+                current_value,
+                &integral,
+                &prev_error
+                );
+
+        // Pseudocode shows * 0.1 (LE)
+//        float motor_response = control_signal * 0.05;  // Motor response model
+        float motor_response = control_signal * 0.1;
+
+        current_value += motor_response; // LE
+
+        // SE: Control Signal = %f, not %d
+        printf("Iteration %d: Control Signal = %f, Current Position = %f\n",
+               i,
+               control_signal,
+               current_value
+               );
+
+
+        // Sleep for time_step seconds missing (LE)?
+        sleep_ms((int)(time_step * 1000));
+//        usleep((useconds_t)(time_step));
+
+    }
+
+    return 0;
+}