From 39a14943b7fcd57dd433896aadc9d99cd8c260ed Mon Sep 17 00:00:00 2001 From: Fatima Idrees Date: Sun, 18 May 2025 13:38:58 +0300 Subject: [PATCH] New Motor Version --- examples/Master/Master.ino | 139 ++++++++++++++------ examples/Slave/ESP32MotorI2C.h | 225 --------------------------------- examples/Slave/Slave.ino | 167 +++++++++++++++++++++++- src/I2CMotor.cpp | 36 +++++- src/I2CMotor.h | 10 +- 5 files changed, 307 insertions(+), 270 deletions(-) delete mode 100644 examples/Slave/ESP32MotorI2C.h diff --git a/examples/Master/Master.ino b/examples/Master/Master.ino index 7495181..448f220 100644 --- a/examples/Master/Master.ino +++ b/examples/Master/Master.ino @@ -1,54 +1,119 @@ #include +#include -// User can change these pins as needed: +// Define I2C pins and slave addresses #define SDA_PIN 8 #define SCL_PIN 3 -#define I2C_FREQ 100000 +#define INITIAL_SLAVE_ADDR 0x08 +#define NEW_SLAVE_ADDR 0x10 -I2CMotor motor(SDA_PIN, SCL_PIN, I2C_FREQ); +I2CMotor motor(SDA_PIN, SCL_PIN); + +// Scan I2C bus for motor controllers and print their info +void scanI2CBusForMotors() { + Serial.println("Scanning I2C bus for motor controllers..."); + for (uint8_t addr = 1; addr < 127; addr++) { + Wire.beginTransmission(addr); + uint8_t error = Wire.endTransmission(); + if (error == 0) { + // If device ACKs, try to identify it + Wire.beginTransmission(addr); + Wire.write('i'); + if (Wire.endTransmission() == 0) { + Wire.requestFrom(addr, 48); + String id; + while (Wire.available()) { + char c = Wire.read(); + if (c == '\0') break; + id += c; + } + if (id.length() && id.indexOf("GENERIC") >= 0) { + Serial.print("Found motor controller at 0x"); + Serial.print(addr, HEX); + Serial.print(": "); + Serial.println(id); + } + } + } + delay(5); + } + Serial.println("Scan complete."); +} + +// Send the command to change the slave's I2C address +void setSlaveAddress(uint8_t currentAddr, uint8_t newAddr) { + Wire.beginTransmission(currentAddr); + Wire.write(0x01); // CMD_SET_ADDR + Wire.write(newAddr); // New address + Wire.endTransmission(); + Serial.print("Sent address change command to 0x"); + Serial.print(currentAddr, HEX); + Serial.print(". New address should be 0x"); + Serial.println(newAddr, HEX); +} void setup() { Serial.begin(115200); + delay(2000); // Wait for serial and slave to boot + + // Initialize I2C and motor library + Wire.begin(SDA_PIN, SCL_PIN); motor.begin(); + motor.setAddress(INITIAL_SLAVE_ADDR); - if (!motor.found()) { - Serial.println("No motor controller found. Halting."); - while (1); - } + // 1. Set device name + motor.setDeviceName("TestMotor"); + delay(200); - motor.identify(); + // 2. Identify device + String id = motor.identify(); + Serial.print("Identify: "); Serial.println(id); + delay(200); - // Set device name and min/max PWM - motor.setDeviceName("FM"); - motor.setMinMax(40, 255); + // 3. Control Motor 1: 80% speed, forward + motor.motorControl(1, 80, 0); + delay(2000); - Serial.println("All tests complete."); + // 4. Control Motor 2: 60% speed, reverse + motor.motorControl(2, 60, 1); + delay(2000); + + // 5. Control both motors: A 100% forward, B 100% reverse + motor.bothMotorsControl(100, 0, 100, 1); + delay(2000); + + // 6. Stop all motors + motor.stopAllMotors(); + delay(1000); + + // 7. All stop (emergency stop) + motor.allStop(); + delay(1000); + + // 8. Scan I2C bus for motor controllers + scanI2CBusForMotors(); + + // 9. Change slave address + setSlaveAddress(INITIAL_SLAVE_ADDR, NEW_SLAVE_ADDR); + Serial.println("Waiting for slave to reboot with new address..."); + delay(2000); // Give time for slave to reboot + + // 10. Use the new address for all further commands + motor.setAddress(NEW_SLAVE_ADDR); + + // 11. Identify device at new address + String id2 = motor.identify(); + Serial.print("Identify at new address: "); Serial.println(id2); + + // 12. Run a test at new address (Motor 1, 50% forward) + motor.motorControl(1, 50, 0); + delay(2000); + + // 13. Stop all motors at new address + motor.stopAllMotors(); + delay(1000); } void loop() { - // Set direction forward and test speeds - motor.setMotorDirection(1, 1); // Motor 1, forward - motor.setMotorSpeed(1, 50); - delay(2000); - motor.setMotorSpeed(1, 60); - delay(2000); - motor.setMotorSpeed(1, 80); - delay(2000); - motor.setMotorSpeed(1, 100); - delay(2000); - - // Set direction backward and test speeds - motor.setMotorDirection(1, 2); // Motor 1, backward - motor.setMotorSpeed(1, 50); - delay(2000); - motor.setMotorSpeed(1, 60); - delay(2000); - motor.setMotorSpeed(1, 80); - delay(2000); - motor.setMotorSpeed(1, 100); - delay(2000); - - // Stop - motor.emergencyStop(); - delay(2000); + // Nothing here } diff --git a/examples/Slave/ESP32MotorI2C.h b/examples/Slave/ESP32MotorI2C.h deleted file mode 100644 index 166d3a0..0000000 --- a/examples/Slave/ESP32MotorI2C.h +++ /dev/null @@ -1,225 +0,0 @@ -#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 diff --git a/examples/Slave/Slave.ino b/examples/Slave/Slave.ino index 062322f..299f982 100644 --- a/examples/Slave/Slave.ino +++ b/examples/Slave/Slave.ino @@ -1,12 +1,169 @@ -#include "ESP32MotorI2C.h" +#include +#include -// EN1, IN1, IN2, IN3, IN4, EN2 (change pins as needed for your hardware) -ESP32MotorI2C motor(0, 1, 2, 4, 5, 6); +#define EEPROM_SIZE 64 +#define EEPROM_ADDR_LOC 0 +#define EEPROM_NAME_LOC 1 +#define DEVICE_NAME_LEN 24 +#define DEFAULT_I2C_ADDR 0x08 + +// Motor pins +#define EN1 0 +#define IN1 1 +#define IN2 2 +#define IN3 4 +#define IN4 5 +#define EN2 6 + +char deviceName[DEVICE_NAME_LEN] = "Motor"; +uint8_t i2cAddress = DEFAULT_I2C_ADDR; + +// I2C command codes +enum Command : uint8_t { + CMD_SET_ADDR = 0x01, + CMD_SET_NAME = 0x02, + CMD_MOTOR_CTRL = 0x10, + CMD_BOTH_CTRL = 0x11, +}; + +// Store address to EEPROM +void storeAddressToEEPROM(uint8_t addr) { + EEPROM.write(EEPROM_ADDR_LOC, addr); + EEPROM.commit(); +} + +// Read address from EEPROM (with fallback to default) +uint8_t readAddressFromEEPROM() { + uint8_t addr = EEPROM.read(EEPROM_ADDR_LOC); + if (addr == 0xFF || addr == 0x00) { // Uninitialized or invalid + addr = DEFAULT_I2C_ADDR; + storeAddressToEEPROM(addr); + } + return addr; +} + +void storeNameToEEPROM(const char* name) { + for (int i = 0; i < DEVICE_NAME_LEN; i++) { + EEPROM.write(EEPROM_NAME_LOC + i, name[i]); + if (name[i] == '\0') break; + } + EEPROM.commit(); +} + +void readNameFromEEPROM(char* name) { + for (int i = 0; i < DEVICE_NAME_LEN; i++) { + name[i] = EEPROM.read(EEPROM_NAME_LOC + i); + if (name[i] == '\0') break; + } + name[DEVICE_NAME_LEN - 1] = '\0'; +} + +// Helper: Set motor speed and direction +void setMotor(uint8_t enPin, uint8_t in1, uint8_t in2, uint8_t speed, uint8_t dir) { + analogWrite(enPin, speed); + if (dir == 0) { + digitalWrite(in1, HIGH); + digitalWrite(in2, LOW); + } else { + digitalWrite(in1, LOW); + digitalWrite(in2, HIGH); + } +} + +// Stop motor +void stopMotor(uint8_t enPin, uint8_t in1, uint8_t in2) { + analogWrite(enPin, 0); + digitalWrite(in1, LOW); + digitalWrite(in2, LOW); +} + +// I2C receive handler +void onReceive(int len) { + if (len < 1) return; + uint8_t cmd = Wire.read(); + switch (cmd) { + case CMD_SET_ADDR: + if (Wire.available()) { + uint8_t newAddr = Wire.read(); + if (newAddr > 0x00 && newAddr < 0x78) { // Valid 7-bit address + storeAddressToEEPROM(newAddr); + ESP.restart(); // Or NVIC_SystemReset() on STM32, or just reset the MCU as needed + } + } + break; + case CMD_SET_NAME: { + int i = 0; + while (Wire.available() && i < DEVICE_NAME_LEN - 1) { + deviceName[i++] = Wire.read(); + } + deviceName[i] = '\0'; + storeNameToEEPROM(deviceName); + break; + } + case CMD_MOTOR_CTRL: { + if (Wire.available() >= 3) { + uint8_t motor_id = Wire.read(); // 1: MotorA, 2: MotorB + uint8_t speed_pct = Wire.read(); // 0-100 + uint8_t dir = Wire.read(); // 0: FWD, 1: REV + uint8_t pwm = map(speed_pct, 0, 100, 0, 255); + + if (motor_id == 1) { // MotorA + setMotor(EN1, IN1, IN2, pwm, dir); + } else if (motor_id == 2) { // MotorB + setMotor(EN2, IN3, IN4, pwm, dir); + } + } + break; + } + case CMD_BOTH_CTRL: { + if (Wire.available() >= 4) { + uint8_t speedA = Wire.read(); + uint8_t dirA = Wire.read(); + uint8_t speedB = Wire.read(); + uint8_t dirB = Wire.read(); + uint8_t pwmA = map(speedA, 0, 100, 0, 255); + uint8_t pwmB = map(speedB, 0, 100, 0, 255); + + setMotor(EN1, IN1, IN2, pwmA, dirA); + setMotor(EN2, IN3, IN4, pwmB, dirB); + } + break; + } + default: break; + } +} + +// I2C request handler +void onRequest() { + String response = String(i2cAddress, HEX) + ",GENERIC," + deviceName; + Wire.write((const uint8_t*)response.c_str(), response.length()); +} void setup() { - motor.begin(8, 9, 100000); // Use your actual I2C pins and frequency + Serial.begin(115200); + EEPROM.begin(EEPROM_SIZE); + + i2cAddress = readAddressFromEEPROM(); + readNameFromEEPROM(deviceName); + + // Motor pins setup + pinMode(EN1, OUTPUT); + pinMode(IN1, OUTPUT); + pinMode(IN2, OUTPUT); + pinMode(EN2, OUTPUT); + pinMode(IN3, OUTPUT); + pinMode(IN4, OUTPUT); + + Wire.begin(i2cAddress); + Wire.onReceive(onReceive); + Wire.onRequest(onRequest); + + Serial.print("I2C Slave started at address 0x"); + Serial.println(i2cAddress, HEX); + Serial.print("Device name: "); + Serial.println(deviceName); } void loop() { - // Nothing needed for slave + // Nothing here } diff --git a/src/I2CMotor.cpp b/src/I2CMotor.cpp index 8a17afb..5c53999 100644 --- a/src/I2CMotor.cpp +++ b/src/I2CMotor.cpp @@ -50,12 +50,18 @@ void I2CMotor::setMotorSpeed(uint8_t motor, uint8_t speed_percent) { Wire.endTransmission(); } -void I2CMotor::emergencyStop() { +// Renamed from emergencyStop +void I2CMotor::allStop() { Wire.beginTransmission(slave_addr_); Wire.write(0x05); Wire.endTransmission(); } +// New: Set address at runtime +void I2CMotor::setAddress(uint8_t addr) { + slave_addr_ = addr; +} + uint8_t I2CMotor::scanForMotorController() { for (uint8_t addr = 1; addr < 127; addr++) { Wire.beginTransmission(addr); @@ -100,4 +106,30 @@ String I2CMotor::queryIdentification(uint8_t addr) { Serial.println(response); return response; } - \ No newline at end of file + +// --- New functions for L298N motor control --- + +bool I2CMotor::motorControl(uint8_t motor, uint8_t speed_percent, uint8_t direction) { + if (!found()) return false; + Wire.beginTransmission(slave_addr_); + Wire.write(0x10); // CMD_MOTOR_CTRL + Wire.write(motor); + Wire.write(speed_percent); + Wire.write(direction); + return Wire.endTransmission() == 0; +} + +bool I2CMotor::bothMotorsControl(uint8_t speedA, uint8_t dirA, uint8_t speedB, uint8_t dirB) { + if (!found()) return false; + Wire.beginTransmission(slave_addr_); + Wire.write(0x11); // CMD_BOTH_CTRL + Wire.write(speedA); + Wire.write(dirA); + Wire.write(speedB); + Wire.write(dirB); + return Wire.endTransmission() == 0; +} + +void I2CMotor::stopAllMotors() { + bothMotorsControl(0, 0, 0, 0); +} diff --git a/src/I2CMotor.h b/src/I2CMotor.h index 50475a4..d6b910c 100644 --- a/src/I2CMotor.h +++ b/src/I2CMotor.h @@ -16,7 +16,15 @@ public: void setMinMax(uint8_t min_pwm, uint8_t max_pwm); void setMotorDirection(uint8_t motor, uint8_t dir); void setMotorSpeed(uint8_t motor, uint8_t speed_percent); - void emergencyStop(); + void allStop(); // Renamed from emergencyStop + + // New: Set address at runtime + void setAddress(uint8_t addr); + + // --- New functions for L298N motor control --- + bool motorControl(uint8_t motor, uint8_t speed_percent, uint8_t direction); + bool bothMotorsControl(uint8_t speedA, uint8_t dirA, uint8_t speedB, uint8_t dirB); + void stopAllMotors(); private: uint8_t sda_, scl_;