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

more code work

Browse Source
This commit is contained in:
janik
2026-01-05 17:09:40 +07:00
parent 70b20af874
commit 82539a83dc
14 changed files with 8744 additions and 8380 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -38,16 +38,19 @@
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
#define FEED_DISTANCE 127863 // pi * 40.7mm dia in um, per revolution
#define REVOLUTION_COUNT 370800 // 360 * 1030 (gear ratio)
#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
/* USER CODE END PD */
@@ -92,6 +95,9 @@ int32_t i_max = 500;
int32_t pid_max_step = 10;
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;
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
@@ -110,6 +116,7 @@ static void MX_TIM14_Init(void);
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);
/* USER CODE END PFP */
@@ -702,14 +709,29 @@ void HAL_TIM_PeriodElapsedCallback (TIM_HandleTypeDef * htim)
target_count+= INT32_MAX/4;
}
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);
set_Feeder_PWM(motor_cmd.pwm,motor_cmd.dir);
}
if (htim == &htim3) return; // PWM timer
if (htim == &htim1) return; // PWM timer
else if (htim == &htim3) // encoder overflow
{
// will this fire on rising / falling overflow the same ?
// will fire upon wraparound if update IT is enabled
}
if (htim == &htim16) //SW1 timer
{
@@ -775,6 +797,14 @@ void set_LED (uint8_t R, uint8_t G, uint8_t B)
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;
@@ -790,43 +820,201 @@ void handleRS485Message(uint8_t *buffer, uint8_t size)
{
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:
// no reply
update_Feeder_Target(feed_distance);
break;
case MOVE_FEED_BACKWARD:
// no reply
update_Feeder_Target(-feed_distance);
break;
case MOVE_FEED_STATUS:
/* TODO
* STATUS_WRONG_FEEDER_ID = 0x01,
STATUS_COULDNT_REACH = 0x02,
STATUS_UNINITIALIZED_FEEDER = 0x03,
STATUS_FEEDING_IN_PROGRESS = 0x04,
STATUS_FAIL = 0x05,
STATUS_TIMEOUT = 0xFE,
STATUS_UNKNOWN_ERROR = 0xFF
*/
break;
case VENDOR_OPTIONS:
memcpy(response.payload.vendorOptions.options,vendor_options,VENDOR_SPECIFIC_OPTIONS_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.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 length 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.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;
// todo: call identify function
HAL_UART_Transmit(&huart2,(uint8_t *)&response,packet_len,100);
break;
case PROGRAM_FEEDER_FLOOR:
// todo write address into EEPROM (1wire)
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;
}
}
void set_Feeder_PWM(uint16_t PWM, uint8_t direction)
{
if (direction)