ESP32 S3 ESP-NOW Modul 3 motor V-1.00.10 - Wokwi ESP32, STM32, Arduino Simulator

// ==================== MODUL 3 - KRMILJENJE SENČENJA Z MOTORJEM ====================
// Avtor: SmartVitroGrov
// Opis: Krmiljenje NEMA17 motorja preko TB6600 driverja za upravljanje senčnika
// Funkcije: Dvojedrno izvajanje, linearni mehki zagon, absolutno štetje korakov
// ===================================================================================

#include <esp_now.h>
#include <WiFi.h>
#include <Preferences.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/semphr.h"
#include <esp_wifi.h>

// ==================== PIN DEFINICIJE ====================
#define STEP_PIN 18
#define DIR_PIN 19
#define ENABLE_PIN 21
#define LIMIT_SWITCH_OPEN 4
#define LIMIT_SWITCH_CLOSED 5
#define STATUS_LED 2
#define MANUAL_OPEN_BUTTON 14
#define MANUAL_CLOSE_BUTTON 15

// ==================== KONFIGURACIJA ====================
#define MODULE_ID 3
#define MODULE_TYPE 3
#define SEND_INTERVAL 5000

// ==================== NASTAVITVE HITROSTI ====================
#define SPEED_START        400      // Začetna hitrost (korakov/s)
#define SPEED_MAX          4000     // Maksimalna hitrost (korakov/s)
#define ACCEL_STEPS        600      // Število korakov za pospeševanje
#define DECEL_STEPS        600      // Število korakov za zaviranje

// ==================== NASTAVITVE ZA KALIBRACIJO ====================
#define CALIB_SPEED        400      // Hitrost med kalibracijo (korakov/s) - POČASNEJE ZA ZANESLJIVOST
#define CALIB_TIMEOUT      300000   // Časovna omejitev kalibracije (300 sekund = 5 minut)
#define CALIB_MAX_STEPS    150000   // Maksimalno število korakov

// ==================== MAC NASLOV GLAVNEGA SISTEMA ====================
uint8_t masterMAC[] = {0xB4, 0x3A, 0x45, 0xF3, 0xEB, 0xF0};

// ==================== ESP-NOW STRUKTURE ====================
enum ModuleType {
    MODULE_WEATHER = 1,
    MODULE_IRRIGATION = 2,
    MODULE_SHADE_MOTOR = 3
};

enum ErrorCode {
    ERR_NONE = 0,
    ERR_MOTOR_STALL = 10,
    ERR_LIMIT_SWITCH = 11,
    ERR_CALIBRATION = 12,
    ERR_OVER_CURRENT = 13
};

struct ModuleData {
    uint8_t moduleId;
    ModuleType moduleType;
    unsigned long timestamp;
    ErrorCode errorCode;
    float batteryVoltage;
    
    union {
        struct {
            float temperature;
            float humidity;
            float pressure;
            float lux;
            float bmpTemperature;
        } weather;
        
        struct {
            float flowRate1, flowRate2, flowRate3;
            float totalFlow1, totalFlow2, totalFlow3;
            bool relay1State, relay2State, relay3State;
        } irrigation;
        
        struct {
            int currentPosition;
            int targetPosition;
            bool isMoving;
            bool isCalibrated;
            bool limitSwitchOpen;
            bool limitSwitchClosed;
            float motorCurrent;
            int motorSpeed;
        } shade;
    };
};

struct CommandData {
    uint8_t targetModuleId;
    uint8_t command;
    float param1;
    float param2;
};

enum ShadeCommands {
    CMD_MOVE_TO_POSITION = 1,
    CMD_MOVE_STEP = 2,
    CMD_STOP = 3,
    CMD_CALIBRATE = 4,
    CMD_SET_SPEED = 5,
    CMD_SET_AUTO_MODE = 6,
    CMD_EMERGENCY_STOP = 7
};

// ==================== GLOBALNE SPREMENLJIVKE ====================
SemaphoreHandle_t motorMutex = NULL;
volatile bool calibrationRunning = false;

// Nastavitev smeri - če motor teče v napačno smer, spremenite to vrednost na true
bool reverseDirection = false;  // Spremenite na true, če motor teče v napačno smer

struct MotorState {
    int command;
    int targetPosition;
    int speedCmd;
    bool commandPending;
    bool isMoving;
    int currentPos;
    int targetPos;
    int totalSteps;
    int absoluteSteps;
    bool calibrated;
    bool calibrating;
};

MotorState motorState = {
    .command = 0,
    .targetPosition = 50,
    .speedCmd = SPEED_MAX,
    .commandPending = false,
    .isMoving = false,
    .currentPos = 50,
    .targetPos = 50,
    .totalSteps = 32027,
    .absoluteSteps = 16013,
    .calibrated = true,
    .calibrating = false
};

ModuleData sendData;
int currentMotorSpeed = SPEED_MAX;
unsigned long lastRealtimeSend = 0;
int lastSentPercent = -1;

volatile bool limitOpenPressed = false;
volatile bool limitClosedPressed = false;
unsigned long lastLimitOpenTime = 0;
unsigned long lastLimitClosedTime = 0;
#define DEBOUNCE_TIME 50

Preferences preferences;

// ==================== PROTOTIPI ====================
void initMotor();
void enableMotor(bool enable);
void setDirection(bool closeDirection);
void doStep();
void motorTask(void *pvParameters);
void communicationTask(void *pvParameters);
bool isOpenSwitchPressed();
bool isClosedSwitchPressed();
void sendStatus();
void saveCalibration();
void loadCalibration();
bool autoCalibrate();
void onReceiveCommand(const esp_now_recv_info_t *recv_info, const uint8_t *incomingData, int len);
void testLimitSwitches();
void testMotorDirection();

