ควบคุมเซอร์โวด้วยโพเทนชิออมิเตอร์

// UnityMbed — ควบคุมเซอร์โวมอเตอร์ด้วยตัวต้านทานปรับค่าได้ (ADC Potentiometer) @ N32G031
#include "n32g031.h"
#include "n32g031_gpio.h"
#include "n32g031_rcc.h"
#include "n32g031_adc.h"
#include "n32g031_usart.h"
#include <stdio.h>

/* ====================================================================
 * [โซนตั้งค่าฮาร์ดแวร์]
 * ==================================================================== */
#define PULSE_MIN     2250
#define PULSE_MAX     5500
#define KNOB_MAX      4095

/* ฟังก์ชันหน่วงเวลา */
void Delay_Loop(volatile uint32_t count) {
  while(count--) { __NOP(); }
}

/* ฟังก์ชัน Kalman Filter สำหรับกรองสัญญาณรบกวน (1D Simple Kalman) */
float kalman_update(float measurement, float* state, float* pc) {
  float k_gain = *pc / (*pc + 0.1f);     // 0.1 คือ R (Measurement Noise) ปรับเพิ่มถ้าวอลลุ่มแกว่งมาก
  *pc = (1.0f - k_gain) * (*pc) + 0.01f; // 0.01 คือ Q (Process Noise)
  *state = *state + k_gain * (measurement - *state);
  return *state;
}

/* ฟังก์ชันตั้งค่าตัวหมุน (ADC Channel 0 บนขา PA0) */
void Setup_Knob(void) {
  RCC_EnableAPB2PeriphClk(RCC_APB2_PERIPH_GPIOA, ENABLE);
  RCC_EnableAHBPeriphClk(RCC_AHB_PERIPH_ADC, ENABLE);

  GPIO_InitType GPIO_InitStructure;
  GPIO_InitStruct(&GPIO_InitStructure);
  GPIO_InitStructure.Pin = GPIO_PIN_0;
  GPIO_InitStructure.GPIO_Mode = GPIO_MODE_ANALOG;
  GPIO_InitPeripheral(GPIOA, &GPIO_InitStructure);

  ADC_InitType ADC_InitStructure;
  ADC_InitStruct(&ADC_InitStructure);
  ADC_InitStructure.MultiChEn      = DISABLE;
  ADC_InitStructure.ContinueConvEn = DISABLE;
  ADC_InitStructure.ExtTrigSelect  = ADC_EXT_TRIGCONV_NONE;
  ADC_InitStructure.DatAlign       = ADC_DAT_ALIGN_R;
  ADC_InitStructure.ChsNumber      = 1;
  ADC_Init(ADC, &ADC_InitStructure);
  ADC_Enable(ADC, ENABLE);
}

/* ตั้งค่า PA9 เป็น USART1 TX สำหรับส่งข้อมูลไปยัง Serial Monitor */
void Setup_UART(void) {
  RCC_EnableAPB2PeriphClk(RCC_APB2_PERIPH_GPIOA | RCC_APB2_PERIPH_USART1, ENABLE);

  GPIO_InitType GPIO_InitStructure;
  GPIO_InitStruct(&GPIO_InitStructure);
  GPIO_InitStructure.Pin            = GPIO_PIN_9;
  GPIO_InitStructure.GPIO_Mode      = GPIO_MODE_AF_PP;
  GPIO_InitStructure.GPIO_Alternate = GPIO_AF4_USART1;
  GPIO_InitPeripheral(GPIOA, &GPIO_InitStructure);

  USART_InitType USART_InitStructure;
  USART_InitStructure.BaudRate            = 115200;
  USART_InitStructure.WordLength          = USART_WL_8B;
  USART_InitStructure.StopBits            = USART_STPB_1;
  USART_InitStructure.Parity              = USART_PE_NO;
  USART_InitStructure.HardwareFlowControl = USART_HFCTRL_NONE;
  USART_InitStructure.Mode                = USART_MODE_TX;
  USART_Init(USART1, &USART_InitStructure);

  USART_Enable(USART1, ENABLE);
}

