121 lines
3.2 KiB
C++
121 lines
3.2 KiB
C++
#ifndef I2C_MOTOR_MASTER_H
|
|
#define I2C_MOTOR_MASTER_H
|
|
|
|
#include <Wire.h>
|
|
|
|
class I2CMotorMaster {
|
|
public:
|
|
I2CMotorMaster(uint8_t sda = 8, uint8_t scl = 9, uint32_t freq = 100000)
|
|
: sda_(sda), scl_(scl), freq_(freq), slave_addr_(0) {}
|
|
|
|
void begin() {
|
|
Wire.begin(sda_, scl_, freq_);
|
|
// delay(1000);
|
|
slave_addr_ = scanForMotorController();
|
|
// delay(1000);
|
|
}
|
|
|
|
bool found() { return slave_addr_ != 0; }
|
|
uint8_t address() { return slave_addr_; }
|
|
|
|
String identify() {
|
|
return queryIdentification(slave_addr_);
|
|
}
|
|
|
|
void setDeviceName(const char* newname) {
|
|
uint8_t len = strlen(newname);
|
|
Wire.beginTransmission(slave_addr_);
|
|
Wire.write(0x06);
|
|
Wire.write(len);
|
|
Wire.write((const uint8_t*)newname, len);
|
|
Wire.endTransmission();
|
|
// delay(100);
|
|
}
|
|
|
|
void setMinMax(uint8_t min_pwm, uint8_t max_pwm) {
|
|
Wire.beginTransmission(slave_addr_);
|
|
Wire.write(0x07);
|
|
Wire.write(min_pwm);
|
|
Wire.write(max_pwm);
|
|
Wire.endTransmission();
|
|
// delay(100);
|
|
}
|
|
|
|
void setMotorDirection(uint8_t motor, uint8_t dir) {
|
|
Wire.beginTransmission(slave_addr_);
|
|
Wire.write(0x03);
|
|
Wire.write(motor);
|
|
Wire.write(dir); // 1=forward, 2=backward
|
|
Wire.endTransmission();
|
|
// delay(500);
|
|
}
|
|
|
|
void setMotorSpeed(uint8_t motor, uint8_t speed_percent) {
|
|
Wire.beginTransmission(slave_addr_);
|
|
Wire.write(0x02);
|
|
Wire.write(motor);
|
|
Wire.write(speed_percent);
|
|
Wire.endTransmission();
|
|
// delay(1000);
|
|
}
|
|
|
|
void emergencyStop() {
|
|
Wire.beginTransmission(slave_addr_);
|
|
Wire.write(0x05);
|
|
Wire.endTransmission();
|
|
// delay(1000);
|
|
}
|
|
|
|
private:
|
|
uint8_t sda_, scl_;
|
|
uint32_t freq_;
|
|
uint8_t slave_addr_;
|
|
|
|
uint8_t scanForMotorController() {
|
|
for (uint8_t addr = 1; addr < 127; addr++) {
|
|
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("dc motor") >= 0) {
|
|
Serial.print("Found motor controller at 0x");
|
|
Serial.print(addr, HEX);
|
|
Serial.print(": ");
|
|
Serial.println(id);
|
|
return addr;
|
|
}
|
|
}
|
|
delay(5);
|
|
}
|
|
Serial.println("Motor controller not found!");
|
|
return 0;
|
|
}
|
|
|
|
String queryIdentification(uint8_t addr) {
|
|
Wire.beginTransmission(addr);
|
|
Wire.write('i');
|
|
Wire.endTransmission();
|
|
delay(10);
|
|
Wire.requestFrom(addr, 48);
|
|
String response;
|
|
while (Wire.available()) {
|
|
char c = Wire.read();
|
|
if (c == '\0') break;
|
|
response += c;
|
|
}
|
|
Serial.print("Identification at 0x");
|
|
Serial.print(addr, HEX);
|
|
Serial.print(": ");
|
|
Serial.println(response);
|
|
return response;
|
|
}
|
|
};
|
|
|
|
#endif
|