Change LED distance logic for OC laser and ultrasonic sensors

oc_sensors/laser_distance_G030K8/Core/Src/main.c

@@ -37,7 +37,9 @@
 /* USER CODE BEGIN PD */
 #define SENSOR_ID 192
 #define DATA_BAUD_RATE 256000
+#define METADATA_DELAY_MS 80
 #define MAX_SENSOR_RANGE 4000 //MM
+
 const ev3_mode_info m_normal = {S_MODE_NORMAL,
 							    "Normal", 6,
 							    0, MAX_SENSOR_RANGE,
@@ -65,7 +67,7 @@ const ev3_mode_info m_short = {S_MODE_SHORT,
 const sensor_info s_info = {SENSOR_ID, S_MODE_COUNT, S_MODE_COUNT, DATA_BAUD_RATE,
 		 	 	 	 	 	{m_normal, m_fov, m_short}};
 
-oc_laser_sensor laser_sensor = {0, 0, MAX_SENSOR_RANGE+1, 0x52};
+oc_laser_sensor laser_sensor = {0, 0, 0, 0x52};
 /* USER CODE END PD */
 
 /* Private macro -------------------------------------------------------------*/
@@ -162,11 +164,15 @@ int main(void)
 				}
 				GREEN_off();
 				RED_off();
-				HAL_Delay(80);
+				int delay_counter = 0;
+				while(sensor_state == S_MODE_WAIT_ACK && delay_counter < METADATA_DELAY_MS) {
+					delay_counter++;
+					HAL_Delay(1);
+				}
 				RED_on();
 				GREEN_on();
 			}
-			laser_sensor.compVAL = MAX_SENSOR_RANGE+1;
+			laser_sensor.compVAL = 0;
 			change_sensor_settings(&laser_sensor, sensor_state);
 			LL_USART_Disable(USART1);
 			MX_USART1_UART_Init();
@@ -174,7 +180,7 @@ int main(void)
 			while (sensor_state > S_MODE_INIT) {
 				get_sensor_data(&laser_sensor);
 				send_data(sensor_state, &laser_sensor.Distance, sizeof(laser_sensor.Distance));
-				if (laser_sensor.compVAL <= laser_sensor.Distance) {
+				if (laser_sensor.Distance < laser_sensor.compVAL) {
 					GREEN_on();
 				} else {
 					GREEN_off();

oc_sensors/open_ultrasonic/Core/Src/main.c

@@ -85,7 +85,7 @@ volatile uint8_t recflag2=0;
 uint8_t ranging[5];
 uint8_t out_data[5];
 uint32_t result=0;
-uint16_t comp_val = MAX_SENSOR_RANGE+1;
+uint16_t comp_val = 0;
 
 int sensor_state = S_MODE_INIT;
 
@@ -267,7 +267,7 @@ int main(void)
     /* USER CODE END WHILE */
 
     /* USER CODE BEGIN 3 */
-	  comp_val = MAX_SENSOR_RANGE+1;
+	  comp_val = 0;
 	  while(sensor_state == S_MODE_INIT || sensor_state == S_MODE_WAIT_ACK) {
 			send_metadata(&s_info);
 			if (sensor_state == S_MODE_INIT) {
@@ -295,7 +295,7 @@ int main(void)
 				 if(result>9999){
 					 result = 9999;
 				 }
-				 if(result>comp_val){
+				 if(result<comp_val){
 					 GREEN_on();
 				 }else{
 					 GREEN_off();