B4L/feeder_mk2
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

bugfixes

Browse Source
This commit is contained in:
janik
2026-02-27 17:31:42 +07:00
parent 87bd6ef982
commit eddce0bc68
13 changed files with 15687 additions and 14973 deletions
Download Patch File Download Diff File Expand all files Collapse all files

342
code/Core/Src/main.c
View File

@@ -145,12 +145,12 @@ uint8_t DMA_buffer[64];
uint8_t my_address = 0xFF;
int32_t total_count = 0;
int32_t target_count = 0;
int32_t kp = 4;
int32_t kp = 6;
int32_t ki = 1;
int32_t kd = 0;
int32_t i_min = -500;
int32_t i_max = 500;
int32_t pid_max_step = 10;
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];
@@ -178,6 +178,12 @@ 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
@@ -229,6 +235,7 @@ 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);
@@ -327,8 +334,10 @@ int main(void)
HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL);
// Start PWM timer (TIM1) for motor control
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
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);
@@ -373,147 +382,167 @@ int main(void)
{
if ((driving_direction && sw2_state) || (!driving_direction && sw1_state))
{
// Button released, stop driving
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 // Not currently driving, check for button presses
{
// Check for both buttons pressed (long hold)
if (sw1_pressed && sw2_pressed && !sw1_state && !sw2_state)
else if (!drive_mode)
{
uint16_t time1 = htim16.Instance->CNT;
uint16_t time2 = htim17.Instance->CNT;
uint16_t min_time = (time1 < time2) ? time1 : time2;
if (min_time > 500 && !both_pressed_handled)
// 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)
{
// Both long press - toggle drive 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
}
show_version();
}
else if (both_pressed_handled)
else if (hold_time > 4000 && hold_time < 6000)
{
uint32_t hold_time = HAL_GetTick() - both_pressed_start;
if (hold_time > 2000 && hold_time < 2100)
{
// Show version after 2 seconds
show_version();
}
else if (hold_time > 4000 && hold_time < 6000)
{
// Flash warning for bootloader
set_LED((hold_time / 100) % 2, 0, !((hold_time / 100) % 2));
}
else if (hold_time >= 6000)
{
// Reboot into bootloader
set_LED(1, 0, 1); // Magenta
HAL_Delay(100);
NVIC_SystemReset();
}
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();
}
}
// SW1 (bottom button) handling
else if (sw1_pressed)
{
uint16_t time_pressed = htim16.Instance->CNT;
}
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;
if (!sw1_state && time_pressed > 500 && !sw1_long_handled) // button still pressed, long press
// 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)
{
// Long press SW1 - continuous backward
sw1_long_handled = 1;
set_LED(1, 1, 1); // White while driving
set_LED(1, 1, 1);
if (drive_mode)
{
peel_motor(0); // peel backward
}
peel_motor(0);
else
{
drive_continuous(0); // drive backward
}
drive_continuous(0);
driving = 1;
driving_direction = 0;
}
else if (sw1_state && time_pressed <= 500 && time_pressed > 50) // released, short press
else if (sw1_state && time1 <= 2000 && time1 > 100)
{
// Short press SW1 - feed backward 2mm
set_LED(1, 1, 1);
start_feed(20, 0); // 20 tenths = 2mm backward
start_feed(20, 0);
HAL_TIM_Base_Stop(&htim16);
sw1_pressed = 0;
sw1_long_handled = 0;
}
else if (sw1_state) // released
else if (sw1_state)
{
HAL_TIM_Base_Stop(&htim16);
sw1_pressed = 0;
sw1_long_handled = 0;
if (!driving) set_LED(0, 0, 0);
}
}
// SW2 (top button) handling
else if (sw2_pressed)
else if (sw2_pressed && !sw1_pressed)
{
uint16_t time_pressed = htim17.Instance->CNT;
if (!sw2_state && time_pressed > 500 && !sw2_long_handled) // button still pressed, long press
// Single SW2 handling
if (!sw2_state && time2 > 2000 && !sw2_long_handled)
{
// Long press SW2 - continuous forward
sw2_long_handled = 1;
set_LED(1, 1, 1); // White while driving
set_LED(1, 1, 1);
if (drive_mode)
{
peel_motor(1); // peel forward
}
peel_motor(1);
else
{
drive_continuous(1); // drive forward
}
drive_continuous(1);
driving = 1;
driving_direction = 1;
}
else if (sw2_state && time_pressed <= 500 && time_pressed > 50) // released, short press
else if (sw2_state && time2 <= 2000 && time2 > 100)
{
// Short press SW2 - feed forward 2mm
set_LED(1, 1, 1);
start_feed(20, 1); // 20 tenths = 2mm forward
start_feed(20, 1);
HAL_TIM_Base_Stop(&htim17);
sw2_pressed = 0;
sw2_long_handled = 0;
}
else if (sw2_state) // released
else if (sw2_state)
{
HAL_TIM_Base_Stop(&htim17);
sw2_pressed = 0;
sw2_long_handled = 0;
if (!driving) set_LED(0, 0, 0);
}
}
// Reset both_pressed_handled when buttons released
if ((sw1_state || !sw1_pressed) && (sw2_state || !sw2_pressed))
else if (sw1_state && sw2_state)
{
if (both_pressed_handled)
{
both_pressed_handled = 0;
HAL_Delay(50); // debounce
set_LED(0, 0, 0);
}
// 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();
@@ -1002,15 +1031,7 @@ static void MX_GPIO_Init(void)
HAL_NVIC_EnableIRQ(EXTI4_15_IRQn);
/* USER CODE BEGIN MX_GPIO_Init_2 */
/*Configure GPIO pins : PEEL1_Pin PEEL2_Pin */
HAL_GPIO_WritePin(GPIOA, PEEL1_Pin|PEEL2_Pin, GPIO_PIN_RESET);
GPIO_InitStruct.Pin = PEEL1_Pin|PEEL2_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);
// PEEL1/PEEL2 (PA2/PA3) are TIM1_CH3/CH4 - configured by MX_TIM1_Init
/* USER CODE END MX_GPIO_Init_2 */
}
@@ -1092,12 +1113,13 @@ void HAL_TIM_PeriodElapsedCallback (TIM_HandleTypeDef * htim)
}
}
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
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
@@ -1108,6 +1130,7 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
if (!sw2_pressed)
{
htim17.Instance->CNT = 0;
HAL_TIM_Base_Start_IT(&htim17);
sw2_pressed = 1;
}
@@ -1482,8 +1505,12 @@ void update_Feeder_Target(int32_t difference)
#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;
@@ -1498,37 +1525,69 @@ void set_Feeder_PWM(uint16_t PWM, uint8_t direction)
void peel_motor(uint8_t forward)
{
if (forward)
{
HAL_GPIO_WritePin(PEEL1_GPIO_Port, PEEL1_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(PEEL2_GPIO_Port, PEEL2_Pin, GPIO_PIN_RESET);
}
else
{
HAL_GPIO_WritePin(PEEL1_GPIO_Port, PEEL1_Pin, GPIO_PIN_RESET);
HAL_GPIO_WritePin(PEEL2_GPIO_Port, PEEL2_Pin, GPIO_PIN_SET);
}
peel_target_pwm = forward ? PWM_MAX : -PWM_MAX;
}
void peel_brake(void)
{
// Both high = brake (or both low depending on driver)
HAL_GPIO_WritePin(PEEL1_GPIO_Port, PEEL1_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(PEEL2_GPIO_Port, PEEL2_Pin, GPIO_PIN_SET);
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 full PWM for continuous drive
// Bypass PID - set target far away in the desired direction so PID drives at max
pid_reset(&motor_pid);
if (forward)
{
htim1.Instance->CCR1 = 0;
htim1.Instance->CCR2 = PWM_MAX;
target_count = total_count + 10000;
}
else
{
htim1.Instance->CCR1 = PWM_MAX;
htim1.Instance->CCR2 = 0;
target_count = total_count - 10000;
}
}
@@ -1538,8 +1597,11 @@ void halt_all(void)
htim1.Instance->CCR1 = PWM_MAX;
htim1.Instance->CCR2 = PWM_MAX;
// Stop peel motor
peel_brake();
// 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);
@@ -1628,11 +1690,15 @@ void start_feed(int16_t distance_tenths, uint8_t forward)
if (forward)
{
// Forward feed: start with peeling
// 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
{
@@ -1658,27 +1724,13 @@ void feed_state_machine_update(void)
switch (feed_state)
{
case FEED_STATE_PEEL_FORWARD:
// Peeling film before forward drive
// Peeling film while feed motor drives simultaneously
if (elapsed >= feed_state_duration)
{
peel_motor(0); // Brief reverse peel
feed_state = FEED_STATE_PEEL_BACKOFF;
feed_state_start_time = now;
feed_state_duration = PEEL_BACKOFF_TIME;
}
break;
case FEED_STATE_PEEL_BACKOFF:
// Brief reverse peel to reduce tension
if (elapsed >= feed_state_duration)
{
peel_brake();
// Start driving
peel_brake(); // Peel done, feed motor continues via PID
feed_state = FEED_STATE_DRIVING;
feed_state_start_time = now;
feed_timeout_time = now + (feed_distance_tenths * TIMEOUT_TIME_PER_TENTH_MM) + 500;
feed_target_position = total_count + tenths_to_counts(feed_distance_tenths);
target_count = feed_target_position;
// feed_target_position and feed_timeout_time already set in start_feed
}
break;
@@ -2307,9 +2359,17 @@ void debug_output(void)
*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);