B4L/feeder_mk2
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
33426570bb93eb0543ebcd3ddcfa4c452b9fa299
feeder_mk2/code/Core/Src/main.c
janik eddce0bc68 bugfixes
2026-02-27 17:31:42 +07:00

2441 lines
66 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 40 // 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 5 // encoder counts tolerance for "arrived"
// Stall detection
#define STALL_HISTORY_SIZE 20 // Number of samples for stall detection
#define STALL_THRESHOLD 5 // Max tick delta to consider stalled
#define STALL_SAMPLE_INTERVAL_MS 1 // How often to sample position
// 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 ---------------------------------------------------------*/
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];
uint8_t msg_buffer1_empty = 1;
uint8_t msg_buffer2_empty = 1;
uint8_t msg_buffer1_size = 0;
uint8_t msg_buffer2_size = 0;
uint8_t msg_buffer1 [64];
uint8_t msg_buffer2 [64];
uint8_t DMA_buffer[64];
uint8_t my_address = 0xFF;
int32_t total_count = 0;
int32_t target_count = 0;
int32_t kp = 6;
int32_t ki = 1;
int32_t kd = 3;
int32_t i_min = -800;
int32_t i_max = 800;
int32_t pid_max_step = 25;
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;
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
// 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
// Tape type detection
uint8_t first_feed_since_load = 1; // flag for first feed calibration
uint8_t beefy_tape = 0; // thick tape detected
// 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
// Stall detection
int32_t stall_history[STALL_HISTORY_SIZE];
uint8_t stall_history_index = 0;
uint32_t last_stall_sample_time = 0;
uint8_t stall_cooldown = 0;
uint32_t stall_cooldown_time = 0;
uint8_t current_drive_value = 30; // Current PWM value during approach
// 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[80];
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);
/* 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);
void check_tape_loaded(void);
// Stall detection
void stall_detection_init(void);
uint8_t check_stall(int32_t current_tick);
void stall_detection_update(int32_t current_tick);
// OneWire / EEPROM functions
void onewire_delay_us(uint32_t us);
void onewire_set_output(void);
void onewire_set_input(void);
void onewire_write_low(void);
void onewire_write_high(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();
/* 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);
// Initialize stall detection
stall_detection_init();
// 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);
}
HAL_UARTEx_ReceiveToIdle_DMA (&huart2,DMA_buffer,64);
/* 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
}
}
if (!msg_buffer1_empty) // msg 1 buffer has a message
{
handleRS485Message(msg_buffer1, msg_buffer1_size);
for (uint8_t i = 0; i<64 ; i++)
{
msg_buffer1[i]=0;
}
msg_buffer1_size = 0;
msg_buffer1_empty = 1;
}
if (!msg_buffer2_empty) // msg 2 buffer has a message
{
handleRS485Message(msg_buffer2, msg_buffer2_size);
for (uint8_t i = 0; i<64 ; i++)
{
msg_buffer2[i]=0;
}
msg_buffer2_size = 0;
msg_buffer2_empty = 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 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, 0, 0) != 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_GPIOC_CLK_ENABLE();
__HAL_RCC_GPIOB_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 pins : USART2_NRE_Pin ONEWIRE_Pin */
GPIO_InitStruct.Pin = USART2_NRE_Pin|ONEWIRE_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(GPIOA, &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;
}
motor_cmd = pid_update_motor(&motor_pid,target_count,total_count);
debug_pid_output = motor_cmd.dir ? motor_cmd.pwm : -motor_cmd.pwm; // Capture for debug
set_Feeder_PWM(motor_cmd.pwm,motor_cmd.dir);
// 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
if (msg_buffer1_empty)
{
memcpy(msg_buffer1, DMA_buffer, Size);
msg_buffer1_empty = 0;
msg_buffer1_size = Size;
}
else if (msg_buffer2_empty)
{
memcpy(msg_buffer2, DMA_buffer, Size);
msg_buffer2_empty = 0;
msg_buffer2_size = Size;
}
else // no free buffer available todo error handling
{
return;
}
HAL_UARTEx_ReceiveToIdle_DMA(&huart2, DMA_buffer, 64);
}
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 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);
}
void handleRS485Message(uint8_t *buffer, uint8_t size)
{
PhotonPacketHeader *header = (PhotonPacketHeader *) buffer;
// Validate minimum packet size
if (size < sizeof(PhotonPacketHeader) + 1) // header + at least commandId
{
return; // packet too small
}
// 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)
{
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))
{
return; // message not for us
}
// this message is relevant to this device (unicast to us or broadcast)
{
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;
comp_crc_header(&crc,&response);
payload_ptr =(uint8_t*) &response.payload;
for (uint32_t i = 0; i<sizeof(response.payload.getFeederId)+1; i++)
{
CRC8_107_add(&crc,*(payload_ptr+i));
}
response.header.crc = CRC8_107_getChecksum(&crc);
response.header.payloadLength = sizeof(response.payload.getFeederId)+1; // +1 for the length byte
packet_len = sizeof(PhotonPacketHeader) + response.header.payloadLength;
HAL_UART_Transmit(&huart2,(uint8_t *)&response,packet_len,100);
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;
}
comp_crc_header(&crc,&response);
payload_ptr =(uint8_t*) &response.payload;
for (uint32_t i = 0; i<sizeof(response.payload.initializeFeeder)+1; i++)
{
CRC8_107_add(&crc,*(payload_ptr+i));
}
response.header.crc = CRC8_107_getChecksum(&crc);
response.header.payloadLength = sizeof(response.payload.initializeFeeder)+1; // +1 for the length byte
packet_len = sizeof(PhotonPacketHeader) + response.header.payloadLength;
HAL_UART_Transmit(&huart2,(uint8_t *)&response,packet_len,100);
break;
case GET_VERSION:
response.status = STATUS_OK;
response.payload.protocolVersion.version = PROTOCOL_VERSION;
comp_crc_header(&crc,&response);
payload_ptr =(uint8_t*) &response.payload;
for (uint32_t i = 0; i<sizeof(response.payload.protocolVersion)+1; i++)
{
CRC8_107_add(&crc,*(payload_ptr+i));
}
response.header.crc = CRC8_107_getChecksum(&crc);
response.header.payloadLength = sizeof(response.payload.protocolVersion)+1; // +1 for the length byte
packet_len = sizeof(PhotonPacketHeader) + response.header.payloadLength;
HAL_UART_Transmit(&huart2,(uint8_t *)&response,packet_len,100);
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;
HAL_UART_Transmit(&huart2, (uint8_t *)&response, packet_len, 100);
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;
HAL_UART_Transmit(&huart2, (uint8_t *)&response, packet_len, 100);
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;
HAL_UART_Transmit(&huart2, (uint8_t *)&response, packet_len, 100);
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;
HAL_UART_Transmit(&huart2, (uint8_t *)&response, packet_len, 100);
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;
HAL_UART_Transmit(&huart2,(uint8_t *)&response,packet_len,100);
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;
HAL_UART_Transmit(&huart2, (uint8_t *)&response, packet_len, 100);
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;
HAL_UART_Transmit(&huart2,(uint8_t *)&response,packet_len,100);
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;
HAL_UART_Transmit(&huart2,(uint8_t *)&response,packet_len,100);
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;
HAL_UART_Transmit(&huart2,(uint8_t *)&response,packet_len,100);
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;
HAL_UART_Transmit(&huart2, (uint8_t *)&response, packet_len, 100);
}
break;
case UNINITIALIZED_FEEDERS_RESPOND:
if (is_initialized) return;
memcpy(response.payload.getFeederId.uuid,UUID,UUID_LENGTH);
response.header.payloadLength=sizeof(response.payload)+1;
response.status=STATUS_OK;
payload_ptr =(uint8_t*) &response.payload;
for (uint32_t i = 0; i<sizeof(response.payload.getFeederId)+1; i++)
{
CRC8_107_add(&crc,*(payload_ptr+i));
}
response.header.crc = CRC8_107_getChecksum(&crc);
response.header.payloadLength = sizeof(response.payload.getFeederId)+1; // +1 for the length byte
packet_len = sizeof(PhotonPacketHeader) + response.header.payloadLength;
HAL_UART_Transmit(&huart2,(uint8_t *)&response,packet_len,100);
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;
}
}
#define FEED_PWM_MIN_THRESHOLD 840 // 35% of 2400 - below this, don't drive
void set_Feeder_PWM(uint16_t PWM, uint8_t direction)
{
if (PWM > 0 && PWM < FEED_PWM_MIN_THRESHOLD) PWM = 0;
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)
{
// Bypass PID - set target far away in the desired direction so PID drives at max
pid_reset(&motor_pid);
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;
// Reset PID state to prevent sudden movement
pid_reset(&motor_pid);
target_count = total_count; // Set target to current position
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 * 1000 * GEAR_RATIO * ENCODER_CPR) / UM_PER_REV
int64_t temp = ((int64_t)tenths * 1000 * 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);
if (first_feed_since_load)
{
time += 1000; // Extra time for tape detection
}
else
{
time += 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
}
// Check for first feed calibration
if (first_feed_since_load)
{
check_tape_loaded();
first_feed_since_load = 0;
}
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 and stall detection
{
int32_t error = feed_target_position - total_count;
if (error < 0) error = -error;
// Update stall detection
stall_detection_update(total_count);
// Check for stall and increase power if needed
if (check_stall(total_count) && !stall_cooldown)
{
current_drive_value += 3;
if (current_drive_value > 255) current_drive_value = 255;
stall_cooldown = 1;
stall_cooldown_time = now;
// If we're stalling a lot, mark as beefy tape
if (current_drive_value > 200)
{
beefy_tape = 1;
}
}
// Reset stall cooldown after 5ms
if (stall_cooldown && (now - stall_cooldown_time) > 5)
{
stall_cooldown = 0;
}
if (error < FEED_POSITION_TOLERANCE)
{
// Position reached
stall_detection_init(); // Reset stall detection
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 (now > feed_timeout_time)
{
// Timeout
feed_state = FEED_STATE_TIMEOUT;
}
}
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;
break;
case FEED_STATE_TIMEOUT:
// Handle timeout - could retry or fail
if (feed_retry_count < FEED_RETRY_LIMIT)
{
feed_retry_count++;
// Move back slightly and retry
target_count = total_count - tenths_to_counts(5); // Back up 0.5mm
HAL_Delay(50);
// Restart the feed
if (feed_direction)
{
feed_state = FEED_STATE_PEEL_FORWARD;
feed_state_start_time = HAL_GetTick();
feed_state_duration = feed_distance_tenths * PEEL_TIME_PER_TENTH_MM;
peel_motor(1);
}
else
{
feed_state = FEED_STATE_UNPEEL;
feed_state_start_time = HAL_GetTick();
feed_state_duration = feed_distance_tenths * BACKWARDS_PEEL_TIME_PER_TENTH_MM;
peel_motor(0);
}
}
else
{
// Retry limit exceeded
feed_state = FEED_STATE_IDLE;
feed_in_progress = 0;
last_feed_status = STATUS_COULDNT_REACH;
feed_just_completed = 1;
halt_all();
}
break;
default:
feed_state = FEED_STATE_IDLE;
break;
}
}
// Check what type of tape is loaded (thick vs thin)
void check_tape_loaded(void)
{
// Take up any backlash slack
set_Feeder_PWM(250, 0); // Brief reverse
HAL_Delay(2);
set_Feeder_PWM(20, 0);
HAL_Delay(100);
set_Feeder_PWM(0, 0);
HAL_Delay(10);
// Find starting threshold of movement
int32_t starting_count = total_count;
int moved_at = 256;
for (int pwm = 20; pwm < 255; pwm += 5)
{
set_Feeder_PWM(pwm, 0); // Drive backward
HAL_Delay(75);
int32_t diff = total_count - starting_count;
if (diff < 0) diff = -diff;
if (diff > 3) // Movement detected
{
moved_at = pwm;
break;
}
}
set_Feeder_PWM(0, 0);
HAL_Delay(10);
// Reset position
pid_reset(&motor_pid);
target_count = total_count;
if (moved_at > 140)
{
beefy_tape = 1; // Thick tape detected
}
else
{
beefy_tape = 0;
}
}
// 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;
}
}
// ============================================================================
// Stall Detection Functions
// ============================================================================
void stall_detection_init(void)
{
for (int i = 0; i < STALL_HISTORY_SIZE; i++)
{
stall_history[i] = 0;
}
stall_history_index = 0;
last_stall_sample_time = 0;
stall_cooldown = 0;
current_drive_value = beefy_tape ? 255 : 30;
}
void stall_detection_update(int32_t current_tick)
{
uint32_t now = HAL_GetTick();
if ((now - last_stall_sample_time) >= STALL_SAMPLE_INTERVAL_MS)
{
last_stall_sample_time = now;
stall_history[stall_history_index] = current_tick;
stall_history_index++;
if (stall_history_index >= STALL_HISTORY_SIZE)
{
stall_history_index = 0;
}
}
}
uint8_t check_stall(int32_t current_tick)
{
// Find min and max in history
int32_t min_val = stall_history[0];
int32_t max_val = stall_history[0];
for (int i = 1; i < STALL_HISTORY_SIZE; i++)
{
if (stall_history[i] < min_val) min_val = stall_history[i];
if (stall_history[i] > max_val) max_val = stall_history[i];
}
int32_t delta = max_val - min_val;
return (delta <= STALL_THRESHOLD);
}
// ============================================================================
// OneWire Functions (Bit-banging for DS2431 EEPROM)
// ============================================================================
// Microsecond delay using DWT cycle counter or busy loop
void onewire_delay_us(uint32_t us)
{
// Simple busy-wait delay - approximately calibrated for 48MHz
volatile uint32_t count = us * 12; // Adjust multiplier as needed
while (count--) { __NOP(); }
}
void onewire_set_output(void)
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
GPIO_InitStruct.Pin = ONEWIRE_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_OD; // Open-drain for OneWire
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
HAL_GPIO_Init(ONEWIRE_GPIO_Port, &GPIO_InitStruct);
}
void onewire_set_input(void)
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
GPIO_InitStruct.Pin = ONEWIRE_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_PULLUP;
HAL_GPIO_Init(ONEWIRE_GPIO_Port, &GPIO_InitStruct);
}
void onewire_write_low(void)
{
HAL_GPIO_WritePin(ONEWIRE_GPIO_Port, ONEWIRE_Pin, GPIO_PIN_RESET);
}
void onewire_write_high(void)
{
HAL_GPIO_WritePin(ONEWIRE_GPIO_Port, ONEWIRE_Pin, GPIO_PIN_SET);
}
uint8_t onewire_read_bit(void)
{
return HAL_GPIO_ReadPin(ONEWIRE_GPIO_Port, ONEWIRE_Pin);
}
// Reset pulse - returns 1 if device present, 0 if not
uint8_t onewire_reset(void)
{
uint8_t presence;
onewire_set_output();
onewire_write_low();
onewire_delay_us(ONEWIRE_DELAY_H); // 480us
onewire_set_input();
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)
{
onewire_set_output();
if (bit)
{
// Write 1: pull low briefly, then release
onewire_write_low();
onewire_delay_us(ONEWIRE_DELAY_A); // 6us
onewire_write_high();
onewire_delay_us(ONEWIRE_DELAY_B); // 64us
}
else
{
// Write 0: pull low for full slot
onewire_write_low();
onewire_delay_us(ONEWIRE_DELAY_C); // 60us
onewire_write_high();
onewire_delay_us(ONEWIRE_DELAY_D); // 10us
}
}
uint8_t onewire_read_bit_slot(void)
{
uint8_t bit;
onewire_set_output();
onewire_write_low();
onewire_delay_us(ONEWIRE_DELAY_A); // 6us
onewire_set_input();
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++ = '0' + beefy_tape;
*p++ = ','; *p++ = 'A'; *p++ = ':';
p += debug_hex8(my_address, p);
*p++ = ','; *p++ = 'B'; *p++ = ':';
*p++ = '0' + sw1_pressed;
*p++ = '0' + sw2_pressed;
// Raw GPIO pin state: 1=high(released), 0=low(pressed)
*p++ = ','; *p++ = 'G'; *p++ = ':';
*p++ = '0' + (uint8_t)HAL_GPIO_ReadPin(SW1_GPIO_Port, SW1_Pin);
*p++ = '0' + (uint8_t)HAL_GPIO_ReadPin(SW2_GPIO_Port, SW2_Pin);
*p++ = ','; *p++ = 'D'; *p++ = ':';
p += debug_itoa(current_drive_value, p);
*p++ = ','; *p++ = 'M'; *p++ = ':';
*p++ = '0' + drive_mode;
*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