New Motor Version
This commit is contained in:
parent
ddeba773a5
commit
39a14943b7
@ -1,54 +1,119 @@
|
|||||||
#include <I2CMotor.h>
|
#include <I2CMotor.h>
|
||||||
|
#include <Wire.h>
|
||||||
|
|
||||||
// User can change these pins as needed:
|
// Define I2C pins and slave addresses
|
||||||
#define SDA_PIN 8
|
#define SDA_PIN 8
|
||||||
#define SCL_PIN 3
|
#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() {
|
void setup() {
|
||||||
Serial.begin(115200);
|
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.begin();
|
||||||
|
motor.setAddress(INITIAL_SLAVE_ADDR);
|
||||||
|
|
||||||
if (!motor.found()) {
|
// 1. Set device name
|
||||||
Serial.println("No motor controller found. Halting.");
|
motor.setDeviceName("TestMotor");
|
||||||
while (1);
|
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
|
// 3. Control Motor 1: 80% speed, forward
|
||||||
motor.setDeviceName("FM");
|
motor.motorControl(1, 80, 0);
|
||||||
motor.setMinMax(40, 255);
|
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() {
|
void loop() {
|
||||||
// Set direction forward and test speeds
|
// Nothing here
|
||||||
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);
|
|
||||||
}
|
}
|
||||||
|
|||||||
@ -1,225 +0,0 @@
|
|||||||
#ifndef ESP32MOTORI2C_H
|
|
||||||
#define ESP32MOTORI2C_H
|
|
||||||
|
|
||||||
#include <Wire.h>
|
|
||||||
#include <EEPROM.h>
|
|
||||||
|
|
||||||
#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
|
|
||||||
@ -1,12 +1,169 @@
|
|||||||
#include "ESP32MotorI2C.h"
|
#include <Wire.h>
|
||||||
|
#include <EEPROM.h>
|
||||||
|
|
||||||
// EN1, IN1, IN2, IN3, IN4, EN2 (change pins as needed for your hardware)
|
#define EEPROM_SIZE 64
|
||||||
ESP32MotorI2C motor(0, 1, 2, 4, 5, 6);
|
#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() {
|
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() {
|
void loop() {
|
||||||
// Nothing needed for slave
|
// Nothing here
|
||||||
}
|
}
|
||||||
|
|||||||
@ -50,12 +50,18 @@ void I2CMotor::setMotorSpeed(uint8_t motor, uint8_t speed_percent) {
|
|||||||
Wire.endTransmission();
|
Wire.endTransmission();
|
||||||
}
|
}
|
||||||
|
|
||||||
void I2CMotor::emergencyStop() {
|
// Renamed from emergencyStop
|
||||||
|
void I2CMotor::allStop() {
|
||||||
Wire.beginTransmission(slave_addr_);
|
Wire.beginTransmission(slave_addr_);
|
||||||
Wire.write(0x05);
|
Wire.write(0x05);
|
||||||
Wire.endTransmission();
|
Wire.endTransmission();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// New: Set address at runtime
|
||||||
|
void I2CMotor::setAddress(uint8_t addr) {
|
||||||
|
slave_addr_ = addr;
|
||||||
|
}
|
||||||
|
|
||||||
uint8_t I2CMotor::scanForMotorController() {
|
uint8_t I2CMotor::scanForMotorController() {
|
||||||
for (uint8_t addr = 1; addr < 127; addr++) {
|
for (uint8_t addr = 1; addr < 127; addr++) {
|
||||||
Wire.beginTransmission(addr);
|
Wire.beginTransmission(addr);
|
||||||
@ -100,4 +106,30 @@ String I2CMotor::queryIdentification(uint8_t addr) {
|
|||||||
Serial.println(response);
|
Serial.println(response);
|
||||||
return response;
|
return response;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// --- 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);
|
||||||
|
}
|
||||||
|
|||||||
@ -16,7 +16,15 @@ public:
|
|||||||
void setMinMax(uint8_t min_pwm, uint8_t max_pwm);
|
void setMinMax(uint8_t min_pwm, uint8_t max_pwm);
|
||||||
void setMotorDirection(uint8_t motor, uint8_t dir);
|
void setMotorDirection(uint8_t motor, uint8_t dir);
|
||||||
void setMotorSpeed(uint8_t motor, uint8_t speed_percent);
|
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:
|
private:
|
||||||
uint8_t sda_, scl_;
|
uint8_t sda_, scl_;
|
||||||
|
|||||||
Loading…
x
Reference in New Issue
Block a user