Drive a servo with a potentiometer

// UnityMbed — Real-time Servo Control with Potentiometer (ADC) @ N32G031
#include "n32g031.h"
#include "n32g031_gpio.h"
#include "n32g031_rcc.h"
#include "n32g031_adc.h"
#include "n32g031_usart.h"
#include <stdio.h>

/* ====================================================================
 * [Hardware Configuration Zone]
 * ==================================================================== */
#define PULSE_MIN     2250
#define PULSE_MAX     5500
#define KNOB_MAX      4095

/* Simple software delay loop */
void Delay_Loop(volatile uint32_t count) {
  while(count--) { __NOP(); }
}

/* Kalman Filter function for sensor noise rejection (1D Simple Kalman) */
float kalman_update(float measurement, float* state, float* pc) {
  float k_gain = *pc / (*pc + 0.1f);     // 0.1 is R (Measurement Noise)
  *pc = (1.0f - k_gain) * (*pc) + 0.01f; // 0.01 is Q (Process Noise)
  *state = *state + k_gain * (measurement - *state);
  return *state;
}

/* Initialize Potentiometer ADC on PA0 (ADC Channel 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);
}

/* Configure PA9 as USART1 TX for sending data to the 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);
}

/* Read analog value from potentiometer knob (Returns 0 to 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);
}

/* Send a string character by character over UART */
void UART_SendString(const char* str) {
  while(*str) {
      USART_SendData(USART1, (uint8_t)*str++);
      while (USART_GetFlagStatus(USART1, USART_FLAG_TXDE) == RESET);
  }
}

/* Transmit control signal pulse to servo motor */
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);
}

/* ====================================================================
 * Main Application Loop
 * ==================================================================== */
int main(void) {
  /* --- 1. System Initialization --- */
  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");

  // Initialize Servo Motor Control Pin (PA1) as 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. Variable Setup --- */
  uint32_t raw_knob = 0;
  uint32_t target_pulse = PULSE_MIN;

  // Read initial baseline to prevent the servo from jerking on startup
  float k_state = (float)Read_Knob();
  float k_pc = 0.0f;

  char msg_buffer[64];
  uint32_t print_counter = 0; // Used to limit UART output frequency

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

  /* --- 3. Main Control Loop --- */
  while(1) {
      // Step A: Read raw analog value from the potentiometer
      raw_knob = Read_Knob();

      // Step B: Apply Kalman Filter to remove electrical noise/jitter
      kalman_update((float)raw_knob, &k_state, &k_pc);

      // Step C: Map the filtered ADC value (0-4095) to Servo Pulse Width (2250-5500)
      // Note: The math is inverted here (PULSE_MAX - ...) to match the physical knob direction
      target_pulse = PULSE_MAX - (((uint32_t)k_state * (PULSE_MAX - PULSE_MIN)) / KNOB_MAX);

      // Step D: Send the calculated pulse to physically move the servo
      Servo_Step(target_pulse);

      // Step E: Print telemetry data to Serial Monitor (Only once every 50 loops to avoid spam)
      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; // Reset counter
      }
  }
}