B4L/feeder_mk2
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
542a5ffd2c8c87c930c1a8a52eba8d5f6b398e77
feeder_mk2/code/Core/Src/main.c

2479 lines
68 KiB
C
Raw Blame History

/* 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 <string.h>
#include <stdint.h>
#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; 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;
case INITIALIZE_FEEDER:
memcpy(response.payload.initializeFeeder.uuid,UUID,UUID_LENGTH);
if(memcmp(UUID,command->payload.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; 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;
case GET_VERSION:
response.status = STATUS_OK;
response.payload.protocolVersion.version = PROTOCOL_VERSION;
response.header.payloadLength = sizeof(response.payload.protocolVersion)+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;
case MOVE_FEED_FORWARD:
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, 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; i<sizeof(response.payload.vendorOptions)+1; i++)
{
CRC8_107_add(&crc,*(payload_ptr+i));
}
response.header.crc = CRC8_107_getChecksum(&crc);
response.header.payloadLength = sizeof(response.payload.vendorOptions)+1; // +1 for the status byte
packet_len = sizeof(PhotonPacketHeader) + response.header.payloadLength;
rs485_transmit((uint8_t *)&response, packet_len);
break;
case GET_FEEDER_ADDRESS:
if(memcmp(UUID,command->payload.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<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;
default:
// todo error handling
return;
break;
}
}
}
void update_Feeder_Target(int32_t difference)
{
int64_t temp = (difference * 1000 * GEAR_RATIO * ENCODER_CPR) / UM_PER_REV;
if (temp == 0)
{
//todo error
return;
}
else if (temp > 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 */
Reference in New Issue View Git Blame Copy Permalink