// ==================== INTERRUPTI ====================
void IRAM_ATTR limitOpenISR() {
    unsigned long now = millis();
    if (now - lastLimitOpenTime > DEBOUNCE_TIME) {
        lastLimitOpenTime = now;
        limitOpenPressed = true;
    }
}

void IRAM_ATTR limitClosedISR() {
    unsigned long now = millis();
    if (now - lastLimitClosedTime > DEBOUNCE_TIME) {
        lastLimitClosedTime = now;
        limitClosedPressed = true;
    }
}

// ==================== FUNKCIJE ZA MOTOR ====================
void initMotor() {
    pinMode(STEP_PIN, OUTPUT);
    pinMode(DIR_PIN, OUTPUT);
    pinMode(ENABLE_PIN, OUTPUT);
    digitalWrite(ENABLE_PIN, HIGH);
    
    pinMode(LIMIT_SWITCH_OPEN, INPUT_PULLUP);
    pinMode(LIMIT_SWITCH_CLOSED, INPUT_PULLUP);
    
    attachInterrupt(digitalPinToInterrupt(LIMIT_SWITCH_OPEN), limitOpenISR, RISING);
    attachInterrupt(digitalPinToInterrupt(LIMIT_SWITCH_CLOSED), limitClosedISR, RISING);
    
    pinMode(MANUAL_OPEN_BUTTON, INPUT_PULLUP);
    pinMode(MANUAL_CLOSE_BUTTON, INPUT_PULLUP);
    
    motorMutex = xSemaphoreCreateMutex();
    
    Serial.printf("  Začetna hitrost: %d korakov/s\n", SPEED_START);
    Serial.printf("  Maksimalna hitrost: %d korakov/s\n", SPEED_MAX);
    Serial.printf("  Pospeševanje: %d korakov\n", ACCEL_STEPS);
    Serial.printf("  Zaviranje: %d korakov\n", DECEL_STEPS);
    Serial.println("  Način: Absolutno štetje korakov");
}

void enableMotor(bool enable) {
    digitalWrite(ENABLE_PIN, enable ? LOW : HIGH);
    delayMicroseconds(100);
}

void setDirection(bool closeDirection) {
    bool actualDirection = reverseDirection ? !closeDirection : closeDirection;
    digitalWrite(DIR_PIN, actualDirection ? HIGH : LOW);
    delayMicroseconds(100);
}

void doStep() {
    digitalWrite(STEP_PIN, HIGH);
    delayMicroseconds(5);
    digitalWrite(STEP_PIN, LOW);
    delayMicroseconds(5);
}

bool isOpenSwitchPressed() {
    return digitalRead(LIMIT_SWITCH_OPEN) == HIGH;
}

bool isClosedSwitchPressed() {
    return digitalRead(LIMIT_SWITCH_CLOSED) == HIGH;
}

// ==================== TEST SMERI MOTORJA ====================
void testMotorDirection() {
    Serial.println("\n=== TEST SMERI MOTORJA ===");
    Serial.println("Motor se bo kratek čas vrtel v obe smeri.");
    Serial.println("Preverite, v katero smer se vrti gred motorja.");
    
    enableMotor(true);
    
    Serial.println("\n1. Test ODPIRANJE (proti 0%):");
    setDirection(false);
    for(int i = 0; i < 400; i++) {
        doStep();
        delayMicroseconds(2000);
    }
    delay(500);
    
    Serial.println("\n2. Test ZAPIRANJE (proti 100%):");
    setDirection(true);
    for(int i = 0; i < 400; i++) {
        doStep();
        delayMicroseconds(2000);
    }
    delay(500);
    
    enableMotor(false);
    
    Serial.println("\n✅ Test končan!");
    Serial.println("   Če se je motor vrtel v napačno smer glede na ukaz,");
    Serial.println("   spremenite spremenljivko 'reverseDirection' na true");
    Serial.println("   v kodi in ponovno naložite.\n");
}

// ==================== TEST LIMITNIH STIKAL ====================
void testLimitSwitches() {
    Serial.println("\n=== TEST LIMITNIH STIKAL ===");
    
    bool openPressed = isOpenSwitchPressed();
    bool closedPressed = isClosedSwitchPressed();
    
    Serial.printf("ODPRTO stikalo (pin 4): %s\n", openPressed ? "PRITISNJENO" : "NI PRITISNJENO");
    Serial.printf("ZAPRTO stikalo (pin 5): %s\n", closedPressed ? "PRITISNJENO" : "NI PRITISNJENO");
    
    if (!openPressed && !closedPressed) {
        Serial.println("\n⚠️ Nobeno stikalo ni pritisnjeno!");
        Serial.println("   Preverite fizične povezave:");
        Serial.println("   - ODPRTO stikalo: pin 4 -> GND ob pritisku");
        Serial.println("   - ZAPRTO stikalo: pin 5 -> GND ob pritisku");
        Serial.println("\n   Ročno pritisnite stikalo in opazujte spremembo.");
    } else if (openPressed && closedPressed) {
        Serial.println("\n⚠️ Obe stikali sta pritisnjeni hkrati!");
        Serial.println("   To ni normalno stanje. Preverite povezave.");
    } else if (openPressed) {
        Serial.println("\n✅ ODPRTO stikalo deluje pravilno!");
    } else if (closedPressed) {
        Serial.println("\n✅ ZAPRTO stikalo deluje pravilno!");
    }
    
    Serial.println("===========================\n");
}

// ==================== KALIBRACIJA ====================
void saveCalibration() {
    preferences.begin("motor-calib", false);
    if (xSemaphoreTake(motorMutex, portMAX_DELAY)) {
        preferences.putInt("totalSteps", motorState.totalSteps);
        preferences.putBool("calibrated", motorState.calibrated);
        preferences.putInt("position", motorState.currentPos);
        preferences.putInt("absoluteSteps", motorState.absoluteSteps);
        xSemaphoreGive(motorMutex);
    }
    preferences.end();
    Serial.printf("💾 Kalibracija shranjena\n");
}

