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

more code

Browse Source
This commit is contained in:
janik
2025-12-19 17:49:10 +07:00
parent b92a583b4d
commit 70b20af874
44 changed files with 11261 additions and 7991 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -21,6 +21,12 @@
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "photon_protocol.h"
#include <stdio.h>
#include <string.h>
#include <stdint.h>
#include "pid.h"
#include "crc.h"
/* USER CODE END Includes */
@@ -37,6 +43,12 @@
#define CNT_MAX 65535
#define CNT_LIMIT_ZONE 1000
#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
/* USER CODE END PD */
/* Private macro -------------------------------------------------------------*/
@@ -51,16 +63,35 @@ TIM_HandleTypeDef htim3;
TIM_HandleTypeDef htim14;
TIM_HandleTypeDef htim16;
TIM_HandleTypeDef htim17;
DMA_HandleTypeDef hdma_tim3_up;
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 [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 = 4;
int32_t ki = 1;
int32_t kd = 0;
int32_t i_min = -500;
int32_t i_max = 500;
int32_t pid_max_step = 10;
pid_i32_t motor_pid;
pid_motor_cmd_t motor_cmd;
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
@@ -76,6 +107,10 @@ 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);
/* USER CODE END PFP */
/* Private user code ---------------------------------------------------------*/
@@ -101,6 +136,8 @@ int main(void)
/* 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 */
@@ -122,12 +159,67 @@ int main(void)
MX_TIM14_Init();
/* USER CODE BEGIN 2 */
uint32_t * puuid = (uint32_t *)UUID;
*puuid = HAL_GetUIDw0();
*(puuid+1) = HAL_GetUIDw1();
*(puuid+2) = HAL_GetUIDw2();
HAL_UARTEx_ReceiveToIdle_DMA (&huart2,DMA_buffer,64);
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
if (sw1_pressed)
{
uint16_t time_pressed = htim16.Instance->CNT;
if ((HAL_GPIO_ReadPin(SW1_GPIO_Port,SW1_Pin)) && (time_pressed > 750)) // button still pressed
{
// todo long button action
}
else if (!HAL_GPIO_ReadPin(SW1_GPIO_Port,SW1_Pin) && (time_pressed <= 750) && (time_pressed >75)) // release in short window
{
// todo short button action
sw1_pressed = 0;
}
else if (!HAL_GPIO_ReadPin(SW1_GPIO_Port,SW1_Pin)) sw1_pressed = 0; // release in long window
}
if (sw2_pressed)
{
uint16_t time_pressed = htim17.Instance->CNT;
if ((HAL_GPIO_ReadPin(SW2_GPIO_Port,SW2_Pin)) && (time_pressed > 750)) // button still pressed
{
// todo long button action
}
else if (!HAL_GPIO_ReadPin(SW2_GPIO_Port,SW2_Pin) && (time_pressed <= 750)) // release in short window
{
// todo short button action
sw2_pressed = 0;
}
else if (!HAL_GPIO_ReadPin(SW2_GPIO_Port,SW2_Pin)) sw2_pressed = 0; // release in long window
}
if (!msg_buffer2_empty) // msg 2 buffer has a message
{
// decode message from msg_buffer2
for (uint8_t i = 0; i<64 ; i++)
{
msg_buffer2[i]=0;
}
msg_buffer2_empty = 1;
}
if (!msg_buffer1_empty)
{
for (uint8_t i = 0; i<64 ; i++)
{
msg_buffer1[i]=0;
}
msg_buffer1_empty = 1;
}
/* USER CODE END WHILE */
@@ -476,7 +568,7 @@ static void MX_USART2_UART_Init(void)
/* USER CODE END USART2_Init 1 */
huart2.Instance = USART2;
huart2.Init.BaudRate = 115200;
huart2.Init.BaudRate = 57600;
huart2.Init.WordLength = UART_WORDLENGTH_8B;
huart2.Init.StopBits = UART_STOPBITS_1;
huart2.Init.Parity = UART_PARITY_NONE;
@@ -490,11 +582,11 @@ static void MX_USART2_UART_Init(void)
{
Error_Handler();
}
if (HAL_UARTEx_SetTxFifoThreshold(&huart2, UART_TXFIFO_THRESHOLD_1_8) != HAL_OK)
if (HAL_UARTEx_SetTxFifoThreshold(&huart2, UART_TXFIFO_THRESHOLD_1_2) != HAL_OK)
{
Error_Handler();
}
if (HAL_UARTEx_SetRxFifoThreshold(&huart2, UART_RXFIFO_THRESHOLD_1_8) != HAL_OK)
if (HAL_UARTEx_SetRxFifoThreshold(&huart2, UART_RXFIFO_THRESHOLD_1_2) != HAL_OK)
{
Error_Handler();
}
@@ -521,6 +613,9 @@ static void MX_DMA_Init(void)
/* 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);
}
@@ -595,8 +690,20 @@ void HAL_TIM_PeriodElapsedCallback (TIM_HandleTypeDef * htim)
return;
}
int32_t total_count = (encoder_count_extra * CNT_MAX) + count;
// todo run PID on target position with previous error and current error (assume previous target == current target)
total_count = (encoder_count_extra * CNT_MAX) + count;
if (total_count > INT32_MAX/2)
{
total_count-= INT32_MAX/4;
target_count-= INT32_MAX/4;
}
else if (total_count < INT32_MIN/2)
{
total_count+= INT32_MAX/4;
target_count+= INT32_MAX/4;
}
motor_cmd = pid_update_motor(&motor_pid,target_count,total_count);
set_Feeder_PWM(motor_cmd.pwm,motor_cmd.dir);
}
if (htim == &htim3) return; // PWM timer
@@ -638,6 +745,102 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
}
}
void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size)
{
if (Size > 64) return; // todo error handling
if (msg_buffer1_empty)
{
memcpy(DMA_buffer,msg_buffer1,Size);
msg_buffer1_empty = 0;
}
else if (msg_buffer2_empty)
{
memcpy(DMA_buffer,msg_buffer2,Size);
msg_buffer2_empty = 0;
}
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 handleRS485Message(uint8_t *buffer, uint8_t size)
{
PhotonPacketHeader *header = (PhotonPacketHeader *) buffer;
// check if message is for this device or is broadcast
if (!(header->toAddress == PHOTON_NETWORK_BROADCAST_ADDRESS))
{
if (!(header->toAddress == my_address))
{
return;
}
}
else // this message is relevant to this device
{
PhotonCommand *command = (PhotonCommand *) buffer;
PhotonResponse response;
response.header.fromAddress = my_address;
response.header.packetId = command->header.packetId;
response.header.toAddress = command->header.fromAddress;
switch (command->commandId)
{
case GET_FEEDER_ID:
break;
case INITIALIZE_FEEDER:
break;
case GET_VERSION:
break;
case MOVE_FEED_FORWARD:
break;
case MOVE_FEED_BACKWARD:
break;
case MOVE_FEED_STATUS:
break;
case VENDOR_OPTIONS:
break;
case GET_FEEDER_ADDRESS:
break;
case IDENTIFY_FEEDER:
break;
case PROGRAM_FEEDER_FLOOR:
break;
case UNINITIALIZED_FEEDERS_RESPOND:
break;
default:
// todo error handling
break;
}
}
}
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;
}
}
/* USER CODE END 4 */