commit ffe9f79432e34a2214bd2dc4198f92e791cd29a7 Author: engr-ugaif engr-ugaif Date: Mon Mar 31 19:38:03 2025 -0400 Hopefully Final Control Update...minus esp bug diff --git a/MQTTdigitalPot.ino b/MQTTdigitalPot.ino new file mode 100644 index 0000000..2f13fb5 --- /dev/null +++ b/MQTTdigitalPot.ino @@ -0,0 +1,128 @@ +#include // Correct library header for M5StickC Plus2 // Include M5StickCPlus2 library +#include +#include +#include + +#define CS_PIN 32 // Chip Select (CS) for MCP4231 +#define SCK_PIN 33 +#define MISO_PIN 25 +#define MOSI_PIN 26 + +#define SPI_SPEED 1000000 // 1MHz SPI speed +byte addressPot = B00000000; + +const char* ssid = "IOT-pecan"; +const char* password = "aaaaaaaa"; +const char* mqtt_server = "192.168.1.110"; +const char* topic = "jc/status/light:0"; + +WiFiClient espClient; +PubSubClient client(espClient); + +void setPot(uint8_t value) { + digitalWrite(CS_PIN, LOW); // Select MCP4231 + SPI.transfer(addressPot); // Command: Write to Wiper 0 + SPI.transfer(value); // Set wiper position (0-255) + digitalWrite(CS_PIN, HIGH); // Deselect MCP4231 +} + +void callback(char* topic, byte* payload, unsigned int length) { + char payloadStr[10]; + if (length >= sizeof(payloadStr)) length = sizeof(payloadStr) - 1; + strncpy(payloadStr, (char*)payload, length); + payloadStr[length] = '\0'; + + int potValue = atoi(payloadStr); + + setPot(potValue); + Serial.print("Wiper set to: "); + Serial.println(potValue); + + // Display on the M5StickC Plus2 screen + M5.Lcd.fillScreen(TFT_BLACK); + M5.Lcd.setCursor(0, 20); + M5.Lcd.println("Output:"); + M5.Lcd.print(potValue); + +} + +void setup() { + M5.begin(); // Initialize M5StickC Plus2 + SPI.begin(SCK_PIN, MISO_PIN, MOSI_PIN, CS_PIN); // SCK, MISO (not used), MOSI, CS + pinMode(CS_PIN, OUTPUT); + digitalWrite(CS_PIN, HIGH); + + Serial.begin(115200); + Serial.println("MCP4231 Digital Potentiometer Test"); + + M5.Lcd.setRotation(0); + M5.Lcd.fillScreen(TFT_BLACK); + M5.Lcd.setTextColor(TFT_WHITE); + M5.Lcd.setTextSize(2.5); + + // Set potentiometer to mid-range (127) + setPot(60); + + WiFi.begin(ssid, password); + Serial.print("Connecting to WiFi"); + unsigned long startTime = millis(); + while (WiFi.status() != WL_CONNECTED && millis() - startTime < 10000) { + delay(500); + Serial.print("."); + } + + if (WiFi.status() == WL_CONNECTED) { + Serial.println("\nConnected to WiFi"); + } else { + Serial.println("\nWiFi Connection Failed!"); + return; + } + + client.setServer(mqtt_server, 1883); + client.setCallback(callback); + + while (!client.connected()) { + Serial.print("Connecting to MQTT..."); + if (client.connect("M5StickCPlus2")) { + Serial.println("Connected!"); + client.subscribe(topic); + } else { + Serial.print("Failed, rc="); + Serial.println(client.state()); + delay(2000); + } + } +} + +void reconnect() { + while (!client.connected()) { + Serial.print("Reconnecting to MQTT..."); + if (client.connect("M5StickCPlus2")) { + Serial.println("Reconnected!"); + client.subscribe(topic); + } else { + Serial.print("Failed, rc="); + Serial.println(client.state()); + delay(5000); // Wait before retrying + } + } +} + +void loop() { + if (!client.connected()) { + reconnect(); // Reconnect to MQTT if disconnected + } + client.loop(); +} + + + + + + + + + + + + diff --git a/bytetrack_custom.yaml b/bytetrack_custom.yaml new file mode 100644 index 0000000..fc9c3e5 --- /dev/null +++ b/bytetrack_custom.yaml @@ -0,0 +1,14 @@ +# Ultralytics 🚀 AGPL-3.0 License - https://ultralytics.com/license + +# Default Ultralytics settings for ByteTrack tracker when using mode="track" +# For documentation and examples see https://docs.ultralytics.com/modes/track/ +# For ByteTrack source code see https://github.com/ifzhang/ByteTrack + +tracker_type: bytetrack # tracker type, ['botsort', 'bytetrack'] +track_high_thresh: 0.45 # threshold for the first association +track_low_thresh: 0.05 # threshold for the second association +new_track_thresh: 0.45 # threshold for init new track if the detection does not match any tracks +track_buffer: 15 # buffer to calculate the time when to remove tracks +match_thresh: 0.75 # threshold for matching tracks +fuse_score: True # Whether to fuse confidence scores with the iou distances before matching +# min_box_area: 10 # threshold for min box areas(for tracker evaluation, not used for now) diff --git a/jcControl.py b/jcControl.py new file mode 100644 index 0000000..2111201 --- /dev/null +++ b/jcControl.py @@ -0,0 +1,262 @@ +#################################################################################### +# Load Libraries + +from ultralytics import YOLO +import cvzone +import cv2 +import threading +import queue +import paho.mqtt.client as mqtt +import time +from filterpy.kalman import UnscentedKalmanFilter as UKF +from filterpy.kalman import MerweScaledSigmaPoints +import numpy as np +import pandas as pd + +#################################################################################### +# Initialize Variables + +# Load YOLOv11 model +device = 'cuda' +model = YOLO("yolo11n-pecan.pt") # Update with correct model path + +cy1 = 475 # Count Threshold (Center Y-coordinate) +offset = 60 # Offset for Center Y-coordinate +ids = set() +pecanCount = 0 +samplePeriod = 0.1 # seconds +refThroughput = 0 # Count / Second +switchState = 0 # 0 = Off, 1 = On +prevSwitchState = 0 # 0 = Off, 1 = On + +#################################################################################### +# Initialize Camera + +cap = cv2.VideoCapture(0) # Ensure that you have the correct camera index + +# Set the resolution and other properties for each camera +cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280) +cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720) +cap.set(cv2.CAP_PROP_FPS, 120) + +#################################################################################### +# Unscented Kalman Filter + +# State transition function (identity - no control input) +def fx(x, dt): + """ State transition function (no external control) """ + return x # No change in state without an input + +# Measurement function (identity) +def hx(x): + """ Measurement function (direct observation) """ + return x # We measure the state directly + +points = MerweScaledSigmaPoints(n=1, alpha=0.1, beta=2, kappa=0)# Define sigma points + +ukf = UKF(dim_x=1, dim_z=1, fx=fx, hx=hx, points=points, dt=samplePeriod) # Initial State Estimate +ukf.x = np.array([refThroughput]) # Initial state estimate +ukf.Q = np.array([[0.02]]) # Process noise covariance (Q) - controls how much the state changes naturally +ukf.R = np.array([[0.5]]) # Measurement noise covariance (R) - how noisy the measurements are +ukf.P = np.eye(1) * 0.1 # Initial state covariance (P) - initial uncertainty + +#################################################################################### +# PI Controller + +class PIController: + def __init__(self, Kp, Ki, Ts): + self.Kp = Kp + self.Ki = Ki + self.Ts = Ts + self.prevError = 0 + self.prevOutput = 55 + self.error = 0 + + + def compute(self, setpoint, measurement): + """Compute PI control output.""" + self.error = setpoint - measurement + + output = self.prevOutput + self.Kp * self.error + (self.Ki * self.Ts - self.Kp) * self.prevError # Z domain + + self.prevError = self.error + + # Apply saturation limits (0 to 90) + if output > 90: + output = 90 + elif output < 0: + output = 0 + + self.prevOutput = output + + return output + +# PI Initialization +controller = PIController(0.063, 0.5, samplePeriod) # Kp, Ki, Ts + +#################################################################################### +# MQTT + +# Mqtt Configuration +MQTT_BROKER = "192.168.1.110" +MQTT_PORT = 1883 +MQTT_REF_TOPIC = "/jc/feedrate/" +MQTT_COUNT_TOPIC = "/jc/feedrate/count/" +MQTT_CONTROL_TOPIC = "jc/status/light:0" +MQTT_CONTROL_VISUAL_TOPIC = "/pi_controller/output" + +# MQTT Handling +def on_connect(client, userdata, flags, rc, properties=None): + print("Connected with result code " + str(rc)) + client.subscribe(MQTT_REF_TOPIC) + +def on_message(client, userdata, message): + global refThroughput, switchState, prevSwitchState, controller + + with refThroughput_lock: + refThroughput = float(message.payload.decode()) + + + with ukf_lock: + ukf.x = np.array([refThroughput]) + + with switchState_lock: + if refThroughput == 0: + switchState = 0 + elif prevSwitchState == 0 and refThroughput != 0: + switchState = 1 + with controller_lock: + controller.prevError = 0 + controller.prevOutput = 60 + else: + switchState = 1 + + prevSwitchState = switchState + +# Initialize MQTT client +mqtt_client = mqtt.Client() +mqtt_client.on_connect = on_connect +mqtt_client.on_message = on_message +mqtt_client.connect(MQTT_BROKER, MQTT_PORT, 60) +mqtt_client.loop_start() # Starts the loop in the background + +#################################################################################### +# Thread Initialization + +# Queue to hold frames captured by the capture thread +frame_queue = queue.Queue(maxsize=1) # Limit to 1 frames in the queue + +# Locks for thread synchronization +refThroughput_lock = threading.Lock() +ukf_lock = threading.Lock() +switchState_lock = threading.Lock() +controller_lock = threading.Lock() + +#################################################################################### +# Camera Thread + +def capture_thread(): + + while True: + ret, frame = cap.read() + if not ret: + break + + # Define the black box position (adjust as needed) + top_left = (0, 600) + bottom_right = (1280, 720) + + # Draw a black rectangle (filled) + cv2.rectangle(frame, top_left, bottom_right, (0, 0, 0), thickness=-1) + + # Flip Frame + frame = cv2.flip(frame, 0) + + # Put the frame into the queue + if not frame_queue.full(): + frame_queue.put(frame) + +#################################################################################### +# Processing Thread + +def processing_thread(): + global pecanCount, ids + sampleStart = time.time() + + while True: + # Check if there are any frames in the queue + if not frame_queue.empty(): + frame = frame_queue.get() + + results = model.track(frame, persist=True, classes=0, device=device, stream=True, tracker='bytetrack_custom.yaml', iou = 0.6) + + for result in results: + # Check if there are any boxes in the result + if result.boxes is not None and result.boxes.id is not None: + # Get the boxes (x, y, w, h), class IDs, track IDs, and confidences + boxes = result.boxes.xyxy.int().tolist() # Bounding boxes + track_ids = result.boxes.id.int().tolist() # Track IDs + + # Iterate over each detected object + for box, track_id in zip(boxes, track_ids): + x1, y1, x2, y2 = box + cy = int((y1 + y2) // 2) # Center Y-coordinate + + if cy < (cy1 + offset) and cy > (cy1 - offset) and track_id not in ids: + pecanCount += 1 + ids.add(track_id) + + sampleEnd = time.time() + if (sampleEnd - sampleStart) > samplePeriod: + measuredCount = pecanCount / samplePeriod + + with ukf_lock: + ukf.predict() + ukf.update(np.array([measuredCount])) + + filteredCount = ukf.x[0] + + if switchState == 1: + with controller_lock: + controllerOutput = (controller.compute(refThroughput, filteredCount)) + mqtt_client.publish(MQTT_CONTROL_TOPIC, str(int((controllerOutput) / 90 * 129))) + mqtt_client.publish(MQTT_CONTROL_VISUAL_TOPIC, str(controllerOutput)) + + mqtt_client.publish(MQTT_COUNT_TOPIC, str(filteredCount)) + + pecanCount = 0 # Reset count for next sample period + sampleStart = time.time() # Reset start time for next sample period + +#################################################################################### +# Threading + +# Start capture and processing threads +capture_thread = threading.Thread(target=capture_thread) +capture_thread.daemon = True # Ensures the thread exits when the main program exits +capture_thread.start() + +processing_thread = threading.Thread(target=processing_thread) +processing_thread.daemon = True # Ensures the thread exits when the main program exits +processing_thread.start() + +#################################################################################### +# Clean Up + +try: + while True: + # Keep the main loop alive + if cv2.waitKey(1) & 0xFF == ord('q'): # Press 'q' to quit + break +except KeyboardInterrupt: + print("Process interrupted") + +# Release resources when exiting +cap.release() +cv2.destroyAllWindows() +mqtt_client.loop_stop() +mqtt_client.disconnect() +print("Capture Released") + +#################################################################################### + + diff --git a/yolo11n-pecan.pt b/yolo11n-pecan.pt new file mode 100644 index 0000000..559a62f Binary files /dev/null and b/yolo11n-pecan.pt differ