void loadCalibration() {
    preferences.begin("motor-calib", true);
    
    if (preferences.isKey("totalSteps")) {
        if (xSemaphoreTake(motorMutex, portMAX_DELAY)) {
            motorState.totalSteps = preferences.getInt("totalSteps", 32027);
            motorState.calibrated = preferences.getBool("calibrated", true);
            motorState.currentPos = preferences.getInt("position", 50);
            motorState.absoluteSteps = preferences.getInt("absoluteSteps", motorState.totalSteps / 2);
            motorState.targetPos = motorState.currentPos;
            xSemaphoreGive(motorMutex);
        }
        Serial.printf("📀 Kalibracija naložena: korakov=%d, pozicija=%d%%, absolutni koraki=%d\n", 
                      motorState.totalSteps, motorState.currentPos, motorState.absoluteSteps);
    } else {
        Serial.println("⚠ Kalibracija ni najdena");
        if (xSemaphoreTake(motorMutex, portMAX_DELAY)) {
            motorState.totalSteps = 32027;
            motorState.calibrated = false;
            motorState.currentPos = 50;
            motorState.absoluteSteps = 16013;
            xSemaphoreGive(motorMutex);
        }
    }
    preferences.end();
}

// ==================== IZBOLJŠANA KALIBRACIJA ====================
bool autoCalibrate() {
    Serial.println("\n╔══════════════════════════════════════════════════════════════╗");
    Serial.println("║              AVTOMATSKA KALIBRACIJA SENČNIKA                 ║");
    Serial.println("╚══════════════════════════════════════════════════════════════╝");
    Serial.println("⚠️ POMEMBNO: Kalibracija lahko traja do 5 minut!");
    Serial.printf("   Motor se bo premikal s hitrostjo %d korakov/s.\n", CALIB_SPEED);
    Serial.printf("   Smer vrtenja: %s\n", reverseDirection ? "OBRNJENA" : "NORMALNA");
    
    calibrationRunning = true;
    
    if (xSemaphoreTake(motorMutex, portMAX_DELAY)) {
        motorState.calibrating = true;
        xSemaphoreGive(motorMutex);
    }
    
    // Preveri začetno stanje stikal
    bool openPressed = isOpenSwitchPressed();
    bool closedPressed = isClosedSwitchPressed();
    
    Serial.printf("\n📌 Začetno stanje - ODPRTO: %s, ZAPRTO: %s\n", 
                  openPressed ? "PRITISNJENO" : "NI PRITISNJENO", 
                  closedPressed ? "PRITISNJENO" : "NI PRITISNJENO");
    
    // Če je že na stikalu, ni potrebna kalibracija
    if(openPressed) {
        if (xSemaphoreTake(motorMutex, portMAX_DELAY)) {
            motorState.currentPos = 0;
            motorState.absoluteSteps = 0;
            motorState.calibrated = true;
            motorState.calibrating = false;
            xSemaphoreGive(motorMutex);
        }
        saveCalibration();
        Serial.println("✅ Motor že na ODPRTO stikalu - kalibracija ni potrebna!");
        calibrationRunning = false;
        return true;
    }
    if(closedPressed) {
        if (xSemaphoreTake(motorMutex, portMAX_DELAY)) {
            motorState.currentPos = 100;
            motorState.absoluteSteps = motorState.totalSteps;
            motorState.calibrated = true;
            motorState.calibrating = false;
            xSemaphoreGive(motorMutex);
        }
        saveCalibration();
        Serial.println("✅ Motor že na ZAPRTO stikalu - kalibracija ni potrebna!");
        calibrationRunning = false;
        return true;
    }
    
    // ===== 1. Najprej se premakni proti ODPRTO stikalu =====
    Serial.println("\n📍 KORAK 1/3: Iskanje ODPRTO stikala...");
    enableMotor(true);
    setDirection(false);  // false = ODPIRANJE (proti 0%)
    
    limitOpenPressed = false;
    unsigned long startTime = millis();
    int stepsToOpen = 0;
    int calibSpeed = CALIB_SPEED;
    unsigned long lastStepTime = micros();
    int currentCalibDelay = 1000000 / calibSpeed;
    int lastReportStep = 0;
    
    Serial.print("   Premikanje proti ODPRTO (0%)");
    
    while (!limitOpenPressed && stepsToOpen < CALIB_MAX_STEPS && (millis() - startTime < CALIB_TIMEOUT)) {
        yield();
        
        unsigned long now = micros();
        
        if (now - lastStepTime >= currentCalibDelay) {
            doStep();
            lastStepTime = now;
            stepsToOpen++;
            
            if (stepsToOpen - lastReportStep >= 500) {
                lastReportStep = stepsToOpen;
                Serial.printf(".");
                if (stepsToOpen % 5000 == 0) {
                    Serial.printf(" %d korakov", stepsToOpen);
                }
            }
            
            if (isOpenSwitchPressed()) {
                limitOpenPressed = true;
                Serial.printf("\n✅ ODPRTO stikalo doseženo po %d korakih!\n", stepsToOpen);
                break;
            }
        }
        delayMicroseconds(50);
    }
    
    if (!limitOpenPressed) {
        Serial.printf("\n⚠️ ODPRTO stikalo NI doseženo! Opravljenih korakov: %d\n", stepsToOpen);
        Serial.println("   Motor se bo obrnil in poiskal ZAPRTO stikalo...");
        
        // Če ni našel ODPRTO stikala, se obrni in pojdi proti ZAPRTO
        setDirection(true);
        lastReportStep = stepsToOpen;
        
        while (!limitClosedPressed && stepsToOpen < CALIB_MAX_STEPS * 2 && (millis() - startTime < CALIB_TIMEOUT)) {
            yield();
            
            unsigned long now = micros();
            
            if (now - lastStepTime >= currentCalibDelay) {
                doStep();
                lastStepTime = now;
                stepsToOpen++;
                
                if (stepsToOpen - lastReportStep >= 500) {
                    lastReportStep = stepsToOpen;
                    Serial.printf(".");
                    if (stepsToOpen % 5000 == 0) {
                        Serial.printf(" %d korakov", stepsToOpen);
                    }
                }
                
                if (isClosedSwitchPressed()) {
                    limitClosedPressed = true;
                    Serial.printf("\n✅ ZAPRTO stikalo doseženo po %d korakih!\n", stepsToOpen);
                    break;
                }
            }
            delayMicroseconds(50);
        }
        
        if (!limitClosedPressed) {
            Serial.println("\n❌ NAPAKA: Nobeno stikalo ni bilo doseženo!");
            Serial.println("   Preverite fizične povezave in ali se motor vrti.");
            enableMotor(false);
            if (xSemaphoreTake(motorMutex, portMAX_DELAY)) {
                motorState.calibrating = false;
                xSemaphoreGive(motorMutex);
            }
            calibrationRunning = false;
            return false;
        }
        
        // Če smo našli ZAPRTO stikalo, je to zdaj 100%
        if (xSemaphoreTake(motorMutex, portMAX_DELAY)) {
            motorState.currentPos = 100;
            motorState.absoluteSteps = stepsToOpen;
            motorState.totalSteps = stepsToOpen;
            xSemaphoreGive(motorMutex);
        }
        
        // Vrni se na 50%
        Serial.println("\n📍 Vračanje na 50% pozicijo...");
        setDirection(false);
        
        int stepsToMiddle = stepsToOpen / 2;
        int stepsDone = 0;
        lastStepTime = micros();
        
        Serial.printf("   Vračanje na sredino: %d korakov\n", stepsToMiddle);
        Serial.print("   Premikanje");
        
        for (int i = 0; i < stepsToMiddle; i++) {
            if (i % 100 == 0) yield();
            
            unsigned long now = micros();
            
            if (now - lastStepTime >= currentCalibDelay) {
                doStep();
                lastStepTime = now;
                stepsDone++;
                
                if (stepsDone % 1000 == 0) {
                    Serial.printf(".");
                    if (stepsDone % 5000 == 0) {
                        Serial.printf(" %d/%d", stepsDone, stepsToMiddle);
                    }
                }
            }
            delayMicroseconds(50);
        }
        
        Serial.printf("\n✅ Vračanje končano!\n");
        
    } else {
        // Našli smo ODPRTO stikalo - zabeleži pozicijo
        if (xSemaphoreTake(motorMutex, portMAX_DELAY)) {
            motorState.currentPos = 0;
            motorState.absoluteSteps = 0;
            xSemaphoreGive(motorMutex);
        }
        delay(500);
        yield();
        
        // ===== 2. Poišči ZAPRTO stikalo =====
        Serial.println("\n📍 KORAK 2/3: Iskanje ZAPRTO stikala...");
        setDirection(true);
        limitClosedPressed = false;
        
        int stepsToClosed = 0;
        startTime = millis();
        lastStepTime = micros();
        lastReportStep = 0;
        
        Serial.print("   Premikanje proti ZAPRTO (100%)");
        
        while (!limitClosedPressed && stepsToClosed < CALIB_MAX_STEPS && (millis() - startTime < CALIB_TIMEOUT)) {
            yield();
            
            unsigned long now = micros();
            
            if (now - lastStepTime >= currentCalibDelay) {
                doStep();
                lastStepTime = now;
                stepsToClosed++;
                
                if (stepsToClosed - lastReportStep >= 500) {
                    lastReportStep = stepsToClosed;
                    Serial.printf(".");
                    if (stepsToClosed % 5000 == 0) {
                        Serial.printf(" %d korakov", stepsToClosed);
                    }
                }
                
                if (isClosedSwitchPressed()) {
                    limitClosedPressed = true;
                    Serial.printf("\n✅ ZAPRTO stikalo doseženo po %d korakih!\n", stepsToClosed);
                    break;
                }
            }
            delayMicroseconds(50);
        }
        
        if (!limitClosedPressed) {
            Serial.println("\n❌ NAPAKA: ZAPRTO stikalo ni najdeno!");
            enableMotor(false);
            if (xSemaphoreTake(motorMutex, portMAX_DELAY)) {
                motorState.calibrating = false;
                xSemaphoreGive(motorMutex);
            }
            calibrationRunning = false;
            return false;
        }
        
        // Zabeleži skupno število korakov
        if (xSemaphoreTake(motorMutex, portMAX_DELAY)) {
            motorState.totalSteps = stepsToClosed;
            motorState.absoluteSteps = stepsToClosed;
            xSemaphoreGive(motorMutex);
        }
        delay(500);
        yield();
        
        // ===== 3. Vrni se na 50% =====
        Serial.println("\n📍 KORAK 3/3: Vračanje na 50% pozicijo...");
        setDirection(false);
        
        int stepsToMiddle = stepsToClosed / 2;
        int stepsDone = 0;
        lastStepTime = micros();
        
        Serial.printf("   Vračanje na sredino: %d korakov\n", stepsToMiddle);
        Serial.print("   Premikanje");
        
        for (int i = 0; i < stepsToMiddle; i++) {
            if (i % 100 == 0) yield();
            
            unsigned long now = micros();
            
            if (now - lastStepTime >= currentCalibDelay) {
                doStep();
                lastStepTime = now;
                stepsDone++;
                
                if (stepsDone % 1000 == 0) {
                    Serial.printf(".");
                    if (stepsDone % 5000 == 0) {
                        Serial.printf(" %d/%d", stepsDone, stepsToMiddle);
                    }
                }
            }
            delayMicroseconds(50);
        }
        
        Serial.printf("\n✅ Vračanje končano! Opravljenih korakov: %d\n", stepsDone);
    }
    
    enableMotor(true);
    
    if (xSemaphoreTake(motorMutex, portMAX_DELAY)) {
        motorState.calibrated = true;
        motorState.currentPos = 50;
        motorState.targetPos = 50;
        motorState.absoluteSteps = motorState.totalSteps / 2;
        motorState.calibrating = false;
        xSemaphoreGive(motorMutex);
    }
    saveCalibration();
    
    Serial.println("\n╔══════════════════════════════════════════════════════════════╗");
    Serial.println("║              KALIBRACIJA USPEŠNO ZAKLJUČENA!                ║");
    Serial.println("╚══════════════════════════════════════════════════════════════╝");
    Serial.printf("  📊 Skupaj korakov med stikaloma: %d\n", motorState.totalSteps);
    Serial.printf("  📍 Sredinska pozicija: %d korakov\n", motorState.totalSteps / 2);
    Serial.printf("  🎯 Trenutna pozicija: 50%%\n");
    Serial.printf("  ⚡ Hitrost kalibracije: %d korakov/s\n", CALIB_SPEED);
    
    calibrationRunning = false;
    return true;
}

