103 lines
2.7 KiB
C++
103 lines
2.7 KiB
C++
#include "I2CMotor.h"
|
|
|
|
I2CMotor::I2CMotor() {}
|
|
|
|
I2CMotor::I2CMotor(uint8_t sda, uint8_t scl, uint32_t freq)
|
|
: sda_(sda), scl_(scl), freq_(freq), slave_addr_(0) {}
|
|
|
|
void I2CMotor::begin() {
|
|
Wire.begin(sda_, scl_, freq_);
|
|
slave_addr_ = scanForMotorController();
|
|
}
|
|
|
|
bool I2CMotor::found() { return slave_addr_ != 0; }
|
|
uint8_t I2CMotor::address() { return slave_addr_; }
|
|
|
|
String I2CMotor::identify() {
|
|
return queryIdentification(slave_addr_);
|
|
}
|
|
|
|
void I2CMotor::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();
|
|
}
|
|
|
|
void I2CMotor::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();
|
|
}
|
|
|
|
void I2CMotor::setMotorDirection(uint8_t motor, uint8_t dir) {
|
|
Wire.beginTransmission(slave_addr_);
|
|
Wire.write(0x03);
|
|
Wire.write(motor);
|
|
Wire.write(dir);
|
|
Wire.endTransmission();
|
|
}
|
|
|
|
void I2CMotor::setMotorSpeed(uint8_t motor, uint8_t speed_percent) {
|
|
Wire.beginTransmission(slave_addr_);
|
|
Wire.write(0x02);
|
|
Wire.write(motor);
|
|
Wire.write(speed_percent);
|
|
Wire.endTransmission();
|
|
}
|
|
|
|
void I2CMotor::emergencyStop() {
|
|
Wire.beginTransmission(slave_addr_);
|
|
Wire.write(0x05);
|
|
Wire.endTransmission();
|
|
}
|
|
|
|
uint8_t I2CMotor::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 I2CMotor::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;
|
|
}
|
|
|