使用可變電阻控制伺服馬達

// UnityMbed — 使用可變電阻（ADC）即時控制伺服馬達 @ 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(); }
}

/* 用於抑制感測器雜訊的卡爾曼濾波函式（一維簡化卡爾曼濾波） */
float kalman_update(float measurement, float* state, float* pc) {
  float k_gain = *pc / (*pc + 0.1f);     // 0.1 為 R（量測雜訊）
  *pc = (1.0f - k_gain) * (*pc) + 0.01f; // 0.01 為 Q（程序雜訊）
  *state = *state + k_gain * (measurement - *state);
  return *state;
}

/* 初始化 PA0 上的可變電阻 ADC（ADC 通道 0） */
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，以便將資料傳送至序列埠監控視窗 */
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 逐字元傳送字串 */
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 初始化完成，接線正確。\r\n");

  Setup_Knob();
  UART_SendString("[DEBUG] ADC 初始化完成，正在讀取基準值……\r\n");

  // 將伺服馬達控制接腳 PA1 初始化為推挽輸出
  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("系統初始化完成，開始控制伺服馬達……\r\n");

  /* --- 3. 主控制迴圈 --- */
  while(1) {
      // 步驟 A：讀取可變電阻的原始類比值
      raw_knob = Read_Knob();

      // 步驟 B：套用卡爾曼濾波器以去除電氣雜訊與抖動
      kalman_update((float)raw_knob, &k_state, &k_pc);

      // 步驟 C：將濾波後的 ADC 值（0–4095）對應至伺服馬達脈衝寬度（2250–5500）
      // 注意：此處採用反向計算（PULSE_MAX - ...），以符合旋鈕的實際轉動方向
      target_pulse = PULSE_MAX - (((uint32_t)k_state * (PULSE_MAX - PULSE_MIN)) / KNOB_MAX);

      // 步驟 D：送出計算完成的脈衝以驅動伺服馬達
      Servo_Step(target_pulse);

      // 步驟 E：將遙測資料輸出至序列埠監控視窗（每 50 次迴圈輸出一次，以避免訊息過多）
      if (++print_counter >= 50) {
          sprintf(msg_buffer, "原始 ADC： %4lu | 濾波後： %4d | 脈衝： %4lu\r\n",
                  raw_knob, (int)k_state, target_pulse);
          UART_SendString(msg_buffer);
          print_counter = 0; // 重設計數器
      }
  }
}