// ==================== POŠILJANJE STATUSA ====================
void sendStatus() {
    if (!esp_now_is_peer_exist(masterMAC)) return;
    
    int currentPos = 0, targetPos = 0;
    bool isMoving = false, calibrated = false;
    
    if (xSemaphoreTake(motorMutex, portMAX_DELAY)) {
        currentPos = motorState.currentPos;
        targetPos = motorState.targetPos;
        isMoving = motorState.isMoving;
        calibrated = motorState.calibrated;
        xSemaphoreGive(motorMutex);
    }
    
    sendData.moduleId = MODULE_ID;
    sendData.moduleType = MODULE_SHADE_MOTOR;
    sendData.timestamp = millis();
    sendData.errorCode = ERR_NONE;
    sendData.batteryVoltage = 0;
    sendData.shade.currentPosition = currentPos;
    sendData.shade.targetPosition = targetPos;
    sendData.shade.isMoving = isMoving;
    sendData.shade.isCalibrated = calibrated;
    sendData.shade.limitSwitchOpen = isOpenSwitchPressed();
    sendData.shade.limitSwitchClosed = isClosedSwitchPressed();
    sendData.shade.motorSpeed = currentMotorSpeed;
    
    esp_err_t result = esp_now_send(masterMAC, (uint8_t *)&sendData, sizeof(sendData));
    if (result == ESP_OK) {
        // Uspešno poslano
    }
}