/* ฟังก์ชันอ่านค่าจากตัวหมุน (คืนค่าเป็นตัวเลข 0 ถึง 4095) */
uint32_t Read_Knob(void) {
  ADC_ConfigRegularChannel(ADC, ADC_CH_0, 1, ADC_SAMP_TIME_56CYCLES5);
  ADC_EnableSoftwareStartConv(ADC, ENABLE);
  while(ADC_GetFlagStatus(ADC, ADC_FLAG_ENDC) == RESET);
  ADC_ClearFlag(ADC, ADC_FLAG_ENDC);
  return ADC_GetDat(ADC);
}

/* ส่งข้อความทีละตัวอักษรผ่าน UART ไปยัง Serial Monitor */
void UART_SendString(const char* str) {
  while(*str) {
      USART_SendData(USART1, (uint8_t)*str++);
      while (USART_GetFlagStatus(USART1, USART_FLAG_TXDE) == RESET);
  }
}

/* ฟังก์ชันส่งสัญญาณให้เซอร์โว */
void Servo_Step(uint32_t pulse_width) {
  GPIO_SetBits(GPIOA, GPIO_PIN_1);
  Delay_Loop(pulse_width);
  GPIO_ResetBits(GPIOA, GPIO_PIN_1);
  Delay_Loop(100000);
}

/* ====================================================================
 * [โซนทำงานหลัก]
 * ==================================================================== */
int main(void) {
  /* 1. เริ่มต้นระบบและอุปกรณ์ */
  Setup_UART();
  UART_SendString("\r\n[DEBUG] UART Initialized! Wiring is correct.\r\n");

  Setup_Knob();
  UART_SendString("[DEBUG] ADC Initialized. Reading baseline...\r\n");

  // ตั้งค่าขาควบคุมเซอร์โวมอเตอร์ PA1 เป็น Push-Pull Output
  GPIO_InitType GPIO_InitStructure;
  GPIO_InitStruct(&GPIO_InitStructure);
  GPIO_InitStructure.Pin = GPIO_PIN_1;
  GPIO_InitStructure.GPIO_Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitPeripheral(GPIOA, &GPIO_InitStructure);

  /* 2. เตรียมตัวแปร */
  uint32_t raw_knob = 0;
  uint32_t target_pulse = PULSE_MIN;

  // อ่านค่าเริ่มต้นเพื่อป้องกันเซอร์โวกระตุกตอนเปิดระบบ
  float k_state = (float)Read_Knob();
  float k_pc = 0.0f;

  char msg_buffer[64];
  uint32_t print_counter = 0; // จำกัดความถี่การส่งข้อมูล UART

  UART_SendString("System Initialized. Starting Servo Control...\r\n");

  /* 3. ลูปควบคุมหลัก */
  while(1) {
      // ขั้นตอน A: อ่านค่า ADC ดิบจาก Potentiometer
      raw_knob = Read_Knob();

      // ขั้นตอน B: ใช้ Kalman Filter ลด noise และ jitter
      kalman_update((float)raw_knob, &k_state, &k_pc);

      // ขั้นตอน C: แปลงค่า ADC 0-4095 เป็น Pulse Width 2250-5500 โดยกลับทิศทางให้ตรงกับการหมุนจริง
      target_pulse = PULSE_MAX - (((uint32_t)k_state * (PULSE_MAX - PULSE_MIN)) / KNOB_MAX);

      // ขั้นตอน D: ส่งพัลส์เพื่อขยับเซอร์โวไปยังตำแหน่งที่คำนวณได้
      Servo_Step(target_pulse);

      // ขั้นตอน E: ส่งข้อมูล Telemetry ทุก 50 รอบ เพื่อลดข้อความใน Serial Monitor
      if (++print_counter >= 50) {
          sprintf(msg_buffer, "Raw ADC: %4lu | Filtered: %4d | Pulse: %4lu\r\n",
                  raw_knob, (int)k_state, target_pulse);
          UART_SendString(msg_buffer);
          print_counter = 0; // รีเซ็ตตัวนับ
      }
  }
}