Hopefully Final Control Update...minus esp bug
This commit is contained in:
engr-ugaif engr-ugaif
commit
ffe9f79432
|
|
@ -0,0 +1,128 @@
|
|||
#include <M5StickCPlus2.h> // Correct library header for M5StickC Plus2 // Include M5StickCPlus2 library
|
||||
#include <SPI.h>
|
||||
#include <WiFi.h>
|
||||
#include <PubSubClient.h>
|
||||
|
||||
#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();
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -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)
|
||||
|
|
@ -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")
|
||||
|
||||
####################################################################################
|
||||
|
||||
|
||||
Binary file not shown.
Loading…
Reference in New Issue