// ==================== FUNKCIJA ZA PREJEM UKAZOV ====================
void onReceiveCommand(const esp_now_recv_info_t *recv_info, const uint8_t *incomingData, int len) {
    if (len != sizeof(CommandData)) return;
    
    const uint8_t* mac_addr = recv_info->src_addr;
    for(int i = 0; i < 6; i++) {
        if(mac_addr[i] != masterMAC[i]) return;
    }
    
    CommandData cmd;
    memcpy(&cmd, incomingData, sizeof(cmd));
    
    if (cmd.targetModuleId != MODULE_ID) return;
    
    Serial.printf("\n📡 PREJET UKAZ: cmd=%d, p1=%.1f, p2=%.1f\n", cmd.command, cmd.param1, cmd.param2);
    
    if (xSemaphoreTake(motorMutex, portMAX_DELAY)) {
        switch(cmd.command) {
            case CMD_MOVE_TO_POSITION:
                motorState.targetPosition = (int)cmd.param1;
                motorState.command = 1;
                motorState.commandPending = true;
                Serial.printf("  ✅ Premik na %d%%\n", motorState.targetPosition);
                break;
            case CMD_MOVE_STEP:
                {
                    int step = (int)cmd.param1;
                    int newTarget = constrain(motorState.currentPos + step, 0, 100);
                    motorState.targetPosition = newTarget;
                    motorState.command = 1;
                    motorState.commandPending = true;
                    Serial.printf("  ✅ Premik za %d%% na %d%%\n", step, newTarget);
                }
                break;
            case CMD_STOP:
                motorState.command = 2;
                motorState.commandPending = true;
                Serial.println("  ✅ Zaustavitev");
                break;
            case CMD_EMERGENCY_STOP:
                motorState.command = 2;
                motorState.commandPending = true;
                enableMotor(false);
                Serial.println("  🚨 NUJNA ZAUSTAVITEV");
                break;
            case CMD_CALIBRATE:
                if (!calibrationRunning) {
                    Serial.println("  🔧 Začenjam kalibracijo...");
                    xSemaphoreGive(motorMutex);
                    autoCalibrate();
                    xSemaphoreTake(motorMutex, portMAX_DELAY);
                } else {
                    Serial.println("  ⏳ Kalibracija že poteka!");
                }
                break;
            case CMD_SET_SPEED:
                motorState.speedCmd = (int)cmd.param1;
                motorState.command = 4;
                motorState.commandPending = true;
                Serial.printf("  ⚡ Hitrost %d\n", motorState.speedCmd);
                break;
            default:
                Serial.printf("  ❌ Neznan ukaz: %d\n", cmd.command);
                break;
        }
        xSemaphoreGive(motorMutex);
        sendStatus();
    }
}

