New Motor Version
This commit is contained in:
parent
ddeba773a5
commit
39a14943b7
@ -1,54 +1,119 @@
|
||||
#include <I2CMotor.h>
|
||||
#include <Wire.h>
|
||||
|
||||
// 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
|
||||
}
|
||||
|
||||
@ -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)
|
||||
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
|
||||
}
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
// --- 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 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_;
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user