more code work

code/Core/Src/main.c

@@ -38,16 +38,19 @@
 /* Private define ------------------------------------------------------------*/
 /* USER CODE BEGIN PD */
 
-#define FEED_DISTANCE 127863 // pi * 40.7mm dia in um, per revolution
+#define UM_PER_REV 127863 // pi * 40.7mm dia in um, per revolution
-#define REVOLUTION_COUNT 370800 // 360 * 1030 (gear ratio)
+#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)

code/Debug/Core/Src/crc.cyclo

@@ -0,0 +1,2 @@
+../Core/Src/crc.c:10:6:CRC8_107_add	3
+../Core/Src/crc.c:21:9:CRC8_107_getChecksum	1

code/Debug/Core/Src/crc.d

@@ -0,0 +1,2 @@
+Core/Src/crc.o: ../Core/Src/crc.c ../Core/Inc/crc.h
+../Core/Inc/crc.h:

code/Debug/Core/Src/crc.su

@@ -0,0 +1,2 @@
+../Core/Src/crc.c:10:6:CRC8_107_add	24	static
+../Core/Src/crc.c:21:9:CRC8_107_getChecksum	16	static

code/Debug/Core/Src/main.cyclo

@@ -1,21 +1,24 @@
 ../Core/Inc/pid.h:39:23:clamp_i32	3
 ../Core/Inc/pid.h:46:20:pid_init	1
 ../Core/Inc/pid.h:77:31:pid_update_motor	8
-../Core/Src/main.c:124:5:main	18
+../Core/Inc/crc.h:18:20:CRC8_107_init	1
-../Core/Src/main.c:234:6:SystemClock_Config	3
+../Core/Src/main.c:130:5:main	18
-../Core/Src/main.c:273:13:MX_TIM1_Init	10
+../Core/Src/main.c:240:6:SystemClock_Config	3
-../Core/Src/main.c:367:13:MX_TIM3_Init	3
+../Core/Src/main.c:279:13:MX_TIM1_Init	10
-../Core/Src/main.c:416:13:MX_TIM14_Init	2
+../Core/Src/main.c:373:13:MX_TIM3_Init	3
-../Core/Src/main.c:447:13:MX_TIM16_Init	2
+../Core/Src/main.c:422:13:MX_TIM14_Init	2
-../Core/Src/main.c:479:13:MX_TIM17_Init	2
+../Core/Src/main.c:453:13:MX_TIM16_Init	2
-../Core/Src/main.c:511:13:MX_USART1_UART_Init	5
+../Core/Src/main.c:485:13:MX_TIM17_Init	2
-../Core/Src/main.c:559:13:MX_USART2_UART_Init	5
+../Core/Src/main.c:517:13:MX_USART1_UART_Init	5
-../Core/Src/main.c:605:13:MX_DMA_Init	1
+../Core/Src/main.c:565:13:MX_USART2_UART_Init	5
-../Core/Src/main.c:626:13:MX_GPIO_Init	1
+../Core/Src/main.c:611:13:MX_DMA_Init	1
-../Core/Src/main.c:676:6:HAL_TIM_PeriodElapsedCallback	11
+../Core/Src/main.c:632:13:MX_GPIO_Init	1
-../Core/Src/main.c:724:6:HAL_GPIO_EXTI_Callback	5
+../Core/Src/main.c:682:6:HAL_TIM_PeriodElapsedCallback	12
-../Core/Src/main.c:746:6:HAL_UARTEx_RxEventCallback	4
+../Core/Src/main.c:746:6:HAL_GPIO_EXTI_Callback	5
-../Core/Src/main.c:766:6:set_LED	4
+../Core/Src/main.c:768:6:HAL_UARTEx_RxEventCallback	4
-../Core/Src/main.c:776:6:handleRS485Message	1
+../Core/Src/main.c:788:6:set_LED	4
-../Core/Src/main.c:794:6:set_Feeder_PWM	2
+../Core/Src/main.c:798:6:comp_crc_header	1
-../Core/Src/main.c:815:6:Error_Handler	1
+../Core/Src/main.c:806:6:handleRS485Message	26
+../Core/Src/main.c:989:6:update_Feeder_Target	1
+../Core/Src/main.c:996:6:set_Feeder_PWM	2
+../Core/Src/main.c:1017:6:Error_Handler	1