// ==================== TASK ZA MOTOR ====================
void motorTask(void *pvParameters) {
    Serial.println("🔧 Naloga motorja zagnana na jedru 1");
    Serial.println("   Način: Tekoče premikanje na ciljno pozicijo");
    
    bool isMoving = false;
    bool movingDirection = false;
    int targetPos = 50;
    int totalSteps = 32027;
    int stepsToMove = 0;
    int stepsDone = 0;
    int absoluteSteps = 16013;
    int currentSpeed = SPEED_START;
    unsigned long stepDelay = 1000000 / currentSpeed;
    unsigned long lastStepMicros = 0;
    int lastPrintedPos = -1;
    
    while(1) {
        if (xSemaphoreTake(motorMutex, portMAX_DELAY)) {
            if (motorState.commandPending) {
                int cmd = motorState.command;
                int newTarget = motorState.targetPosition;
                int newSpeed = motorState.speedCmd;
                int currentTotalSteps = motorState.totalSteps;
                int currentAbsoluteSteps = motorState.absoluteSteps;
                motorState.commandPending = false;
                xSemaphoreGive(motorMutex);
                
                switch(cmd) {
                    case 1:
                        if (!motorState.calibrating && motorState.calibrated) {
                            totalSteps = currentTotalSteps;
                            absoluteSteps = currentAbsoluteSteps;
                            int currentPos = (absoluteSteps * 100) / totalSteps;
                            currentPos = constrain(currentPos, 0, 100);
                            newTarget = constrain(newTarget, 0, 100);
                            
                            if (newTarget != currentPos) {
                                movingDirection = (newTarget > currentPos);
                                int targetSteps = (newTarget * totalSteps) / 100;
                                stepsToMove = abs(targetSteps - absoluteSteps);
                                stepsDone = 0;
                                isMoving = true;
                                targetPos = newTarget;
                                currentSpeed = SPEED_START;
                                stepDelay = 1000000 / currentSpeed;
                                lastPrintedPos = -1;
                                
                                if (xSemaphoreTake(motorMutex, portMAX_DELAY)) {
                                    motorState.isMoving = true;
                                    motorState.targetPos = targetPos;
                                    xSemaphoreGive(motorMutex);
                                }
                                
                                enableMotor(true);
                                setDirection(movingDirection);
                                Serial.printf("🎯 Tekoči premik na %d%% (razdalja: %d korakov, smer: %s)\n", 
                                              targetPos, stepsToMove, 
                                              movingDirection ? "ZAPIRANJE" : "ODPIRANJE");
                            }
                        }
                        break;
                    case 2:
                        isMoving = false;
                        if (xSemaphoreTake(motorMutex, portMAX_DELAY)) {
                            motorState.isMoving = false;
                            xSemaphoreGive(motorMutex);
                        }
                        break;
                    case 4:
                        currentMotorSpeed = constrain(newSpeed, SPEED_START, SPEED_MAX);
                        break;
                }
            } else {
                xSemaphoreGive(motorMutex);
            }
        }
        
        if (isMoving && stepsDone < stepsToMove) {
            unsigned long now = micros();
            int stepsLeft = stepsToMove - stepsDone;
            
            if (stepsDone < ACCEL_STEPS) {
                float progress = (float)stepsDone / ACCEL_STEPS;
                currentSpeed = SPEED_START + (currentMotorSpeed - SPEED_START) * progress;
                stepDelay = 1000000 / currentSpeed;
            } else if (stepsLeft < DECEL_STEPS) {
                float progress = (float)stepsLeft / DECEL_STEPS;
                currentSpeed = SPEED_START + (currentMotorSpeed - SPEED_START) * progress;
                stepDelay = 1000000 / currentSpeed;
            } else {
                currentSpeed = currentMotorSpeed;
                stepDelay = 1000000 / currentSpeed;
            }
            
            if (now - lastStepMicros >= stepDelay) {
                doStep();
                lastStepMicros = now;
                stepsDone++;
                
                if (movingDirection) absoluteSteps++;
                else absoluteSteps--;
                absoluteSteps = constrain(absoluteSteps, 0, totalSteps);
                
                int currentPos = (absoluteSteps * 100) / totalSteps;
                currentPos = constrain(currentPos, 0, 100);
                
                if (xSemaphoreTake(motorMutex, portMAX_DELAY)) {
                    motorState.absoluteSteps = absoluteSteps;
                    motorState.currentPos = currentPos;
                    xSemaphoreGive(motorMutex);
                }
                
                if (currentPos != lastPrintedPos && (currentPos % 5 == 0 || stepsDone == stepsToMove)) {
                    lastPrintedPos = currentPos;
                    Serial.printf("  Pozicija: %d%% (hitrost: %d korakov/s)\n", currentPos, currentSpeed);
                }
                
                if (movingDirection && isClosedSwitchPressed()) {
                    isMoving = false;
                    absoluteSteps = totalSteps;
                    if (xSemaphoreTake(motorMutex, portMAX_DELAY)) {
                        motorState.isMoving = false;
                        motorState.absoluteSteps = totalSteps;
                        motorState.currentPos = 100;
                        xSemaphoreGive(motorMutex);
                    }
                    Serial.println("⚠ ZAPRTO stikalo doseženo - ustavljam");
                    break;
                }
                if (!movingDirection && isOpenSwitchPressed()) {
                    isMoving = false;
                    absoluteSteps = 0;
                    if (xSemaphoreTake(motorMutex, portMAX_DELAY)) {
                        motorState.isMoving = false;
                        motorState.absoluteSteps = 0;
                        motorState.currentPos = 0;
                        xSemaphoreGive(motorMutex);
                    }
                    Serial.println("⚠ ODPRTO stikalo doseženo - ustavljam");
                    break;
                }
                
                if (stepsDone >= stepsToMove) {
                    isMoving = false;
                    if (xSemaphoreTake(motorMutex, portMAX_DELAY)) {
                        motorState.isMoving = false;
                        motorState.currentPos = targetPos;
                        xSemaphoreGive(motorMutex);
                    }
                    Serial.printf("✅ Ciljna pozicija %d%% dosežena!\n", targetPos);
                }
            }
        }
        
        bool openPressed = (digitalRead(MANUAL_OPEN_BUTTON) == LOW);
        bool closePressed = (digitalRead(MANUAL_CLOSE_BUTTON) == LOW);
        
        if (openPressed && !isMoving && !motorState.calibrating && motorState.calibrated) {
            if (xSemaphoreTake(motorMutex, portMAX_DELAY)) {
                totalSteps = motorState.totalSteps;
                absoluteSteps = motorState.absoluteSteps;
                xSemaphoreGive(motorMutex);
            }
            int currentPos = (absoluteSteps * 100) / totalSteps;
            int newPos = max(0, currentPos - 10);
            if (newPos != currentPos) {
                int targetSteps = (newPos * totalSteps) / 100;
                stepsToMove = abs(targetSteps - absoluteSteps);
                stepsDone = 0;
                isMoving = true;
                targetPos = newPos;
                movingDirection = false;
                currentSpeed = SPEED_START;
                stepDelay = 1000000 / currentSpeed;
                lastPrintedPos = -1;
                enableMotor(true);
                setDirection(false);
                
                if (xSemaphoreTake(motorMutex, portMAX_DELAY)) {
                    motorState.isMoving = true;
                    motorState.targetPos = targetPos;
                    xSemaphoreGive(motorMutex);
                }
                Serial.printf("🔘 Ročni ODPRI na %d%%\n", targetPos);
            }
            delay(300);
        }
        
        if (closePressed && !isMoving && !motorState.calibrating && motorState.calibrated) {
            if (xSemaphoreTake(motorMutex, portMAX_DELAY)) {
                totalSteps = motorState.totalSteps;
                absoluteSteps = motorState.absoluteSteps;
                xSemaphoreGive(motorMutex);
            }
            int currentPos = (absoluteSteps * 100) / totalSteps;
            int newPos = min(100, currentPos + 10);
            if (newPos != currentPos) {
                int targetSteps = (newPos * totalSteps) / 100;
                stepsToMove = abs(targetSteps - absoluteSteps);
                stepsDone = 0;
                isMoving = true;
                targetPos = newPos;
                movingDirection = true;
                currentSpeed = SPEED_START;
                stepDelay = 1000000 / currentSpeed;
                lastPrintedPos = -1;
                enableMotor(true);
                setDirection(true);
                
                if (xSemaphoreTake(motorMutex, portMAX_DELAY)) {
                    motorState.isMoving = true;
                    motorState.targetPos = targetPos;
                    xSemaphoreGive(motorMutex);
                }
                Serial.printf("🔘 Ročni ZAPRI na %d%%\n", targetPos);
            }
            delay(300);
        }
        
        delay(1);
    }
}

