#ifndef ESP32MOTORI2C_H #define ESP32MOTORI2C_H #include #include #define DEFAULT_ADDR 0x08 #define EEPROM_SIZE 64 #define DEVICENAME_MAXLEN 24 #define EEPROM_MIN_PWM 32 #define EEPROM_MAX_PWM 33 #define PWM_RESOLUTION_BITS 8 #define PWM_RESOLUTION_MAX ((1 << PWM_RESOLUTION_BITS) - 1) class ESP32MotorI2C { public: ESP32MotorI2C(uint8_t en1, uint8_t in1, uint8_t in2, uint8_t in3, uint8_t in4, uint8_t en2) : en1_(en1), in1_(in1), in2_(in2), in3_(in3), in4_(in4), en2_(en2), id_requested_(false) { memset(device_name_, 0, sizeof(device_name_)); min_pwm_ = 0; max_pwm_ = PWM_RESOLUTION_MAX; } void begin(uint8_t sda = 8, uint8_t scl = 9, uint32_t freq = 100000) { Serial.begin(115200); Serial.println("[SLAVE] Booting..."); EEPROM.begin(EEPROM_SIZE); i2c_address_ = EEPROM.read(0); if(i2c_address_ < 0x08 || i2c_address_ > 0x77) { i2c_address_ = DEFAULT_ADDR; EEPROM.write(0, i2c_address_); EEPROM.commit(); } Serial.print("[SLAVE] I2C address: 0x"); Serial.println(i2c_address_, HEX); loadDeviceName(); Serial.print("[SLAVE] Device name: "); Serial.println(device_name_); min_pwm_ = EEPROM.read(EEPROM_MIN_PWM); max_pwm_ = EEPROM.read(EEPROM_MAX_PWM); enforcePwmBounds(); Serial.print("[SLAVE] Min PWM: "); Serial.println(min_pwm_); Serial.print("[SLAVE] Max PWM: "); Serial.println(max_pwm_); pinMode(in1_, OUTPUT); pinMode(in2_, OUTPUT); pinMode(in3_, OUTPUT); pinMode(in4_, OUTPUT); analogWriteResolution(en1_, PWM_RESOLUTION_BITS); analogWriteResolution(en2_, PWM_RESOLUTION_BITS); analogWriteFrequency(en1_, 5000); analogWriteFrequency(en2_, 5000); Wire.onReceive(receiveEventStatic); Wire.onRequest(requestEventStatic); Wire.begin(i2c_address_, sda, scl, freq); instance_ = this; Serial.println("[SLAVE] Ready for I2C commands."); } void setMotorSpeed(uint8_t motor, uint8_t percent) { percent = constrain(percent, 0, 100); uint16_t duty = map(percent, 0, 100, min_pwm_, max_pwm_); if (motor == 1) analogWrite(en1_, duty); else if (motor == 2) analogWrite(en2_, duty); Serial.print("[SLAVE] Motor "); Serial.print(motor); Serial.print(" speed set to "); Serial.print(percent); Serial.print("% (PWM="); Serial.print(duty); Serial.println(")"); } void setMotorDirection(uint8_t motor, uint8_t dir) { if (motor == 1) { digitalWrite(in1_, dir & 0x01); digitalWrite(in2_, (dir >> 1) & 0x01); } else if (motor == 2) { digitalWrite(in3_, dir & 0x01); digitalWrite(in4_, (dir >> 1) & 0x01); } Serial.print("[SLAVE] Motor "); Serial.print(motor); Serial.print(" direction set to "); Serial.println(dir, BIN); } void emergencyStop() { analogWrite(en1_, 0); analogWrite(en2_, 0); digitalWrite(in1_, LOW); digitalWrite(in2_, LOW); digitalWrite(in3_, LOW); digitalWrite(in4_, LOW); Serial.println("[SLAVE] Emergency stop!"); } private: uint8_t en1_, in1_, in2_, in3_, in4_, en2_; uint8_t i2c_address_; volatile bool id_requested_; char id_response_[64]; char device_name_[DEVICENAME_MAXLEN + 1]; uint16_t min_pwm_, max_pwm_; static ESP32MotorI2C* instance_; void enforcePwmBounds() { if (min_pwm_ > PWM_RESOLUTION_MAX) min_pwm_ = 0; if (max_pwm_ > PWM_RESOLUTION_MAX) max_pwm_ = PWM_RESOLUTION_MAX; if (min_pwm_ > max_pwm_) min_pwm_ = max_pwm_; } void loadDeviceName() { uint8_t len = EEPROM.read(1); if (len > DEVICENAME_MAXLEN) len = DEVICENAME_MAXLEN; for (uint8_t i = 0; i < len; ++i) device_name_[i] = EEPROM.read(2 + i); device_name_[len] = '\0'; if (len == 0 || device_name_[0] == 0xFF || device_name_[0] == 0x00) strncpy(device_name_, "dc motor", DEVICENAME_MAXLEN); } void saveDeviceName(const char* newname, uint8_t len) { if (len > DEVICENAME_MAXLEN) len = DEVICENAME_MAXLEN; EEPROM.write(1, len); for (uint8_t i = 0; i < len; ++i) EEPROM.write(2 + i, newname[i]); EEPROM.commit(); strncpy(device_name_, newname, len); device_name_[len] = '\0'; Serial.print("[SLAVE] Device name changed to: "); Serial.println(device_name_); } void saveMinMaxPWM(uint16_t minv, uint16_t maxv) { if (minv > PWM_RESOLUTION_MAX) minv = 0; if (maxv > PWM_RESOLUTION_MAX) maxv = PWM_RESOLUTION_MAX; if (minv > maxv) minv = maxv; EEPROM.write(EEPROM_MIN_PWM, minv); EEPROM.write(EEPROM_MAX_PWM, maxv); EEPROM.commit(); min_pwm_ = minv; max_pwm_ = maxv; Serial.print("[SLAVE] Min PWM set to: "); Serial.println(min_pwm_); Serial.print("[SLAVE] Max PWM set to: "); Serial.println(max_pwm_); } void receiveEvent(int numBytes) { if (numBytes < 1) return; uint8_t cmd = Wire.read(); if (cmd == 'i') { memset(id_response_, 0, sizeof(id_response_)); snprintf(id_response_, sizeof(id_response_), "%02X,dc motor,%s,%u,%u", i2c_address_, device_name_, min_pwm_, max_pwm_); id_requested_ = true; Serial.println("[SLAVE] Identification requested."); return; } switch (cmd) { case 0x01: // Change address if (Wire.available()) { uint8_t new_addr = Wire.read(); EEPROM.write(0, new_addr); EEPROM.commit(); Serial.print("[SLAVE] Address changed to 0x"); Serial.println(new_addr, HEX); delay(100); ESP.restart(); } break; case 0x02: // Set speed if (Wire.available() >= 2) { uint8_t motor = Wire.read(); uint8_t percent = Wire.read(); setMotorSpeed(motor, percent); } break; case 0x03: // Set direction if (Wire.available() >= 2) { uint8_t motor = Wire.read(); uint8_t dir = Wire.read(); setMotorDirection(motor, dir); } break; case 0x05: // Emergency stop emergencyStop(); break; case 0x06: // Set device name (variable length) if (Wire.available() >= 1) { uint8_t len = Wire.read(); char newname[DEVICENAME_MAXLEN + 1]; uint8_t i = 0; while (Wire.available() && i < len && i < DEVICENAME_MAXLEN) { newname[i++] = Wire.read(); } newname[i] = '\0'; saveDeviceName(newname, i); } break; case 0x07: // Set min/max PWM if (Wire.available() >= 2) { uint16_t minv = Wire.read(); uint16_t maxv = Wire.read(); saveMinMaxPWM(minv, maxv); } break; } } void requestEvent() { if (id_requested_) { size_t len = strnlen(id_response_, sizeof(id_response_)); Wire.write((uint8_t*)id_response_, len); id_requested_ = false; } } static void receiveEventStatic(int numBytes) { if (instance_) instance_->receiveEvent(numBytes); } static void requestEventStatic() { if (instance_) instance_->requestEvent(); } }; ESP32MotorI2C* ESP32MotorI2C::instance_ = nullptr; #endif