#include "Arduino.h"
// ============================================================================
// STUDENT CONFIGURATION
// Replace 0 with your student number (numeric digits only)
// Example: For UP1234567, enter 1234567
// ============================================================================
#define STUDENT_NUMBER 2195942
// ============================================================================
// SYSTEM PARAMETERS - AUTO-GENERATED FROM STUDENT_NUMBER
// DO NOT MODIFY MANUALLY - Computed by generateParameters()
// ============================================================================
struct SystemParams
{
float v1, v2, v3; // Conveyor speeds (cm/s)
float d0, d1, d2, d3, d4; // Conv1 distances from obstruction sensor (cm)
float d5, d6; // Conv2/Conv3 total lengths (cm)
float L_min, L_max, L_threshold; // Length criteria (cm)
float W_min, W_max, W_threshold; // Weight criteria (g)
float T_gate; // Gate mechanical response time (ms)
int C1, C2, C3; // Area capacities (products)
int productInterval; // Product loading interval (ms)
};
SystemParams sp;
#define DIST_LEAD 10.0f // Fixed: entry point to obstruction sensor (cm)
#define DEBUG_PIN 12
// ============================================================================
// TYPE DEFINITIONS
// ============================================================================
enum class ProductEventType
{
OBSTRUCTION_SENSOR_RISE,
OBSTRUCTION_SENSOR_FALL,
G2_CHECK, // Size gate check
WEIGHT_SENSOR_ENTER_WINDOW,
WEIGHT_SENSOR_EXIT_WINDOW,
G3_CHECK, // Weight gate check
BARCODE_SENSOR_ENTER_WINDOW,
BARCODE_SENSOR_EXIT_WINDOW,
G4_CHECK, // Routing gate check
ARRIVE_DESTINATION, // Product reaches final area
FINISH
};
struct Product
{
float length; // Product length (cm)
float weight; // Product weight (g)
int barcodeID; // Barcode tracking number
};
struct ProductEvent
{
uint64_t triggerTimeMicros;
ProductEventType eventType;
int gateExpected; // For gate checks: expected gate state (0 or 1)
int destArea; // For ARRIVE: destination area (0=A1, 1=A2, 2=A3)
};
struct ProductScheduler
{
Product product;
ProductEvent events[16];
int totalEvents;
int currentEventIndex;
int debugPin;
hw_timer_t *timerHandle;
};
volatile int errorGateCNT[3] = {0};
// Ground truth area counts (maintained by simulator)
volatile int simAreaCount[3] = {0, 0, 0}; // Area1, Area2, Area3
extern void startSimulator(void);
// ============================================================================
// PARAMETER GENERATION (deterministic from student number)
// ============================================================================
static uint32_t paramHash(uint32_t seed, uint32_t idx)
{
uint32_t v = seed * 2654435761u + idx * 2246822519u;
v ^= v >> 16;
v *= 0x45d9f3bu;
v ^= v >> 16;
return v;
}
static float paramRangeF(uint32_t seed, uint32_t idx, float lo, float hi)
{
return lo + (float)(paramHash(seed, idx) % 10001u) / 10000.0f * (hi - lo);
}
static int paramRangeI(uint32_t seed, uint32_t idx, int lo, int hi)
{
return lo + (int)(paramHash(seed, idx) % (uint32_t)(hi - lo + 1));
}
void generateParameters(uint32_t sn)
{
if (sn == 0)
{
// Default parameters (no student number set)
sp.v1 = 100.0f;
sp.v2 = 120.0f;
sp.v3 = 80.0f;
sp.d0 = 15.0f;
sp.d1 = 25.0f;
sp.d2 = 35.0f;
sp.d3 = 45.0f;
sp.d4 = 55.0f;
sp.d5 = 40.0f;
sp.d6 = 40.0f;
sp.L_min = 4.5f;
sp.L_max = 5.5f;
sp.L_threshold = 5.0f;
sp.W_min = 90.0f;
sp.W_max = 110.0f;
sp.W_threshold = 100.0f;
sp.T_gate = 3.0f;
sp.C1 = 5;
sp.C2 = 10;
sp.C3 = 10;
sp.productInterval = 500;
return;
}
sp.v1 = paramRangeF(sn, 1, 80.0f, 120.0f);
sp.v2 = paramRangeF(sn, 2, 100.0f, 150.0f);
sp.v3 = paramRangeF(sn, 3, 60.0f, 100.0f);
sp.d0 = paramRangeF(sn, 4, 12.0f, 20.0f);
sp.d1 = sp.d0 + paramRangeF(sn, 5, 8.0f, 15.0f);
sp.d2 = sp.d1 + paramRangeF(sn, 6, 8.0f, 15.0f);
sp.d3 = sp.d2 + paramRangeF(sn, 7, 8.0f, 15.0f);
sp.d4 = sp.d3 + paramRangeF(sn, 8, 8.0f, 15.0f);
sp.d5 = paramRangeF(sn, 9, 30.0f, 50.0f);
sp.d6 = paramRangeF(sn, 10, 30.0f, 50.0f);
sp.L_min = paramRangeF(sn, 11, 4.0f, 5.0f);
sp.L_max = sp.L_min + paramRangeF(sn, 12, 0.8f, 1.5f);
sp.L_threshold = (sp.L_min + sp.L_max) / 2.0f;
sp.W_min = paramRangeF(sn, 13, 80.0f, 100.0f);
sp.W_max = sp.W_min + paramRangeF(sn, 14, 15.0f, 30.0f);
sp.W_threshold = (sp.W_min + sp.W_max) / 2.0f;
sp.T_gate = paramRangeF(sn, 15, 2.0f, 5.0f);
sp.C1 = paramRangeI(sn, 16, 3, 8);
sp.C2 = paramRangeI(sn, 17, 8, 15);
sp.C3 = paramRangeI(sn, 18, 8, 15);
sp.productInterval = paramRangeI(sn, 19, 400, 600);
}
void printParameters()
{
Serial.println("========== SmartSort System Parameters ==========");
Serial.printf("Student Number: %u\n", (unsigned)STUDENT_NUMBER);
Serial.println("----- Conveyor Speeds -----");
Serial.printf(" v1 (Conv1): %.1f cm/s\n", sp.v1);
Serial.printf(" v2 (Conv2): %.1f cm/s\n", sp.v2);
Serial.printf(" v3 (Conv3): %.1f cm/s\n", sp.v3);
Serial.println("----- Distances (from obstruction sensor, cm) -----");
Serial.printf(" d0 (to G2): %.1f\n", sp.d0);
Serial.printf(" d1 (to Weight): %.1f\n", sp.d1);
Serial.printf(" d2 (to G3): %.1f\n", sp.d2);
Serial.printf(" d3 (to Barcode): %.1f\n", sp.d3);
Serial.printf(" d4 (to G4): %.1f\n", sp.d4);
Serial.printf(" d5 (Conv2 len): %.1f\n", sp.d5);
Serial.printf(" d6 (Conv3 len): %.1f\n", sp.d6);
Serial.println("----- Length Criteria (cm) -----");
Serial.printf(" L_min=%.2f L_max=%.2f L_threshold=%.2f\n", sp.L_min, sp.L_max, sp.L_threshold);
Serial.println("----- Weight Criteria (g) -----");
Serial.printf(" W_min=%.2f W_max=%.2f W_threshold=%.2f\n", sp.W_min, sp.W_max, sp.W_threshold);
Serial.println("----- Timing & Capacity -----");
Serial.printf(" T_gate=%.1f ms Interval=%d ms\n", sp.T_gate, sp.productInterval);
Serial.printf(" C1=%d C2=%d C3=%d\n", sp.C1, sp.C2, sp.C3);
Serial.println("=================================================");
}
// ============================================================================
// PRODUCT ARRAY - Generated based on parameters to cover all sorting paths
// Students may modify this array for additional testing
// ============================================================================
#define MAX_TEST_PRODUCTS 20
Product productArray[MAX_TEST_PRODUCTS];
int numProducts = 0;
void generateProductArray()
{
int idx = 0;
float Lmid = (sp.L_min + sp.L_max) / 2.0f;
float Wmid = (sp.W_min + sp.W_max) / 2.0f;
// Category 1: Size abnormal (too short) -> Area 1 via G2
productArray[idx++] = {sp.L_min - 0.5f, Wmid, 1001};
productArray[idx++] = {sp.L_min - 1.0f, Wmid + 3.0f, 1002};
// Category 2: Size abnormal (too long) -> Area 1 via G2
productArray[idx++] = {sp.L_max + 0.5f, Wmid, 1003};
productArray[idx++] = {sp.L_max + 0.8f, Wmid - 2.0f, 1004};
// Category 3: Normal size, weight too light -> Area 1 via G3
productArray[idx++] = {Lmid, sp.W_min - 5.0f, 1005};
productArray[idx++] = {sp.L_min + 0.1f, sp.W_min - 3.0f, 1006};
// Category 4: Normal size, weight too heavy -> Area 1 via G3
productArray[idx++] = {Lmid, sp.W_max + 5.0f, 1007};
productArray[idx++] = {sp.L_max - 0.1f, sp.W_max + 3.0f, 1008};
// Category 5: Normal, small & light -> Area 2 (L<=Lthresh AND W<=Wthresh)
productArray[idx++] = {sp.L_min + 0.1f, sp.W_min + 1.0f, 1009};
productArray[idx++] = {sp.L_threshold, sp.W_threshold, 1010}; // Edge case: exactly at threshold
productArray[idx++] = {sp.L_threshold - 0.1f, sp.W_threshold - 1.0f, 1011};
// Category 6: Normal, large or heavy -> Area 3
productArray[idx++] = {sp.L_threshold + 0.1f, sp.W_threshold - 1.0f, 1012};
productArray[idx++] = {sp.L_min + 0.2f, sp.W_threshold + 1.0f, 1013};
productArray[idx++] = {sp.L_max - 0.1f, sp.W_max - 1.0f, 1014};
productArray[idx++] = {sp.L_threshold + 0.2f, sp.W_threshold + 2.0f, 1015};
// Edge cases
productArray[idx++] = {sp.L_min, sp.W_min, 1016}; // Exactly at min boundaries -> Area 2
productArray[idx++] = {sp.L_max, sp.W_max, 1017}; // Exactly at max -> Area 3
productArray[idx++] = {Lmid, Wmid, 1018}; // Center values -> Area 2
numProducts = idx;
}
/*!!!!!!!!!!!!!!!!!!ANY CODE ABOVE THIS LINE SHOULD NOT BE MODIFIED BY STUDENTS!!!!!!!!!!!!!!!!!!!*/
/*=========================== INTERFACE BETWEEN SIMULATOR AND STUDENT CODE ============================*/
/*
g_obstructionSensorPin outputs the physical signal of the obstruction sensor (for oscilloscope).
*/
#define g_obstructionSensorPin 32
#define g_barcodeSensorPin 34
#define g_weightSensorPin 35
/*
Debug output pins for gate signals (for oscilloscope verification).
The simulator does NOT read these - they are for your debugging only.
*/
// #define gate2DebugPin 33
#define gate3DebugPin 27
#define gate4DebugPin 14
#define sizeAbnormalPin 25
#define weightAbnormalPin 26
#define g4DebugPin 27
/*
SHARED VARIABLES - DO NOT RENAME
The simulator uses these to provide sensor data and check your gate controls.
*/
// ========== Simulator writes, students read ==========
volatile bool g_obstructionSensor = false; // Obstruction sensor state
volatile int g_barcodeReader = 0; // Barcode reading (0=invalid/not in window)
volatile float g_weightSensor = 0.0f; // Weight reading (0=invalid/not in window)
// ========== Students write, simulator reads ==========
// G1 (Intake): true=allow products in, false=block entry
volatile bool g_gate1Ctrl = true;
// G2 (Size divert): false=let pass, true=activate pusher (divert to Area 1)
volatile bool g_gate2Ctrl = false;
// G3 (Weight divert): false=let pass, true=activate pusher (divert to Area 1)
volatile bool g_gate3Ctrl = false;
// G4 (Routing): false=route to Conv2/Area2, true=route to Conv3/Area3
volatile bool g_gate4Ctrl = false;
volatile bool emergencyActive = false;
/*================================= IMPLEMENT YOUR RTOS CODE BELOW ===================================*/
// ESP32 Board Library (ESP32 by Espressif) version: V3.3.7
//
// Your controller must:
// 1. Detect products via the obstruction sensor interrupt
// 2. Calculate product length from sensor blocking duration
// 3. Control G2 to divert size-abnormal products to Area 1
// 4. Read weight sensor when product is centered at d1
// 5. Control G3 to divert weight-abnormal products to Area 1
// 6. Read barcode when product is centered at d3
// 7. Control G4 to route products to Conv2 (Area 2) or Conv3 (Area 3)
// 8. Control G1 to prevent area overflow (capacity management)
// 9. Output real-time status and respond to serial commands
//
// Classification rules:
// - Size abnormal (L < L_min or L > L_max) -> Area 1 via G2
// - Weight abnormal (W < W_min or W > W_max) -> Area 1 via G3
// - L <= L_threshold AND W <= W_threshold -> Area 2 via G4 (Conv2)
// - Otherwise -> Area 3 via G4 (Conv3)
//
// Access parameters via: sp.v1, sp.d0, sp.L_min, sp.T_gate, sp.C1, etc.
//
// Timing hint: after obstruction sensor fall edge, the time for product center
// to reach the weight sensor is approximately:
// T = (sp.d1 - product_length/2) / sp.v1 * 1000 (in ms, from sensor position)
//
// Key APIs:
// esp_timer_get_time() - ISR-safe microsecond timestamp
// xQueueCreate() - Create FreeRTOS queue
// xQueueSendFromISR() - Send data from ISR to task
// xQueueReceive() - Receive data in task (blocking)
// xTaskCreatePinnedToCore() - Create task on specific core
// xTimerCreate() - Create software timer
// vTaskDelay() - Delay task (ms / portTICK_PERIOD_MS)
// ============================================================================
// OBSTRUCTION SENSOR ISR
// This ISR is called by the simulator when the obstruction sensor state changes.
// It runs in hardware timer ISR context — only use ISR-safe functions.
//
// When g_obstructionSensor == true: rising edge (product front arrived)
// When g_obstructionSensor == false: falling edge (product back passed)
//
// TODO: Implement this ISR to capture timestamps and communicate with tasks
// Ref: Lab session material - Interrupts, Queues
// ============================================================================
/* ================= GLOBAL ================= */
QueueHandle_t sensorQueue;
struct SensorEvent
{
bool state;
uint64_t time;
};
struct ProductState
{
float length;
float weight;
int barcode;
uint64_t t_rise;
bool weightRead;
bool barcodeRead;
int destination; // 0=Area1, 1=Area2, 2=Area3
bool finished;
// Gate flags
bool g2_triggered;
bool g3_triggered;
bool g4_triggered;
// Timing
uint64_t gate_off_time; // Reuse for all gates
int active_gate; // Which gate is currently active (-1, 2, 3, 4)
bool diverted; // True if product was diverted by G2 or G3
int trackingNum;
bool sizeAbnormal;
bool weightAbnormal;
};
#define MAX_PRODUCTS 20
ProductState products[MAX_PRODUCTS];
int productIndex = 0;
/* ================= ISR ================= */
void IRAM_ATTR obstructionSensorInterrupt()
{
SensorEvent evt;
evt.state = g_obstructionSensor;
evt.time = esp_timer_get_time();
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
xQueueSendFromISR(sensorQueue, &evt, &xHigherPriorityTaskWoken);
if (xHigherPriorityTaskWoken)
portYIELD_FROM_ISR();
}
/* ================= CORE TASK ================= */
void controlTask(void *pvParameters)
{
SensorEvent evt;
uint64_t riseTime = 0;
bool measuring = false;
float epsilon = 0.0001f;
while (1)
{
if (emergencyActive) {
vTaskDelay(10 / portTICK_PERIOD_MS);
continue;
}
// Process sensor events
if (xQueueReceive(sensorQueue, &evt, 0))
{
if (evt.state == true)
{
riseTime = evt.time;
measuring = true;
}
else if (measuring)
{
float duration = (evt.time - riseTime) / 1000000.0;
float length = duration * sp.v1;
float epsilon = 0.001;
if (productIndex < MAX_PRODUCTS)
{
ProductState &p = products[productIndex];
p.length = length;
p.t_rise = riseTime;
p.weight = 0;
p.barcode = 0;
p.weightRead = false;
p.barcodeRead = false;
p.destination = -1; // Unknown - not yet classified
p.finished = false;
p.diverted = false;
p.sizeAbnormal = (length < sp.L_min - epsilon || length > sp.L_max + epsilon);
p.weightAbnormal = false;
// Gate flags
p.g2_triggered = false;
p.g3_triggered = false;
p.g4_triggered = false;
// Timing
p.gate_off_time = 0;
p.active_gate = -1;
p.trackingNum = 0;
productIndex++;
measuring = false;
}
}
}
uint64_t now = esp_timer_get_time();
for (int i = 0; i < productIndex; i++)
{
ProductState &p = products[i];
if (p.finished)
continue;
// === G2 - SIZE DIVERSION ===
if (!p.g2_triggered && p.destination == -1 && !p.diverted)
{
uint64_t d0_time = p.t_rise + (uint64_t)((sp.d0 / sp.v1) * 1000000);
uint64_t gate_activate_start = d0_time - (uint64_t)(sp.T_gate * 1000) - 30000;
uint64_t gate_activate_end = d0_time + 30000;
if (now >= gate_activate_start && now <= gate_activate_end)
{
bool sizeAbnormal = (p.length < sp.L_min - epsilon || p.length > sp.L_max + epsilon);
if (sizeAbnormal)
{
g_gate2Ctrl = true;
p.active_gate = 2;
p.destination = 0; // Area 1
p.diverted = true;
p.gate_off_time = d0_time + (uint64_t)((p.length / sp.v1) * 1000000);
digitalWrite(sizeAbnormalPin, HIGH);
}
else
{
g_gate2Ctrl = false;
}
p.g2_triggered = true;
}
}
// === RESET G2 ===
if (p.active_gate == 2 && now >= p.gate_off_time)
{
g_gate2Ctrl = false;
p.active_gate = -1;
digitalWrite(sizeAbnormalPin, LOW);
}
// === WEIGHT MEASUREMENT ===
if (!p.weightRead && !p.diverted)
{
uint64_t d1_center = p.t_rise + (uint64_t)((sp.d1 / sp.v1) * 1000000);
uint64_t window_start = d1_center - 50000;
uint64_t window_end = d1_center + 50000;
if (now >= window_start && now <= window_end)
{
if (g_weightSensor > 0 && !p.weightRead)
{
p.weight = g_weightSensor;
p.weightRead = true;
p.weightAbnormal = (p.weight < sp.W_min || p.weight > sp.W_max);
}
}
else if (now > window_end && !p.weightRead)
{
p.weightRead = true;
p.weightAbnormal = true; // Mark as abnormal if no reading
}
}
// === G3 - WEIGHT DIVERSION ===
// ONLY run G3 if product is NOT size abnormal, NOT diverted, weight is read, and weight > 0
if (!p.g3_triggered && !p.diverted && p.weightRead && p.weight > 0 && !p.sizeAbnormal)
{
uint64_t d2_time = p.t_rise + (uint64_t)((sp.d2 / sp.v1) * 1000000);
uint64_t gate_activate_start = d2_time - (uint64_t)(sp.T_gate * 1000) - 20000;
uint64_t gate_activate_end = d2_time + 20000;
if (now >= gate_activate_start && now <= gate_activate_end)
{
bool weightAbnormal = (p.weight < sp.W_min - epsilon || p.weight > sp.W_max + epsilon);
if (weightAbnormal)
{
g_gate3Ctrl = true;
p.active_gate = 3;
p.destination = 0; // Area 1
p.diverted = true;
p.gate_off_time = d2_time + (uint64_t)((p.length / sp.v1) * 1000000);
digitalWrite(weightAbnormalPin, HIGH);
}
else
{
g_gate3Ctrl = false;
}
p.g3_triggered = true;
}
}
else if (!p.g3_triggered && (p.diverted || p.sizeAbnormal))
{
// Skip G3 for already diverted or size abnormal products
p.g3_triggered = true;
}
// === RESET G3 ===
if (p.active_gate == 3 && now >= p.gate_off_time)
{
g_gate3Ctrl = false;
p.active_gate = -1;
digitalWrite(weightAbnormalPin, LOW);
}
// === BARCODE SCANNING ===
if (!p.barcodeRead && !p.diverted && p.weightRead)
{
uint64_t d3_center = p.t_rise + (uint64_t)((sp.d3 / sp.v1) * 1000000);
uint64_t window_start = d3_center - 20000;
uint64_t window_end = d3_center + 50000;
if (now >= window_start && now <= window_end)
{
if (g_barcodeReader != 0 && !p.barcodeRead)
{
p.barcode = g_barcodeReader;
p.barcodeRead = true;
p.trackingNum = g_barcodeReader;
}
}
else if (now > window_end && !p.barcodeRead)
{
p.barcode = 0;
p.barcodeRead = true;
p.trackingNum = 0;
}
}
// === G4 - FINAL ROUTING ===
// ONLY run G4 if NOT diverted, G2/G3 done, barcode read
if (!p.g4_triggered && !p.diverted && p.barcodeRead && p.g2_triggered && p.g3_triggered)
{
uint64_t d4_time = p.t_rise + (uint64_t)((sp.d4 / sp.v1) * 1000000);
uint64_t gate_activate_start = d4_time - (uint64_t)(sp.T_gate * 1000) - 20000;
uint64_t gate_activate_end = d4_time + 20000;
if (now >= gate_activate_start && now <= gate_activate_end)
{
// Route based on classification
bool toArea2 = (p.length <= sp.L_threshold + epsilon && p.weight <= sp.W_threshold + epsilon);
if (toArea2)
{
g_gate4Ctrl = false;
p.destination = 1; // Area 2
digitalWrite(g4DebugPin, LOW);
}
else
{
g_gate4Ctrl = true;
p.destination = 2; // Area 3
digitalWrite(g4DebugPin, HIGH);
}
p.g4_triggered = true;
p.active_gate = 4;
p.gate_off_time = d4_time + (uint64_t)((p.length / sp.v1) * 1000000);
}
}
// === RESET G4 ===
if (p.active_gate == 4 && now >= p.gate_off_time)
{
g_gate4Ctrl = false;
p.active_gate = -1;
digitalWrite(g4DebugPin, LOW);
}
// === MARK PRODUCT AS FINISHED ===
if (p.g4_triggered && !p.finished)
{
uint64_t finish_time = 0;
if (p.destination == 1) // Area 2
{
uint64_t travel_time = (uint64_t)((sp.d5 / sp.v2) * 1000000);
uint64_t g4_time = p.t_rise + (uint64_t)((sp.d4 / sp.v1) * 1000000);
finish_time = g4_time + travel_time;
}
else if (p.destination == 2) // Area 3
{
uint64_t travel_time = (uint64_t)((sp.d6 / sp.v3) * 1000000);
uint64_t g4_time = p.t_rise + (uint64_t)((sp.d4 / sp.v1) * 1000000);
finish_time = g4_time + travel_time;
}
else if (p.destination == 0) // Area 1 - diverted
{
finish_time = p.t_rise + (uint64_t)(((sp.d4 + 30) / sp.v1) * 1000000);
}
finish_time += 5000;
if (now >= finish_time)
{
p.finished = true;
// Update in-transit counters (if you have them)
// if (p.destination == 1 && inTransitToArea2 > 0) inTransitToArea2--;
// else if (p.destination == 2 && inTransitToArea3 > 0) inTransitToArea3--;
// else if (p.destination == 0 && inTransitToArea1 > 0) inTransitToArea1--;
}
}
}
vTaskDelay(1 / portTICK_PERIOD_MS);
}
}
/* ================= G1 CONTROL ================= */
void gate1Task(void *pvParameters)
{
while (1)
{
if (emergencyActive) {
vTaskDelay(10 / portTICK_PERIOD_MS);
continue;
}
// Check if there's ANY capacity in the system
// bool area1HasSpace = (simAreaCount[0] < sp.C1);
bool area2HasSpace = (simAreaCount[1] < sp.C2);
bool area3HasSpace = (simAreaCount[2] < sp.C3);
bool anySpace = /*area1HasSpace ||*/ area2HasSpace || area3HasSpace;
// Also check total system capacity
int totalOccupied = simAreaCount[0] + simAreaCount[1] + simAreaCount[2];
int totalCapacity = sp.C1 + sp.C2 + sp.C3;
// Only close G1 if NO space available anywhere
if (!anySpace || totalOccupied >= totalCapacity)
{
if (g_gate1Ctrl)
{
g_gate1Ctrl = false;
digitalWrite(gate4DebugPin, HIGH);
// Serial.printf("[G1] CLOSED - No capacity left\n");
}
}
else
{
if (!g_gate1Ctrl)
{
g_gate1Ctrl = true;
digitalWrite(gate4DebugPin, HIGH);
// Serial.printf("[G1] OPEN - Capacity available\n");
}
}
vTaskDelay(100 / portTICK_PERIOD_MS);
}
}
void statusOutputTask(void *pvParameters)
{
while (1) {
if (emergencyActive) {
vTaskDelay(100 / portTICK_PERIOD_MS);
continue;
}
vTaskDelay(1000 / portTICK_PERIOD_MS);
// FIX: Use %03d for 3-digit zero padding
Serial.printf("sortRT: %03d,%03d,%03d",
simAreaCount[0], simAreaCount[1], simAreaCount[2]);
for (int i = 0; i < productIndex; i++) {
ProductState &p = products[i];
if (!p.finished && p.destination != -1) {
char dest = (p.destination == 0) ? '1' : ((p.destination == 1) ? '2' : '3');
int tn = (p.trackingNum != 0) ? p.trackingNum : (i + 1);
Serial.printf(", TN%04d,%.1f,%.1f,A%c",
tn, p.length, p.weight, dest);
}
}
Serial.println();
}
}
void commandTask(void *pvParameters)
{
while (1)
{
if (Serial.available())
{
String cmd = Serial.readStringUntil('\n');
cmd.trim();
if (cmd == "stat")
{
// Variables for overall averages
float totalLength = 0;
float totalWeight = 0;
int totalProducts = 0;
// Variables for area-specific averages
float totalLengthArea2 = 0;
float totalWeightArea2 = 0;
int area2Count = 0;
float totalLengthArea3 = 0;
float totalWeightArea3 = 0;
int area3Count = 0;
// Iterate through all products that have finished
for (int i = 0; i < productIndex; i++)
{
ProductState &p = products[i];
// Only include finished products (already arrived at destination)
if (p.finished)
{
// Overall averages (include all products)
totalLength += p.length;
totalWeight += (p.weight > 0 ? p.weight : 0);
totalProducts++;
// Area-specific averages (Area2 and Area3 only)
if (p.destination == 1)
{ // Area 2
totalLengthArea2 += p.length;
totalWeightArea2 += (p.weight > 0 ? p.weight : 0);
area2Count++;
}
else if (p.destination == 2)
{ // Area 3
totalLengthArea3 += p.length;
totalWeightArea3 += (p.weight > 0 ? p.weight : 0);
area3Count++;
}
// Area 1 products are NOT included in area-specific averages
}
}
// Calculate averages (avoid division by zero)
float Lavg = (totalProducts > 0) ? (totalLength / totalProducts) : 0;
float Wavg = (totalProducts > 0) ? (totalWeight / totalProducts) : 0;
float L2avg = (area2Count > 0) ? (totalLengthArea2 / area2Count) : 0;
float W2avg = (area2Count > 0) ? (totalWeightArea2 / area2Count) : 0;
float L3avg = (area3Count > 0) ? (totalLengthArea3 / area3Count) : 0;
float W3avg = (area3Count > 0) ? (totalWeightArea3 / area3Count) : 0;
// Output in required format: sortSTAT: Lavg, L2avg, L3avg, Wavg, W2avg, W3avg
Serial.printf("sortSTAT: %.2f,%.2f,%.2f,%.2f,%.2f,%.2f\n",
Lavg, L2avg, L3avg, Wavg, W2avg, W3avg);
}
else if (cmd == "reset")
{
// Wait for all active products to finish (200ms should be enough)
vTaskDelay(500 / portTICK_PERIOD_MS);
// Reset all system states
productIndex = 0;
for (int i = 0; i < 3; i++)
{
simAreaCount[i] = 0;
errorGateCNT[i] = 0;
}
// Clear all products
for (int i = 0; i < MAX_PRODUCTS; i++)
{
products[i] = ProductState();
}
// Reset gates
g_gate1Ctrl = true;
g_gate2Ctrl = false;
g_gate3Ctrl = false;
g_gate4Ctrl = false;
emergencyActive = false;
// CRITICAL: Clear the sensor queue
SensorEvent dummy;
while (xQueueReceive(sensorQueue, &dummy, 0))
{
// Empty the queue
}
}
else if (cmd == "emergency")
{
emergencyActive = true;
g_gate1Ctrl = false;
g_gate2Ctrl = false;
g_gate3Ctrl = false;
g_gate4Ctrl = false;
Serial.println("EMERGENCY: System halted");
}
}
vTaskDelay(100 / portTICK_PERIOD_MS);
}
}
/* ================= SETUP ================= */
void setup()
{
Serial.begin(115200);
// Initialize sensor pins (inputs)
pinMode(g_obstructionSensorPin, INPUT_PULLUP); // Obstruction sensor
pinMode(g_barcodeSensorPin, INPUT); // Barcode sensor
pinMode(g_weightSensorPin, INPUT); // Weight sensor
// Initialize gate pins (outputs)
pinMode(sizeAbnormalPin, OUTPUT); // G2 LED
pinMode(weightAbnormalPin, OUTPUT); // G3 LED
pinMode(gate3DebugPin, OUTPUT); // G4 LED
pinMode(gate4DebugPin, OUTPUT); // G1 LED
// Initialize all gates to OFF
digitalWrite(sizeAbnormalPin, LOW);
digitalWrite(weightAbnormalPin, LOW);
digitalWrite(gate3DebugPin, LOW);
digitalWrite(gate4DebugPin, LOW);
sensorQueue = xQueueCreate(10, sizeof(SensorEvent));
xTaskCreatePinnedToCore(controlTask, "Control", 4096, NULL, 2, NULL, 0);
xTaskCreatePinnedToCore(gate1Task, "Gate1", 2048, NULL, 1, NULL, 1);
// Create tasks
xTaskCreatePinnedToCore(statusOutputTask, "Status", 4096, NULL, 1, NULL, 0);
xTaskCreatePinnedToCore(commandTask, "Commands", 4096, NULL, 1, NULL, 1);
startSimulator();
}
void loop()
{
vTaskDelay(portMAX_DELAY);
}
/******************************************** END OF STUDENT CODE *****************************************/
/*!!!!!!!!!!!!!!!!!!!!!!!!!!! ANY CODE BELOW THIS LINE SHOULD NOT BE MODIFIED !!!!!!!!!!!!!!!!!!!!!!!!!!!*/
/*================================ START OF SIMULATOR ENGINE ============================================*/
volatile int currentProductIndex = 0;
const int MAX_SCHEDULERS = 4;
ProductScheduler g_schedulers[MAX_SCHEDULERS];
static inline uint64_t distanceToMicros(float distance_cm, float speed_cm_s)
{
double tSec = (double)distance_cm / (double)speed_cm_s;
return (uint64_t)(tSec * 1e6);
}
void fillProductEvents(ProductScheduler &sch)
{
const Product &p = sch.product;
sch.totalEvents = 0;
sch.currentEventIndex = 0;
auto &events = sch.events;
int idx = 0;
uint64_t leadTime = distanceToMicros(DIST_LEAD, sp.v1);
// Determine product's ground-truth path
bool sizeOk = (p.length >= sp.L_min && p.length <= sp.L_max);
bool weightOk = (p.weight >= sp.W_min && p.weight <= sp.W_max);
bool isSmallLight = sizeOk && weightOk &&
(p.length <= sp.L_threshold) && (p.weight <= sp.W_threshold);
// Pre-compute destination area index (0=A1, 1=A2, 2=A3)
int destArea;
if (!sizeOk)
destArea = 0;
else if (!weightOk)
destArea = 0;
else if (isSmallLight)
destArea = 1;
else
destArea = 2;
// 1. Obstruction sensor: front edge arrives
events[idx].triggerTimeMicros = leadTime;
events[idx].eventType = ProductEventType::OBSTRUCTION_SENSOR_RISE;
events[idx].gateExpected = 0;
events[idx].destArea = 0;
idx++;
// 2. Obstruction sensor: back edge leaves
uint64_t fallTime = distanceToMicros(p.length, sp.v1);
events[idx].triggerTimeMicros = leadTime + fallTime;
events[idx].eventType = ProductEventType::OBSTRUCTION_SENSOR_FALL;
events[idx].gateExpected = 0;
events[idx].destArea = 0;
idx++;
// 3. G2 check (size gate) - front edge at d0
// G2: true=divert (size abnormal), expected true if !sizeOk
uint64_t g2Time = distanceToMicros(sp.d0, sp.v1) + leadTime;
events[idx].triggerTimeMicros = g2Time;
events[idx].eventType = ProductEventType::G2_CHECK;
events[idx].gateExpected = sizeOk ? 0 : 1;
events[idx].destArea = 0;
idx++;
if (!sizeOk)
{
// Product diverted at G2 -> Area 1
events[idx].triggerTimeMicros = g2Time + 5000;
events[idx].eventType = ProductEventType::ARRIVE_DESTINATION;
events[idx].gateExpected = 0;
events[idx].destArea = 0;
idx++;
events[idx].triggerTimeMicros = g2Time + 10000;
events[idx].eventType = ProductEventType::FINISH;
events[idx].gateExpected = 0;
events[idx].destArea = 0;
idx++;
}
else
{
// 4. Weight sensor window
uint64_t centerAtWeight = distanceToMicros(sp.d1 + p.length / 2.0f, sp.v1) + leadTime;
uint64_t halfWindowW = distanceToMicros(p.length * 0.1f, sp.v1);
events[idx].triggerTimeMicros = centerAtWeight - halfWindowW;
events[idx].eventType = ProductEventType::WEIGHT_SENSOR_ENTER_WINDOW;
events[idx].gateExpected = 0;
events[idx].destArea = 0;
idx++;
events[idx].triggerTimeMicros = centerAtWeight + halfWindowW;
events[idx].eventType = ProductEventType::WEIGHT_SENSOR_EXIT_WINDOW;
events[idx].gateExpected = 0;
events[idx].destArea = 0;
idx++;
// 5. G3 check (weight gate) - front edge at d2
// G3: true=divert (weight abnormal), expected true if !weightOk
uint64_t g3Time = distanceToMicros(sp.d2, sp.v1) + leadTime;
events[idx].triggerTimeMicros = g3Time;
events[idx].eventType = ProductEventType::G3_CHECK;
events[idx].gateExpected = weightOk ? 0 : 1;
events[idx].destArea = 0;
idx++;
if (!weightOk)
{
// Product diverted at G3 -> Area 1
events[idx].triggerTimeMicros = g3Time + 5000;
events[idx].eventType = ProductEventType::ARRIVE_DESTINATION;
events[idx].gateExpected = 0;
events[idx].destArea = 0;
idx++;
events[idx].triggerTimeMicros = g3Time + 10000;
events[idx].eventType = ProductEventType::FINISH;
events[idx].gateExpected = 0;
events[idx].destArea = 0;
idx++;
}
else
{
// 6. Barcode sensor window
uint64_t centerAtBarcode = distanceToMicros(sp.d3 + p.length / 2.0f, sp.v1) + leadTime;
uint64_t halfWindowB = distanceToMicros(p.length * 0.1f, sp.v1);
events[idx].triggerTimeMicros = centerAtBarcode - halfWindowB;
events[idx].eventType = ProductEventType::BARCODE_SENSOR_ENTER_WINDOW;
events[idx].gateExpected = 0;
events[idx].destArea = 0;
idx++;
events[idx].triggerTimeMicros = centerAtBarcode + halfWindowB;
events[idx].eventType = ProductEventType::BARCODE_SENSOR_EXIT_WINDOW;
events[idx].gateExpected = 0;
events[idx].destArea = 0;
idx++;
// 7. G4 check (routing gate) - front edge at d4
// G4: false=Conv2/Area2, true=Conv3/Area3
uint64_t g4Time = distanceToMicros(sp.d4, sp.v1) + leadTime;
events[idx].triggerTimeMicros = g4Time;
events[idx].eventType = ProductEventType::G4_CHECK;
events[idx].gateExpected = isSmallLight ? 0 : 1;
events[idx].destArea = 0;
idx++;
// 8. Arrive at destination area
uint64_t arriveTime;
if (isSmallLight)
{
arriveTime = g4Time + distanceToMicros(sp.d5, sp.v2);
}
else
{
arriveTime = g4Time + distanceToMicros(sp.d6, sp.v3);
}
events[idx].triggerTimeMicros = arriveTime;
events[idx].eventType = ProductEventType::ARRIVE_DESTINATION;
events[idx].gateExpected = 0;
events[idx].destArea = destArea;
idx++;
events[idx].triggerTimeMicros = arriveTime + 5000;
events[idx].eventType = ProductEventType::FINISH;
events[idx].gateExpected = 0;
events[idx].destArea = 0;
idx++;
}
}
sch.totalEvents = idx;
// Sort events by time
for (int i = 0; i < sch.totalEvents - 1; i++)
{
for (int j = i + 1; j < sch.totalEvents; j++)
{
if (events[i].triggerTimeMicros > events[j].triggerTimeMicros)
{
ProductEvent tmp = events[i];
events[i] = events[j];
events[j] = tmp;
}
}
}
}
void printProductEvents(const ProductScheduler &sch)
{
Serial.println("===== Product Event Timeline =====");
// Serial.printf("Length: %.2f cm Weight: %.2f g Barcode: %d\n",
// sch.product.length, sch.product.weight, sch.product.barcodeID);
bool sizeOk = (sch.product.length >= sp.L_min && sch.product.length <= sp.L_max);
bool weightOk = (sch.product.weight >= sp.W_min && sch.product.weight <= sp.W_max);
const char *dest = "???";
if (!sizeOk)
dest = "Area1(size)";
else if (!weightOk)
dest = "Area1(weight)";
else if (sch.product.length <= sp.L_threshold && sch.product.weight <= sp.W_threshold)
dest = "Area2";
else
dest = "Area3";
for (int i = 0; i < sch.totalEvents; i++)
{
const ProductEvent &e = sch.events[i];
// Serial.printf("[%10llu µs] Event: ", e.triggerTimeMicros);
switch (e.eventType)
{
case ProductEventType::OBSTRUCTION_SENSOR_RISE:
Serial.print("Obstruction Rise");
break;
case ProductEventType::OBSTRUCTION_SENSOR_FALL:
Serial.print("Obstruction Fall");
break;
case ProductEventType::G2_CHECK:
// Serial.printf("G2 Check (expected=%d)", e.gateExpected);
break;
case ProductEventType::WEIGHT_SENSOR_ENTER_WINDOW:
Serial.print("Weight Sensor Enter");
break;
case ProductEventType::WEIGHT_SENSOR_EXIT_WINDOW:
Serial.print("Weight Sensor Exit");
break;
case ProductEventType::G3_CHECK:
// Serial.printf("G3 Check (expected=%d)", e.gateExpected);
break;
case ProductEventType::BARCODE_SENSOR_ENTER_WINDOW:
Serial.print("Barcode Sensor Enter");
break;
case ProductEventType::BARCODE_SENSOR_EXIT_WINDOW:
Serial.print("Barcode Sensor Exit");
break;
case ProductEventType::G4_CHECK:
// Serial.printf("G4 Check (expected=%d)", e.gateExpected);
break;
case ProductEventType::ARRIVE_DESTINATION:
// Serial.printf("Arrive %s", dest);
break;
case ProductEventType::FINISH:
Serial.print("Finish");
break;
}
Serial.println();
}
Serial.println("==============================");
}
// Hardware timer ISR - executes product events
void ARDUINO_ISR_ATTR onTimerInterrupt(void *arg)
{
ProductScheduler *sch = (ProductScheduler *)arg;
hw_timer_t *timer = sch->timerHandle;
if (!timer)
return;
if (sch->currentEventIndex >= sch->totalEvents)
{
timerEnd(timer);
sch->timerHandle = nullptr;
return;
}
// Get current event
ProductEvent evt = sch->events[sch->currentEventIndex];
sch->currentEventIndex++;
// Execute event logic (no floating-point comparisons - all pre-computed)
int evtType = (int)evt.eventType;
if (evtType == (int)ProductEventType::OBSTRUCTION_SENSOR_RISE)
{
g_obstructionSensor = true;
digitalWrite(g_obstructionSensorPin, HIGH);
obstructionSensorInterrupt();
}
else if (evtType == (int)ProductEventType::OBSTRUCTION_SENSOR_FALL)
{
g_obstructionSensor = false;
digitalWrite(g_obstructionSensorPin, LOW);
obstructionSensorInterrupt();
}
else if (evtType == (int)ProductEventType::WEIGHT_SENSOR_ENTER_WINDOW)
{
// Serial.printf("[SIM DEBUG] Weight sensor ENTER for product %d, setting weight=%.2f\n",
// sch->product.barcodeID, sch->product.weight);
g_weightSensor = sch->product.weight;
}
else if (evtType == (int)ProductEventType::WEIGHT_SENSOR_EXIT_WINDOW)
{
// Serial.printf("[SIM DEBUG] Weight sensor EXIT for product %d, setting weight=0\n",
// sch->product.barcodeID);
g_weightSensor = 0.0f;
}
else if (evtType == (int)ProductEventType::BARCODE_SENSOR_ENTER_WINDOW)
{
// Serial.printf("[SIM DEBUG] Barcode sensor ENTER for product %d, setting barcode=%d\n",
// sch->product.barcodeID, sch->product.barcodeID);
g_barcodeReader = sch->product.barcodeID;
}
else if (evtType == (int)ProductEventType::BARCODE_SENSOR_EXIT_WINDOW)
{
g_barcodeReader = 0;
}
else if (evtType == (int)ProductEventType::G2_CHECK)
{
// G2: true=divert. Pre-computed expected value in gateExpected
if ((int)g_gate2Ctrl != evt.gateExpected)
{
errorGateCNT[0]++;
}
}
else if (evtType == (int)ProductEventType::G3_CHECK)
{
// G3: true=divert. Pre-computed expected value in gateExpected
if ((int)g_gate3Ctrl != evt.gateExpected)
{
errorGateCNT[1]++;
}
}
else if (evtType == (int)ProductEventType::G4_CHECK)
{
// G4: false=Conv2, true=Conv3. Pre-computed expected value
if ((int)g_gate4Ctrl != evt.gateExpected)
{
errorGateCNT[2]++;
}
}
else if (evtType == (int)ProductEventType::ARRIVE_DESTINATION)
{
simAreaCount[evt.destArea]++;
}
else if (evtType == (int)ProductEventType::FINISH)
{
digitalWrite(sch->debugPin, LOW);
}
// Set up next event or end timer
if (sch->currentEventIndex < sch->totalEvents)
{
timerAlarm(timer, sch->events[sch->currentEventIndex].triggerTimeMicros, false, 0);
}
else
{
timerEnd(timer);
sch->timerHandle = nullptr;
}
}
hw_timer_t *allocateTimerForProduct(ProductScheduler &sch)
{
digitalWrite(sch.debugPin, HIGH);
fillProductEvents(sch);
hw_timer_t *timer = timerBegin(1000000); // 1MHz = 1us resolution
if (!timer)
return nullptr;
sch.timerHandle = timer;
timerAttachInterruptArg(timer, &onTimerInterrupt, (void *)&sch);
if (sch.totalEvents > 0)
{
digitalWrite(sch.debugPin, LOW);
timerWrite(timer, 0);
timerAlarm(timer, sch.events[0].triggerTimeMicros, false, 0);
}
return timer;
}
// Status reporting task - prints simulator state periodically
void statusReportTask(void *pvParameters)
{
vTaskDelay(2000 / portTICK_PERIOD_MS); // Initial delay
while (true)
{
// Check if all products finished
if (currentProductIndex >= numProducts)
{
bool allDone = true;
for (int i = 0; i < MAX_SCHEDULERS; i++)
{
if (g_schedulers[i].timerHandle != nullptr)
{
allDone = false;
break;
}
}
if (allDone)
{
int totalArrived = simAreaCount[0] + simAreaCount[1] + simAreaCount[2];
Serial.println("\n=== SIMULATION COMPLETE ===");
if (errorGateCNT[0] == 0 && errorGateCNT[1] == 0 && errorGateCNT[2] == 0)
{
Serial.println("Result: ALL GATES CORRECT!");
}
else
{
Serial.println("Result: GATE ERRORS DETECTED - check your control logic");
}
Serial.println("===========================\n");
vTaskDelete(NULL);
return;
}
}
vTaskDelay(2000 / portTICK_PERIOD_MS);
}
}
// Product loading task - loads products at fixed intervals, checks G1 gate
void productLoadingTask(void *pvParameters)
{
TickType_t xLastWakeTime = xTaskGetTickCount();
while (true)
{
if (currentProductIndex < numProducts)
{
// Check G1 intake gate
if (g_gate1Ctrl)
{
bool schedulerFound = false;
for (int i = 0; i < MAX_SCHEDULERS; i++)
{
if (g_schedulers[i].timerHandle == nullptr)
{
g_schedulers[i].product = productArray[currentProductIndex];
g_schedulers[i].debugPin = DEBUG_PIN + currentProductIndex % 3;
hw_timer_t *t = allocateTimerForProduct(g_schedulers[i]);
if (!t)
{
Serial.println("ERROR: No hardware timer available!");
}
else
{
currentProductIndex++;
schedulerFound = true;
}
break;
}
}
if (!schedulerFound)
{
// No scheduler available this cycle, will retry next interval
}
}
else
{
// G1 is closed - product blocked at intake
// The product remains in the queue, will be loaded when G1 opens
}
}
vTaskDelayUntil(&xLastWakeTime, sp.productInterval / portTICK_PERIOD_MS);
}
}
void startSimulator(void)
{
// Generate parameters from student number
generateParameters(STUDENT_NUMBER);
printParameters();
// Generate test product array
generateProductArray();
// Serial.printf("\nGenerated %d test products:\n", numProducts);
for (int i = 0; i < numProducts; i++)
{
bool sizeOk = (productArray[i].length >= sp.L_min && productArray[i].length <= sp.L_max);
bool weightOk = (productArray[i].weight >= sp.W_min && productArray[i].weight <= sp.W_max);
const char *dest;
if (!sizeOk)
dest = "A1(size)";
else if (!weightOk)
dest = "A1(weight)";
else if (productArray[i].length <= sp.L_threshold && productArray[i].weight <= sp.W_threshold)
dest = "A2";
else
dest = "A3";
// Serial.printf(" [%d] L=%.2f W=%.2f BC=%d -> %s\n",
// i, productArray[i].length, productArray[i].weight, productArray[i].barcodeID, dest);
}
Serial.println();
// Setup GPIO pins
pinMode(g_obstructionSensorPin, OUTPUT);
pinMode(DEBUG_PIN, OUTPUT);
pinMode(DEBUG_PIN + 1, OUTPUT);
pinMode(DEBUG_PIN + 2, OUTPUT);
digitalWrite(DEBUG_PIN, LOW);
digitalWrite(DEBUG_PIN + 1, LOW);
digitalWrite(DEBUG_PIN + 2, LOW);
// Initialize sensor outputs
g_obstructionSensor = false;
g_barcodeReader = 0;
g_weightSensor = 0.0f;
// Initialize gate controls to default
g_gate1Ctrl = true;
g_gate2Ctrl = false;
g_gate3Ctrl = false;
g_gate4Ctrl = false;
// Reset counters
currentProductIndex = 0;
for (int i = 0; i < MAX_SCHEDULERS; i++)
{
g_schedulers[i].timerHandle = nullptr;
}
for (int i = 0; i < 3; i++)
{
errorGateCNT[i] = 0;
simAreaCount[i] = 0;
}
// Create product loading task (highest priority)
xTaskCreate(
productLoadingTask,
"ProductLoader",
2048,
NULL,
configMAX_PRIORITIES - 1,
NULL);
// Create status reporting task
xTaskCreate(
statusReportTask,
"StatusReport",
2048,
NULL,
1,
NULL);
Serial.println("=== SmartSort Simulator Started ===\n");
}