// ==================== TASK ZA KOMUNIKACIJO ====================
void communicationTask(void *pvParameters) {
    Serial.println("📡 Naloga komunikacije zagnana na jedru 0");
    
    // ===== POMEMBNO: PRAVILNA INICIALIZACIJA WIFI =====
    WiFi.disconnect(true);
    delay(100);
    
    WiFi.mode(WIFI_STA);
    delay(100);
    
    // Počakaj, da se WiFi pravilno inicializira
    String macAddress = WiFi.macAddress();
    int retryCount = 0;
    while (macAddress == "00:00:00:00:00:00" && retryCount < 5) {
        delay(500);
        WiFi.mode(WIFI_AP_STA);
        delay(100);
        WiFi.mode(WIFI_STA);
        delay(500);
        macAddress = WiFi.macAddress();
        retryCount++;
        Serial.printf("  Poskus %d: MAC naslov: %s\n", retryCount, macAddress.c_str());
    }
    
    Serial.print("📡 DEJANSKI MAC naslov MODULA 3: ");
    Serial.println(WiFi.macAddress());
    
    // ===== NASTAVI KANAL NA 1 =====
    esp_wifi_set_promiscuous(true);
    esp_wifi_set_channel(1, WIFI_SECOND_CHAN_NONE);
    esp_wifi_set_promiscuous(false);
    
    Serial.println("📡 Kanal nastavljen na 1");
    Serial.printf("📡 MODUL 3 - Kanal: %d\n", WiFi.channel());
    
    delay(100);
    
    // ===== INICIALIZACIJA ESP-NOW =====
    esp_err_t initResult = esp_now_init();
    if (initResult != ESP_OK) {
        Serial.printf("⚠️ ESP-NOW init napaka: %d, poskus ponovno...\n", initResult);
        WiFi.mode(WIFI_OFF);
        delay(500);
        WiFi.mode(WIFI_STA);
        delay(500);
        initResult = esp_now_init();
        if (initResult != ESP_OK) {
            Serial.printf("❌ ESP-NOW init končno neuspešen: %d\n", initResult);
            while(1) delay(100);
        }
    }
    Serial.println("✅ ESP-NOW inicializiran");
    
    esp_now_register_recv_cb(onReceiveCommand);
    Serial.println("✅ Callback registriran");
    
    // Odstrani obstoječega peer-ja, če obstaja
    if (esp_now_is_peer_exist(masterMAC)) {
        esp_now_del_peer(masterMAC);
        delay(100);
    }
    
    // Dodaj peer
    esp_now_peer_info_t peerInfo;
    memset(&peerInfo, 0, sizeof(peerInfo));
    memcpy(peerInfo.peer_addr, masterMAC, 6);
    peerInfo.channel = 1;   // KANAL 1 - ENAKO KOT GLAVNI SISTEM
    peerInfo.encrypt = false;
    peerInfo.ifidx = WIFI_IF_STA;
    
    if (esp_now_add_peer(&peerInfo) == ESP_OK) {
        Serial.println("✅ Master peer dodan (kanal 1)");
    } else {
        Serial.println("❌ Napaka pri dodajanju master peer-ja!");
    }
    
    Serial.println("\n✅ Modul 3 pripravljen!\n");
    Serial.println("📌 Za testiranje uporabite ročna gumba na modulu 3:");
    Serial.println("   - Gumb ODPRI (pin 14) - premik proti 0%");
    Serial.println("   - Gumb ZAPRI (pin 15) - premik proti 100%");
    
    unsigned long lastStatusSend = 0;
    unsigned long lastDebugPrint = 0;
    
    while(1) {
        unsigned long now = millis();
        
        if (now - lastDebugPrint > 30000) {
            Serial.println("🔁 Čakam na ukaze...");
            lastDebugPrint = now;
        }
        
        if (now - lastStatusSend >= SEND_INTERVAL) {
            sendStatus();
            lastStatusSend = now;
        }
        
        delay(10);
    }
}

