120 lines
3.3 KiB
C++
120 lines
3.3 KiB
C++
#include <I2CMotor.h>
|
|
#include <Wire.h>
|
|
|
|
// Define I2C pins and slave addresses
|
|
#define SDA_PIN 8
|
|
#define SCL_PIN 3
|
|
#define INITIAL_SLAVE_ADDR 0x08
|
|
#define NEW_SLAVE_ADDR 0x10
|
|
|
|
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);
|
|
|
|
// 1. Set device name
|
|
motor.setDeviceName("TestMotor");
|
|
delay(200);
|
|
|
|
// 2. Identify device
|
|
String id = motor.identify();
|
|
Serial.print("Identify: "); Serial.println(id);
|
|
delay(200);
|
|
|
|
// 3. Control Motor 1: 80% speed, forward
|
|
motor.motorControl(1, 80, 0);
|
|
delay(2000);
|
|
|
|
// 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() {
|
|
// Nothing here
|
|
}
|