code/Debug/Core/Src/main.d

@@ -29,7 +29,7 @@ Core/Src/main.o: ../Core/Src/main.c ../Core/Inc/main.h \
  ../Drivers/STM32C0xx_HAL_Driver/Inc/stm32c0xx_hal_tim_ex.h \
  ../Drivers/STM32C0xx_HAL_Driver/Inc/stm32c0xx_hal_uart.h \
  ../Drivers/STM32C0xx_HAL_Driver/Inc/stm32c0xx_hal_uart_ex.h \
- ../Core/Inc/photon_protocol.h ../Core/Inc/pid.h
+ ../Core/Inc/photon_protocol.h ../Core/Inc/pid.h ../Core/Inc/crc.h
 ../Core/Inc/main.h:
 ../Drivers/STM32C0xx_HAL_Driver/Inc/stm32c0xx_hal.h:
 ../Drivers/STM32C0xx_HAL_Driver/Inc/stm32c0xx_ll_system.h:
@@ -63,3 +63,4 @@ Core/Src/main.o: ../Core/Src/main.c ../Core/Inc/main.h \
 ../Drivers/STM32C0xx_HAL_Driver/Inc/stm32c0xx_hal_uart_ex.h:
 ../Core/Inc/photon_protocol.h:
 ../Core/Inc/pid.h:
+../Core/Inc/crc.h:

code/Debug/Core/Src/main.su

@@ -1,21 +1,24 @@
 ../Core/Inc/pid.h:39:23:clamp_i32	24	static
 ../Core/Inc/pid.h:46:20:pid_init	24	static
 ../Core/Inc/pid.h:77:31:pid_update_motor	72	static
-../Core/Src/main.c:124:5:main	56	static
+../Core/Inc/crc.h:18:20:CRC8_107_init	16	static
-../Core/Src/main.c:234:6:SystemClock_Config	64	static
+../Core/Src/main.c:130:5:main	56	static
-../Core/Src/main.c:273:13:MX_TIM1_Init	120	static
+../Core/Src/main.c:240:6:SystemClock_Config	64	static
-../Core/Src/main.c:367:13:MX_TIM3_Init	64	static
+../Core/Src/main.c:279:13:MX_TIM1_Init	120	static
-../Core/Src/main.c:416:13:MX_TIM14_Init	8	static
+../Core/Src/main.c:373:13:MX_TIM3_Init	64	static
-../Core/Src/main.c:447:13:MX_TIM16_Init	8	static
+../Core/Src/main.c:422:13:MX_TIM14_Init	8	static
-../Core/Src/main.c:479:13:MX_TIM17_Init	8	static
+../Core/Src/main.c:453:13:MX_TIM16_Init	8	static
-../Core/Src/main.c:511:13:MX_USART1_UART_Init	8	static
+../Core/Src/main.c:485:13:MX_TIM17_Init	8	static
-../Core/Src/main.c:559:13:MX_USART2_UART_Init	8	static
+../Core/Src/main.c:517:13:MX_USART1_UART_Init	8	static
-../Core/Src/main.c:605:13:MX_DMA_Init	16	static
+../Core/Src/main.c:565:13:MX_USART2_UART_Init	8	static
-../Core/Src/main.c:626:13:MX_GPIO_Init	56	static
+../Core/Src/main.c:611:13:MX_DMA_Init	16	static
-../Core/Src/main.c:676:6:HAL_TIM_PeriodElapsedCallback	24	static
+../Core/Src/main.c:632:13:MX_GPIO_Init	56	static
-../Core/Src/main.c:724:6:HAL_GPIO_EXTI_Callback	16	static
+../Core/Src/main.c:682:6:HAL_TIM_PeriodElapsedCallback	32	static
-../Core/Src/main.c:746:6:HAL_UARTEx_RxEventCallback	16	static
+../Core/Src/main.c:746:6:HAL_GPIO_EXTI_Callback	16	static
-../Core/Src/main.c:766:6:set_LED	24	static
+../Core/Src/main.c:768:6:HAL_UARTEx_RxEventCallback	16	static
-../Core/Src/main.c:776:6:handleRS485Message	24	static
+../Core/Src/main.c:788:6:set_LED	24	static
-../Core/Src/main.c:794:6:set_Feeder_PWM	16	static
+../Core/Src/main.c:798:6:comp_crc_header	16	static
-../Core/Src/main.c:815:6:Error_Handler	8	static,ignoring_inline_asm
+../Core/Src/main.c:806:6:handleRS485Message	88	static
+../Core/Src/main.c:989:6:update_Feeder_Target	16	static
+../Core/Src/main.c:996:6:set_Feeder_PWM	16	static
+../Core/Src/main.c:1017:6:Error_Handler	8	static,ignoring_inline_asm