// ==================== SETUP ====================
void setup() {
    Serial.begin(115200);
    delay(1000);
    
    Serial.println("\n\n╔════════════════════════════════════════════════════╗");
    Serial.println("║     MODUL 3 - KRMILJENJE SENČENJA                ║");
    Serial.println("╚════════════════════════════════════════════════════╝");
    
    pinMode(STATUS_LED, OUTPUT);
    digitalWrite(STATUS_LED, LOW);
    
    initMotor();
    loadCalibration();
    
    // ===== NAJPREJ PRAVILNO INICIALIZIRAJ WIFI =====
    Serial.println("\n--- Inicializacija WiFi ---");
    
    WiFi.disconnect(true);
    delay(100);
    
    WiFi.mode(WIFI_STA);
    delay(100);
    
    // Počakaj, da se WiFi pravilno inicializira
    String macAddress = WiFi.macAddress();
    int retryCount = 0;
    while (macAddress == "00:00:00:00:00:00" && retryCount < 5) {
        Serial.printf("⚠️ Neveljaven MAC naslov (%s), poskus %d/5...\n", macAddress.c_str(), retryCount + 1);
        delay(500);
        WiFi.mode(WIFI_AP_STA);
        delay(100);
        WiFi.mode(WIFI_STA);
        delay(500);
        macAddress = WiFi.macAddress();
        retryCount++;
    }
    
    Serial.print("📡 MAC naslov MODULA 3: ");
    Serial.println(WiFi.macAddress());
    
    // Nastavi kanal na 1
    esp_wifi_set_promiscuous(true);
    esp_wifi_set_channel(1, WIFI_SECOND_CHAN_NONE);
    esp_wifi_set_promiscuous(false);
    Serial.println("📡 Kanal nastavljen na 1");
    
    Serial.println("\n========== MAC NASLOVI ==========");
    Serial.print("📡 MAC MASTER (v kodi): ");
    for(int i = 0; i < 6; i++) {
        Serial.printf("%02X", masterMAC[i]);
        if(i < 5) Serial.print(":");
    }
    Serial.println();
    Serial.println("================================\n");
    
    testLimitSwitches();
    
    Serial.println("\n📌 TEST SMERI MOTORJA:");
    Serial.println("   Motor se bo kratek čas vrtel v obe smeri.");
    Serial.println("   Preverite, v katero smer se vrti gred motorja.\n");
    delay(2000);
    
    testMotorDirection();
    
    if (!motorState.calibrated) {
        Serial.println("\n⚠ Motor ni kalibriran! Začenjam kalibracijo...");
        Serial.println("   Prepričajte se, da se motor vrti v pravo smer!");
        delay(3000);
        autoCalibrate();
    } else {
        Serial.println("✅ Motor je kalibriran!");
        enableMotor(true);
    }
    
    currentMotorSpeed = SPEED_MAX;
    if (xSemaphoreTake(motorMutex, portMAX_DELAY)) {
        motorState.speedCmd = SPEED_MAX;
        xSemaphoreGive(motorMutex);
    }
    
    xTaskCreatePinnedToCore(motorTask, "MotorTask", 8192, NULL, 2, NULL, 1);
    xTaskCreatePinnedToCore(communicationTask, "CommTask", 8192, NULL, 1, NULL, 0);
    
    Serial.println("\n╔════════════════════════════════════════════════════╗");
    Serial.println("║              MODUL 3 PRIPRAVLJEN                  ║");
    Serial.println("╚════════════════════════════════════════════════════╝");
    Serial.printf("  Kalibriran: %s\n", motorState.calibrated ? "DA" : "NE");
    Serial.printf("  Pozicija: %d%%\n", motorState.currentPos);
    Serial.printf("  Korakov: %d/%d\n", motorState.absoluteSteps, motorState.totalSteps);
    Serial.printf("  DEJANSKI MAC: %s\n", WiFi.macAddress().c_str());
    Serial.printf("  Hitrost premikanja: %d korakov/s\n", SPEED_MAX);
    Serial.printf("  Hitrost kalibracije: %d korakov/s\n", CALIB_SPEED);
    Serial.printf("  Smer vrtenja: %s\n", reverseDirection ? "OBRNJENA" : "NORMALNA");
    Serial.println("  Jedro 0: Komunikacija (ESP-NOW)");
    Serial.println("  Jedro 1: Krmiljenje motorja");
    Serial.println("  Čakam na ukaze...\n");
    
    for(int i = 0; i < 3; i++) {
        digitalWrite(STATUS_LED, HIGH);
        delay(100);
        digitalWrite(STATUS_LED, LOW);
        delay(100);
    }
}

// ==================== GLAVNA ZANKA ====================
void loop() {
    delay(1000);
}
Senzor Pin na modulu Povezava AHT20 VIN 3.3V 3.3V AHT20 GND GND GND AHT20 SDA GPIO4 SDA AHT20 SCL GPIO5 SCL BMP280 VIN 3.3V 3.3V BMP280 GND GND GND BMP280 SDA GPIO4 SDA BMP280 SCL GPIO5 SCL LDR en konec 3.3V 3.3V LDR drug konec GPIO6 A0 10k upor GPIO6 -> GND Pull-down