From 1f9d6c8227bf655129be29f51b3fd67a7e371394 Mon Sep 17 00:00:00 2001 From: chris1320 Date: Thu, 11 Jul 2024 22:28:15 +0800 Subject: [PATCH 01/13] fix(mcu): read IR data constantly Signed-off-by: chris1320 --- mcu/mcu.ino | 282 +++++++++++++++++++--------------------------------- 1 file changed, 105 insertions(+), 177 deletions(-) diff --git a/mcu/mcu.ino b/mcu/mcu.ino index 2ec5398..40c64b8 100644 --- a/mcu/mcu.ino +++ b/mcu/mcu.ino @@ -22,12 +22,12 @@ #define pin_ir_bucket5 6 #define pin_error_led 9 -#define servo_angle_open_platform 0 // Angle to open the servo of the platform -#define servo_angle_open12 35 // Angle to open the servo 1 & 2 -#define servo_angle_open34 155 // Angle to open the servos 3 & 4 -#define servo_angle_close 90 // Angle to close the servo +#define servo_angle_open_platform 0 // Angle to open the servo of the platform +#define servo_angle_open12 35 // Angle to open the servo 1 & 2 +#define servo_angle_open34 155 // Angle to open the servos 3 & 4 +#define servo_angle_close 90 // Angle to close the servo -#define debounce 50 // Debounce delay in milliseconds +#define debounce 50 // Debounce delay in milliseconds #define LCD_COLS 20 #define LCD_ROWS 4 @@ -36,55 +36,55 @@ // Format: `{definition}{PROTOCOL_SEP}` // The commands that can be sent to the MCU. -#define PCMD_GET_PROTOCOL_VERSION "pro" // Get the MCU's protocol version -#define PCMD_STANDBY "stb" // Put the MCU in standby mode -#define PCMD_SHOW_STATISTICS "sts" // Show the statistics of the system - -#define PCMD_GATE1_STATUS "g1s" // Get gate 1 status -#define PCMD_GATE2_STATUS "g2s" // Get gate 2 status -#define PCMD_GATE3_STATUS "g3s" // Get gate 3 status -#define PCMD_GATE4_STATUS "g4s" // Get gate 4 status -#define PCMD_COUNT_GATE5 "ct5" // Count the object in bucket 5 - -#define PCMD_PLATFORM_STATUS "ps" // Get the platform status -#define PCMD_PLATFORM_OPEN "pso" // Open platform -#define PCMD_PLATFORM_CLOSE "psc" // Close platform - -#define PCMD_GATE1_OPEN "g1o" // Open gate 1 -#define PCMD_GATE2_OPEN "g2o" // Open gate 2 -#define PCMD_GATE3_OPEN "g3o" // Open gate 3 -#define PCMD_GATE4_OPEN "g4o" // Open gate 4 -#define PCMD_GATE1_CLOSE "g1c" // Close gate 1 -#define PCMD_GATE2_CLOSE "g2c" // Close gate 2 -#define PCMD_GATE3_CLOSE "g3c" // Close gate 3 -#define PCMD_GATE4_CLOSE "g4c" // Close gate 4 - -#define PCMD_ERR_LED_STATUS "els" // Get error LED status -#define PCMD_ERR_LED_ON "elh" // Turn on error LED -#define PCMD_ERR_LED_OFF "ell" // Turn off error LED - -#define PCMD_IR_STATUS "irs" // Get IR sensor status -#define PCMD_IR_ACK_1 "ir1" // Acknowledge IR sensor 1 detection -#define PCMD_IR_ACK_2 "ir2" // Acknowledge IR sensor 2 detection -#define PCMD_IR_ACK_3 "ir3" // Acknowledge IR sensor 3 detection -#define PCMD_IR_ACK_4 "ir4" // Acknowledge IR sensor 4 detection -#define PCMD_IR_ACK_5 "ir5" // Acknowledge IR sensor 5 detection +#define PCMD_GET_PROTOCOL_VERSION "pro" // Get the MCU's protocol version +#define PCMD_STANDBY "stb" // Put the MCU in standby mode +#define PCMD_SHOW_STATISTICS "sts" // Show the statistics of the system + +#define PCMD_GATE1_STATUS "g1s" // Get gate 1 status +#define PCMD_GATE2_STATUS "g2s" // Get gate 2 status +#define PCMD_GATE3_STATUS "g3s" // Get gate 3 status +#define PCMD_GATE4_STATUS "g4s" // Get gate 4 status +#define PCMD_COUNT_GATE5 "ct5" // Count the object in bucket 5 + +#define PCMD_PLATFORM_STATUS "ps" // Get the platform status +#define PCMD_PLATFORM_OPEN "pso" // Open platform +#define PCMD_PLATFORM_CLOSE "psc" // Close platform + +#define PCMD_GATE1_OPEN "g1o" // Open gate 1 +#define PCMD_GATE2_OPEN "g2o" // Open gate 2 +#define PCMD_GATE3_OPEN "g3o" // Open gate 3 +#define PCMD_GATE4_OPEN "g4o" // Open gate 4 +#define PCMD_GATE1_CLOSE "g1c" // Close gate 1 +#define PCMD_GATE2_CLOSE "g2c" // Close gate 2 +#define PCMD_GATE3_CLOSE "g3c" // Close gate 3 +#define PCMD_GATE4_CLOSE "g4c" // Close gate 4 + +#define PCMD_ERR_LED_STATUS "els" // Get error LED status +#define PCMD_ERR_LED_ON "elh" // Turn on error LED +#define PCMD_ERR_LED_OFF "ell" // Turn off error LED + +#define PCMD_IR_STATUS "irs" // Get IR sensor status +#define PCMD_IR_ACK_1 "ir1" // Acknowledge IR sensor 1 detection +#define PCMD_IR_ACK_2 "ir2" // Acknowledge IR sensor 2 detection +#define PCMD_IR_ACK_3 "ir3" // Acknowledge IR sensor 3 detection +#define PCMD_IR_ACK_4 "ir4" // Acknowledge IR sensor 4 detection +#define PCMD_IR_ACK_5 "ir5" // Acknowledge IR sensor 5 detection // The responses that can be received from the MCU. -#define PRES_READY "RDY" // The MCU is ready -#define PRES_SUCCESS "OK" // The command was successful -#define PRES_PLATFORM_SUCCESS "OKP" // The platform command was successful -#define PRES_FAILURE "KO" // The command failed -#define PRES_PVER_PREFIX "PV:" // The protocol version prefix -#define PRES_GATE_OPEN "GO" // The gate is open -#define PRES_GATE_CLOSED "GC" // The gate is closed -#define PRES_COUNT_GATE5 "CT" // The count object 5 -#define PRES_PLATFORM_OPEN "PO" // The platform is open -#define PRES_PLATFORM_CLOSED "PC" // The platform is closed -#define PRES_ERR_LED_ON "ELH" // The error LED is on -#define PRES_ERR_LED_OFF "ELL" // The error LED is off - -#define PRES_IR_STATUS "IS:" // IR sensor status prefix +#define PRES_READY "RDY" // The MCU is ready +#define PRES_SUCCESS "OK" // The command was successful +#define PRES_PLATFORM_SUCCESS "OKP" // The platform command was successful +#define PRES_FAILURE "KO" // The command failed +#define PRES_PVER_PREFIX "PV:" // The protocol version prefix +#define PRES_GATE_OPEN "GO" // The gate is open +#define PRES_GATE_CLOSED "GC" // The gate is closed +#define PRES_COUNT_GATE5 "CT" // The count object 5 +#define PRES_PLATFORM_OPEN "PO" // The platform is open +#define PRES_PLATFORM_CLOSED "PC" // The platform is closed +#define PRES_ERR_LED_ON "ELH" // The error LED is on +#define PRES_ERR_LED_OFF "ELL" // The error LED is off + +#define PRES_IR_STATUS "IS:" // IR sensor status prefix // Track the state of servos bool gate1_open = false; @@ -124,49 +124,40 @@ Servo platform; LiquidCrystal_I2C lcd(0x27, LCD_COLS, LCD_ROWS); String encodeMessage(String message) { return message + PROTOCOL_SEP; } -String decodeMessage(String message) -{ +String decodeMessage(String message) { return message.substring(0, message.indexOf(PROTOCOL_SEP)); } void updateLCDContents(bool clear, String line1, String line2, String line3, - String line4) -{ - if (clear) - lcd.clear(); + String line4) { + if (clear) lcd.clear(); lcd.setCursor(0, 0); lcd.print(line1); - for (int i = line1.length(); i < LCD_COLS; i++) - { + for (int i = line1.length(); i < LCD_COLS; i++) { lcd.print(" "); } lcd.setCursor(0, 1); lcd.print(line2); - for (int i = line2.length(); i < LCD_COLS; i++) - { + for (int i = line2.length(); i < LCD_COLS; i++) { lcd.print(" "); } lcd.setCursor(0, 2); lcd.print(line3); - for (int i = line3.length(); i < LCD_COLS; i++) - { + for (int i = line3.length(); i < LCD_COLS; i++) { lcd.print(" "); } lcd.setCursor(0, 3); lcd.print(line4); - for (int i = line4.length(); i < LCD_COLS; i++) - { + for (int i = line4.length(); i < LCD_COLS; i++) { lcd.print(" "); } } -void showStandbyScreen() -{ +void showStandbyScreen() { updateLCDContents(true, PROGRAM_NAME, PROTOCOL_VERSION, "Waiting...", ""); } -void showBucketStatistics() -{ +void showBucketStatistics() { lcd.clear(); lcd.setCursor(0, 0); lcd.print("Canned Goods: " + String(bucket1_count)); @@ -179,23 +170,19 @@ void showBucketStatistics() } void checkProximitySensor(int sensorPin, unsigned long &lastDebounceTime, - bool &ir_detected) -{ + bool &ir_detected) { int sensorValue = digitalRead(sensorPin); // Change HIGH to LOW to check for absence of object - if (sensorValue == LOW) - { + if (sensorValue == LOW) { unsigned long currentTime = millis(); - if ((currentTime - lastDebounceTime) > debounce) - { + if ((currentTime - lastDebounceTime) > debounce) { ir_detected = true; lastDebounceTime = currentTime; } } } -void setup() -{ +void setup() { // Initialize the servos gate1.attach(pin_servo_gate1); gate2.attach(pin_servo_gate2); @@ -220,147 +207,101 @@ void setup() pinMode(pin_error_led, OUTPUT); // Initialize the LCD - lcd.begin(LCD_COLS, LCD_ROWS); + // lcd.begin(LCD_COLS, LCD_ROWS); + lcd.init(); lcd.backlight(); showStandbyScreen(); Serial.begin(BAUDRATE); - Serial.print(encodeMessage(PRES_READY)); // signal to SBC that MCU is ready + Serial.print(encodeMessage(PRES_READY)); // signal to SBC that MCU is ready } -void loop() -{ - if (Serial.available() > 0) - { +void loop() { + if (Serial.available() > 0) { String command = decodeMessage(Serial.readStringUntil(PROTOCOL_SEP)); - if (command.equalsIgnoreCase(PCMD_GET_PROTOCOL_VERSION)) - { + if (command.equalsIgnoreCase(PCMD_GET_PROTOCOL_VERSION)) { Serial.print( encodeMessage(String(PRES_PVER_PREFIX) + String(PROTOCOL_VERSION))); - } - else if (command.equalsIgnoreCase(PCMD_STANDBY)) - { + } else if (command.equalsIgnoreCase(PCMD_STANDBY)) { showStandbyScreen(); Serial.print(encodeMessage(PRES_SUCCESS)); - } - else if (command.equalsIgnoreCase(PCMD_SHOW_STATISTICS)) - { + } else if (command.equalsIgnoreCase(PCMD_SHOW_STATISTICS)) { showBucketStatistics(); Serial.print(encodeMessage(PRES_SUCCESS)); - } - else if (command.equalsIgnoreCase(PCMD_ERR_LED_STATUS)) - { + } else if (command.equalsIgnoreCase(PCMD_ERR_LED_STATUS)) { Serial.print( encodeMessage(is_err_led_on ? PRES_ERR_LED_ON : PRES_ERR_LED_OFF)); - } - else if (command.equalsIgnoreCase(PCMD_ERR_LED_ON)) - { + } else if (command.equalsIgnoreCase(PCMD_ERR_LED_ON)) { digitalWrite(pin_error_led, HIGH); is_err_led_on = true; Serial.print(encodeMessage(PRES_SUCCESS)); - } - else if (command.equalsIgnoreCase(PCMD_ERR_LED_OFF)) - { + } else if (command.equalsIgnoreCase(PCMD_ERR_LED_OFF)) { digitalWrite(pin_error_led, LOW); is_err_led_on = false; Serial.print(encodeMessage(PRES_SUCCESS)); - } - else if (command.equalsIgnoreCase(PCMD_GATE1_STATUS)) - { + } else if (command.equalsIgnoreCase(PCMD_GATE1_STATUS)) { Serial.print( encodeMessage(gate1_open ? PRES_GATE_OPEN : PRES_GATE_CLOSED)); - } - else if (command.equalsIgnoreCase(PCMD_GATE2_STATUS)) - { + } else if (command.equalsIgnoreCase(PCMD_GATE2_STATUS)) { Serial.print( encodeMessage(gate2_open ? PRES_GATE_OPEN : PRES_GATE_CLOSED)); - } - else if (command.equalsIgnoreCase(PCMD_GATE3_STATUS)) - { + } else if (command.equalsIgnoreCase(PCMD_GATE3_STATUS)) { Serial.print( encodeMessage(gate3_open ? PRES_GATE_OPEN : PRES_GATE_CLOSED)); - } - else if (command.equalsIgnoreCase(PCMD_GATE4_STATUS)) - { + } else if (command.equalsIgnoreCase(PCMD_GATE4_STATUS)) { Serial.print( encodeMessage(gate4_open ? PRES_GATE_OPEN : PRES_GATE_CLOSED)); - } - else if (command.equalsIgnoreCase(PCMD_GATE1_OPEN)) - { + } else if (command.equalsIgnoreCase(PCMD_GATE1_OPEN)) { gate1.write(servo_angle_open12); gate1_open = true; bucket1_count++; Serial.print(encodeMessage(PRES_SUCCESS)); - } - else if (command.equalsIgnoreCase(PCMD_GATE2_OPEN)) - { + } else if (command.equalsIgnoreCase(PCMD_GATE2_OPEN)) { gate2.write(servo_angle_open12); gate2_open = true; bucket2_count++; Serial.print(encodeMessage(PRES_SUCCESS)); - } - else if (command.equalsIgnoreCase(PCMD_GATE3_OPEN)) - { + } else if (command.equalsIgnoreCase(PCMD_GATE3_OPEN)) { gate3.write(servo_angle_open34); gate3_open = true; bucket3_count++; Serial.print(encodeMessage(PRES_SUCCESS)); - } - else if (command.equalsIgnoreCase(PCMD_GATE4_OPEN)) - { + } else if (command.equalsIgnoreCase(PCMD_GATE4_OPEN)) { gate4.write(servo_angle_open34); gate4_open = true; bucket4_count++; Serial.print(encodeMessage(PRES_SUCCESS)); - } - else if (command.equalsIgnoreCase(PCMD_GATE1_CLOSE)) - { + } else if (command.equalsIgnoreCase(PCMD_GATE1_CLOSE)) { gate1.write(servo_angle_close); gate1_open = false; Serial.print(encodeMessage(PRES_SUCCESS)); - } - else if (command.equalsIgnoreCase(PCMD_GATE2_CLOSE)) - { + } else if (command.equalsIgnoreCase(PCMD_GATE2_CLOSE)) { gate2.write(servo_angle_close); gate2_open = false; Serial.print(encodeMessage(PRES_SUCCESS)); - } - else if (command.equalsIgnoreCase(PCMD_GATE3_CLOSE)) - { + } else if (command.equalsIgnoreCase(PCMD_GATE3_CLOSE)) { gate3.write(servo_angle_close); gate3_open = false; Serial.print(encodeMessage(PRES_SUCCESS)); - } - else if (command.equalsIgnoreCase(PCMD_GATE4_CLOSE)) - { + } else if (command.equalsIgnoreCase(PCMD_GATE4_CLOSE)) { gate4.write(servo_angle_close); gate4_open = false; Serial.print(encodeMessage(PRES_SUCCESS)); - } - else if (command.equalsIgnoreCase(PCMD_COUNT_GATE5)) - { + } else if (command.equalsIgnoreCase(PCMD_COUNT_GATE5)) { bucket5_count++; Serial.print(encodeMessage(PRES_SUCCESS)); - } - else if (command.equalsIgnoreCase(PCMD_PLATFORM_STATUS)) - { + } else if (command.equalsIgnoreCase(PCMD_PLATFORM_STATUS)) { Serial.print(encodeMessage(platform_open ? PRES_PLATFORM_OPEN : PRES_PLATFORM_CLOSED)); - } - else if (command.equalsIgnoreCase(PCMD_PLATFORM_OPEN)) - { + } else if (command.equalsIgnoreCase(PCMD_PLATFORM_OPEN)) { platform.write(servo_angle_open_platform); platform_open = true; Serial.print(encodeMessage(PRES_PLATFORM_SUCCESS)); - } - else if (command.equalsIgnoreCase(PCMD_PLATFORM_CLOSE)) - { + } else if (command.equalsIgnoreCase(PCMD_PLATFORM_CLOSE)) { platform.write(servo_angle_close); platform_open = false; Serial.print(encodeMessage(PRES_PLATFORM_SUCCESS)); - } - else if (command.equalsIgnoreCase(PCMD_IR_STATUS)) - { + } else if (command.equalsIgnoreCase(PCMD_IR_STATUS)) { String ir_status = PRES_IR_STATUS; ir_status += ir_detected_bucket1 ? "1" : "0"; ir_status += ir_detected_bucket2 ? "1" : "0"; @@ -368,42 +309,29 @@ void loop() ir_status += ir_detected_bucket4 ? "1" : "0"; ir_status += ir_detected_bucket5 ? "1" : "0"; Serial.print(encodeMessage(ir_status)); - } - else if (command.equalsIgnoreCase(PCMD_IR_ACK_1)) - { + } else if (command.equalsIgnoreCase(PCMD_IR_ACK_1)) { ir_detected_bucket1 = false; Serial.print(encodeMessage(PRES_SUCCESS)); - } - else if (command.equalsIgnoreCase(PCMD_IR_ACK_2)) - { + } else if (command.equalsIgnoreCase(PCMD_IR_ACK_2)) { ir_detected_bucket2 = false; Serial.print(encodeMessage(PRES_SUCCESS)); - } - else if (command.equalsIgnoreCase(PCMD_IR_ACK_3)) - { + } else if (command.equalsIgnoreCase(PCMD_IR_ACK_3)) { ir_detected_bucket3 = false; Serial.print(encodeMessage(PRES_SUCCESS)); - } - else if (command.equalsIgnoreCase(PCMD_IR_ACK_4)) - { + } else if (command.equalsIgnoreCase(PCMD_IR_ACK_4)) { ir_detected_bucket4 = false; Serial.print(encodeMessage(PRES_SUCCESS)); - } - else if (command.equalsIgnoreCase(PCMD_IR_ACK_5)) - { + } else if (command.equalsIgnoreCase(PCMD_IR_ACK_5)) { ir_detected_bucket5 = false; Serial.print(encodeMessage(PRES_SUCCESS)); - } - else - { + } else { Serial.print(encodeMessage(PRES_FAILURE)); } - - // Check the proximity sensors - checkProximitySensor(pin_ir_bucket1, ldt_gate1, ir_detected_bucket1); - checkProximitySensor(pin_ir_bucket2, ldt_gate2, ir_detected_bucket2); - checkProximitySensor(pin_ir_bucket3, ldt_gate3, ir_detected_bucket3); - checkProximitySensor(pin_ir_bucket4, ldt_gate4, ir_detected_bucket4); - checkProximitySensor(pin_ir_bucket5, ldt_gate5, ir_detected_bucket5); } + // Check the proximity sensors + checkProximitySensor(pin_ir_bucket1, ldt_gate1, ir_detected_bucket1); + checkProximitySensor(pin_ir_bucket2, ldt_gate2, ir_detected_bucket2); + checkProximitySensor(pin_ir_bucket3, ldt_gate3, ir_detected_bucket3); + checkProximitySensor(pin_ir_bucket4, ldt_gate4, ir_detected_bucket4); + checkProximitySensor(pin_ir_bucket5, ldt_gate5, ir_detected_bucket5); } From 3287ddd593b8d96532e10d9ea4fede4d361cdf63 Mon Sep 17 00:00:00 2001 From: chris1320 Date: Thu, 11 Jul 2024 22:54:48 +0800 Subject: [PATCH 02/13] feat(mcu): blink error LED 2 times when ready Signed-off-by: chris1320 --- mcu/mcu.ino | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/mcu/mcu.ino b/mcu/mcu.ino index 40c64b8..bac6a1f 100644 --- a/mcu/mcu.ino +++ b/mcu/mcu.ino @@ -214,6 +214,17 @@ void setup() { Serial.begin(BAUDRATE); Serial.print(encodeMessage(PRES_READY)); // signal to SBC that MCU is ready + + // Signal to the user that the system is ready using the error LED by + // blinking it for two times within 1s. + digitalWrite(pin_error_led, HIGH); + delay(250); + digitalWrite(pin_error_led, LOW); + delay(250); + digitalWrite(pin_error_led, HIGH); + delay(250); + digitalWrite(pin_error_led, LOW); + delay(250); } void loop() { From aea88703851657994bde25d9443f440099a3c729 Mon Sep 17 00:00:00 2001 From: chris1320 Date: Thu, 11 Jul 2024 22:56:59 +0800 Subject: [PATCH 03/13] fix(host): incorrect protocol separator (forgot to return?) Signed-off-by: chris1320 --- host/aidsorter/info.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/host/aidsorter/info.py b/host/aidsorter/info.py index 113b141..fba7656 100644 --- a/host/aidsorter/info.py +++ b/host/aidsorter/info.py @@ -8,7 +8,7 @@ VERSION = (0, 3, 0) TITLE = f"{NAME} v{'.'.join(map(str, VERSION))}" PROTOCOL_VERSION = "1.0" -PROTOCOL_SEP = "\r\n" +PROTOCOL_SEP = "\n" DEBUG_MODE = ( logging.DEBUG From f439f48d058a0d1f8fd632461f6e1283b8aee452 Mon Sep 17 00:00:00 2001 From: chris1320 Date: Fri, 12 Jul 2024 00:01:56 +0800 Subject: [PATCH 04/13] feat(host): update config Signed-off-by: chris1320 --- host/aidsorter.conf | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/host/aidsorter.conf b/host/aidsorter.conf index ba8de33..9e101d8 100644 --- a/host/aidsorter.conf +++ b/host/aidsorter.conf @@ -7,7 +7,7 @@ cpu_threads=0 # in the image recognition system, but also # consumes more processing power. # If unset, the default is 640x480 -resolution=1024x768 +resolution=640x480 # The debounce in frames for the system to wait # before registering a new object detection. detector_debounce=5 @@ -20,7 +20,7 @@ mcu_connection_timeout=10.0 # This is separated by a comma # Example # bucket1_contents= -bucket1_contents=person -bucket2_contents=bottle,cup -bucket3_contents=cell phone,laptop -bucket4_contents= +bucket1_contents=noodles +bucket2_contents=biscuit +bucket3_contents=canned_goods +bucket4_contents=hygiene From dfbcc5f42d6a90a71a7210822c4226953549c0bc Mon Sep 17 00:00:00 2001 From: chris1320 Date: Fri, 12 Jul 2024 00:29:08 +0800 Subject: [PATCH 05/13] feat(host): hintaying mahulog (sa tamang bucket) - Do not process items in the platform until the item that is being processes has fell into its respective bucket. - Recover from a recoverable error after 5 seconds. Signed-off-by: chris1320 --- host/aidsorter/camera.py | 122 +++++++++++++++++++++------------------ host/aidsorter/info.py | 1 + 2 files changed, 66 insertions(+), 57 deletions(-) diff --git a/host/aidsorter/camera.py b/host/aidsorter/camera.py index 4028a2a..693f91e 100644 --- a/host/aidsorter/camera.py +++ b/host/aidsorter/camera.py @@ -10,6 +10,7 @@ from typing import Optional import cv2 + from aidsorter import detector, exceptions, info, visualizer from aidsorter.fps_config import FPSConfig from aidsorter.logger import LoggerFactory @@ -88,6 +89,22 @@ def capture( ], ) + if object_sorting_in_progress != -1: + # Here, we wait for the object to fall into the bucket. + try: + if mcu.ir_states[object_sorting_in_progress - 1]: + mcu.set_gate_state(object_sorting_in_progress, False) + _ = mcu.acknowledge_ir_state(object_sorting_in_progress) + logger.info( + "Nahulog na siya kay %s.", object_sorting_in_progress + ) + object_sorting_in_progress = -1 + + except IndexError: + logger.error("Unable to get IR statuses. Please check the MCU.") + + continue + if len(detection_result.detections) > 1: mcu.set_err_led(True) logger.error( @@ -97,64 +114,48 @@ def capture( elif len(detection_result.detections) == 1: mcu.set_err_led(False) - if object_sorting_in_progress != -1: - # Here, we wait for the object to fall into the bucket. - if mcu.ir_states[object_sorting_in_progress - 1] == 1: - mcu.set_gate_state(object_sorting_in_progress, False) - object_sorting_in_progress = -1 - logger.info( - "Nahulog na siya kay %s.", object_sorting_in_progress - ) + logger.debug( + "object_sorting_in_progress=%s", object_sorting_in_progress + ) - else: - # Here, we sort the object to the correct bucket. - object_category: str = ( - detection_result.detections[0].categories[0].category_name - ) - if prev_object_category != object_category: - if object_category in config.bucket_contents[0]: - logger.info( - "Object %s belongs to Bucket 1.", object_category - ) - mcu.set_gate_state(1, True) - mcu.platform_activate() - object_sorting_in_progress = 1 - - elif object_category in config.bucket_contents[1]: - logger.info( - "Object %s belongs to Bucket 2.", object_category - ) - mcu.set_gate_state(2, True) - mcu.platform_activate() - object_sorting_in_progress = 2 - - elif object_category in config.bucket_contents[2]: - logger.info( - "Object %s belongs to Bucket 3.", object_category - ) - mcu.set_gate_state(3, True) - mcu.platform_activate() - object_sorting_in_progress = 3 - - elif object_category in config.bucket_contents[3]: - logger.info( - "Object %s belongs to Bucket 4.", object_category - ) - mcu.set_gate_state(4, True) - mcu.platform_activate() - object_sorting_in_progress = 4 - - else: - logger.warning( - "Unknown object category: %s", object_category - ) - logger.warning("Object will be put in Bucket 5.") - mcu.platform_activate() - object_sorting_in_progress = 5 - - prev_object_category = ( - object_category # Update the previous object category - ) + # Here, we sort the object to the correct bucket. + object_category: str = ( + detection_result.detections[0].categories[0].category_name + ) + if prev_object_category != object_category: + if object_category in config.bucket_contents[0]: + logger.info("Object %s belongs to Bucket 1.", object_category) + mcu.set_gate_state(1, True) + mcu.platform_activate() + object_sorting_in_progress = 1 + + elif object_category in config.bucket_contents[1]: + logger.info("Object %s belongs to Bucket 2.", object_category) + mcu.set_gate_state(2, True) + mcu.platform_activate() + object_sorting_in_progress = 2 + + elif object_category in config.bucket_contents[2]: + logger.info("Object %s belongs to Bucket 3.", object_category) + mcu.set_gate_state(3, True) + mcu.platform_activate() + object_sorting_in_progress = 3 + + elif object_category in config.bucket_contents[3]: + logger.info("Object %s belongs to Bucket 4.", object_category) + mcu.set_gate_state(4, True) + mcu.platform_activate() + object_sorting_in_progress = 4 + + else: + logger.warning("Unknown object category: %s", object_category) + logger.warning("Object will be put in Bucket 5.") + mcu.platform_activate() + object_sorting_in_progress = 5 + + prev_object_category = ( + object_category # Update the previous object category + ) # Calculate the FPS if fps.frame_count % fps.avg_frame_count == 0: @@ -189,6 +190,13 @@ def capture( logger.info("KeyboardInterrupt detected.") break + except Exception as e: # pylint: disable=broad-exception-caught + logger.critical("An error occurred: %s", e) + logger.info("Restarting after %s seconds...", info.ERROR_RESTART_DELAY) + mcu.set_err_led(True) + time.sleep(info.ERROR_RESTART_DELAY) + mcu.set_err_led(False) + logger.info("Releasing camera...") cam_cap.release() logger.info("Destroying all windows...") diff --git a/host/aidsorter/info.py b/host/aidsorter/info.py index fba7656..a5af87d 100644 --- a/host/aidsorter/info.py +++ b/host/aidsorter/info.py @@ -9,6 +9,7 @@ TITLE = f"{NAME} v{'.'.join(map(str, VERSION))}" PROTOCOL_VERSION = "1.0" PROTOCOL_SEP = "\n" +ERROR_RESTART_DELAY = 5 # in seconds DEBUG_MODE = ( logging.DEBUG From 32bfbbc77d00c2de7d0effaed34875203f2ccfc3 Mon Sep 17 00:00:00 2001 From: chris1320 Date: Fri, 12 Jul 2024 00:31:57 +0800 Subject: [PATCH 06/13] feat(host): read responses from MCU Signed-off-by: chris1320 --- host/aidsorter/mcu.py | 41 +++++++++++++++++++++++++++++++++-------- 1 file changed, 33 insertions(+), 8 deletions(-) diff --git a/host/aidsorter/mcu.py b/host/aidsorter/mcu.py index 81f7572..b048ebc 100644 --- a/host/aidsorter/mcu.py +++ b/host/aidsorter/mcu.py @@ -9,6 +9,7 @@ from typing import Union import serial + from aidsorter import exceptions, info from aidsorter.logger import LoggerFactory @@ -107,7 +108,7 @@ def is_platform_open(self) -> bool: return self.connection.read_until(self.sep).decode() @property - def is_err_led_one(self) -> bool: + def is_err_led_on(self) -> bool: _ = self.connection.write(self._encode_command(Commands.ERR_LED_STATUS)) return self.connection.read_until(self.sep).decode() @@ -117,12 +118,12 @@ def ir_states(self) -> list[bool]: raise exceptions.MCUConnectionError("The connection is not open.") _ = self.connection.write(self._encode_command(Commands.IR_STATUS)) - return [ - x == "1" - for x in self.connection.read_until(self.sep) - .decode() - .partition(Responses.IR_STATUS.value)[2] - ] + res = self.connection.read_until(self.sep).decode() + self.logger.debug(res) + if not res.startswith(Responses.IR_STATUS.value): + raise exceptions.MCUConnectionError("Invalid IR status from the MCU.") + + return [x == "1" for x in res.partition(Responses.IR_STATUS.value)[2]] @property def is_gate1_open(self) -> bool: @@ -182,18 +183,21 @@ def set_err_led(self, turn_on: bool) -> None: Commands.ERR_LED_ON if turn_on else Commands.ERR_LED_OFF ) ) + _ = self.connection.read_until(self.sep) def platform_activate(self): if not self.connection.is_open: raise exceptions.MCUConnectionError("The connection is not open.") _ = self.connection.write(self._encode_command(Commands.PLATFORM_OPEN)) + _ = self.connection.read_until(self.sep) def platform_deactivate(self): if not self.connection.is_open: raise exceptions.MCUConnectionError("The connection is not open.") - _ = self.connection.write(self._encode_command(Commands.PLATFORM_OPEN)) + _ = self.connection.write(self._encode_command(Commands.PLATFORM_CLOSE)) + _ = self.connection.read_until(self.sep) def set_gate_state(self, gate: int, open_gate: bool) -> None: if not self.connection.is_open: @@ -233,3 +237,24 @@ def set_gate_state(self, gate: int, open_gate: bool) -> None: else: raise ValueError("Invalid gate number.") + + _ = self.connection.read_until(self.sep) + + def acknowledge_ir_state(self, ir_number: int) -> Responses: + if not self.connection.is_open: + raise exceptions.MCUConnectionError("The connection is not open.") + + if ir_number == 1: + _ = self.connection.write(self._encode_command(Commands.IR_ACK_1)) + elif ir_number == 2: + _ = self.connection.write(self._encode_command(Commands.IR_ACK_2)) + elif ir_number == 3: + _ = self.connection.write(self._encode_command(Commands.IR_ACK_3)) + elif ir_number == 4: + _ = self.connection.write(self._encode_command(Commands.IR_ACK_4)) + elif ir_number == 5: + _ = self.connection.write(self._encode_command(Commands.IR_ACK_5)) + else: + raise ValueError("Invalid IR number.") + + return self._decode_response(self.connection.read_until(self.sep)) From a02abc1c5ec2ce684e5d25ee977fd16f59b36295 Mon Sep 17 00:00:00 2001 From: chris1320 Date: Fri, 12 Jul 2024 01:51:14 +0800 Subject: [PATCH 07/13] fix(host): attempt to restart camera capture after crash Signed-off-by: chris1320 --- host/aidsorter/camera.py | 30 +++++++++++++++++++++++------- 1 file changed, 23 insertions(+), 7 deletions(-) diff --git a/host/aidsorter/camera.py b/host/aidsorter/camera.py index 693f91e..6ab4a2a 100644 --- a/host/aidsorter/camera.py +++ b/host/aidsorter/camera.py @@ -18,6 +18,20 @@ from aidsorter.mcu_config import MCUConfig +def getCameraCapture(camera_id: int, resolution: tuple[int, int]) -> cv2.VideoCapture: + logger = LoggerFactory().get_logger(__name__) + logger.info("Getting camera capture...") + cam_cap = cv2.VideoCapture(camera_id) + logger.info("Backend API name: %s", cam_cap.getBackendName()) + if not cam_cap.set(cv2.CAP_PROP_FRAME_WIDTH, resolution[0]): + raise exceptions.CameraError("Unable to set the camera width.") + + if not cam_cap.set(cv2.CAP_PROP_FRAME_HEIGHT, resolution[1]): + raise exceptions.CameraError("Unable to set the camera height.") + + return cam_cap + + def capture( config_path: str, camera_id: int = 1, @@ -57,13 +71,7 @@ def capture( "Using camera resolution: %sx%s", config.resolution[0], config.resolution[1] ) logger.info("Using CPU threads: %s", config.cpu_threads) - cam_cap = cv2.VideoCapture(camera_id) - logger.info("Backend API name: %s", cam_cap.getBackendName()) - if not cam_cap.set(cv2.CAP_PROP_FRAME_WIDTH, config.resolution[0]): - raise exceptions.CameraError("Unable to set the camera width.") - - if not cam_cap.set(cv2.CAP_PROP_FRAME_HEIGHT, config.resolution[1]): - raise exceptions.CameraError("Unable to set the camera height.") + cam_cap = getCameraCapture(camera_id, config.resolution) logger.info("Starting camera loop...") fps.start_time = time.time() # Start the timer @@ -190,6 +198,14 @@ def capture( logger.info("KeyboardInterrupt detected.") break + except exceptions.CameraError as e: + logger.error("An error occurred: %s", e) + logger.info("Restarting after %s seconds...", info.ERROR_RESTART_DELAY) + mcu.set_err_led(True) + time.sleep(info.ERROR_RESTART_DELAY) + mcu.set_err_led(False) + cam_cap = getCameraCapture(camera_id, config.resolution) + except Exception as e: # pylint: disable=broad-exception-caught logger.critical("An error occurred: %s", e) logger.info("Restarting after %s seconds...", info.ERROR_RESTART_DELAY) From 575e74a0fb57b30e6d0687e2e9ff664e52028241 Mon Sep 17 00:00:00 2001 From: chris1320 Date: Fri, 12 Jul 2024 02:00:13 +0800 Subject: [PATCH 08/13] fix(host): properly acknowledge IR states Signed-off-by: chris1320 --- host/aidsorter/camera.py | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/host/aidsorter/camera.py b/host/aidsorter/camera.py index 6ab4a2a..d90db41 100644 --- a/host/aidsorter/camera.py +++ b/host/aidsorter/camera.py @@ -102,7 +102,11 @@ def capture( try: if mcu.ir_states[object_sorting_in_progress - 1]: mcu.set_gate_state(object_sorting_in_progress, False) - _ = mcu.acknowledge_ir_state(object_sorting_in_progress) + # just acknowledge all IR states... + # _ = mcu.acknowledge_ir_state(object_sorting_in_progress) + for i in range(1, 6): + _ = mcu.acknowledge_ir_state(i) + logger.info( "Nahulog na siya kay %s.", object_sorting_in_progress ) @@ -130,7 +134,9 @@ def capture( object_category: str = ( detection_result.detections[0].categories[0].category_name ) - if prev_object_category != object_category: + if ( + prev_object_category != object_category + ) and object_sorting_in_progress == -1: if object_category in config.bucket_contents[0]: logger.info("Object %s belongs to Bucket 1.", object_category) mcu.set_gate_state(1, True) From 0bc7eb306a9744bce057beff81a7d7acd5e1889d Mon Sep 17 00:00:00 2001 From: chris1320 Date: Fri, 12 Jul 2024 02:00:36 +0800 Subject: [PATCH 09/13] fix(host): properly parse MCU handshare reply Signed-off-by: chris1320 --- host/aidsorter/mcu.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/host/aidsorter/mcu.py b/host/aidsorter/mcu.py index b048ebc..bedde58 100644 --- a/host/aidsorter/mcu.py +++ b/host/aidsorter/mcu.py @@ -82,7 +82,7 @@ def __init__( if not self.connection.is_open: raise exceptions.MCUConnectionError("Could not connect to the MCU.") - if self._decode_response(self.connection.readline()) != Responses.READY: + if self.connection.readline() != self._encode_command(Responses.READY): raise exceptions.MCUConnectionError("The MCU did not respond as expected.") self.logger.info("MCU connection established.") From f69a9f7aa2156617e18f993161bef7f0d2dde725 Mon Sep 17 00:00:00 2001 From: chris1320 Date: Fri, 12 Jul 2024 02:10:23 +0800 Subject: [PATCH 10/13] fix(host): deactivate platform when conveyor belt is cleared Signed-off-by: chris1320 --- host/aidsorter/camera.py | 1 + 1 file changed, 1 insertion(+) diff --git a/host/aidsorter/camera.py b/host/aidsorter/camera.py index d90db41..942d31a 100644 --- a/host/aidsorter/camera.py +++ b/host/aidsorter/camera.py @@ -111,6 +111,7 @@ def capture( "Nahulog na siya kay %s.", object_sorting_in_progress ) object_sorting_in_progress = -1 + mcu.platform_deactivate() except IndexError: logger.error("Unable to get IR statuses. Please check the MCU.") From 1dbbe582ccbb170e856a10f73a809c86cb84f7a2 Mon Sep 17 00:00:00 2001 From: chris1320 Date: Fri, 12 Jul 2024 03:03:17 +0800 Subject: [PATCH 11/13] feat(host): use a different algorithm in determining object category Signed-off-by: chris1320 --- host/aidsorter.conf | 4 ++ host/aidsorter/camera.py | 108 +++++++++++++++++++++++------------ host/aidsorter/mcu.py | 4 +- host/aidsorter/mcu_config.py | 14 +++++ 4 files changed, 91 insertions(+), 39 deletions(-) diff --git a/host/aidsorter.conf b/host/aidsorter.conf index 9e101d8..c8d8e39 100644 --- a/host/aidsorter.conf +++ b/host/aidsorter.conf @@ -11,6 +11,10 @@ resolution=640x480 # The debounce in frames for the system to wait # before registering a new object detection. detector_debounce=5 +# How many samples should the program take before +# considering the object as something that belongs +# to a bucket. +detector_samples=10 # The baudrate of the serial connection baudrate=115200 diff --git a/host/aidsorter/camera.py b/host/aidsorter/camera.py index 942d31a..d23be16 100644 --- a/host/aidsorter/camera.py +++ b/host/aidsorter/camera.py @@ -7,7 +7,6 @@ # pyright: reportDeprecated=false import time -from typing import Optional import cv2 @@ -61,8 +60,9 @@ def capture( stats_style = visualizer.StatsStyle() tf_detector = detector.create_detector(model_name, config.cpu_threads) mcu = MCU(mcu_port, config.baudrate, config.mcu_connection_timeout) - prev_object_category: Optional[str] = None object_sorting_in_progress: int = -1 # which bucket is targeted for the object + object_category_samples: list[str] = [] + object_category_samples_expiration: int = 0 logger = LoggerFactory().get_logger(__name__) logger.info("Starting the detection process...") @@ -104,6 +104,7 @@ def capture( mcu.set_gate_state(object_sorting_in_progress, False) # just acknowledge all IR states... # _ = mcu.acknowledge_ir_state(object_sorting_in_progress) + logger.debug("Acknowledging all IR states...") for i in range(1, 6): _ = mcu.acknowledge_ir_state(i) @@ -112,6 +113,7 @@ def capture( ) object_sorting_in_progress = -1 mcu.platform_deactivate() + mcu.show_statistics() except IndexError: logger.error("Unable to get IR statuses. Please check the MCU.") @@ -119,6 +121,7 @@ def capture( continue if len(detection_result.detections) > 1: + object_category_samples_expiration = 0 mcu.set_err_led(True) logger.error( "%s object/s detected. Will not proceed.", @@ -126,51 +129,82 @@ def capture( ) elif len(detection_result.detections) == 1: + object_category_samples_expiration = 0 mcu.set_err_led(False) logger.debug( "object_sorting_in_progress=%s", object_sorting_in_progress ) - # Here, we sort the object to the correct bucket. object_category: str = ( detection_result.detections[0].categories[0].category_name ) + if ( - prev_object_category != object_category - ) and object_sorting_in_progress == -1: - if object_category in config.bucket_contents[0]: - logger.info("Object %s belongs to Bucket 1.", object_category) - mcu.set_gate_state(1, True) - mcu.platform_activate() - object_sorting_in_progress = 1 - - elif object_category in config.bucket_contents[1]: - logger.info("Object %s belongs to Bucket 2.", object_category) - mcu.set_gate_state(2, True) - mcu.platform_activate() - object_sorting_in_progress = 2 - - elif object_category in config.bucket_contents[2]: - logger.info("Object %s belongs to Bucket 3.", object_category) - mcu.set_gate_state(3, True) - mcu.platform_activate() - object_sorting_in_progress = 3 - - elif object_category in config.bucket_contents[3]: - logger.info("Object %s belongs to Bucket 4.", object_category) - mcu.set_gate_state(4, True) - mcu.platform_activate() - object_sorting_in_progress = 4 - - else: - logger.warning("Unknown object category: %s", object_category) - logger.warning("Object will be put in Bucket 5.") - mcu.platform_activate() - object_sorting_in_progress = 5 - - prev_object_category = ( - object_category # Update the previous object category + len(object_category_samples) < config.detector_samples + and object_sorting_in_progress == -1 + ): + logger.debug("Taking sample #%s.", len(object_category_samples) + 1) + object_category_samples.append(object_category) + continue + + logger.debug("Samples: %s", object_category_samples) + object_category = max( + set(object_category_samples), key=object_category_samples.count + ) + logger.info( + "Object category from %s samples: %s (%s%%)", + config.detector_samples, + object_category, + object_category_samples.count(object_category) + / config.detector_samples + * 100, ) + object_category_samples.clear() + + # Here, we sort the object to the correct bucket. + if object_category in config.bucket_contents[0]: + logger.info("Object %s belongs to Bucket 1.", object_category) + mcu.set_gate_state(1, True) + mcu.platform_activate() + object_sorting_in_progress = 1 + + elif object_category in config.bucket_contents[1]: + logger.info("Object %s belongs to Bucket 2.", object_category) + mcu.set_gate_state(2, True) + mcu.platform_activate() + object_sorting_in_progress = 2 + + elif object_category in config.bucket_contents[2]: + logger.info("Object %s belongs to Bucket 3.", object_category) + mcu.set_gate_state(3, True) + mcu.platform_activate() + object_sorting_in_progress = 3 + + elif object_category in config.bucket_contents[3]: + logger.info("Object %s belongs to Bucket 4.", object_category) + mcu.set_gate_state(4, True) + mcu.platform_activate() + object_sorting_in_progress = 4 + + else: + logger.warning("Unknown object category: %s", object_category) + logger.warning("Object will be put in Bucket 5.") + mcu.platform_activate() + object_sorting_in_progress = 5 + + elif len(detection_result.detections) == 0: + if len(object_category_samples) != 0: + if object_category_samples_expiration < config.detector_samples: + object_category_samples_expiration += 1 + continue + + object_category_samples_expiration = 0 + logger.info( + "No object detected after %s frames. Clearing %s samples.", + config.detector_samples, + len(object_category_samples), + ) + object_category_samples.clear() # Calculate the FPS if fps.frame_count % fps.avg_frame_count == 0: diff --git a/host/aidsorter/mcu.py b/host/aidsorter/mcu.py index bedde58..4708dc4 100644 --- a/host/aidsorter/mcu.py +++ b/host/aidsorter/mcu.py @@ -119,7 +119,7 @@ def ir_states(self) -> list[bool]: _ = self.connection.write(self._encode_command(Commands.IR_STATUS)) res = self.connection.read_until(self.sep).decode() - self.logger.debug(res) + self.logger.debug("ir_states=%s", res) if not res.startswith(Responses.IR_STATUS.value): raise exceptions.MCUConnectionError("Invalid IR status from the MCU.") @@ -167,7 +167,7 @@ def show_statistics(self) -> None: if not self.connection.is_open: raise exceptions.MCUConnectionError("The connection is not open.") - _ = self.connection.write(self._encode_command(Commands.STANDBY)) + _ = self.connection.write(self._encode_command(Commands.SHOW_STATISTICS)) if self._decode_response(self.connection.readline()) == Responses.SUCCESS: self.logger.debug("Stats has been shown.") diff --git a/host/aidsorter/mcu_config.py b/host/aidsorter/mcu_config.py index ca4a45f..a87b0f1 100644 --- a/host/aidsorter/mcu_config.py +++ b/host/aidsorter/mcu_config.py @@ -51,6 +51,9 @@ def __init__(self, config_path: str) -> None: elif key == "detector_debounce": self.__detector_debounce = int(value) + elif key == "detector_samples": + self.__detector_samples: int = int(value) + elif key == "baudrate": self.__baudrate = int(value) @@ -124,6 +127,17 @@ def detector_debounce(self) -> int: return self.__detector_debounce + @property + def detector_samples(self) -> int: + """How many samples should the program take before + considering the object as something that belongs + to a bucket. + Returns: + The number of samples to take for object detection. + """ + + return self.__detector_samples + @property def baudrate(self) -> int: """Get the baudrate of the MCU. From f7e5c1ee3c382316720eec3e8a737da0aba99010 Mon Sep 17 00:00:00 2001 From: chris1320 Date: Fri, 12 Jul 2024 03:19:55 +0800 Subject: [PATCH 12/13] feat(host): acknowledge object sort Signed-off-by: chris1320 --- host/aidsorter/camera.py | 2 ++ host/aidsorter/mcu.py | 7 +++++++ 2 files changed, 9 insertions(+) diff --git a/host/aidsorter/camera.py b/host/aidsorter/camera.py index d23be16..56b3ac0 100644 --- a/host/aidsorter/camera.py +++ b/host/aidsorter/camera.py @@ -192,6 +192,8 @@ def capture( mcu.platform_activate() object_sorting_in_progress = 5 + mcu.acknowledge_object_sort() + elif len(detection_result.detections) == 0: if len(object_category_samples) != 0: if object_category_samples_expiration < config.detector_samples: diff --git a/host/aidsorter/mcu.py b/host/aidsorter/mcu.py index 4708dc4..e2bb54c 100644 --- a/host/aidsorter/mcu.py +++ b/host/aidsorter/mcu.py @@ -43,6 +43,7 @@ class Commands(Enum): ERR_LED_STATUS = "els" # Error LED status ERR_LED_ON = "elh" # Turn on the error LED ERR_LED_OFF = "ell" # Turn off the error LED + ACK_OBJ_SORT = "aos" # Acknowledge object sort IR_STATUS = "irs" # Get the status of the IR sensor IR_ACK_1 = "ir1" # Acknowledge IR sensor 1 detection @@ -185,6 +186,12 @@ def set_err_led(self, turn_on: bool) -> None: ) _ = self.connection.read_until(self.sep) + def acknowledge_object_sort(self) -> None: + if not self.connection.is_open: + raise exceptions.MCUConnectionError("The connection is not open.") + _ = self.connection.write(self._encode_command(Commands.ACK_OBJ_SORT)) + _ = self.connection.read_until(self.sep) + def platform_activate(self): if not self.connection.is_open: raise exceptions.MCUConnectionError("The connection is not open.") From af03df8593e620e1c03bbcbceaee3a64839021e5 Mon Sep 17 00:00:00 2001 From: chris1320 Date: Fri, 12 Jul 2024 03:20:11 +0800 Subject: [PATCH 13/13] feat(mcu): acknowledge object sort Signed-off-by: chris1320 --- mcu/mcu.ino | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/mcu/mcu.ino b/mcu/mcu.ino index bac6a1f..a6ce0ba 100644 --- a/mcu/mcu.ino +++ b/mcu/mcu.ino @@ -62,6 +62,7 @@ #define PCMD_ERR_LED_STATUS "els" // Get error LED status #define PCMD_ERR_LED_ON "elh" // Turn on error LED #define PCMD_ERR_LED_OFF "ell" // Turn off error LED +#define PCMD_ACK_OBJ_SORT "aos" // Acknowledge object sort #define PCMD_IR_STATUS "irs" // Get IR sensor status #define PCMD_IR_ACK_1 "ir1" // Acknowledge IR sensor 1 detection @@ -250,6 +251,14 @@ void loop() { digitalWrite(pin_error_led, LOW); is_err_led_on = false; Serial.print(encodeMessage(PRES_SUCCESS)); + } else if (command.equalsIgnoreCase(PCMD_ACK_OBJ_SORT)) { + for (int i = 0; i < 3; i++) { + digitalWrite(pin_error_led, HIGH); + delay(300); + digitalWrite(pin_error_led, LOW); + delay(300); + } + Serial.print(encodeMessage(PRES_SUCCESS)); } else if (command.equalsIgnoreCase(PCMD_GATE1_STATUS)) { Serial.print( encodeMessage(gate1_open ? PRES_GATE_OPEN : PRES_GATE_CLOSED));