code/Debug/Core/Src/subdir.mk

@@ -5,6 +5,7 @@
 
 # Add inputs and outputs from these tool invocations to the build variables 
 C_SRCS += \
+../Core/Src/crc.c \
 ../Core/Src/main.c \
 ../Core/Src/stm32c0xx_hal_msp.c \
 ../Core/Src/stm32c0xx_it.c \
@@ -13,6 +14,7 @@ C_SRCS += \
 ../Core/Src/system_stm32c0xx.c 
 
 OBJS += \
+./Core/Src/crc.o \
 ./Core/Src/main.o \
 ./Core/Src/stm32c0xx_hal_msp.o \
 ./Core/Src/stm32c0xx_it.o \
@@ -21,6 +23,7 @@ OBJS += \
 ./Core/Src/system_stm32c0xx.o 
 
 C_DEPS += \
+./Core/Src/crc.d \
 ./Core/Src/main.d \
 ./Core/Src/stm32c0xx_hal_msp.d \
 ./Core/Src/stm32c0xx_it.d \
@@ -36,7 +39,7 @@ Core/Src/%.o Core/Src/%.su Core/Src/%.cyclo: ../Core/Src/%.c Core/Src/subdir.mk
 clean: clean-Core-2f-Src
 
 clean-Core-2f-Src:
-	-$(RM) ./Core/Src/main.cyclo ./Core/Src/main.d ./Core/Src/main.o ./Core/Src/main.su ./Core/Src/stm32c0xx_hal_msp.cyclo ./Core/Src/stm32c0xx_hal_msp.d ./Core/Src/stm32c0xx_hal_msp.o ./Core/Src/stm32c0xx_hal_msp.su ./Core/Src/stm32c0xx_it.cyclo ./Core/Src/stm32c0xx_it.d ./Core/Src/stm32c0xx_it.o ./Core/Src/stm32c0xx_it.su ./Core/Src/syscalls.cyclo ./Core/Src/syscalls.d ./Core/Src/syscalls.o ./Core/Src/syscalls.su ./Core/Src/sysmem.cyclo ./Core/Src/sysmem.d ./Core/Src/sysmem.o ./Core/Src/sysmem.su ./Core/Src/system_stm32c0xx.cyclo ./Core/Src/system_stm32c0xx.d ./Core/Src/system_stm32c0xx.o ./Core/Src/system_stm32c0xx.su
+	-$(RM) ./Core/Src/crc.cyclo ./Core/Src/crc.d ./Core/Src/crc.o ./Core/Src/crc.su ./Core/Src/main.cyclo ./Core/Src/main.d ./Core/Src/main.o ./Core/Src/main.su ./Core/Src/stm32c0xx_hal_msp.cyclo ./Core/Src/stm32c0xx_hal_msp.d ./Core/Src/stm32c0xx_hal_msp.o ./Core/Src/stm32c0xx_hal_msp.su ./Core/Src/stm32c0xx_it.cyclo ./Core/Src/stm32c0xx_it.d ./Core/Src/stm32c0xx_it.o ./Core/Src/stm32c0xx_it.su ./Core/Src/syscalls.cyclo ./Core/Src/syscalls.d ./Core/Src/syscalls.o ./Core/Src/syscalls.su ./Core/Src/sysmem.cyclo ./Core/Src/sysmem.d ./Core/Src/sysmem.o ./Core/Src/sysmem.su ./Core/Src/system_stm32c0xx.cyclo ./Core/Src/system_stm32c0xx.d ./Core/Src/system_stm32c0xx.o ./Core/Src/system_stm32c0xx.su
 
 .PHONY: clean-Core-2f-Src
 

code/Debug/objects.list

@@ -1,3 +1,4 @@
+"./Core/Src/crc.o"
 "./Core/Src/main.o"
 "./Core/Src/stm32c0xx_hal_msp.o"
 "./Core/Src/stm32c0xx_it.o"