/* USER CODE BEGIN Header */ /** ****************************************************************************** * @file : main.c * @brief : Main program body ****************************************************************************** * @attention * * Copyright (c) 2025 STMicroelectronics. * All rights reserved. * * This software is licensed under terms that can be found in the LICENSE file * in the root directory of this software component. * If no LICENSE file comes with this software, it is provided AS-IS. * ****************************************************************************** */ /* USER CODE END Header */ /* Includes ------------------------------------------------------------------*/ #include "main.h" /* Private includes ----------------------------------------------------------*/ /* USER CODE BEGIN Includes */ #include "photon_protocol.h" #include #include #include "pid.h" #include "crc.h" /* USER CODE END Includes */ /* Private typedef -----------------------------------------------------------*/ /* USER CODE BEGIN PTD */ /* USER CODE END PTD */ /* Private define ------------------------------------------------------------*/ /* USER CODE BEGIN PD */ #define UM_PER_REV 127863 // pi * 40.7mm dia in um, per revolution #define GEAR_RATIO 1030 // 1030 (gear ratio) #define CNT_MAX 65535 #define CNT_LIMIT_ZONE 1000 #define ENCODER_PPR 7 #define ENCODER_CPR ENCODER_PPR *4 #define UUID_LENGTH 12 // 12 8bit values #define PHOTON_NETWORK_CONTROLLER_ADDRESS 0x00 #define PHOTON_NETWORK_BROADCAST_ADDRESS 0xFF #define PWM_MAX 2400 #define MAX_PWM_DIFFERENCE 10 #define PROTOCOL_VERSION 1 // Feed timing constants (from original firmware) #define PEEL_TIME_PER_TENTH_MM 18 // ms per 0.1mm for forward peel #define BACKWARDS_PEEL_TIME_PER_TENTH_MM 30 // ms per 0.1mm for backward unpeel #define PEEL_BACKOFF_TIME 30 // ms to reverse peel after forward peel #define TIMEOUT_TIME_PER_TENTH_MM 100 // ms per 0.1mm before timeout #define BACKWARDS_FEED_FILM_SLACK_REMOVAL_TIME 350 // ms to take up slack after backward #define BACKLASH_COMP_TENTH_MM 10 // 1mm backlash compensation #define FEED_SETTLE_TIME 50 // ms to wait for position to settle #define FEED_POSITION_TOLERANCE 10 // encoder counts tolerance (~0.044mm) // OneWire timing (microseconds) #define ONEWIRE_DELAY_A 6 #define ONEWIRE_DELAY_B 64 #define ONEWIRE_DELAY_C 60 #define ONEWIRE_DELAY_D 10 #define ONEWIRE_DELAY_E 9 #define ONEWIRE_DELAY_F 55 #define ONEWIRE_DELAY_G 0 #define ONEWIRE_DELAY_H 480 #define ONEWIRE_DELAY_I 70 #define ONEWIRE_DELAY_J 410 // DS2431 EEPROM commands #define DS2431_SKIP_ROM 0xCC #define DS2431_READ_MEMORY 0xF0 #define DS2431_WRITE_SCRATCHPAD 0x0F #define DS2431_READ_SCRATCHPAD 0xAA #define DS2431_COPY_SCRATCHPAD 0x55 // Floor address EEPROM location #define FLOOR_ADDRESS_LOCATION 0x00 #define FLOOR_ADDRESS_NOT_PROGRAMMED 0x00 #define FLOOR_ADDRESS_NOT_DETECTED 0xFF // Position overflow prevention #define POSITION_OVERFLOW_THRESHOLD (INT32_MAX / 2) #define POSITION_RESET_AMOUNT (INT32_MAX / 4) // Feed state machine states typedef enum { FEED_STATE_IDLE = 0, FEED_STATE_PEEL_FORWARD, // Forward feed: peeling film FEED_STATE_PEEL_BACKOFF, // Forward feed: brief reverse peel FEED_STATE_UNPEEL, // Backward feed: unspooling film FEED_STATE_DRIVING, // Driving tape to target FEED_STATE_SLACK_REMOVAL, // Backward feed: taking up slack FEED_STATE_DRIVING_BACKLASH, // Backward feed: final forward approach FEED_STATE_SETTLING, // Waiting for position to settle FEED_STATE_COMPLETE, FEED_STATE_TIMEOUT } FeedState; /* USER CODE END PD */ /* Private macro -------------------------------------------------------------*/ /* USER CODE BEGIN PM */ /* USER CODE END PM */ /* Private variables ---------------------------------------------------------*/ ADC_HandleTypeDef hadc1; TIM_HandleTypeDef htim1; TIM_HandleTypeDef htim3; TIM_HandleTypeDef htim14; TIM_HandleTypeDef htim16; TIM_HandleTypeDef htim17; UART_HandleTypeDef huart1; UART_HandleTypeDef huart2; DMA_HandleTypeDef hdma_usart2_rx; DMA_HandleTypeDef hdma_usart2_tx; /* USER CODE BEGIN PV */ uint8_t sw1_pressed,sw2_pressed = 0; int32_t encoder_count_extra=0; uint16_t encoder_previous=0; uint8_t UUID[UUID_LENGTH]; uint8_t is_initialized=0; uint8_t network_buffer_RX[64]; #define MSG_BUF_COUNT 54 volatile uint8_t msg_buf_empty[MSG_BUF_COUNT]; // initialized in code volatile uint8_t msg_buf_size[MSG_BUF_COUNT] = {0}; uint8_t msg_buf[MSG_BUF_COUNT][64]; uint8_t DMA_buffer[64]; volatile uint16_t rx_msg_count = 0; uint8_t my_address = 0xFF; int32_t total_count = 0; int32_t target_count = 0; int32_t kp = 4; int32_t ki = 1; int32_t kd = 6; int32_t i_min = -400; int32_t i_max = 400; int32_t pid_max_step = 30; pid_i32_t motor_pid; pid_motor_cmd_t motor_cmd; uint8_t vendor_options[VENDOR_SPECIFIC_OPTIONS_LENGTH]; uint8_t feed_distance = 4; int32_t pid_add = 0; volatile int32_t brake_time_tenths = 30; // Adaptive braking in 0.1ms units (3.0ms initial) uint8_t last_feed_status = STATUS_OK; volatile uint8_t feed_in_progress = 0; volatile uint8_t feed_just_completed = 0; // flag set by ISR when feed completes volatile uint16_t feed_ok_count = 0; volatile uint16_t feed_fail_count = 0; volatile uint16_t feed_retry_total = 0; // Feed state machine volatile FeedState feed_state = FEED_STATE_IDLE; uint32_t feed_state_start_time = 0; // when current state started uint32_t feed_state_duration = 0; // how long current state should last uint32_t feed_timeout_time = 0; // absolute timeout for driving phase int16_t feed_distance_tenths = 0; // requested feed distance in 0.1mm uint8_t feed_direction = 1; // 1 = forward, 0 = backward int32_t feed_target_position = 0; // target encoder position for current phase uint8_t feed_retry_count = 0; // retry counter #define FEED_RETRY_LIMIT 3 // Version string const char VERSION_STRING[] = "2.0.0-dev"; // Peel motor ramp state #define PEEL_RAMP_TIME_MS 100 int16_t peel_target_pwm = 0; // Target: positive=fwd, negative=rev, 0=stop int16_t peel_current_pwm = 0; // Current ramped value uint32_t peel_last_ramp_time = 0; // Button/driving state uint8_t drive_mode = 0; // 0 = tape drive, 1 = film peel uint8_t driving = 0; // currently in continuous drive mode uint8_t driving_direction = 0; // 0 = backward, 1 = forward uint8_t sw1_long_handled = 0; // flag to prevent repeated long press triggers uint8_t sw2_long_handled = 0; uint8_t both_pressed_handled = 0; uint32_t both_pressed_start = 0; // timestamp for both-button hold timing // Floor address (from EEPROM) uint8_t floor_address = FLOOR_ADDRESS_NOT_DETECTED; uint8_t floor_address_status = 0; // 0=not detected, 1=not programmed, 2=valid // Position tracking for overflow prevention int32_t mm_position = 0; // Position in tenths of mm // Debug output #define DEBUG_OUTPUT_INTERVAL_MS 100 uint32_t last_debug_output_time = 0; uint8_t debug_enabled = 1; char debug_tx_buffer[96]; volatile int16_t debug_pid_output = 0; // Captured from ISR for debug /* USER CODE END PV */ /* Private function prototypes -----------------------------------------------*/ void SystemClock_Config(void); static void MX_GPIO_Init(void); static void MX_DMA_Init(void); static void MX_TIM1_Init(void); static void MX_TIM3_Init(void); static void MX_USART1_UART_Init(void); static void MX_USART2_UART_Init(void); static void MX_TIM16_Init(void); static void MX_TIM17_Init(void); static void MX_TIM14_Init(void); static void MX_ADC1_Init(void); /* USER CODE BEGIN PFP */ void set_LED (uint8_t R, uint8_t G, uint8_t B); void handleRS485Message(uint8_t *buffer, uint8_t size); void set_Feeder_PWM(uint16_t PWM, uint8_t direction); void update_Feeder_Target(int32_t difference); void peel_motor(uint8_t forward); void peel_brake(void); void peel_ramp_update(void); void drive_continuous(uint8_t forward); void halt_all(void); void identify_feeder(void); void show_version(void); // Feed state machine functions void start_feed(int16_t distance_tenths, uint8_t forward); void feed_state_machine_update(void); void set_feed_target_counts(int16_t distance_tenths); int32_t tenths_to_counts(int16_t tenths); uint16_t calculate_expected_feed_time(uint8_t distance, uint8_t forward); // Stall detection // OneWire / EEPROM functions void onewire_delay_us(uint32_t us); void onewire_drive_low(void); void onewire_release(void); uint8_t onewire_read_bit(void); uint8_t onewire_reset(void); void onewire_write_bit(uint8_t bit); void onewire_write_byte(uint8_t byte); uint8_t onewire_read_byte(void); uint8_t read_floor_address(void); uint8_t write_floor_address(uint8_t address); // Position management void reset_position_if_needed(void); void handle_vendor_options(uint8_t *options, uint8_t *response); // Debug output (lightweight, no printf) void debug_output(void); uint8_t debug_itoa(int32_t val, char *buf); uint8_t debug_hex8(uint8_t val, char *buf); /* USER CODE END PFP */ /* Private user code ---------------------------------------------------------*/ /* USER CODE BEGIN 0 */ /* USER CODE END 0 */ /** * @brief The application entry point. * @retval int */ int main(void) { /* USER CODE BEGIN 1 */ /* USER CODE END 1 */ /* MCU Configuration--------------------------------------------------------*/ /* Reset of all peripherals, Initializes the Flash interface and the Systick. */ HAL_Init(); /* USER CODE BEGIN Init */ pid_init(&motor_pid,kp,ki,kd,i_min,i_max,PWM_MAX,pid_max_step); /* USER CODE END Init */ /* Configure the system clock */ SystemClock_Config(); /* USER CODE BEGIN SysInit */ /* USER CODE END SysInit */ /* Initialize all configured peripherals */ MX_GPIO_Init(); MX_DMA_Init(); MX_TIM1_Init(); MX_TIM3_Init(); MX_USART1_UART_Init(); MX_USART2_UART_Init(); MX_TIM16_Init(); MX_TIM17_Init(); MX_TIM14_Init(); MX_ADC1_Init(); /* USER CODE BEGIN 2 */ uint32_t * puuid = (uint32_t *)UUID; *puuid = HAL_GetUIDw0(); *(puuid+1) = HAL_GetUIDw1(); *(puuid+2) = HAL_GetUIDw2(); // Start encoder timer (TIM3) in encoder mode HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL); // Start PWM timer (TIM1) for motor control HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1); // Feed motor HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2); // Feed motor HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3); // Peel motor HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4); // Peel motor // Start PID control timer (TIM14) with interrupt HAL_TIM_Base_Start_IT(&htim14); // Read floor address from EEPROM floor_address = read_floor_address(); if (floor_address == FLOOR_ADDRESS_NOT_DETECTED) { floor_address_status = 0; set_LED(1, 0, 0); // Red = EEPROM not detected } else if (floor_address == FLOOR_ADDRESS_NOT_PROGRAMMED) { floor_address_status = 1; set_LED(0, 0, 1); // Blue = not programmed } else { floor_address_status = 2; my_address = floor_address; set_LED(0, 1, 0); // Green briefly = valid address HAL_Delay(200); set_LED(0, 0, 0); } for (uint8_t i = 0; i < MSG_BUF_COUNT; i++) msg_buf_empty[i] = 1; HAL_UARTEx_ReceiveToIdle_DMA (&huart2,DMA_buffer,64); __HAL_DMA_DISABLE_IT(huart2.hdmarx, DMA_IT_HT); // Disable half-transfer interrupt __HAL_UART_ENABLE_IT(&huart2, UART_IT_ERR); // Enable error interrupt for overrun recovery /* USER CODE END 2 */ /* Infinite loop */ /* USER CODE BEGIN WHILE */ while (1) { uint8_t sw1_state = HAL_GPIO_ReadPin(SW1_GPIO_Port, SW1_Pin); // 1 = released, 0 = pressed uint8_t sw2_state = HAL_GPIO_ReadPin(SW2_GPIO_Port, SW2_Pin); // Handle ongoing continuous drive - stop when button released if (driving) { if ((driving_direction && sw2_state) || (!driving_direction && sw1_state)) { halt_all(); driving = 0; HAL_TIM_Base_Stop(&htim16); HAL_TIM_Base_Stop(&htim17); sw1_pressed = 0; sw2_pressed = 0; sw1_long_handled = 0; sw2_long_handled = 0; set_LED(0, 0, 0); } else if (!drive_mode) { // Tape mode: keep target ahead of current position if (driving_direction) target_count = total_count + 10000; else target_count = total_count - 10000; } } else if (both_pressed_handled) { // Both-press mode: wait for release, handle long-hold actions if (sw1_state && sw2_state) { // Both released - show mode color briefly then clear both_pressed_handled = 0; HAL_TIM_Base_Stop(&htim16); HAL_TIM_Base_Stop(&htim17); sw1_pressed = 0; sw2_pressed = 0; sw1_long_handled = 0; sw2_long_handled = 0; HAL_Delay(400); set_LED(0, 0, 0); } else { // Still holding - check for long-hold actions uint32_t hold_time = HAL_GetTick() - both_pressed_start; if (hold_time > 2000 && hold_time < 2100) { show_version(); } else if (hold_time > 4000 && hold_time < 6000) { set_LED((hold_time / 100) % 2, 0, !((hold_time / 100) % 2)); } else if (hold_time >= 6000) { set_LED(1, 0, 1); HAL_Delay(100); NVIC_SystemReset(); } } } else if (sw1_pressed || sw2_pressed) { // At least one button pressed - use decision window uint16_t time1 = sw1_pressed ? htim16.Instance->CNT : 0; uint16_t time2 = sw2_pressed ? htim17.Instance->CNT : 0; uint16_t max_time = (time1 > time2) ? time1 : time2; // Wait 100ms decision window before acting (unless already past it) if (max_time < 100) { // Still in decision window - do nothing yet } else if (sw1_pressed && sw2_pressed && !sw1_state && !sw2_state) { // Both pressed - toggle mode both_pressed_handled = 1; both_pressed_start = HAL_GetTick(); if (drive_mode) { drive_mode = 0; set_LED(0, 0, 1); // Blue = tape mode } else { drive_mode = 1; set_LED(1, 1, 0); // Yellow = peel mode } } else if (sw1_pressed && !sw2_pressed) { // Single SW1 handling if (!sw1_state && time1 > 2000 && !sw1_long_handled) { sw1_long_handled = 1; set_LED(1, 1, 1); if (drive_mode) peel_motor(0); else drive_continuous(0); driving = 1; driving_direction = 0; } else if (sw1_state && time1 <= 2000 && time1 > 100) { set_LED(1, 1, 1); start_feed(20, 0); HAL_TIM_Base_Stop(&htim16); sw1_pressed = 0; sw1_long_handled = 0; } else if (sw1_state) { HAL_TIM_Base_Stop(&htim16); sw1_pressed = 0; sw1_long_handled = 0; if (!driving) set_LED(0, 0, 0); } } else if (sw2_pressed && !sw1_pressed) { // Single SW2 handling if (!sw2_state && time2 > 2000 && !sw2_long_handled) { sw2_long_handled = 1; set_LED(1, 1, 1); if (drive_mode) peel_motor(1); else drive_continuous(1); driving = 1; driving_direction = 1; } else if (sw2_state && time2 <= 2000 && time2 > 100) { set_LED(1, 1, 1); start_feed(20, 1); HAL_TIM_Base_Stop(&htim17); sw2_pressed = 0; sw2_long_handled = 0; } else if (sw2_state) { HAL_TIM_Base_Stop(&htim17); sw2_pressed = 0; sw2_long_handled = 0; if (!driving) set_LED(0, 0, 0); } } else if (sw1_state && sw2_state) { // Both released without triggering both-press (one was released too fast) HAL_TIM_Base_Stop(&htim16); HAL_TIM_Base_Stop(&htim17); sw1_pressed = 0; sw2_pressed = 0; sw1_long_handled = 0; sw2_long_handled = 0; } } // Update feed state machine feed_state_machine_update(); // Ramp peel motor PWM peel_ramp_update(); // Debug output via USART1 debug_output(); // Turn off LED when feed completes if (feed_just_completed) { feed_just_completed = 0; if (last_feed_status == STATUS_OK) { set_LED(0, 0, 0); // Success - LED off // Reset position tracking to prevent overflow reset_position_if_needed(); } else { set_LED(1, 0, 0); // Error - LED red } } for (uint8_t bi = 0; bi < MSG_BUF_COUNT; bi++) { if (!msg_buf_empty[bi]) { handleRS485Message(msg_buf[bi], msg_buf_size[bi]); msg_buf_size[bi] = 0; msg_buf_empty[bi] = 1; } } /* USER CODE END WHILE */ /* USER CODE BEGIN 3 */ } /* USER CODE END 3 */ } /** * @brief System Clock Configuration * @retval None */ void SystemClock_Config(void) { RCC_OscInitTypeDef RCC_OscInitStruct = {0}; RCC_ClkInitTypeDef RCC_ClkInitStruct = {0}; __HAL_FLASH_SET_LATENCY(FLASH_LATENCY_1); /** Initializes the RCC Oscillators according to the specified parameters * in the RCC_OscInitTypeDef structure. */ RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI; RCC_OscInitStruct.HSIState = RCC_HSI_ON; RCC_OscInitStruct.HSIDiv = RCC_HSI_DIV1; RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT; if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) { Error_Handler(); } /** Initializes the CPU, AHB and APB buses clocks */ RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK |RCC_CLOCKTYPE_PCLK1; RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_HSI; RCC_ClkInitStruct.SYSCLKDivider = RCC_SYSCLK_DIV1; RCC_ClkInitStruct.AHBCLKDivider = RCC_HCLK_DIV1; RCC_ClkInitStruct.APB1CLKDivider = RCC_APB1_DIV1; if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_1) != HAL_OK) { Error_Handler(); } } /** * @brief ADC1 Initialization Function * @param None * @retval None */ static void MX_ADC1_Init(void) { /* USER CODE BEGIN ADC1_Init 0 */ /* USER CODE END ADC1_Init 0 */ ADC_ChannelConfTypeDef sConfig = {0}; /* USER CODE BEGIN ADC1_Init 1 */ /* USER CODE END ADC1_Init 1 */ /** Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion) */ hadc1.Instance = ADC1; hadc1.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV2; hadc1.Init.Resolution = ADC_RESOLUTION_12B; hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT; hadc1.Init.ScanConvMode = ADC_SCAN_SEQ_FIXED; hadc1.Init.EOCSelection = ADC_EOC_SINGLE_CONV; hadc1.Init.LowPowerAutoWait = DISABLE; hadc1.Init.LowPowerAutoPowerOff = DISABLE; hadc1.Init.ContinuousConvMode = DISABLE; hadc1.Init.NbrOfConversion = 1; hadc1.Init.DiscontinuousConvMode = DISABLE; hadc1.Init.ExternalTrigConv = ADC_SOFTWARE_START; hadc1.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE; hadc1.Init.DMAContinuousRequests = DISABLE; hadc1.Init.Overrun = ADC_OVR_DATA_PRESERVED; hadc1.Init.SamplingTimeCommon1 = ADC_SAMPLETIME_1CYCLE_5; hadc1.Init.OversamplingMode = DISABLE; hadc1.Init.TriggerFrequencyMode = ADC_TRIGGER_FREQ_HIGH; if (HAL_ADC_Init(&hadc1) != HAL_OK) { Error_Handler(); } /** Configure Regular Channel */ sConfig.Channel = ADC_CHANNEL_7; sConfig.Rank = ADC_RANK_CHANNEL_NUMBER; if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK) { Error_Handler(); } /** Configure Regular Channel */ sConfig.Channel = ADC_CHANNEL_22; if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK) { Error_Handler(); } /* USER CODE BEGIN ADC1_Init 2 */ /* USER CODE END ADC1_Init 2 */ } /** * @brief TIM1 Initialization Function * @param None * @retval None */ static void MX_TIM1_Init(void) { /* USER CODE BEGIN TIM1_Init 0 */ /* USER CODE END TIM1_Init 0 */ TIM_ClockConfigTypeDef sClockSourceConfig = {0}; TIM_MasterConfigTypeDef sMasterConfig = {0}; TIM_OC_InitTypeDef sConfigOC = {0}; TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig = {0}; /* USER CODE BEGIN TIM1_Init 1 */ /* USER CODE END TIM1_Init 1 */ htim1.Instance = TIM1; htim1.Init.Prescaler = 0; htim1.Init.CounterMode = TIM_COUNTERMODE_UP; htim1.Init.Period = 2400; htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; htim1.Init.RepetitionCounter = 0; htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; if (HAL_TIM_Base_Init(&htim1) != HAL_OK) { Error_Handler(); } sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL; if (HAL_TIM_ConfigClockSource(&htim1, &sClockSourceConfig) != HAL_OK) { Error_Handler(); } if (HAL_TIM_PWM_Init(&htim1) != HAL_OK) { Error_Handler(); } sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; sMasterConfig.MasterOutputTrigger2 = TIM_TRGO2_RESET; sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; if (HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig) != HAL_OK) { Error_Handler(); } sConfigOC.OCMode = TIM_OCMODE_PWM1; sConfigOC.Pulse = 0; sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH; sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET; sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET; if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_1) != HAL_OK) { Error_Handler(); } if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_2) != HAL_OK) { Error_Handler(); } if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_3) != HAL_OK) { Error_Handler(); } if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_4) != HAL_OK) { Error_Handler(); } sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE; sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE; sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF; sBreakDeadTimeConfig.DeadTime = 0; sBreakDeadTimeConfig.BreakState = TIM_BREAK_DISABLE; sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH; sBreakDeadTimeConfig.BreakFilter = 0; sBreakDeadTimeConfig.BreakAFMode = TIM_BREAK_AFMODE_INPUT; sBreakDeadTimeConfig.Break2State = TIM_BREAK2_DISABLE; sBreakDeadTimeConfig.Break2Polarity = TIM_BREAK2POLARITY_HIGH; sBreakDeadTimeConfig.Break2Filter = 0; sBreakDeadTimeConfig.Break2AFMode = TIM_BREAK_AFMODE_INPUT; sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE; if (HAL_TIMEx_ConfigBreakDeadTime(&htim1, &sBreakDeadTimeConfig) != HAL_OK) { Error_Handler(); } /* USER CODE BEGIN TIM1_Init 2 */ /* USER CODE END TIM1_Init 2 */ HAL_TIM_MspPostInit(&htim1); } /** * @brief TIM3 Initialization Function * @param None * @retval None */ static void MX_TIM3_Init(void) { /* USER CODE BEGIN TIM3_Init 0 */ /* USER CODE END TIM3_Init 0 */ TIM_Encoder_InitTypeDef sConfig = {0}; TIM_MasterConfigTypeDef sMasterConfig = {0}; /* USER CODE BEGIN TIM3_Init 1 */ /* USER CODE END TIM3_Init 1 */ htim3.Instance = TIM3; htim3.Init.Prescaler = 0; htim3.Init.CounterMode = TIM_COUNTERMODE_UP; htim3.Init.Period = 65535; htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; htim3.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; sConfig.EncoderMode = TIM_ENCODERMODE_TI12; sConfig.IC1Polarity = TIM_ICPOLARITY_RISING; sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI; sConfig.IC1Prescaler = TIM_ICPSC_DIV1; sConfig.IC1Filter = 0; sConfig.IC2Polarity = TIM_ICPOLARITY_RISING; sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI; sConfig.IC2Prescaler = TIM_ICPSC_DIV1; sConfig.IC2Filter = 0; if (HAL_TIM_Encoder_Init(&htim3, &sConfig) != HAL_OK) { Error_Handler(); } sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) != HAL_OK) { Error_Handler(); } /* USER CODE BEGIN TIM3_Init 2 */ /* USER CODE END TIM3_Init 2 */ } /** * @brief TIM14 Initialization Function * @param None * @retval None */ static void MX_TIM14_Init(void) { /* USER CODE BEGIN TIM14_Init 0 */ /* USER CODE END TIM14_Init 0 */ /* USER CODE BEGIN TIM14_Init 1 */ /* USER CODE END TIM14_Init 1 */ htim14.Instance = TIM14; htim14.Init.Prescaler = 480-1; htim14.Init.CounterMode = TIM_COUNTERMODE_UP; htim14.Init.Period = 50; htim14.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; htim14.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; if (HAL_TIM_Base_Init(&htim14) != HAL_OK) { Error_Handler(); } /* USER CODE BEGIN TIM14_Init 2 */ /* USER CODE END TIM14_Init 2 */ } /** * @brief TIM16 Initialization Function * @param None * @retval None */ static void MX_TIM16_Init(void) { /* USER CODE BEGIN TIM16_Init 0 */ /* USER CODE END TIM16_Init 0 */ /* USER CODE BEGIN TIM16_Init 1 */ /* USER CODE END TIM16_Init 1 */ htim16.Instance = TIM16; htim16.Init.Prescaler = 48000-1; htim16.Init.CounterMode = TIM_COUNTERMODE_UP; htim16.Init.Period = 65535; htim16.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; htim16.Init.RepetitionCounter = 0; htim16.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; if (HAL_TIM_Base_Init(&htim16) != HAL_OK) { Error_Handler(); } /* USER CODE BEGIN TIM16_Init 2 */ /* USER CODE END TIM16_Init 2 */ } /** * @brief TIM17 Initialization Function * @param None * @retval None */ static void MX_TIM17_Init(void) { /* USER CODE BEGIN TIM17_Init 0 */ /* USER CODE END TIM17_Init 0 */ /* USER CODE BEGIN TIM17_Init 1 */ /* USER CODE END TIM17_Init 1 */ htim17.Instance = TIM17; htim17.Init.Prescaler = 48000-1; htim17.Init.CounterMode = TIM_COUNTERMODE_UP; htim17.Init.Period = 65535; htim17.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; htim17.Init.RepetitionCounter = 0; htim17.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; if (HAL_TIM_Base_Init(&htim17) != HAL_OK) { Error_Handler(); } /* USER CODE BEGIN TIM17_Init 2 */ /* USER CODE END TIM17_Init 2 */ } /** * @brief USART1 Initialization Function * @param None * @retval None */ static void MX_USART1_UART_Init(void) { /* USER CODE BEGIN USART1_Init 0 */ /* USER CODE END USART1_Init 0 */ /* USER CODE BEGIN USART1_Init 1 */ /* USER CODE END USART1_Init 1 */ huart1.Instance = USART1; huart1.Init.BaudRate = 115200; huart1.Init.WordLength = UART_WORDLENGTH_8B; huart1.Init.StopBits = UART_STOPBITS_1; huart1.Init.Parity = UART_PARITY_NONE; huart1.Init.Mode = UART_MODE_TX_RX; huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE; huart1.Init.OverSampling = UART_OVERSAMPLING_16; huart1.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE; huart1.Init.ClockPrescaler = UART_PRESCALER_DIV1; huart1.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT; if (HAL_UART_Init(&huart1) != HAL_OK) { Error_Handler(); } if (HAL_UARTEx_SetTxFifoThreshold(&huart1, UART_TXFIFO_THRESHOLD_1_8) != HAL_OK) { Error_Handler(); } if (HAL_UARTEx_SetRxFifoThreshold(&huart1, UART_RXFIFO_THRESHOLD_1_8) != HAL_OK) { Error_Handler(); } if (HAL_UARTEx_DisableFifoMode(&huart1) != HAL_OK) { Error_Handler(); } /* USER CODE BEGIN USART1_Init 2 */ /* USER CODE END USART1_Init 2 */ } /** * @brief USART2 Initialization Function * @param None * @retval None */ static void MX_USART2_UART_Init(void) { /* USER CODE BEGIN USART2_Init 0 */ /* USER CODE END USART2_Init 0 */ /* USER CODE BEGIN USART2_Init 1 */ /* USER CODE END USART2_Init 1 */ huart2.Instance = USART2; huart2.Init.BaudRate = 57600; huart2.Init.WordLength = UART_WORDLENGTH_8B; huart2.Init.StopBits = UART_STOPBITS_1; huart2.Init.Parity = UART_PARITY_NONE; huart2.Init.Mode = UART_MODE_TX_RX; huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE; huart2.Init.OverSampling = UART_OVERSAMPLING_16; huart2.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE; huart2.Init.ClockPrescaler = UART_PRESCALER_DIV1; huart2.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT; if (HAL_RS485Ex_Init(&huart2, UART_DE_POLARITY_HIGH, 31, 31) != HAL_OK) { Error_Handler(); } if (HAL_UARTEx_SetTxFifoThreshold(&huart2, UART_TXFIFO_THRESHOLD_1_2) != HAL_OK) { Error_Handler(); } if (HAL_UARTEx_SetRxFifoThreshold(&huart2, UART_RXFIFO_THRESHOLD_1_2) != HAL_OK) { Error_Handler(); } if (HAL_UARTEx_DisableFifoMode(&huart2) != HAL_OK) { Error_Handler(); } /* USER CODE BEGIN USART2_Init 2 */ /* USER CODE END USART2_Init 2 */ } /** * Enable DMA controller clock */ static void MX_DMA_Init(void) { /* DMA controller clock enable */ __HAL_RCC_DMA1_CLK_ENABLE(); /* DMA interrupt init */ /* DMA1_Channel1_IRQn interrupt configuration */ HAL_NVIC_SetPriority(DMA1_Channel1_IRQn, 0, 0); HAL_NVIC_EnableIRQ(DMA1_Channel1_IRQn); /* DMA1_Channel2_3_IRQn interrupt configuration */ HAL_NVIC_SetPriority(DMA1_Channel2_3_IRQn, 0, 0); HAL_NVIC_EnableIRQ(DMA1_Channel2_3_IRQn); } /** * @brief GPIO Initialization Function * @param None * @retval None */ static void MX_GPIO_Init(void) { GPIO_InitTypeDef GPIO_InitStruct = {0}; /* USER CODE BEGIN MX_GPIO_Init_1 */ /* USER CODE END MX_GPIO_Init_1 */ /* GPIO Ports Clock Enable */ __HAL_RCC_GPIOF_CLK_ENABLE(); __HAL_RCC_GPIOA_CLK_ENABLE(); __HAL_RCC_GPIOB_CLK_ENABLE(); __HAL_RCC_GPIOC_CLK_ENABLE(); /*Configure GPIO pin Output Level */ HAL_GPIO_WritePin(GPIOA, USART2_NRE_Pin|ONEWIRE_Pin, GPIO_PIN_RESET); /*Configure GPIO pin Output Level */ HAL_GPIO_WritePin(GPIOB, LED_R_Pin|LED_B_Pin|LED_G_Pin, GPIO_PIN_RESET); /*Configure GPIO pin : USART2_NRE_Pin */ GPIO_InitStruct.Pin = USART2_NRE_Pin; GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; HAL_GPIO_Init(USART2_NRE_GPIO_Port, &GPIO_InitStruct); /*Configure GPIO pin : ONEWIRE_Pin */ GPIO_InitStruct.Pin = ONEWIRE_Pin; GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_OD; GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; HAL_GPIO_Init(ONEWIRE_GPIO_Port, &GPIO_InitStruct); /*Configure GPIO pins : LED_R_Pin LED_B_Pin LED_G_Pin */ GPIO_InitStruct.Pin = LED_R_Pin|LED_B_Pin|LED_G_Pin; GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); /*Configure GPIO pins : SW2_Pin SW1_Pin */ GPIO_InitStruct.Pin = SW2_Pin|SW1_Pin; GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING; GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); /* EXTI interrupt init*/ HAL_NVIC_SetPriority(EXTI4_15_IRQn, 0, 0); HAL_NVIC_EnableIRQ(EXTI4_15_IRQn); /* USER CODE BEGIN MX_GPIO_Init_2 */ // PEEL1/PEEL2 (PA2/PA3) are TIM1_CH3/CH4 - configured by MX_TIM1_Init /* USER CODE END MX_GPIO_Init_2 */ } /* USER CODE BEGIN 4 */ void HAL_TIM_PeriodElapsedCallback (TIM_HandleTypeDef * htim) { if (htim == &htim14) // encoder check timer (runs at 20khz) { uint16_t count = htim3.Instance->CNT; if ((encoder_previous > (CNT_MAX-CNT_LIMIT_ZONE)) && (count < CNT_LIMIT_ZONE)) // positive turnaround { encoder_count_extra ++; } else if ((encoder_previous < CNT_LIMIT_ZONE) && (count > CNT_MAX-CNT_LIMIT_ZONE)) // negative turnaround { encoder_count_extra --; } encoder_previous = count; // update previous for next cycle total_count = (encoder_count_extra * 65536) + count; // Position overflow prevention - reset counts when they get too large if (total_count > POSITION_OVERFLOW_THRESHOLD) { int32_t adjustment = POSITION_RESET_AMOUNT; total_count -= adjustment; target_count -= adjustment; feed_target_position -= adjustment; encoder_count_extra -= (adjustment / 65536); mm_position = 0; // Reset mm tracking too } else if (total_count < -POSITION_OVERFLOW_THRESHOLD) { int32_t adjustment = POSITION_RESET_AMOUNT; total_count += adjustment; target_count += adjustment; feed_target_position += adjustment; encoder_count_extra += (adjustment / 65536); mm_position = 0; } if (pid_add!=0) { int64_t temp = target_count + pid_add; pid_add = 0; if (temp < (INT32_MIN+10000)) { //todo throw error } else if(temp > (INT32_MAX-10000)) { //todo throw error } target_count = temp; } // Velocity-based predictive braking { // Measure velocity every 1ms (20 ISR cycles at 20kHz) static int32_t vel_prev_pos = 0; static uint8_t vel_counter = 0; static int32_t velocity = 0; // counts per 1ms vel_counter++; if (vel_counter >= 20) { velocity = total_count - vel_prev_pos; vel_prev_pos = total_count; vel_counter = 0; } // Detect standstill: position unchanged for 2ms static int32_t last_moved_pos = 0; static uint16_t still_counter = 0; if (total_count != last_moved_pos) { last_moved_pos = total_count; still_counter = 0; } else if (still_counter < 1000) { still_counter++; } uint8_t is_stopped = still_counter > 40; // 40 * 50us = 2ms int32_t error = target_count - total_count; int32_t brake_distance = velocity * brake_time_tenths / 10; if (error > FEED_POSITION_TOLERANCE && (is_stopped || error > brake_distance)) { set_Feeder_PWM(PWM_MAX, 1); // Full forward debug_pid_output = PWM_MAX; } else if (error < -FEED_POSITION_TOLERANCE) { // Significantly past target: reverse (retry backup) set_Feeder_PWM(PWM_MAX, 0); // Full reverse debug_pid_output = -PWM_MAX; } else { // Within tolerance: brake htim1.Instance->CCR1 = PWM_MAX; htim1.Instance->CCR2 = PWM_MAX; debug_pid_output = 0; } } // Note: Feed completion is now handled by feed_state_machine_update() in main loop } if (htim == &htim1) return; // PWM timer else if (htim == &htim3) // encoder overflow { // will fire upon wraparound if update IT is enabled } if (htim == &htim16) //SW1 timer { sw1_pressed = 0; //todo handle overflow after ~65seconds (48MHz / 48000) * } else if (htim == &htim17) //SW2 timer { //todo sw2_pressed = 0; } } void HAL_GPIO_EXTI_Falling_Callback(uint16_t GPIO_Pin) { if(GPIO_Pin == SW1_Pin) // SW1 (lower button) { if (!sw1_pressed) { htim16.Instance->CNT = 0; HAL_TIM_Base_Start_IT(&htim16); sw1_pressed = 1; // now the main loop has to sample sw1_pressed and act. It can check how long its been pressed by reading TIM->CNT // the main loop has to sample GPIO_IDR to check pin state if its still pressed to determine which function is to be called } } else if (GPIO_Pin == SW2_Pin) // SW2 (upper button) { if (!sw2_pressed) { htim17.Instance->CNT = 0; HAL_TIM_Base_Start_IT(&htim17); sw2_pressed = 1; } } } void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size) { if (Size > 64) return; // todo error handling rx_msg_count++; for (uint8_t i = 0; i < MSG_BUF_COUNT; i++) { if (msg_buf_empty[i]) { memcpy(msg_buf[i], DMA_buffer, Size); msg_buf_size[i] = Size; msg_buf_empty[i] = 0; break; } } // Always re-arm DMA — even if all buffers full, keep listening HAL_UARTEx_ReceiveToIdle_DMA(&huart2, DMA_buffer, 64); __HAL_DMA_DISABLE_IT(huart2.hdmarx, DMA_IT_HT); } volatile uint16_t uart_error_count = 0; void HAL_UART_ErrorCallback(UART_HandleTypeDef *huart) { if (huart->Instance == USART2) { uart_error_count++; // Clear all error flags and re-arm DMA __HAL_UART_CLEAR_FLAG(huart, UART_CLEAR_OREF | UART_CLEAR_FEF | UART_CLEAR_NEF | UART_CLEAR_PEF); HAL_UARTEx_ReceiveToIdle_DMA(&huart2, DMA_buffer, 64); __HAL_DMA_DISABLE_IT(huart2.hdmarx, DMA_IT_HT); } } void set_LED (uint8_t R, uint8_t G, uint8_t B) { if (R) R = GPIO_PIN_SET; if (G) G = GPIO_PIN_SET; if (B) B = GPIO_PIN_SET; HAL_GPIO_WritePin(LED_R_GPIO_Port,LED_R_Pin,R); HAL_GPIO_WritePin(LED_G_GPIO_Port,LED_G_Pin,G); HAL_GPIO_WritePin(LED_B_GPIO_Port,LED_B_Pin,B); } void rs485_transmit(uint8_t *data, uint16_t len) { // Turnaround delay — let controller switch to RX mode HAL_Delay(1); // Disable receiver, transmit, re-enable receiver, re-arm DMA HAL_GPIO_WritePin(USART2_NRE_GPIO_Port, USART2_NRE_Pin, GPIO_PIN_SET); // NRE high = receiver off HAL_UART_Transmit(&huart2, data, len, 100); HAL_GPIO_WritePin(USART2_NRE_GPIO_Port, USART2_NRE_Pin, GPIO_PIN_RESET); // NRE low = receiver on HAL_UARTEx_ReceiveToIdle_DMA(&huart2, DMA_buffer, 64); __HAL_DMA_DISABLE_IT(huart2.hdmarx, DMA_IT_HT); } void comp_crc_header(CRC8_107 *lcrc, PhotonResponse *lresponse) { CRC8_107_add(lcrc,lresponse->header.toAddress); CRC8_107_add(lcrc,lresponse->header.fromAddress); CRC8_107_add(lcrc,lresponse->header.packetId); CRC8_107_add(lcrc,lresponse->header.payloadLength); } volatile uint16_t drop_size = 0; volatile uint16_t drop_crc = 0; volatile uint16_t drop_addr = 0; volatile uint16_t msg_handled = 0; volatile uint8_t last_rx_size = 0; volatile uint8_t last_rx_to = 0; volatile uint8_t last_rx_cmd = 0; volatile uint8_t last_rx_bytes[8] = {0}; volatile uint8_t my_addr_bytes[8] = {0}; // trap: last packet addressed to us volatile uint8_t my_addr_size = 0; volatile uint8_t my_addr_crc_exp = 0; void handleRS485Message(uint8_t *buffer, uint8_t size) { PhotonPacketHeader *header = (PhotonPacketHeader *) buffer; last_rx_size = size; for (uint8_t i = 0; i < 8 && i < size; i++) last_rx_bytes[i] = buffer[i]; // Trap packets addressed to us (before any validation) if (size >= 1 && buffer[0] == my_address) { my_addr_size = size; for (uint8_t i = 0; i < 8 && i < size; i++) my_addr_bytes[i] = buffer[i]; } // Validate minimum packet size if (size < sizeof(PhotonPacketHeader) + 1) // header + at least commandId { drop_size++; return; // packet too small } last_rx_to = header->toAddress; // Validate CRC CRC8_107 rx_crc; CRC8_107_init(&rx_crc); CRC8_107_add(&rx_crc, header->toAddress); CRC8_107_add(&rx_crc, header->fromAddress); CRC8_107_add(&rx_crc, header->packetId); CRC8_107_add(&rx_crc, header->payloadLength); // Add payload bytes to CRC (everything after header) for (uint8_t i = 0; i < header->payloadLength; i++) { CRC8_107_add(&rx_crc, buffer[sizeof(PhotonPacketHeader) + i]); } if (CRC8_107_getChecksum(&rx_crc) != header->crc) { drop_crc++; if (header->toAddress == my_address) my_addr_crc_exp = CRC8_107_getChecksum(&rx_crc); return; // CRC mismatch, discard packet } // check if message is for this device or is broadcast if ((header->toAddress != PHOTON_NETWORK_BROADCAST_ADDRESS) && (header->toAddress != my_address)) { drop_addr++; return; // message not for us } // this message is relevant to this device (unicast to us or broadcast) msg_handled++; last_rx_cmd = buffer[sizeof(PhotonPacketHeader)]; // commandId { PhotonCommand *command = (PhotonCommand *) buffer; PhotonResponse response; CRC8_107 crc; CRC8_107_init(&crc); response.header.fromAddress = my_address; response.header.packetId = command->header.packetId; response.header.toAddress = command->header.fromAddress; uint8_t *payload_ptr; size_t packet_len; switch (command->commandId) { case GET_FEEDER_ID: memcpy(response.payload.getFeederId.uuid,UUID,UUID_LENGTH); response.status = STATUS_OK; response.header.payloadLength = sizeof(response.payload.getFeederId)+1; // +1 for status byte comp_crc_header(&crc,&response); payload_ptr =(uint8_t*) &response.status; for (uint32_t i = 0; ipayload.initializeFeeder.uuid,UUID_LENGTH) == 0) { is_initialized = 1; response.status = STATUS_OK; } else { response.status = STATUS_WRONG_FEEDER_ID; } response.header.payloadLength = sizeof(response.payload.initializeFeeder)+1; comp_crc_header(&crc,&response); payload_ptr =(uint8_t*) &response.status; for (uint32_t i = 0; ipayload.move.distance, 1); uint16_t exp_time_be = (exp_time >> 8) | (exp_time << 8); // byte swap for network order response.status = STATUS_OK; response.payload.expectedTimeToFeed.expectedFeedTime = exp_time_be; response.header.payloadLength = sizeof(response.payload.expectedTimeToFeed) + 1; comp_crc_header(&crc, &response); payload_ptr = (uint8_t*)&response.status; for (uint32_t i = 0; i < response.header.payloadLength; i++) { CRC8_107_add(&crc, *(payload_ptr + i)); } response.header.crc = CRC8_107_getChecksum(&crc); packet_len = sizeof(PhotonPacketHeader) + response.header.payloadLength; rs485_transmit((uint8_t *)&response, packet_len); start_feed(command->payload.move.distance, 1); } break; case MOVE_FEED_BACKWARD: if (!is_initialized) { response.status = STATUS_UNINITIALIZED_FEEDER; memcpy(response.payload.initializeFeeder.uuid, UUID, UUID_LENGTH); response.header.payloadLength = sizeof(response.payload.initializeFeeder) + 1; comp_crc_header(&crc, &response); payload_ptr = (uint8_t*)&response.status; for (uint32_t i = 0; i < response.header.payloadLength; i++) { CRC8_107_add(&crc, *(payload_ptr + i)); } response.header.crc = CRC8_107_getChecksum(&crc); packet_len = sizeof(PhotonPacketHeader) + response.header.payloadLength; rs485_transmit((uint8_t *)&response, packet_len); break; } { uint16_t exp_time = calculate_expected_feed_time(command->payload.move.distance, 0); uint16_t exp_time_be = (exp_time >> 8) | (exp_time << 8); // byte swap for network order response.status = STATUS_OK; response.payload.expectedTimeToFeed.expectedFeedTime = exp_time_be; response.header.payloadLength = sizeof(response.payload.expectedTimeToFeed) + 1; comp_crc_header(&crc, &response); payload_ptr = (uint8_t*)&response.status; for (uint32_t i = 0; i < response.header.payloadLength; i++) { CRC8_107_add(&crc, *(payload_ptr + i)); } response.header.crc = CRC8_107_getChecksum(&crc); packet_len = sizeof(PhotonPacketHeader) + response.header.payloadLength; rs485_transmit((uint8_t *)&response, packet_len); start_feed(command->payload.move.distance, 0); } break; case MOVE_FEED_STATUS: if (feed_in_progress) { response.status = STATUS_FEEDING_IN_PROGRESS; } else { response.status = last_feed_status; } response.header.payloadLength = 1; // only status byte comp_crc_header(&crc,&response); CRC8_107_add(&crc, response.status); response.header.crc = CRC8_107_getChecksum(&crc); packet_len = sizeof(PhotonPacketHeader) + response.header.payloadLength; rs485_transmit((uint8_t *)&response, packet_len); break; case VENDOR_OPTIONS: if (!is_initialized) { response.status = STATUS_UNINITIALIZED_FEEDER; memcpy(response.payload.initializeFeeder.uuid, UUID, UUID_LENGTH); response.header.payloadLength = sizeof(response.payload.initializeFeeder) + 1; comp_crc_header(&crc, &response); payload_ptr = (uint8_t*)&response.status; for (uint32_t i = 0; i < response.header.payloadLength; i++) { CRC8_107_add(&crc, *(payload_ptr + i)); } response.header.crc = CRC8_107_getChecksum(&crc); packet_len = sizeof(PhotonPacketHeader) + response.header.payloadLength; rs485_transmit((uint8_t *)&response, packet_len); break; } handle_vendor_options(command->payload.vendorOptions.options, response.payload.vendorOptions.options); response.status = STATUS_OK; comp_crc_header(&crc,&response); payload_ptr =(uint8_t*) &response.status; for (uint32_t i = 0; ipayload.getFeederAddress.uuid,UUID_LENGTH) == 0) { response.status = STATUS_OK; } else { return; // this makes no sense, but original code behaves like this } response.header.payloadLength = 1; // only status byte comp_crc_header(&crc,&response); CRC8_107_add(&crc,response.status); response.header.crc = CRC8_107_getChecksum(&crc); packet_len = sizeof(PhotonPacketHeader) + response.header.payloadLength; rs485_transmit((uint8_t *)&response, packet_len); break; case IDENTIFY_FEEDER: if(memcmp(UUID,command->payload.identifyFeeder.uuid,UUID_LENGTH) == 0) { response.status = STATUS_OK; } else { return; // UUID doesn't match, ignore } response.header.payloadLength = 1; // only status byte comp_crc_header(&crc,&response); CRC8_107_add(&crc,response.status); response.header.crc = CRC8_107_getChecksum(&crc); packet_len = sizeof(PhotonPacketHeader) + response.header.payloadLength; rs485_transmit((uint8_t *)&response, packet_len); identify_feeder(); break; case PROGRAM_FEEDER_FLOOR: { uint8_t new_address = command->payload.programFeederFloorAddress.address; uint8_t write_success = write_floor_address(new_address); if (write_success) { my_address = new_address; floor_address = new_address; floor_address_status = 2; response.status = STATUS_OK; } else { response.status = STATUS_FAIL; } response.header.payloadLength = 1; // only status byte comp_crc_header(&crc, &response); CRC8_107_add(&crc, response.status); response.header.crc = CRC8_107_getChecksum(&crc); packet_len = sizeof(PhotonPacketHeader) + response.header.payloadLength; rs485_transmit((uint8_t *)&response, packet_len); } break; case UNINITIALIZED_FEEDERS_RESPOND: if (is_initialized) return; memcpy(response.payload.getFeederId.uuid,UUID,UUID_LENGTH); response.status=STATUS_OK; response.header.payloadLength = sizeof(response.payload.getFeederId)+1; comp_crc_header(&crc,&response); payload_ptr =(uint8_t*) &response.status; for (uint32_t i = 0; i INT32_MAX/2) { //todo error return; } else if (temp < INT32_MIN/2) { //todo error return; } else { pid_add += temp; return; } } void set_Feeder_PWM(uint16_t PWM, uint8_t direction) { if (direction) { htim1.Instance->CCR1 = PWM; htim1.Instance->CCR2 = 0; } else { htim1.Instance->CCR1 = 0; htim1.Instance->CCR2 = PWM; } } void peel_motor(uint8_t forward) { peel_target_pwm = forward ? PWM_MAX : -PWM_MAX; } void peel_brake(void) { peel_target_pwm = 0; } void peel_ramp_update(void) { uint32_t now = HAL_GetTick(); uint32_t dt = now - peel_last_ramp_time; if (dt == 0) return; peel_last_ramp_time = now; if (peel_current_pwm == peel_target_pwm) return; // Step size: full range (PWM_MAX) in PEEL_RAMP_TIME_MS int16_t step = (int16_t)((int32_t)PWM_MAX * dt / PEEL_RAMP_TIME_MS); if (step < 1) step = 1; if (peel_target_pwm > peel_current_pwm) { peel_current_pwm += step; if (peel_current_pwm > peel_target_pwm) peel_current_pwm = peel_target_pwm; } else { peel_current_pwm -= step; if (peel_current_pwm < peel_target_pwm) peel_current_pwm = peel_target_pwm; } // Apply to TIM1 CH3/CH4 if (peel_current_pwm > 0) { htim1.Instance->CCR3 = peel_current_pwm; htim1.Instance->CCR4 = 0; } else if (peel_current_pwm < 0) { htim1.Instance->CCR3 = 0; htim1.Instance->CCR4 = -peel_current_pwm; } else { htim1.Instance->CCR3 = 0; htim1.Instance->CCR4 = 0; } } void drive_continuous(uint8_t forward) { // Set target far away so motor drives at full speed if (forward) target_count = total_count + 10000; else target_count = total_count - 10000; } void halt_all(void) { // Stop drive motor (brake) htim1.Instance->CCR1 = PWM_MAX; htim1.Instance->CCR2 = PWM_MAX; // Stop peel motor immediately peel_target_pwm = 0; peel_current_pwm = 0; htim1.Instance->CCR3 = 0; htim1.Instance->CCR4 = 0; // Set target to current position to prevent further movement target_count = total_count; pid_add = 0; } void identify_feeder(void) { // Flash LED white 3 times for (int i = 0; i < 3; i++) { set_LED(1, 1, 1); HAL_Delay(300); set_LED(0, 0, 0); HAL_Delay(300); } } void show_version(void) { // Flash green for release version set_LED(0, 1, 0); HAL_Delay(250); set_LED(0, 0, 0); HAL_Delay(250); } // Convert distance in tenths of mm to encoder counts int32_t tenths_to_counts(int16_t tenths) { // counts = (tenths * 100 * GEAR_RATIO * ENCODER_CPR) / UM_PER_REV // 1 tenth-mm = 100um int64_t temp = ((int64_t)tenths * 100 * GEAR_RATIO * ENCODER_CPR) / UM_PER_REV; return (int32_t)temp; } // Calculate expected feed time in milliseconds uint16_t calculate_expected_feed_time(uint8_t distance, uint8_t forward) { if (forward) { // Forward: peel time + peel backoff + drive time + extra for first feed uint16_t time = (distance * PEEL_TIME_PER_TENTH_MM) + PEEL_BACKOFF_TIME + (distance * TIMEOUT_TIME_PER_TENTH_MM) + 200; return time; } else { // Backward: unpeel + drive (with backlash) + slack removal return (distance * BACKWARDS_PEEL_TIME_PER_TENTH_MM) + ((distance + (BACKLASH_COMP_TENTH_MM * 2)) * TIMEOUT_TIME_PER_TENTH_MM) + BACKWARDS_FEED_FILM_SLACK_REMOVAL_TIME + 50; } } // Start a feed operation void start_feed(int16_t distance_tenths, uint8_t forward) { if (feed_state != FEED_STATE_IDLE) { return; // Already feeding } feed_distance_tenths = distance_tenths; feed_direction = forward; feed_retry_count = 0; feed_in_progress = 1; last_feed_status = STATUS_OK; set_LED(1, 1, 1); // White during feed if (forward) { // Forward feed: drive both motors simultaneously feed_state = FEED_STATE_PEEL_FORWARD; feed_state_start_time = HAL_GetTick(); feed_state_duration = distance_tenths * PEEL_TIME_PER_TENTH_MM; peel_motor(1); // Peel forward // Start feed motor at the same time feed_timeout_time = HAL_GetTick() + (distance_tenths * TIMEOUT_TIME_PER_TENTH_MM) + 500; feed_target_position = total_count + tenths_to_counts(distance_tenths); target_count = feed_target_position; } else { // Backward feed: start with unpeeling to give slack feed_state = FEED_STATE_UNPEEL; feed_state_start_time = HAL_GetTick(); feed_state_duration = distance_tenths * BACKWARDS_PEEL_TIME_PER_TENTH_MM; peel_motor(0); // Peel backward (unpeel) } } // Update feed state machine - called from main loop void feed_state_machine_update(void) { if (feed_state == FEED_STATE_IDLE) { return; } uint32_t now = HAL_GetTick(); uint32_t elapsed = now - feed_state_start_time; switch (feed_state) { case FEED_STATE_PEEL_FORWARD: // Peeling film while feed motor drives simultaneously if (elapsed >= feed_state_duration) { peel_brake(); // Peel done, feed motor continues via PID feed_state = FEED_STATE_DRIVING; feed_state_start_time = now; // feed_target_position and feed_timeout_time already set in start_feed } break; case FEED_STATE_UNPEEL: // Unpeeling film before backward drive if (elapsed >= feed_state_duration) { peel_brake(); // Start driving backward with backlash overshoot feed_state = FEED_STATE_DRIVING; feed_state_start_time = now; int16_t total_backward = feed_distance_tenths + BACKLASH_COMP_TENTH_MM; feed_timeout_time = now + (total_backward * TIMEOUT_TIME_PER_TENTH_MM) + 500; feed_target_position = total_count - tenths_to_counts(total_backward); target_count = feed_target_position; } break; case FEED_STATE_DRIVING: // Check for position reached, overshoot, and timeout { int32_t error = feed_target_position - total_count; int32_t abs_error = error < 0 ? -error : error; // Brake time adjustment based on error magnitude // <5: none, >=5: ±5, >=10: ±10, >=20: ±20 int32_t brake_adj = abs_error >= 20 ? 20 : abs_error >= 10 ? 10 : abs_error >= 5 ? 5 : 0; if (abs_error < FEED_POSITION_TOLERANCE) { // Position reached - fine-tune brake time based on where we stopped if (error < 0) // overshot brake_time_tenths += brake_adj; else if (error > 0 && brake_time_tenths > brake_adj) // undershot brake_time_tenths -= brake_adj; else if (error > 0) brake_time_tenths = 1; if (!feed_direction) { // Backward feed: take up slack then do final approach feed_state = FEED_STATE_SLACK_REMOVAL; feed_state_start_time = now; feed_state_duration = BACKWARDS_FEED_FILM_SLACK_REMOVAL_TIME; peel_motor(1); // Peel forward to take up slack } else { // Forward feed complete feed_state = FEED_STATE_SETTLING; feed_state_start_time = now; feed_state_duration = FEED_SETTLE_TIME; } } else if (error < -FEED_POSITION_TOLERANCE) { // Overshot - increase braking time, back up 1mm and re-approach brake_time_tenths += brake_adj; feed_retry_total++; target_count = total_count - tenths_to_counts(10); HAL_Delay(200); htim1.Instance->CCR1 = PWM_MAX; htim1.Instance->CCR2 = PWM_MAX; HAL_Delay(50); target_count = feed_target_position; feed_state_start_time = now; feed_timeout_time = HAL_GetTick() + (feed_distance_tenths * TIMEOUT_TIME_PER_TENTH_MM) + 500; } else if (now > feed_timeout_time) { // Fell short - decrease braking time, back up 1mm and re-approach if (brake_time_tenths > brake_adj) brake_time_tenths -= brake_adj; else brake_time_tenths = 1; feed_retry_total++; target_count = total_count - tenths_to_counts(10); HAL_Delay(200); htim1.Instance->CCR1 = PWM_MAX; htim1.Instance->CCR2 = PWM_MAX; HAL_Delay(50); target_count = feed_target_position; feed_state_start_time = now; feed_timeout_time = HAL_GetTick() + (feed_distance_tenths * TIMEOUT_TIME_PER_TENTH_MM) + 500; } } break; case FEED_STATE_SLACK_REMOVAL: // Taking up film slack after backward movement if (elapsed >= feed_state_duration) { peel_brake(); // Final forward approach for backlash compensation feed_state = FEED_STATE_DRIVING_BACKLASH; feed_state_start_time = now; feed_timeout_time = now + (BACKLASH_COMP_TENTH_MM * TIMEOUT_TIME_PER_TENTH_MM) + 200; // Move forward by backlash amount to final position feed_target_position = total_count + tenths_to_counts(BACKLASH_COMP_TENTH_MM); target_count = feed_target_position; } break; case FEED_STATE_DRIVING_BACKLASH: // Final forward approach after backward feed { int32_t error = feed_target_position - total_count; if (error < 0) error = -error; if (error < FEED_POSITION_TOLERANCE) { feed_state = FEED_STATE_SETTLING; feed_state_start_time = now; feed_state_duration = FEED_SETTLE_TIME; } else if (now > feed_timeout_time) { feed_state = FEED_STATE_TIMEOUT; } } break; case FEED_STATE_SETTLING: // Wait for position to settle if (elapsed >= feed_state_duration) { feed_state = FEED_STATE_COMPLETE; } break; case FEED_STATE_COMPLETE: feed_state = FEED_STATE_IDLE; feed_in_progress = 0; last_feed_status = STATUS_OK; feed_just_completed = 1; feed_ok_count++; break; case FEED_STATE_TIMEOUT: // Handle timeout - back up 1mm to clear backlash, then retry if (feed_retry_count < FEED_RETRY_LIMIT) { feed_retry_count++; feed_retry_total++; // Back up 1mm (10 tenths) to overcome backlash target_count = total_count - tenths_to_counts(10); HAL_Delay(200); // Let motor reverse past backlash // Brake and re-approach original target htim1.Instance->CCR1 = PWM_MAX; htim1.Instance->CCR2 = PWM_MAX; HAL_Delay(50); // Settle target_count = feed_target_position; feed_state = FEED_STATE_DRIVING; feed_state_start_time = now; feed_timeout_time = HAL_GetTick() + (feed_distance_tenths * TIMEOUT_TIME_PER_TENTH_MM) + 500; } else { // Retry limit exceeded feed_state = FEED_STATE_IDLE; feed_in_progress = 0; last_feed_status = STATUS_COULDNT_REACH; feed_just_completed = 1; feed_fail_count++; halt_all(); } break; default: feed_state = FEED_STATE_IDLE; break; } } // Handle vendor-specific options void handle_vendor_options(uint8_t *options, uint8_t *response) { uint8_t command = options[0]; // Commands 0x00-0x0F: Get firmware version string in chunks if (command <= 0x0F) { size_t version_len = strlen(VERSION_STRING); size_t start_index = command * VENDOR_SPECIFIC_OPTIONS_LENGTH; if (start_index < version_len) { size_t remaining = version_len - start_index; size_t copy_len = (remaining < VENDOR_SPECIFIC_OPTIONS_LENGTH) ? remaining : VENDOR_SPECIFIC_OPTIONS_LENGTH; memcpy(response, VERSION_STRING + start_index, copy_len); // Null-terminate if there's room if (copy_len < VENDOR_SPECIFIC_OPTIONS_LENGTH) { response[copy_len] = '\0'; } } else { memset(response, 0, VENDOR_SPECIFIC_OPTIONS_LENGTH); } return; } switch (command) { case 0x10: { // LED control uint8_t led_mask = options[1]; int blue = led_mask & 1; int green = (led_mask >> 1) & 1; int red = (led_mask >> 2) & 1; int set = (led_mask >> 3) & 1; if (set) { set_LED(red, green, blue); } break; } default: // Unknown command, return zeros memset(response, 0, VENDOR_SPECIFIC_OPTIONS_LENGTH); break; } } // ============================================================================ // OneWire Functions (Bit-banging for DS2431 EEPROM) // ============================================================================ // Microsecond delay using SysTick (cycle-accurate at 48MHz) void onewire_delay_us(uint32_t us) { uint32_t cycles = us * 48; uint32_t reload = SysTick->LOAD; uint32_t prev = SysTick->VAL; uint32_t elapsed = 0; while (elapsed < cycles) { uint32_t now = SysTick->VAL; if (now <= prev) elapsed += prev - now; else elapsed += prev + reload + 1 - now; prev = now; } } // Pin is always open-drain: SET = release (high-Z), RESET = drive low // Reading IDR works regardless since it's open-drain void onewire_drive_low(void) { ONEWIRE_GPIO_Port->BRR = ONEWIRE_Pin; // Direct register for speed } void onewire_release(void) { ONEWIRE_GPIO_Port->BSRR = ONEWIRE_Pin; // Direct register for speed } uint8_t onewire_read_bit(void) { return (ONEWIRE_GPIO_Port->IDR & ONEWIRE_Pin) ? 1 : 0; } // Reset pulse - returns 1 if device present, 0 if not uint8_t onewire_reset(void) { uint8_t presence; onewire_drive_low(); onewire_delay_us(ONEWIRE_DELAY_H); // 480us onewire_release(); onewire_delay_us(ONEWIRE_DELAY_I); // 70us presence = !onewire_read_bit(); // Device pulls low if present onewire_delay_us(ONEWIRE_DELAY_J); // 410us return presence; } void onewire_write_bit(uint8_t bit) { if (bit) { // Write 1: pull low briefly, then release onewire_drive_low(); onewire_delay_us(ONEWIRE_DELAY_A); // 6us onewire_release(); onewire_delay_us(ONEWIRE_DELAY_B); // 64us } else { // Write 0: pull low for full slot onewire_drive_low(); onewire_delay_us(ONEWIRE_DELAY_C); // 60us onewire_release(); onewire_delay_us(ONEWIRE_DELAY_D); // 10us } } uint8_t onewire_read_bit_slot(void) { uint8_t bit; onewire_drive_low(); onewire_delay_us(ONEWIRE_DELAY_A); // 6us onewire_release(); onewire_delay_us(ONEWIRE_DELAY_E); // 9us bit = onewire_read_bit(); onewire_delay_us(ONEWIRE_DELAY_F); // 55us return bit; } void onewire_write_byte(uint8_t byte) { for (int i = 0; i < 8; i++) { onewire_write_bit(byte & 0x01); byte >>= 1; } } uint8_t onewire_read_byte(void) { uint8_t byte = 0; for (int i = 0; i < 8; i++) { byte >>= 1; if (onewire_read_bit_slot()) { byte |= 0x80; } } return byte; } // Read floor address from DS2431 EEPROM uint8_t read_floor_address(void) { // Disable interrupts for timing-critical OneWire __disable_irq(); if (!onewire_reset()) { __enable_irq(); return FLOOR_ADDRESS_NOT_DETECTED; } // Skip ROM (only one device on bus) onewire_write_byte(DS2431_SKIP_ROM); // Read Memory command onewire_write_byte(DS2431_READ_MEMORY); // Address (2 bytes, LSB first) onewire_write_byte(FLOOR_ADDRESS_LOCATION & 0xFF); onewire_write_byte((FLOOR_ADDRESS_LOCATION >> 8) & 0xFF); // Read the data byte uint8_t address = onewire_read_byte(); __enable_irq(); return address; } // Write floor address to DS2431 EEPROM uint8_t write_floor_address(uint8_t address) { // Disable interrupts for timing-critical OneWire __disable_irq(); if (!onewire_reset()) { __enable_irq(); return 0; // Device not present } // Skip ROM onewire_write_byte(DS2431_SKIP_ROM); // Write Scratchpad command onewire_write_byte(DS2431_WRITE_SCRATCHPAD); // Address (2 bytes, LSB first) onewire_write_byte(FLOOR_ADDRESS_LOCATION & 0xFF); onewire_write_byte((FLOOR_ADDRESS_LOCATION >> 8) & 0xFF); // Write the data (8 bytes for DS2431, we only need 1) onewire_write_byte(address); for (int i = 1; i < 8; i++) { onewire_write_byte(0xFF); // Pad with 0xFF } // Small delay onewire_delay_us(100); // Reset for next command if (!onewire_reset()) { __enable_irq(); return 0; } // Skip ROM onewire_write_byte(DS2431_SKIP_ROM); // Read Scratchpad to get authorization bytes onewire_write_byte(DS2431_READ_SCRATCHPAD); uint8_t ta1 = onewire_read_byte(); // Target address 1 uint8_t ta2 = onewire_read_byte(); // Target address 2 uint8_t es = onewire_read_byte(); // E/S register // Read back data to verify (8 bytes) uint8_t verify = onewire_read_byte(); if (verify != address) { __enable_irq(); return 0; // Scratchpad write failed } // Skip remaining bytes for (int i = 1; i < 8; i++) { onewire_read_byte(); } // Reset for copy command if (!onewire_reset()) { __enable_irq(); return 0; } // Skip ROM onewire_write_byte(DS2431_SKIP_ROM); // Copy Scratchpad command onewire_write_byte(DS2431_COPY_SCRATCHPAD); // Send authorization bytes onewire_write_byte(ta1); onewire_write_byte(ta2); onewire_write_byte(es); __enable_irq(); // Wait for copy to complete (10ms typical for EEPROM) HAL_Delay(15); // Verify by reading back uint8_t read_back = read_floor_address(); return (read_back == address) ? 1 : 0; } // ============================================================================ // Debug Output Functions (lightweight, no printf/snprintf) // ============================================================================ // Convert int32 to decimal string, returns length written uint8_t debug_itoa(int32_t val, char *buf) { char tmp[12]; uint8_t i = 0, len = 0; uint8_t neg = 0; if (val < 0) { neg = 1; val = -val; } if (val == 0) { tmp[i++] = '0'; } else { while (val > 0) { tmp[i++] = '0' + (val % 10); val /= 10; } } if (neg) { *buf++ = '-'; len++; } while (i > 0) { *buf++ = tmp[--i]; len++; } return len; } // Convert uint8 to 2-digit hex, returns 2 uint8_t debug_hex8(uint8_t val, char *buf) { const char hex[] = "0123456789ABCDEF"; buf[0] = hex[(val >> 4) & 0x0F]; buf[1] = hex[val & 0x0F]; return 2; } void debug_output(void) { if (!debug_enabled) return; uint32_t now = HAL_GetTick(); if ((now - last_debug_output_time) < DEBUG_OUTPUT_INTERVAL_MS) return; last_debug_output_time = now; // Format: $P:pos:tgt:err,I:pid,S:state,F:flags,A:addr,D:drv*\r\n char *p = debug_tx_buffer; *p++ = '$'; *p++ = 'P'; *p++ = ':'; p += debug_itoa(total_count, p); *p++ = ':'; p += debug_itoa(target_count, p); *p++ = ':'; p += debug_itoa(target_count - total_count, p); *p++ = ','; *p++ = 'I'; *p++ = ':'; p += debug_itoa(debug_pid_output, p); *p++ = ','; *p++ = 'S'; *p++ = ':'; *p++ = '0' + feed_state; // State as single digit 0-9 *p++ = ','; *p++ = 'F'; *p++ = ':'; *p++ = '0' + is_initialized; *p++ = '0' + feed_in_progress; *p++ = ','; *p++ = 'C'; *p++ = ':'; p += debug_itoa(feed_ok_count, p); *p++ = ':'; p += debug_itoa(feed_fail_count, p); *p++ = ':'; p += debug_itoa(brake_time_tenths, p); *p++ = ':'; p += debug_itoa(feed_retry_total, p); *p++ = ','; *p++ = 'A'; *p++ = ':'; p += debug_hex8(my_address, p); *p++ = ','; *p++ = 'R'; *p++ = ':'; p += debug_itoa(drop_crc, p); *p++ = ':'; p += debug_itoa(msg_handled, p); *p++ = ':'; p += debug_itoa(uart_error_count, p); *p++ = ','; *p++ = 'T'; *p++ = ':'; p += debug_itoa(my_addr_size, p); *p++ = ':'; for (uint8_t i = 0; i < 8 && i < my_addr_size; i++) p += debug_hex8(my_addr_bytes[i], p); *p++ = ':'; p += debug_hex8(my_addr_crc_exp, p); *p++ = '*'; *p++ = '\r'; *p++ = '\n'; HAL_UART_Transmit(&huart1, (uint8_t*)debug_tx_buffer, p - debug_tx_buffer, 10); } // ============================================================================ // Position Management // ============================================================================ void reset_position_if_needed(void) { // This is called from main loop periodically or after feed completes // to reset position tracking and prevent overflow // Only reset when position is at a "round" value to avoid drift int32_t counts_per_mm = tenths_to_counts(10); // counts per 1mm // Check if we're at a position that's a multiple of 1mm if ((total_count % counts_per_mm) == 0 && feed_state == FEED_STATE_IDLE) { // Calculate how many mm we've moved int32_t mm_moved = total_count / counts_per_mm; mm_position += mm_moved; // Reset encoder tracking __disable_irq(); encoder_count_extra = 0; htim3.Instance->CNT = 0; encoder_previous = 0; total_count = 0; target_count = 0; feed_target_position = 0; __enable_irq(); } } /* USER CODE END 4 */ /** * @brief This function is executed in case of error occurrence. * @retval None */ void Error_Handler(void) { /* USER CODE BEGIN Error_Handler_Debug */ /* User can add his own implementation to report the HAL error return state */ __disable_irq(); while (1) { } /* USER CODE END Error_Handler_Debug */ } #ifdef USE_FULL_ASSERT /** * @brief Reports the name of the source file and the source line number * where the assert_param error has occurred. * @param file: pointer to the source file name * @param line: assert_param error line source number * @retval None */ void assert_failed(uint8_t *file, uint32_t line) { /* USER CODE BEGIN 6 */ /* User can add his own implementation to report the file name and line number, ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */ /* USER CODE END 6 */ } #endif /* USE_FULL_ASSERT */